mavwire 0.1.0

no_std, no_alloc parser and encoder for MAVLink v1/v2 telemetry messages
Documentation
//! Message-id-dispatched decoding/encoding of a frame's payload into a
//! typed [`MavMessage`].

mod attitude;
mod command_ack;
mod command_long;
mod global_position_int;
mod gps_raw_int;
mod heartbeat;
mod param_value;
mod payload;
mod rc_channels;
mod statustext;
mod sys_status;
mod text;
mod vfr_hud;

pub use attitude::Attitude;
pub use command_ack::CommandAck;
pub use command_long::CommandLong;
pub use global_position_int::GlobalPositionInt;
pub use gps_raw_int::GpsRawInt;
pub use heartbeat::Heartbeat;
pub use param_value::ParamValue;
pub use rc_channels::RcChannels;
pub use statustext::StatusText;
pub use sys_status::SysStatus;
pub use text::FixedText;
pub use vfr_hud::VfrHud;

use crate::error::MessageError;
use crate::frame::Frame;

/// A decoded `MAVLink` message, one variant per supported message id.
///
/// `#[non_exhaustive]`: more message ids will be added.
///
/// Does not derive `Eq` (only `PartialEq`): several variants carry `f32`
/// fields, and `f32` has no total order (`NaN`).
#[derive(Debug, Clone, Copy, PartialEq)]
#[non_exhaustive]
pub enum MavMessage {
    /// HEARTBEAT (id 0).
    Heartbeat(Heartbeat),
    /// `SYS_STATUS` (id 1).
    SysStatus(SysStatus),
    /// `GPS_RAW_INT` (id 24).
    GpsRawInt(GpsRawInt),
    /// ATTITUDE (id 30).
    Attitude(Attitude),
    /// `GLOBAL_POSITION_INT` (id 33).
    GlobalPositionInt(GlobalPositionInt),
    /// `RC_CHANNELS` (id 65).
    RcChannels(RcChannels),
    /// `VFR_HUD` (id 74).
    VfrHud(VfrHud),
    /// `PARAM_VALUE` (id 22).
    ParamValue(ParamValue),
    /// `COMMAND_LONG` (id 76).
    CommandLong(CommandLong),
    /// `COMMAND_ACK` (id 77).
    CommandAck(CommandAck),
    /// `STATUSTEXT` (id 253).
    StatusText(StatusText),
}

impl MavMessage {
    /// The `crc_extra` byte for a given message id, or `None` if this crate
    /// doesn't decode that message.
    #[must_use]
    pub const fn crc_extra(message_id: u32) -> Option<u8> {
        match message_id {
            heartbeat::ID => Some(heartbeat::CRC_EXTRA),
            sys_status::ID => Some(sys_status::CRC_EXTRA),
            gps_raw_int::ID => Some(gps_raw_int::CRC_EXTRA),
            attitude::ID => Some(attitude::CRC_EXTRA),
            global_position_int::ID => Some(global_position_int::CRC_EXTRA),
            rc_channels::ID => Some(rc_channels::CRC_EXTRA),
            vfr_hud::ID => Some(vfr_hud::CRC_EXTRA),
            param_value::ID => Some(param_value::CRC_EXTRA),
            command_long::ID => Some(command_long::CRC_EXTRA),
            command_ack::ID => Some(command_ack::CRC_EXTRA),
            statustext::ID => Some(statustext::CRC_EXTRA),
            _ => None,
        }
    }

    /// Decodes a parsed [`Frame`]'s payload, first verifying its checksum
    /// using this message's `crc_extra` byte.
    ///
    /// # Errors
    /// Returns [`MessageError::UnknownMessageId`] if the frame's message id
    /// isn't one this crate decodes, or [`MessageError::ChecksumMismatch`]
    /// if the checksum doesn't match.
    pub fn decode(frame: &Frame<'_>) -> Result<Self, MessageError> {
        let message_id = frame.message_id();
        let crc_extra =
            Self::crc_extra(message_id).ok_or(MessageError::UnknownMessageId(message_id))?;
        if !frame.verify_checksum(crc_extra) {
            return Err(MessageError::ChecksumMismatch);
        }
        let payload = frame.payload();
        match message_id {
            heartbeat::ID => Ok(Self::Heartbeat(Heartbeat::decode(payload))),
            sys_status::ID => Ok(Self::SysStatus(SysStatus::decode(payload))),
            gps_raw_int::ID => Ok(Self::GpsRawInt(GpsRawInt::decode(payload))),
            attitude::ID => Ok(Self::Attitude(Attitude::decode(payload))),
            global_position_int::ID => {
                Ok(Self::GlobalPositionInt(GlobalPositionInt::decode(payload)))
            }
            rc_channels::ID => Ok(Self::RcChannels(RcChannels::decode(payload))),
            vfr_hud::ID => Ok(Self::VfrHud(VfrHud::decode(payload))),
            param_value::ID => Ok(Self::ParamValue(ParamValue::decode(payload))),
            command_long::ID => Ok(Self::CommandLong(CommandLong::decode(payload))),
            command_ack::ID => Ok(Self::CommandAck(CommandAck::decode(payload))),
            statustext::ID => Ok(Self::StatusText(StatusText::decode(payload))),
            other => Err(MessageError::UnknownMessageId(other)),
        }
    }

    /// Encodes this message into a v2 frame in `buf`, returning the number
    /// of bytes written. See [`crate::frame::encode_v2`] for the buffer
    /// sizing/error contract.
    ///
    /// # Errors
    /// Returns a [`crate::error::FrameError`] under the same conditions as
    /// [`crate::frame::encode_v2`].
    pub fn encode_v2(
        &self,
        buf: &mut [u8],
        system_id: u8,
        component_id: u8,
        sequence: u8,
    ) -> Result<usize, crate::error::FrameError> {
        // One line per variant instead of a full crate::frame::encode_v2(..)
        // call each: every arm passes the same five leading arguments, only
        // the module (for ID/CRC_EXTRA) and the message value differ.
        macro_rules! encode {
            ($module:ident, $m:expr) => {
                crate::frame::encode_v2(
                    buf,
                    system_id,
                    component_id,
                    sequence,
                    $module::ID,
                    &$m.encode(),
                    $module::CRC_EXTRA,
                )
            };
        }
        match self {
            Self::Heartbeat(m) => encode!(heartbeat, m),
            Self::SysStatus(m) => encode!(sys_status, m),
            Self::GpsRawInt(m) => encode!(gps_raw_int, m),
            Self::Attitude(m) => encode!(attitude, m),
            Self::GlobalPositionInt(m) => encode!(global_position_int, m),
            Self::RcChannels(m) => encode!(rc_channels, m),
            Self::VfrHud(m) => encode!(vfr_hud, m),
            Self::ParamValue(m) => encode!(param_value, m),
            Self::CommandLong(m) => encode!(command_long, m),
            Self::CommandAck(m) => encode!(command_ack, m),
            Self::StatusText(m) => encode!(statustext, m),
        }
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::frame::MAX_V2_FRAME_LEN;

    #[test]
    fn decodes_a_real_heartbeat_frame() {
        let hex = "fd090000000101000000000000000203510403e71e";
        let mut buf = [0u8; 32];
        for (i, chunk) in hex.as_bytes().chunks(2).enumerate() {
            buf[i] = u8::from_str_radix(core::str::from_utf8(chunk).unwrap(), 16).unwrap();
        }
        let frame = Frame::parse(&buf[..hex.len() / 2]).unwrap();
        let msg = MavMessage::decode(&frame).unwrap();
        let MavMessage::Heartbeat(hb) = msg else {
            panic!("expected Heartbeat");
        };
        assert_eq!(hb.mav_type, 2);
        assert_eq!(hb.mavlink_version, 3);
    }

    #[test]
    fn rejects_an_unknown_message_id() {
        let mut buf = [0u8; 32];
        // v2 header for message id 9999, empty-ish payload, bogus checksum.
        buf[0] = 0xFD;
        buf[1] = 1;
        buf[7] = 0x0F;
        buf[8] = 0x27; // 9999 = 0x270F, low16
        let frame = Frame::parse(&buf[..13]).unwrap();
        assert_eq!(
            MavMessage::decode(&frame),
            Err(MessageError::UnknownMessageId(9999))
        );
    }

    #[test]
    fn rejects_a_bad_checksum() {
        let hex = "fd090000000101000000000000000203510403e71e";
        let mut buf = [0u8; 32];
        for (i, chunk) in hex.as_bytes().chunks(2).enumerate() {
            buf[i] = u8::from_str_radix(core::str::from_utf8(chunk).unwrap(), 16).unwrap();
        }
        buf[19] ^= 0xFF; // flip a checksum byte
        let frame = Frame::parse(&buf[..hex.len() / 2]).unwrap();
        assert_eq!(
            MavMessage::decode(&frame),
            Err(MessageError::ChecksumMismatch)
        );
    }

    #[test]
    fn encode_then_decode_round_trips_through_a_real_frame() {
        let original = MavMessage::Heartbeat(Heartbeat {
            mav_type: 2,
            autopilot: 3,
            base_mode: 81,
            custom_mode: 0,
            system_status: 4,
            mavlink_version: 3,
        });

        let mut buf = [0u8; MAX_V2_FRAME_LEN];
        let n = original.encode_v2(&mut buf, 1, 1, 0).unwrap();

        let frame = Frame::parse(&buf[..n]).unwrap();
        let decoded = MavMessage::decode(&frame).unwrap();
        assert_eq!(decoded, original);
    }
}

// Fuzzes MavMessage::decode over arbitrary frame bytes, not just the
// hand-picked fixtures above: every byte pattern is a legal input (decode
// only rejects on message id/checksum), so this exercises the same
// zero-extension logic in payload.rs against arbitrary truncation lengths.
#[cfg(test)]
mod proptests {
    extern crate std;

    use std::vec;

    use proptest::prelude::*;

    use super::MavMessage;
    use crate::frame::Frame;

    proptest! {
        #[test]
        fn decode_never_panics(bytes in prop::collection::vec(any::<u8>(), 0..40)) {
            if let Ok(frame) = Frame::parse(&bytes) {
                let _ = MavMessage::decode(&frame);
            }
        }

        // Every known message id decodes successfully for *any* payload
        // bytes: none of the 10 messages this crate supports have a field
        // whose raw wire value can be "invalid" (every u8/u16/i32/f32 bit
        // pattern is a legal, if perhaps meaningless, field value), so a
        // correct crc_extra should always be enough to decode. This
        // exercises every message's decode path, not just HEARTBEAT's,
        // against arbitrary field values and arbitrary truncation points.
        #[test]
        fn decode_succeeds_for_every_known_message_id_with_arbitrary_payload(
            (message_id, crc_extra) in prop_oneof![
                Just((super::heartbeat::ID, super::heartbeat::CRC_EXTRA)),
                Just((super::sys_status::ID, super::sys_status::CRC_EXTRA)),
                Just((super::gps_raw_int::ID, super::gps_raw_int::CRC_EXTRA)),
                Just((super::attitude::ID, super::attitude::CRC_EXTRA)),
                Just((super::global_position_int::ID, super::global_position_int::CRC_EXTRA)),
                Just((super::rc_channels::ID, super::rc_channels::CRC_EXTRA)),
                Just((super::vfr_hud::ID, super::vfr_hud::CRC_EXTRA)),
                Just((super::param_value::ID, super::param_value::CRC_EXTRA)),
                Just((super::command_long::ID, super::command_long::CRC_EXTRA)),
                Just((super::command_ack::ID, super::command_ack::CRC_EXTRA)),
                Just((super::statustext::ID, super::statustext::CRC_EXTRA)),
            ],
            payload in prop::collection::vec(any::<u8>(), 0..60),
            system_id: u8,
            component_id: u8,
            sequence: u8,
        ) {
            let mut buf = [0u8; crate::frame::MAX_V2_FRAME_LEN];
            let n = crate::frame::encode_v2(&mut buf, system_id, component_id, sequence, message_id, &payload, crc_extra).unwrap();
            let frame = Frame::parse(&buf[..n]).unwrap();
            prop_assert!(MavMessage::decode(&frame).is_ok());
        }
    }
}