boards/kakute-f7-mini-v3: publish LIDAR range telemetry

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

@@ -1,10 +1,11 @@
 //! Kakute F7 Mini V3 static hardware selections.
 
 use kaptan_actuator::{ArmingDuration, MotorProtocol};
+use kaptan_drivers::lidar_lite_v3::AcquisitionMode;
 use kaptan_drivers::mpu6000::{
     AccelerometerRange, DigitalLowPassFilter, GyroscopeRange, Mpu6000Config,
 };
-use kaptan_stm32f7::stm32f722::Tim2PeriodicConfig;
+use kaptan_stm32f7::stm32f722::{I2c1Config, Tim2PeriodicConfig};
 
 /// Core frequency in whole megahertz after selecting the PLL216 board profile.
 pub const CORE_CLOCK_MHZ: u32 = 216;
@@ -12,6 +13,27 @@ pub const CORE_CLOCK_MHZ: u32 = 216;
 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::DshotConfig::new(
@@ -33,6 +55,17 @@ pub const MOTOR_PROTOCOL: MotorProtocol = MotorProtocol::Dshot(
 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 {

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

@@ -4,7 +4,7 @@
 use core::sync::atomic::{AtomicBool, AtomicU8, Ordering};
 
 use cortex_m_rt as _;
-use kaptan_runtime::LatestImuMailbox;
+use kaptan_runtime::{LatestImuMailbox, LatestRangeMailbox};
 use panic_halt as _;
 
 mod actuator;
@@ -20,12 +20,15 @@ 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])]
@@ -33,36 +36,41 @@ 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_RECEIVED, MAVLINK_EVENT_SAMPLE, MAVLINK_EVENTS,
-        MAVLINK_QUEUED, Ordering, SAMPLES, UART_COMPLETION,
+        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, IMU_SERVICE_TIMER_CONFIG, MAVLINK_COMPONENT_ID, MAVLINK_SYSTEM_ID,
-        MOTOR_PROTOCOL, MPU6000_CONFIG, USART1_BAUD, USART1_CLOCK_HZ,
+        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::mpu6000::Mpu6000;
+    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};
+    use kaptan_runtime::{ImuEvent, ImuService, RangeEvent, RangeService};
     use kaptan_spi::SpiCompletion;
     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_pll216_profile,
+        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<DwtMicrosecondClock, I2c1>,
     }
 
     #[local]
@@ -74,6 +82,9 @@ mod app {
         imu_service_timer: Tim2Periodic,
         heartbeat_divider: u8,
         observed_sample: u32,
+        observed_range: u32,
+        lidar_startup_ticks: u8,
+        raw_imu_telemetry_ticks: u8,
         esc: KakuteMotorOutput,
     }
 
@@ -93,13 +104,20 @@ mod app {
         };
 
         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,
@@ -118,6 +136,7 @@ mod app {
         (
             Shared {
                 mavlink_receiver: MavlinkReceiver::new(),
+                range,
             },
             Local {
                 imu,
@@ -131,6 +150,9 @@ mod app {
                 ),
                 heartbeat_divider: 0,
                 observed_sample: 0,
+                observed_range: 0,
+                lidar_startup_ticks: 0,
+                raw_imu_telemetry_ticks: 0,
                 esc,
             },
         )
@@ -187,13 +209,44 @@ mod app {
         KakuteMotorOutput::service_tim5_interrupt();
     }
 
-    #[task(binds = TIM2, priority = 4, local = [imu_service_timer])]
-    fn tim2(context: tim2::Context) {
+    #[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() {
@@ -269,7 +322,7 @@ mod app {
         }
     }
 
-    #[task(priority = 9, shared = [mavlink_receiver], local = [mavlink_encoder, mavlink_transmitter, led, heartbeat_divider, observed_sample, esc])]
+    #[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 {
@@ -284,6 +337,7 @@ mod app {
                 .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
@@ -293,6 +347,24 @@ mod app {
                 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 {
@@ -351,6 +423,21 @@ mod app {
         }
     }
 
+    /// 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() {
@@ -358,6 +445,13 @@ mod app {
         }
     }
 
+    fn publish_range_event(event: RangeEvent<kaptan_stm32f7::stm32f722::I2c1Error>) {
+        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| {