cu_bdshot/
board.rs

1use crate::decode::{decode_telemetry_packet, extract_telemetry_payload_with_crc_test, fold_gcr};
2use crate::esc_channel::EscChannel;
3use crate::messages::{DShotTelemetry, EscCommand};
4use cortex_m::asm;
5use cu29::prelude::CuError;
6use cu29::CuResult;
7use hal::dma::Word;
8use hal::pac;
9use hal::pio::{PIOBuilder, StateMachineIndex, ValidStateMachine};
10use pio_proc::pio_file;
11use rp235x_hal as hal;
12use spin::Mutex;
13
14use crate::bridge::BdshotBoardProvider;
15
16pub trait BdshotBoard {
17    const CHANNEL_COUNT: usize;
18
19    fn exchange(&mut self, channel: usize, frame: u32) -> Option<DShotTelemetry>;
20
21    fn delay(&mut self, micros: u32);
22}
23
24pub struct Rp2350Board {
25    cycles_per_micro: u32,
26    ch0: EscChannel<pac::PIO0, hal::pio::SM0, Word, Word>,
27    ch1: EscChannel<pac::PIO0, hal::pio::SM1, Word, Word>,
28    ch2: EscChannel<pac::PIO0, hal::pio::SM2, Word, Word>,
29    ch3: EscChannel<pac::PIO0, hal::pio::SM3, Word, Word>,
30}
31
32#[derive(Clone, Copy)]
33pub struct Rp2350BoardConfig {
34    pub pins: [u8; 4],
35    pub target_pio_clock_hz: u32,
36}
37
38impl Default for Rp2350BoardConfig {
39    fn default() -> Self {
40        Self {
41            pins: [6, 7, 8, 9],
42            target_pio_clock_hz: 15_300_000,
43        }
44    }
45}
46
47pub struct Rp2350BoardResources {
48    pub pio: hal::pio::PIO<pac::PIO0>,
49    pub sm0: hal::pio::UninitStateMachine<(pac::PIO0, hal::pio::SM0)>,
50    pub sm1: hal::pio::UninitStateMachine<(pac::PIO0, hal::pio::SM1)>,
51    pub sm2: hal::pio::UninitStateMachine<(pac::PIO0, hal::pio::SM2)>,
52    pub sm3: hal::pio::UninitStateMachine<(pac::PIO0, hal::pio::SM3)>,
53}
54
55impl Rp2350BoardResources {
56    pub fn new(
57        pio: hal::pio::PIO<pac::PIO0>,
58        sm0: hal::pio::UninitStateMachine<(pac::PIO0, hal::pio::SM0)>,
59        sm1: hal::pio::UninitStateMachine<(pac::PIO0, hal::pio::SM1)>,
60        sm2: hal::pio::UninitStateMachine<(pac::PIO0, hal::pio::SM2)>,
61        sm3: hal::pio::UninitStateMachine<(pac::PIO0, hal::pio::SM3)>,
62    ) -> Self {
63        Self {
64            pio,
65            sm0,
66            sm1,
67            sm2,
68            sm3,
69        }
70    }
71}
72
73impl Rp2350Board {
74    pub fn new(
75        resources: Rp2350BoardResources,
76        system_clock_hz: u32,
77        config: Rp2350BoardConfig,
78    ) -> CuResult<Self> {
79        let Rp2350BoardResources {
80            mut pio,
81            sm0,
82            sm1,
83            sm2,
84            sm3,
85        } = resources;
86        let pins = config.pins;
87        let (div_int, div_frac) = pio_clkdiv_8p8(system_clock_hz, config.target_pio_clock_hz);
88        let program = pio_file!("src/dshot.pio", select_program("bdshot_300"));
89        let installed = pio
90            .install(&program.program)
91            .map_err(|_| CuError::from("Failed to install BDShot PIO program"))?;
92
93        let ch0 = crate::build_ch!(installed, sm0, pins[0], div_int, div_frac);
94        let ch1 = crate::build_ch!(installed, sm1, pins[1], div_int, div_frac);
95        let ch2 = crate::build_ch!(installed, sm2, pins[2], div_int, div_frac);
96        let ch3 = crate::build_ch!(installed, sm3, pins[3], div_int, div_frac);
97
98        Ok(Self {
99            cycles_per_micro: system_clock_hz / 1_000_000,
100            ch0,
101            ch1,
102            ch2,
103            ch3,
104        })
105    }
106}
107
108impl BdshotBoard for Rp2350Board {
109    const CHANNEL_COUNT: usize = 4;
110
111    fn exchange(&mut self, channel: usize, frame: u32) -> Option<DShotTelemetry> {
112        let cycles = self.cycles_per_micro;
113        match channel {
114            0 => Self::exchange_impl(cycles, &mut self.ch0, frame),
115            1 => Self::exchange_impl(cycles, &mut self.ch1, frame),
116            2 => Self::exchange_impl(cycles, &mut self.ch2, frame),
117            3 => Self::exchange_impl(cycles, &mut self.ch3, frame),
118            _ => None,
119        }
120    }
121
122    fn delay(&mut self, micros: u32) {
123        Self::delay_ticks(self.cycles_per_micro, micros as u64);
124    }
125}
126
127impl Rp2350Board {
128    /// Basic DSHOT transaction, sending a frame and attempting to read a telemetry response.
129    fn exchange_impl<SM>(
130        cycles_per_micro: u32,
131        ch: &mut EscChannel<pac::PIO0, SM, Word, Word>,
132        frame: u32,
133    ) -> Option<DShotTelemetry>
134    where
135        SM: StateMachineIndex,
136        (pac::PIO0, SM): ValidStateMachine,
137    {
138        if ch.is_full() {
139            return None;
140        }
141        ch.write(frame);
142        // the frame needs to go through several layers of decoding before becoming useful
143        if let Some(resp) = ch.read() {
144            // first the 21 bit GCR needs to be folded down to 20 bits
145            let raw20 = fold_gcr(resp);
146            // then decode the 20 bits to 16 bits according to the GCR encoding
147            let data = crate::decode::gcr_to_16bit(raw20)?;
148            // We finally extract the actual payload and check for any corruption with the CRC
149            let payload = extract_telemetry_payload_with_crc_test(data)?;
150            return Some(decode_telemetry_packet(payload));
151        }
152        Self::delay_ticks(cycles_per_micro, 150);
153        ch.restart();
154        None
155    }
156
157    fn delay_ticks(cycles_per_micro: u32, mut micros: u64) {
158        let cycles = cycles_per_micro.max(1) as u64;
159        while micros > 0 {
160            let chunk = micros.min(1000);
161            let total = cycles * chunk;
162            asm::delay(total as u32);
163            micros -= chunk;
164        }
165    }
166}
167
168/// Math to compute the PIO clock divider from the system clock and target PIO clock
169fn pio_clkdiv_8p8(sys_hz: u32, target_hz: u32) -> (u16, u8) {
170    let div_fixed = ((sys_hz as f32 / target_hz as f32) * 256.0 + 0.5) as u32;
171    ((div_fixed >> 8) as u16, (div_fixed & 0xFF) as u8)
172}
173
174/// Construct an encoded DSHOT frame from payload (MCU -> ESC)
175fn bdshot_frame(payload: u16) -> u32 {
176    let d = ((payload & 0x07FF) << 1) | 1;
177    let inv = (!d) & 0x0FFF;
178    let crc = !(!((inv ^ (inv >> 4) ^ (inv >> 8)) & 0x0F)) & 0x0F;
179    !((d << 4) | crc) as u32
180}
181
182/// Encodes a throttle command and telemetry request into a DSHOT frame
183pub fn encode_frame(command: EscCommand) -> u32 {
184    let mut bits = (command.throttle & 0x07FF) << 1;
185    if command.request_telemetry {
186        bits |= 0x01;
187    }
188    bdshot_frame(bits)
189}
190
191/// Global to be able to send the actual HW interface to the bridge
192static RP_BOARD_SLOT: Mutex<Option<Rp2350Board>> = Mutex::new(None);
193
194/// Global to be able to send the actual HW interface to the bridge
195pub fn register_rp2350_board(board: Rp2350Board) -> CuResult<()> {
196    let mut slot = RP_BOARD_SLOT.lock();
197    if slot.is_some() {
198        return Err(CuError::from("RP2350 BDShot board already registered"));
199    }
200    *slot = Some(board);
201    Ok(())
202}
203
204pub struct Rp2350BoardProvider;
205
206impl BdshotBoardProvider for Rp2350BoardProvider {
207    type Board = Rp2350Board;
208
209    fn create_board() -> CuResult<Self::Board> {
210        RP_BOARD_SLOT
211            .lock()
212            .take()
213            .ok_or_else(|| CuError::from("No RP2350 BDShot board registered"))
214    }
215}
cu_bdshot/board.rs

cu_bdshot/
board.rs
