diff --git a/boards/kakute-f7-mini-v3/src/board.rs b/boards/kakute-f7-mini-v3/src/board.rs index 7f2dcc0..2df4ece 100644 --- a/boards/kakute-f7-mini-v3/src/board.rs +++ b/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 { diff --git a/boards/kakute-f7-mini-v3/src/main.rs b/boards/kakute-f7-mini-v3/src/main.rs index dd27fdf..e971894 100644 --- a/boards/kakute-f7-mini-v3/src/main.rs +++ b/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, } #[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) { + 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| {