libs/kaptan-data-channel: own outbound data flow

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" }

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<DistanceSensorConfig>,
+    /// MTF-01 sensor ID for `OPTICAL_FLOW`; `None` disables it.
+    pub optical_flow_sensor_id: Option<u8>,
+    /// 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<SERVICE_HZ>,
+    sources: DataSources<'a>,
+    vertical: VerticalDataConfig,
+    observed_imu: u32,
+    observed_range: u32,
+    last_range: Option<RangeSnapshot>,
+    range_age_ticks: u8,
+    observed_optical_flow: u32,
+    last_optical_flow: Option<OpticalFlowSnapshot>,
+    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<Self, DataChannelError> {
+        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<const SERVICE_HZ: usize> {
+    phase: usize,
+    due: [Option<DueData>; SERVICE_HZ],
+}
+
+impl<const SERVICE_HZ: usize> Schedule<SERVICE_HZ> {
+    fn from_rates(rates: DataChannelRates) -> Result<Self, DataChannelError> {
+        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<DueData> {
+        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<Self, DataChannelError> {
+        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);
+    }
+}

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<const WORDS: usize>: 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<Value, const WORDS: usize> {
+    sequence: AtomicU32,
+    words: [AtomicU32; WORDS],
+    value: PhantomData<Value>,
+}
+
+impl<Value: MailboxValue<WORDS>, const WORDS: usize> LatestMailbox<Value, WORDS> {
+    /// 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<Value> {
+        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<Value: MailboxValue<WORDS>, const WORDS: usize> Default for LatestMailbox<Value, WORDS> {
+    fn default() -> Self {
+        Self::new()
+    }
+}
+
+/// Latest-value mailbox for IMU data-channel snapshots.
+pub type LatestImuMailbox = LatestMailbox<ImuSnapshot, 9>;
+/// Latest-value mailbox for range data-channel snapshots.
+pub type LatestRangeMailbox = LatestMailbox<RangeSnapshot, 3>;
+/// Latest-value mailbox for optical-flow data-channel snapshots.
+pub type LatestOpticalFlowMailbox = LatestMailbox<OpticalFlowSnapshot, 6>;
+
+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));
+    }
+}