basic pid controller working???

spinnyboy_rust/src/bin/dc_driver/dshot.rs

@@ -149,8 +149,15 @@ fn decode_bidi_telemetry(pulses: &[PulseCode]) -> Option<u16> {
 
     // Process edges.
     // TODO: strip until we find the first negative edge.
-    assert_eq!(edges[0].0, false);
+    // assert_eq!(edges[0].0, false);
+    let mut idx = 0;
+    for i in 00..edges.len() {
+        if edges[i].0 == false{
 
+            idx = i;
+            break
+        }
+    }
     // Approximate. Should be calculated instead, in practice these pulses are
     // around 1.34us.
     let period = 104u32;
@@ -158,7 +165,7 @@ fn decode_bidi_telemetry(pulses: &[PulseCode]) -> Option<u16> {
     // Grad middle of each period, find latest applying period. Shitty
     // algorithm.
     let mut res = [false; 22];
-    for i in 0..res.len() {
+    for i in idx..res.len() {
         let pos = (i as u32)*period + period/2;
         let mut level = edges[0].0;
         for edge in edges[..nedges].iter() {

spinnyboy_rust/src/bin/main.rs

@@ -28,7 +28,7 @@ use discrete_pid::time::InstantLike;
 use core::cell::RefCell;
 use core::mem::forget;
 use critical_section::Mutex;
-use esp_backtrace as _;
+use esp_backtrace::{self as _, Backtrace};
 use rtt_target::rprintln;
 mod dc_driver;
 
@@ -44,16 +44,19 @@ use crate::peripherals::ErrCommand;
 use esp_hal::rmt::RxChannelConfig;
 use esp_hal::rmt::RxChannelCreator;
 #[panic_handler]
-fn panic(_: &core::panic::PanicInfo) -> ! {
-    rprintln!("PANIC!");
-    loop {}
+fn panic(panic: &core::panic::PanicInfo) -> ! {
+    rprintln!("{}",panic.message());
+    let trace = Backtrace::capture();
+    loop {
+        // rprintln!("{}",panic.location().unwrap())
+    }
 }
 use alloc::format;
 //
 static EMERGENCY_BUTTON: Mutex<RefCell<Option<Input>>> = Mutex::new(RefCell::new(None));
 extern crate alloc;
 //target RPM
-const DEFAULT_TARGET_RPM: u32 = 2000;
+const DEFAULT_TARGET_RPM: u32 = 500;
 //in seconds
 const DEFAULT_SPIN_TIME: u32 = 10;
 // This creates a default app-descriptor required by the esp-idf bootloader.
@@ -121,10 +124,10 @@ fn main() -> ! {
 
     //dshot_esc.arm(&delay).unwrap();
     //PID-Controller
-    let loop_time = Duration::from_micros(125);
+    let loop_time = Duration::from_micros(10);
     let cfg = pid::PidConfigBuilder::default()
     .kp(2.0)
-    .ki(1.5)
+    .ki(1.5).output_limits(100.00, 2040.00)
     .sample_time(loop_time)
     .filter_tc(0.1)
     .build()
@@ -134,13 +137,13 @@ fn main() -> ! {
     let set_point = DEFAULT_TARGET_RPM;
     dshot_esc.arm();
     let mut rpm = 0;
-    let mut control: f32 = 0.0;
+    let mut control: f32 = 200.0;
     // let timestamp = Instant::now();
     loop {
         dshot_esc.process();
         //let control_val = dc_driver::
         //this bad boy needs floats, this will be fun :)
-        dshot_esc.set_throttle(400);
+        dshot_esc.set_throttle(control as _);
         rpm = match dshot_esc.get_rpm() {
             Some(x) => {
                 // rprintln!("GOT RPM {}", x);
@@ -154,10 +157,10 @@ fn main() -> ! {
         // rprintln!("RPM:{}",rpm);
         let last_time = esp_hal::time::Instant::now();
      
-        let timestamp = last_time + Duration::from_millis(Instant::now().duration_since_epoch().as_millis() as _); 
-        (control, ctx) = controller.compute(ctx, rpm as _, set_point as _, timestamp, None);
+        let timestamp = esp_hal::time::Instant::now().duration_since_epoch().as_millis();
+        (control, ctx) = controller.compute(ctx, rpm as _, set_point as _, Millis(timestamp), None);
         let dumped_context = ctx.last_time().expect("LAST TIME!"); 
-        rprintln!("control:{},rpm:{},timestamp:{:?}", control,rpm,dumped_context);
+        rprintln!("control:{},rpm:{}", control,rpm);
     }
 
     // rprintln!("RMT SENT!");