refactor: move firmware workspace under firmware

firmware/libs/kaptan-data-channel/src/lib.rs

@@ -0,0 +1,497 @@
+//! 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, a declared MAVLink state, arming state,
+//! and a transport sink.
+
+#![no_std]
+
+use kaptan_board::{BoardConfig, DataChannelRates};
+use kaptan_dma::TxCompletion;
+use kaptan_log::KaptanError;
+use kaptan_mavlink::message::MessageData;
+use kaptan_mavlink::{
+    MavlinkFrame,
+    encoder::{DistanceSensorConfig, MavlinkEncoder},
+    message::{MavCmd, MavResult, MavState},
+    transmitter::FrameClass,
+    transmitter::TransmitFaultKind,
+};
+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) -> FrameAdmission;
+
+    /// Releases or faults the active DMA frame after a terminal interrupt.
+    fn on_dma_complete(&mut self, completion: TxCompletion) -> DmaCompletion;
+
+    /// Returns unconfigurable sole-link loss counters.
+    fn health(&self) -> TxHealth;
+
+    /// Removes one exact terminal transport-fault category for the central
+    /// observability owner. It is never encoded from an ISR or transport
+    /// driver directly.
+    fn take_transport_fault(&mut self) -> Option<TransmitFaultKind>;
+}
+
+/// Outcome returned to the one platform telemetry owner for each attempted
+/// frame admission. It never triggers logging recursively.
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+pub enum FrameAdmission {
+    /// The queue retained this encoded frame, possibly after replacing a
+    /// pending lower-priority frame.
+    Retained(kaptan_mavlink::transmitter::EnqueueResult),
+}
+
+impl FrameAdmission {
+    /// Whether the frame is represented by the lower transport and the log
+    /// hub may acknowledge its source record.
+    pub const fn is_retained(self) -> bool {
+        matches!(
+            self,
+            Self::Retained(
+                kaptan_mavlink::transmitter::EnqueueResult::Enqueued
+                    | kaptan_mavlink::transmitter::EnqueueResult::ReplacedBestEffort
+                    | kaptan_mavlink::transmitter::EnqueueResult::ReplacedCritical
+            )
+        )
+    }
+
+    /// Whether no pending queue slot accepted the source frame.
+    pub const fn is_queue_full(self) -> bool {
+        matches!(
+            self,
+            Self::Retained(
+                kaptan_mavlink::transmitter::EnqueueResult::VitalQueueFull
+                    | kaptan_mavlink::transmitter::EnqueueResult::CriticalQueueFull
+                    | kaptan_mavlink::transmitter::EnqueueResult::BestEffortDropped
+            )
+        )
+    }
+}
+
+/// Terminal outcome from the sole DMA owner.
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+pub enum DmaCompletion {
+    /// No new transport fault was observed.
+    Complete,
+    /// A DMA terminal or restart fault was counted by the platform.
+    Fault,
+}
+
+/// Unconfigurable loss counters from sole-link admission/DMA ownership.
+#[derive(Clone, Copy, Debug, Default, Eq, PartialEq)]
+pub struct TxHealth {
+    /// Dropped or displaced pending frames, including vital admission loss.
+    pub admission_loss_count: u32,
+    /// DMA preparation/start/terminal transfer faults.
+    pub dma_fault_count: u32,
+    /// DMA-visible memory preparation faults.
+    pub memory_fault_count: u32,
+    /// DMA-engine transfer-start faults.
+    pub engine_fault_count: u32,
+    /// Active-DMA terminal faults or halts.
+    pub terminal_fault_count: u32,
+}
+
+/// 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, KaptanError)]
+pub enum DataChannelError {
+    /// Rejected profiles must not construct a physical telemetry channel.
+    NotTelemetryProfile,
+    /// 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 telemetry = config
+            .telemetry()
+            .ok_or(DataChannelError::NotTelemetryProfile)?;
+        let schedule = Schedule::from_rates(telemetry.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, system_status: MavState, 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) {
+                    let _ = 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) {
+                    let _ = 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)
+                {
+                    let _ = self.sink.send(
+                        FrameClass::BestEffort,
+                        self.encoder.optical_flow_snapshot(&flow, sensor_id),
+                    );
+                }
+            }
+            Some(DueData::Heartbeat) => {
+                let _ = self.sink.send(
+                    FrameClass::Vital,
+                    self.encoder.heartbeat(system_status, 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) -> DmaCompletion {
+        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) {
+        let _ = self.sink.send(
+            FrameClass::Critical,
+            self.encoder.command_ack(command, result),
+        );
+    }
+
+    /// Encodes an adapter-owned generated MAVLink data record through the
+    /// channel's sole encoder and bounded transport sink.
+    pub fn send_custom<M: MessageData>(
+        &mut self,
+        class: FrameClass,
+        message: &M,
+    ) -> FrameAdmission {
+        self.sink.send(class, self.encoder.custom(message))
+    }
+
+    /// Returns local sole-link accounting for platform diagnostics. It is not
+    /// serialized through Kaptan's bounded diagnostic messages.
+    pub fn transport_health(&self) -> TxHealth {
+        self.sink.health()
+    }
+
+    /// Removes one exact sole-link transport fault category after the platform
+    /// has counted it. The platform maps it to a typed log record on its
+    /// bounded telemetry worker.
+    pub fn take_transport_fault(&mut self) -> Option<TransmitFaultKind> {
+        self.sink.take_transport_fault()
+    }
+
+    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, DmaCompletion, FrameAdmission, FrameSink, LatestImuMailbox,
+        LatestOpticalFlowMailbox, LatestRangeMailbox, TransmitFaultKind, TxHealth,
+        VerticalDataConfig,
+    };
+    use kaptan_board::{
+        BoardConfig, DataChannelRates, ImuDevice, ImuMount, MavlinkEndpoint, MavlinkPort,
+        VerticalSensingKind,
+    };
+    use kaptan_dma::TxCompletion;
+    use kaptan_log::Severity;
+    use kaptan_mavlink::{MavlinkFrame, message::MavState, 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) -> FrameAdmission {
+            match class {
+                FrameClass::Vital => self.critical += 1,
+                FrameClass::BestEffort => self.best_effort += 1,
+                FrameClass::Critical => self.critical += 1,
+            }
+            FrameAdmission::Retained(kaptan_mavlink::transmitter::EnqueueResult::Enqueued)
+        }
+
+        fn on_dma_complete(&mut self, _: TxCompletion) -> DmaCompletion {
+            self.completions += 1;
+            DmaCompletion::Complete
+        }
+
+        fn health(&self) -> TxHealth {
+            TxHealth::default()
+        }
+
+        fn take_transport_fault(&mut self) -> Option<TransmitFaultKind> {
+            None
+        }
+    }
+
+    const CONFIG: BoardConfig = BoardConfig {
+        name: "test",
+        mavlink: MavlinkEndpoint {
+            system_id: 1,
+            component_id: 1,
+            port: MavlinkPort::Serial1,
+            baud: 115_200,
+        },
+        profile: kaptan_board::BoardProfile::TelemetryOnly(kaptan_board::TelemetryProfile {
+            imu: ImuDevice::Mpu6000,
+            imu_mount: ImuMount::BodyAligned,
+            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,
+            },
+            maximum_log_level: Severity::Info,
+        }),
+    };
+
+    #[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(MavState::MAV_STATE_STANDBY, 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);
+    }
+}