use super::payload::{f32_at, u32_at};
pub const ID: u32 = 30;
pub const CRC_EXTRA: u8 = 39;
const LEN: usize = 28;
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Attitude {
pub time_boot_ms: u32,
pub roll: f32,
pub pitch: f32,
pub yaw: f32,
pub rollspeed: f32,
pub pitchspeed: f32,
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() {
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));
}
}