feat(mavlink): adopt the kaptan-native dialect

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

@@ -2,21 +2,21 @@
 //!
 //! 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.
+//! supplies only mailbox references, vertical-data freshness, and a transport
+//! sink.
 
 #![no_std]
 
 use kaptan_dma::TxCompletion;
 use kaptan_err_reportable::KaptanError;
-use kaptan_mavlink::message::MessageData;
 use kaptan_mavlink::{
     MavlinkFrame,
-    encoder::{DistanceSensorConfig, MavlinkEncoder},
-    message::MavState,
+    encoder::MavlinkEncoder,
+    generated::kaptan::MavMessage,
     transmitter::{EnqueueResult, FrameClass},
 };
 use kaptan_sensor::{optical_flow::OpticalFlowSnapshot, range::RangeSnapshot};
+use mavlink_core::MessageData;
 
 mod mailbox;
 
@@ -29,10 +29,8 @@ pub use mailbox::{LatestImuMailbox, LatestOpticalFlowMailbox, LatestRangeMailbox
 /// after validating the active user configuration against immutable hardware.
 #[derive(Clone, Copy, Debug, Eq, PartialEq)]
 pub struct DataChannelRates {
-    /// Frequency of the platform service tick.
-    pub service_hz: usize,
-    /// Raw-IMU publication rate.
-    pub raw_imu_hz: usize,
+    /// IMU publication rate.
+    pub imu_hz: usize,
     /// Range publication rate; `None` disables the stream.
     pub range_hz: Option<usize>,
     /// Optical-flow publication rate; `None` disables the stream.
@@ -65,17 +63,6 @@ pub struct DataSources<'a> {
     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 {
@@ -96,7 +83,7 @@ pub struct DataChannel<'a, Sink, const SERVICE_HZ: usize> {
     encoder: MavlinkEncoder,
     schedule: Schedule<SERVICE_HZ>,
     sources: DataSources<'a>,
-    vertical: VerticalDataConfig,
+    maximum_vertical_age_ticks: u8,
     observed_imu: u32,
     observed_range: u32,
     last_range: Option<RangeSnapshot>,
@@ -116,7 +103,7 @@ where
         system_id: u8,
         component_id: u8,
         rates: DataChannelRates,
-        vertical: VerticalDataConfig,
+        maximum_vertical_age_ticks: u8,
         sources: DataSources<'a>,
     ) -> Result<Self, DataChannelError> {
         let schedule = Schedule::from_rates(rates)?;
@@ -125,7 +112,7 @@ where
             encoder: MavlinkEncoder::new(system_id, component_id),
             schedule,
             sources,
-            vertical,
+            maximum_vertical_age_ticks,
             observed_imu: 0,
             observed_range: 0,
             last_range: None,
@@ -141,43 +128,36 @@ where
     /// 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) {
+    pub fn on_service_tick(&mut self) {
         self.refresh_vertical_data();
 
         match self.schedule.next_due() {
-            Some(DueData::RawImu) => {
+            Some(DueData::Imu) => {
                 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),
-                    );
+                    let _ = self
+                        .sink
+                        .send(FrameClass::BestEffort, self.encoder.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::Distance) if self.range_age_ticks <= self.maximum_vertical_age_ticks => {
+                if let Some(range) = self.last_range {
+                    let _ = self
+                        .sink
+                        .send(FrameClass::BestEffort, self.encoder.range_snapshot(&range));
                 }
             }
             Some(DueData::OpticalFlow)
-                if self.optical_flow_age_ticks <= self.vertical.maximum_age_ticks =>
+                if self.optical_flow_age_ticks <= self.maximum_vertical_age_ticks =>
             {
-                if let (Some(flow), Some(sensor_id)) =
-                    (self.last_optical_flow, self.vertical.optical_flow_sensor_id)
-                {
+                if let Some(flow) = self.last_optical_flow {
                     let _ = self.sink.send(
                         FrameClass::BestEffort,
-                        self.encoder.optical_flow_snapshot(&flow, sensor_id),
+                        self.encoder.optical_flow_snapshot(&flow),
                     );
                 }
             }
             Some(DueData::Heartbeat) => {
-                let _ = self.sink.send(
-                    FrameClass::Vital,
-                    self.encoder.heartbeat(system_status, motors_armed),
-                );
+                let _ = self.sink.send(FrameClass::Vital, self.encoder.heartbeat());
             }
             Some(DueData::Distance | DueData::OpticalFlow) | None => {}
         }
@@ -190,8 +170,12 @@ where
 
     /// 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) -> EnqueueResult {
-        self.sink.send(class, self.encoder.custom(message))
+    pub fn send_message<M: MessageData<Message = MavMessage>>(
+        &mut self,
+        class: FrameClass,
+        message: &M,
+    ) -> EnqueueResult {
+        self.sink.send(class, self.encoder.encode(message))
     }
 
     fn refresh_vertical_data(&mut self) {
@@ -217,7 +201,7 @@ where
 #[derive(Clone, Copy, Debug, Eq, PartialEq)]
 enum DueData {
     Heartbeat,
-    RawImu,
+    Imu,
     Distance,
     OpticalFlow,
 }
@@ -233,15 +217,11 @@ impl<const SERVICE_HZ: usize> Schedule<SERVICE_HZ> {
             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::Imu, rates.imu_hz)?
         .with(DueData::Heartbeat, rates.heartbeat_hz)?;
         let schedule = match rates.range_hz {
             Some(hertz) => schedule.with(DueData::Distance, hertz)?,
@@ -297,12 +277,11 @@ impl<const SERVICE_HZ: usize> Schedule<SERVICE_HZ> {
 mod tests {
     use super::{
         DataChannel, DataChannelRates, DataSources, FrameSink, LatestImuMailbox,
-        LatestOpticalFlowMailbox, LatestRangeMailbox, VerticalDataConfig,
+        LatestOpticalFlowMailbox, LatestRangeMailbox,
     };
     use kaptan_dma::TxCompletion;
     use kaptan_mavlink::{
         MavlinkFrame,
-        message::MavState,
         transmitter::{EnqueueResult, FrameClass},
     };
     use kaptan_sensor::{
@@ -332,8 +311,7 @@ mod tests {
     }
 
     const RATES: DataChannelRates = DataChannelRates {
-        service_hz: 100,
-        raw_imu_hz: 20,
+        imu_hz: 20,
         range_hz: Some(20),
         optical_flow_hz: Some(20),
         heartbeat_hz: 2,
@@ -353,17 +331,7 @@ mod tests {
             1,
             1,
             RATES,
-            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,
-            },
+            10,
             DataSources {
                 imu: &imu,
                 range: &range,
@@ -386,7 +354,7 @@ mod tests {
                 100,
                 100,
             ));
-            channel.on_service_tick(MavState::MAV_STATE_STANDBY, false);
+            channel.on_service_tick();
         }
         channel.on_dma_complete(TxCompletion::Complete);
         let sink = channel.sink;