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
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
|
|
}
|
|
}
|