libs/kaptan-mavlink: add distance sensor telemetry

libs/kaptan-mavlink/src/encoder.rs

@@ -1,6 +1,7 @@
 //! MAVLink frame encoding API.
 
-use kaptan_sensor::ImuSnapshot;
+use kaptan_sensor::imu::ImuSnapshot;
+use kaptan_sensor::range::RangeSnapshot;
 use mavlink::MessageData;
 
 use crate::{MavlinkFrame, message};
@@ -81,13 +82,44 @@ impl MavlinkEncoder {
 
         self.serialize(&raw_imu)
     }
+
+    /// Encodes a standard `DISTANCE_SENSOR` frame for a laser rangefinder.
+    ///
+    /// MAVLink represents distance in centimetres; the sensor domain keeps
+    /// millimetres so this conversion happens only at the protocol boundary.
+    pub fn distance_sensor_snapshot(
+        &mut self,
+        snapshot: &RangeSnapshot,
+        sensor_id: u8,
+        orientation: message::MavSensorOrientation,
+        minimum_distance_centimetres: u16,
+        maximum_distance_centimetres: u16,
+    ) -> MavlinkFrame {
+        let current_distance = (snapshot.distance_millimetres / 10).clamp(
+            u32::from(minimum_distance_centimetres),
+            u32::from(maximum_distance_centimetres),
+        ) as u16;
+        let distance_sensor = message::DISTANCE_SENSOR_DATA {
+            time_boot_ms: (snapshot.timestamp.as_microseconds() / 1_000) as u32,
+            min_distance: minimum_distance_centimetres,
+            max_distance: maximum_distance_centimetres,
+            current_distance,
+            mavtype: message::MavDistanceSensor::MAV_DISTANCE_SENSOR_LASER,
+            id: sensor_id,
+            orientation,
+            covariance: u8::MAX,
+        };
+
+        self.serialize(&distance_sensor)
+    }
 }
 
 #[cfg(test)]
 mod tests {
     use super::MavlinkEncoder;
     use crate::message::MavState;
-    use kaptan_sensor::ImuSnapshot;
+    use kaptan_sensor::imu::ImuSnapshot;
+    use kaptan_sensor::range::RangeSnapshot;
     use kaptan_time::Timestamp;
 
     #[test]
@@ -135,4 +167,24 @@ mod tests {
 
         assert_ne!(frame.raw_bytes()[16] & 0x80, 0);
     }
+
+    #[test]
+    fn range_snapshot_uses_distance_sensor_centimetres() {
+        let mut encoder = MavlinkEncoder::new(1, 1);
+        let snapshot = RangeSnapshot::new(Timestamp::from_microseconds(123_000), 2_910);
+
+        let frame = encoder.distance_sensor_snapshot(
+            &snapshot,
+            0,
+            crate::message::MavSensorOrientation::MAV_SENSOR_ROTATION_PITCH_270,
+            20,
+            4_000,
+        );
+        let bytes = frame.raw_bytes();
+
+        assert_eq!(&bytes[7..10], &[132, 0, 0]);
+        // MAVLink payload starts after the ten-byte MAVLink 2 header. The
+        // current distance field follows time, minimum, and maximum range.
+        assert_eq!(&bytes[18..20], &291u16.to_le_bytes());
+    }
 }

libs/kaptan-mavlink/src/transmitter.rs

@@ -6,7 +6,7 @@ use kaptan_tx::{PendingFrame, TransmitQueue};
 use crate::MavlinkFrame;
 
 /// Delivery importance used for bounded MAVLink transmission admission.
-pub use kaptan_tx::FrameClass;
+pub use kaptan_tx::{EnqueueResult, FrameClass};
 
 /// Failure while preparing or starting a MAVLink frame transfer.
 #[derive(Debug)]
@@ -45,13 +45,17 @@ where
     Engine: DmaTxEngine,
 {
     /// Admits an already-serialized MAVLink frame for UART transmission.
+    ///
+    /// A full queue is a normal bounded-admission outcome, returned as
+    /// [`EnqueueResult`]. DMA preparation and transfer-start failures remain
+    /// [`MavlinkTransmitFault`] errors.
     pub fn send_frame(
         &mut self,
         class: FrameClass,
         frame: MavlinkFrame,
-    ) -> Result<(), MavlinkTransmitFault<Memory::Error, Engine::StartError>> {
-        let _ = self.transmit.enqueue(PendingFrame { class, frame });
-        self.start_next()
+    ) -> Result<EnqueueResult, MavlinkTransmitFault<Memory::Error, Engine::StartError>> {
+        let admission = self.transmit.enqueue(PendingFrame { class, frame });
+        self.start_next().map(|()| admission)
     }
 
     /// Releases a terminal DMA frame and opportunistically starts the next.
@@ -85,7 +89,7 @@ where
 
 #[cfg(test)]
 mod tests {
-    use super::{FrameClass, MavlinkTransmitter};
+    use super::{EnqueueResult, FrameClass, MavlinkTransmitter};
     use crate::{encoder::MavlinkEncoder, message::MavState};
     use kaptan_dma::{DmaReadableBuffer, DmaTxBufferFactory, DmaTxEngine, TxCompletion};
 
@@ -131,4 +135,38 @@ mod tests {
         transmitter.on_dma_complete(TxCompletion::TransferFault);
         assert!(!transmitter.transmit.is_active());
     }
+
+    #[test]
+    fn reports_critical_queue_saturation_to_the_policy_owner() {
+        let mut encoder = MavlinkEncoder::new(1, 1);
+        let mut transmitter = MavlinkTransmitter::<Memory, Engine, 1>::new(Memory, Engine);
+
+        assert_eq!(
+            transmitter
+                .send_frame(
+                    FrameClass::Critical,
+                    encoder.heartbeat(MavState::MAV_STATE_STANDBY, false),
+                )
+                .unwrap(),
+            EnqueueResult::Enqueued
+        );
+        assert_eq!(
+            transmitter
+                .send_frame(
+                    FrameClass::Critical,
+                    encoder.heartbeat(MavState::MAV_STATE_STANDBY, false),
+                )
+                .unwrap(),
+            EnqueueResult::Enqueued
+        );
+        assert_eq!(
+            transmitter
+                .send_frame(
+                    FrameClass::Critical,
+                    encoder.heartbeat(MavState::MAV_STATE_STANDBY, false),
+                )
+                .unwrap(),
+            EnqueueResult::CriticalQueueFull
+        );
+    }
 }