boards/kakute-f7-mini-v3: add selectable motor outputs

Signed-off-by: Onur Özkan <work@onurozkan.dev>

boards/kakute-f7-mini-v3/README.md

@@ -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.

boards/kakute-f7-mini-v3/src/actuator/mod.rs

@@ -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(),
+        }
+    }
+}

boards/kakute-f7-mini-v3/src/actuator/tim3_dshot.rs

@@ -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()
+    });
+}

boards/kakute-f7-mini-v3/src/actuator/tim3_pwm.rs

@@ -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()
+    });
+}

boards/kakute-f7-mini-v3/src/board.rs

@@ -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.
+/// Core frequency in whole megahertz after selecting the PLL216 board profile.
-pub const CORE_CLOCK_MHZ: u32 = 16;
+pub const CORE_CLOCK_MHZ: u32 = 216;
-/// APB2 frequency driving USART1 in the HSI16 board profile.
+/// APB2 frequency driving USART1 in the PLL216 board profile.
-pub const USART1_CLOCK_HZ: u32 = 16_000_000;
+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
+/// This matches the MPU6000 output data rate selected by `MPU6000_CONFIG`.
-/// intermediate ticks for the MPU6000 reset-settling interval and DWT epoch.
+/// The runtime emits its one-hertz heartbeat every hundredth tick.
-pub const HEARTBEAT_TIMER_CONFIG: Tim2PeriodicConfig = Tim2PeriodicConfig {
+pub const IMU_SERVICE_TIMER_CONFIG: Tim2PeriodicConfig = Tim2PeriodicConfig {
-    prescaler: 15_999,
+    prescaler: 10_799,
     auto_reload: 99,
 };

boards/kakute-f7-mini-v3/src/main.rs

@@ -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_EVENT_DRDY, IMU_EVENT_START, IMU_EVENT_TIMER, IMU_EVENT_TRANSFER_COMPLETE,
-        IMU_EVENTS, IMU_QUEUED, MAVLINK_EVENT_DMA, MAVLINK_EVENT_HEARTBEAT, MAVLINK_EVENT_RECEIVED,
+        IMU_EVENT_TRANSFER_FAULT, IMU_EVENTS, IMU_QUEUED, MAVLINK_EVENT_DMA,
-        MAVLINK_EVENT_SAMPLE, MAVLINK_EVENTS, MAVLINK_QUEUED, Ordering, SAMPLES, UART_COMPLETION,
+        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,
+        CORE_CLOCK_MHZ, IMU_SERVICE_TIMER_CONFIG, MAVLINK_COMPONENT_ID, MAVLINK_SYSTEM_ID,
-        MPU6000_CONFIG, USART1_BAUD, USART1_CLOCK_HZ,
+        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));
+        let imu = ImuService::new(clock, Mpu6000::new(route, MPU6000_CONFIG));
-        if imu.begin().is_err() {
-            safe_idle();
-        }
 
         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
+                    context.local.mavlink_encoder.heartbeat(
-                        .local
+                        kaptan_mavlink::message::MavState::MAV_STATE_STANDBY,
-                        .mavlink_encoder
+                        context.local.esc.is_armed(),
-                        .heartbeat(kaptan_mavlink::message::MavState::MAV_STATE_STANDBY),
+                    ),
                 );
                 context.local.led.toggle();
             }
@@ -282,11 +313,26 @@ mod app {
                 .mavlink_receiver
                 .lock(MavlinkReceiver::take_received)
             {
-                // TODO: Echo the frame for testing.
+                if let Some(request) =
-                let _ = context
+                    decode_arm_request(&frame, MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID)
-                    .local
+                {
-                    .mavlink_transmitter
+                    // TODO: Acknowledge arm only after output activation is confirmed.
-                    .send_frame(FrameClass::Critical, frame);
+                    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);

libs/kaptan-mavlink/src/arm.rs

@@ -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);
+    }
+}

libs/kaptan-mavlink/src/encoder.rs

@@ -37,13 +37,17 @@ impl MavlinkEncoder {
         raw
     }
 
-    /// Encodes a Kaptan heartbeat.
+    /// Encodes a Kaptan heartbeat with its current arming state.
-    pub fn heartbeat(&mut self, system_status: message::MavState) -> MavlinkFrame {
+    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);
+    }
 }

libs/kaptan-mavlink/src/lib.rs

@@ -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.

libs/kaptan-mavlink/src/transmitter.rs

@@ -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());

scripts/mavlink-debug.py

@@ -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)

scripts/test-mavlink.py

@@ -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)