rewrite esc controller: periodic process func

spinnyboy_rust/src/bin/dc_driver/dshot.rs

@@ -2,8 +2,9 @@
 use esp_hal::{
     delay::Delay,
     gpio::Level,
-    rmt::{Channel, PulseCode, Rx, Tx, TxChannelCreator},
+    rmt::{Channel, PulseCode, SingleShotTxTransaction},
 };
+use core::cell::{Ref, RefCell};
 use num_traits::float::FloatCore;
 use rtt_target::{rprint, rprintln};
 
@@ -49,7 +50,6 @@ impl BitTicks {
     pub fn from_clk(
         clk_speed: u32,
         clk_divider: u8,
-        bit_times: BitTimes,
         speed: DShotSpeed,
     ) -> Self {
         let tick_len = (1. / clk_speed as f32) * (clk_divider as f32) * 1_000_000.;
@@ -78,6 +78,7 @@ impl BitTimes {
 }
 
 #[derive(Debug, Clone, Copy)]
+#[allow(dead_code)]
 pub enum DShotSpeed {
     DShot150,
     DShot300,
@@ -108,24 +109,18 @@ impl DShotSpeed {
             Self::DShot1200 => BitTimes::new(0.625, 0.313),
         }
     }
-
-    /// These are for an 80 MHz clock with a clock divider setting of 1
-    pub fn default_bit_ticks(&self) -> BitTicks {
-        match &self {
-            Self::DShot150 => BitTicks::new(400, 200),
-            Self::DShot300 => BitTicks::new(200, 100),
-            Self::DShot600 => BitTicks::new(100, 50),
-            Self::DShot1200 => BitTicks::new(50, 25),
-        }
-    }
 }
 
 #[allow(dead_code)]
 pub struct DShot<'a> {
     rx_channel: &'a mut Channel<'static, esp_hal::Blocking, esp_hal::rmt::Rx>,
-    tx_channel: &'a mut Channel<'static, esp_hal::Blocking, esp_hal::rmt::Tx>,
     speed: DShotSpeed,
     bit_ticks: BitTicks,
+
+    pulses: RefCell<[u32; 17]>,
+    requested_throttle: RefCell<u16>,
+    current_tx: RefCell<Option<SingleShotTxTransaction<'a, 'a, u32>>>,
+
 }
 
 impl<'a> DShot<'a> {
@@ -139,19 +134,17 @@ impl<'a> DShot<'a> {
     ) -> Self {
         let clk_speed = clk_speed.unwrap_or(80_000_000);
         let clk_divider = clk_divider.unwrap_or(1);
-        let bit_ticks = BitTicks::from_clk(clk_speed, clk_divider, speed.bit_times(), speed);
-        rprint!(
-            "bit_ticks.t1_h:{},t1_l:{},t0_h:{},t0_l{}",
-            bit_ticks.t1_h,
-            bit_ticks.t1_l,
-            bit_ticks.t0_h,
-            bit_ticks.t0_l
-        );
+        let bit_ticks = BitTicks::from_clk(clk_speed, clk_divider, speed);
+
+        let txc = tx_channel.reborrow().transmit(&[0u32]).unwrap();
         Self {
             rx_channel: rx_channel,
-            tx_channel: tx_channel,
             speed,
             bit_ticks,
+
+            pulses: RefCell::new([0u32; 17]),
+            requested_throttle: RefCell::new(0),
+            current_tx: RefCell::new(Some(txc)),
         }
     }
 
@@ -169,16 +162,13 @@ impl<'a> DShot<'a> {
     }
 
     #[allow(clippy::needless_range_loop)]
-    pub fn create_pulses(&mut self, throttle_value: u16, telemetry: bool) -> [u32; 17] {
+    pub fn create_pulses(&self, throttle_value: u16, telemetry: bool) -> [u32; 17] {
         let frame = Self::create_frame(throttle_value, telemetry);
         let mut pulses = [0; 17];
-        rprintln!("CREATING NEW FRAME!");
-        rprintln!("--------------------");
         for i in 0..16 {
             let bit = (frame >> (15 - i)) & 1;
 
             pulses[i] = if bit == 1 {
-                rprint!("1");
                 PulseCode::new(
                     Level::High,
                     self.bit_ticks.t1_h,
@@ -187,7 +177,6 @@ impl<'a> DShot<'a> {
                 )
                 .into()
             } else {
-                rprint!("0");
                 PulseCode::new(
                     Level::High,
                     self.bit_ticks.t0_h,
@@ -197,30 +186,56 @@ impl<'a> DShot<'a> {
                 .into()
             };
         }
-        rprintln!("");
-        rprintln!("--------------------");
 
         pulses[16] = 0;
         pulses
     }
 
-    pub fn write_throttle(&mut self, throttle: u16, telemetry: bool) -> Result<(), &'static str> {
-        let pulses = self.create_pulses(throttle, telemetry);
-        let tx_chann = self.tx_channel.reborrow();
-        let transaction = tx_chann
-            .transmit(&pulses)
-            .unwrap_or(return Err("Failed to send frame"))
-            .wait()
-            .unwrap_or(return Err("Waiting failed!"));
+    pub fn process(&'a self) {
+        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();
 
-        Ok(())
+                //self.current_tx = Some(m);
+                *rc = Some(m);
+            }
+        } else {
+            // Nothing to do, twiddle our thumbs.
+        }
     }
 
-    pub fn arm(&mut self, delay: &mut Delay) -> Result<(), &'static str> {
-        for _ in 0..100 {
-            self.write_throttle(0, false)?;
-            delay.delay_millis(50);
+    pub fn set_throttle(&self, v: u16) {
+        *self.requested_throttle.borrow_mut() = v;
+    }
+
+    #[allow(dead_code)]
+    pub fn arm(&'a self) -> Result<(), &'static str> {
+        let start = esp_hal::time::Instant::now();
+
+        rprintln!("ARM: Sending pre-throttle...");
+        while start.elapsed().as_secs() < 2 {
+            self.set_throttle(100);
+            self.process();
         }
+        rprintln!("ARM: Sending 0...");
+        while start.elapsed().as_secs() < 4 {
+            self.set_throttle(0);
+            self.process();
+        }
+        rprintln!("ARM: Done.");
 
         Ok(())
     }