From 827983fb569e82ea35303aa6d13a66f7ffcc5ed5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Onur=20=C3=96zkan?= Date: Sat, 1 Aug 2026 20:54:36 +0300 Subject: [PATCH] boards/kakute-f7-mini-v3: declare F722 flight profile --- boards/kakute-f7-mini-v3/Cargo.toml | 15 +- boards/kakute-f7-mini-v3/README.md | 3 +- boards/kakute-f7-mini-v3/src/actuator/mod.rs | 84 ---- .../src/actuator/tim3_dshot.rs | 402 --------------- .../src/actuator/tim3_pwm.rs | 180 ------- boards/kakute-f7-mini-v3/src/board.rs | 135 +++-- boards/kakute-f7-mini-v3/src/main.rs | 468 +----------------- boards/kakute-f7-mini-v3/src/routes.rs | 72 --- 8 files changed, 62 insertions(+), 1297 deletions(-) delete mode 100644 boards/kakute-f7-mini-v3/src/actuator/mod.rs delete mode 100644 boards/kakute-f7-mini-v3/src/actuator/tim3_dshot.rs delete mode 100644 boards/kakute-f7-mini-v3/src/actuator/tim3_pwm.rs delete mode 100644 boards/kakute-f7-mini-v3/src/routes.rs diff --git a/boards/kakute-f7-mini-v3/Cargo.toml b/boards/kakute-f7-mini-v3/Cargo.toml index b4fbfed..27ac628 100644 --- a/boards/kakute-f7-mini-v3/Cargo.toml +++ b/boards/kakute-f7-mini-v3/Cargo.toml @@ -11,20 +11,11 @@ workspace = true [features] firmware = [] +lidar-lite-v3 = [] [dependencies] -cortex-m = "0.7" -cortex-m-rt = "0.7" -kaptan-actuator = { path = "../../libs/kaptan-actuator" } -kaptan-drivers = { path = "../../libs/kaptan-drivers" } -kaptan-dma = { path = "../../libs/kaptan-dma" } -kaptan-mavlink = { path = "../../libs/kaptan-mavlink" } -kaptan-runtime = { path = "../../libs/kaptan-runtime" } -kaptan-spi = { path = "../../libs/kaptan-spi" } -kaptan-stm32f7 = { path = "../../libs/kaptan-stm32f7", features = ["stm32f722"] } -panic-halt = "1" -rtic = { version = "2.3", default-features = false, features = ["thumbv7-backend"] } -stm32f7 = { version = "0.16", default-features = false, features = ["rt", "stm32f722"] } +kaptan-board = { path = "../../libs/kaptan-board" } +platform-stm32f7 = { path = "../../platforms/stm32f7" } [[bin]] name = "kaptan-kakute-f7-mini-v3" diff --git a/boards/kakute-f7-mini-v3/README.md b/boards/kakute-f7-mini-v3/README.md index f013939..77a3f34 100644 --- a/boards/kakute-f7-mini-v3/README.md +++ b/boards/kakute-f7-mini-v3/README.md @@ -30,8 +30,7 @@ The converter works with any firmware ELF. It uses rustup's `llvm-tools-preview` 4. Flash the generated image: ```sh -dfu-util --alt 0 --dfuse-address 0x08000000:leave \ - --download target/thumbv7em-none-eabihf/release/kaptan-kakute-f7-mini-v3.bin +dfu-util --alt 0 --dfuse-address 0x08000000:leave --download target/thumbv7em-none-eabihf/release/kaptan-kakute-f7-mini-v3.bin ``` ## UART diff --git a/boards/kakute-f7-mini-v3/src/actuator/mod.rs b/boards/kakute-f7-mini-v3/src/actuator/mod.rs deleted file mode 100644 index 2ff8c1a..0000000 --- a/boards/kakute-f7-mini-v3/src/actuator/mod.rs +++ /dev/null @@ -1,84 +0,0 @@ -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(), - } - } -} diff --git a/boards/kakute-f7-mini-v3/src/actuator/tim3_dshot.rs b/boards/kakute-f7-mini-v3/src/actuator/tim3_dshot.rs deleted file mode 100644 index e0cd7b6..0000000 --- a/boards/kakute-f7-mini-v3/src/actuator/tim3_dshot.rs +++ /dev/null @@ -1,402 +0,0 @@ -use core::sync::atomic::{AtomicBool, Ordering}; - -use kaptan_actuator::dshot::{ - ArmSequence, Config, 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: Config, - ) -> 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() - }); -} diff --git a/boards/kakute-f7-mini-v3/src/actuator/tim3_pwm.rs b/boards/kakute-f7-mini-v3/src/actuator/tim3_pwm.rs deleted file mode 100644 index 3968eca..0000000 --- a/boards/kakute-f7-mini-v3/src/actuator/tim3_pwm.rs +++ /dev/null @@ -1,180 +0,0 @@ -use core::sync::atomic::{AtomicU16, Ordering}; - -use kaptan_actuator::pwm::{ArmSequence, Config, 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: Config) -> 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() - }); -} diff --git a/boards/kakute-f7-mini-v3/src/board.rs b/boards/kakute-f7-mini-v3/src/board.rs index 7ffae19..6779205 100644 --- a/boards/kakute-f7-mini-v3/src/board.rs +++ b/boards/kakute-f7-mini-v3/src/board.rs @@ -1,85 +1,64 @@ -//! Kakute F7 Mini V3 static hardware selections. +//! Declarative Kakute F7 Mini V3 board description. +//! +//! This module deliberately contains no MCU registers, RTIC concepts, unsafe +//! code, interrupt names, DMA handling, or task scheduling. Those concerns +//! belong to the STM32F7 platform crate. -use kaptan_actuator::{ArmingDuration, MotorProtocol}; -use kaptan_drivers::lidar_lite_v3::AcquisitionMode; -use kaptan_drivers::mpu6000::{ - AccelerometerRange, DigitalLowPassFilter, GyroscopeRange, Mpu6000Config, +use kaptan_board::{BoardConfig, DataChannelRates, MavlinkEndpoint, VerticalSensingKind}; +use platform_stm32f7::{ + BoardError, F7Board, F7BoardBuilder, F722BoardRoutes, Mavlink, MotorOutputs, Mpu6000, Pa4, + VerticalSensor, }; -use kaptan_stm32f7::stm32f722::{I2c1Config, Tim2PeriodicConfig}; -/// 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; -/// Emit one raw-IMU MAVLink frame every this many 100 Hz service ticks. -/// -/// This caps telemetry at 20 Hz while preserving the full acquisition rate for -/// flight-control work and leaves bandwidth for range and critical MAVLink -/// traffic on the 115200-baud UART. -pub const MAVLINK_RAW_IMU_TICK_DIVISOR: u8 = 5; -/// I²C1 timing for 54 MHz PCLK1 and 100 kHz Standard-mode operation. -/// -/// This conservative setting removes Fast-mode rise-time sensitivity while -/// bringing up the board route. LIDAR-Lite v3 is I²C-compatible and accepts -/// Standard-mode transfers. The range service starts at most one of its five -/// STOP-separated transfers per 100 Hz tick, which also bounds range MAVLink -/// output to 20 Hz. -pub const I2C1_CONFIG: I2c1Config = I2c1Config::from_timing_register(0x2040_4768); -/// LIDAR acquisition command used for each measurement cycle. -pub const LIDAR_ACQUISITION_MODE: AcquisitionMode = AcquisitionMode::WithReceiverBiasCorrection; -/// Number of 100 Hz service ticks to wait before the first LIDAR transaction. -/// -/// Three ticks provide 30 ms, exceeding the sensor's required 22 ms startup -/// interval without waiting in RTIC initialization. -pub const LIDAR_STARTUP_TICKS: u8 = 3; -/// ESC protocol selected for this board's M1–M4 output route. -pub const MOTOR_PROTOCOL: MotorProtocol = MotorProtocol::Dshot( - match kaptan_actuator::dshot::Config::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"), +/// Holybro Kakute F7 Mini V3. +pub struct KakuteF7MiniV3; + +#[cfg(not(feature = "lidar-lite-v3"))] +const CONFIG: BoardConfig = BoardConfig { + name: "Kakute F7 Mini V3", + mavlink: MavlinkEndpoint { + system_id: 1, + component_id: 1, + }, + vertical_sensing: VerticalSensingKind::RangeAndOpticalFlow, + data_channel: DataChannelRates { + service_hz: 100, + raw_imu_hz: 20, + range_hz: Some(20), + optical_flow_hz: Some(20), + heartbeat_hz: 2, }, -); -/// MAVLink sender system identifier. -pub const MAVLINK_SYSTEM_ID: u8 = 1; -/// MAVLink sender component identifier. -pub const MAVLINK_COMPONENT_ID: u8 = 1; -/// MAVLink sensor instance for the LIDAR-Lite v3. -pub const LIDAR_SENSOR_ID: u8 = 0; -/// LIDAR-Lite v3 minimum usable range, in MAVLink centimetres. -pub const LIDAR_MIN_DISTANCE_CENTIMETRES: u16 = 20; -/// LIDAR-Lite v3 maximum usable range, in MAVLink centimetres. -pub const LIDAR_MAX_DISTANCE_CENTIMETRES: u16 = 4_000; -/// Mounting orientation published with the range measurement. -/// -/// Change this when the sensor is not mounted facing downward. -pub const LIDAR_ORIENTATION: kaptan_mavlink::message::MavSensorOrientation = - kaptan_mavlink::message::MavSensorOrientation::MAV_SENSOR_ROTATION_PITCH_270; - -/// MPU6000 full-scale profile used by this board. -pub const MPU6000_CONFIG: Mpu6000Config = Mpu6000Config { - accelerometer: AccelerometerRange::PlusMinus4G, - gyroscope: GyroscopeRange::PlusMinus500DegreesPerSecond, - digital_low_pass_filter: DigitalLowPassFilter::Bandwidth42Hz, - sample_rate_divider: 9, }; -/// One-hundred-hertz IMU service update from the 108 MHz APB1 timer clock. -/// -/// 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, +#[cfg(feature = "lidar-lite-v3")] +const CONFIG: BoardConfig = BoardConfig { + name: "Kakute F7 Mini V3", + mavlink: MavlinkEndpoint { + system_id: 1, + component_id: 1, + }, + vertical_sensing: VerticalSensingKind::Range, + data_channel: DataChannelRates { + service_hz: 100, + raw_imu_hz: 20, + range_hz: Some(20), + optical_flow_hz: None, + heartbeat_hz: 2, + }, }; + +impl F7Board for KakuteF7MiniV3 { + const CONFIG: BoardConfig = CONFIG; + + fn build(platform: F7BoardBuilder) -> Result { + let platform = platform + .imu(Mpu6000::on_spi1().with_drdy(Pa4)) + .motors(MotorOutputs::kakute_tim3_tim5().dshot600()) + .mavlink(Mavlink::on_usart1()); + #[cfg(feature = "lidar-lite-v3")] + let platform = platform.vertical_sensor(VerticalSensor::lidar_lite_v3_on_i2c1()); + #[cfg(not(feature = "lidar-lite-v3"))] + let platform = platform.vertical_sensor(VerticalSensor::mtf01_on_uart4()); + platform.build() + } +} diff --git a/boards/kakute-f7-mini-v3/src/main.rs b/boards/kakute-f7-mini-v3/src/main.rs index e971894..9301ef2 100644 --- a/boards/kakute-f7-mini-v3/src/main.rs +++ b/boards/kakute-f7-mini-v3/src/main.rs @@ -1,472 +1,6 @@ #![no_std] #![no_main] -use core::sync::atomic::{AtomicBool, AtomicU8, Ordering}; - -use cortex_m_rt as _; -use kaptan_runtime::{LatestImuMailbox, LatestRangeMailbox}; -use panic_halt as _; - -mod actuator; mod board; -mod routes; -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; -const MAVLINK_EVENT_RECEIVED: u8 = 1 << 3; -const MAVLINK_EVENT_RANGE: u8 = 1 << 4; - -static IMU_EVENTS: AtomicU8 = AtomicU8::new(0); -static MAVLINK_EVENTS: AtomicU8 = AtomicU8::new(0); -static IMU_QUEUED: AtomicBool = AtomicBool::new(false); -static MAVLINK_QUEUED: AtomicBool = AtomicBool::new(false); -static RAW_IMU_TELEMETRY_DUE: AtomicBool = AtomicBool::new(false); -static SAMPLES: LatestImuMailbox = LatestImuMailbox::new(); -static RANGES: LatestRangeMailbox = LatestRangeMailbox::new(); -static UART_COMPLETION: AtomicU8 = AtomicU8::new(0); - -#[rtic::app(device = stm32f7::stm32f722, peripherals = true, dispatchers = [TIM6_DAC, TIM7])] -mod app { - use super::{ - 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_RANGE, MAVLINK_EVENT_RECEIVED, MAVLINK_EVENT_SAMPLE, - MAVLINK_EVENTS, MAVLINK_QUEUED, Ordering, RANGES, RAW_IMU_TELEMETRY_DUE, SAMPLES, - UART_COMPLETION, - }; - use crate::actuator::KakuteMotorOutput; - use crate::board::{ - CORE_CLOCK_MHZ, I2C1_CONFIG, IMU_SERVICE_TIMER_CONFIG, LIDAR_ACQUISITION_MODE, - LIDAR_MAX_DISTANCE_CENTIMETRES, LIDAR_MIN_DISTANCE_CENTIMETRES, LIDAR_ORIENTATION, - LIDAR_SENSOR_ID, LIDAR_STARTUP_TICKS, MAVLINK_COMPONENT_ID, MAVLINK_RAW_IMU_TICK_DIVISOR, - 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::{lidar_lite_v3::LidarLiteV3, mpu6000::Mpu6000}; - use kaptan_mavlink::{ - arm::{ArmRequest, decode_arm_request}, - encoder::MavlinkEncoder, - receiver::MavlinkReceiver, - transmitter::{FrameClass, MavlinkTransmitter}, - }; - use kaptan_runtime::{ImuEvent, ImuService, RangeEvent, RangeService}; - use kaptan_spi::SpiCompletion; - use kaptan_stm32f7::stm32f722::{ - ActiveLowLedPd2, DcacheDisabledDmaMemory, DwtMicrosecondClock, I2c1, Spi1, Tim2Periodic, - Usart1Tx, Usart1TxDma, configure_exti4_pin_pa4, configure_i2c1_pins_pb8_pb9, - configure_spi1_pins_pa5_pa6_pa7, configure_usart1_rx_pin_pa10, configure_usart1_tx_pin_pa9, - select_pll216_profile, - }; - use stm32f7::stm32f722; - - #[shared] - struct Shared { - mavlink_receiver: MavlinkReceiver<4>, - range: RangeService, - } - - #[local] - struct Local { - imu: ImuService, - mavlink_encoder: MavlinkEncoder, - mavlink_transmitter: MavlinkTransmitter, - led: ActiveLowLedPd2, - imu_service_timer: Tim2Periodic, - heartbeat_divider: u8, - observed_sample: u32, - observed_range: u32, - lidar_startup_ticks: u8, - raw_imu_telemetry_ticks: u8, - esc: KakuteMotorOutput, - } - - #[init] - fn init(mut context: init::Context) -> (Shared, Local) { - let device = context.device; - if select_pll216_profile(&device.RCC, &device.PWR, &device.FLASH).is_err() { - safe_idle(); - } - context.core.SCB.disable_dcache(&mut context.core.CPUID); - let clock = match DwtMicrosecondClock::start::( - &mut context.core.DCB, - &mut context.core.DWT, - ) { - Ok(clock) => clock, - Err(_) => safe_idle(), - }; - - configure_spi1_pins_pa5_pa6_pa7(&device.RCC, &device.GPIOA); - configure_i2c1_pins_pb8_pb9(&device.RCC, &device.GPIOB); - 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 range_clock = clock.fork(); - let imu = ImuService::new(clock, Mpu6000::new(route, MPU6000_CONFIG)); - let range = RangeService::new( - range_clock, - LidarLiteV3::new(I2c1::new(&device.RCC, device.I2C1, I2C1_CONFIG)), - LIDAR_ACQUISITION_MODE, - ); - - let uart = match Usart1Tx::new(&device.RCC, device.USART1, USART1_CLOCK_HZ, USART1_BAUD) { - Ok(uart) => uart, - Err(_) => safe_idle(), - }; - // SAFETY: D-cache is disabled above and `memory.x` excludes DTCM. - let memory = unsafe { DcacheDisabledDmaMemory::new() }; - let mavlink_transmitter = - 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(), - range, - }, - Local { - imu, - mavlink_encoder: MavlinkEncoder::new(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID), - mavlink_transmitter, - led: ActiveLowLedPd2::new(&device.RCC, device.GPIOD), - imu_service_timer: Tim2Periodic::new( - &device.RCC, - device.TIM2, - IMU_SERVICE_TIMER_CONFIG, - ), - heartbeat_divider: 0, - observed_sample: 0, - observed_range: 0, - lidar_startup_ticks: 0, - raw_imu_telemetry_ticks: 0, - esc, - }, - ) - } - - #[idle] - fn idle(_: idle::Context) -> ! { - loop { - cortex_m::asm::wfi(); - } - } - - #[task(binds = EXTI4, priority = 15)] - fn exti4(_: exti4::Context) { - // SAFETY: this handler exclusively acknowledges the board's EXTI4 bit. - unsafe { - (&*stm32f722::EXTI::ptr()) - .pr() - .write(|write| write.pr4().clear()) - }; - schedule_imu(IMU_EVENT_DRDY); - } - - #[task(binds = DMA2_STREAM0, priority = 14)] - fn spi_rx_dma(_: spi_rx_dma::Context) { - match Spi1::acknowledge_rx_interrupt() { - SpiCompletion::Complete => schedule_imu(IMU_EVENT_TRANSFER_COMPLETE), - SpiCompletion::TransferFault => schedule_imu(IMU_EVENT_TRANSFER_FAULT), - SpiCompletion::NotPending => {} - } - } - - #[task(binds = DMA2_STREAM3, priority = 14)] - fn spi_tx_dma(_: spi_tx_dma::Context) { - if Spi1::acknowledge_tx_interrupt() == SpiCompletion::TransferFault { - schedule_imu(IMU_EVENT_TRANSFER_FAULT); - } - } - - #[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, shared = [range], local = [imu_service_timer, lidar_startup_ticks, raw_imu_telemetry_ticks])] - fn tim2(mut context: tim2::Context) { - context.local.imu_service_timer.acknowledge_update(); - schedule_imu(IMU_EVENT_TIMER); - if raw_imu_telemetry_due(context.local.raw_imu_telemetry_ticks) { - RAW_IMU_TELEMETRY_DUE.store(true, Ordering::Release); - } - if *context.local.lidar_startup_ticks < LIDAR_STARTUP_TICKS { - *context.local.lidar_startup_ticks += 1; - if *context.local.lidar_startup_ticks == LIDAR_STARTUP_TICKS { - context.shared.range.lock(|range| { - let _ = range.finish_startup(); - }); - } - } else { - context.shared.range.lock(|range| { - publish_range_event(range.on_periodic_tick()); - }); - } - schedule_mavlink(MAVLINK_EVENT_HEARTBEAT); - } - - #[task(binds = I2C1_EV, priority = 11, shared = [range])] - fn i2c1_event(mut context: i2c1_event::Context) { - context.shared.range.lock(|range| { - let event = range.on_controller_interrupt(|i2c| i2c.acknowledge_event_interrupt()); - publish_range_event(event); - }); - } - - #[task(binds = I2C1_ER, priority = 11, shared = [range])] - fn i2c1_error(mut context: i2c1_error::Context) { - context.shared.range.lock(|range| { - let event = range.on_controller_interrupt(|i2c| i2c.acknowledge_error_interrupt()); - publish_range_event(event); - }); - } - - #[task(binds = DMA2_STREAM7, priority = 10)] - fn uart_dma(_: uart_dma::Context) { - match Usart1TxDma::acknowledge_interrupt() { - TxCompletion::Complete => { - UART_COMPLETION.store(1, Ordering::Release); - schedule_mavlink(MAVLINK_EVENT_DMA); - } - TxCompletion::TransferFault | TxCompletion::Halted => { - UART_COMPLETION.store(2, Ordering::Release); - schedule_mavlink(MAVLINK_EVENT_DMA); - } - TxCompletion::NotPending => {} - } - } - - #[task(binds = USART1, priority = 8, shared = [mavlink_receiver])] - fn uart_rx(mut context: uart_rx::Context) { - match Usart1Tx::acknowledge_receive_interrupt() { - kaptan_stm32f7::stm32f722::ReceiveInterrupt::Byte(byte) => { - let admitted = context - .shared - .mavlink_receiver - .lock(|receiver| receiver.receive_byte(byte)); - if admitted { - schedule_mavlink(MAVLINK_EVENT_RECEIVED); - } - } - kaptan_stm32f7::stm32f722::ReceiveInterrupt::Fault => { - context.shared.mavlink_receiver.lock(MavlinkReceiver::reset); - } - kaptan_stm32f7::stm32f722::ReceiveInterrupt::NotPending => {} - } - } - - #[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 - .imu - .on_transfer_complete(SpiCompletion::TransferFault); - } - if events & IMU_EVENT_TRANSFER_COMPLETE != 0 { - match context - .local - .imu - .on_transfer_complete(SpiCompletion::Complete) - { - ImuEvent::Sample(sample) => { - SAMPLES.publish(sample); - schedule_mavlink(MAVLINK_EVENT_SAMPLE); - } - ImuEvent::Fault(_) | ImuEvent::Initialized | ImuEvent::Idle => {} - } - } - if events & IMU_EVENT_TIMER != 0 { - let _ = context.local.imu.on_periodic_tick(); - } - if events & IMU_EVENT_DRDY != 0 { - let _ = context.local.imu.on_data_ready(); - } - IMU_QUEUED.store(false, Ordering::Release); - if IMU_EVENTS.load(Ordering::Acquire) != 0 - && !IMU_QUEUED.swap(true, Ordering::AcqRel) - && imu_worker::spawn().is_err() - { - IMU_QUEUED.store(false, Ordering::Release); - } - } - - #[task(priority = 9, shared = [mavlink_receiver], local = [mavlink_encoder, mavlink_transmitter, led, heartbeat_divider, observed_sample, observed_range, esc])] - async fn mavlink_worker(mut context: mavlink_worker::Context) { - let events = MAVLINK_EVENTS.swap(0, Ordering::AcqRel); - if events & MAVLINK_EVENT_DMA != 0 { - let completion = match UART_COMPLETION.swap(0, Ordering::AcqRel) { - 1 => TxCompletion::Complete, - 2 => TxCompletion::TransferFault, - _ => TxCompletion::NotPending, - }; - context - .local - .mavlink_transmitter - .on_dma_complete(completion); - } - if events & MAVLINK_EVENT_SAMPLE != 0 - && RAW_IMU_TELEMETRY_DUE.swap(false, Ordering::AcqRel) - && let Some(sample) = SAMPLES.take_newer(context.local.observed_sample) - { - // MAVLink transmission never blocks flight work. A later sample - // or heartbeat gives the bounded queue another chance to send. - let _ = context.local.mavlink_transmitter.send_frame( - FrameClass::BestEffort, - context.local.mavlink_encoder.raw_imu_snapshot(&sample), - ); - } - if events & MAVLINK_EVENT_RANGE != 0 - && let Some(sample) = RANGES.take_newer(context.local.observed_range) - { - // A range observation is control-relevant and must not be - // perpetually displaced by the much higher-rate raw IMU stream. - // Critical admission may evict pending best-effort IMU traffic, - // while an already-active DMA transfer remains untouched. - let _ = context.local.mavlink_transmitter.send_frame( - FrameClass::Critical, - context.local.mavlink_encoder.distance_sensor_snapshot( - &sample, - LIDAR_SENSOR_ID, - LIDAR_ORIENTATION, - LIDAR_MIN_DISTANCE_CENTIMETRES, - LIDAR_MAX_DISTANCE_CENTIMETRES, - ), - ); - } - if events & MAVLINK_EVENT_HEARTBEAT != 0 { - *context.local.heartbeat_divider = context.local.heartbeat_divider.wrapping_add(1); - 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.esc.is_armed(), - ), - ); - context.local.led.toggle(); - } - } - if events & MAVLINK_EVENT_RECEIVED != 0 { - while let Some(frame) = context - .shared - .mavlink_receiver - .lock(MavlinkReceiver::take_received) - { - 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); - if MAVLINK_EVENTS.load(Ordering::Acquire) != 0 - && !MAVLINK_QUEUED.swap(true, Ordering::AcqRel) - && mavlink_worker::spawn().is_err() - { - MAVLINK_QUEUED.store(false, Ordering::Release); - } - } - - fn schedule_imu(event: u8) { - IMU_EVENTS.fetch_or(event, Ordering::Release); - if !IMU_QUEUED.swap(true, Ordering::AcqRel) && imu_worker::spawn().is_err() { - IMU_QUEUED.store(false, Ordering::Release); - } - } - - /// Advances the raw-IMU admission divider. - /// - /// Starting from zero, exactly twenty of every one hundred 100 Hz ticks - /// return `true`; keeping the initial permit false avoids a boot-time - /// twenty-first transmission. - fn raw_imu_telemetry_due(divider: &mut u8) -> bool { - *divider = divider.wrapping_add(1); - if *divider == MAVLINK_RAW_IMU_TICK_DIVISOR { - *divider = 0; - true - } else { - false - } - } - - fn schedule_mavlink(event: u8) { - MAVLINK_EVENTS.fetch_or(event, Ordering::Release); - if !MAVLINK_QUEUED.swap(true, Ordering::AcqRel) && mavlink_worker::spawn().is_err() { - MAVLINK_QUEUED.store(false, Ordering::Release); - } - } - - fn publish_range_event(event: RangeEvent) { - if let RangeEvent::Measurement(sample) = event { - RANGES.publish(sample); - schedule_mavlink(MAVLINK_EVENT_RANGE); - } - } - - fn configure_exti4(rcc: &stm32f722::RCC, syscfg: stm32f722::SYSCFG, exti: stm32f722::EXTI) { - rcc.apb2enr().modify(|_, write| write.syscfgen().set_bit()); - syscfg.exticr2().modify(|_, write| { - // SAFETY: EXTI4 is connected to GPIOA by selector value zero. - unsafe { write.exti4().bits(0) } - }); - exti.imr().modify(|_, write| write.im4().set_bit()); - exti.rtsr().modify(|_, write| write.tr4().set_bit()); - exti.ftsr().modify(|_, write| write.tr4().clear_bit()); - exti.pr().write(|write| write.pr4().clear()); - } - - fn safe_idle() -> ! { - loop { - cortex_m::asm::wfi(); - } - } -} +platform_stm32f7::flight_firmware!(board::KakuteF7MiniV3); diff --git a/boards/kakute-f7-mini-v3/src/routes.rs b/boards/kakute-f7-mini-v3/src/routes.rs deleted file mode 100644 index 56709cb..0000000 --- a/boards/kakute-f7-mini-v3/src/routes.rs +++ /dev/null @@ -1,72 +0,0 @@ -//! Static electrical routes selected by the Kakute F7 Mini V3. -//! -//! The board describes which controller and pins are connected. Protocol -//! framing and controller state remain in their respective reusable crates. - -use kaptan_spi::{SpiCompletion, SpiDevice}; -use kaptan_stm32f7::stm32f722::{Spi1, Spi1Error}; -use stm32f7::stm32f722; - -/// Chip-select-scoped SPI1/PB2 route wired to the MPU6000. -pub struct KakuteMpu6000SpiRoute { - spi: Spi1, - chip_select: stm32f722::GPIOB, -} - -impl KakuteMpu6000SpiRoute { - /// Configures board-selected PB2 chip select and combines it with SPI1. - pub(crate) fn new(rcc: &stm32f722::RCC, spi: Spi1, chip_select: stm32f722::GPIOB) -> Self { - rcc.ahb1enr().modify(|_, write| write.gpioben().set_bit()); - chip_select - .moder() - .modify(|_, write| write.moder2().output()); - chip_select - .otyper() - .modify(|_, write| write.ot2().push_pull()); - chip_select - .ospeedr() - .modify(|_, write| write.ospeedr2().low_speed()); - chip_select - .pupdr() - .modify(|_, write| write.pupdr2().floating()); - chip_select.bsrr().write(|write| write.bs2().set_bit()); - - Self { spi, chip_select } - } - - fn select(&self) { - self.chip_select.bsrr().write(|write| write.br2().set_bit()); - } - - fn deselect(&self) { - self.chip_select.bsrr().write(|write| write.bs2().set_bit()); - } -} - -impl SpiDevice for KakuteMpu6000SpiRoute { - type StartError = Spi1Error; - - fn start_transfer( - &mut self, - transmit: &[u8], - receive: &mut [u8], - ) -> Result<(), Self::StartError> { - self.select(); - if let Err(error) = self.spi.start_transfer(transmit, receive) { - self.deselect(); - return Err(error); - } - - Ok(()) - } - - fn complete_transfer(&mut self, completion: SpiCompletion) { - self.spi.complete_transfer(completion); - if matches!( - completion, - SpiCompletion::Complete | SpiCompletion::TransferFault - ) { - self.deselect(); - } - } -}