From 9bce7f04166b4d3bf0b333d38f6aa6ef881a4770 Mon Sep 17 00:00:00 2001
From: Serge Bazanski <q3k@q3k.org>
Date: Sat, 28 Feb 2026 21:01:30 +0100
Subject: [PATCH] Adjust PID, RPM math (still not correct)

---
 spinnyboy_rust/Cargo.lock                 | 10 ++++++++++
 spinnyboy_rust/Cargo.toml                 |  1 +
 spinnyboy_rust/src/bin/dc_driver/dshot.rs | 17 ++++-------------
 spinnyboy_rust/src/bin/main.rs            |  6 +++---
 4 files changed, 18 insertions(+), 16 deletions(-)

diff --git a/spinnyboy_rust/Cargo.lock b/spinnyboy_rust/Cargo.lock
index 742f3de..ef595c7 100644
--- a/spinnyboy_rust/Cargo.lock
+++ b/spinnyboy_rust/Cargo.lock
@@ -213,6 +213,15 @@ dependencies = [
  "crypto-common",
 ]
 
+[[package]]
+name = "discrete_pid"
+version = "0.1.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "8d6406befa7a91e856133d660b608d13e8eb23d2a9df7b90c9a87339dd8b5558"
+dependencies = [
+ "num-traits",
+]
+
 [[package]]
 name = "document-features"
 version = "0.2.12"
@@ -1134,6 +1143,7 @@ name = "spinnyboy_rust"
 version = "0.1.0"
 dependencies = [
  "critical-section",
+ "discrete_pid",
  "embedded-hal 1.0.0",
  "esp-alloc",
  "esp-backtrace",
diff --git a/spinnyboy_rust/Cargo.toml b/spinnyboy_rust/Cargo.toml
index 38277ee..9e98e01 100644
--- a/spinnyboy_rust/Cargo.toml
+++ b/spinnyboy_rust/Cargo.toml
@@ -21,6 +21,7 @@ embedded-hal = "1.0.0"
 esp-backtrace = {version = "0.18.1",features = ["esp32c6","defmt"]}
 #esp-hal-dshot = {version = "0.3.1",default-features = false ,features = ["esp32c6"]}
 num-traits = { version = "0.2.19", default-features = false, features = ["libm"] }
+discrete_pid = { version = "0.1.0", default-features = false }
 
 [profile.dev]
 # Rust debug is too slow.
diff --git a/spinnyboy_rust/src/bin/dc_driver/dshot.rs b/spinnyboy_rust/src/bin/dc_driver/dshot.rs
index c504a11..a4fd8ff 100644
--- a/spinnyboy_rust/src/bin/dc_driver/dshot.rs
+++ b/spinnyboy_rust/src/bin/dc_driver/dshot.rs
@@ -15,7 +15,7 @@ fn calculate_crc(frame: u16) -> u16 {
 /*https://de.aliexpress.com/item/1005009484033753.html?spm=a2g0o.productlist.main.2.6389651cV8lVw8&algo_pvid=9cfe024b-8aff-49ea-981b-824f392de976&algo_exp_id=9cfe024b-8aff-49ea-981b-824f392de976-1&pdp_ext_f=%7B%22order%22%3A%2232%22%2C%22eval%22%3A%221%22%2C%22fromPage%22%3A%22search%22%7D&pdp_npi=6%40dis%21EUR%2111.99%2111.99%21%21%2113.55%2113.55%21%40211b813b17620408595223963e91a7%2112000049243823801%21sea%21DE%212739924950%21X%211%210%21n_tag%3A-29919%3Bd%3Abce7cd6%3Bm03_new_user%3A-29895&curPageLogUid=GNUFzsTU8oDr&utparam-url=scene%3Asearch%7Cquery_from%3A%7Cx_object_id%3A1005009484033753%7C_p_origin_prod%3A
 IFlight XING-E Pro 2207 2750KV 
 */
-const XING_EPRO_22_COILS: u32 = 12; 
+const XING_EPRO_22_POLES: u32 = 4; 
 
 #[allow(dead_code)]
 #[allow(non_camel_case_types)]
@@ -300,7 +300,7 @@ impl<'a> DShot<'a> {
                     let mut buf = [PulseCode::default(); 32];
                     {
                         let mut rxt = rx.receive(&mut buf).unwrap();
-                        while start.elapsed().as_micros() < 100 { 
+                        while start.elapsed().as_micros() < 150 { 
                             // Got something within this time.
                             if rxt.poll() {
                                 // Receive it fully.
@@ -316,22 +316,13 @@ impl<'a> DShot<'a> {
                                         if data != 0xfff && crc == (frame&0b1111) {
                                             // Fucking got it.
 
-                                            //    ESC_telemetry[4] = (SerialBuf[7] << 8) | SerialBuf[8]; // eRpM *100
-                                            // rprintln!("SETTING RPM");
-                                            // rprintln!("DATA:{}",data.to_le());
                                             let exp = data >> 9;
 
                                             let mantissa = data & 0b111111111;
                                             let period_ms: u32 = (mantissa << exp).into();
-                                            let erpm = 60*100_000/period_ms*2;
-                                            // value sent by ESC is a period between each pole changes [us].
-                                            // to achive eRPM we need to find out how many of these changes are in one minute.
-                                             // eRPM = (60*1000 000)/T_us next RPM can be achived -> RPM = eRPM/(poles/2):
+                                            let erpm = (60 * 1_000_000) / period_ms;
 
-                                            let rpm = (((erpm))/((XING_EPRO_22_COILS)));                                            
-                                            // rprintln!("RPM: {}", rpm);
-                                            // rprintln!("ERPM: {}", erpm);
-                                            // rprintln!("period_ms: {}", period_ms);
+                                            let rpm = (((erpm))/((XING_EPRO_22_POLES)));
                                             self.rpm.set(Some(rpm))
                                         }
                                     }
diff --git a/spinnyboy_rust/src/bin/main.rs b/spinnyboy_rust/src/bin/main.rs
index aa6fc36..834c2da 100644
--- a/spinnyboy_rust/src/bin/main.rs
+++ b/spinnyboy_rust/src/bin/main.rs
@@ -56,7 +56,7 @@ 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 = 500;
+const DEFAULT_TARGET_RPM: u32 = 4000;
 //in seconds
 const DEFAULT_SPIN_TIME: u32 = 10;
 // This creates a default app-descriptor required by the esp-idf bootloader.
@@ -126,8 +126,8 @@ fn main() -> ! {
     //PID-Controller
     let loop_time = Duration::from_micros(10);
     let cfg = pid::PidConfigBuilder::default()
-    .kp(2.0)
-    .ki(1.5).output_limits(100.00, 2040.00)
+    .kp(3.0)
+    .ki(1.0).output_limits(100.00, 2040.00)
     .sample_time(loop_time)
     .filter_tc(0.1)
     .build()