mavwire 0.1.0

no_std, no_alloc parser and encoder for MAVLink v1/v2 telemetry messages
Documentation
//! `GPS_RAW_INT` (message id 24): raw GPS fix, before sensor fusion.

use super::payload::{i32_at, u8_at, u16_at, u32_at, u64_at};

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

/// A decoded `GPS_RAW_INT` message.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct GpsRawInt {
    /// Timestamp, microseconds since UNIX epoch (`0` if unknown).
    pub time_usec: u64,
    /// GPS fix type.
    pub fix_type: u8,
    /// Latitude in `WGS84`, degrees * 1e7.
    pub lat: i32,
    /// Longitude in `WGS84`, degrees * 1e7.
    pub lon: i32,
    /// Altitude (MSL), millimeters.
    pub alt: i32,
    /// GPS horizontal dilution of precision (`u16::MAX` if unknown).
    pub eph: u16,
    /// GPS vertical dilution of precision (`u16::MAX` if unknown).
    pub epv: u16,
    /// GPS ground speed, cm/s (`u16::MAX` if unknown).
    pub vel: u16,
    /// Course over ground, centidegrees (`u16::MAX` if unknown).
    pub cog: u16,
    /// Number of visible satellites (`u8::MAX` if unknown).
    pub satellites_visible: u8,
    /// Altitude above WGS84 ellipsoid, millimeters.
    pub alt_ellipsoid: i32,
    /// Position uncertainty, millimeters.
    pub h_acc: u32,
    /// Altitude uncertainty, millimeters.
    pub v_acc: u32,
    /// Speed uncertainty, mm/s.
    pub vel_acc: u32,
    /// Heading/track uncertainty, degrees * 1e5.
    pub hdg_acc: u32,
    /// Yaw in earth frame, centidegrees (`0` if unknown, angle wraps to
    /// 36000 rather than reaching 0).
    pub yaw: u16,
}

impl GpsRawInt {
    pub(crate) fn decode(payload: &[u8]) -> Self {
        Self {
            time_usec: u64_at(payload, 0),
            lat: i32_at(payload, 8),
            lon: i32_at(payload, 12),
            alt: i32_at(payload, 16),
            eph: u16_at(payload, 20),
            epv: u16_at(payload, 22),
            vel: u16_at(payload, 24),
            cog: u16_at(payload, 26),
            fix_type: u8_at(payload, 28),
            satellites_visible: u8_at(payload, 29),
            alt_ellipsoid: i32_at(payload, 30),
            h_acc: u32_at(payload, 34),
            v_acc: u32_at(payload, 38),
            vel_acc: u32_at(payload, 42),
            hdg_acc: u32_at(payload, 46),
            yaw: u16_at(payload, 50),
        }
    }

    pub(crate) fn encode(&self) -> [u8; LEN] {
        let mut buf = [0u8; LEN];
        buf[0..8].copy_from_slice(&self.time_usec.to_le_bytes());
        buf[8..12].copy_from_slice(&self.lat.to_le_bytes());
        buf[12..16].copy_from_slice(&self.lon.to_le_bytes());
        buf[16..20].copy_from_slice(&self.alt.to_le_bytes());
        buf[20..22].copy_from_slice(&self.eph.to_le_bytes());
        buf[22..24].copy_from_slice(&self.epv.to_le_bytes());
        buf[24..26].copy_from_slice(&self.vel.to_le_bytes());
        buf[26..28].copy_from_slice(&self.cog.to_le_bytes());
        buf[28] = self.fix_type;
        buf[29] = self.satellites_visible;
        buf[30..34].copy_from_slice(&self.alt_ellipsoid.to_le_bytes());
        buf[34..38].copy_from_slice(&self.h_acc.to_le_bytes());
        buf[38..42].copy_from_slice(&self.v_acc.to_le_bytes());
        buf[42..46].copy_from_slice(&self.vel_acc.to_le_bytes());
        buf[46..50].copy_from_slice(&self.hdg_acc.to_le_bytes());
        buf[50..52].copy_from_slice(&self.yaw.to_le_bytes());
        buf
    }
}

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

    #[test]
    fn decodes_a_real_captured_frame() {
        // Real wire payload from pymavlink (truncated to 48 of 52 bytes -
        // trailing zero bytes stripped, see frame.rs): time_usec=123456789
        // fix_type=3 lat=473977418 lon=85455940 alt=150000 eph=100 epv=150
        // vel=500 cog=9000 satellites_visible=12 alt_ellipsoid=150500
        // h_acc=1000 v_acc=1500 vel_acc=200 hdg_acc=300 yaw=0 (zero-extended).
        let hex = "15cd5b07000000004a52401c44f41705f049020064009600f4012823030ce44b0200e8030000dc050000c80000002c01";
        let mut payload = [0u8; 48];
        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 g = GpsRawInt::decode(&payload);
        assert_eq!(g.time_usec, 123_456_789);
        assert_eq!(g.fix_type, 3);
        assert_eq!(g.lat, 473_977_418);
        assert_eq!(g.lon, 85_455_940);
        assert_eq!(g.alt, 150_000);
        assert_eq!(g.satellites_visible, 12);
        assert_eq!(g.hdg_acc, 300);
        assert_eq!(g.yaw, 0); // zero-extended past the truncated payload
    }

    #[test]
    fn round_trips_through_decode() {
        let g = GpsRawInt {
            time_usec: u64::MAX,
            fix_type: 4,
            lat: -1,
            lon: i32::MIN,
            alt: 12345,
            eph: 1,
            epv: 2,
            vel: 3,
            cog: 4,
            satellites_visible: 20,
            alt_ellipsoid: -5,
            h_acc: 6,
            v_acc: 7,
            vel_acc: 8,
            hdg_acc: 9,
            yaw: 36000,
        };
        assert_eq!(GpsRawInt::decode(&g.encode()), g);
    }
}