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

use crate::packets::CrsfPacket;
use crate::packets::PacketType;
use crate::CrsfParsingError;

/// Represents a VTX Telemetry packet.
#[derive(Clone, Debug, PartialEq)]
#[cfg_attr(feature = "defmt", derive(defmt::Format))]
pub struct VtxTelemetry {
    /// Origin device address.
    pub origin_address: u8,
    /// VTX power in dBm.
    pub power_dbm: u8,
    /// VTX frequency in MHz.
    pub frequency_mhz: u16,
    /// Pit mode status (0=Off, 1=On).
    pub pit_mode: bool,
    /// Pit mode control (0=Off, 1=On, 2=Switch, 3=Failsafe).
    pub pitmode_control: u8,
    /// Pit mode switch (0=Ch5, 1=Ch5 Inv, ...).
    pub pitmode_switch: u8,
}

impl VtxTelemetry {
    pub fn new(
        origin_address: u8,
        power_dbm: u8,
        frequency_mhz: u16,
        pit_mode: bool,
        pitmode_control: u8,
        pitmode_switch: u8,
    ) -> Result<Self, CrsfParsingError> {
        Ok(Self {
            origin_address,
            power_dbm,
            frequency_mhz,
            pit_mode,
            pitmode_control,
            pitmode_switch,
        })
    }
}

impl CrsfPacket for VtxTelemetry {
    const PACKET_TYPE: PacketType = PacketType::VtxTelemetry;
    const MIN_PAYLOAD_SIZE: usize = 5;

    fn to_bytes(&self, buffer: &mut [u8]) -> Result<usize, CrsfParsingError> {
        self.validate_buffer_size(buffer)?;
        buffer[0] = self.origin_address;
        buffer[1] = self.power_dbm;
        buffer[2..4].copy_from_slice(&self.frequency_mhz.to_be_bytes());
        let pit_byte =
            (u8::from(self.pit_mode)) | (self.pitmode_control << 1) | (self.pitmode_switch << 3);
        buffer[4] = pit_byte;
        Ok(Self::MIN_PAYLOAD_SIZE)
    }

    fn from_bytes(data: &[u8]) -> Result<Self, CrsfParsingError> {
        if data.len() < Self::MIN_PAYLOAD_SIZE {
            return Err(CrsfParsingError::InvalidPayloadLength);
        }
        let pit_byte = data[4];
        Ok(Self {
            origin_address: data[0],
            power_dbm: data[1],
            frequency_mhz: u16::from_be_bytes(
                data[2..4]
                    .try_into()
                    .map_err(|_| CrsfParsingError::InvalidPayloadLength)?,
            ),
            pit_mode: (pit_byte & 0b1) != 0,
            pitmode_control: (pit_byte >> 1) & 0b11,
            pitmode_switch: (pit_byte >> 3) & 0b1111,
        })
    }
}

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

    #[test]
    fn test_vtx_telemetry_to_bytes() {
        let vtx_telemetry = VtxTelemetry::new(0xEE, 25, 5800, true, 2, 5).unwrap();

        let mut buffer = [0u8; VtxTelemetry::MIN_PAYLOAD_SIZE];
        vtx_telemetry.to_bytes(&mut buffer).unwrap();

        // pit_byte = 1 | (2 << 1) | (5 << 3) = 1 | 4 | 40 = 45 = 0b00101101
        let expected_bytes: [u8; VtxTelemetry::MIN_PAYLOAD_SIZE] =
            [0xEE, 25, 0x16, 0xA8, 0b00101101];

        assert_eq!(buffer, expected_bytes);
    }

    #[test]
    fn test_vtx_telemetry_from_bytes() {
        let data: [u8; VtxTelemetry::MIN_PAYLOAD_SIZE] = [0xEE, 25, 0x16, 0xA8, 0b00101101];

        let vtx_telemetry = VtxTelemetry::from_bytes(&data).unwrap();

        assert_eq!(
            vtx_telemetry,
            VtxTelemetry {
                origin_address: 0xEE,
                power_dbm: 25,
                frequency_mhz: 5800,
                pit_mode: true,
                pitmode_control: 2,
                pitmode_switch: 5,
            }
        );
    }

    #[test]
    fn test_vtx_telemetry_round_trip() {
        let vtx_telemetry = VtxTelemetry {
            origin_address: 0xCE,
            power_dbm: 10,
            frequency_mhz: 5740,
            pit_mode: false,
            pitmode_control: 1,
            pitmode_switch: 10,
        };

        let mut buffer = [0u8; VtxTelemetry::MIN_PAYLOAD_SIZE];
        vtx_telemetry.to_bytes(&mut buffer).unwrap();

        let round_trip_vtx_telemetry = VtxTelemetry::from_bytes(&buffer).unwrap();

        assert_eq!(vtx_telemetry, round_trip_vtx_telemetry);
    }
}