libs/kaptan-runtime: add single-owner telemetry service

libs/kaptan-runtime/src/telemetry.rs

@@ -0,0 +1,149 @@
+//! Single-owner MAVLink transmit service.
+
+use kaptan_dma::{DmaTxBufferFactory, DmaTxEngine, TxCompletion};
+use kaptan_mavlink::{MavlinkFrame, MavlinkSender, message::MavState};
+use kaptan_sensor::ImuSnapshot;
+use kaptan_tx::{FrameClass, PendingFrame, TransmitQueue};
+
+/// Terminal fault from the telemetry worker.
+#[derive(Debug)]
+pub enum TelemetryFault<MemoryError, EngineError> {
+    /// DMA-visible frame preparation failed.
+    Memory(MemoryError),
+    /// The UART DMA route rejected or faulted a transfer.
+    Engine(EngineError),
+    /// The DMA route cannot continue without explicit board recovery.
+    RouteHalted,
+}
+
+/// Owns MAVLink packet sequencing and UART DMA frame lifecycle.
+///
+/// Exactly one worker invokes this service. It is not shared with sensor or
+/// hardware IRQ execution.
+pub struct TelemetryService<Memory, Engine, const N: usize> {
+    mavlink: MavlinkSender,
+    transmit: TransmitQueue<MavlinkFrame, N>,
+    memory: Memory,
+    engine: Engine,
+}
+
+impl<Memory, Engine, const N: usize> TelemetryService<Memory, Engine, N> {
+    /// Creates an empty telemetry service.
+    pub const fn new(system_id: u8, component_id: u8, memory: Memory, engine: Engine) -> Self {
+        Self {
+            mavlink: MavlinkSender::new(system_id, component_id),
+            transmit: TransmitQueue::new(),
+            memory,
+            engine,
+        }
+    }
+}
+
+impl<Memory, Engine, const N: usize> TelemetryService<Memory, Engine, N>
+where
+    Memory: DmaTxBufferFactory,
+    Engine: DmaTxEngine,
+{
+    /// Queues the newest accepted IMU observation.
+    pub fn publish_sample(
+        &mut self,
+        sample: &ImuSnapshot,
+    ) -> Result<(), TelemetryFault<Memory::Error, Engine::StartError>> {
+        let frame = self.mavlink.raw_imu_snapshot(sample);
+        self.enqueue_and_start(FrameClass::BestEffort, frame)
+    }
+
+    /// Queues one liveness heartbeat.
+    pub fn publish_heartbeat(
+        &mut self,
+    ) -> Result<(), TelemetryFault<Memory::Error, Engine::StartError>> {
+        let frame = self.mavlink.heartbeat(MavState::MAV_STATE_STANDBY);
+        self.enqueue_and_start(FrameClass::Critical, frame)
+    }
+
+    /// Releases a completed DMA frame and starts at most one next frame.
+    pub fn on_dma_complete(
+        &mut self,
+        completion: TxCompletion,
+    ) -> Result<(), TelemetryFault<Memory::Error, Engine::StartError>> {
+        match completion {
+            TxCompletion::NotPending => Ok(()),
+            TxCompletion::Complete => {
+                let _ = self.transmit.complete();
+                self.start_next()
+            }
+            TxCompletion::TransferFault | TxCompletion::Halted => {
+                let _ = self.transmit.fail();
+                Err(TelemetryFault::RouteHalted)
+            }
+        }
+    }
+
+    fn enqueue_and_start(
+        &mut self,
+        class: FrameClass,
+        frame: MavlinkFrame,
+    ) -> Result<(), TelemetryFault<Memory::Error, Engine::StartError>> {
+        let _ = self.transmit.enqueue(PendingFrame { class, frame });
+        self.start_next()
+    }
+
+    fn start_next(&mut self) -> Result<(), TelemetryFault<Memory::Error, Engine::StartError>> {
+        let (transmit, memory, engine) = (&mut self.transmit, &mut self.memory, &mut self.engine);
+        transmit
+            .start_with(|frame| {
+                let buffer = memory
+                    .prepare(frame.raw_bytes())
+                    .map_err(TelemetryFault::Memory)?;
+                engine.start(buffer).map_err(TelemetryFault::Engine)
+            })
+            .map(|_| ())
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::{TelemetryFault, TelemetryService};
+    use kaptan_dma::{DmaReadableBuffer, DmaTxBufferFactory, DmaTxEngine, TxCompletion};
+    use kaptan_sensor::ImuSnapshot;
+    use kaptan_time::Timestamp;
+
+    struct Memory;
+
+    impl DmaTxBufferFactory for Memory {
+        type Error = core::convert::Infallible;
+
+        fn prepare<'a>(&mut self, bytes: &'a [u8]) -> Result<DmaReadableBuffer<'a>, Self::Error> {
+            // SAFETY: this host-only test keeps the queue's active frame alive.
+            Ok(unsafe { DmaReadableBuffer::new_unchecked(bytes) })
+        }
+    }
+
+    struct Engine;
+
+    impl DmaTxEngine for Engine {
+        type StartError = core::convert::Infallible;
+
+        fn start(&mut self, _: DmaReadableBuffer<'_>) -> Result<(), Self::StartError> {
+            Ok(())
+        }
+    }
+
+    fn sample(timestamp: u64) -> ImuSnapshot {
+        ImuSnapshot::new(Timestamp::from_microseconds(timestamp), [0; 3], [0; 3], 0)
+    }
+
+    #[test]
+    fn transfer_fault_halts_without_starting_the_next_frame() {
+        let mut service = TelemetryService::<Memory, Engine, 2>::new(1, 1, Memory, Engine);
+        service.publish_sample(&sample(1)).unwrap();
+        service.publish_sample(&sample(2)).unwrap();
+        assert!(service.transmit.is_active());
+
+        assert!(matches!(
+            service.on_dma_complete(TxCompletion::TransferFault),
+            Err(TelemetryFault::RouteHalted)
+        ));
+        assert!(!service.transmit.is_active());
+    }
+}