dshot_codec/
gcr_frame.rs

1use core::ops::Deref;
2
3use super::{DshotError, DshotTelemetryFrame};
4
5/// A captured GCR frame received straight from the PIO FIFO block.
6#[derive(Debug, Copy, Clone, Default, Eq, PartialEq, PartialOrd, Ord)]
7pub struct GcrFrame(u32);
8
9impl From<GcrFrame> for u32 {
10    #[inline]
11    fn from(frame: GcrFrame) -> Self {
12        frame.0
13    }
14}
15
16impl Deref for GcrFrame {
17    type Target = u32;
18
19    #[inline]
20    fn deref(&self) -> &Self::Target {
21        &self.0
22    }
23}
24
25impl GcrFrame {
26    // Standard 5-bit GCR to 4-bit Nibble translation map
27    const INVALID_NIBBLE: u16 = 255;
28    const QUINTET_TO_NIBBLE: [u16; 32] = [
29        255, 255, 255, 255, 255, 255, 255, 255, 255, 9, 10, 11, 255, 13, 14, 15, 255, 255, 2, 3, 255, 5, 6, 7, 255, 0,
30        8, 1, 255, 4, 12, 255,
31    ];
32
33    #[inline]
34    #[must_use]
35    pub const fn from_raw(raw_pio: u32) -> Self {
36        // The PIO delivers 20 bits of decoded GCR payload
37        Self(raw_pio & 0x000F_FFFF)
38    }
39
40    #[inline]
41    #[must_use]
42    pub const fn raw_20(self) -> u32 {
43        self.0
44    }
45
46    /// Decodes the 20-bit GCR stream into a standard 16-bit `Dshot` frame.
47    /// # Errors
48    #[inline]
49    pub fn try_decode(self) -> Result<DshotTelemetryFrame, DshotError> {
50        let gcr20 = self.0;
51
52        // Extract the 5-bit quintets.
53        // Because the PIO shifts LSB-first, the chronological data order is inverted:
54        // The first bits received land in the highest positions (bits 15-19).
55        let quintet3 = (gcr20 >> 15) & 0x1F; // First received (MSB of DShot frame)
56        let quintet2 = (gcr20 >> 10) & 0x1F;
57        let quintet1 = (gcr20 >> 5) & 0x1F;
58        let quintet0 = gcr20 & 0x1F; // Last received (LSB of DShot frame)
59
60        let nibble3 = Self::QUINTET_TO_NIBBLE[quintet3 as usize];
61        let nibble2 = Self::QUINTET_TO_NIBBLE[quintet2 as usize];
62        let nibble1 = Self::QUINTET_TO_NIBBLE[quintet1 as usize];
63        let nibble0 = Self::QUINTET_TO_NIBBLE[quintet0 as usize];
64
65        // If any translation hits an invalid code pattern (255), drop the packet
66        if nibble0 == Self::INVALID_NIBBLE
67            || nibble1 == Self::INVALID_NIBBLE
68            || nibble2 == Self::INVALID_NIBBLE
69            || nibble3 == Self::INVALID_NIBBLE
70        {
71            return Err(DshotError::InvalidNrziData);
72        }
73
74        // Reconstruct the original 16-bit DShot telemetry word layout
75        let telemetry_word = nibble0 | (nibble1 << 4) | (nibble2 << 8) | (nibble3 << 12);
76
77        let frame = DshotTelemetryFrame::try_from(telemetry_word)?;
78
79        if frame.checksum_is_ok() {
80            Ok(frame)
81        } else {
82            Err(DshotError::InvalidChecksum)
83        }
84    }
85}
86
87#[cfg(test)]
88mod test_traits {
89    use super::*;
90
91    fn is_full_eq<T: Sized + Send + Sync + Unpin + Copy + Clone + Default + Eq + PartialEq>() {}
92
93    #[test]
94    fn normal_types() {
95        is_full_eq::<GcrFrame>();
96    }
97}
98
99#[cfg(test)]
100mod tests {
101    #![allow(clippy::unwrap_used)]
102    use super::*;
103
104    // Helper function to build a raw 20-bit GCR integer from four 5-bit quintets.
105    // Simulates how the PIO loads them LSB-first into the register buffer.
106    fn make_raw_pio_gcr(q3: u32, q2: u32, q1: u32, q0: u32) -> u32 {
107        (q3 << 15) | (q2 << 10) | (q1 << 5) | q0
108    }
109
110    #[test]
111    fn test_valid_zero_erpm_frame() {
112        // Payload: 12-bit value = 0x000 (Nibbles: q3=0, q2=0, q1=0) -> GCR: 0x19
113        // Checksum: !(0 ^ 0 ^ 0) & 0x0F = 0x0F.     (Nibble q0=0x0F) -> GCR: 0x0F
114
115        let gcr_payload_zero = 0x19; // Decodes to 0x0
116        let gcr_checksum_zero = 0x0F; // Decodes to 0x0F
117
118        let raw_pio = make_raw_pio_gcr(gcr_payload_zero, gcr_payload_zero, gcr_payload_zero, gcr_checksum_zero);
119        let frame = GcrFrame::from_raw(raw_pio);
120
121        let decode_result = frame.try_decode();
122        assert!(decode_result.is_ok(), "Expected valid zero eRPM frame decoding to succeed");
123    }
124
125    #[test]
126    fn test_valid_active_erpm_frame() {
127        // Simulating a moving motor with a 12-bit period telemetry value of 0x55A
128        //   nibble3 = 0x5 -> GCR: 0x15
129        //   nibble2 = 0x5 -> GCR: 0x15
130        //   nibble1 = 0xA -> GCR: 0x0A
131        // Checksum calculation: !(0x5 ^ 0x5 ^ 0xA) & 0x0F = !0xA & 0x0F = 0x5
132        //   nibble0 = 0x5 -> GCR: 0x15
133
134        let gcr_5 = 0x15;
135        let gcr_a = 0x0A;
136
137        let raw_pio = make_raw_pio_gcr(gcr_5, gcr_5, gcr_a, gcr_5);
138        let frame = GcrFrame::from_raw(raw_pio);
139
140        let decode_result = frame.try_decode();
141
142        assert!(decode_result.is_ok(), "Expected valid active eRPM frame decoding to succeed");
143
144        // If your test harness handles actual structural return verification,
145        // you can assert the final parsed u16 word evaluates to 0x55A5:
146        let decoded_frame = decode_result.unwrap();
147        assert_eq!(decoded_frame.raw_16(), 0x55A5, "Decoded DShot telemetry word layout mismatch");
148    }
149    #[test]
150    fn test_valid_edt_temperature_frame() {
151        // Let's simulate an EDT Temperature frame.
152        // Say the 12-bit payload is 0x24E:
153        //   nibble3 = 0x2 (EDT Temperature category) -> GCR: 0x12
154        //   nibble2 = 0x4 (Data high)                -> GCR: 0x1D
155        //   nibble1 = 0xE (Data low)                 -> GCR: 0x0E
156        // Checksum: !(0x2 ^ 0x4 ^ 0xE) = !0x8 = 0x7 -> GCR: 0x17
157
158        let raw_pio = make_raw_pio_gcr(0x12, 0x1D, 0x0E, 0x17);
159        let frame = GcrFrame::from_raw(raw_pio);
160
161        let decode_result = frame.try_decode();
162        assert!(decode_result.is_ok(), "Expected valid EDT frame to decode successfully");
163    }
164
165    #[test]
166    fn test_invalid_gcr_sequence() {
167        // The 5-bit value 0x00 is completely forbidden in GCR (violates run-length constraints)
168        // and maps to 255 (0xFF) in our lookup array.
169        let raw_pio = make_raw_pio_gcr(0x00, 0x19, 0x19, 0x19);
170        let frame = GcrFrame::from_raw(raw_pio);
171
172        let decode_result = frame.try_decode();
173        assert!(
174            matches!(decode_result, Err(DshotError::InvalidNrziData)),
175            "Expected failure due to invalid wire patterns"
176        );
177    }
178
179    #[test]
180    fn test_corrupted_checksum() {
181        // Payload: 12-bit value = 0x000 (GCR quintets: 0x19, 0x19, 0x19)
182        // Correct checksum should decode to 0x0F (GCR: 0x0F)
183        // Let's corrupt it by sending an incorrect checksum token (GCR: 0x15)
184
185        let raw_pio = make_raw_pio_gcr(0x19, 0x19, 0x19, 0x15);
186        let frame = GcrFrame::from_raw(raw_pio);
187
188        let decode_result = frame.try_decode();
189        assert!(
190            matches!(decode_result, Err(DshotError::InvalidChecksum)),
191            "Expected validation rejection due to checksum mismatch"
192        );
193    }
194}
dshot_codec/gcr_frame.rs

dshot_codec/
gcr_frame.rs
