use super::payload::{i16_at, i32_at, u16_at, u32_at};
pub const ID: u32 = 33;
pub const CRC_EXTRA: u8 = 104;
const LEN: usize = 28;
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct GlobalPositionInt {
pub time_boot_ms: u32,
pub lat: i32,
pub lon: i32,
pub alt: i32,
pub relative_alt: i32,
pub vx: i16,
pub vy: i16,
pub vz: i16,
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() {
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);
}
}