Decode rc channel packet

src/lib.rs

@@ -1,8 +1,8 @@
-#![cfg_attr(not(test), no_std)]
+// #![cfg_attr(not(test), no_std)]
 
 use bitfield::bitfield;
 use crc::{Crc, CRC_8_DVB_S2};
-use heapless::Deque;
+use heapless::{Deque, Vec};
 use num_enum::{IntoPrimitive, TryFromPrimitive};
 
 #[derive(Debug, PartialEq, Eq, Clone, Copy, IntoPrimitive, TryFromPrimitive)]
@@ -37,6 +37,48 @@ pub enum FrameType {
     ArdupilotResp = 0x80,
 }
 
+#[derive(Debug, PartialEq, Eq, Clone, Copy)]
+pub enum CrsfPacket {
+    RcChannels([u16; 16]),
+}
+
+impl CrsfPacket {
+    fn from_type_and_data(
+        frame_type: FrameType,
+        data: impl Iterator<Item = u8> + ExactSizeIterator,
+    ) -> Option<Self> {
+        match frame_type {
+            FrameType::RcChannelsPacked => {
+                if data.len() != 22 {
+                    return None;
+                }
+                let mut buf = Vec::<u8, 22>::new();
+                buf.extend(data);
+                let channels = RcChannelsPacked(&mut buf[..]);
+                Some(Self::RcChannels([
+                    channels.ch1(),
+                    channels.ch2(),
+                    channels.ch3(),
+                    channels.ch4(),
+                    channels.ch5(),
+                    channels.ch6(),
+                    channels.ch7(),
+                    channels.ch8(),
+                    channels.ch9(),
+                    channels.ch10(),
+                    channels.ch11(),
+                    channels.ch12(),
+                    channels.ch13(),
+                    channels.ch14(),
+                    channels.ch15(),
+                    channels.ch16(),
+                ]))
+            }
+            _ => None,
+        }
+    }
+}
+
 #[derive(Debug, PartialEq, Eq, Clone, Copy, IntoPrimitive, TryFromPrimitive)]
 #[repr(u8)]
 pub enum Address {
@@ -63,6 +105,15 @@ pub struct Header {
     frame_type: FrameType,
 }
 
+impl Header {
+    const fn valid(self) -> bool {
+        match self.frame_type {
+            FrameType::RcChannelsPacked => self.size == 24,
+            _ => true,
+        }
+    }
+}
+
 bitfield! {
     struct RcChannelsPacked([u8]);
     impl Debug;
@@ -118,7 +169,7 @@ impl CrsfParser {
         let _ = self.buffer.push_front(byte);
     }
 
-    pub fn next_packet(&mut self) -> Option<Header> {
+    pub fn next_packet(&mut self) -> Option<(Address, CrsfPacket)> {
         const CRC8_ALG: Crc<u8> = Crc::<u8>::new(&CRC_8_DVB_S2);
 
         while Address::try_from(*self.buffer.back()?).is_err() {
@@ -141,6 +192,10 @@ impl CrsfParser {
             frame_type,
         };
         // TODO: check the frame size against the frame type.
+        if !header.valid() {
+            self.buffer.pop_back();
+            return None;
+        }
 
         // The size field contains the number of bytes coming after it.
         let packet_length = header.size + 2;
@@ -164,19 +219,20 @@ impl CrsfParser {
             return None;
         }
 
-        // TODO: decode payload using type field
+        // decode payload using type field
+        let packet = CrsfPacket::from_type_and_data(header.frame_type, payload_iter);
 
         // sucessfully decoded a packet
         for _ in 0..packet_length {
             self.buffer.pop_back()?;
         }
-        Some(header)
+        packet.map(|packet| (header.address, packet))
     }
 }
 
 #[cfg(test)]
 mod tests {
-    use crate::{Address, CrsfParser, FrameType, Header};
+    use crate::{Address, CrsfPacket, CrsfParser};
 
     #[test]
     fn rc_channels() {
@@ -193,11 +249,12 @@ mod tests {
 
         assert_eq!(
             parser.next_packet(),
-            Some(Header {
-                address: Address::FlightController,
-                size: 24,
-                frame_type: FrameType::RcChannelsPacked
-            })
+            Some((
+                Address::FlightController,
+                CrsfPacket::RcChannels([
+                    992, 992, 716, 992, 191, 988, 986, 172, 992, 0, 0, 0, 0, 0, 1630, 1758
+                ])
+            ))
         );
         assert_eq!(parser.next_packet(), None);
     }