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

#![no_std]
#![feature(test)]

#[derive(Default, PartialEq, Debug)]
pub struct SbusData {
    pub channels: [u16; 16],
    pub channel17: bool,
    pub channel18: bool,
    pub frame_lost: bool,
    pub failsafe: bool,
}

#[repr(C)]
pub struct SbusPacket {
    _padding: u8,
    header: u8,
    channel_words: [u16; 11],
    digital_and_flags: u8,
    footer: u8,
}

pub const SBUS_PACKET_BEGIN: u8 = 0xF;
pub const SBUS_PACKET_SIZE: usize = core::mem::size_of::<SbusPacket>() - 1;

pub fn is_sbus_packet_end(byte: u8) -> bool {
    match byte {
        0x0 => true,  // S.BUS 1
        0x4 => true,  // S.BUS 2 receiver voltage
        0x14 => true, // S.BUS 2 GPS/baro
        0x24 => true, // Unknown SBUS2 data
        0x34 => true, // Unknown SBUS2 data
        _ => false,
    }
}

impl SbusPacket {
    pub fn from_bytes<'a>(bytes: &'a [u8; SBUS_PACKET_SIZE + 1]) -> Option<&Self> {
        Some(unsafe { core::mem::transmute(bytes) })
    }

    pub fn try_parse(&self) -> Option<SbusData> {
        if self.header != SBUS_PACKET_BEGIN || !is_sbus_packet_end(self.footer) {
            return None;
        }
        Some(self.parse())
    }

    pub fn parse(&self) -> SbusData {
        const SHIFT: [u8; 16] = [0, 5, 10, 15, 4, 9, 14, 3, 8, 13, 2, 7, 12, 1, 6, 11];
        const INDEX: [u8; 16] = [0, 1, 2, 3, 3, 4, 5, 5, 6, 7, 7, 8, 9, 9, 10, 10];

        let mut data = SbusData::default();
        let mut bits: u32 = 0;
        for i in 0..16 {
            let word = u16::from_le(self.channel_words[INDEX[i] as usize]) as u32;
            bits |= word << (SHIFT[i] as usize);
            data.channels[i] = bits as u16 & ((1 << 11) - 1);
            bits >>= 11;
        }

        data.channel17 = (self.digital_and_flags & (1 << 7)) > 0;
        data.channel18 = (self.digital_and_flags & (1 << 6)) > 0;
        data.frame_lost = (self.digital_and_flags & (1 << 5)) > 0;
        data.failsafe = (self.digital_and_flags & (1 << 4)) > 0;
        data
    }
}

#[cfg(test)]
mod tests {

    extern crate test;
    use test::Bencher;

    #[test]
    fn test_sbus() {
        use super::{SbusData, SbusPacket, SBUS_PACKET_SIZE};

        assert_eq!(SBUS_PACKET_SIZE, 25);
        let bytes: [u8; SBUS_PACKET_SIZE + 1] = [
            0x00, 0x0F, 0xE0, 0x03, 0x1F, 0x58, 0xC0, 0x07, 0x16, 0xB0, 0x80, 0x05, 0x2C, 0x60,
            0x01, 0x0B, 0xF8, 0xC0, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0x00,
        ];
        let sbus_packet: &SbusPacket = SbusPacket::from_bytes(&bytes).unwrap();
        let result = sbus_packet.try_parse();
        assert_eq!(
            result,
            Some(SbusData {
                channels: [992, 992, 352, 992, 352, 352, 352, 352, 352, 352, 992, 992, 0, 0, 0, 0],
                channel17: false,
                channel18: false,
                frame_lost: false,
                failsafe: false,
            })
        )
    }

    #[bench]
    fn bench_sbus(b: &mut Bencher) {
        use super::{SbusPacket, SBUS_PACKET_SIZE};

        let bytes: [u8; SBUS_PACKET_SIZE + 1] = [
            0x00, 0x0F, 0xE0, 0x03, 0x1F, 0x58, 0xC0, 0x07, 0x16, 0xB0, 0x80, 0x05, 0x2C, 0x60,
            0x01, 0x0B, 0xF8, 0xC0, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0x00,
        ];
        let sbus_packet: &SbusPacket = SbusPacket::from_bytes(&bytes).unwrap();
        b.iter(|| sbus_packet.parse());
    }
}