dshot_codec/
esc_dshot.rs

1use super::DshotSpeed;
2
3#[derive(Copy, Clone, Debug, PartialEq)]
4pub struct EscDshot {
5    protocol: DshotSpeed,
6    cpu_frequency: u32,
7    data_high_pulse_width: u16,
8    data_low_pulse_width: u16,
9    wrap_cycle_count: u16,
10}
11
12impl Default for EscDshot {
13    fn default() -> Self {
14        Self::new(DshotSpeed::Dshot150)
15    }
16}
17
18impl EscDshot {
19    pub const DEFAULT_CPU_FREQUENCY: u32 = 150_000_000;
20    const DSHOT_BIT_COUNT: usize = 16;
21    const DMA_BUFFER_SIZE: usize = Self::DSHOT_BIT_COUNT + 1;
22
23    const DSHOT150_T0H: u32 = 2500;
24    const DSHOT150_T1H: u32 = 5000;
25    const DSHOT150_T: u32 = 6680;
26
27    #[must_use]
28    pub const fn new(protocol: DshotSpeed) -> Self {
29        let mut this = Self {
30            protocol,
31            cpu_frequency: Self::DEFAULT_CPU_FREQUENCY,
32
33            data_high_pulse_width: 0,
34            data_low_pulse_width: 0,
35            wrap_cycle_count: 0,
36        };
37        this.set_protocol(protocol);
38        this
39    }
40
41    /// Set the `cpu_frequency` of a newly constructed `EscDshot`.
42    #[must_use]
43    pub const fn with_cpu_frequency(mut self, cpu_frequency: u32) -> Self {
44        self.set_cpu_frequency(cpu_frequency);
45        self
46    }
47}
48
49#[allow(unused)]
50impl EscDshot {
51    #[must_use]
52    pub const fn nano_seconds_to_cycles(self, nano_seconds: u32) -> u16 {
53        // note: the k values cancel out, but give greater precision in the calculation
54        const K: u64 = 128;
55        let d = K * 1_000_000_000 / (self.cpu_frequency as u64);
56        #[allow(clippy::cast_possible_truncation)]
57        {
58            ((nano_seconds as u64) * K / d) as u16
59        }
60    }
61
62    pub const fn set_cpu_frequency(&mut self, cpu_frequency: u32) {
63        self.cpu_frequency = cpu_frequency;
64        self.set_protocol(self.protocol);
65    }
66
67    pub const fn set_protocol(&mut self, protocol: DshotSpeed) {
68        self.protocol = protocol;
69
70        // data_low_pulse_width and data_high_pulse_width are in processor cycles
71        // for RPI_PICO: default CPU frequency is 150MHz, that is 0.15GHz
72        self.data_low_pulse_width = self.nano_seconds_to_cycles(Self::DSHOT150_T0H); // =  375 = 2500 * 0.15GHz
73        self.data_high_pulse_width = self.nano_seconds_to_cycles(Self::DSHOT150_T1H); // =  750 = 5000 * 0.15GHz
74        self.wrap_cycle_count = self.nano_seconds_to_cycles(Self::DSHOT150_T); // = 1002 = 6680 * 0.15GHz
75
76        match protocol {
77            DshotSpeed::Dshot150 => {
78                _ = protocol;
79            }
80            DshotSpeed::Dshot300 => {
81                self.data_low_pulse_width /= 2;
82                self.data_high_pulse_width /= 2;
83                self.wrap_cycle_count /= 2;
84            }
85            DshotSpeed::Dshot600 => {
86                self.data_low_pulse_width /= 4;
87                self.data_high_pulse_width /= 4;
88                self.wrap_cycle_count /= 4;
89            }
90            DshotSpeed::Dshot1200 => {
91                self.data_low_pulse_width /= 8;
92                self.data_high_pulse_width /= 8;
93                self.wrap_cycle_count /= 8;
94            }
95        }
96    }
97
98    /// Returns an array of duty cycles for use in PWM DMA.
99    ///
100    /// The array an extra element set to zero to ensure that PWM output gets pulled low at the end of the sequence.
101    #[must_use]
102    pub fn duty_cycles_u16(&self, frame: u16) -> [u16; Self::DMA_BUFFER_SIZE] {
103        let mut ret = [0u16; Self::DMA_BUFFER_SIZE];
104
105        let mut mask_bit = 1 << (Self::DSHOT_BIT_COUNT - 1);
106        for item in &mut ret {
107            *item = if frame & mask_bit == 0 { self.data_high_pulse_width } else { self.data_low_pulse_width };
108            mask_bit >>= 1;
109        }
110
111        // Set last value to zero, (DMA_BUFFER_SIZE = DSHOT_BIT_COUNT + 1).
112        ret[Self::DMA_BUFFER_SIZE - 1] = 0;
113        ret
114    }
115
116    /// Returns an array of duty cycles for use in PWM DMA.
117    ///
118    /// The array an extra element set to zero to ensure that PWM output gets pulled low at the end of the sequence.
119    #[must_use]
120    pub fn duty_cycles_u32(&self, frame: u16, use_high_order_bits: bool) -> [u32; Self::DMA_BUFFER_SIZE] {
121        let mut ret = [0u32; Self::DMA_BUFFER_SIZE];
122
123        let mut mask_bit = 1 << (Self::DSHOT_BIT_COUNT - 1);
124        if use_high_order_bits {
125            for item in &mut ret {
126                let byte = if frame & mask_bit == 0 {
127                    u32::from(self.data_high_pulse_width)
128                } else {
129                    u32::from(self.data_low_pulse_width)
130                };
131                *item = byte << 16;
132                mask_bit >>= 1;
133            }
134        } else {
135            for item in &mut ret {
136                *item = if frame & mask_bit == 0 {
137                    u32::from(self.data_high_pulse_width)
138                } else {
139                    u32::from(self.data_low_pulse_width)
140                };
141                mask_bit >>= 1;
142            }
143        }
144
145        // Set last value to zero, (DMA_BUFFER_SIZE = DSHOT_BIT_COUNT + 1).
146        ret[Self::DMA_BUFFER_SIZE - 1] = 0;
147        ret
148    }
149}
150
151#[cfg(test)]
152mod test_traits {
153    use super::*;
154
155    fn is_full<T: Sized + Send + Sync + Unpin + Copy + Clone + Default + PartialEq>() {}
156
157    #[test]
158    fn normal_types() {
159        is_full::<EscDshot>();
160    }
161}
162
163#[cfg(test)]
164mod test {
165
166    use super::*;
167
168    #[test]
169    fn nano_seconds_to_cycles() {
170        let esc = EscDshot::new(DshotSpeed::Dshot150);
171        assert_eq!(126, esc.nano_seconds_to_cycles(840));
172        assert_eq!(187, esc.nano_seconds_to_cycles(1250));
173        assert_eq!(313, esc.nano_seconds_to_cycles(2090));
174        assert_eq!(375, esc.nano_seconds_to_cycles(2500));
175        assert_eq!(750, esc.nano_seconds_to_cycles(5000));
176        assert_eq!(1125, esc.nano_seconds_to_cycles(7500));
177
178        assert_eq!(350, esc.nano_seconds_to_cycles(2333));
179        assert_eq!(700, esc.nano_seconds_to_cycles(4666));
180        assert_eq!(1000, esc.nano_seconds_to_cycles(6666));
181    }
182    #[test]
183    fn pulse_widths() {
184        let esc150 = EscDshot::new(DshotSpeed::Dshot150);
185        assert_eq!(750, esc150.data_high_pulse_width);
186        assert_eq!(375, esc150.data_low_pulse_width);
187
188        let esc300 = EscDshot::new(DshotSpeed::Dshot300);
189        assert_eq!(375, esc300.data_high_pulse_width);
190        assert_eq!(187, esc300.data_low_pulse_width);
191
192        let esc600 = EscDshot::new(DshotSpeed::Dshot600);
193        assert_eq!(187, esc600.data_high_pulse_width);
194        assert_eq!(93, esc600.data_low_pulse_width);
195    }
196}
dshot_codec/esc_dshot.rs

dshot_codec/
esc_dshot.rs
