support bi-directional mavlink

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

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