uf_crsf/packets/
vtx_telemetry.rs

1use crate::packets::CrsfPacket;
2use crate::packets::PacketType;
3use crate::CrsfParsingError;
4
5/// Represents a VTX Telemetry packet.
6#[derive(Clone, Debug, PartialEq)]
7#[cfg_attr(feature = "defmt", derive(defmt::Format))]
8pub struct VtxTelemetry {
9    /// Origin device address.
10    pub origin_address: u8,
11    /// VTX power in dBm.
12    pub power_dbm: u8,
13    /// VTX frequency in MHz.
14    pub frequency_mhz: u16,
15    /// Pit mode status (0=Off, 1=On).
16    pub pit_mode: bool,
17    /// Pit mode control (0=Off, 1=On, 2=Switch, 3=Failsafe).
18    pub pitmode_control: u8,
19    /// Pit mode switch (0=Ch5, 1=Ch5 Inv, ...).
20    pub pitmode_switch: u8,
21}
22
23impl VtxTelemetry {
24    pub fn new(
25        origin_address: u8,
26        power_dbm: u8,
27        frequency_mhz: u16,
28        pit_mode: bool,
29        pitmode_control: u8,
30        pitmode_switch: u8,
31    ) -> Result<Self, CrsfParsingError> {
32        Ok(Self {
33            origin_address,
34            power_dbm,
35            frequency_mhz,
36            pit_mode,
37            pitmode_control,
38            pitmode_switch,
39        })
40    }
41}
42
43impl CrsfPacket for VtxTelemetry {
44    const PACKET_TYPE: PacketType = PacketType::VtxTelemetry;
45    const MIN_PAYLOAD_SIZE: usize = 5;
46
47    fn to_bytes(&self, buffer: &mut [u8]) -> Result<usize, CrsfParsingError> {
48        self.validate_buffer_size(buffer)?;
49        buffer[0] = self.origin_address;
50        buffer[1] = self.power_dbm;
51        buffer[2..4].copy_from_slice(&self.frequency_mhz.to_be_bytes());
52        let pit_byte =
53            (u8::from(self.pit_mode)) | (self.pitmode_control << 1) | (self.pitmode_switch << 3);
54        buffer[4] = pit_byte;
55        Ok(Self::MIN_PAYLOAD_SIZE)
56    }
57
58    fn from_bytes(data: &[u8]) -> Result<Self, CrsfParsingError> {
59        if data.len() < Self::MIN_PAYLOAD_SIZE {
60            return Err(CrsfParsingError::InvalidPayloadLength);
61        }
62        let pit_byte = data[4];
63        Ok(Self {
64            origin_address: data[0],
65            power_dbm: data[1],
66            frequency_mhz: u16::from_be_bytes(
67                data[2..4]
68                    .try_into()
69                    .map_err(|_| CrsfParsingError::InvalidPayloadLength)?,
70            ),
71            pit_mode: (pit_byte & 0b1) != 0,
72            pitmode_control: (pit_byte >> 1) & 0b11,
73            pitmode_switch: (pit_byte >> 3) & 0b1111,
74        })
75    }
76}
77
78#[cfg(test)]
79mod tests {
80    use super::*;
81
82    #[test]
83    fn test_vtx_telemetry_to_bytes() {
84        let vtx_telemetry = VtxTelemetry::new(0xEE, 25, 5800, true, 2, 5).unwrap();
85
86        let mut buffer = [0u8; VtxTelemetry::MIN_PAYLOAD_SIZE];
87        vtx_telemetry.to_bytes(&mut buffer).unwrap();
88
89        // pit_byte = 1 | (2 << 1) | (5 << 3) = 1 | 4 | 40 = 45 = 0b00101101
90        let expected_bytes: [u8; VtxTelemetry::MIN_PAYLOAD_SIZE] =
91            [0xEE, 25, 0x16, 0xA8, 0b00101101];
92
93        assert_eq!(buffer, expected_bytes);
94    }
95
96    #[test]
97    fn test_vtx_telemetry_from_bytes() {
98        let data: [u8; VtxTelemetry::MIN_PAYLOAD_SIZE] = [0xEE, 25, 0x16, 0xA8, 0b00101101];
99
100        let vtx_telemetry = VtxTelemetry::from_bytes(&data).unwrap();
101
102        assert_eq!(
103            vtx_telemetry,
104            VtxTelemetry {
105                origin_address: 0xEE,
106                power_dbm: 25,
107                frequency_mhz: 5800,
108                pit_mode: true,
109                pitmode_control: 2,
110                pitmode_switch: 5,
111            }
112        );
113    }
114
115    #[test]
116    fn test_vtx_telemetry_round_trip() {
117        let vtx_telemetry = VtxTelemetry {
118            origin_address: 0xCE,
119            power_dbm: 10,
120            frequency_mhz: 5740,
121            pit_mode: false,
122            pitmode_control: 1,
123            pitmode_switch: 10,
124        };
125
126        let mut buffer = [0u8; VtxTelemetry::MIN_PAYLOAD_SIZE];
127        vtx_telemetry.to_bytes(&mut buffer).unwrap();
128
129        let round_trip_vtx_telemetry = VtxTelemetry::from_bytes(&buffer).unwrap();
130
131        assert_eq!(vtx_telemetry, round_trip_vtx_telemetry);
132    }
133}
uf_crsf/packets/vtx_telemetry.rs

uf_crsf/packets/
vtx_telemetry.rs
