support bi-directional mavlink

Signed-off-by: Onur Özkan <work@onurozkan.dev>

libs/kaptan-mavlink/src/encoder.rs

@@ -0,0 +1,117 @@
+//! MAVLink frame encoding API.
+
+use kaptan_sensor::ImuSnapshot;
+use mavlink::MessageData;
+
+use crate::{MavlinkFrame, message};
+
+/// Serializes FC-domain data with MAVLink sender identity and sequence state.
+pub struct MavlinkEncoder {
+    system_id: u8,
+    component_id: u8,
+    next_sequence: u8,
+}
+
+impl MavlinkEncoder {
+    /// Creates an encoder beginning with MAVLink packet sequence zero.
+    pub const fn new(system_id: u8, component_id: u8) -> Self {
+        Self {
+            system_id,
+            component_id,
+            next_sequence: 0,
+        }
+    }
+
+    fn serialize<M: MessageData>(&mut self, message: &M) -> MavlinkFrame {
+        let mut raw = MavlinkFrame::new();
+        raw.serialize_message_data(
+            crate::message::MavHeader {
+                sequence: self.next_sequence,
+                system_id: self.system_id,
+                component_id: self.component_id,
+            },
+            message,
+        );
+        self.next_sequence = self.next_sequence.wrapping_add(1);
+
+        raw
+    }
+
+    /// Encodes a Kaptan heartbeat.
+    pub fn heartbeat(&mut self, system_status: message::MavState) -> MavlinkFrame {
+        let heartbeat = message::HEARTBEAT_DATA {
+            custom_mode: 0,
+            mavtype: message::MavType::MAV_TYPE_QUADROTOR,
+            autopilot: message::MavAutopilot::MAV_AUTOPILOT_GENERIC,
+            base_mode: message::MavModeFlag::empty(),
+            system_status,
+            mavlink_version: 3,
+        };
+
+        self.serialize(&heartbeat)
+    }
+
+    /// Encodes a standard `RAW_IMU` frame.
+    pub fn raw_imu_snapshot(&mut self, snapshot: &ImuSnapshot) -> MavlinkFrame {
+        let raw_imu = message::RAW_IMU_DATA {
+            time_usec: snapshot.timestamp.as_microseconds(),
+            xacc: snapshot.acceleration_milligravity[0],
+            yacc: snapshot.acceleration_milligravity[1],
+            zacc: snapshot.acceleration_milligravity[2],
+            xgyro: snapshot.angular_rate_milliradians_per_second[0],
+            ygyro: snapshot.angular_rate_milliradians_per_second[1],
+            zgyro: snapshot.angular_rate_milliradians_per_second[2],
+            xmag: 0,
+            ymag: 0,
+            zmag: 0,
+        };
+
+        self.serialize(&raw_imu)
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::MavlinkEncoder;
+    use crate::message::MavState;
+    use kaptan_sensor::ImuSnapshot;
+    use kaptan_time::Timestamp;
+
+    #[test]
+    fn heartbeat_has_the_configured_sender_and_sequence() {
+        let mut encoder = MavlinkEncoder::new(42, 7);
+        let frame = encoder.heartbeat(MavState::MAV_STATE_STANDBY);
+        let bytes = frame.raw_bytes();
+
+        assert_eq!(bytes[0], 0xFD);
+        assert_eq!(bytes[2], 0);
+        assert_eq!(bytes[3], 0);
+        assert_eq!(bytes[4], 0);
+        assert_eq!(bytes[5], 42);
+        assert_eq!(bytes[6], 7);
+        assert_eq!(&bytes[7..10], &[0, 0, 0]);
+    }
+
+    #[test]
+    fn heartbeat_preserves_the_requested_health_state() {
+        let mut encoder = MavlinkEncoder::new(1, 1);
+        let frame = encoder.heartbeat(MavState::MAV_STATE_CRITICAL);
+
+        assert_eq!(frame.raw_bytes()[17], MavState::MAV_STATE_CRITICAL as u8);
+    }
+
+    #[test]
+    fn snapshot_adapter_uses_flight_data_timestamp_and_units() {
+        let mut encoder = MavlinkEncoder::new(1, 1);
+        let snapshot = ImuSnapshot::new(
+            Timestamp::from_microseconds(123),
+            [1, 2, 3],
+            [4, 5, 6],
+            2_500,
+        );
+
+        let frame = encoder.raw_imu_snapshot(&snapshot);
+
+        assert_eq!(&frame.raw_bytes()[7..10], &[27, 0, 0]);
+    }
+}

libs/kaptan-mavlink/src/lib.rs

@@ -2,78 +2,16 @@
 
 #![no_std]
 
-use kaptan_sensor::ImuSnapshot;
-use mavlink::MessageData;
+/// MAVLink frame encoding API for boards.
+pub mod encoder;
+/// Serial-byte MAVLink receive API for boards.
+pub mod receiver;
+/// UART DMA MAVLink frame transmission API for boards.
+pub mod transmitter;
 
-/// An owned MAVLink 2 frame supplied by rust-mavlink.
+/// An owned MAVLink 2 frame suitable for UART transmission.
 pub type MavlinkFrame = message::MAVLinkV2MessageRaw;
 
-/// Owns MAVLink sender identity and packet sequence state.
-#[derive(Clone, Copy, Debug, Eq, PartialEq)]
-pub struct MavlinkSender {
-    system_id: u8,
-    component_id: u8,
-    next_sequence: u8,
-}
-
-impl MavlinkSender {
-    /// Creates a sender beginning with MAVLink packet sequence zero.
-    pub const fn new(system_id: u8, component_id: u8) -> Self {
-        Self {
-            system_id,
-            component_id,
-            next_sequence: 0,
-        }
-    }
-
-    /// Serializes a Kaptan heartbeat with the supplied system-health state.
-    pub fn heartbeat(&mut self, system_status: message::MavState) -> MavlinkFrame {
-        let heartbeat = message::HEARTBEAT_DATA {
-            custom_mode: 0,
-            mavtype: message::MavType::MAV_TYPE_QUADROTOR,
-            autopilot: message::MavAutopilot::MAV_AUTOPILOT_GENERIC,
-            base_mode: message::MavModeFlag::empty(),
-            system_status,
-            mavlink_version: 3,
-        };
-
-        self.serialize(&heartbeat)
-    }
-
-    /// Serializes a standard `RAW_IMU` frame from a flight-data snapshot.
-    pub fn raw_imu_snapshot(&mut self, snapshot: &ImuSnapshot) -> MavlinkFrame {
-        let raw_imu = message::RAW_IMU_DATA {
-            time_usec: snapshot.timestamp.as_microseconds(),
-            xacc: snapshot.acceleration_milligravity[0],
-            yacc: snapshot.acceleration_milligravity[1],
-            zacc: snapshot.acceleration_milligravity[2],
-            xgyro: snapshot.angular_rate_milliradians_per_second[0],
-            ygyro: snapshot.angular_rate_milliradians_per_second[1],
-            zgyro: snapshot.angular_rate_milliradians_per_second[2],
-            xmag: 0,
-            ymag: 0,
-            zmag: 0,
-        };
-
-        self.serialize(&raw_imu)
-    }
-
-    fn serialize<M: MessageData>(&mut self, message: &M) -> MavlinkFrame {
-        let mut raw = message::MAVLinkV2MessageRaw::new();
-        raw.serialize_message_data(
-            message::MavHeader {
-                sequence: self.next_sequence,
-                system_id: self.system_id,
-                component_id: self.component_id,
-            },
-            message,
-        );
-        self.next_sequence = self.next_sequence.wrapping_add(1);
-
-        raw
-    }
-}
-
 /// Supported MAVLink message types for Kaptan.
 pub mod message {
     pub use mavlink::MAVLinkV2MessageRaw;
@@ -83,53 +21,3 @@ pub mod message {
     #[cfg(kaptan_has_messages)]
     include!(concat!(env!("OUT_DIR"), "/mod.rs"));
 }
-
-#[cfg(test)]
-mod tests {
-    use super::MavlinkSender;
-    use kaptan_sensor::ImuSnapshot;
-    use kaptan_time::Timestamp;
-
-    #[test]
-    fn heartbeat_has_the_configured_sender_and_sequence() {
-        let mut sender = MavlinkSender::new(42, 7);
-        let frame = sender.heartbeat(crate::message::MavState::MAV_STATE_STANDBY);
-        let bytes = frame.raw_bytes();
-
-        assert_eq!(bytes[0], 0xFD);
-        assert_eq!(bytes[2], 0);
-        assert_eq!(bytes[3], 0);
-        assert_eq!(bytes[4], 0);
-        assert_eq!(bytes[5], 42);
-        assert_eq!(bytes[6], 7);
-        assert_eq!(&bytes[7..10], &[0, 0, 0]);
-    }
-
-    #[test]
-    fn heartbeat_preserves_the_requested_health_state() {
-        let mut sender = MavlinkSender::new(1, 1);
-        let frame = sender.heartbeat(crate::message::MavState::MAV_STATE_CRITICAL);
-
-        // MAVLink 2 header is 10 octets; HEARTBEAT.system_status is payload
-        // octet seven according to the common dialect wire layout.
-        assert_eq!(
-            frame.raw_bytes()[17],
-            crate::message::MavState::MAV_STATE_CRITICAL as u8
-        );
-    }
-
-    #[test]
-    fn snapshot_adapter_uses_flight_data_timestamp_and_units() {
-        let mut sender = MavlinkSender::new(1, 1);
-        let snapshot = ImuSnapshot::new(
-            Timestamp::from_microseconds(123),
-            [1, 2, 3],
-            [4, 5, 6],
-            2_500,
-        );
-
-        let frame = sender.raw_imu_snapshot(&snapshot);
-
-        assert_eq!(&frame.raw_bytes()[7..10], &[27, 0, 0]);
-    }
-}

libs/kaptan-mavlink/src/receiver.rs

@@ -0,0 +1,181 @@
+//! MAVLink serial-byte receive API.
+
+use crate::{MavlinkFrame, message};
+
+/// Bounded MAVLink 2 receiver for a board's serial-byte interrupt.
+///
+/// Boards pass UART bytes to [`Self::receive_byte`] and take complete,
+/// CRC-valid frames from [`Self::take_received`]. The API never decodes or
+/// dispatches received message payloads.
+pub struct MavlinkReceiver<const N: usize> {
+    frame: MavlinkFrame,
+    received: usize,
+    expected: usize,
+    frames: [Option<MavlinkFrame>; N],
+    front: usize,
+    length: usize,
+}
+
+impl<const N: usize> Default for MavlinkReceiver<N> {
+    fn default() -> Self {
+        Self::new()
+    }
+}
+
+impl<const N: usize> MavlinkReceiver<N> {
+    /// Creates a receiver with space for `N` complete MAVLink frames.
+    pub const fn new() -> Self {
+        Self {
+            frame: MavlinkFrame::new(),
+            received: 0,
+            expected: 0,
+            frames: [None; N],
+            front: 0,
+            length: 0,
+        }
+    }
+
+    /// Receives one UART byte and returns whether it queued a complete frame.
+    ///
+    /// Bytes outside MAVLink 2 frames and frames with invalid CRCs are
+    /// discarded. If the completed-frame queue is full, the new frame is
+    /// dropped without displacing an earlier received frame.
+    pub fn receive_byte(&mut self, byte: u8) -> bool {
+        if self.received == 0 {
+            if byte == 0xfd {
+                self.frame.as_mut_slice()[0] = byte;
+                self.received = 1;
+            }
+            return false;
+        }
+
+        self.frame.as_mut_slice()[self.received] = byte;
+        self.received += 1;
+
+        if self.received == 10 {
+            let header = self.frame.as_slice();
+            let incompatibility_flags = header[2];
+            if incompatibility_flags & !1 != 0 {
+                self.reset();
+                return false;
+            }
+            self.expected = 1 + 9 + usize::from(header[1]) + 2;
+            if incompatibility_flags & 1 != 0 {
+                self.expected += 13;
+            }
+        }
+
+        if self.expected != 0 && self.received == self.expected {
+            let frame = core::mem::replace(&mut self.frame, MavlinkFrame::new());
+            self.received = 0;
+            self.expected = 0;
+            if frame.has_valid_crc::<message::MavMessage>() {
+                return self.push(frame);
+            }
+        }
+
+        false
+    }
+
+    /// Discards the in-progress frame after a UART transport fault.
+    pub fn reset(&mut self) {
+        self.frame = MavlinkFrame::new();
+        self.received = 0;
+        self.expected = 0;
+    }
+
+    /// Takes the oldest complete received frame, if one is available.
+    pub fn take_received(&mut self) -> Option<MavlinkFrame> {
+        if self.length == 0 {
+            return None;
+        }
+        let frame = self.frames[self.front].take();
+        self.front = (self.front + 1) % N;
+        self.length -= 1;
+        frame
+    }
+
+    fn push(&mut self, frame: MavlinkFrame) -> bool {
+        if N == 0 || self.length == N {
+            return false;
+        }
+        let index = (self.front + self.length) % N;
+        self.frames[index] = Some(frame);
+        self.length += 1;
+        true
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::MavlinkReceiver;
+    use crate::{MavlinkFrame, message};
+
+    fn heartbeat(sequence: u8) -> MavlinkFrame {
+        let mut frame = MavlinkFrame::new();
+        frame.serialize_message_data(
+            message::MavHeader {
+                sequence,
+                system_id: 42,
+                component_id: 7,
+            },
+            &message::HEARTBEAT_DATA {
+                custom_mode: 0,
+                mavtype: message::MavType::MAV_TYPE_QUADROTOR,
+                autopilot: message::MavAutopilot::MAV_AUTOPILOT_GENERIC,
+                base_mode: message::MavModeFlag::empty(),
+                system_status: message::MavState::MAV_STATE_STANDBY,
+                mavlink_version: 3,
+            },
+        );
+        frame
+    }
+
+    #[test]
+    fn returns_complete_crc_valid_frames_unchanged() {
+        let expected = heartbeat(0);
+        let mut receiver = MavlinkReceiver::<1>::new();
+
+        for byte in expected.raw_bytes() {
+            receiver.receive_byte(*byte);
+        }
+
+        assert_eq!(
+            receiver.take_received().unwrap().raw_bytes(),
+            expected.raw_bytes()
+        );
+    }
+
+    #[test]
+    fn discards_frames_with_an_invalid_crc() {
+        let frame = heartbeat(0);
+        let mut bytes = frame.into_inner();
+        bytes[10] ^= 1;
+        let corrupt = MavlinkFrame::from_bytes_unparsed(bytes);
+        let mut receiver = MavlinkReceiver::<1>::new();
+
+        for byte in corrupt.raw_bytes() {
+            assert!(!receiver.receive_byte(*byte));
+        }
+    }
+
+    #[test]
+    fn preserves_arrival_order() {
+        let first = heartbeat(0);
+        let second = heartbeat(1);
+        let mut receiver = MavlinkReceiver::<2>::new();
+
+        for byte in first.raw_bytes().iter().chain(second.raw_bytes()) {
+            receiver.receive_byte(*byte);
+        }
+
+        assert_eq!(
+            receiver.take_received().unwrap().raw_bytes(),
+            first.raw_bytes()
+        );
+        assert_eq!(
+            receiver.take_received().unwrap().raw_bytes(),
+            second.raw_bytes()
+        );
+    }
+}

libs/kaptan-mavlink/src/transmitter.rs

@@ -0,0 +1,134 @@
+//! 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::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.
+    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()
+    }
+
+    /// 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::{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),
+            )
+            .unwrap();
+        transmitter
+            .send_frame(
+                FrameClass::BestEffort,
+                encoder.heartbeat(MavState::MAV_STATE_STANDBY),
+            )
+            .unwrap();
+        assert!(transmitter.transmit.is_active());
+
+        transmitter.on_dma_complete(TxCompletion::TransferFault);
+        assert!(!transmitter.transmit.is_active());
+    }
+}