boards/kakute-f7-mini-v3: add selectable motor outputs
Signed-off-by: Onur Özkan <work@onurozkan.dev>
This commit is contained in:
Onur Özkan committed
1 parent
4a5d0313ca
commit
117e1abe97
12 files changed
+987
-88
No files matched your search
@@ -41,12 +41,3 @@ dfu-util --alt 0 --dfuse-address 0x08000000:leave \
|
||||
| `RX` | `T1` |
|
||||
| `GND` | `G` |
|
||||
| `TX` | `R1` |
|
||||
|
||||
The firmware emits unsigned MAVLink 2 at 115200 baud, 8N1. It is binary
|
||||
data, not terminal text. Read it with the python decoder:
|
||||
|
||||
```sh
|
||||
./scripts/test-mavlink.py $serial_device_path
|
||||
```
|
||||
|
||||
The test helper sends MAVLink `TIMESYNC` and`PING` frames while printing all received MAVLink messages.
|
||||
@@ -0,0 +1,84 @@
|
||||
mod tim3_dshot;
|
||||
mod tim3_pwm;
|
||||
|
||||
use core::sync::atomic::{AtomicU8, Ordering};
|
||||
|
||||
use kaptan_actuator::{MotorOutput, MotorProtocol};
|
||||
use stm32f7::stm32f722;
|
||||
|
||||
use self::{tim3_dshot::Tim3Dshot, tim3_pwm::Tim3Pwm};
|
||||
|
||||
const TRANSPORT_NONE: u8 = 0;
|
||||
const TRANSPORT_PWM: u8 = 1;
|
||||
const TRANSPORT_DSHOT: u8 = 2;
|
||||
|
||||
static ACTIVE_TRANSPORT: AtomicU8 = AtomicU8::new(TRANSPORT_NONE);
|
||||
|
||||
/// Kakute F7 Mini V3's protocol-neutral M1–M4 output route.
|
||||
pub(crate) struct KakuteMotorOutput {
|
||||
transport: Transport,
|
||||
}
|
||||
|
||||
enum Transport {
|
||||
Pwm(Tim3Pwm),
|
||||
Dshot(Tim3Dshot),
|
||||
}
|
||||
|
||||
impl KakuteMotorOutput {
|
||||
pub(crate) fn new(
|
||||
rcc: &stm32f722::RCC,
|
||||
tim3: stm32f722::TIM3,
|
||||
tim5: stm32f722::TIM5,
|
||||
protocol: MotorProtocol,
|
||||
) -> Self {
|
||||
let transport = match protocol {
|
||||
MotorProtocol::Pwm(config) => {
|
||||
ACTIVE_TRANSPORT.store(TRANSPORT_PWM, Ordering::Release);
|
||||
let _ = tim5;
|
||||
Transport::Pwm(Tim3Pwm::new(rcc, tim3, config))
|
||||
}
|
||||
MotorProtocol::Dshot(config) => {
|
||||
ACTIVE_TRANSPORT.store(TRANSPORT_DSHOT, Ordering::Release);
|
||||
Transport::Dshot(Tim3Dshot::new(rcc, tim3, tim5, config))
|
||||
}
|
||||
};
|
||||
Self { transport }
|
||||
}
|
||||
|
||||
/// Services the stable TIM3 interrupt route.
|
||||
pub(crate) fn service_tim3_interrupt() {
|
||||
if ACTIVE_TRANSPORT.load(Ordering::Acquire) == TRANSPORT_PWM {
|
||||
Tim3Pwm::on_tim3_update();
|
||||
}
|
||||
}
|
||||
|
||||
/// Services the stable TIM5 interrupt route.
|
||||
pub(crate) fn service_tim5_interrupt() {
|
||||
if ACTIVE_TRANSPORT.load(Ordering::Acquire) == TRANSPORT_DSHOT {
|
||||
Tim3Dshot::on_tim5_update();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl MotorOutput for KakuteMotorOutput {
|
||||
fn arm(&mut self) {
|
||||
match &mut self.transport {
|
||||
Transport::Pwm(output) => output.arm(),
|
||||
Transport::Dshot(output) => output.arm(),
|
||||
}
|
||||
}
|
||||
|
||||
fn disarm(&mut self) {
|
||||
match &mut self.transport {
|
||||
Transport::Pwm(output) => output.disarm(),
|
||||
Transport::Dshot(output) => output.disarm(),
|
||||
}
|
||||
}
|
||||
|
||||
fn is_armed(&self) -> bool {
|
||||
match &self.transport {
|
||||
Transport::Pwm(output) => output.is_armed(),
|
||||
Transport::Dshot(output) => output.is_armed(),
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,403 @@
|
||||
use core::sync::atomic::{AtomicBool, Ordering};
|
||||
|
||||
use kaptan_actuator::{
|
||||
DshotConfig,
|
||||
dshot::{ArmSequence, FRAME_SYMBOLS, FrameValue, Throttle, Timing, symbols},
|
||||
};
|
||||
use stm32f7::stm32f722;
|
||||
|
||||
const APB1_TIMER_CLOCK_HZ: u32 = 108_000_000;
|
||||
const DSHOT_CHANNELS: usize = 4;
|
||||
const DSHOT_BUFFER_LENGTH: usize = FRAME_SYMBOLS * DSHOT_CHANNELS;
|
||||
const TIM3_CHANNEL_ENABLE: u32 = 0x1111;
|
||||
const PWM_MODE_1_WITH_PRELOAD: u32 = 0x6868;
|
||||
const TIM3_DMAR_CCR1_OFFSET: u8 = 13;
|
||||
const DEFAULT_ARMED_IDLE_THROTTLE: Throttle = match Throttle::new(48) {
|
||||
Some(throttle) => throttle,
|
||||
None => panic!("valid DShot throttle"),
|
||||
};
|
||||
|
||||
// These static buffers live in DMA-visible SRAM. They are initialized before
|
||||
// DMA is enabled and are subsequently read-only to the DMA controller.
|
||||
static mut DSHOT_ZERO_FRAME: [u32; DSHOT_BUFFER_LENGTH] = [0; DSHOT_BUFFER_LENGTH];
|
||||
static mut DSHOT_ARMED_IDLE_FRAME: [u32; DSHOT_BUFFER_LENGTH] = [0; DSHOT_BUFFER_LENGTH];
|
||||
static ARM_SEQUENCE: ArmSequence = ArmSequence::new(DEFAULT_ARMED_IDLE_THROTTLE, 0);
|
||||
static DMA_RUNNING: AtomicBool = AtomicBool::new(false);
|
||||
|
||||
pub(super) struct Tim3Dshot {
|
||||
timer: stm32f722::TIM3,
|
||||
dma_available: bool,
|
||||
}
|
||||
|
||||
impl Tim3Dshot {
|
||||
pub(super) fn new(
|
||||
rcc: &stm32f722::RCC,
|
||||
timer: stm32f722::TIM3,
|
||||
scheduler: stm32f722::TIM5,
|
||||
config: DshotConfig,
|
||||
) -> Self {
|
||||
let rate = config.rate();
|
||||
let dshot_timer_hz = rate.timer_base_hz();
|
||||
let timing = match Timing::from_timer_clock(dshot_timer_hz, rate) {
|
||||
Some(timing) => timing,
|
||||
None => unreachable!(),
|
||||
};
|
||||
let timer_prescaler = APB1_TIMER_CLOCK_HZ / dshot_timer_hz;
|
||||
assert!(APB1_TIMER_CLOCK_HZ.is_multiple_of(dshot_timer_hz));
|
||||
ARM_SEQUENCE
|
||||
.configure_zero_frames(rate.frames_for_millis(config.arming_duration().milliseconds()));
|
||||
ARM_SEQUENCE.configure_armed_throttle(config.armed_idle_throttle());
|
||||
|
||||
rcc.ahb1enr().modify(|_, write| write.gpioben().set_bit());
|
||||
rcc.ahb1enr().modify(|_, write| write.dma1en().set_bit());
|
||||
rcc.apb1rstr().modify(|_, write| write.tim3rst().set_bit());
|
||||
rcc.apb1rstr()
|
||||
.modify(|_, write| write.tim3rst().clear_bit());
|
||||
rcc.apb1enr().modify(|_, write| write.tim3en().set_bit());
|
||||
|
||||
timer
|
||||
.cr1()
|
||||
.modify(|_, write| write.cen().clear_bit().arpe().enabled());
|
||||
timer.dier().modify(|_, write| write.ude().disabled());
|
||||
timer.ccer().write(|write| unsafe { write.bits(0) });
|
||||
timer
|
||||
.psc()
|
||||
.write(|write| unsafe { write.psc().bits((timer_prescaler - 1) as u16) });
|
||||
timer
|
||||
.arr()
|
||||
.write(|write| unsafe { write.bits(u32::from(timing.bit_ticks() - 1)) });
|
||||
timer
|
||||
.ccmr1_output()
|
||||
.write(|write| unsafe { write.bits(PWM_MODE_1_WITH_PRELOAD) });
|
||||
timer
|
||||
.ccmr2_output()
|
||||
.write(|write| unsafe { write.bits(PWM_MODE_1_WITH_PRELOAD) });
|
||||
timer.dcr().write(|write| unsafe {
|
||||
write
|
||||
.dba()
|
||||
.bits(TIM3_DMAR_CCR1_OFFSET)
|
||||
.dbl()
|
||||
.bits((DSHOT_CHANNELS - 1) as u8)
|
||||
});
|
||||
timer.egr().write(|write| write.ug().set_bit());
|
||||
let dma_available = disable_dma_stream();
|
||||
initialize_dma_frames(timing, config.armed_idle_throttle());
|
||||
|
||||
rcc.apb1rstr().modify(|_, write| write.tim5rst().set_bit());
|
||||
rcc.apb1rstr()
|
||||
.modify(|_, write| write.tim5rst().clear_bit());
|
||||
rcc.apb1enr().modify(|_, write| write.tim5en().set_bit());
|
||||
scheduler
|
||||
.psc()
|
||||
.write(|write| unsafe { write.psc().bits(0) });
|
||||
scheduler
|
||||
.arr()
|
||||
.write(|write| unsafe { write.bits(APB1_TIMER_CLOCK_HZ / rate.output_rate_hz() - 1) });
|
||||
scheduler.egr().write(|write| write.ug().set_bit());
|
||||
scheduler.sr().modify(|_, write| write.uif().clear());
|
||||
scheduler.dier().write(|write| write.uie().disabled());
|
||||
scheduler.cr1().write(|write| write.cen().clear_bit());
|
||||
configure_gpio_low();
|
||||
|
||||
Self {
|
||||
timer,
|
||||
dma_available,
|
||||
}
|
||||
}
|
||||
|
||||
pub(super) fn arm(&mut self) {
|
||||
if ARM_SEQUENCE.is_armed() {
|
||||
return;
|
||||
}
|
||||
if !self.dma_available {
|
||||
self.dma_available = disable_dma_stream();
|
||||
if !self.dma_available {
|
||||
return;
|
||||
}
|
||||
}
|
||||
ARM_SEQUENCE.arm();
|
||||
if !self.start_stream(zero_frame()) {
|
||||
ARM_SEQUENCE.disarm();
|
||||
return;
|
||||
}
|
||||
let scheduler = unsafe { &*stm32f722::TIM5::ptr() };
|
||||
scheduler.sr().modify(|_, write| write.uif().clear());
|
||||
scheduler.dier().modify(|_, write| write.uie().enabled());
|
||||
scheduler.cr1().modify(|_, write| write.cen().set_bit());
|
||||
}
|
||||
|
||||
pub(super) fn disarm(&mut self) {
|
||||
ARM_SEQUENCE.disarm();
|
||||
let scheduler = unsafe { &*stm32f722::TIM5::ptr() };
|
||||
scheduler.cr1().modify(|_, write| write.cen().clear_bit());
|
||||
scheduler.dier().modify(|_, write| write.uie().disabled());
|
||||
self.stop_stream();
|
||||
self.timer.ccer().write(|write| unsafe { write.bits(0) });
|
||||
configure_gpio_low();
|
||||
}
|
||||
|
||||
pub(super) fn is_armed(&self) -> bool {
|
||||
ARM_SEQUENCE.is_armed()
|
||||
}
|
||||
|
||||
fn start_stream(&mut self, frame: *const u32) -> bool {
|
||||
self.timer.cr1().modify(|_, write| write.cen().clear_bit());
|
||||
DMA_RUNNING.store(false, Ordering::Release);
|
||||
if !configure_dma_stream(frame) {
|
||||
return false;
|
||||
}
|
||||
self.timer
|
||||
.ccer()
|
||||
.write(|write| unsafe { write.bits(TIM3_CHANNEL_ENABLE) });
|
||||
configure_gpio_alternate_function();
|
||||
self.timer.egr().write(|write| write.ug().set_bit());
|
||||
DMA_RUNNING.store(true, Ordering::Release);
|
||||
dma_stream().cr().modify(|_, write| write.en().enabled());
|
||||
self.timer.dier().modify(|_, write| write.ude().enabled());
|
||||
self.timer.cr1().modify(|_, write| write.cen().set_bit());
|
||||
true
|
||||
}
|
||||
|
||||
fn stop_stream(&mut self) {
|
||||
self.timer.cr1().modify(|_, write| write.cen().clear_bit());
|
||||
DMA_RUNNING.store(false, Ordering::Release);
|
||||
self.timer.dier().modify(|_, write| write.ude().disabled());
|
||||
let _ = disable_dma_stream();
|
||||
}
|
||||
|
||||
pub(super) fn on_tim5_update() {
|
||||
let scheduler = unsafe { &*stm32f722::TIM5::ptr() };
|
||||
scheduler.sr().modify(|_, write| write.uif().clear());
|
||||
let timer = unsafe { &*stm32f722::TIM3::ptr() };
|
||||
timer.dier().modify(|_, write| write.ude().disabled());
|
||||
if !disable_dma_stream() {
|
||||
return;
|
||||
}
|
||||
if !DMA_RUNNING.load(Ordering::Acquire) {
|
||||
return;
|
||||
}
|
||||
let frame = match ARM_SEQUENCE.next_frame() {
|
||||
Some(FrameValue::Stop) => zero_frame(),
|
||||
Some(FrameValue::Throttle(_)) => armed_idle_frame(),
|
||||
None => return,
|
||||
};
|
||||
dma_stream()
|
||||
.ndtr()
|
||||
.write(|write| unsafe { write.bits(DSHOT_BUFFER_LENGTH as u32) });
|
||||
dma_stream()
|
||||
.m0ar()
|
||||
.write(|write| unsafe { write.bits(frame as u32) });
|
||||
dma_stream().cr().modify(|_, write| write.en().enabled());
|
||||
timer.dier().modify(|_, write| write.ude().enabled());
|
||||
}
|
||||
}
|
||||
|
||||
fn initialize_dma_frames(timing: Timing, armed_idle: Throttle) {
|
||||
write_dma_frame(
|
||||
core::ptr::addr_of_mut!(DSHOT_ZERO_FRAME).cast(),
|
||||
FrameValue::Stop,
|
||||
timing,
|
||||
);
|
||||
write_dma_frame(
|
||||
core::ptr::addr_of_mut!(DSHOT_ARMED_IDLE_FRAME).cast(),
|
||||
FrameValue::Throttle(armed_idle),
|
||||
timing,
|
||||
);
|
||||
}
|
||||
|
||||
fn write_dma_frame(destination: *mut u32, value: FrameValue, timing: Timing) {
|
||||
let symbols = symbols(
|
||||
value.packet(),
|
||||
timing.zero_high_ticks(),
|
||||
timing.one_high_ticks(),
|
||||
);
|
||||
let mut bit = 0;
|
||||
while bit < FRAME_SYMBOLS {
|
||||
let mut channel = 0;
|
||||
while channel < DSHOT_CHANNELS {
|
||||
unsafe {
|
||||
destination
|
||||
.add(bit * DSHOT_CHANNELS + channel)
|
||||
.write(symbols[bit] as u32)
|
||||
};
|
||||
channel += 1;
|
||||
}
|
||||
bit += 1;
|
||||
}
|
||||
}
|
||||
|
||||
fn zero_frame() -> *const u32 {
|
||||
core::ptr::addr_of!(DSHOT_ZERO_FRAME).cast()
|
||||
}
|
||||
fn armed_idle_frame() -> *const u32 {
|
||||
core::ptr::addr_of!(DSHOT_ARMED_IDLE_FRAME).cast()
|
||||
}
|
||||
|
||||
fn dma() -> &'static stm32f722::dma2::RegisterBlock {
|
||||
unsafe { &*stm32f722::DMA1::ptr() }
|
||||
}
|
||||
fn dma_stream() -> &'static stm32f722::dma2::ST {
|
||||
dma().st(2)
|
||||
}
|
||||
|
||||
fn disable_dma_stream() -> bool {
|
||||
dma_stream().cr().modify(|_, write| write.en().disabled());
|
||||
let mut spins = 1_024;
|
||||
while dma_stream().cr().read().en().is_enabled() && spins != 0 {
|
||||
spins -= 1;
|
||||
}
|
||||
if dma_stream().cr().read().en().is_enabled() {
|
||||
return false;
|
||||
}
|
||||
dma().lifcr().write(|write| {
|
||||
write
|
||||
.cfeif2()
|
||||
.clear()
|
||||
.cdmeif2()
|
||||
.clear()
|
||||
.cteif2()
|
||||
.clear()
|
||||
.chtif2()
|
||||
.clear()
|
||||
.ctcif2()
|
||||
.clear()
|
||||
});
|
||||
true
|
||||
}
|
||||
|
||||
fn configure_dma_stream(frame: *const u32) -> bool {
|
||||
if !disable_dma_stream() {
|
||||
return false;
|
||||
}
|
||||
dma_stream().cr().write(|write| unsafe {
|
||||
write
|
||||
.chsel()
|
||||
.bits(5)
|
||||
.dir()
|
||||
.memory_to_peripheral()
|
||||
.pl()
|
||||
.very_high()
|
||||
.minc()
|
||||
.incremented()
|
||||
.pinc()
|
||||
.fixed()
|
||||
.msize()
|
||||
.bits32()
|
||||
.psize()
|
||||
.bits32()
|
||||
.circ()
|
||||
.disabled()
|
||||
.tcie()
|
||||
.disabled()
|
||||
.teie()
|
||||
.disabled()
|
||||
.dmeie()
|
||||
.disabled()
|
||||
});
|
||||
dma_stream()
|
||||
.ndtr()
|
||||
.write(|write| unsafe { write.bits(DSHOT_BUFFER_LENGTH as u32) });
|
||||
dma_stream()
|
||||
.par()
|
||||
.write(|write| unsafe { write.bits(tim3_dmar_address()) });
|
||||
dma_stream()
|
||||
.m0ar()
|
||||
.write(|write| unsafe { write.bits(frame as u32) });
|
||||
dma_stream()
|
||||
.fcr()
|
||||
.write(|write| unsafe { write.fth().bits(3).dmdis().set_bit() });
|
||||
true
|
||||
}
|
||||
|
||||
fn tim3_dmar_address() -> u32 {
|
||||
unsafe { (&*stm32f722::TIM3::ptr()).dmar().as_ptr() as u32 }
|
||||
}
|
||||
|
||||
fn port() -> &'static stm32f722::gpiob::RegisterBlock {
|
||||
unsafe { &*stm32f722::GPIOB::ptr() }
|
||||
}
|
||||
|
||||
fn configure_gpio_low() {
|
||||
let port = port();
|
||||
port.moder().modify(|_, write| {
|
||||
write
|
||||
.moder0()
|
||||
.output()
|
||||
.moder1()
|
||||
.output()
|
||||
.moder4()
|
||||
.output()
|
||||
.moder5()
|
||||
.output()
|
||||
});
|
||||
port.otyper().modify(|_, write| {
|
||||
write
|
||||
.ot0()
|
||||
.push_pull()
|
||||
.ot1()
|
||||
.push_pull()
|
||||
.ot4()
|
||||
.push_pull()
|
||||
.ot5()
|
||||
.push_pull()
|
||||
});
|
||||
port.pupdr().modify(|_, write| {
|
||||
write
|
||||
.pupdr0()
|
||||
.floating()
|
||||
.pupdr1()
|
||||
.floating()
|
||||
.pupdr4()
|
||||
.floating()
|
||||
.pupdr5()
|
||||
.floating()
|
||||
});
|
||||
port.bsrr().write(|write| {
|
||||
write
|
||||
.br0()
|
||||
.set_bit()
|
||||
.br1()
|
||||
.set_bit()
|
||||
.br4()
|
||||
.set_bit()
|
||||
.br5()
|
||||
.set_bit()
|
||||
});
|
||||
}
|
||||
|
||||
fn configure_gpio_alternate_function() {
|
||||
let port = port();
|
||||
port.afrl().modify(|_, write| unsafe {
|
||||
write
|
||||
.afrl0()
|
||||
.bits(2)
|
||||
.afrl1()
|
||||
.bits(2)
|
||||
.afrl4()
|
||||
.bits(2)
|
||||
.afrl5()
|
||||
.bits(2)
|
||||
});
|
||||
port.moder().modify(|_, write| {
|
||||
write
|
||||
.moder0()
|
||||
.alternate()
|
||||
.moder1()
|
||||
.alternate()
|
||||
.moder4()
|
||||
.alternate()
|
||||
.moder5()
|
||||
.alternate()
|
||||
});
|
||||
port.ospeedr().modify(|_, write| {
|
||||
write
|
||||
.ospeedr0()
|
||||
.high_speed()
|
||||
.ospeedr1()
|
||||
.high_speed()
|
||||
.ospeedr4()
|
||||
.high_speed()
|
||||
.ospeedr5()
|
||||
.high_speed()
|
||||
});
|
||||
}
|
||||
@@ -0,0 +1,183 @@
|
||||
use core::sync::atomic::{AtomicU16, Ordering};
|
||||
|
||||
use kaptan_actuator::{
|
||||
PwmConfig,
|
||||
pwm::{ArmSequence, FrameValue, PulseWidth},
|
||||
};
|
||||
use stm32f7::stm32f722;
|
||||
|
||||
const APB1_TIMER_CLOCK_HZ: u32 = 108_000_000;
|
||||
const TIMER_TICK_HZ: u32 = 1_000_000;
|
||||
const PWM_CHANNEL_ENABLE: u32 = 0x1111;
|
||||
const PWM_MODE_1_WITH_PRELOAD: u32 = 0x6868;
|
||||
const DEFAULT_MINIMUM_PULSE: PulseWidth = match PulseWidth::new(1_000) {
|
||||
Some(pulse) => pulse,
|
||||
None => panic!("valid PWM pulse"),
|
||||
};
|
||||
const DEFAULT_ARMED_IDLE_PULSE: PulseWidth = match PulseWidth::new(1_000) {
|
||||
Some(pulse) => pulse,
|
||||
None => panic!("valid PWM pulse"),
|
||||
};
|
||||
|
||||
static ARM_SEQUENCE: ArmSequence = ArmSequence::new(DEFAULT_ARMED_IDLE_PULSE, 0);
|
||||
static MINIMUM_PULSE: AtomicU16 = AtomicU16::new(DEFAULT_MINIMUM_PULSE.microseconds());
|
||||
|
||||
pub(super) struct Tim3Pwm {
|
||||
timer: stm32f722::TIM3,
|
||||
minimum_pulse: PulseWidth,
|
||||
}
|
||||
|
||||
impl Tim3Pwm {
|
||||
pub(super) fn new(rcc: &stm32f722::RCC, timer: stm32f722::TIM3, config: PwmConfig) -> Self {
|
||||
let period_us = match config.frame_rate().period_microseconds() {
|
||||
Some(period) => period,
|
||||
None => unreachable!(),
|
||||
};
|
||||
ARM_SEQUENCE.configure_minimum_frames(
|
||||
config
|
||||
.frame_rate()
|
||||
.frames_for_millis(config.arming_duration().milliseconds()),
|
||||
);
|
||||
ARM_SEQUENCE.configure_armed_idle(config.armed_idle_pulse());
|
||||
MINIMUM_PULSE.store(config.minimum_pulse().microseconds(), Ordering::Release);
|
||||
|
||||
rcc.ahb1enr().modify(|_, write| write.gpioben().set_bit());
|
||||
rcc.apb1rstr().modify(|_, write| write.tim3rst().set_bit());
|
||||
rcc.apb1rstr()
|
||||
.modify(|_, write| write.tim3rst().clear_bit());
|
||||
rcc.apb1enr().modify(|_, write| write.tim3en().set_bit());
|
||||
|
||||
configure_gpio_alternate_function();
|
||||
timer
|
||||
.cr1()
|
||||
.write(|write| write.cen().clear_bit().arpe().enabled());
|
||||
timer.psc().write(|write| unsafe {
|
||||
write
|
||||
.psc()
|
||||
.bits((APB1_TIMER_CLOCK_HZ / TIMER_TICK_HZ - 1) as u16)
|
||||
});
|
||||
timer
|
||||
.arr()
|
||||
.write(|write| unsafe { write.bits(u32::from(period_us - 1)) });
|
||||
timer
|
||||
.ccmr1_output()
|
||||
.write(|write| unsafe { write.bits(PWM_MODE_1_WITH_PRELOAD) });
|
||||
timer
|
||||
.ccmr2_output()
|
||||
.write(|write| unsafe { write.bits(PWM_MODE_1_WITH_PRELOAD) });
|
||||
write_pulse(&timer, config.minimum_pulse());
|
||||
timer
|
||||
.ccer()
|
||||
.write(|write| unsafe { write.bits(PWM_CHANNEL_ENABLE) });
|
||||
timer.egr().write(|write| write.ug().set_bit());
|
||||
timer.sr().modify(|_, write| write.uif().clear());
|
||||
timer.dier().write(|write| write.uie().enabled());
|
||||
timer.cr1().modify(|_, write| write.cen().set_bit());
|
||||
|
||||
Self {
|
||||
timer,
|
||||
minimum_pulse: config.minimum_pulse(),
|
||||
}
|
||||
}
|
||||
|
||||
pub(super) fn arm(&mut self) {
|
||||
ARM_SEQUENCE.arm();
|
||||
}
|
||||
|
||||
pub(super) fn disarm(&mut self) {
|
||||
ARM_SEQUENCE.disarm();
|
||||
write_pulse(&self.timer, self.minimum_pulse);
|
||||
}
|
||||
|
||||
pub(super) fn is_armed(&self) -> bool {
|
||||
ARM_SEQUENCE.is_armed()
|
||||
}
|
||||
|
||||
pub(super) fn on_tim3_update() {
|
||||
let timer = unsafe { &*stm32f722::TIM3::ptr() };
|
||||
timer.sr().modify(|_, write| write.uif().clear());
|
||||
let pulse = match ARM_SEQUENCE.next_frame() {
|
||||
Some(FrameValue::ArmedIdle(pulse)) => pulse,
|
||||
Some(FrameValue::Minimum) | None => configured_minimum_pulse(),
|
||||
};
|
||||
write_pulse(timer, pulse);
|
||||
}
|
||||
}
|
||||
|
||||
fn configured_minimum_pulse() -> PulseWidth {
|
||||
match PulseWidth::new(MINIMUM_PULSE.load(Ordering::Acquire)) {
|
||||
Some(pulse) => pulse,
|
||||
None => unreachable!(),
|
||||
}
|
||||
}
|
||||
|
||||
fn write_pulse(timer: &stm32f722::tim3::RegisterBlock, pulse: PulseWidth) {
|
||||
let ticks = u32::from(pulse.microseconds());
|
||||
timer.ccr1().write(|write| unsafe { write.bits(ticks) });
|
||||
timer.ccr2().write(|write| unsafe { write.bits(ticks) });
|
||||
timer.ccr3().write(|write| unsafe { write.bits(ticks) });
|
||||
timer.ccr4().write(|write| unsafe { write.bits(ticks) });
|
||||
}
|
||||
|
||||
fn port() -> &'static stm32f722::gpiob::RegisterBlock {
|
||||
unsafe { &*stm32f722::GPIOB::ptr() }
|
||||
}
|
||||
|
||||
fn configure_gpio_alternate_function() {
|
||||
let port = port();
|
||||
port.afrl().modify(|_, write| unsafe {
|
||||
write
|
||||
.afrl0()
|
||||
.bits(2)
|
||||
.afrl1()
|
||||
.bits(2)
|
||||
.afrl4()
|
||||
.bits(2)
|
||||
.afrl5()
|
||||
.bits(2)
|
||||
});
|
||||
port.moder().modify(|_, write| {
|
||||
write
|
||||
.moder0()
|
||||
.alternate()
|
||||
.moder1()
|
||||
.alternate()
|
||||
.moder4()
|
||||
.alternate()
|
||||
.moder5()
|
||||
.alternate()
|
||||
});
|
||||
port.otyper().modify(|_, write| {
|
||||
write
|
||||
.ot0()
|
||||
.push_pull()
|
||||
.ot1()
|
||||
.push_pull()
|
||||
.ot4()
|
||||
.push_pull()
|
||||
.ot5()
|
||||
.push_pull()
|
||||
});
|
||||
port.ospeedr().modify(|_, write| {
|
||||
write
|
||||
.ospeedr0()
|
||||
.high_speed()
|
||||
.ospeedr1()
|
||||
.high_speed()
|
||||
.ospeedr4()
|
||||
.high_speed()
|
||||
.ospeedr5()
|
||||
.high_speed()
|
||||
});
|
||||
port.pupdr().modify(|_, write| {
|
||||
write
|
||||
.pupdr0()
|
||||
.floating()
|
||||
.pupdr1()
|
||||
.floating()
|
||||
.pupdr4()
|
||||
.floating()
|
||||
.pupdr5()
|
||||
.floating()
|
||||
});
|
||||
}
|
||||
@@ -1,16 +1,34 @@
|
||||
//! Kakute F7 Mini V3 static hardware selections.
|
||||
|
||||
use kaptan_actuator::{ArmingDuration, MotorProtocol};
|
||||
use kaptan_drivers::mpu6000::{
|
||||
AccelerometerRange, DigitalLowPassFilter, GyroscopeRange, Mpu6000Config,
|
||||
};
|
||||
use kaptan_stm32f7::stm32f722::Tim2PeriodicConfig;
|
||||
|
||||
/// Core frequency in whole megahertz after selecting the HSI16 board profile.
|
||||
pub const CORE_CLOCK_MHZ: u32 = 16;
|
||||
/// APB2 frequency driving USART1 in the HSI16 board profile.
|
||||
pub const USART1_CLOCK_HZ: u32 = 16_000_000;
|
||||
/// Core frequency in whole megahertz after selecting the PLL216 board profile.
|
||||
pub const CORE_CLOCK_MHZ: u32 = 216;
|
||||
/// APB2 frequency driving USART1 in the PLL216 board profile.
|
||||
pub const USART1_CLOCK_HZ: u32 = 108_000_000;
|
||||
/// Telemetry UART wire rate.
|
||||
pub const USART1_BAUD: u32 = 115_200;
|
||||
/// ESC protocol selected for this board's M1–M4 output route.
|
||||
pub const MOTOR_PROTOCOL: MotorProtocol = MotorProtocol::Dshot(
|
||||
match kaptan_actuator::DshotConfig::new(
|
||||
kaptan_actuator::dshot::Rate::Dshot600,
|
||||
match kaptan_actuator::dshot::Throttle::new(300) {
|
||||
Some(throttle) => throttle,
|
||||
None => panic!("armed idle throttle must be valid"),
|
||||
},
|
||||
match ArmingDuration::from_millis(2_000) {
|
||||
Some(duration) => duration,
|
||||
None => panic!("arming duration must be valid"),
|
||||
},
|
||||
) {
|
||||
Some(config) => config,
|
||||
None => panic!("DShot motor protocol must be valid"),
|
||||
},
|
||||
);
|
||||
/// MAVLink sender system identifier.
|
||||
pub const MAVLINK_SYSTEM_ID: u8 = 1;
|
||||
/// MAVLink sender component identifier.
|
||||
@@ -24,11 +42,11 @@ pub const MPU6000_CONFIG: Mpu6000Config = Mpu6000Config {
|
||||
sample_rate_divider: 9,
|
||||
};
|
||||
|
||||
/// Ten-hertz TIM2 update from the 16 MHz HSI16 board profile.
|
||||
/// One-hundred-hertz IMU service update from the 108 MHz APB1 timer clock.
|
||||
///
|
||||
/// The runtime emits its one-hertz heartbeat every tenth tick and uses the
|
||||
/// intermediate ticks for the MPU6000 reset-settling interval and DWT epoch.
|
||||
pub const HEARTBEAT_TIMER_CONFIG: Tim2PeriodicConfig = Tim2PeriodicConfig {
|
||||
prescaler: 15_999,
|
||||
/// This matches the MPU6000 output data rate selected by `MPU6000_CONFIG`.
|
||||
/// The runtime emits its one-hertz heartbeat every hundredth tick.
|
||||
pub const IMU_SERVICE_TIMER_CONFIG: Tim2PeriodicConfig = Tim2PeriodicConfig {
|
||||
prescaler: 10_799,
|
||||
auto_reload: 99,
|
||||
};
|
||||
@@ -7,6 +7,7 @@ use cortex_m_rt as _;
|
||||
use kaptan_runtime::LatestImuMailbox;
|
||||
use panic_halt as _;
|
||||
|
||||
mod actuator;
|
||||
mod board;
|
||||
mod routes;
|
||||
|
||||
@@ -14,6 +15,7 @@ const IMU_EVENT_DRDY: u8 = 1 << 0;
|
||||
const IMU_EVENT_TRANSFER_COMPLETE: u8 = 1 << 1;
|
||||
const IMU_EVENT_TRANSFER_FAULT: u8 = 1 << 2;
|
||||
const IMU_EVENT_TIMER: u8 = 1 << 3;
|
||||
const IMU_EVENT_START: u8 = 1 << 4;
|
||||
const MAVLINK_EVENT_SAMPLE: u8 = 1 << 0;
|
||||
const MAVLINK_EVENT_HEARTBEAT: u8 = 1 << 1;
|
||||
const MAVLINK_EVENT_DMA: u8 = 1 << 2;
|
||||
@@ -26,21 +28,25 @@ static MAVLINK_QUEUED: AtomicBool = AtomicBool::new(false);
|
||||
static SAMPLES: LatestImuMailbox = LatestImuMailbox::new();
|
||||
static UART_COMPLETION: AtomicU8 = AtomicU8::new(0);
|
||||
|
||||
#[rtic::app(device = stm32f7::stm32f722, peripherals = true, dispatchers = [TIM3, TIM5])]
|
||||
#[rtic::app(device = stm32f7::stm32f722, peripherals = true, dispatchers = [TIM6_DAC, TIM7])]
|
||||
mod app {
|
||||
use super::{
|
||||
IMU_EVENT_DRDY, IMU_EVENT_TIMER, IMU_EVENT_TRANSFER_COMPLETE, IMU_EVENT_TRANSFER_FAULT,
|
||||
IMU_EVENTS, IMU_QUEUED, MAVLINK_EVENT_DMA, MAVLINK_EVENT_HEARTBEAT, MAVLINK_EVENT_RECEIVED,
|
||||
MAVLINK_EVENT_SAMPLE, MAVLINK_EVENTS, MAVLINK_QUEUED, Ordering, SAMPLES, UART_COMPLETION,
|
||||
IMU_EVENT_DRDY, IMU_EVENT_START, IMU_EVENT_TIMER, IMU_EVENT_TRANSFER_COMPLETE,
|
||||
IMU_EVENT_TRANSFER_FAULT, IMU_EVENTS, IMU_QUEUED, MAVLINK_EVENT_DMA,
|
||||
MAVLINK_EVENT_HEARTBEAT, MAVLINK_EVENT_RECEIVED, MAVLINK_EVENT_SAMPLE, MAVLINK_EVENTS,
|
||||
MAVLINK_QUEUED, Ordering, SAMPLES, UART_COMPLETION,
|
||||
};
|
||||
use crate::actuator::KakuteMotorOutput;
|
||||
use crate::board::{
|
||||
CORE_CLOCK_MHZ, HEARTBEAT_TIMER_CONFIG, MAVLINK_COMPONENT_ID, MAVLINK_SYSTEM_ID,
|
||||
MPU6000_CONFIG, USART1_BAUD, USART1_CLOCK_HZ,
|
||||
CORE_CLOCK_MHZ, IMU_SERVICE_TIMER_CONFIG, MAVLINK_COMPONENT_ID, MAVLINK_SYSTEM_ID,
|
||||
MOTOR_PROTOCOL, MPU6000_CONFIG, USART1_BAUD, USART1_CLOCK_HZ,
|
||||
};
|
||||
use crate::routes::KakuteMpu6000SpiRoute;
|
||||
use kaptan_actuator::MotorOutput;
|
||||
use kaptan_dma::TxCompletion;
|
||||
use kaptan_drivers::mpu6000::Mpu6000;
|
||||
use kaptan_mavlink::{
|
||||
arm::{ArmRequest, decode_arm_request},
|
||||
encoder::MavlinkEncoder,
|
||||
receiver::MavlinkReceiver,
|
||||
transmitter::{FrameClass, MavlinkTransmitter},
|
||||
@@ -50,7 +56,7 @@ mod app {
|
||||
use kaptan_stm32f7::stm32f722::{
|
||||
ActiveLowLedPd2, DcacheDisabledDmaMemory, DwtMicrosecondClock, Spi1, Tim2Periodic,
|
||||
Usart1Tx, Usart1TxDma, configure_exti4_pin_pa4, configure_spi1_pins_pa5_pa6_pa7,
|
||||
configure_usart1_rx_pin_pa10, configure_usart1_tx_pin_pa9, select_hsi16_profile,
|
||||
configure_usart1_rx_pin_pa10, configure_usart1_tx_pin_pa9, select_pll216_profile,
|
||||
};
|
||||
use stm32f7::stm32f722;
|
||||
|
||||
@@ -65,15 +71,16 @@ mod app {
|
||||
mavlink_encoder: MavlinkEncoder,
|
||||
mavlink_transmitter: MavlinkTransmitter<DcacheDisabledDmaMemory, Usart1TxDma, 4>,
|
||||
led: ActiveLowLedPd2,
|
||||
heartbeat_timer: Tim2Periodic,
|
||||
imu_service_timer: Tim2Periodic,
|
||||
heartbeat_divider: u8,
|
||||
observed_sample: u32,
|
||||
esc: KakuteMotorOutput,
|
||||
}
|
||||
|
||||
#[init]
|
||||
fn init(mut context: init::Context) -> (Shared, Local) {
|
||||
let device = context.device;
|
||||
if select_hsi16_profile(&device.RCC).is_err() {
|
||||
if select_pll216_profile(&device.RCC, &device.PWR, &device.FLASH).is_err() {
|
||||
safe_idle();
|
||||
}
|
||||
context.core.SCB.disable_dcache(&mut context.core.CPUID);
|
||||
@@ -89,12 +96,10 @@ mod app {
|
||||
configure_exti4_pin_pa4(&device.GPIOA);
|
||||
configure_usart1_tx_pin_pa9(&device.GPIOA);
|
||||
configure_usart1_rx_pin_pa10(&device.GPIOA);
|
||||
let esc = KakuteMotorOutput::new(&device.RCC, device.TIM3, device.TIM5, MOTOR_PROTOCOL);
|
||||
let spi = Spi1::new(&device.RCC, device.SPI1, device.DMA2);
|
||||
let route = KakuteMpu6000SpiRoute::new(&device.RCC, spi, device.GPIOB);
|
||||
let mut imu = ImuService::new(clock, Mpu6000::new(route, MPU6000_CONFIG));
|
||||
if imu.begin().is_err() {
|
||||
safe_idle();
|
||||
}
|
||||
let imu = ImuService::new(clock, Mpu6000::new(route, MPU6000_CONFIG));
|
||||
|
||||
let uart = match Usart1Tx::new(&device.RCC, device.USART1, USART1_CLOCK_HZ, USART1_BAUD) {
|
||||
Ok(uart) => uart,
|
||||
@@ -106,6 +111,10 @@ mod app {
|
||||
MavlinkTransmitter::new(memory, Usart1TxDma::new(&device.RCC, uart));
|
||||
|
||||
configure_exti4(&device.RCC, device.SYSCFG, device.EXTI);
|
||||
// The first SPI DMA transaction is submitted by `imu_worker`, after
|
||||
// RTIC has enabled the DMA and SPI IRQs. Initialization itself only
|
||||
// constructs inert hardware owners.
|
||||
IMU_EVENTS.store(IMU_EVENT_START, Ordering::Release);
|
||||
(
|
||||
Shared {
|
||||
mavlink_receiver: MavlinkReceiver::new(),
|
||||
@@ -115,13 +124,14 @@ mod app {
|
||||
mavlink_encoder: MavlinkEncoder::new(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID),
|
||||
mavlink_transmitter,
|
||||
led: ActiveLowLedPd2::new(&device.RCC, device.GPIOD),
|
||||
heartbeat_timer: Tim2Periodic::new(
|
||||
imu_service_timer: Tim2Periodic::new(
|
||||
&device.RCC,
|
||||
device.TIM2,
|
||||
HEARTBEAT_TIMER_CONFIG,
|
||||
IMU_SERVICE_TIMER_CONFIG,
|
||||
),
|
||||
heartbeat_divider: 0,
|
||||
observed_sample: 0,
|
||||
esc,
|
||||
},
|
||||
)
|
||||
}
|
||||
@@ -160,9 +170,26 @@ mod app {
|
||||
}
|
||||
}
|
||||
|
||||
#[task(binds = TIM2, priority = 4, local = [heartbeat_timer])]
|
||||
#[task(binds = SPI1, priority = 14)]
|
||||
fn spi_error(_: spi_error::Context) {
|
||||
if Spi1::acknowledge_error_interrupt() {
|
||||
schedule_imu(IMU_EVENT_TRANSFER_FAULT);
|
||||
}
|
||||
}
|
||||
|
||||
#[task(binds = TIM3, priority = 12)]
|
||||
fn motor_update(_: motor_update::Context) {
|
||||
KakuteMotorOutput::service_tim3_interrupt();
|
||||
}
|
||||
|
||||
#[task(binds = TIM5, priority = 12)]
|
||||
fn motor_scheduler(_: motor_scheduler::Context) {
|
||||
KakuteMotorOutput::service_tim5_interrupt();
|
||||
}
|
||||
|
||||
#[task(binds = TIM2, priority = 4, local = [imu_service_timer])]
|
||||
fn tim2(context: tim2::Context) {
|
||||
context.local.heartbeat_timer.acknowledge_update();
|
||||
context.local.imu_service_timer.acknowledge_update();
|
||||
schedule_imu(IMU_EVENT_TIMER);
|
||||
schedule_mavlink(MAVLINK_EVENT_HEARTBEAT);
|
||||
}
|
||||
@@ -203,7 +230,11 @@ mod app {
|
||||
|
||||
#[task(priority = 13, local = [imu])]
|
||||
async fn imu_worker(context: imu_worker::Context) {
|
||||
// TODO: Route IMU faults to a dedicated supervisor.
|
||||
let events = IMU_EVENTS.swap(0, Ordering::AcqRel);
|
||||
if events & IMU_EVENT_START != 0 {
|
||||
let _ = context.local.imu.begin();
|
||||
}
|
||||
if events & IMU_EVENT_TRANSFER_FAULT != 0 {
|
||||
let _ = context
|
||||
.local
|
||||
@@ -238,7 +269,7 @@ mod app {
|
||||
}
|
||||
}
|
||||
|
||||
#[task(priority = 9, shared = [mavlink_receiver], local = [mavlink_encoder, mavlink_transmitter, led, heartbeat_divider, observed_sample])]
|
||||
#[task(priority = 9, shared = [mavlink_receiver], local = [mavlink_encoder, mavlink_transmitter, led, heartbeat_divider, observed_sample, esc])]
|
||||
async fn mavlink_worker(mut context: mavlink_worker::Context) {
|
||||
let events = MAVLINK_EVENTS.swap(0, Ordering::AcqRel);
|
||||
if events & MAVLINK_EVENT_DMA != 0 {
|
||||
@@ -264,14 +295,14 @@ mod app {
|
||||
}
|
||||
if events & MAVLINK_EVENT_HEARTBEAT != 0 {
|
||||
*context.local.heartbeat_divider = context.local.heartbeat_divider.wrapping_add(1);
|
||||
if *context.local.heartbeat_divider == 10 {
|
||||
if *context.local.heartbeat_divider == 100 {
|
||||
*context.local.heartbeat_divider = 0;
|
||||
let _ = context.local.mavlink_transmitter.send_frame(
|
||||
FrameClass::Critical,
|
||||
context
|
||||
.local
|
||||
.mavlink_encoder
|
||||
.heartbeat(kaptan_mavlink::message::MavState::MAV_STATE_STANDBY),
|
||||
context.local.mavlink_encoder.heartbeat(
|
||||
kaptan_mavlink::message::MavState::MAV_STATE_STANDBY,
|
||||
context.local.esc.is_armed(),
|
||||
),
|
||||
);
|
||||
context.local.led.toggle();
|
||||
}
|
||||
@@ -282,11 +313,26 @@ mod app {
|
||||
.mavlink_receiver
|
||||
.lock(MavlinkReceiver::take_received)
|
||||
{
|
||||
// TODO: Echo the frame for testing.
|
||||
let _ = context
|
||||
.local
|
||||
.mavlink_transmitter
|
||||
.send_frame(FrameClass::Critical, frame);
|
||||
if let Some(request) =
|
||||
decode_arm_request(&frame, MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID)
|
||||
{
|
||||
// TODO: Acknowledge arm only after output activation is confirmed.
|
||||
let _ = context.local.mavlink_transmitter.send_frame(
|
||||
FrameClass::Critical,
|
||||
context.local.mavlink_encoder.command_ack(
|
||||
kaptan_mavlink::message::MavCmd::MAV_CMD_COMPONENT_ARM_DISARM,
|
||||
kaptan_mavlink::message::MavResult::MAV_RESULT_ACCEPTED,
|
||||
),
|
||||
);
|
||||
match request {
|
||||
ArmRequest::Arm => {
|
||||
context.local.esc.arm();
|
||||
}
|
||||
ArmRequest::Disarm => {
|
||||
context.local.esc.disarm();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
MAVLINK_QUEUED.store(false, Ordering::Release);
|
||||
|
||||
@@ -0,0 +1,102 @@
|
||||
//! Narrow MAVLink arm/disarm command decoding.
|
||||
//!
|
||||
//! This module deliberately accepts no actuator-value or motor-test command.
|
||||
//! A board must pass accepted requests to its single actuator-state owner; this
|
||||
//! decoder has no access to output hardware.
|
||||
|
||||
use mavlink::Message;
|
||||
|
||||
use crate::{MavlinkFrame, message};
|
||||
|
||||
/// A requested change to the vehicle's arming state.
|
||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||
pub enum ArmRequest {
|
||||
/// Request the armed-idle state.
|
||||
Arm,
|
||||
/// Request electrically inactive outputs.
|
||||
Disarm,
|
||||
}
|
||||
|
||||
/// Decodes a targeted MAVLink `MAV_CMD_COMPONENT_ARM_DISARM` command.
|
||||
///
|
||||
/// Only `param1` values exactly equal to `0.0` (disarm) or `1.0` (arm) are
|
||||
/// accepted. Broadcast targets are intentionally rejected: an actuator command
|
||||
/// must name this vehicle and its component explicitly.
|
||||
pub fn decode_arm_request(
|
||||
frame: &MavlinkFrame,
|
||||
system_id: u8,
|
||||
component_id: u8,
|
||||
) -> Option<ArmRequest> {
|
||||
let message = message::MavMessage::parse(
|
||||
mavlink::MavlinkVersion::V2,
|
||||
frame.message_id(),
|
||||
frame.payload(),
|
||||
)
|
||||
.ok()?;
|
||||
|
||||
let message::MavMessage::COMMAND_LONG(command) = message else {
|
||||
return None;
|
||||
};
|
||||
if command.command != message::MavCmd::MAV_CMD_COMPONENT_ARM_DISARM
|
||||
|| command.target_system != system_id
|
||||
|| command.target_component != component_id
|
||||
{
|
||||
return None;
|
||||
}
|
||||
|
||||
match command.param1 {
|
||||
0.0 => Some(ArmRequest::Disarm),
|
||||
1.0 => Some(ArmRequest::Arm),
|
||||
_ => None,
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::{ArmRequest, decode_arm_request};
|
||||
use crate::{MavlinkFrame, message};
|
||||
|
||||
fn command(target_system: u8, target_component: u8, value: f32) -> MavlinkFrame {
|
||||
let mut frame = MavlinkFrame::new();
|
||||
frame.serialize_message_data(
|
||||
message::MavHeader {
|
||||
sequence: 0,
|
||||
system_id: 42,
|
||||
component_id: 7,
|
||||
},
|
||||
&message::COMMAND_LONG_DATA {
|
||||
param1: value,
|
||||
param2: 0.0,
|
||||
param3: 0.0,
|
||||
param4: 0.0,
|
||||
param5: 0.0,
|
||||
param6: 0.0,
|
||||
param7: 0.0,
|
||||
command: message::MavCmd::MAV_CMD_COMPONENT_ARM_DISARM,
|
||||
target_system,
|
||||
target_component,
|
||||
confirmation: 0,
|
||||
},
|
||||
);
|
||||
frame
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn accepts_explicit_targeted_arm_and_disarm() {
|
||||
assert_eq!(
|
||||
decode_arm_request(&command(1, 1, 1.0), 1, 1),
|
||||
Some(ArmRequest::Arm)
|
||||
);
|
||||
assert_eq!(
|
||||
decode_arm_request(&command(1, 1, 0.0), 1, 1),
|
||||
Some(ArmRequest::Disarm)
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn rejects_broadcast_wrong_target_and_invalid_parameter() {
|
||||
assert_eq!(decode_arm_request(&command(0, 0, 1.0), 1, 1), None);
|
||||
assert_eq!(decode_arm_request(&command(2, 1, 1.0), 1, 1), None);
|
||||
assert_eq!(decode_arm_request(&command(1, 1, 0.5), 1, 1), None);
|
||||
}
|
||||
}
|
||||
@@ -37,13 +37,17 @@ impl MavlinkEncoder {
|
||||
raw
|
||||
}
|
||||
|
||||
/// Encodes a Kaptan heartbeat.
|
||||
pub fn heartbeat(&mut self, system_status: message::MavState) -> MavlinkFrame {
|
||||
/// Encodes a Kaptan heartbeat with its current arming state.
|
||||
pub fn heartbeat(&mut self, system_status: message::MavState, armed: bool) -> MavlinkFrame {
|
||||
let heartbeat = message::HEARTBEAT_DATA {
|
||||
custom_mode: 0,
|
||||
mavtype: message::MavType::MAV_TYPE_QUADROTOR,
|
||||
autopilot: message::MavAutopilot::MAV_AUTOPILOT_GENERIC,
|
||||
base_mode: message::MavModeFlag::empty(),
|
||||
base_mode: if armed {
|
||||
message::MavModeFlag::MAV_MODE_FLAG_SAFETY_ARMED
|
||||
} else {
|
||||
message::MavModeFlag::empty()
|
||||
},
|
||||
system_status,
|
||||
mavlink_version: 3,
|
||||
};
|
||||
@@ -51,6 +55,15 @@ impl MavlinkEncoder {
|
||||
self.serialize(&heartbeat)
|
||||
}
|
||||
|
||||
/// Acknowledges a supported MAVLink command.
|
||||
pub fn command_ack(
|
||||
&mut self,
|
||||
command: message::MavCmd,
|
||||
result: message::MavResult,
|
||||
) -> MavlinkFrame {
|
||||
self.serialize(&message::COMMAND_ACK_DATA { command, result })
|
||||
}
|
||||
|
||||
/// Encodes a standard `RAW_IMU` frame.
|
||||
pub fn raw_imu_snapshot(&mut self, snapshot: &ImuSnapshot) -> MavlinkFrame {
|
||||
let raw_imu = message::RAW_IMU_DATA {
|
||||
@@ -80,7 +93,7 @@ mod tests {
|
||||
#[test]
|
||||
fn heartbeat_has_the_configured_sender_and_sequence() {
|
||||
let mut encoder = MavlinkEncoder::new(42, 7);
|
||||
let frame = encoder.heartbeat(MavState::MAV_STATE_STANDBY);
|
||||
let frame = encoder.heartbeat(MavState::MAV_STATE_STANDBY, false);
|
||||
let bytes = frame.raw_bytes();
|
||||
|
||||
assert_eq!(bytes[0], 0xFD);
|
||||
@@ -95,7 +108,7 @@ mod tests {
|
||||
#[test]
|
||||
fn heartbeat_preserves_the_requested_health_state() {
|
||||
let mut encoder = MavlinkEncoder::new(1, 1);
|
||||
let frame = encoder.heartbeat(MavState::MAV_STATE_CRITICAL);
|
||||
let frame = encoder.heartbeat(MavState::MAV_STATE_CRITICAL, false);
|
||||
|
||||
assert_eq!(frame.raw_bytes()[17], MavState::MAV_STATE_CRITICAL as u8);
|
||||
}
|
||||
@@ -114,4 +127,12 @@ mod tests {
|
||||
|
||||
assert_eq!(&frame.raw_bytes()[7..10], &[27, 0, 0]);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn armed_heartbeat_sets_the_standard_armed_flag() {
|
||||
let mut encoder = MavlinkEncoder::new(1, 1);
|
||||
let frame = encoder.heartbeat(MavState::MAV_STATE_ACTIVE, true);
|
||||
|
||||
assert_ne!(frame.raw_bytes()[16] & 0x80, 0);
|
||||
}
|
||||
}
|
||||
@@ -2,6 +2,8 @@
|
||||
|
||||
#![no_std]
|
||||
|
||||
/// Narrow MAVLink arm/disarm command decoding.
|
||||
pub mod arm;
|
||||
/// MAVLink frame encoding API for boards.
|
||||
pub mod encoder;
|
||||
/// Serial-byte MAVLink receive API for boards.
|
||||
|
||||
@@ -117,13 +117,13 @@ mod tests {
|
||||
transmitter
|
||||
.send_frame(
|
||||
FrameClass::BestEffort,
|
||||
encoder.heartbeat(MavState::MAV_STATE_STANDBY),
|
||||
encoder.heartbeat(MavState::MAV_STATE_STANDBY, false),
|
||||
)
|
||||
.unwrap();
|
||||
transmitter
|
||||
.send_frame(
|
||||
FrameClass::BestEffort,
|
||||
encoder.heartbeat(MavState::MAV_STATE_STANDBY),
|
||||
encoder.heartbeat(MavState::MAV_STATE_STANDBY, false),
|
||||
)
|
||||
.unwrap();
|
||||
assert!(transmitter.transmit.is_active());
|
||||
|
||||
Executable
+84
@@ -0,0 +1,84 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Small MAVLink command-line debugger for a Kaptan flight controller."""
|
||||
|
||||
import argparse
|
||||
import os
|
||||
import sys
|
||||
import time
|
||||
|
||||
|
||||
def parse_arguments():
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("serial_device", help="serial device connected to the flight controller")
|
||||
parser.add_argument("command", choices=("arm", "disarm", "read"))
|
||||
parser.add_argument("--baud", type=int, default=115200, help="serial baud rate (default: 115200)")
|
||||
parser.add_argument("--system-id", type=int, default=1, help="target MAVLink system ID (default: 1)")
|
||||
parser.add_argument(
|
||||
"--component-id", type=int, default=1, help="target MAVLink component ID (default: 1)"
|
||||
)
|
||||
return parser.parse_args()
|
||||
|
||||
|
||||
def print_messages(connection, timeout):
|
||||
deadline = time.monotonic() + timeout
|
||||
while time.monotonic() < deadline:
|
||||
message = connection.recv_match(blocking=True, timeout=deadline - time.monotonic())
|
||||
if message is None:
|
||||
return None
|
||||
print(message)
|
||||
yield message
|
||||
|
||||
|
||||
def send_arm_command(connection, mavutil, arguments, armed):
|
||||
for _ in range(3):
|
||||
connection.mav.command_long_send(
|
||||
arguments.system_id,
|
||||
arguments.component_id,
|
||||
mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
|
||||
0,
|
||||
1.0 if armed else 0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
)
|
||||
for message in print_messages(connection, timeout=1.0):
|
||||
if (
|
||||
message.get_type() == "COMMAND_ACK"
|
||||
and message.command == mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM
|
||||
):
|
||||
return
|
||||
action = "arm" if armed else "disarm"
|
||||
raise SystemExit(f"timed out waiting for {action} acknowledgement")
|
||||
|
||||
|
||||
def read_messages(connection):
|
||||
try:
|
||||
while True:
|
||||
message = connection.recv_match(blocking=True, timeout=1.0)
|
||||
if message is not None:
|
||||
print(message)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
|
||||
|
||||
def main():
|
||||
arguments = parse_arguments()
|
||||
# Kaptan currently accepts MAVLink 2 frames only.
|
||||
os.environ["MAVLINK20"] = "1"
|
||||
from pymavlink import mavutil
|
||||
|
||||
connection = mavutil.mavlink_connection(arguments.serial_device, baud=arguments.baud)
|
||||
if arguments.command == "read":
|
||||
read_messages(connection)
|
||||
else:
|
||||
send_arm_command(connection, mavutil, arguments, armed=arguments.command == "arm")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
try:
|
||||
main()
|
||||
except KeyboardInterrupt:
|
||||
sys.exit(130)
|
||||
@@ -1,35 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
import sys
|
||||
import time
|
||||
|
||||
from pymavlink import mavutil
|
||||
|
||||
if len(sys.argv) != 2:
|
||||
sys.exit(f"usage: {sys.argv[0]} <serial-device>")
|
||||
|
||||
connection = mavutil.mavlink_connection(sys.argv[1], baud=115200)
|
||||
next_timesync = 0.0
|
||||
next_ping = 0.0
|
||||
ping_sequence = 0
|
||||
|
||||
while True:
|
||||
now = time.monotonic()
|
||||
if now >= next_timesync:
|
||||
connection.mav.timesync_send(0, time.monotonic_ns() // 1_000)
|
||||
next_timesync = now + 0.2
|
||||
|
||||
if now >= next_ping:
|
||||
connection.mav.ping_send(
|
||||
int(time.time_ns() // 1_000),
|
||||
ping_sequence,
|
||||
0,
|
||||
0,
|
||||
)
|
||||
ping_sequence = (ping_sequence + 1) & 0xFFFFFFFF
|
||||
next_ping = now + 0.1
|
||||
|
||||
while message := connection.recv_match(blocking=False):
|
||||
print(message)
|
||||
|
||||
time.sleep(0.01)
|
||||
Reference in new issue
Block a user