uf-crsf 0.5.0

A `no_std` Rust library for parsing the TBS Crossfire protocol, designed for embedded environments
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139

use crate::packets::{CrsfPacket, PacketType};
use crate::CrsfParsingError;
use core::mem::size_of;

/// Represents a MAVLink System Status Sensor packet (frame type 0xAC).
///
/// To decode data packed within the frame, please refer to the official MAVLink documentation.
#[derive(Clone, Debug, PartialEq)]
#[cfg_attr(feature = "defmt", derive(defmt::Format))]
pub struct MavLinkSensor {
    pub dst_addr: u8,
    pub src_addr: u8,
    /// Bitmask of sensors present.
    pub sensor_present: u32,
    /// Bitmask of sensors enabled.
    pub sensor_enabled: u32,
    /// Bitmask of sensors health.
    pub sensor_health: u32,
}

impl MavLinkSensor {
    pub fn new(
        dst_addr: u8,
        src_addr: u8,
        sensor_present: u32,
        sensor_enabled: u32,
        sensor_health: u32,
    ) -> Result<Self, CrsfParsingError> {
        Ok(Self {
            dst_addr,
            src_addr,
            sensor_present,
            sensor_enabled,
            sensor_health,
        })
    }
}

impl CrsfPacket for MavLinkSensor {
    const PACKET_TYPE: PacketType = PacketType::MavLinkSensor;
    const MIN_PAYLOAD_SIZE: usize = 2 * size_of::<u8>() + 3 * size_of::<u32>();

    fn from_bytes(data: &[u8]) -> Result<Self, CrsfParsingError> {
        if data.len() < Self::MIN_PAYLOAD_SIZE {
            return Err(CrsfParsingError::InvalidPayloadLength);
        }
        Ok(Self {
            dst_addr: data[0],
            src_addr: data[1],
            sensor_present: u32::from_be_bytes(data[2..6].try_into().expect("infallible")),
            sensor_enabled: u32::from_be_bytes(data[6..10].try_into().expect("infallible")),
            sensor_health: u32::from_be_bytes(data[10..14].try_into().expect("infallible")),
        })
    }

    fn to_bytes(&self, buffer: &mut [u8]) -> Result<usize, CrsfParsingError> {
        self.validate_buffer_size(buffer)?;
        buffer[0] = self.dst_addr;
        buffer[1] = self.src_addr;
        buffer[2..6].copy_from_slice(&self.sensor_present.to_be_bytes());
        buffer[6..10].copy_from_slice(&self.sensor_enabled.to_be_bytes());
        buffer[10..14].copy_from_slice(&self.sensor_health.to_be_bytes());
        Ok(Self::MIN_PAYLOAD_SIZE)
    }
}

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

    #[test]
    fn test_mavlink_sensor_from_bytes() {
        assert_eq!(MavLinkSensor::MIN_PAYLOAD_SIZE, 14);
        let data: [u8; 14] = [
            0x00, 0x01, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B, 0x0C,
        ];
        let packet = MavLinkSensor::from_bytes(&data).unwrap();

        assert_eq!(packet.dst_addr, 0x00);
        assert_eq!(packet.src_addr, 0x01);
        assert_eq!(packet.sensor_present, 0x01020304);
        assert_eq!(packet.sensor_enabled, 0x05060708);
        assert_eq!(packet.sensor_health, 0x090A0B0C);
    }

    #[test]
    fn test_mavlink_sensor_to_bytes() {
        let packet = MavLinkSensor {
            sensor_present: 0x01020304,
            dst_addr: 0,
            src_addr: 1,
            sensor_enabled: 0x05060708,
            sensor_health: 0x090A0B0C,
        };
        let mut buffer = [0u8; 14];
        let len = packet.to_bytes(&mut buffer).unwrap();
        assert_eq!(len, 14);
        let expected: [u8; 14] = [
            0x00, 0x01, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B, 0x0C,
        ];
        assert_eq!(buffer, expected);
    }

    #[test]
    fn test_mavlink_sensor_round_trip() {
        let packet = MavLinkSensor {
            dst_addr: 0,
            src_addr: 1,
            sensor_present: 123,
            sensor_enabled: 456,
            sensor_health: 789,
        };
        let mut buffer = [0u8; 14];
        packet.to_bytes(&mut buffer).unwrap();
        let round_trip = MavLinkSensor::from_bytes(&buffer).unwrap();
        assert_eq!(packet, round_trip);
    }

    #[test]
    fn test_mavlink_sensor_from_bytes_too_small() {
        let data: [u8; 13] = [0; 13];
        let result = MavLinkSensor::from_bytes(&data);
        assert_eq!(result, Err(CrsfParsingError::InvalidPayloadLength));
    }

    #[test]
    fn test_mavlink_sensor_to_bytes_too_small() {
        let packet = MavLinkSensor {
            dst_addr: 0,
            src_addr: 1,
            sensor_present: 1,
            sensor_enabled: 2,
            sensor_health: 3,
        };
        let mut buffer = [0u8; 13];
        let result = packet.to_bytes(&mut buffer);
        assert_eq!(result, Err(CrsfParsingError::BufferOverflow));
    }
}