uf_sbus/
lib.rs

1#![no_std]
2#![forbid(unsafe_code)]
3
4const SBUS_PACKET_SIZE: usize = 25;
5const SBUS_NUM_CHANNELS: usize = 16;
6const SBUS_HEADER: u8 = 0x0F;
7const SBUS_FLAG_BYTE_MASK: u8 = 0xF0;
8const SBUS_FOOTER: u8 = 0x00;
9const CHAN_MASK: u16 = 0x07FF;
10
11#[derive(Debug, PartialEq, Eq)]
12#[cfg_attr(feature = "defmt", derive(defmt::Format))]
13pub enum SbusParserError {
14    InvalidFooter(u8),
15    InvalidFlags(u8),
16}
17
18#[inline]
19fn is_sbus_footer(byte: u8) -> bool {
20    match byte {
21        0x00 => true, // SBUS packet end
22        0x04 => true, // SBUS telemetry slot 0 to Slot 7
23        0x14 => true, // SBUS telemetry slot 8 to Slot 15
24        0x24 => true, // SBUS telemetry slot 16 to Slot 23
25        0x34 => true, // SBUS telemetry slot 24 to Slot 31
26        _ => false,
27    }
28}
29
30#[inline]
31fn is_flag_set_at_position(flag_byte: u8, shift_by: u8) -> bool {
32    (flag_byte >> shift_by) & 1 == 1
33}
34
35#[derive(Debug, Ord, PartialOrd, Eq, PartialEq, Copy, Clone)]
36#[cfg_attr(feature = "defmt", derive(defmt::Format))]
37pub struct RawSbusPacket {
38    bytes: [u8; SBUS_PACKET_SIZE],
39}
40
41impl RawSbusPacket {
42    pub fn new(bytes: &[u8; SBUS_PACKET_SIZE]) -> Self {
43        Self { bytes: *bytes }
44    }
45
46    pub fn as_bytes(&self) -> &[u8] {
47        &self.bytes
48    }
49}
50
51#[derive(Debug, Ord, PartialOrd, Eq, PartialEq, Copy, Clone)]
52#[cfg_attr(feature = "defmt", derive(defmt::Format))]
53pub struct SbusPacket {
54    pub channels: [u16; SBUS_NUM_CHANNELS],
55    pub channel_17: bool,
56    pub channel_18: bool,
57    pub failsafe: bool,
58    pub frame_lost: bool,
59}
60
61impl SbusPacket {
62    pub fn parse(raw_packet: &RawSbusPacket) -> Self {
63        let buf = raw_packet.as_bytes();
64        // Initialize channels with 11-bit mask
65        let mut ch: [u16; SBUS_NUM_CHANNELS] = [CHAN_MASK; SBUS_NUM_CHANNELS];
66
67        ch[0] &= (buf[1] as u16) | ((buf[2] as u16) << 8);
68        ch[1] &= ((buf[2] as u16) >> 3) | ((buf[3] as u16) << 5);
69        ch[2] &= ((buf[3] as u16) >> 6) | ((buf[4] as u16) << 2) | ((buf[5] as u16) << 10);
70        ch[3] &= ((buf[5] as u16) >> 1) | ((buf[6] as u16) << 7);
71        ch[4] &= ((buf[6] as u16) >> 4) | ((buf[7] as u16) << 4);
72        ch[5] &= ((buf[7] as u16) >> 7) | ((buf[8] as u16) << 1) | ((buf[9] as u16) << 9);
73        ch[6] &= ((buf[9] as u16) >> 2) | ((buf[10] as u16) << 6);
74        ch[7] &= ((buf[10] as u16) >> 5) | ((buf[11] as u16) << 3);
75
76        ch[8] &= (buf[12] as u16) | ((buf[13] as u16) << 8);
77        ch[9] &= ((buf[13] as u16) >> 3) | ((buf[14] as u16) << 5);
78        ch[10] &= ((buf[14] as u16) >> 6) | ((buf[15] as u16) << 2) | ((buf[16] as u16) << 10);
79        ch[11] &= ((buf[16] as u16) >> 1) | ((buf[17] as u16) << 7);
80        ch[12] &= ((buf[17] as u16) >> 4) | ((buf[18] as u16) << 4);
81        ch[13] &= ((buf[18] as u16) >> 7) | ((buf[19] as u16) << 1) | ((buf[20] as u16) << 9);
82        ch[14] &= ((buf[20] as u16) >> 2) | ((buf[21] as u16) << 6);
83        ch[15] &= ((buf[21] as u16) >> 5) | ((buf[22] as u16) << 3);
84
85        let flag_byte = buf[23];
86
87        SbusPacket {
88            channels: ch,
89            channel_17: is_flag_set_at_position(flag_byte, 0),
90            channel_18: is_flag_set_at_position(flag_byte, 1),
91            frame_lost: is_flag_set_at_position(flag_byte, 2),
92            failsafe: is_flag_set_at_position(flag_byte, 3),
93        }
94    }
95}
96
97#[derive(Debug, Default, Ord, PartialOrd, Eq, PartialEq, Copy, Clone)]
98pub enum State {
99    #[default]
100    AwaitingHead,
101    Reading(usize),
102}
103
104#[derive(Debug, Default)]
105pub struct SbusParser {
106    buffer: [u8; SBUS_PACKET_SIZE],
107    state: State,
108}
109
110pub struct PacketIterator<'a, 'b> {
111    parser: &'a mut SbusParser,
112    remaining_data: &'b [u8],
113}
114
115impl Iterator for PacketIterator<'_, '_> {
116    type Item = Result<SbusPacket, SbusParserError>;
117
118    fn next(&mut self) -> Option<Self::Item> {
119        loop {
120            if self.remaining_data.is_empty() {
121                break;
122            }
123
124            let byte = self.remaining_data[0];
125            self.remaining_data = &self.remaining_data[1..];
126
127            if let Some(result) = self.parser.push_byte(byte) {
128                return Some(result);
129            }
130        }
131        None
132    }
133}
134
135pub struct RawPacketIterator<'a, 'b> {
136    parser: &'a mut SbusParser,
137    remaining_data: &'b [u8],
138}
139
140impl Iterator for RawPacketIterator<'_, '_> {
141    type Item = Result<RawSbusPacket, SbusParserError>;
142
143    fn next(&mut self) -> Option<Self::Item> {
144        loop {
145            if self.remaining_data.is_empty() {
146                break;
147            }
148
149            let byte = self.remaining_data[0];
150            self.remaining_data = &self.remaining_data[1..];
151
152            if let Some(result) = self.parser.push_byte_raw(byte) {
153                return Some(result);
154            }
155        }
156        None
157    }
158}
159
160impl SbusParser {
161    pub fn new() -> Self {
162        Self {
163            buffer: [0; SBUS_PACKET_SIZE],
164            state: State::AwaitingHead,
165        }
166    }
167    pub fn push_byte_raw(&mut self, byte: u8) -> Option<Result<RawSbusPacket, SbusParserError>> {
168        match self.state {
169            State::AwaitingHead => {
170                if byte == SBUS_HEADER {
171                    self.buffer[0] = byte;
172                    self.state = State::Reading(1);
173                }
174            }
175            State::Reading(n) if n == SBUS_PACKET_SIZE - 1 => {
176                self.buffer[n] = byte;
177                self.state = State::AwaitingHead;
178                return Some(self.try_parse());
179            }
180            State::Reading(n) => {
181                self.buffer[n] = byte;
182                self.state = State::Reading(n + 1)
183            }
184        }
185        None
186    }
187    pub fn push_byte(&mut self, byte: u8) -> Option<Result<SbusPacket, SbusParserError>> {
188        self.push_byte_raw(byte)
189            .map(|res| res.map(|raw_packet| SbusPacket::parse(&raw_packet)))
190    }
191
192    pub fn reset(&mut self) {
193        self.state = State::AwaitingHead;
194    }
195
196    fn try_parse(&self) -> Result<RawSbusPacket, SbusParserError> {
197        if self.state != State::Reading(SBUS_PACKET_SIZE) {
198            self.validate_frame()?;
199        }
200        Ok(RawSbusPacket::new(&self.buffer))
201    }
202
203    pub fn validate_frame(&self) -> Result<(), SbusParserError> {
204        let footer = self.buffer[SBUS_PACKET_SIZE - 1];
205        let flags = self.buffer[SBUS_PACKET_SIZE - 2];
206
207        if !is_sbus_footer(footer) {
208            Err(SbusParserError::InvalidFooter(footer))
209        } else if flags & SBUS_FLAG_BYTE_MASK != 0 {
210            Err(SbusParserError::InvalidFlags(flags))
211        } else {
212            Ok(())
213        }
214    }
215    pub fn iter_packets<'a, 'b>(&'a mut self, data: &'b [u8]) -> PacketIterator<'a, 'b> {
216        PacketIterator {
217            parser: self,
218            remaining_data: data,
219        }
220    }
221    pub fn iter_packets_raw<'a, 'b>(&'a mut self, data: &'b [u8]) -> RawPacketIterator<'a, 'b> {
222        RawPacketIterator {
223            parser: self,
224            remaining_data: data,
225        }
226    }
227}
228
229#[inline(always)]
230pub fn encode_packet(buf: &mut [u8; SBUS_PACKET_SIZE], packet: &SbusPacket) {
231    let ch = &packet.channels;
232
233    // Start byte
234    buf[0] = SBUS_HEADER;
235
236    // Encode channels by setting specific bits while preserving others
237    // Ch 0: all bits in buf[1], bits 0-2 in buf[2]
238    buf[1] = ch[0] as u8;
239    buf[2] = (buf[2] & !0x07) | ((ch[0] >> 8) & 0x07) as u8;
240    // Overlay ch[1] bits 0-4 into buf[2] bits 3-7
241    buf[2] = (buf[2] & !0xF8) | ((ch[1] & 0x1F) << 3) as u8;
242
243    // Ch 1 remaining bits + start of Ch 2
244    buf[3] = ((ch[1] >> 5) & 0x3F) as u8 | ((ch[2] & 0x03) << 6) as u8;
245
246    // Ch 2 middle bits
247    buf[4] = ((ch[2] >> 2) & 0xFF) as u8;
248    // Ch 2 last bit + Ch 3 first 7 bits
249    buf[5] = ((ch[2] >> 10) & 0x01) as u8 | ((ch[3] & 0x7F) << 1) as u8;
250
251    // Ch 3 remaining bits + Ch 4 first 4 bits
252    buf[6] = ((ch[3] >> 7) & 0x0F) as u8 | ((ch[4] & 0x0F) << 4) as u8;
253
254    // Ch 4 remaining bits + Ch 5 first bit
255    buf[7] = ((ch[4] >> 4) & 0x7F) as u8 | ((ch[5] & 0x01) << 7) as u8;
256
257    // Ch 5 middle bits
258    buf[8] = ((ch[5] >> 1) & 0xFF) as u8;
259    // Ch 5 last 2 bits + Ch 6 first 6 bits
260    buf[9] = ((ch[5] >> 9) & 0x03) as u8 | ((ch[6] & 0x3F) << 2) as u8;
261
262    // Ch 6 remaining bits + Ch 7 first 3 bits
263    buf[10] = ((ch[6] >> 6) & 0x1F) as u8 | ((ch[7] & 0x07) << 5) as u8;
264
265    // Ch 7 remaining bits
266    buf[11] = ((ch[7] >> 3) & 0xFF) as u8;
267
268    // Channels 8-15 follow same pattern
269    buf[12] = ch[8] as u8;
270    buf[13] = ((ch[8] >> 8) & 0x07) as u8 | ((ch[9] & 0x1F) << 3) as u8;
271
272    buf[14] = ((ch[9] >> 5) & 0x3F) as u8 | ((ch[10] & 0x03) << 6) as u8;
273    buf[15] = ((ch[10] >> 2) & 0xFF) as u8;
274    buf[16] = ((ch[10] >> 10) & 0x01) as u8 | ((ch[11] & 0x7F) << 1) as u8;
275
276    buf[17] = ((ch[11] >> 7) & 0x0F) as u8 | ((ch[12] & 0x0F) << 4) as u8;
277    buf[18] = ((ch[12] >> 4) & 0x7F) as u8 | ((ch[13] & 0x01) << 7) as u8;
278
279    buf[19] = ((ch[13] >> 1) & 0xFF) as u8;
280    buf[20] = ((ch[13] >> 9) & 0x03) as u8 | ((ch[14] & 0x3F) << 2) as u8;
281
282    buf[21] = ((ch[14] >> 6) & 0x1F) as u8 | ((ch[15] & 0x07) << 5) as u8;
283    buf[22] = ((ch[15] >> 3) & 0xFF) as u8;
284    // clear byte first then set nesseary bits
285    buf[23] = 0x00;
286    buf[23] = buf[23]
287        | (packet.channel_17 as u8)
288        | ((packet.channel_18 as u8) << 1)
289        | ((packet.frame_lost as u8) << 2)
290        | ((packet.failsafe as u8) << 3);
291
292    buf[24] = SBUS_FOOTER;
293}
294
295#[cfg(test)]
296mod tests {
297    use super::*;
298    extern crate std;
299    const RAW_BYTES: [u8; SBUS_PACKET_SIZE] = [
300        0x0F, 0xE0, 0x03, 0x1F, 0x58, 0xC0, 0x07, 0x16, 0xB0, 0x80, 0x05, 0x2C, 0x60, 0x01, 0x0B,
301        0xF8, 0xC0, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0x00,
302    ];
303
304    #[test]
305    fn test_basic_packing_unpacking() {
306        let mut p = SbusParser::new();
307        assert!(p.state == State::AwaitingHead);
308
309        for b in &RAW_BYTES[0..RAW_BYTES.len() - 1] {
310            assert!(p.push_byte(*b).is_none());
311        }
312        let packet = p.push_byte(RAW_BYTES[24]).unwrap().unwrap();
313        let expected = SbusPacket {
314            channels: [
315                992, 992, 352, 992, 352, 352, 352, 352, 352, 352, 992, 992, 0, 0, 0, 0,
316            ],
317            channel_17: true,
318            channel_18: true,
319            failsafe: false,
320            frame_lost: false,
321        };
322        assert!(packet == expected);
323
324        // dirty buffer
325        let mut buffer: [u8; SBUS_PACKET_SIZE] = [255; SBUS_PACKET_SIZE];
326        encode_packet(&mut buffer, &packet);
327        assert!(buffer == RAW_BYTES);
328    }
329
330    #[test]
331    fn test_low_value() {
332        const EXPECTED: [u8; SBUS_PACKET_SIZE] = [
333            0x0F, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
334            0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
335        ];
336
337        let packet = SbusPacket {
338            channels: [0; 16],
339            channel_17: false,
340            channel_18: false,
341            failsafe: false,
342            frame_lost: false,
343        };
344        // dirty buffer
345        let mut buffer: [u8; SBUS_PACKET_SIZE] = [255; SBUS_PACKET_SIZE];
346        encode_packet(&mut buffer, &packet);
347        assert!(buffer == EXPECTED);
348    }
349
350    #[test]
351    fn test_high_value() {
352        const EXPECTED: [u8; SBUS_PACKET_SIZE] = [
353            0x0F, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
354            0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x0F, 0x00,
355        ];
356
357        let packet = SbusPacket {
358            channels: [2047; 16],
359            channel_17: true,
360            channel_18: true,
361            failsafe: true,
362            frame_lost: true,
363        };
364
365        let mut buffer: [u8; SBUS_PACKET_SIZE] = [0; SBUS_PACKET_SIZE];
366        encode_packet(&mut buffer, &packet);
367        assert!(buffer == EXPECTED);
368    }
369    #[test]
370    fn test_malformed_footer() {
371        let mut p = SbusParser::new();
372        const BAD_FOOTER: [u8; SBUS_PACKET_SIZE] = [
373            0x0F, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
374            0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x0F, 0xFF,
375        ];
376
377        assert!(p.state == State::AwaitingHead);
378        for b in &BAD_FOOTER[0..BAD_FOOTER.len() - 1] {
379            assert!(p.push_byte(*b).is_none());
380        }
381        let err = p.push_byte(BAD_FOOTER[24]).unwrap();
382        assert!(err == Err(SbusParserError::InvalidFooter(0xff)));
383    }
384
385    #[test]
386    fn test_malformed_flags() {
387        let mut p = SbusParser::new();
388        const BAD_FLAGS: [u8; SBUS_PACKET_SIZE] = [
389            0x0F, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
390            0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00,
391        ];
392
393        assert!(p.state == State::AwaitingHead);
394        for b in &BAD_FLAGS[0..BAD_FLAGS.len() - 1] {
395            assert!(p.push_byte(*b).is_none());
396        }
397        let err = p.push_byte(BAD_FLAGS[24]).unwrap();
398        assert!(err == Err(SbusParserError::InvalidFlags(0xff)));
399    }
400
401    #[test]
402    fn test_basic_raw_packet() {
403        let mut p = SbusParser::new();
404        for b in &RAW_BYTES[0..RAW_BYTES.len() - 1] {
405            assert!(p.push_byte(*b).is_none());
406        }
407        let raw_packet = p.push_byte_raw(RAW_BYTES[24]).unwrap().unwrap();
408        let packet = SbusPacket::parse(&raw_packet);
409
410        let expected = SbusPacket {
411            channels: [
412                992, 992, 352, 992, 352, 352, 352, 352, 352, 352, 992, 992, 0, 0, 0, 0,
413            ],
414            channel_17: true,
415            channel_18: true,
416            failsafe: false,
417            frame_lost: false,
418        };
419        assert!(packet == expected);
420    }
421
422    #[test]
423    fn test_basic_iterator() {
424        let data = [
425            0x0F, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
426            0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x0F, 0xFF, 0x0F, 0xE0, 0x03,
427            0x1F, 0x58, 0xC0, 0x07, 0x16, 0xB0, 0x80, 0x05, 0x2C, 0x60, 0x01, 0x0B, 0xF8, 0xC0,
428            0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0x00, 0x0F, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
429            0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
430            0xFF, 0xFF, 0xFF, 0x0F, 0x00, 0x0F, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
431            0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
432            0x00, 0x00, 0x0F, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
433            0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00,
434        ];
435        let mut parser = SbusParser::new();
436        let expected = SbusPacket {
437            channels: [
438                992, 992, 352, 992, 352, 352, 352, 352, 352, 352, 992, 992, 0, 0, 0, 0,
439            ],
440            channel_17: true,
441            channel_18: true,
442            failsafe: false,
443            frame_lost: false,
444        };
445
446        let results: std::vec::Vec<Result<SbusPacket, SbusParserError>> =
447            parser.iter_packets(&data).collect();
448        assert!(results.len() == 5);
449        assert!(results[0].is_err());
450        assert!(results[1] == Ok(expected));
451        assert!(results[4].is_err());
452
453        let raw_results: std::vec::Vec<Result<RawSbusPacket, SbusParserError>> =
454            parser.iter_packets_raw(&data).collect();
455        assert!(raw_results.len() == 5);
456        assert!(raw_results[0].is_err());
457
458        assert!(SbusPacket::parse(&raw_results[1].as_ref().unwrap()) == expected);
459        assert!(raw_results[4].is_err());
460    }
461}
uf_sbus/lib.rs

uf_sbus/
lib.rs
