libs/kaptan-mavlink: add distance sensor telemetry

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
+        );
+    }
 }