mavwire 0.1.0

no_std, no_alloc parser and encoder for MAVLink v1/v2 telemetry messages
Documentation
//! `COMMAND_LONG` (message id 76): sends a command with up to seven
//! parameters, expecting a `COMMAND_ACK` in response.

use super::payload::{f32_at, u8_at, u16_at};

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

/// A decoded `COMMAND_LONG` message.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct CommandLong {
    /// System id of the target.
    pub target_system: u8,
    /// Component id of the target.
    pub target_component: u8,
    /// Command to execute (`MAV_CMD`).
    pub command: u16,
    /// `0`: first transmission, `1+`: confirmation/retransmission count.
    pub confirmation: u8,
    /// Command-specific parameters 1-7; meaning depends on `command`.
    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() {
        // Real wire payload from pymavlink, truncated to 32 of 33 bytes
        // (confirmation=0 is the last field and gets stripped, see
        // frame.rs): target_system=1 target_component=1 command=400
        // confirmation=0 param1=1, rest 0.
        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); // zero-extended past the truncated payload
        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);
        }
    }
}