use super::payload::{u8_at, u32_at};
pub const ID: u32 = 0;
pub const CRC_EXTRA: u8 = 50;
const LEN: usize = 9;
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct Heartbeat {
pub mav_type: u8,
pub autopilot: u8,
pub base_mode: u8,
pub custom_mode: u32,
pub system_status: u8,
pub mavlink_version: u8,
}
impl Heartbeat {
pub(crate) fn decode(payload: &[u8]) -> Self {
Self {
custom_mode: u32_at(payload, 0),
mav_type: u8_at(payload, 4),
autopilot: u8_at(payload, 5),
base_mode: u8_at(payload, 6),
system_status: u8_at(payload, 7),
mavlink_version: u8_at(payload, 8),
}
}
pub(crate) fn encode(&self) -> [u8; LEN] {
let mut buf = [0u8; LEN];
buf[0..4].copy_from_slice(&self.custom_mode.to_le_bytes());
buf[4] = self.mav_type;
buf[5] = self.autopilot;
buf[6] = self.base_mode;
buf[7] = self.system_status;
buf[8] = self.mavlink_version;
buf
}
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn decodes_a_real_captured_frame() {
let payload = [0x00, 0x00, 0x00, 0x00, 0x02, 0x03, 0x51, 0x04, 0x03];
let hb = Heartbeat::decode(&payload);
assert_eq!(
hb,
Heartbeat {
mav_type: 2,
autopilot: 3,
base_mode: 81,
custom_mode: 0,
system_status: 4,
mavlink_version: 3,
}
);
}
#[test]
fn encode_matches_the_real_captured_payload() {
let hb = Heartbeat {
mav_type: 2,
autopilot: 3,
base_mode: 81,
custom_mode: 0,
system_status: 4,
mavlink_version: 3,
};
assert_eq!(
hb.encode(),
[0x00, 0x00, 0x00, 0x00, 0x02, 0x03, 0x51, 0x04, 0x03]
);
}
#[test]
fn round_trips_through_decode() {
let hb = Heartbeat {
mav_type: 6,
autopilot: 12,
base_mode: 128,
custom_mode: 0x1234_5678,
system_status: 3,
mavlink_version: 3,
};
assert_eq!(Heartbeat::decode(&hb.encode()), hb);
}
}