386 lines
12 KiB
Rust
386 lines
12 KiB
Rust
// use embedded_hal_async::delay::DelayNs;
|
|
use esp_hal::{
|
|
delay::{self, Delay},
|
|
gpio::Level,
|
|
rmt::{Channel, PulseCode, SingleShotTxTransaction},
|
|
};
|
|
use core::{cell::{self, 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
|
|
}
|
|
|
|
/*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_POLES: u32 = 4;
|
|
|
|
#[allow(dead_code)]
|
|
#[allow(non_camel_case_types)]
|
|
#[derive(Debug)]
|
|
pub enum DSHOT_TELEMETRY_CMD {
|
|
MOTOR_STOP = 0x0,
|
|
BEEP1 = 0x01,
|
|
BEEP2 = 0x02,
|
|
BEEP3 = 0x03,
|
|
BEEP4 = 0x04,
|
|
BEEP5 = 0x05,
|
|
ESC_INFO = 0x06,
|
|
SPIN_DIRECTION_1 = 0x07,
|
|
SPIN_DIRECTION_2 = 0x08,
|
|
MODE_3D_OFF = 0x09,
|
|
MODE_3D_ON = 0x0A,
|
|
SETTINGS_REQUEST = 0x0B,
|
|
SAVE_SETTINGS = 0x0C,
|
|
EXTENDED_TELEMETRY_ENABLE = 0x0D,
|
|
EXTENDED_TELEMETRY_DISABLE = 0x0E,
|
|
}
|
|
|
|
#[derive(Debug, Clone, Copy)]
|
|
pub struct BitTicks {
|
|
t0_h: u16,
|
|
t0_l: u16,
|
|
t1_h: u16,
|
|
t1_l: u16,
|
|
}
|
|
|
|
impl BitTicks {
|
|
pub fn new(t1_h: u16, t0_h: u16) -> Self {
|
|
Self {
|
|
t0_h,
|
|
t1_h,
|
|
t0_l: t1_h,
|
|
t1_l: t0_h,
|
|
}
|
|
}
|
|
|
|
pub fn from_clk(
|
|
clk_speed: u32,
|
|
clk_divider: u8,
|
|
speed: DShotSpeed,
|
|
) -> Self {
|
|
let tick_len = (1. / clk_speed as f32) * (clk_divider as f32) * 1_000_000.;
|
|
let bit_period_us = speed.bit_period_ns() as f32 / 1000.0;
|
|
let bit_ticks = (bit_period_us / tick_len).round() as u16;
|
|
let t1_h = (speed.bit_times().t1_h / tick_len).round() as u16;
|
|
let t0_h = (speed.bit_times().t0_h / tick_len).round() as u16;
|
|
let mut bittick = Self::new(t1_h, t0_h);
|
|
bittick.t0_l = bit_ticks - t0_h;
|
|
bittick.t1_l = bit_ticks - t1_h;
|
|
bittick
|
|
}
|
|
}
|
|
|
|
/// High and Low Times in µs
|
|
#[derive(Debug, Clone, Copy)]
|
|
pub struct BitTimes {
|
|
t0_h: f32,
|
|
t1_h: f32,
|
|
}
|
|
|
|
impl BitTimes {
|
|
pub fn new(t1_h: f32, t0_h: f32) -> Self {
|
|
Self { t0_h, t1_h }
|
|
}
|
|
}
|
|
|
|
#[derive(Debug, Clone, Copy)]
|
|
#[allow(dead_code)]
|
|
pub enum DShotSpeed {
|
|
DShot150,
|
|
DShot300,
|
|
DShot600,
|
|
DShot1200,
|
|
}
|
|
|
|
impl DShotSpeed {
|
|
pub fn bit_period_ns(&self) -> u32 {
|
|
match self {
|
|
// 6.67µs per bit
|
|
Self::DShot150 => 6667,
|
|
// 3.33µs per bit
|
|
Self::DShot300 => 3333,
|
|
// 1.67µs per bit
|
|
Self::DShot600 => 1667,
|
|
// 0.83µs per bit
|
|
Self::DShot1200 => 833,
|
|
}
|
|
}
|
|
|
|
/// High and Low Times in µs
|
|
pub fn bit_times(&self) -> BitTimes {
|
|
match &self {
|
|
Self::DShot150 => BitTimes::new(5.00, 2.50),
|
|
Self::DShot300 => BitTimes::new(2.50, 1.25),
|
|
Self::DShot600 => BitTimes::new(1.25, 0.625),
|
|
Self::DShot1200 => BitTimes::new(0.625, 0.313),
|
|
}
|
|
}
|
|
}
|
|
|
|
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);
|
|
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;
|
|
|
|
// Grad middle of each period, find latest applying period. Shitty
|
|
// algorithm.
|
|
let mut res = [false; 22];
|
|
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() {
|
|
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: RefCell<&'a mut Channel<'static, esp_hal::Blocking, esp_hal::rmt::Rx>>,
|
|
speed: DShotSpeed,
|
|
bit_ticks: BitTicks,
|
|
rpm: Cell<Option<u32>>,
|
|
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> {
|
|
pub fn new(
|
|
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,
|
|
clk_speed: Option<u32>,
|
|
clk_divider: Option<u8>,
|
|
) -> 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);
|
|
let cell = Cell::new(None);
|
|
let txc = tx_channel.reborrow().transmit(&[0u32]).unwrap();
|
|
Self {
|
|
rx_channel: RefCell::new(rx_channel),
|
|
speed,
|
|
bit_ticks,
|
|
rpm: cell,
|
|
pulses: RefCell::new([0u32; 17]),
|
|
requested_throttle: RefCell::new(0),
|
|
current_tx: RefCell::new(Some(txc)),
|
|
current_tx_done_at: RefCell::new(None),
|
|
}
|
|
}
|
|
fn set_rpm(&self,new_rpm:u32){
|
|
self.rpm.set(Some(new_rpm));
|
|
}
|
|
pub fn get_rpm(&self) -> Option<u32> {
|
|
self.rpm.get()
|
|
}
|
|
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 = calculate_crc(frame);
|
|
|
|
(frame << 4) | crc
|
|
}
|
|
|
|
#[allow(clippy::needless_range_loop)]
|
|
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];
|
|
for i in 0..16 {
|
|
let bit = (frame >> (15 - i)) & 1;
|
|
|
|
pulses[i] = if bit == 1 {
|
|
PulseCode::new(
|
|
Level::Low,
|
|
self.bit_ticks.t1_h,
|
|
Level::High,
|
|
self.bit_ticks.t1_l,
|
|
)
|
|
.into()
|
|
} else {
|
|
PulseCode::new(
|
|
Level::Low,
|
|
self.bit_ticks.t0_h,
|
|
Level::High,
|
|
self.bit_ticks.t0_l,
|
|
)
|
|
.into()
|
|
};
|
|
}
|
|
|
|
pulses[16] = 0;
|
|
pulses
|
|
}
|
|
|
|
pub fn process(&'a self) {
|
|
let mut rc = self.current_tx.borrow_mut();
|
|
if let Some(current) = rc.as_mut() {
|
|
if current.poll() {
|
|
// We're done - but we might still be waiting for a response over bidi.
|
|
let mut ended = self.current_tx_done_at.borrow_mut();
|
|
if ended.is_none() {
|
|
let start = esp_hal::time::Instant::now();
|
|
*ended = Some(start);
|
|
|
|
// Try to receive a response for up to 100us.
|
|
let mut rx = self.rx_channel.borrow_mut();
|
|
let rx = rx.reborrow();
|
|
let mut buf = [PulseCode::default(); 32];
|
|
{
|
|
let mut rxt = rx.receive(&mut buf).unwrap();
|
|
while start.elapsed().as_micros() < 150 {
|
|
// Got something within this time.
|
|
if rxt.poll() {
|
|
// Receive it fully.
|
|
let (len, _) = rxt.wait().unwrap();
|
|
|
|
// Parse out telemetry data.
|
|
if len != 0 {
|
|
// Decode to a 16 bit value.
|
|
if let Some(frame) = decode_bidi_telemetry(&buf[..len]) {
|
|
// Check CRC.
|
|
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 period_ms: u32 = (mantissa << exp).into();
|
|
let erpm = (60 * 1_000_000) / period_ms;
|
|
|
|
let rpm = (((erpm))/((XING_EPRO_22_POLES)));
|
|
self.rpm.set(Some(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, true);
|
|
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.
|
|
}
|
|
}
|
|
|
|
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(())
|
|
}
|
|
}
|