Firmware to drive ESP32 based FC with built-in: Gyro Baro Current Sensor Voltage reading OSD NRF24L01+ 4x ESC backplane
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
 
 

101 lines
3.1 KiB

#include "Arduino.h"
#include "dshot.h"
dshot::dshot(void) {
}
/***
(>>4) = 000010000010 # right shift value by 4
(^) = 100010101110 # XOR with value
(>>8) = 000000001000 # right shift value by 8
(^) = 100010100110 # XOR with previous XOR
(0x0F) = 000000001111 # Mask 0x0F
(&) = 000000000110 # CRC
***/
uint8_t dshot::crc(uint16_t val) {
uint16_t a = val ^ (val >> 4);
return (a ^ (a >> 8)) & 0x0F;
}
void dshot::send_cmd(uint8_t pin, uint16_t speed, bool telemetry) {
// Over limit protection
if(speed > 2047)
speed = 2047;
// Speed left shift by 1 and add the telemetry bit
uint16_t val = (speed << 1) | telemetry;
// Preparing packets
if(!telemetry) {
uint16_t packet = (val << 4) | crc(val);
// Serial.print(F("PACKET: "));
// Serial.println(packet, BIN);
bool _loop = false;
uint8_t bits = 16;
uint16_t time_on_start, time_off_start, duration_high, duration_low;
// uint32_t time_profiling_start, time_profiling_end;
// Serial.print(F("Transmitting bits:"));
// time_profiling_start = ESP.getCycleCount();
while(bits != 0) {
bits--;
// Serial.print(F("."));
// Preparing the pulse timing
if((packet >> bits) & 0x0001) {
// HIGH PULSE timing
duration_high = DSHOT_CLOCK_CYCLE_T1H;
duration_low = DSHOT_CLOCK_CYCLE_T1L;
} else {
// LOW PULSE timing
duration_high = DSHOT_CLOCK_CYCLE_T0H;
duration_low = DSHOT_CLOCK_CYCLE_T0L;
}
// __
// If high then generate high signal pulse _| |_|
// Setting pin high for T1H
GPIO.out_w1ts = ((uint32_t)1 << pin);
time_on_start = ESP.getCycleCount();
/***
for(uint16_t i = 0; i < duration_high; i++) {
asm volatile("nop");
}
***/
_loop = true;
// Wait till the T1H/T1L time reached
while(_loop) {
if((uint32_t)(ESP.getCycleCount() - time_on_start) > duration_high)
_loop = false;
}
// Setting pin low for T1L
GPIO.out_w1tc = ((uint32_t)1 << pin);
time_off_start = ESP.getCycleCount();
/***
for(uint16_t i = 0; i < duration_low; i++) {
asm volatile("nop");
}
***/
_loop = true;
// Wait till the T1L/T0L time reached
while(_loop) {
if((uint32_t)(ESP.getCycleCount() - time_off_start) > duration_low)
_loop = false;
}
} // End of while
// Serial.println(F("Done"));
// time_profiling_end = ESP.getCycleCount();
// uint32_t time_profiling_result = time_profiling_end - time_profiling_start;
// Serial.print(F("1 Byte cpu cycles: "));
// Serial.println(time_profiling_result);
} else {
// TODO: FUTURE for TURTLE MODE
}
}