diff --git a/Cargo.lock b/Cargo.lock index b5fb327..b1bfe03 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -156,6 +156,7 @@ dependencies = [ "cortex-m-rt", "kaptan-dma", "kaptan-drivers", + "kaptan-mavlink", "kaptan-runtime", "kaptan-spi", "kaptan-stm32f7", @@ -168,8 +169,10 @@ dependencies = [ name = "kaptan-mavlink" version = "0.1.0" dependencies = [ + "kaptan-dma", "kaptan-sensor", "kaptan-time", + "kaptan-tx", "mavlink", "mavlink-bindgen", "mavlink-core", @@ -179,13 +182,10 @@ dependencies = [ name = "kaptan-runtime" version = "0.1.0" dependencies = [ - "kaptan-dma", "kaptan-drivers", - "kaptan-mavlink", "kaptan-sensor", "kaptan-spi", "kaptan-time", - "kaptan-tx", ] [[package]] diff --git a/boards/kakute-f7-mini-v3/Cargo.toml b/boards/kakute-f7-mini-v3/Cargo.toml index 7025182..083497e 100644 --- a/boards/kakute-f7-mini-v3/Cargo.toml +++ b/boards/kakute-f7-mini-v3/Cargo.toml @@ -17,6 +17,7 @@ cortex-m = "0.7" cortex-m-rt = "0.7" 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"] } @@ -29,4 +30,4 @@ name = "kaptan-kakute-f7-mini-v3" path = "src/main.rs" required-features = ["firmware"] test = false -bench = false \ No newline at end of file +bench = false diff --git a/boards/kakute-f7-mini-v3/README.md b/boards/kakute-f7-mini-v3/README.md index af51c78..48a0bf8 100644 --- a/boards/kakute-f7-mini-v3/README.md +++ b/boards/kakute-f7-mini-v3/README.md @@ -42,9 +42,11 @@ dfu-util --alt 0 --dfuse-address 0x08000000:leave \ | `GND` | `G` | | `TX` | `R1` | -The transmitter emits unsigned MAVLink 2 at 115200 baud, 8N1. It is binary +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/read-mavlink.py $serial_device_path +./scripts/test-mavlink.py $serial_device_path ``` + +The test helper sends MAVLink `TIMESYNC` and`PING` frames while printing all received MAVLink messages. diff --git a/boards/kakute-f7-mini-v3/src/main.rs b/boards/kakute-f7-mini-v3/src/main.rs index 33a9daf..4e00bec 100644 --- a/boards/kakute-f7-mini-v3/src/main.rs +++ b/boards/kakute-f7-mini-v3/src/main.rs @@ -14,14 +14,15 @@ 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 TELEMETRY_EVENT_SAMPLE: u8 = 1 << 0; -const TELEMETRY_EVENT_HEARTBEAT: u8 = 1 << 1; -const TELEMETRY_EVENT_DMA: u8 = 1 << 2; +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; static IMU_EVENTS: AtomicU8 = AtomicU8::new(0); -static TELEMETRY_EVENTS: AtomicU8 = AtomicU8::new(0); +static MAVLINK_EVENTS: AtomicU8 = AtomicU8::new(0); static IMU_QUEUED: AtomicBool = AtomicBool::new(false); -static TELEMETRY_QUEUED: AtomicBool = AtomicBool::new(false); +static MAVLINK_QUEUED: AtomicBool = AtomicBool::new(false); static SAMPLES: LatestImuMailbox = LatestImuMailbox::new(); static UART_COMPLETION: AtomicU8 = AtomicU8::new(0); @@ -29,8 +30,8 @@ static UART_COMPLETION: AtomicU8 = AtomicU8::new(0); mod app { use super::{ IMU_EVENT_DRDY, IMU_EVENT_TIMER, IMU_EVENT_TRANSFER_COMPLETE, IMU_EVENT_TRANSFER_FAULT, - IMU_EVENTS, IMU_QUEUED, Ordering, SAMPLES, TELEMETRY_EVENT_DMA, TELEMETRY_EVENT_HEARTBEAT, - TELEMETRY_EVENT_SAMPLE, TELEMETRY_EVENTS, TELEMETRY_QUEUED, UART_COMPLETION, + IMU_EVENTS, IMU_QUEUED, MAVLINK_EVENT_DMA, MAVLINK_EVENT_HEARTBEAT, MAVLINK_EVENT_RECEIVED, + MAVLINK_EVENT_SAMPLE, MAVLINK_EVENTS, MAVLINK_QUEUED, Ordering, SAMPLES, UART_COMPLETION, }; use crate::board::{ CORE_CLOCK_MHZ, HEARTBEAT_TIMER_CONFIG, MAVLINK_COMPONENT_ID, MAVLINK_SYSTEM_ID, @@ -39,22 +40,30 @@ mod app { use crate::routes::KakuteMpu6000SpiRoute; use kaptan_dma::TxCompletion; use kaptan_drivers::mpu6000::Mpu6000; - use kaptan_runtime::{ImuEvent, ImuService, TelemetryService}; + use kaptan_mavlink::{ + encoder::MavlinkEncoder, + receiver::MavlinkReceiver, + transmitter::{FrameClass, MavlinkTransmitter}, + }; + use kaptan_runtime::{ImuEvent, ImuService}; 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_tx_pin_pa9, select_hsi16_profile, + configure_usart1_rx_pin_pa10, configure_usart1_tx_pin_pa9, select_hsi16_profile, }; use stm32f7::stm32f722; #[shared] - struct Shared {} + struct Shared { + mavlink_receiver: MavlinkReceiver<4>, + } #[local] struct Local { imu: ImuService, - telemetry: TelemetryService, + mavlink_encoder: MavlinkEncoder, + mavlink_transmitter: MavlinkTransmitter, led: ActiveLowLedPd2, heartbeat_timer: Tim2Periodic, heartbeat_divider: u8, @@ -79,6 +88,7 @@ mod app { configure_spi1_pins_pa5_pa6_pa7(&device.RCC, &device.GPIOA); configure_exti4_pin_pa4(&device.GPIOA); configure_usart1_tx_pin_pa9(&device.GPIOA); + configure_usart1_rx_pin_pa10(&device.GPIOA); 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)); @@ -92,19 +102,18 @@ mod app { }; // SAFETY: D-cache is disabled above and `memory.x` excludes DTCM. let memory = unsafe { DcacheDisabledDmaMemory::new() }; - let telemetry = TelemetryService::new( - MAVLINK_SYSTEM_ID, - MAVLINK_COMPONENT_ID, - memory, - Usart1TxDma::new(&device.RCC, uart), - ); + let mavlink_transmitter = + MavlinkTransmitter::new(memory, Usart1TxDma::new(&device.RCC, uart)); configure_exti4(&device.RCC, device.SYSCFG, device.EXTI); ( - Shared {}, + Shared { + mavlink_receiver: MavlinkReceiver::new(), + }, Local { imu, - telemetry, + mavlink_encoder: MavlinkEncoder::new(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID), + mavlink_transmitter, led: ActiveLowLedPd2::new(&device.RCC, device.GPIOD), heartbeat_timer: Tim2Periodic::new( &device.RCC, @@ -155,7 +164,7 @@ mod app { fn tim2(context: tim2::Context) { context.local.heartbeat_timer.acknowledge_update(); schedule_imu(IMU_EVENT_TIMER); - schedule_telemetry(TELEMETRY_EVENT_HEARTBEAT); + schedule_mavlink(MAVLINK_EVENT_HEARTBEAT); } #[task(binds = DMA2_STREAM7, priority = 10)] @@ -163,16 +172,35 @@ mod app { match Usart1TxDma::acknowledge_interrupt() { TxCompletion::Complete => { UART_COMPLETION.store(1, Ordering::Release); - schedule_telemetry(TELEMETRY_EVENT_DMA); + schedule_mavlink(MAVLINK_EVENT_DMA); } TxCompletion::TransferFault | TxCompletion::Halted => { UART_COMPLETION.store(2, Ordering::Release); - schedule_telemetry(TELEMETRY_EVENT_DMA); + 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) { let events = IMU_EVENTS.swap(0, Ordering::AcqRel); @@ -190,7 +218,7 @@ mod app { { ImuEvent::Sample(sample) => { SAMPLES.publish(sample); - schedule_telemetry(TELEMETRY_EVENT_SAMPLE); + schedule_mavlink(MAVLINK_EVENT_SAMPLE); } ImuEvent::Fault(_) | ImuEvent::Initialized | ImuEvent::Idle => {} } @@ -202,47 +230,72 @@ mod app { 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) { - if imu_worker::spawn().is_err() { - 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, local = [telemetry, led, heartbeat_divider, observed_sample])] - async fn telemetry_worker(context: telemetry_worker::Context) { - let events = TELEMETRY_EVENTS.swap(0, Ordering::AcqRel); - if events & TELEMETRY_EVENT_DMA != 0 { + #[task(priority = 9, shared = [mavlink_receiver], local = [mavlink_encoder, mavlink_transmitter, led, heartbeat_divider, observed_sample])] + 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.telemetry.on_dma_complete(completion); + context + .local + .mavlink_transmitter + .on_dma_complete(completion); } - if events & TELEMETRY_EVENT_SAMPLE != 0 { - if let Some(sample) = SAMPLES.take_newer(context.local.observed_sample) { - // Telemetry never blocks flight work. A later sample or - // heartbeat gives the bounded queue another chance to send. - let _ = context.local.telemetry.publish_sample(&sample); - } + if events & MAVLINK_EVENT_SAMPLE != 0 + && 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 & TELEMETRY_EVENT_HEARTBEAT != 0 { + if events & MAVLINK_EVENT_HEARTBEAT != 0 { *context.local.heartbeat_divider = context.local.heartbeat_divider.wrapping_add(1); if *context.local.heartbeat_divider == 10 { *context.local.heartbeat_divider = 0; - let _ = context.local.telemetry.publish_heartbeat(); + let _ = context.local.mavlink_transmitter.send_frame( + FrameClass::Critical, + context + .local + .mavlink_encoder + .heartbeat(kaptan_mavlink::message::MavState::MAV_STATE_STANDBY), + ); context.local.led.toggle(); } } - TELEMETRY_QUEUED.store(false, Ordering::Release); - if TELEMETRY_EVENTS.load(Ordering::Acquire) != 0 - && !TELEMETRY_QUEUED.swap(true, Ordering::AcqRel) - { - if telemetry_worker::spawn().is_err() { - TELEMETRY_QUEUED.store(false, Ordering::Release); + if events & MAVLINK_EVENT_RECEIVED != 0 { + while let Some(frame) = context + .shared + .mavlink_receiver + .lock(MavlinkReceiver::take_received) + { + // TODO: Echo the frame for testing. + let _ = context + .local + .mavlink_transmitter + .send_frame(FrameClass::Critical, frame); } } + 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) { @@ -252,18 +305,19 @@ mod app { } } - fn schedule_telemetry(event: u8) { - TELEMETRY_EVENTS.fetch_or(event, Ordering::Release); - if !TELEMETRY_QUEUED.swap(true, Ordering::AcqRel) && telemetry_worker::spawn().is_err() { - TELEMETRY_QUEUED.store(false, Ordering::Release); + 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 configure_exti4(rcc: &stm32f722::RCC, syscfg: stm32f722::SYSCFG, exti: stm32f722::EXTI) { rcc.apb2enr().modify(|_, write| write.syscfgen().set_bit()); - syscfg - .exticr2() - .modify(|_, write| unsafe { write.exti4().bits(0) }); + 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()); diff --git a/libs/kaptan-mavlink/Cargo.toml b/libs/kaptan-mavlink/Cargo.toml index c48ae23..382f5e4 100644 --- a/libs/kaptan-mavlink/Cargo.toml +++ b/libs/kaptan-mavlink/Cargo.toml @@ -14,10 +14,14 @@ default = ["dialect-kaptan"] dialect-kaptan = [] [dependencies] +kaptan-dma = { path = "../kaptan-dma" } +kaptan-sensor = { path = "../kaptan-sensor" } +kaptan-tx = { path = "../kaptan-tx" } mavlink = { version = "0.18", default-features = false, features = ["embedded", "dialect-common"] } mavlink-core = { version = "0.18", default-features = false, features = ["embedded"] } -kaptan-sensor = { path = "../kaptan-sensor" } + +[dev-dependencies] kaptan-time = { path = "../kaptan-time" } [build-dependencies] -mavlink-bindgen = "0.18" \ No newline at end of file +mavlink-bindgen = "0.18" diff --git a/libs/kaptan-mavlink/src/encoder.rs b/libs/kaptan-mavlink/src/encoder.rs new file mode 100644 index 0000000..d28e325 --- /dev/null +++ b/libs/kaptan-mavlink/src/encoder.rs @@ -0,0 +1,117 @@ +//! MAVLink frame encoding API. + +use kaptan_sensor::ImuSnapshot; +use mavlink::MessageData; + +use crate::{MavlinkFrame, message}; + +/// Serializes FC-domain data with MAVLink sender identity and sequence state. +pub struct MavlinkEncoder { + system_id: u8, + component_id: u8, + next_sequence: u8, +} + +impl MavlinkEncoder { + /// Creates an encoder beginning with MAVLink packet sequence zero. + pub const fn new(system_id: u8, component_id: u8) -> Self { + Self { + system_id, + component_id, + next_sequence: 0, + } + } + + fn serialize(&mut self, message: &M) -> MavlinkFrame { + let mut raw = MavlinkFrame::new(); + raw.serialize_message_data( + crate::message::MavHeader { + sequence: self.next_sequence, + system_id: self.system_id, + component_id: self.component_id, + }, + message, + ); + self.next_sequence = self.next_sequence.wrapping_add(1); + + raw + } + + /// Encodes a Kaptan heartbeat. + pub fn heartbeat(&mut self, system_status: message::MavState) -> 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(), + system_status, + mavlink_version: 3, + }; + + self.serialize(&heartbeat) + } + + /// Encodes a standard `RAW_IMU` frame. + pub fn raw_imu_snapshot(&mut self, snapshot: &ImuSnapshot) -> MavlinkFrame { + let raw_imu = message::RAW_IMU_DATA { + time_usec: snapshot.timestamp.as_microseconds(), + xacc: snapshot.acceleration_milligravity[0], + yacc: snapshot.acceleration_milligravity[1], + zacc: snapshot.acceleration_milligravity[2], + xgyro: snapshot.angular_rate_milliradians_per_second[0], + ygyro: snapshot.angular_rate_milliradians_per_second[1], + zgyro: snapshot.angular_rate_milliradians_per_second[2], + xmag: 0, + ymag: 0, + zmag: 0, + }; + + self.serialize(&raw_imu) + } +} + +#[cfg(test)] +mod tests { + use super::MavlinkEncoder; + use crate::message::MavState; + use kaptan_sensor::ImuSnapshot; + use kaptan_time::Timestamp; + + #[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 bytes = frame.raw_bytes(); + + assert_eq!(bytes[0], 0xFD); + assert_eq!(bytes[2], 0); + assert_eq!(bytes[3], 0); + assert_eq!(bytes[4], 0); + assert_eq!(bytes[5], 42); + assert_eq!(bytes[6], 7); + assert_eq!(&bytes[7..10], &[0, 0, 0]); + } + + #[test] + fn heartbeat_preserves_the_requested_health_state() { + let mut encoder = MavlinkEncoder::new(1, 1); + let frame = encoder.heartbeat(MavState::MAV_STATE_CRITICAL); + + assert_eq!(frame.raw_bytes()[17], MavState::MAV_STATE_CRITICAL as u8); + } + + #[test] + fn snapshot_adapter_uses_flight_data_timestamp_and_units() { + let mut encoder = MavlinkEncoder::new(1, 1); + let snapshot = ImuSnapshot::new( + Timestamp::from_microseconds(123), + [1, 2, 3], + [4, 5, 6], + 2_500, + ); + + let frame = encoder.raw_imu_snapshot(&snapshot); + + assert_eq!(&frame.raw_bytes()[7..10], &[27, 0, 0]); + } +} diff --git a/libs/kaptan-mavlink/src/lib.rs b/libs/kaptan-mavlink/src/lib.rs index c70d876..3dd695c 100644 --- a/libs/kaptan-mavlink/src/lib.rs +++ b/libs/kaptan-mavlink/src/lib.rs @@ -2,78 +2,16 @@ #![no_std] -use kaptan_sensor::ImuSnapshot; -use mavlink::MessageData; +/// MAVLink frame encoding API for boards. +pub mod encoder; +/// Serial-byte MAVLink receive API for boards. +pub mod receiver; +/// UART DMA MAVLink frame transmission API for boards. +pub mod transmitter; -/// An owned MAVLink 2 frame supplied by rust-mavlink. +/// An owned MAVLink 2 frame suitable for UART transmission. pub type MavlinkFrame = message::MAVLinkV2MessageRaw; -/// Owns MAVLink sender identity and packet sequence state. -#[derive(Clone, Copy, Debug, Eq, PartialEq)] -pub struct MavlinkSender { - system_id: u8, - component_id: u8, - next_sequence: u8, -} - -impl MavlinkSender { - /// Creates a sender beginning with MAVLink packet sequence zero. - pub const fn new(system_id: u8, component_id: u8) -> Self { - Self { - system_id, - component_id, - next_sequence: 0, - } - } - - /// Serializes a Kaptan heartbeat with the supplied system-health state. - pub fn heartbeat(&mut self, system_status: message::MavState) -> 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(), - system_status, - mavlink_version: 3, - }; - - self.serialize(&heartbeat) - } - - /// Serializes a standard `RAW_IMU` frame from a flight-data snapshot. - pub fn raw_imu_snapshot(&mut self, snapshot: &ImuSnapshot) -> MavlinkFrame { - let raw_imu = message::RAW_IMU_DATA { - time_usec: snapshot.timestamp.as_microseconds(), - xacc: snapshot.acceleration_milligravity[0], - yacc: snapshot.acceleration_milligravity[1], - zacc: snapshot.acceleration_milligravity[2], - xgyro: snapshot.angular_rate_milliradians_per_second[0], - ygyro: snapshot.angular_rate_milliradians_per_second[1], - zgyro: snapshot.angular_rate_milliradians_per_second[2], - xmag: 0, - ymag: 0, - zmag: 0, - }; - - self.serialize(&raw_imu) - } - - fn serialize(&mut self, message: &M) -> MavlinkFrame { - let mut raw = message::MAVLinkV2MessageRaw::new(); - raw.serialize_message_data( - message::MavHeader { - sequence: self.next_sequence, - system_id: self.system_id, - component_id: self.component_id, - }, - message, - ); - self.next_sequence = self.next_sequence.wrapping_add(1); - - raw - } -} - /// Supported MAVLink message types for Kaptan. pub mod message { pub use mavlink::MAVLinkV2MessageRaw; @@ -83,53 +21,3 @@ pub mod message { #[cfg(kaptan_has_messages)] include!(concat!(env!("OUT_DIR"), "/mod.rs")); } - -#[cfg(test)] -mod tests { - use super::MavlinkSender; - use kaptan_sensor::ImuSnapshot; - use kaptan_time::Timestamp; - - #[test] - fn heartbeat_has_the_configured_sender_and_sequence() { - let mut sender = MavlinkSender::new(42, 7); - let frame = sender.heartbeat(crate::message::MavState::MAV_STATE_STANDBY); - let bytes = frame.raw_bytes(); - - assert_eq!(bytes[0], 0xFD); - assert_eq!(bytes[2], 0); - assert_eq!(bytes[3], 0); - assert_eq!(bytes[4], 0); - assert_eq!(bytes[5], 42); - assert_eq!(bytes[6], 7); - assert_eq!(&bytes[7..10], &[0, 0, 0]); - } - - #[test] - fn heartbeat_preserves_the_requested_health_state() { - let mut sender = MavlinkSender::new(1, 1); - let frame = sender.heartbeat(crate::message::MavState::MAV_STATE_CRITICAL); - - // MAVLink 2 header is 10 octets; HEARTBEAT.system_status is payload - // octet seven according to the common dialect wire layout. - assert_eq!( - frame.raw_bytes()[17], - crate::message::MavState::MAV_STATE_CRITICAL as u8 - ); - } - - #[test] - fn snapshot_adapter_uses_flight_data_timestamp_and_units() { - let mut sender = MavlinkSender::new(1, 1); - let snapshot = ImuSnapshot::new( - Timestamp::from_microseconds(123), - [1, 2, 3], - [4, 5, 6], - 2_500, - ); - - let frame = sender.raw_imu_snapshot(&snapshot); - - assert_eq!(&frame.raw_bytes()[7..10], &[27, 0, 0]); - } -} diff --git a/libs/kaptan-mavlink/src/receiver.rs b/libs/kaptan-mavlink/src/receiver.rs new file mode 100644 index 0000000..a3df6a5 --- /dev/null +++ b/libs/kaptan-mavlink/src/receiver.rs @@ -0,0 +1,181 @@ +//! MAVLink serial-byte receive API. + +use crate::{MavlinkFrame, message}; + +/// Bounded MAVLink 2 receiver for a board's serial-byte interrupt. +/// +/// Boards pass UART bytes to [`Self::receive_byte`] and take complete, +/// CRC-valid frames from [`Self::take_received`]. The API never decodes or +/// dispatches received message payloads. +pub struct MavlinkReceiver { + frame: MavlinkFrame, + received: usize, + expected: usize, + frames: [Option; N], + front: usize, + length: usize, +} + +impl Default for MavlinkReceiver { + fn default() -> Self { + Self::new() + } +} + +impl MavlinkReceiver { + /// Creates a receiver with space for `N` complete MAVLink frames. + pub const fn new() -> Self { + Self { + frame: MavlinkFrame::new(), + received: 0, + expected: 0, + frames: [None; N], + front: 0, + length: 0, + } + } + + /// Receives one UART byte and returns whether it queued a complete frame. + /// + /// Bytes outside MAVLink 2 frames and frames with invalid CRCs are + /// discarded. If the completed-frame queue is full, the new frame is + /// dropped without displacing an earlier received frame. + pub fn receive_byte(&mut self, byte: u8) -> bool { + if self.received == 0 { + if byte == 0xfd { + self.frame.as_mut_slice()[0] = byte; + self.received = 1; + } + return false; + } + + self.frame.as_mut_slice()[self.received] = byte; + self.received += 1; + + if self.received == 10 { + let header = self.frame.as_slice(); + let incompatibility_flags = header[2]; + if incompatibility_flags & !1 != 0 { + self.reset(); + return false; + } + self.expected = 1 + 9 + usize::from(header[1]) + 2; + if incompatibility_flags & 1 != 0 { + self.expected += 13; + } + } + + if self.expected != 0 && self.received == self.expected { + let frame = core::mem::replace(&mut self.frame, MavlinkFrame::new()); + self.received = 0; + self.expected = 0; + if frame.has_valid_crc::() { + return self.push(frame); + } + } + + false + } + + /// Discards the in-progress frame after a UART transport fault. + pub fn reset(&mut self) { + self.frame = MavlinkFrame::new(); + self.received = 0; + self.expected = 0; + } + + /// Takes the oldest complete received frame, if one is available. + pub fn take_received(&mut self) -> Option { + if self.length == 0 { + return None; + } + let frame = self.frames[self.front].take(); + self.front = (self.front + 1) % N; + self.length -= 1; + frame + } + + fn push(&mut self, frame: MavlinkFrame) -> bool { + if N == 0 || self.length == N { + return false; + } + let index = (self.front + self.length) % N; + self.frames[index] = Some(frame); + self.length += 1; + true + } +} + +#[cfg(test)] +mod tests { + use super::MavlinkReceiver; + use crate::{MavlinkFrame, message}; + + fn heartbeat(sequence: u8) -> MavlinkFrame { + let mut frame = MavlinkFrame::new(); + frame.serialize_message_data( + message::MavHeader { + sequence, + system_id: 42, + component_id: 7, + }, + &message::HEARTBEAT_DATA { + custom_mode: 0, + mavtype: message::MavType::MAV_TYPE_QUADROTOR, + autopilot: message::MavAutopilot::MAV_AUTOPILOT_GENERIC, + base_mode: message::MavModeFlag::empty(), + system_status: message::MavState::MAV_STATE_STANDBY, + mavlink_version: 3, + }, + ); + frame + } + + #[test] + fn returns_complete_crc_valid_frames_unchanged() { + let expected = heartbeat(0); + let mut receiver = MavlinkReceiver::<1>::new(); + + for byte in expected.raw_bytes() { + receiver.receive_byte(*byte); + } + + assert_eq!( + receiver.take_received().unwrap().raw_bytes(), + expected.raw_bytes() + ); + } + + #[test] + fn discards_frames_with_an_invalid_crc() { + let frame = heartbeat(0); + let mut bytes = frame.into_inner(); + bytes[10] ^= 1; + let corrupt = MavlinkFrame::from_bytes_unparsed(bytes); + let mut receiver = MavlinkReceiver::<1>::new(); + + for byte in corrupt.raw_bytes() { + assert!(!receiver.receive_byte(*byte)); + } + } + + #[test] + fn preserves_arrival_order() { + let first = heartbeat(0); + let second = heartbeat(1); + let mut receiver = MavlinkReceiver::<2>::new(); + + for byte in first.raw_bytes().iter().chain(second.raw_bytes()) { + receiver.receive_byte(*byte); + } + + assert_eq!( + receiver.take_received().unwrap().raw_bytes(), + first.raw_bytes() + ); + assert_eq!( + receiver.take_received().unwrap().raw_bytes(), + second.raw_bytes() + ); + } +} diff --git a/libs/kaptan-mavlink/src/transmitter.rs b/libs/kaptan-mavlink/src/transmitter.rs new file mode 100644 index 0000000..618dcbb --- /dev/null +++ b/libs/kaptan-mavlink/src/transmitter.rs @@ -0,0 +1,134 @@ +//! MAVLink frame encoding and UART transmission API. + +use kaptan_dma::{DmaTxBufferFactory, DmaTxEngine, TxCompletion}; +use kaptan_tx::{PendingFrame, TransmitQueue}; + +use crate::MavlinkFrame; + +/// Delivery importance used for bounded MAVLink transmission admission. +pub use kaptan_tx::FrameClass; + +/// Failure while preparing or starting a MAVLink frame transfer. +#[derive(Debug)] +pub enum MavlinkTransmitFault { + /// DMA-visible frame preparation failed. + Memory(MemoryError), + /// The UART DMA route rejected a transfer start. + Engine(EngineError), +} + +/// Owns MAVLink frame admission and UART DMA lifecycle. +/// +/// Exactly one worker invokes this type. It accepts local encoded frames and +/// received frames alike; the caller owns the policy that decides which frames +/// are transmitted. +pub struct MavlinkTransmitter { + transmit: TransmitQueue, + memory: Memory, + engine: Engine, +} + +impl MavlinkTransmitter { + /// Creates an empty MAVLink transmitter. + pub const fn new(memory: Memory, engine: Engine) -> Self { + Self { + transmit: TransmitQueue::new(), + memory, + engine, + } + } +} + +impl MavlinkTransmitter +where + Memory: DmaTxBufferFactory, + Engine: DmaTxEngine, +{ + /// Admits an already-serialized MAVLink frame for UART transmission. + pub fn send_frame( + &mut self, + class: FrameClass, + frame: MavlinkFrame, + ) -> Result<(), MavlinkTransmitFault> { + let _ = self.transmit.enqueue(PendingFrame { class, frame }); + self.start_next() + } + + /// Releases a terminal DMA frame and opportunistically starts the next. + pub fn on_dma_complete(&mut self, completion: TxCompletion) { + match completion { + TxCompletion::NotPending => {} + TxCompletion::Complete => { + let _ = self.transmit.complete(); + let _ = self.start_next(); + } + TxCompletion::TransferFault | TxCompletion::Halted => { + let _ = self.transmit.fail(); + } + } + } + + fn start_next( + &mut self, + ) -> Result<(), MavlinkTransmitFault> { + let (transmit, memory, engine) = (&mut self.transmit, &mut self.memory, &mut self.engine); + transmit + .start_with(|frame| { + let buffer = memory + .prepare(frame.raw_bytes()) + .map_err(MavlinkTransmitFault::Memory)?; + engine.start(buffer).map_err(MavlinkTransmitFault::Engine) + }) + .map(|_| ()) + } +} + +#[cfg(test)] +mod tests { + use super::{FrameClass, MavlinkTransmitter}; + use crate::{encoder::MavlinkEncoder, message::MavState}; + use kaptan_dma::{DmaReadableBuffer, DmaTxBufferFactory, DmaTxEngine, TxCompletion}; + + struct Memory; + + impl DmaTxBufferFactory for Memory { + type Error = core::convert::Infallible; + + fn prepare<'a>(&mut self, bytes: &'a [u8]) -> Result, Self::Error> { + // SAFETY: this host-only test keeps the queue's active frame alive. + Ok(unsafe { DmaReadableBuffer::new_unchecked(bytes) }) + } + } + + struct Engine; + + impl DmaTxEngine for Engine { + type StartError = core::convert::Infallible; + + fn start(&mut self, _: DmaReadableBuffer<'_>) -> Result<(), Self::StartError> { + Ok(()) + } + } + + #[test] + fn transfer_fault_drops_only_the_active_frame() { + let mut encoder = MavlinkEncoder::new(1, 1); + let mut transmitter = MavlinkTransmitter::::new(Memory, Engine); + transmitter + .send_frame( + FrameClass::BestEffort, + encoder.heartbeat(MavState::MAV_STATE_STANDBY), + ) + .unwrap(); + transmitter + .send_frame( + FrameClass::BestEffort, + encoder.heartbeat(MavState::MAV_STATE_STANDBY), + ) + .unwrap(); + assert!(transmitter.transmit.is_active()); + + transmitter.on_dma_complete(TxCompletion::TransferFault); + assert!(!transmitter.transmit.is_active()); + } +} diff --git a/libs/kaptan-runtime/Cargo.toml b/libs/kaptan-runtime/Cargo.toml index 15ba519..7e39dbe 100644 --- a/libs/kaptan-runtime/Cargo.toml +++ b/libs/kaptan-runtime/Cargo.toml @@ -10,10 +10,7 @@ version.workspace = true workspace = true [dependencies] -kaptan-dma = { path = "../kaptan-dma" } kaptan-drivers = { path = "../kaptan-drivers" } -kaptan-mavlink = { path = "../kaptan-mavlink" } kaptan-sensor = { path = "../kaptan-sensor" } kaptan-spi = { path = "../kaptan-spi" } kaptan-time = { path = "../kaptan-time" } -kaptan-tx = { path = "../kaptan-tx" } diff --git a/libs/kaptan-runtime/src/lib.rs b/libs/kaptan-runtime/src/lib.rs index cc6d161..f107a6d 100644 --- a/libs/kaptan-runtime/src/lib.rs +++ b/libs/kaptan-runtime/src/lib.rs @@ -1,8 +1,9 @@ //! RTIC- and PAC-independent flight services. //! -//! Acquisition and telemetry have separate owners. Boards connect hardware -//! interrupts to their dedicated workers through bounded deferred events; they -//! must not place both services behind one priority-ceiling resource. +//! Acquisition services and board-owned output workers have separate owners. +//! Boards connect hardware interrupts to their dedicated workers through +//! bounded deferred events; they must not place unrelated work behind one +//! priority-ceiling resource. #![no_std] @@ -10,9 +11,6 @@ pub mod imu; /// Lock-free worker handoff primitives. pub mod mailbox; -/// Single-owner MAVLink and UART transmit service. -pub mod telemetry; pub use imu::{ImuEvent, ImuService}; pub use mailbox::LatestImuMailbox; -pub use telemetry::{TelemetryFault, TelemetryService}; diff --git a/libs/kaptan-runtime/src/mailbox.rs b/libs/kaptan-runtime/src/mailbox.rs index 801e634..dee0ea6 100644 --- a/libs/kaptan-runtime/src/mailbox.rs +++ b/libs/kaptan-runtime/src/mailbox.rs @@ -7,11 +7,11 @@ use kaptan_time::Timestamp; /// A latest-value IMU mailbox. /// -/// One IMU worker publishes and one telemetry worker consumes. Publishing a -/// newer sample intentionally replaces an unread older sample: telemetry is a -/// best-effort observer and must never delay acquisition. The sequence counter -/// is a seqlock; odd values indicate a writer and even values identify a -/// stable snapshot. +/// One IMU worker publishes and one output worker consumes. Publishing a newer +/// sample intentionally replaces an unread older sample: an output observer is +/// best-effort and must never delay acquisition. The sequence counter is a +/// seqlock; odd values indicate a writer and even values identify a stable +/// snapshot. pub struct LatestImuMailbox { sequence: AtomicU32, timestamp_low: AtomicU32, diff --git a/libs/kaptan-runtime/src/telemetry.rs b/libs/kaptan-runtime/src/telemetry.rs deleted file mode 100644 index d049cfe..0000000 --- a/libs/kaptan-runtime/src/telemetry.rs +++ /dev/null @@ -1,143 +0,0 @@ -//! Single-owner MAVLink transmit service. - -use kaptan_dma::{DmaTxBufferFactory, DmaTxEngine, TxCompletion}; -use kaptan_mavlink::{MavlinkFrame, MavlinkSender, message::MavState}; -use kaptan_sensor::ImuSnapshot; -use kaptan_tx::{FrameClass, PendingFrame, TransmitQueue}; - -/// Failure while preparing or starting one telemetry frame. -#[derive(Debug)] -pub enum TelemetryFault { - /// DMA-visible frame preparation failed. - Memory(MemoryError), - /// The UART DMA route rejected a transfer start. - Engine(EngineError), -} - -/// Owns MAVLink packet sequencing and UART DMA frame lifecycle. -/// -/// Exactly one worker invokes this service. It is not shared with sensor or -/// hardware IRQ execution. -pub struct TelemetryService { - mavlink: MavlinkSender, - transmit: TransmitQueue, - memory: Memory, - engine: Engine, -} - -impl TelemetryService { - /// Creates an empty telemetry service. - pub const fn new(system_id: u8, component_id: u8, memory: Memory, engine: Engine) -> Self { - Self { - mavlink: MavlinkSender::new(system_id, component_id), - transmit: TransmitQueue::new(), - memory, - engine, - } - } -} - -impl TelemetryService -where - Memory: DmaTxBufferFactory, - Engine: DmaTxEngine, -{ - /// Queues the newest accepted IMU observation. - pub fn publish_sample( - &mut self, - sample: &ImuSnapshot, - ) -> Result<(), TelemetryFault> { - let frame = self.mavlink.raw_imu_snapshot(sample); - self.enqueue_and_start(FrameClass::BestEffort, frame) - } - - /// Queues one liveness heartbeat. - pub fn publish_heartbeat( - &mut self, - ) -> Result<(), TelemetryFault> { - let frame = self.mavlink.heartbeat(MavState::MAV_STATE_STANDBY); - self.enqueue_and_start(FrameClass::Critical, frame) - } - - /// Releases a terminal DMA frame and opportunistically starts the next. - /// - /// Telemetry is best-effort: a terminal route fault drops only the active - /// frame and a rejected follow-up start is retried on later publication. - pub fn on_dma_complete(&mut self, completion: TxCompletion) { - match completion { - TxCompletion::NotPending => {} - TxCompletion::Complete => { - let _ = self.transmit.complete(); - let _ = self.start_next(); - } - TxCompletion::TransferFault | TxCompletion::Halted => { - let _ = self.transmit.fail(); - } - } - } - - fn enqueue_and_start( - &mut self, - class: FrameClass, - frame: MavlinkFrame, - ) -> Result<(), TelemetryFault> { - let _ = self.transmit.enqueue(PendingFrame { class, frame }); - self.start_next() - } - - fn start_next(&mut self) -> Result<(), TelemetryFault> { - let (transmit, memory, engine) = (&mut self.transmit, &mut self.memory, &mut self.engine); - transmit - .start_with(|frame| { - let buffer = memory - .prepare(frame.raw_bytes()) - .map_err(TelemetryFault::Memory)?; - engine.start(buffer).map_err(TelemetryFault::Engine) - }) - .map(|_| ()) - } -} - -#[cfg(test)] -mod tests { - use super::TelemetryService; - use kaptan_dma::{DmaReadableBuffer, DmaTxBufferFactory, DmaTxEngine, TxCompletion}; - use kaptan_sensor::ImuSnapshot; - use kaptan_time::Timestamp; - - struct Memory; - - impl DmaTxBufferFactory for Memory { - type Error = core::convert::Infallible; - - fn prepare<'a>(&mut self, bytes: &'a [u8]) -> Result, Self::Error> { - // SAFETY: this host-only test keeps the queue's active frame alive. - Ok(unsafe { DmaReadableBuffer::new_unchecked(bytes) }) - } - } - - struct Engine; - - impl DmaTxEngine for Engine { - type StartError = core::convert::Infallible; - - fn start(&mut self, _: DmaReadableBuffer<'_>) -> Result<(), Self::StartError> { - Ok(()) - } - } - - fn sample(timestamp: u64) -> ImuSnapshot { - ImuSnapshot::new(Timestamp::from_microseconds(timestamp), [0; 3], [0; 3], 0) - } - - #[test] - fn transfer_fault_drops_only_the_active_frame() { - let mut service = TelemetryService::::new(1, 1, Memory, Engine); - service.publish_sample(&sample(1)).unwrap(); - service.publish_sample(&sample(2)).unwrap(); - assert!(service.transmit.is_active()); - - service.on_dma_complete(TxCompletion::TransferFault); - assert!(!service.transmit.is_active()); - } -} diff --git a/libs/kaptan-stm32f7/src/stm32f722/gpio.rs b/libs/kaptan-stm32f7/src/stm32f722/gpio.rs index 2dad6c3..d8301aa 100644 --- a/libs/kaptan-stm32f7/src/stm32f722/gpio.rs +++ b/libs/kaptan-stm32f7/src/stm32f722/gpio.rs @@ -70,6 +70,16 @@ pub fn configure_usart1_tx_pin_pa9(port: &stm32f722::GPIOA) { port.pupdr().modify(|_, write| write.pupdr9().floating()); } +/// Configures PA10 for USART1 receive alternate function 7. +pub fn configure_usart1_rx_pin_pa10(port: &stm32f722::GPIOA) { + port.moder().modify(|_, write| write.moder10().alternate()); + port.afrh().modify(|_, write| { + // SAFETY: AF7 selects USART1 RX on PA10 on STM32F722. + unsafe { write.afrh10().bits(0b0111) } + }); + port.pupdr().modify(|_, write| write.pupdr10().floating()); +} + /// Active-low status LED connected to PD2. pub struct ActiveLowLedPd2 { port: stm32f722::GPIOD, diff --git a/libs/kaptan-stm32f7/src/stm32f722/mod.rs b/libs/kaptan-stm32f7/src/stm32f722/mod.rs index bb2ef04..318d154 100644 --- a/libs/kaptan-stm32f7/src/stm32f722/mod.rs +++ b/libs/kaptan-stm32f7/src/stm32f722/mod.rs @@ -16,10 +16,10 @@ pub use clock::{ClockProfileError, select_hsi16_profile}; pub use dma_buffer::DcacheDisabledDmaMemory; pub use gpio::{ ActiveLowLedPd2, configure_exti4_pin_pa4, configure_spi1_pins_pa5_pa6_pa7, - configure_usart1_tx_pin_pa9, + configure_usart1_rx_pin_pa10, configure_usart1_tx_pin_pa9, }; pub use spi1::{Spi1, Spi1Error}; pub use tim2::{Tim2Periodic, Tim2PeriodicConfig}; pub use time::{DwtClockStartError, DwtMicrosecondClock}; -pub use usart::{BaudRateError, Usart1Tx}; +pub use usart::{BaudRateError, ReceiveInterrupt, Usart1Tx}; pub use usart1_tx_dma2_stream7::{StartError, Usart1TxDma}; diff --git a/libs/kaptan-stm32f7/src/stm32f722/usart.rs b/libs/kaptan-stm32f7/src/stm32f722/usart.rs index df73f54..48bd90a 100644 --- a/libs/kaptan-stm32f7/src/stm32f722/usart.rs +++ b/libs/kaptan-stm32f7/src/stm32f722/usart.rs @@ -11,7 +11,18 @@ pub enum BaudRateError { OutOfRange, } -/// Owns configured transmit-only USART1 hardware. +/// Terminal result from acknowledging the USART1 receive interrupt. +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub enum ReceiveInterrupt { + /// One received byte was removed from the UART data register. + Byte(u8), + /// A serial transport fault invalidated the current receive stream. + Fault, + /// This USART1 interrupt did not have a receive event pending. + NotPending, +} + +/// Owns configured full-duplex USART1 hardware. /// /// Pin alternate-function setup remains board-owned because it depends on the /// schematic. Configure pins before constructing this type. @@ -20,7 +31,7 @@ pub struct Usart1Tx { } impl Usart1Tx { - /// Enables USART1 and configures 8N1 transmit-only operation. + /// Enables USART1 and configures 8N1 transmit and receive operation. pub fn new( rcc: &stm32f722::RCC, peripheral: stm32f722::USART1, @@ -38,9 +49,17 @@ impl Usart1Tx { // SAFETY: `brr` is validated to fit the complete BRR register. unsafe { write.bits(u32::from(brr)) } }); - peripheral - .cr1() - .write(|write| write.te().set_bit().ue().set_bit()); + peripheral.cr1().write(|write| { + write + .te() + .set_bit() + .re() + .set_bit() + .rxneie() + .set_bit() + .ue() + .set_bit() + }); peripheral.cr2().write(|write| write.stop().stop1()); Ok(Self { peripheral }) @@ -53,6 +72,51 @@ impl Usart1Tx { pub(crate) fn transmit_data_register_address(&self) -> u32 { self.peripheral.tdr().as_ptr() as u32 } + + /// Acknowledges one USART1 receive interrupt without accessing TX state. + /// + /// The board invokes this only from the `USART1` interrupt. USART1 TX DMA + /// owns its disjoint data register and DMA stream, so this static route may + /// safely acknowledge RX while that transfer is active. + pub fn acknowledge_receive_interrupt() -> ReceiveInterrupt { + // SAFETY: the board binds USART1 exactly once and this function touches + // only its receive/error registers, never the TX DMA configuration. + let peripheral = unsafe { &*stm32f722::USART1::ptr() }; + let status = peripheral.isr().read(); + let fault = status.pe().is_error() + || status.fe().is_error() + || status.nf().is_noise() + || status.ore().is_overrun(); + + if status.rxne().is_data_ready() { + let byte = peripheral.rdr().read().rdr().bits() as u8; + if fault { + clear_receive_errors(peripheral); + ReceiveInterrupt::Fault + } else { + ReceiveInterrupt::Byte(byte) + } + } else if fault { + clear_receive_errors(peripheral); + ReceiveInterrupt::Fault + } else { + ReceiveInterrupt::NotPending + } + } +} + +fn clear_receive_errors(peripheral: &stm32f722::usart1::RegisterBlock) { + peripheral.icr().write(|write| { + write + .pecf() + .clear() + .fecf() + .clear() + .ncf() + .clear() + .orecf() + .clear() + }); } const fn calculate_brr(peripheral_clock_hz: u32, baud: u32) -> Result {