mavwire 0.1.0

no_std, no_alloc parser and encoder for MAVLink v1/v2 telemetry messages
Documentation
//! ATTITUDE (message id 30): roll/pitch/yaw and their rates.

use super::payload::{f32_at, u32_at};

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

/// A decoded ATTITUDE message.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Attitude {
    /// Timestamp, milliseconds since system boot.
    pub time_boot_ms: u32,
    /// Roll angle, radians (-pi..pi).
    pub roll: f32,
    /// Pitch angle, radians (-pi..pi).
    pub pitch: f32,
    /// Yaw angle, radians (-pi..pi).
    pub yaw: f32,
    /// Roll angular speed, rad/s.
    pub rollspeed: f32,
    /// Pitch angular speed, rad/s.
    pub pitchspeed: f32,
    /// Yaw angular speed, rad/s.
    pub yawspeed: f32,
}

impl Attitude {
    pub(crate) fn decode(payload: &[u8]) -> Self {
        Self {
            time_boot_ms: u32_at(payload, 0),
            roll: f32_at(payload, 4),
            pitch: f32_at(payload, 8),
            yaw: f32_at(payload, 12),
            rollspeed: f32_at(payload, 16),
            pitchspeed: f32_at(payload, 20),
            yawspeed: f32_at(payload, 24),
        }
    }

    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.roll.to_le_bytes());
        buf[8..12].copy_from_slice(&self.pitch.to_le_bytes());
        buf[12..16].copy_from_slice(&self.yaw.to_le_bytes());
        buf[16..20].copy_from_slice(&self.rollspeed.to_le_bytes());
        buf[20..24].copy_from_slice(&self.pitchspeed.to_le_bytes());
        buf[24..28].copy_from_slice(&self.yawspeed.to_le_bytes());
        buf
    }
}

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

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

    #[test]
    fn decodes_a_real_captured_frame() {
        // Real wire payload from pymavlink: time_boot_ms=1000 roll=0.1
        // pitch=-0.2 yaw=1.5 rollspeed=0.01 pitchspeed=-0.02 yawspeed=0.03.
        let hex = "e8030000cdcccc3dcdcc4cbe0000c03f0ad7233c0ad7a3bc8fc2f53c";
        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 a = Attitude::decode(&payload);
        assert_eq!(a.time_boot_ms, 1000);
        assert!(close(a.roll, 0.1));
        assert!(close(a.pitch, -0.2));
        assert!(close(a.yaw, 1.5));
        assert!(close(a.rollspeed, 0.01));
        assert!(close(a.pitchspeed, -0.02));
        assert!(close(a.yawspeed, 0.03));
    }

    #[test]
    fn round_trips_through_decode() {
        let a = Attitude {
            time_boot_ms: 42,
            roll: -3.13,
            pitch: 3.13,
            yaw: 0.0,
            rollspeed: 1.0,
            pitchspeed: -1.0,
            yawspeed: 0.5,
        };
        let decoded = Attitude::decode(&a.encode());
        assert_eq!(decoded.time_boot_ms, a.time_boot_ms);
        assert!(close(decoded.roll, a.roll));
        assert!(close(decoded.pitch, a.pitch));
        assert!(close(decoded.yaw, a.yaw));
        assert!(close(decoded.rollspeed, a.rollspeed));
        assert!(close(decoded.pitchspeed, a.pitchspeed));
        assert!(close(decoded.yawspeed, a.yawspeed));
    }
}