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

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