uf_crsf/packets/
mod.rs

1use crate::constants;
2use crate::error::CrsfParsingError;
3use crate::parser::RawCrsfPacket;
4use crc;
5
6mod airspeed;
7mod attitude;
8mod baro_altitude;
9mod battery;
10mod commands;
11mod device_information;
12mod device_ping;
13mod esp_now;
14mod flight_mode;
15mod game;
16mod gps;
17mod gps_extended;
18mod gps_time;
19mod heartbeat;
20mod link_statistics;
21mod link_statistics_rx;
22mod link_statistics_tx;
23mod logging;
24mod mavlink_envelope;
25mod mavlink_fc;
26mod mavlink_sensor;
27mod rc_channels_packed;
28mod remote;
29mod rpm;
30mod temp;
31mod vario;
32mod voltages;
33mod vtx_telemetry;
34
35pub use airspeed::AirSpeed;
36pub use attitude::Attitude;
37pub use baro_altitude::BaroAltitude;
38pub use battery::Battery;
39pub use commands::DirectCommands;
40pub use device_information::DeviceInformation;
41pub use device_ping::DevicePing;
42pub use esp_now::EspNow;
43pub use flight_mode::FlightMode;
44pub use game::Game;
45pub use gps::Gps;
46pub use gps_extended::GpsExtended;
47pub use gps_time::GpsTime;
48pub use heartbeat::Heartbeat;
49pub use link_statistics::LinkStatistics;
50pub use link_statistics_rx::LinkStatisticsRx;
51pub use link_statistics_tx::LinkStatisticsTx;
52pub use logging::Logging;
53pub use mavlink_envelope::MavlinkEnvelope;
54pub use mavlink_fc::MavLinkFc;
55pub use mavlink_sensor::MavLinkSensor;
56pub use rc_channels_packed::RcChannelsPacked;
57pub use remote::Remote;
58pub use rpm::Rpm;
59pub use temp::Temp;
60pub use vario::VariometerSensor;
61pub use voltages::Voltages;
62pub use vtx_telemetry::VtxTelemetry;
63
64use num_enum::TryFromPrimitive;
65
66/// A trait representing a deserializable CRSF packet.
67pub trait CrsfPacket: Sized {
68    /// The CRSF frame type identifier for this packet.
69    const PACKET_TYPE: PacketType;
70
71    /// The minimum expected length of the packet's payload in bytes.
72    /// For fixed-size packets, this is the same as the payload size.
73    const MIN_PAYLOAD_SIZE: usize;
74
75    /// Creates a packet instance from a payload byte slice.
76    /// The slice is guaranteed to have a length of at least `MIN_PAYLOAD_SIZE`.
77    fn from_bytes(data: &[u8]) -> Result<Self, CrsfParsingError>;
78    fn to_bytes(&self, buffer: &mut [u8]) -> Result<usize, CrsfParsingError>;
79
80    fn validate_buffer_size(&self, buffer: &[u8]) -> Result<(), CrsfParsingError> {
81        if buffer.len() < Self::MIN_PAYLOAD_SIZE {
82            return Err(CrsfParsingError::BufferOverflow);
83        }
84        Ok(())
85    }
86}
87
88#[derive(Clone, Debug, PartialEq)]
89#[cfg_attr(feature = "defmt", derive(defmt::Format))]
90pub enum Packet {
91    LinkStatistics(LinkStatistics),
92    LinkStatisticsRx(LinkStatisticsRx),
93    LinkStatisticsTx(LinkStatisticsTx),
94    RCChannels(RcChannelsPacked),
95    Gps(Gps),
96    GpsTime(GpsTime),
97    GpsExtended(GpsExtended),
98    Vario(VariometerSensor),
99    Battery(Battery),
100    AirSpeed(AirSpeed),
101    BaroAltitude(BaroAltitude),
102    Rpm(Rpm),
103    Temp(Temp),
104    Voltages(Voltages),
105    VtxTelemetry(VtxTelemetry),
106    FlightMode(FlightMode),
107    Heartbeat(Heartbeat),
108    EspNow(EspNow),
109    MavlinkEnvelope(MavlinkEnvelope),
110    MavLinkFc(MavLinkFc),
111    MavLinkSensor(MavLinkSensor),
112    Remote(Remote),
113    Attitude(Attitude),
114    DeviceInformation(DeviceInformation),
115    DevicePing(DevicePing),
116    Game(Game),
117    NotImlemented(PacketType, usize),
118    Commands(DirectCommands),
119    Logging(Logging),
120}
121
122impl Packet {
123    pub fn parse(raw_packet: &RawCrsfPacket<'_>) -> Result<Packet, CrsfParsingError> {
124        let packet_type = PacketType::try_from_primitive(raw_packet.raw_packet_type())
125            .map_err(|_| CrsfParsingError::UnexpectedPacketType(raw_packet.raw_packet_type()))?;
126
127        let data = raw_packet.payload();
128        match packet_type {
129            LinkStatistics::PACKET_TYPE => {
130                Ok(Self::LinkStatistics(LinkStatistics::from_bytes(data)?))
131            }
132            LinkStatisticsTx::PACKET_TYPE => {
133                Ok(Self::LinkStatisticsTx(LinkStatisticsTx::from_bytes(data)?))
134            }
135            LinkStatisticsRx::PACKET_TYPE => {
136                Ok(Self::LinkStatisticsRx(LinkStatisticsRx::from_bytes(data)?))
137            }
138            RcChannelsPacked::PACKET_TYPE => {
139                Ok(Self::RCChannels(RcChannelsPacked::from_bytes(data)?))
140            }
141            Gps::PACKET_TYPE => Ok(Self::Gps(Gps::from_bytes(data)?)),
142            GpsTime::PACKET_TYPE => Ok(Self::GpsTime(GpsTime::from_bytes(data)?)),
143            GpsExtended::PACKET_TYPE => Ok(Self::GpsExtended(GpsExtended::from_bytes(data)?)),
144            AirSpeed::PACKET_TYPE => Ok(Self::AirSpeed(AirSpeed::from_bytes(data)?)),
145            BaroAltitude::PACKET_TYPE => Ok(Self::BaroAltitude(BaroAltitude::from_bytes(data)?)),
146            Battery::PACKET_TYPE => Ok(Self::Battery(Battery::from_bytes(data)?)),
147            FlightMode::PACKET_TYPE => Ok(Self::FlightMode(FlightMode::from_bytes(data)?)),
148            Rpm::PACKET_TYPE => Ok(Self::Rpm(Rpm::from_bytes(data)?)),
149            Temp::PACKET_TYPE => Ok(Self::Temp(Temp::from_bytes(data)?)),
150            Voltages::PACKET_TYPE => Ok(Self::Voltages(Voltages::from_bytes(data)?)),
151            VtxTelemetry::PACKET_TYPE => Ok(Self::VtxTelemetry(VtxTelemetry::from_bytes(data)?)),
152            VariometerSensor::PACKET_TYPE => Ok(Self::Vario(VariometerSensor::from_bytes(data)?)),
153            EspNow::PACKET_TYPE => Ok(Self::EspNow(EspNow::from_bytes(data)?)),
154            Heartbeat::PACKET_TYPE => Ok(Self::Heartbeat(Heartbeat::from_bytes(data)?)),
155            MavLinkFc::PACKET_TYPE => Ok(Self::MavLinkFc(MavLinkFc::from_bytes(data)?)),
156            MavLinkSensor::PACKET_TYPE => Ok(Self::MavLinkSensor(MavLinkSensor::from_bytes(data)?)),
157            Remote::PACKET_TYPE => Ok(Self::Remote(Remote::from_bytes(data)?)),
158            Attitude::PACKET_TYPE => Ok(Self::Attitude(Attitude::from_bytes(data)?)),
159            DevicePing::PACKET_TYPE => Ok(Self::DevicePing(DevicePing::from_bytes(data)?)),
160            Game::PACKET_TYPE => Ok(Self::Game(Game::from_bytes(data)?)),
161            DeviceInformation::PACKET_TYPE => Ok(Self::DeviceInformation(
162                DeviceInformation::from_bytes(data)?,
163            )),
164
165            MavlinkEnvelope::PACKET_TYPE => {
166                Ok(Self::MavlinkEnvelope(MavlinkEnvelope::from_bytes(data)?))
167            }
168            DirectCommands::PACKET_TYPE => Ok(Self::Commands(DirectCommands::from_bytes(data)?)),
169            Logging::PACKET_TYPE => Ok(Self::Logging(Logging::from_bytes(data)?)),
170            _ => Ok(Packet::NotImlemented(
171                packet_type,
172                raw_packet.payload().len(),
173            )),
174        }
175    }
176}
177
178#[non_exhaustive]
179#[derive(Clone, Copy, Debug, PartialEq, Eq, num_enum::TryFromPrimitive)]
180#[cfg_attr(feature = "defmt", derive(defmt::Format))]
181#[repr(u8)]
182pub enum PacketType {
183    Gps = 0x02,
184    GpsTime = 0x03,
185    GpsExtended = 0x06,
186    Vario = 0x07,
187    BatterySensor = 0x08,
188    BaroAltitude = 0x09,
189    AirSpeed = 0x0A,
190    Rpm = 0x0C,
191    Temp = 0x0D,
192    Voltages = 0x0E,
193    VtxTelemetry = 0x10,
194    Heartbeat = 0x0B,
195    LinkStatistics = 0x14,
196    RcChannelsPacked = 0x16,
197    SubsetRcChannelsPacked = 0x17,
198    LinkStatisticsRx = 0x1C,
199    LinkStatisticsTx = 0x1D,
200    Attitude = 0x1E,
201    MavLinkFc = 0x1F,
202    FlightMode = 0x21,
203    EspNow = 0x22,
204    DevicePing = 0x28,
205    DeviceInfo = 0x29,
206    ParameterSettingsEntry = 0x2B,
207    ParameterRead = 0x2C,
208    ParameterWrite = 0x2D,
209    ElrsStatus = 0x2E,
210    Command = 0x32,
211    Logging = 0x34,
212    RadioId = 0x3A,
213    KissRequest = 0x78,
214    KissResponse = 0x79,
215    MspRequest = 0x7A,
216    MspResponse = 0x7B,
217    MspWrite = 0x7C,
218    ArdupilotResponse = 0x80,
219    MavlinkEnvelope = 0xAA,
220    MavLinkSensor = 0xAC,
221    Game = 0x3C,
222}
223
224impl PacketType {
225    pub fn is_extended(self) -> bool {
226        self as u8 >= 0x28
227    }
228}
229
230/// Represents all CRSF packet addresses
231#[non_exhaustive]
232#[derive(Clone, Copy, Debug, PartialEq, Eq, TryFromPrimitive)]
233#[cfg_attr(feature = "defmt", derive(defmt::Format))]
234#[repr(u8)]
235pub enum PacketAddress {
236    Broadcast = 0x00,
237    Cloud = 0x0E,
238    Usb = 0x10,
239    Bluetooth = 0x12,
240    WifiReceiver = 0x13,
241    VideoReceiver = 0x14,
242    TbsCorePnpPro = 0x80,
243    Esc1 = 0x90,
244    Esc2 = 0x91,
245    Esc3 = 0x92,
246    Esc4 = 0x93,
247    Esc5 = 0x94,
248    Esc6 = 0x95,
249    Esc7 = 0x96,
250    Esc8 = 0x97,
251    CurrentSensor = 0xC0,
252    Gps = 0xC2,
253    TbsBlackbox = 0xC4,
254    FlightController = 0xC8,
255    RaceTag = 0xCC,
256    VTX = 0xCE,
257    Handset = 0xEA,
258    Receiver = 0xEC,
259    Transmitter = 0xEE,
260}
261
262pub fn write_packet_to_buffer<T: CrsfPacket>(
263    buffer: &mut [u8],
264    dest: PacketAddress,
265    packet: &T,
266) -> Result<usize, CrsfParsingError> {
267    const MAX_PAYLOAD_SIZE: usize = constants::CRSF_MAX_PACKET_SIZE - 4;
268    static CRC8_DVB_S2: crc::Crc<u8> = crc::Crc::<u8>::new(&crc::CRC_8_DVB_S2);
269    let mut payload_buf = [0u8; MAX_PAYLOAD_SIZE];
270
271    let payload_size = packet.to_bytes(&mut payload_buf)?;
272
273    let total_frame_size = payload_size + 4;
274    if buffer.len() < total_frame_size {
275        return Err(CrsfParsingError::BufferOverflow);
276    }
277
278    // length byte = 2 (type + crc) + payload_size
279    let length_byte = (payload_size + 2) as u8;
280
281    buffer[0] = dest as u8;
282    buffer[1] = length_byte;
283    buffer[2] = T::PACKET_TYPE as u8;
284    buffer[3..3 + payload_size].copy_from_slice(&payload_buf[..payload_size]);
285
286    // CRC is calculated over type and payload
287    let crc_payload = &buffer[2..3 + payload_size];
288    let mut digest = CRC8_DVB_S2.digest();
289    digest.update(crc_payload);
290    let calculated_crc = digest.finalize();
291
292    buffer[3 + payload_size] = calculated_crc;
293
294    Ok(total_frame_size)
295}
296
297#[cfg(test)]
298mod tests {
299    use super::*;
300
301    struct MockPacket {
302        payload: [u8; 2],
303    }
304
305    impl CrsfPacket for MockPacket {
306        const PACKET_TYPE: PacketType = PacketType::Command;
307        const MIN_PAYLOAD_SIZE: usize = 2;
308
309        fn from_bytes(data: &[u8]) -> Result<Self, CrsfParsingError> {
310            // Not needed for this test
311            Ok(Self {
312                payload: [data[0], data[1]],
313            })
314        }
315
316        fn to_bytes(&self, buffer: &mut [u8]) -> Result<usize, CrsfParsingError> {
317            buffer[..2].copy_from_slice(&self.payload);
318            Ok(2)
319        }
320    }
321
322    #[test]
323    fn test_write_packet_to_buffer() {
324        let mut buffer = [0u8; 64];
325        let packet = MockPacket {
326            payload: [0xAA, 0xBB],
327        };
328        let dest = PacketAddress::FlightController;
329
330        let result = write_packet_to_buffer(&mut buffer, dest, &packet);
331
332        assert!(result.is_ok());
333        let bytes_written = result.unwrap();
334        assert_eq!(bytes_written, 6);
335
336        // Destination, Length, Type, Payload, CRC
337        // Length = Type (1) + Payload (2) + CRC (1) = 4
338        // CRC is calculated over Type and Payload
339        let expected_packet: [u8; 6] = [
340            dest as u8,
341            4,
342            MockPacket::PACKET_TYPE as u8,
343            0xAA,
344            0xBB,
345            0x32,
346        ];
347        assert_eq!(&buffer[..bytes_written], &expected_packet[..]);
348    }
349
350    #[test]
351    fn test_write_packet_to_buffer_overflow() {
352        let mut buffer = [0u8; 5]; // Too small for a 6-byte packet
353        let packet = MockPacket {
354            payload: [0xAA, 0xBB],
355        };
356        let dest = PacketAddress::FlightController;
357
358        let result = write_packet_to_buffer(&mut buffer, dest, &packet);
359
360        assert!(result.is_err());
361        assert_eq!(result.unwrap_err(), CrsfParsingError::BufferOverflow);
362    }
363}
uf_crsf/packets/mod.rs

uf_crsf/packets/
mod.rs
