mavwire 0.1.0

no_std, no_alloc parser and encoder for MAVLink v1/v2 telemetry messages
Documentation
//! `GLOBAL_POSITION_INT` (message id 33): fused global position, the only
//! telemetry message defined directly in `MAVLink`'s `standard.xml`.

use super::payload::{i16_at, i32_at, u16_at, u32_at};

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

/// A decoded `GLOBAL_POSITION_INT` message.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct GlobalPositionInt {
    /// Timestamp, milliseconds since system boot.
    pub time_boot_ms: u32,
    /// Latitude in `WGS84`, degrees * 1e7.
    pub lat: i32,
    /// Longitude in `WGS84`, degrees * 1e7.
    pub lon: i32,
    /// Altitude (MSL), millimeters.
    pub alt: i32,
    /// Altitude above ground/home, millimeters.
    pub relative_alt: i32,
    /// Ground X (north) speed, cm/s.
    pub vx: i16,
    /// Ground Y (east) speed, cm/s.
    pub vy: i16,
    /// Ground Z (down) speed, cm/s.
    pub vz: i16,
    /// Vehicle heading, centidegrees (`0`-`35999`, `u16::MAX` if unknown).
    pub hdg: u16,
}

impl GlobalPositionInt {
    pub(crate) fn decode(payload: &[u8]) -> Self {
        Self {
            time_boot_ms: u32_at(payload, 0),
            lat: i32_at(payload, 4),
            lon: i32_at(payload, 8),
            alt: i32_at(payload, 12),
            relative_alt: i32_at(payload, 16),
            vx: i16_at(payload, 20),
            vy: i16_at(payload, 22),
            vz: i16_at(payload, 24),
            hdg: u16_at(payload, 26),
        }
    }

    pub(crate) fn encode(&self) -> [u8; LEN] {
        let mut buf = [0u8; LEN];
        buf[0..4].copy_from_slice(&self.time_boot_ms.to_le_bytes());
        buf[4..8].copy_from_slice(&self.lat.to_le_bytes());
        buf[8..12].copy_from_slice(&self.lon.to_le_bytes());
        buf[12..16].copy_from_slice(&self.alt.to_le_bytes());
        buf[16..20].copy_from_slice(&self.relative_alt.to_le_bytes());
        buf[20..22].copy_from_slice(&self.vx.to_le_bytes());
        buf[22..24].copy_from_slice(&self.vy.to_le_bytes());
        buf[24..26].copy_from_slice(&self.vz.to_le_bytes());
        buf[26..28].copy_from_slice(&self.hdg.to_le_bytes());
        buf
    }
}

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

    #[test]
    fn decodes_a_real_captured_frame() {
        // Real wire payload from pymavlink: time_boot_ms=2000
        // lat=473977418 lon=85455940 alt=150000 relative_alt=50000 vx=100
        // vy=-50 vz=10 hdg=9000.
        let hex = "d00700004a52401c44f41705f049020050c300006400ceff0a002823";
        let mut payload = [0u8; LEN];
        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 p = GlobalPositionInt::decode(&payload);
        assert_eq!(p.time_boot_ms, 2000);
        assert_eq!(p.lat, 473_977_418);
        assert_eq!(p.lon, 85_455_940);
        assert_eq!(p.alt, 150_000);
        assert_eq!(p.relative_alt, 50_000);
        assert_eq!(p.vx, 100);
        assert_eq!(p.vy, -50);
        assert_eq!(p.vz, 10);
        assert_eq!(p.hdg, 9000);
    }

    #[test]
    fn round_trips_through_decode() {
        let p = GlobalPositionInt {
            time_boot_ms: 1,
            lat: i32::MIN,
            lon: i32::MAX,
            alt: -1000,
            relative_alt: 1000,
            vx: -1,
            vy: 1,
            vz: 0,
            hdg: 35999,
        };
        assert_eq!(GlobalPositionInt::decode(&p.encode()), p);
    }
}