use super::payload::{f32_at, u8_at, u16_at};
pub const ID: u32 = 76;
pub const CRC_EXTRA: u8 = 152;
const LEN: usize = 33;
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct CommandLong {
pub target_system: u8,
pub target_component: u8,
pub command: u16,
pub confirmation: u8,
pub params: [f32; 7],
}
impl CommandLong {
pub(crate) fn decode(payload: &[u8]) -> Self {
let mut params = [0f32; 7];
for (i, p) in params.iter_mut().enumerate() {
*p = f32_at(payload, i * 4);
}
Self {
params,
command: u16_at(payload, 28),
target_system: u8_at(payload, 30),
target_component: u8_at(payload, 31),
confirmation: u8_at(payload, 32),
}
}
pub(crate) fn encode(&self) -> [u8; LEN] {
let mut buf = [0u8; LEN];
for (i, p) in self.params.iter().enumerate() {
buf[i * 4..i * 4 + 4].copy_from_slice(&p.to_le_bytes());
}
buf[28..30].copy_from_slice(&self.command.to_le_bytes());
buf[30] = self.target_system;
buf[31] = self.target_component;
buf[32] = self.confirmation;
buf
}
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn decodes_a_real_captured_frame() {
let hex = "0000803f00000000000000000000000000000000000000000000000090010101";
let mut payload = [0u8; 32];
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 c = CommandLong::decode(&payload);
assert_eq!(c.target_system, 1);
assert_eq!(c.target_component, 1);
assert_eq!(c.command, 400);
assert_eq!(c.confirmation, 0); assert!((c.params[0] - 1.0).abs() < f32::EPSILON);
assert!(c.params[1..].iter().all(|&p| p == 0.0));
}
#[test]
fn round_trips_through_decode() {
let c = CommandLong {
target_system: 5,
target_component: 6,
command: 22,
confirmation: 1,
params: [1.0, -2.0, 3.5, 0.0, 0.0, 0.0, 0.0],
};
let decoded = CommandLong::decode(&c.encode());
assert_eq!(decoded.target_system, c.target_system);
assert_eq!(decoded.target_component, c.target_component);
assert_eq!(decoded.command, c.command);
assert_eq!(decoded.confirmation, c.confirmation);
for (a, b) in decoded.params.iter().zip(c.params.iter()) {
assert!((a - b).abs() < f32::EPSILON);
}
}
}