fafo/maasier
16
0
Fork 0
Code Issues 6 Pull requests Projects Releases Packages Wiki Activity Actions

we are now measuring the RPM based on the code from: https://symonb.github.io//docs/drone/ESC/ESC_prot_impl_2_2

Browse source
This commit is contained in:
Robert Schauklies 2026-02-14 19:17:34 +01:00
parent 47f224055b
commit 65761b3f7d
3 changed files with 75 additions and 19 deletions
Download patch file Download diff file Expand all files Collapse all files

24
spinnyboy_rust/src/bin/dc_driver/dshot.rs
View file

@ -304,16 +304,28 @@ impl<'a> DShot<'a> {
// Decode to a 16 bit value.
if let Some(frame) = decode_bidi_telemetry(&buf[..len]) {
// Check CRC.
let data = frame >> 4;
let mut 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 mut rpm: u32 = (mantissa << exp).into();
rpm = (rpm*1000);
// 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 rpm = (((erpm))/((XING_EPRO_22_COILS)));
rprintln!("RPM: {}", rpm);
rprintln!("ERPM: {}", erpm);
rprintln!("period_ms: {}", period_ms);
self.rpm.set(Some(rpm))
}
}
}
@ -330,7 +342,7 @@ impl<'a> DShot<'a> {
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);
*pulses = self.create_pulses(*throttle, true);
let pulses = pulses.as_ref();
// SAFETY: eat shit, rust
let pulses = unsafe {

5
spinnyboy_rust/src/bin/dc_driver/mod.rs
View file

@ -1,2 +1,7 @@
//pub mod afroesc;
pub mod dshot;
pub fn arduino_map(x:i32, in_min:i32, in_max:i32, out_min:i32, out_max:i32) -> i32 {
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

65
spinnyboy_rust/src/bin/main.rs
View file

@ -9,14 +9,20 @@ use alloc::boxed::Box;
use alloc::string::ToString;
use esp_hal::clock::CpuClock;
use esp_hal::delay::Delay;
use esp_hal::{gpio, main};
use esp_hal::uart::{Config, Uart};
use esp_hal::{gpio, main};
use esp_hal::gpio::{Event, OutputConfig};
use esp_hal::gpio::{Input, InputConfig};
use esp_hal::handler;
use esp_hal::rmt::{Rmt, TxChannelConfig, TxChannelCreator};
use esp_hal::time::Rate;
use esp_hal::time::{ Instant, Rate};
//pid-controller
use core::time::Duration;
use discrete_pid::pid::FuncPidController;
use discrete_pid::pid::PidContext;
use discrete_pid::time::Millis;
use discrete_pid::{pid, time};
use core::cell::RefCell;
use core::mem::forget;
@ -70,8 +76,13 @@ fn main() -> ! {
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).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 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);
@ -80,10 +91,7 @@ fn main() -> ! {
.channel0
.configure_tx(tx_pin, tx_config)
.expect("creation of TX_CHANNEL FAILED!");
let mut rx_channel = rmt
.channel3
.configure_rx(rx_pin, rx_config)
.unwrap();
let mut rx_channel = rmt.channel3.configure_rx(rx_pin, rx_config).unwrap();
let dshot_esc = dshot::DShot::new(
&mut rx_channel,
&mut tx_channel,
@ -95,15 +103,46 @@ fn main() -> ! {
let dshot_esc = Box::leak(dshot_esc);
//dshot_esc.arm(&delay).unwrap();
//PID-Controller
let loop_time = core::time::Duration::from_millis(500);
let cfg = pid::PidConfigBuilder::default()
.kp(1.0)
.ki(1.5)
.sample_time(loop_time)
// .filter_tc(0.1)
.output_limits(50.00, 1000.00)
.build()
.expect("Failed to build a PID configuration");
let pid = FuncPidController::new(cfg);
let mut ctx = PidContext::new(Millis(0), 0.0, 0.0);
let set_point = DEFAULT_TARGET_RPM;
dshot_esc.arm();
let mut rpm = 0;
let mut control: f32 = 0.0;
loop {
let mut begin = Instant::now();
dshot_esc.process();
dshot_esc.set_throttle(150);
match dshot_esc.get_rpm() {
Some(x) => {rprintln!("GOT RPM {}",x)},
None => {rprintln!("NO RPM!")}
}
//let control_val = dc_driver::
//this bad boy needs floats, this will be fun :)
dshot_esc.set_throttle(200);
rpm = match dshot_esc.get_rpm() {
Some(x) => {
// rprintln!("GOT RPM {}", x);
x
}
None => {
rprintln!("NO RPM!");
rpm
}
};
// rprintln!("RPM:{}",rpm);
let last_time = ctx.last_time().unwrap_or(Millis(0));
// let timestamp = last_time + Duration::from_millis(begin.elapsed().as_millis() as _);
// (control, ctx) = pid.compute(ctx, rpm as _, set_point as _, timestamp, None);
// rprintln!("control:{},rpm:{}", control,rpm);
}
// rprintln!("RMT SENT!");