uf_crsf/packets/
mavlink_sensor.rs

1use crate::packets::{CrsfPacket, PacketType};
2use crate::CrsfParsingError;
3use core::mem::size_of;
4
5/// Represents a MAVLink System Status Sensor packet (frame type 0xAC).
6///
7/// To decode data packed within the frame, please refer to the official MAVLink documentation.
8#[derive(Clone, Debug, PartialEq)]
9#[cfg_attr(feature = "defmt", derive(defmt::Format))]
10pub struct MavLinkSensor {
11    pub dst_addr: u8,
12    pub src_addr: u8,
13    /// Bitmask of sensors present.
14    pub sensor_present: u32,
15    /// Bitmask of sensors enabled.
16    pub sensor_enabled: u32,
17    /// Bitmask of sensors health.
18    pub sensor_health: u32,
19}
20
21impl MavLinkSensor {
22    pub fn new(
23        dst_addr: u8,
24        src_addr: u8,
25        sensor_present: u32,
26        sensor_enabled: u32,
27        sensor_health: u32,
28    ) -> Result<Self, CrsfParsingError> {
29        Ok(Self {
30            dst_addr,
31            src_addr,
32            sensor_present,
33            sensor_enabled,
34            sensor_health,
35        })
36    }
37}
38
39impl CrsfPacket for MavLinkSensor {
40    const PACKET_TYPE: PacketType = PacketType::MavLinkSensor;
41    const MIN_PAYLOAD_SIZE: usize = 2 * size_of::<u8>() + 3 * size_of::<u32>();
42
43    fn from_bytes(data: &[u8]) -> Result<Self, CrsfParsingError> {
44        if data.len() < Self::MIN_PAYLOAD_SIZE {
45            return Err(CrsfParsingError::InvalidPayloadLength);
46        }
47        Ok(Self {
48            dst_addr: data[0],
49            src_addr: data[1],
50            sensor_present: u32::from_be_bytes(data[2..6].try_into().expect("infallible")),
51            sensor_enabled: u32::from_be_bytes(data[6..10].try_into().expect("infallible")),
52            sensor_health: u32::from_be_bytes(data[10..14].try_into().expect("infallible")),
53        })
54    }
55
56    fn to_bytes(&self, buffer: &mut [u8]) -> Result<usize, CrsfParsingError> {
57        self.validate_buffer_size(buffer)?;
58        buffer[0] = self.dst_addr;
59        buffer[1] = self.src_addr;
60        buffer[2..6].copy_from_slice(&self.sensor_present.to_be_bytes());
61        buffer[6..10].copy_from_slice(&self.sensor_enabled.to_be_bytes());
62        buffer[10..14].copy_from_slice(&self.sensor_health.to_be_bytes());
63        Ok(Self::MIN_PAYLOAD_SIZE)
64    }
65}
66
67#[cfg(test)]
68mod tests {
69    use super::*;
70
71    #[test]
72    fn test_mavlink_sensor_from_bytes() {
73        assert_eq!(MavLinkSensor::MIN_PAYLOAD_SIZE, 14);
74        let data: [u8; 14] = [
75            0x00, 0x01, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B, 0x0C,
76        ];
77        let packet = MavLinkSensor::from_bytes(&data).unwrap();
78
79        assert_eq!(packet.dst_addr, 0x00);
80        assert_eq!(packet.src_addr, 0x01);
81        assert_eq!(packet.sensor_present, 0x01020304);
82        assert_eq!(packet.sensor_enabled, 0x05060708);
83        assert_eq!(packet.sensor_health, 0x090A0B0C);
84    }
85
86    #[test]
87    fn test_mavlink_sensor_to_bytes() {
88        let packet = MavLinkSensor {
89            sensor_present: 0x01020304,
90            dst_addr: 0,
91            src_addr: 1,
92            sensor_enabled: 0x05060708,
93            sensor_health: 0x090A0B0C,
94        };
95        let mut buffer = [0u8; 14];
96        let len = packet.to_bytes(&mut buffer).unwrap();
97        assert_eq!(len, 14);
98        let expected: [u8; 14] = [
99            0x00, 0x01, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B, 0x0C,
100        ];
101        assert_eq!(buffer, expected);
102    }
103
104    #[test]
105    fn test_mavlink_sensor_round_trip() {
106        let packet = MavLinkSensor {
107            dst_addr: 0,
108            src_addr: 1,
109            sensor_present: 123,
110            sensor_enabled: 456,
111            sensor_health: 789,
112        };
113        let mut buffer = [0u8; 14];
114        packet.to_bytes(&mut buffer).unwrap();
115        let round_trip = MavLinkSensor::from_bytes(&buffer).unwrap();
116        assert_eq!(packet, round_trip);
117    }
118
119    #[test]
120    fn test_mavlink_sensor_from_bytes_too_small() {
121        let data: [u8; 13] = [0; 13];
122        let result = MavLinkSensor::from_bytes(&data);
123        assert_eq!(result, Err(CrsfParsingError::InvalidPayloadLength));
124    }
125
126    #[test]
127    fn test_mavlink_sensor_to_bytes_too_small() {
128        let packet = MavLinkSensor {
129            dst_addr: 0,
130            src_addr: 1,
131            sensor_present: 1,
132            sensor_enabled: 2,
133            sensor_health: 3,
134        };
135        let mut buffer = [0u8; 13];
136        let result = packet.to_bytes(&mut buffer);
137        assert_eq!(result, Err(CrsfParsingError::BufferOverflow));
138    }
139}
uf_crsf/packets/mavlink_sensor.rs

uf_crsf/packets/
mavlink_sensor.rs
