kaptan/libs/kaptan-mavlink/src/transmitter.rs

//! MAVLink frame encoding and UART transmission API.

use kaptan_dma::{DmaTxBufferFactory, DmaTxEngine, TxCompletion};
use kaptan_tx::{PendingFrame, TransmitQueue};

use crate::MavlinkFrame;

/// Delivery importance used for bounded MAVLink transmission admission.
pub use kaptan_tx::{EnqueueResult, FrameClass};

/// Failure while preparing or starting a MAVLink frame transfer.
#[derive(Debug)]
pub enum MavlinkTransmitFault<MemoryError, EngineError> {
    /// DMA-visible frame preparation failed.
    Memory(MemoryError),
    /// The UART DMA route rejected a transfer start.
    Engine(EngineError),
}

/// Owns MAVLink frame admission and UART DMA lifecycle.
///
/// Exactly one worker invokes this type. It accepts local encoded frames and
/// received frames alike; the caller owns the policy that decides which frames
/// are transmitted.
pub struct MavlinkTransmitter<Memory, Engine, const N: usize> {
    transmit: TransmitQueue<MavlinkFrame, N>,
    memory: Memory,
    engine: Engine,
}

impl<Memory, Engine, const N: usize> MavlinkTransmitter<Memory, Engine, N> {
    /// Creates an empty MAVLink transmitter.
    pub const fn new(memory: Memory, engine: Engine) -> Self {
        Self {
            transmit: TransmitQueue::new(),
            memory,
            engine,
        }
    }
}

impl<Memory, Engine, const N: usize> MavlinkTransmitter<Memory, Engine, N>
where
    Memory: DmaTxBufferFactory,
    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<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.
    pub fn on_dma_complete(&mut self, completion: TxCompletion) {
        match completion {
            TxCompletion::NotPending => {}
            TxCompletion::Complete => {
                let _ = self.transmit.complete();
                let _ = self.start_next();
            }
            TxCompletion::TransferFault | TxCompletion::Halted => {
                let _ = self.transmit.fail();
            }
        }
    }

    fn start_next(
        &mut self,
    ) -> Result<(), MavlinkTransmitFault<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(MavlinkTransmitFault::Memory)?;
                engine.start(buffer).map_err(MavlinkTransmitFault::Engine)
            })
            .map(|_| ())
    }
}

#[cfg(test)]
mod tests {
    use super::{EnqueueResult, FrameClass, MavlinkTransmitter};
    use crate::{encoder::MavlinkEncoder, message::MavState};
    use kaptan_dma::{DmaReadableBuffer, DmaTxBufferFactory, DmaTxEngine, TxCompletion};

    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(())
        }
    }

    #[test]
    fn transfer_fault_drops_only_the_active_frame() {
        let mut encoder = MavlinkEncoder::new(1, 1);
        let mut transmitter = MavlinkTransmitter::<Memory, Engine, 2>::new(Memory, Engine);
        transmitter
            .send_frame(
                FrameClass::BestEffort,
                encoder.heartbeat(MavState::MAV_STATE_STANDBY, false),
            )
            .unwrap();
        transmitter
            .send_frame(
                FrameClass::BestEffort,
                encoder.heartbeat(MavState::MAV_STATE_STANDBY, false),
            )
            .unwrap();
        assert!(transmitter.transmit.is_active());

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