From 6c38cd59eeca09fe6ad8c02ae86e6eb4be13f718 Mon Sep 17 00:00:00 2001 From: Serge Bazanski Date: Sat, 24 Jan 2026 02:24:17 +0100 Subject: [PATCH] rpm get The fucking receive data format/encoding/modulation is just bonkers. But I think I got it. --- spinnyboy_rust/src/bin/dc_driver/dshot.rs | 181 ++++++++++++++++++---- spinnyboy_rust/src/bin/main.rs | 19 ++- 2 files changed, 164 insertions(+), 36 deletions(-) diff --git a/spinnyboy_rust/src/bin/dc_driver/dshot.rs b/spinnyboy_rust/src/bin/dc_driver/dshot.rs index ca6dfc8..b6a2f42 100644 --- a/spinnyboy_rust/src/bin/dc_driver/dshot.rs +++ b/spinnyboy_rust/src/bin/dc_driver/dshot.rs @@ -1,13 +1,18 @@ // use embedded_hal_async::delay::DelayNs; use esp_hal::{ - delay::Delay, + delay::{self, Delay}, gpio::Level, rmt::{Channel, PulseCode, SingleShotTxTransaction}, }; -use core::cell::{Ref, RefCell}; +use core::{cell::{Ref, RefCell}, ops::Index}; use num_traits::float::FloatCore; use rtt_target::{rprint, rprintln}; +fn calculate_crc(frame: u16) -> u16 { + (0xffff ^ (frame ^ (frame >> 4) ^ (frame >> 8))) & 0xF +} + + #[allow(dead_code)] #[allow(non_camel_case_types)] #[derive(Debug)] @@ -111,16 +116,91 @@ impl DShotSpeed { } } +fn decode_bidi_telemetry(pulses: &[PulseCode]) -> Option { + // Convert to simple list of edges. + let mut edges = [(false, 0u32); 64]; + let mut nedges = 0usize; + let mut time = 0u32; + let mut plength = 0u32; + for (i, p) in pulses.iter().enumerate() { + if p.length1() == 0 { + break; + } else { + nedges += 1; + edges[i*2] = (p.level1() == Level::High, time + plength); + time += plength; + plength = p.length1() as u32; + } + if p.length2() == 0 { + break; + } else { + nedges += 1; + edges[i*2+1] = (p.level2() == Level::High, time + plength); + time += plength; + plength = p.length2() as u32; + } + } + edges[nedges] = (!edges[nedges-1].0, time + plength); + nedges += 1; + + // Process edges. + // TODO: strip until we find the first negative edge. + assert_eq!(edges[0].0, false); + + // Approximate. Should be calculated instead, in practice these pulses are + // around 1.34us. + let period = 104u32; + + // Grad middle of each period, find latest applying period. Shitty + // algorithm. + let mut res = [false; 22]; + for i in 0..res.len() { + let pos = (i as u32)*period + period/2; + let mut level = edges[0].0; + for edge in edges[..nedges].iter() { + if edge.1 > pos { + break; + } + level = edge.0; + } + res[i] = level; + } + + let mut gcr_diff = 0u32; + for i in 0..21 { + gcr_diff |= (res[20-i] as u32) << i; + } + + let gcr = (gcr_diff>>1) ^ gcr_diff; + + // Shitty LUT decode for GCR; + let lut: [u8; 16] = [ + 0x19, 0x1b, 0x12, 0x13, 0x1d, 0x15, 0x16, 0x17, 0x1a, 0x09, 0x0a, 0x0b, + 0x1e, 0x0d, 0x0e, 0x0f, + ]; + + let mut res = 0u16; + for i in 0..4 { + let fibble = ((gcr >> (i*5)) & 0b11111) as u8; + if let Some(nibble) = lut.iter().position(|&v| v == fibble) { + res |= (nibble as u16) << (i*4); + } else { + return None; + } + } + Some(res) +} + #[allow(dead_code)] pub struct DShot<'a> { - rx_channel: &'a mut Channel<'static, esp_hal::Blocking, esp_hal::rmt::Rx>, + rx_channel: RefCell<&'a mut Channel<'static, esp_hal::Blocking, esp_hal::rmt::Rx>>, speed: DShotSpeed, bit_ticks: BitTicks, pulses: RefCell<[u32; 17]>, requested_throttle: RefCell, current_tx: RefCell>>, - + current_tx_done_at: RefCell>, } impl<'a> DShot<'a> { @@ -138,25 +218,22 @@ impl<'a> DShot<'a> { let txc = tx_channel.reborrow().transmit(&[0u32]).unwrap(); Self { - rx_channel: rx_channel, + rx_channel: RefCell::new(rx_channel), speed, bit_ticks, pulses: RefCell::new([0u32; 17]), requested_throttle: RefCell::new(0), current_tx: RefCell::new(Some(txc)), + current_tx_done_at: RefCell::new(None), } } - pub fn calculate_crc(frame: u16) -> u16 { - (frame ^ (frame >> 4) ^ (frame >> 8)) & 0xF - } - pub fn create_frame(value: u16, telemetry: bool) -> u16 { // Mask to 11 bits (0-2047 range) and set telemetry bit let frame = ((value & 0x07FF) << 1) | telemetry as u16; - let crc = Self::calculate_crc(frame); + let crc = calculate_crc(frame); (frame << 4) | crc } @@ -170,17 +247,17 @@ impl<'a> DShot<'a> { pulses[i] = if bit == 1 { PulseCode::new( - Level::High, - self.bit_ticks.t1_h, Level::Low, + self.bit_ticks.t1_h, + Level::High, self.bit_ticks.t1_l, ) .into() } else { PulseCode::new( - Level::High, - self.bit_ticks.t0_h, Level::Low, + self.bit_ticks.t0_h, + Level::High, self.bit_ticks.t0_l, ) .into() @@ -195,22 +272,68 @@ impl<'a> DShot<'a> { let mut rc = self.current_tx.borrow_mut(); if let Some(current) = rc.as_mut() { if current.poll() { - // Schedule new transaction - let tx_chann = rc.take().unwrap().wait().unwrap(); - let mut pulses = self.pulses.borrow_mut(); - let throttle = self.requested_throttle.borrow(); - *pulses = self.create_pulses(*throttle, false); - let pulses = pulses.as_ref(); - // SAFETY: eat shit, rust - let pulses = unsafe { - core::mem::transmute(pulses) - }; - let m = tx_chann - .transmit(pulses) - .unwrap(); + // We're done - but we might still be waiting for a response over bidi. + let mut ended = self.current_tx_done_at.borrow_mut(); + if ended.is_none() { + let start = esp_hal::time::Instant::now(); + *ended = Some(start); + + // Try to receive a response for up to 100us. + let mut rx = self.rx_channel.borrow_mut(); + let rx = rx.reborrow(); + let mut buf = [PulseCode::default(); 32]; + { + let mut rxt = rx.receive(&mut buf).unwrap(); + while start.elapsed().as_micros() < 100 { + // Got something within this time. + if rxt.poll() { + // Receive it fully. + let (len, _) = rxt.wait().unwrap(); - //self.current_tx = Some(m); - *rc = Some(m); + // Parse out telemetry data. + if len != 0 { + // Decode to a 16 bit value. + if let Some(frame) = decode_bidi_telemetry(&buf[..len]) { + // Check CRC. + let data = frame >> 4; + let crc = calculate_crc(data); + if data != 0xfff && crc == (frame&0b1111) { + // Fucking got it. + let exp = data >> 9; + let mantissa = data & 0b111111111; + let rpm = mantissa << exp; + rprintln!("RPM: {}", rpm); + } + } + } + break; + } + } + } + } + + let ended_at = *ended.as_ref().unwrap(); + + if ended_at.elapsed().as_millis() > 1 { + // Schedule new transaction + let tx_chann = rc.take().unwrap().wait().unwrap(); + let mut pulses = self.pulses.borrow_mut(); + let throttle = self.requested_throttle.borrow(); + *pulses = self.create_pulses(*throttle, false); + let pulses = pulses.as_ref(); + // SAFETY: eat shit, rust + let pulses = unsafe { + core::mem::transmute(pulses) + }; + let m = tx_chann + .transmit(pulses) + .unwrap(); + + *rc = Some(m); + *ended = None; + } else{ + // Wait for bidi dshot data. + } } } else { // Nothing to do, twiddle our thumbs. diff --git a/spinnyboy_rust/src/bin/main.rs b/spinnyboy_rust/src/bin/main.rs index c6b4a9e..6c043f1 100644 --- a/spinnyboy_rust/src/bin/main.rs +++ b/spinnyboy_rust/src/bin/main.rs @@ -9,10 +9,10 @@ use alloc::boxed::Box; use alloc::string::ToString; use esp_hal::clock::CpuClock; use esp_hal::delay::Delay; -use esp_hal::main; +use esp_hal::{gpio, main}; use esp_hal::uart::{Config, Uart}; -use esp_hal::gpio::Event; +use esp_hal::gpio::{Event, OutputConfig}; use esp_hal::gpio::{Input, InputConfig}; use esp_hal::handler; use esp_hal::rmt::{Rmt, TxChannelConfig, TxChannelCreator}; @@ -69,15 +69,20 @@ fn main() -> ! { //setup RMT let freq = Rate::from_mhz(80); let rmt = Rmt::new(peripherals.RMT, freq).expect("CAN NOT SET FREQUENCY"); - let rx_config = RxChannelConfig::default().with_clk_divider(1); - let tx_config = TxChannelConfig::default().with_clk_divider(1); + + let rx_config = RxChannelConfig::default().with_clk_divider(1).with_idle_threshold(1000); + let tx_config = TxChannelConfig::default().with_clk_divider(1).with_idle_output(true).with_idle_output_level(esp_hal::gpio::Level::High); + let oc = OutputConfig::default().with_drive_mode(esp_hal::gpio::DriveMode::OpenDrain); + let ic = InputConfig::default(); + let rx_pin = gpio::Input::new(unsafe { peripherals.GPIO23.clone_unchecked() }, ic); + let tx_pin = gpio::Output::new(peripherals.GPIO23, esp_hal::gpio::Level::High, oc); let mut tx_channel = rmt .channel0 - .configure_tx(peripherals.GPIO23, tx_config) + .configure_tx(tx_pin, tx_config) .expect("creation of TX_CHANNEL FAILED!"); let mut rx_channel = rmt .channel3 - .configure_rx(peripherals.GPIO14, rx_config) + .configure_rx(rx_pin, rx_config) .unwrap(); let dshot_esc = dshot::DShot::new( &mut rx_channel, @@ -94,7 +99,7 @@ fn main() -> ! { dshot_esc.arm(); loop { dshot_esc.process(); - dshot_esc.set_throttle(200); + dshot_esc.set_throttle(150); } // rprintln!("RMT SENT!");