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

@@ -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<u16> {
+    // 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<u16>,
     current_tx: RefCell<Option<SingleShotTxTransaction<'a, 'a, u32>>>,
-
+    current_tx_done_at: RefCell<Option<esp_hal::time::Instant>>,
 }
 
 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
+                // We're done - but we might still be waiting for a response over bidi.
-                let tx_chann = rc.take().unwrap().wait().unwrap();
+                let mut ended = self.current_tx_done_at.borrow_mut();
-                let mut pulses = self.pulses.borrow_mut();
+                if ended.is_none() {
-                let throttle = self.requested_throttle.borrow();
+                    let start = esp_hal::time::Instant::now();
-                *pulses = self.create_pulses(*throttle, false);
+                    *ended = Some(start);
-                let pulses = pulses.as_ref();
+                    
-                // SAFETY: eat shit, rust
+                    // Try to receive a response for up to 100us.
-                let pulses = unsafe {
+                    let mut rx = self.rx_channel.borrow_mut();
-                    core::mem::transmute(pulses)
+                    let rx = rx.reborrow();
-                };
+                    let mut buf = [PulseCode::default(); 32];
-                let m = tx_chann
+                    {
-                    .transmit(pulses)
+                        let mut rxt = rx.receive(&mut buf).unwrap();
-                    .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);
+                                // Parse out telemetry data.
-                *rc = Some(m);
+                                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.

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!");