mavwire 0.1.0

no_std, no_alloc parser and encoder for MAVLink v1/v2 telemetry messages
Documentation
//! `VFR_HUD` (message id 74): airspeed/altitude/heading HUD data.

use super::payload::{f32_at, i16_at, u16_at};

/// This message's id.
pub const ID: u32 = 74;
/// This message's `crc_extra` byte (see [`crate::crc`]).
pub const CRC_EXTRA: u8 = 20;
const LEN: usize = 20;

/// A decoded `VFR_HUD` message.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct VfrHud {
    /// Current indicated airspeed, m/s.
    pub airspeed: f32,
    /// Current ground speed, m/s.
    pub groundspeed: f32,
    /// Current altitude (MSL), meters.
    pub alt: f32,
    /// Current climb rate, m/s.
    pub climb: f32,
    /// Current heading, degrees (0-360, negative for left of north).
    pub heading: i16,
    /// Current throttle, percent (0-100).
    pub throttle: u16,
}

impl VfrHud {
    pub(crate) fn decode(payload: &[u8]) -> Self {
        Self {
            airspeed: f32_at(payload, 0),
            groundspeed: f32_at(payload, 4),
            alt: f32_at(payload, 8),
            climb: f32_at(payload, 12),
            heading: i16_at(payload, 16),
            throttle: u16_at(payload, 18),
        }
    }

    pub(crate) fn encode(&self) -> [u8; LEN] {
        let mut buf = [0u8; LEN];
        buf[0..4].copy_from_slice(&self.airspeed.to_le_bytes());
        buf[4..8].copy_from_slice(&self.groundspeed.to_le_bytes());
        buf[8..12].copy_from_slice(&self.alt.to_le_bytes());
        buf[12..16].copy_from_slice(&self.climb.to_le_bytes());
        buf[16..18].copy_from_slice(&self.heading.to_le_bytes());
        buf[18..20].copy_from_slice(&self.throttle.to_le_bytes());
        buf
    }
}

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

    fn close(a: f32, b: f32) -> bool {
        (a - b).abs() < 1e-3
    }

    #[test]
    fn decodes_a_real_captured_frame() {
        // Real wire payload from pymavlink, truncated to 19 of 20 bytes
        // (throttle=50's high byte is zero and gets stripped, see
        // frame.rs): airspeed=12.5 groundspeed=13.0 alt=150.0 climb=0.5
        // heading=270 throttle=50.
        let hex = "0000484100005041000016430000003f0e0132";
        let mut payload = [0u8; 19];
        for (i, chunk) in hex.as_bytes().chunks(2).enumerate() {
            payload[i] = u8::from_str_radix(core::str::from_utf8(chunk).unwrap(), 16).unwrap();
        }
        let v = VfrHud::decode(&payload);
        assert!(close(v.airspeed, 12.5));
        assert!(close(v.groundspeed, 13.0));
        assert!(close(v.alt, 150.0));
        assert!(close(v.climb, 0.5));
        assert_eq!(v.heading, 270);
        assert_eq!(v.throttle, 50);
    }

    #[test]
    fn round_trips_through_decode() {
        let v = VfrHud {
            airspeed: 1.0,
            groundspeed: 2.0,
            alt: -3.0,
            climb: 0.0,
            heading: -180,
            throttle: 100,
        };
        let decoded = VfrHud::decode(&v.encode());
        assert!(close(decoded.airspeed, v.airspeed));
        assert!(close(decoded.groundspeed, v.groundspeed));
        assert!(close(decoded.alt, v.alt));
        assert!(close(decoded.climb, v.climb));
        assert_eq!(decoded.heading, v.heading);
        assert_eq!(decoded.throttle, v.throttle);
    }
}