From 2ae66c49a8d9b21ae61c07a5785030b465108262 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Onur=20=C3=96zkan?= Date: Sat, 1 Aug 2026 20:54:09 +0300 Subject: [PATCH] libs/kaptan-data-channel: own outbound data flow --- libs/kaptan-data-channel/Cargo.toml | 17 ++ libs/kaptan-data-channel/src/lib.rs | 379 ++++++++++++++++++++++++ libs/kaptan-data-channel/src/mailbox.rs | 227 ++++++++++++++ 3 files changed, 623 insertions(+) create mode 100644 libs/kaptan-data-channel/Cargo.toml create mode 100644 libs/kaptan-data-channel/src/lib.rs create mode 100644 libs/kaptan-data-channel/src/mailbox.rs diff --git a/libs/kaptan-data-channel/Cargo.toml b/libs/kaptan-data-channel/Cargo.toml new file mode 100644 index 0000000..d67b370 --- /dev/null +++ b/libs/kaptan-data-channel/Cargo.toml @@ -0,0 +1,17 @@ +[package] +name = "kaptan-data-channel" +edition.workspace = true +license.workspace = true +repository.workspace = true +rust-version.workspace = true +version.workspace = true + +[lints] +workspace = true + +[dependencies] +kaptan-board = { path = "../kaptan-board" } +kaptan-dma = { path = "../kaptan-dma" } +kaptan-mavlink = { path = "../kaptan-mavlink" } +kaptan-sensor = { path = "../kaptan-sensor" } +kaptan-time = { path = "../kaptan-time" } diff --git a/libs/kaptan-data-channel/src/lib.rs b/libs/kaptan-data-channel/src/lib.rs new file mode 100644 index 0000000..754fede --- /dev/null +++ b/libs/kaptan-data-channel/src/lib.rs @@ -0,0 +1,379 @@ +//! Complete outbound MAVLink data flow. +//! +//! This crate owns fixed-rate scheduling, mailbox consumption, vertical-data +//! freshness, MAVLink encoding, and bounded queue classification. A platform +//! supplies only mailbox references, health/arming state, and a transport sink. + +#![no_std] + +use kaptan_board::{BoardConfig, DataChannelRates}; +use kaptan_dma::TxCompletion; +use kaptan_mavlink::{ + MavlinkFrame, + encoder::{DistanceSensorConfig, MavlinkEncoder}, + message::{MavCmd, MavResult, MavState}, + transmitter::FrameClass, +}; +use kaptan_sensor::{optical_flow::OpticalFlowSnapshot, range::RangeSnapshot}; + +mod mailbox; + +pub use mailbox::{LatestImuMailbox, LatestOpticalFlowMailbox, LatestRangeMailbox}; + +/// Platform transport used by a data channel. +/// +/// The channel decides exactly which encoded frame is admitted and with which +/// delivery class. The platform retains only DMA endpoint ownership. +pub trait FrameSink { + /// Admits one fully encoded frame to the platform's bounded transport. + fn send(&mut self, class: FrameClass, frame: MavlinkFrame); + + /// Releases or faults the active DMA frame after a terminal interrupt. + fn on_dma_complete(&mut self, completion: TxCompletion); +} + +/// Mailboxes read by one data-channel worker. +#[derive(Clone, Copy)] +pub struct DataSources<'a> { + /// Latest IMU observation from the acquisition worker. + pub imu: &'a LatestImuMailbox, + /// Latest range observation from either supported vertical route. + pub range: &'a LatestRangeMailbox, + /// Latest optical-flow observation from MTF-01. + pub optical_flow: &'a LatestOpticalFlowMailbox, +} + +/// Vertical-sensor metadata and freshness policy for an outbound data channel. +#[derive(Clone, Copy)] +pub struct VerticalDataConfig { + /// Metadata required to encode `DISTANCE_SENSOR`; `None` disables it. + pub distance: Option, + /// MTF-01 sensor ID for `OPTICAL_FLOW`; `None` disables it. + pub optical_flow_sensor_id: Option, + /// Maximum age, in service ticks, for cached vertical observations. + pub maximum_age_ticks: u8, +} + +/// Data-channel construction failure. +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub enum DataChannelError { + /// A configured channel must be due at least once per second. + ZeroRate, + /// A configured channel exceeds the service frequency. + RateExceedsServiceFrequency, + /// Two configured channels cannot share the one-frame-per-tick budget. + ScheduleFull, +} + +/// Complete outbound data flow for one MAVLink endpoint. +pub struct DataChannel<'a, Sink, const SERVICE_HZ: usize> { + sink: Sink, + encoder: MavlinkEncoder, + schedule: Schedule, + sources: DataSources<'a>, + vertical: VerticalDataConfig, + observed_imu: u32, + observed_range: u32, + last_range: Option, + range_age_ticks: u8, + observed_optical_flow: u32, + last_optical_flow: Option, + optical_flow_age_ticks: u8, +} + +impl<'a, Sink, const SERVICE_HZ: usize> DataChannel<'a, Sink, SERVICE_HZ> +where + Sink: FrameSink, +{ + /// Creates an outbound data flow from validated portable board metadata. + pub fn new( + sink: Sink, + config: BoardConfig, + vertical: VerticalDataConfig, + sources: DataSources<'a>, + ) -> Result { + let schedule = Schedule::from_rates(config.data_channel)?; + Ok(Self { + sink, + encoder: MavlinkEncoder::new(config.mavlink.system_id, config.mavlink.component_id), + schedule, + sources, + vertical, + observed_imu: 0, + observed_range: 0, + last_range: None, + range_age_ticks: u8::MAX, + observed_optical_flow: 0, + last_optical_flow: None, + optical_flow_age_ticks: u8::MAX, + }) + } + + /// Runs all outbound work due for one platform service tick. + /// + /// No schedule token escapes this method. It refreshes cached vertical + /// observations, selects due data, encodes the MAVLink message, and admits + /// it with the correct bounded-queue class. + pub fn on_service_tick(&mut self, imu_healthy: bool, motors_armed: bool) { + self.refresh_vertical_data(); + + match self.schedule.next_due() { + Some(DueData::RawImu) => { + if let Some(sample) = self.sources.imu.take_newer(&mut self.observed_imu) { + self.sink.send( + FrameClass::BestEffort, + self.encoder.raw_imu_snapshot(&sample), + ); + } + } + Some(DueData::Distance) if self.range_age_ticks <= self.vertical.maximum_age_ticks => { + if let (Some(range), Some(config)) = (self.last_range, self.vertical.distance) { + self.sink.send( + FrameClass::BestEffort, + self.encoder.distance_sensor_snapshot(&range, config), + ); + } + } + Some(DueData::OpticalFlow) + if self.optical_flow_age_ticks <= self.vertical.maximum_age_ticks => + { + if let (Some(flow), Some(sensor_id)) = + (self.last_optical_flow, self.vertical.optical_flow_sensor_id) + { + self.sink.send( + FrameClass::BestEffort, + self.encoder.optical_flow_snapshot(&flow, sensor_id), + ); + } + } + Some(DueData::Heartbeat) => self.sink.send( + FrameClass::Critical, + self.encoder.heartbeat( + if imu_healthy { + MavState::MAV_STATE_STANDBY + } else { + MavState::MAV_STATE_CRITICAL + }, + motors_armed, + ), + ), + Some(DueData::Distance | DueData::OpticalFlow) | None => {} + } + } + + /// Completes the active DMA lifecycle for this channel's transport. + pub fn on_dma_complete(&mut self, completion: TxCompletion) { + self.sink.on_dma_complete(completion); + } + + /// Emits a critical acknowledgement for a received MAVLink command. + pub fn acknowledge_command(&mut self, command: MavCmd, result: MavResult) { + self.sink.send( + FrameClass::Critical, + self.encoder.command_ack(command, result), + ); + } + + fn refresh_vertical_data(&mut self) { + if let Some(range) = self.sources.range.take_newer(&mut self.observed_range) { + self.last_range = Some(range); + self.range_age_ticks = 0; + } else { + self.range_age_ticks = self.range_age_ticks.saturating_add(1); + } + if let Some(flow) = self + .sources + .optical_flow + .take_newer(&mut self.observed_optical_flow) + { + self.last_optical_flow = Some(flow); + self.optical_flow_age_ticks = 0; + } else { + self.optical_flow_age_ticks = self.optical_flow_age_ticks.saturating_add(1); + } + } +} + +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +enum DueData { + Heartbeat, + RawImu, + Distance, + OpticalFlow, +} + +struct Schedule { + phase: usize, + due: [Option; SERVICE_HZ], +} + +impl Schedule { + fn from_rates(rates: DataChannelRates) -> Result { + assert!( + SERVICE_HZ != 0, + "data-channel service frequency must be nonzero" + ); + if rates.service_hz != SERVICE_HZ { + return Err(DataChannelError::RateExceedsServiceFrequency); + } + + let schedule = Self { + phase: 0, + due: [None; SERVICE_HZ], + } + .with(DueData::RawImu, rates.raw_imu_hz)? + .with(DueData::Heartbeat, rates.heartbeat_hz)?; + let schedule = match rates.range_hz { + Some(hertz) => schedule.with(DueData::Distance, hertz)?, + None => schedule, + }; + match rates.optical_flow_hz { + Some(hertz) => schedule.with(DueData::OpticalFlow, hertz), + None => Ok(schedule), + } + } + + fn next_due(&mut self) -> Option { + let due = self.due[self.phase]; + self.phase = if self.phase + 1 == SERVICE_HZ { + 0 + } else { + self.phase + 1 + }; + due + } + + fn with(mut self, data: DueData, hertz: usize) -> Result { + if hertz == 0 { + return Err(DataChannelError::ZeroRate); + } + if hertz > SERVICE_HZ { + return Err(DataChannelError::RateExceedsServiceFrequency); + } + + let mut count = 0; + while count < hertz { + let mut index = (count * SERVICE_HZ) / hertz; + let mut probes = 0; + while self.due[index].is_some() { + index = if index + 1 == SERVICE_HZ { + 0 + } else { + index + 1 + }; + probes += 1; + if probes == SERVICE_HZ { + return Err(DataChannelError::ScheduleFull); + } + } + self.due[index] = Some(data); + count += 1; + } + Ok(self) + } +} + +#[cfg(test)] +mod tests { + use super::{ + DataChannel, DataSources, FrameSink, LatestImuMailbox, LatestOpticalFlowMailbox, + LatestRangeMailbox, VerticalDataConfig, + }; + use kaptan_board::{BoardConfig, DataChannelRates, MavlinkEndpoint, VerticalSensingKind}; + use kaptan_dma::TxCompletion; + use kaptan_mavlink::{MavlinkFrame, transmitter::FrameClass}; + use kaptan_sensor::{ + imu::ImuSnapshot, optical_flow::OpticalFlowSnapshot, range::RangeSnapshot, + }; + use kaptan_time::Timestamp; + + struct Sink { + best_effort: u8, + critical: u8, + completions: u8, + } + + impl FrameSink for Sink { + fn send(&mut self, class: FrameClass, _: MavlinkFrame) { + match class { + FrameClass::BestEffort => self.best_effort += 1, + FrameClass::Critical => self.critical += 1, + } + } + + fn on_dma_complete(&mut self, _: TxCompletion) { + self.completions += 1; + } + } + + const CONFIG: BoardConfig = BoardConfig { + name: "test", + 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, + }, + }; + + #[test] + fn owns_rate_selection_encoding_admission_and_completion() { + let imu = LatestImuMailbox::new(); + let range = LatestRangeMailbox::new(); + let flow = LatestOpticalFlowMailbox::new(); + let mut channel: DataChannel<'_, Sink, 100> = DataChannel::new( + Sink { + best_effort: 0, + critical: 0, + completions: 0, + }, + CONFIG, + VerticalDataConfig { + distance: Some(kaptan_mavlink::encoder::DistanceSensorConfig { + sensor_id: 0, + orientation: + kaptan_mavlink::message::MavSensorOrientation::MAV_SENSOR_ROTATION_NONE, + minimum_distance_centimetres: 1, + maximum_distance_centimetres: 1_000, + }), + optical_flow_sensor_id: Some(0), + maximum_age_ticks: 10, + }, + DataSources { + imu: &imu, + range: &range, + optical_flow: &flow, + }, + ) + .unwrap(); + + for tick in 0..100 { + imu.publish(ImuSnapshot::new( + Timestamp::from_microseconds(tick), + [0; 3], + [0; 3], + 0, + )); + range.publish(RangeSnapshot::new(Timestamp::from_microseconds(tick), 100)); + flow.publish(OpticalFlowSnapshot::new( + Timestamp::from_microseconds(tick), + [0; 2], + 100, + 100, + )); + channel.on_service_tick(true, false); + } + channel.on_dma_complete(TxCompletion::Complete); + + let sink = channel.sink; + assert_eq!(sink.best_effort, 60); + assert_eq!(sink.critical, 2); + assert_eq!(sink.completions, 1); + } +} diff --git a/libs/kaptan-data-channel/src/mailbox.rs b/libs/kaptan-data-channel/src/mailbox.rs new file mode 100644 index 0000000..8f604cb --- /dev/null +++ b/libs/kaptan-data-channel/src/mailbox.rs @@ -0,0 +1,227 @@ +//! Lock-free single-producer, single-consumer latest-value handoff. + +use core::{ + marker::PhantomData, + sync::atomic::{AtomicU32, Ordering}, +}; + +use kaptan_sensor::{imu::ImuSnapshot, optical_flow::OpticalFlowSnapshot, range::RangeSnapshot}; +use kaptan_time::Timestamp; + +/// Converts an observation to the atomic word representation used by a +/// [`LatestMailbox`]. +/// +/// This is a data-channel handoff contract, not a sensor-domain contract: +/// sensor crates own their fields and units, while this crate owns their +/// lock-free representation between an acquisition producer and consumer. +pub trait MailboxValue: Copy { + /// Converts this observation into its handoff representation. + fn into_words(self) -> [u32; WORDS]; + + /// Reconstructs an observation from its handoff representation. + fn from_words(words: [u32; WORDS]) -> Self; +} + +/// Lock-free latest-value SPSC mailbox. +/// +/// Publishing a newer value intentionally replaces an unread older value: the +/// output path 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. Exactly one producer and one consumer may use an +/// instance concurrently. Sequence operations are sequentially consistent so +/// a reader cannot validate an old even sequence around concurrently written +/// atomic payload words; the payload words themselves need only be relaxed. +pub struct LatestMailbox { + sequence: AtomicU32, + words: [AtomicU32; WORDS], + value: PhantomData, +} + +impl, const WORDS: usize> LatestMailbox { + /// Creates an empty mailbox. + pub const fn new() -> Self { + Self { + sequence: AtomicU32::new(0), + words: [const { AtomicU32::new(0) }; WORDS], + value: PhantomData, + } + } + + /// Publishes a snapshot, replacing any unread older snapshot. + pub fn publish(&self, value: Value) { + let sequence = self.sequence.fetch_add(1, Ordering::SeqCst); + debug_assert_eq!(sequence & 1, 0, "SPSC producer must not reenter"); + + for (destination, word) in self.words.iter().zip(value.into_words()) { + destination.store(word, Ordering::Relaxed); + } + + self.sequence + .store(sequence.wrapping_add(2), Ordering::SeqCst); + } + + /// Returns the newest stable snapshot different from `observed_sequence`. + pub fn take_newer(&self, observed_sequence: &mut u32) -> Option { + let before = self.sequence.load(Ordering::SeqCst); + if before == *observed_sequence || before & 1 != 0 { + return None; + } + + let mut words = [0; WORDS]; + for (destination, source) in words.iter_mut().zip(&self.words) { + *destination = source.load(Ordering::Relaxed); + } + + if self.sequence.load(Ordering::SeqCst) != before { + return None; + } + + *observed_sequence = before; + Some(Value::from_words(words)) + } +} + +impl, const WORDS: usize> Default for LatestMailbox { + fn default() -> Self { + Self::new() + } +} + +/// Latest-value mailbox for IMU data-channel snapshots. +pub type LatestImuMailbox = LatestMailbox; +/// Latest-value mailbox for range data-channel snapshots. +pub type LatestRangeMailbox = LatestMailbox; +/// Latest-value mailbox for optical-flow data-channel snapshots. +pub type LatestOpticalFlowMailbox = LatestMailbox; + +impl MailboxValue<9> for ImuSnapshot { + fn into_words(self) -> [u32; 9] { + let timestamp = self.timestamp.as_microseconds(); + [ + timestamp as u32, + (timestamp >> 32) as u32, + self.acceleration_milligravity[0] as u16 as u32, + self.acceleration_milligravity[1] as u16 as u32, + self.acceleration_milligravity[2] as u16 as u32, + self.angular_rate_milliradians_per_second[0] as u16 as u32, + self.angular_rate_milliradians_per_second[1] as u16 as u32, + self.angular_rate_milliradians_per_second[2] as u16 as u32, + self.temperature_centidegrees_celsius as u16 as u32, + ] + } + + fn from_words(words: [u32; 9]) -> Self { + Self::new( + timestamp_from_words(words[0], words[1]), + [ + words[2] as u16 as i16, + words[3] as u16 as i16, + words[4] as u16 as i16, + ], + [ + words[5] as u16 as i16, + words[6] as u16 as i16, + words[7] as u16 as i16, + ], + words[8] as u16 as i16, + ) + } +} + +impl MailboxValue<3> for RangeSnapshot { + fn into_words(self) -> [u32; 3] { + let timestamp = self.timestamp.as_microseconds(); + [ + timestamp as u32, + (timestamp >> 32) as u32, + self.distance_millimetres, + ] + } + + fn from_words(words: [u32; 3]) -> Self { + Self::new(timestamp_from_words(words[0], words[1]), words[2]) + } +} + +impl MailboxValue<6> for OpticalFlowSnapshot { + fn into_words(self) -> [u32; 6] { + let timestamp = self.timestamp.as_microseconds(); + [ + timestamp as u32, + (timestamp >> 32) as u32, + self.velocity_millimetres_per_second[0] as u32, + self.velocity_millimetres_per_second[1] as u32, + self.ground_distance_millimetres, + u32::from(self.quality), + ] + } + + fn from_words(words: [u32; 6]) -> Self { + Self::new( + timestamp_from_words(words[0], words[1]), + [words[2] as i32, words[3] as i32], + words[4], + words[5] as u8, + ) + } +} + +const fn timestamp_from_words(low: u32, high: u32) -> Timestamp { + Timestamp::from_microseconds((low as u64) | ((high as u64) << 32)) +} + +#[cfg(test)] +mod tests { + use super::{LatestImuMailbox, LatestOpticalFlowMailbox, LatestRangeMailbox}; + use kaptan_sensor::{ + imu::ImuSnapshot, optical_flow::OpticalFlowSnapshot, range::RangeSnapshot, + }; + use kaptan_time::Timestamp; + + #[test] + fn publishes_only_the_newest_unread_imu_sample() { + let mailbox = LatestImuMailbox::new(); + let mut observed = 0; + mailbox.publish(ImuSnapshot::new( + Timestamp::from_microseconds(1), + [1; 3], + [2; 3], + 3, + )); + mailbox.publish(ImuSnapshot::new( + Timestamp::from_microseconds(2), + [4; 3], + [5; 3], + 6, + )); + + let sample = mailbox.take_newer(&mut observed).unwrap(); + assert_eq!(sample.timestamp, Timestamp::from_microseconds(2)); + assert_eq!(sample.acceleration_milligravity, [4; 3]); + assert!(mailbox.take_newer(&mut observed).is_none()); + } + + #[test] + fn publishes_only_the_newest_unread_range_sample() { + let mailbox = LatestRangeMailbox::new(); + let mut observed = 0; + mailbox.publish(RangeSnapshot::new(Timestamp::from_microseconds(1), 100)); + mailbox.publish(RangeSnapshot::new(Timestamp::from_microseconds(2), 200)); + + assert_eq!( + mailbox.take_newer(&mut observed), + Some(RangeSnapshot::new(Timestamp::from_microseconds(2), 200)) + ); + assert!(mailbox.take_newer(&mut observed).is_none()); + } + + #[test] + fn preserves_optical_flow_fields() { + let mailbox = LatestOpticalFlowMailbox::new(); + let mut observed = 0; + let sample = OpticalFlowSnapshot::new(Timestamp::from_microseconds(1), [-6, 7], 8, 9); + mailbox.publish(sample); + + assert_eq!(mailbox.take_newer(&mut observed), Some(sample)); + } +}