mavwire 0.1.0

no_std, no_alloc parser and encoder for MAVLink v1/v2 telemetry messages
Documentation
//! `SYS_STATUS` (message id 1): onboard sensor and battery health.

use super::payload::{i8_at, i16_at, u16_at, u32_at};

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

/// A decoded `SYS_STATUS` message.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct SysStatus {
    /// Bitmask of onboard sensors/actuators present.
    pub onboard_control_sensors_present: u32,
    /// Bitmask of onboard sensors/actuators enabled.
    pub onboard_control_sensors_enabled: u32,
    /// Bitmask of onboard sensors/actuators reporting healthy.
    pub onboard_control_sensors_health: u32,
    /// Autopilot main loop load, in permille (0-1000 maps to 0-100%).
    pub load: u16,
    /// Battery voltage in millivolts.
    pub voltage_battery: u16,
    /// Battery current, in centiamps (`-1` means not measured).
    pub current_battery: i16,
    /// Communication drop rate, in percent * 100 (permille).
    pub drop_rate_comm: u16,
    /// Communication errors count.
    pub errors_comm: u16,
    /// Autopilot-specific error count 1.
    pub errors_count1: u16,
    /// Autopilot-specific error count 2.
    pub errors_count2: u16,
    /// Autopilot-specific error count 3.
    pub errors_count3: u16,
    /// Autopilot-specific error count 4.
    pub errors_count4: u16,
    /// Remaining battery, in percent (`-1` means not measured).
    pub battery_remaining: i8,
}

impl SysStatus {
    pub(crate) fn decode(payload: &[u8]) -> Self {
        Self {
            onboard_control_sensors_present: u32_at(payload, 0),
            onboard_control_sensors_enabled: u32_at(payload, 4),
            onboard_control_sensors_health: u32_at(payload, 8),
            load: u16_at(payload, 12),
            voltage_battery: u16_at(payload, 14),
            current_battery: i16_at(payload, 16),
            drop_rate_comm: u16_at(payload, 18),
            errors_comm: u16_at(payload, 20),
            errors_count1: u16_at(payload, 22),
            errors_count2: u16_at(payload, 24),
            errors_count3: u16_at(payload, 26),
            errors_count4: u16_at(payload, 28),
            battery_remaining: i8_at(payload, 30),
        }
    }

    pub(crate) fn encode(&self) -> [u8; LEN] {
        let mut buf = [0u8; LEN];
        buf[0..4].copy_from_slice(&self.onboard_control_sensors_present.to_le_bytes());
        buf[4..8].copy_from_slice(&self.onboard_control_sensors_enabled.to_le_bytes());
        buf[8..12].copy_from_slice(&self.onboard_control_sensors_health.to_le_bytes());
        buf[12..14].copy_from_slice(&self.load.to_le_bytes());
        buf[14..16].copy_from_slice(&self.voltage_battery.to_le_bytes());
        buf[16..18].copy_from_slice(&self.current_battery.to_le_bytes());
        buf[18..20].copy_from_slice(&self.drop_rate_comm.to_le_bytes());
        buf[20..22].copy_from_slice(&self.errors_comm.to_le_bytes());
        buf[22..24].copy_from_slice(&self.errors_count1.to_le_bytes());
        buf[24..26].copy_from_slice(&self.errors_count2.to_le_bytes());
        buf[26..28].copy_from_slice(&self.errors_count3.to_le_bytes());
        buf[28..30].copy_from_slice(&self.errors_count4.to_le_bytes());
        buf[30] = self.battery_remaining.to_le_bytes()[0];
        buf
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn decodes_a_real_captured_frame() {
        // Payload from pymavlink's own `SYS_STATUS` encoder: present=1
        // enabled=1 health=1 load=500 voltage=12000 current=1000
        // remaining=80, everything else 0.
        let hex = "010000000100000001000000f401e02ee80300000000000000000000000050";
        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 s = SysStatus::decode(&payload);
        assert_eq!(s.onboard_control_sensors_present, 1);
        assert_eq!(s.load, 500);
        assert_eq!(s.voltage_battery, 12000);
        assert_eq!(s.current_battery, 1000);
        assert_eq!(s.battery_remaining, 80);
    }

    #[test]
    fn round_trips_through_decode() {
        let s = SysStatus {
            onboard_control_sensors_present: 0xFFFF_FFFF,
            onboard_control_sensors_enabled: 0x1234_5678,
            onboard_control_sensors_health: 0,
            load: 999,
            voltage_battery: 11500,
            current_battery: -1,
            drop_rate_comm: 5,
            errors_comm: 1,
            errors_count1: 2,
            errors_count2: 3,
            errors_count3: 4,
            errors_count4: 5,
            battery_remaining: -1,
        };
        assert_eq!(SysStatus::decode(&s.encode()), s);
    }
}