rpm get
The fucking receive data format/encoding/modulation is just bonkers. But I think I got it.
This commit is contained in:
parent
06fd42430f
commit
6c38cd59ee
2 changed files with 164 additions and 36 deletions
|
|
@ -1,13 +1,18 @@
|
|||
// use embedded_hal_async::delay::DelayNs;
|
||||
use esp_hal::{
|
||||
delay::Delay,
|
||||
delay::{self, Delay},
|
||||
gpio::Level,
|
||||
rmt::{Channel, PulseCode, SingleShotTxTransaction},
|
||||
};
|
||||
use core::cell::{Ref, RefCell};
|
||||
use core::{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
|
||||
}
|
||||
|
||||
|
||||
#[allow(dead_code)]
|
||||
#[allow(non_camel_case_types)]
|
||||
#[derive(Debug)]
|
||||
|
|
@ -111,16 +116,91 @@ impl DShotSpeed {
|
|||
}
|
||||
}
|
||||
|
||||
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);
|
||||
|
||||
// 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 0..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: &'a mut Channel<'static, esp_hal::Blocking, esp_hal::rmt::Rx>,
|
||||
rx_channel: RefCell<&'a mut Channel<'static, esp_hal::Blocking, esp_hal::rmt::Rx>>,
|
||||
speed: DShotSpeed,
|
||||
bit_ticks: BitTicks,
|
||||
|
||||
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> {
|
||||
|
|
@ -138,25 +218,22 @@ impl<'a> DShot<'a> {
|
|||
|
||||
let txc = tx_channel.reborrow().transmit(&[0u32]).unwrap();
|
||||
Self {
|
||||
rx_channel: rx_channel,
|
||||
rx_channel: RefCell::new(rx_channel),
|
||||
speed,
|
||||
bit_ticks,
|
||||
|
||||
pulses: RefCell::new([0u32; 17]),
|
||||
requested_throttle: RefCell::new(0),
|
||||
current_tx: RefCell::new(Some(txc)),
|
||||
current_tx_done_at: RefCell::new(None),
|
||||
}
|
||||
}
|
||||
|
||||
pub fn calculate_crc(frame: u16) -> u16 {
|
||||
(frame ^ (frame >> 4) ^ (frame >> 8)) & 0xF
|
||||
}
|
||||
|
||||
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 = Self::calculate_crc(frame);
|
||||
let crc = calculate_crc(frame);
|
||||
|
||||
(frame << 4) | crc
|
||||
}
|
||||
|
|
@ -170,17 +247,17 @@ impl<'a> DShot<'a> {
|
|||
|
||||
pulses[i] = if bit == 1 {
|
||||
PulseCode::new(
|
||||
Level::High,
|
||||
self.bit_ticks.t1_h,
|
||||
Level::Low,
|
||||
self.bit_ticks.t1_h,
|
||||
Level::High,
|
||||
self.bit_ticks.t1_l,
|
||||
)
|
||||
.into()
|
||||
} else {
|
||||
PulseCode::new(
|
||||
Level::High,
|
||||
self.bit_ticks.t0_h,
|
||||
Level::Low,
|
||||
self.bit_ticks.t0_h,
|
||||
Level::High,
|
||||
self.bit_ticks.t0_l,
|
||||
)
|
||||
.into()
|
||||
|
|
@ -195,6 +272,49 @@ impl<'a> DShot<'a> {
|
|||
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() < 100 {
|
||||
// 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 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 rpm = mantissa << exp;
|
||||
rprintln!("RPM: {}", 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();
|
||||
|
|
@ -209,8 +329,11 @@ impl<'a> DShot<'a> {
|
|||
.transmit(pulses)
|
||||
.unwrap();
|
||||
|
||||
//self.current_tx = Some(m);
|
||||
*rc = Some(m);
|
||||
*ended = None;
|
||||
} else{
|
||||
// Wait for bidi dshot data.
|
||||
}
|
||||
}
|
||||
} else {
|
||||
// Nothing to do, twiddle our thumbs.
|
||||
|
|
|
|||
|
|
@ -9,10 +9,10 @@ use alloc::boxed::Box;
|
|||
use alloc::string::ToString;
|
||||
use esp_hal::clock::CpuClock;
|
||||
use esp_hal::delay::Delay;
|
||||
use esp_hal::main;
|
||||
use esp_hal::{gpio, main};
|
||||
use esp_hal::uart::{Config, Uart};
|
||||
|
||||
use esp_hal::gpio::Event;
|
||||
use esp_hal::gpio::{Event, OutputConfig};
|
||||
use esp_hal::gpio::{Input, InputConfig};
|
||||
use esp_hal::handler;
|
||||
use esp_hal::rmt::{Rmt, TxChannelConfig, TxChannelCreator};
|
||||
|
|
@ -69,15 +69,20 @@ fn main() -> ! {
|
|||
//setup RMT
|
||||
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);
|
||||
let tx_config = TxChannelConfig::default().with_clk_divider(1);
|
||||
|
||||
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);
|
||||
let tx_pin = gpio::Output::new(peripherals.GPIO23, esp_hal::gpio::Level::High, oc);
|
||||
let mut tx_channel = rmt
|
||||
.channel0
|
||||
.configure_tx(peripherals.GPIO23, tx_config)
|
||||
.configure_tx(tx_pin, tx_config)
|
||||
.expect("creation of TX_CHANNEL FAILED!");
|
||||
let mut rx_channel = rmt
|
||||
.channel3
|
||||
.configure_rx(peripherals.GPIO14, rx_config)
|
||||
.configure_rx(rx_pin, rx_config)
|
||||
.unwrap();
|
||||
let dshot_esc = dshot::DShot::new(
|
||||
&mut rx_channel,
|
||||
|
|
@ -94,7 +99,7 @@ fn main() -> ! {
|
|||
dshot_esc.arm();
|
||||
loop {
|
||||
dshot_esc.process();
|
||||
dshot_esc.set_throttle(200);
|
||||
dshot_esc.set_throttle(150);
|
||||
}
|
||||
|
||||
// rprintln!("RMT SENT!");
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue