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.
104 lines
3.6 KiB
104 lines
3.6 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);
|
|
uint8_t bits = 16, duration_on, duration_off;
|
|
uint32_t time_on_start, time_off_start, time_on_end, time_off_end;
|
|
while(bits != 0) {
|
|
bits--;
|
|
/***
|
|
if((packet >> bits) & 0x0001) { // HIGH PULSE
|
|
duration_on = DSHOT_CLOCK_CYCLE_T1H;
|
|
duration_off = DSHOT_CLOCK_CYCLE_T1L;
|
|
} else { // LOW PULSE
|
|
duration_on = DSHOT_CLOCK_CYCLE_T0H;
|
|
duration_off = DSHOT_CLOCK_CYCLE_T0L;
|
|
}
|
|
|
|
GPIO.out_w1ts = ((uint32_t)1 << pin);
|
|
time_on_start = ESP.getCycleCount();
|
|
|
|
// Wait till the T1H time reached
|
|
while(time_on_end < time_on_start + duration_on) {
|
|
time_on_end = ESP.getCycleCount();
|
|
}
|
|
|
|
// Setting pin low for T1L
|
|
GPIO.out_w1tc = ((uint32_t)1 << pin);
|
|
time_off_start = ESP.getCycleCount();
|
|
|
|
// Wait till the T1L time reached
|
|
while(time_off_end < time_off_start + duration_off) {
|
|
time_off_end = ESP.getCycleCount();
|
|
}
|
|
***/
|
|
if((packet >> bits) & 0x0001) { // HIGH PULSE
|
|
// __
|
|
// If high then generate high signal pulse _| |_|
|
|
// Setting pin high for T1H
|
|
GPIO.out_w1ts = ((uint32_t)1 << pin);
|
|
time_on_start = ESP.getCycleCount();
|
|
|
|
// Wait till the T1H time reached
|
|
while(ESP.getCycleCount() < time_on_start + DSHOT_CLOCK_CYCLE_T1H) {
|
|
}
|
|
|
|
// Setting pin low for T1L
|
|
GPIO.out_w1tc = ((uint32_t)1 << pin);
|
|
time_off_start = ESP.getCycleCount();
|
|
|
|
// Wait till the T1L time reached
|
|
while(ESP.getCycleCount() < time_off_start + DSHOT_CLOCK_CYCLE_T1L) {
|
|
}
|
|
// delayMicroseconds(1);
|
|
} else { // LOW PULSE
|
|
// _
|
|
// If low then generate high signal pulse _| |__|
|
|
GPIO.out_w1ts = ((uint32_t)1 << pin);
|
|
time_on_start = ESP.getCycleCount();
|
|
|
|
// Wait till the T1H time reached
|
|
while(ESP.getCycleCount() < time_on_start + DSHOT_CLOCK_CYCLE_T0H) {
|
|
}
|
|
|
|
// Setting pin low for T1L
|
|
GPIO.out_w1tc = ((uint32_t)1 << pin);
|
|
time_off_start = ESP.getCycleCount();
|
|
|
|
// Wait till the T1L time reached
|
|
while(ESP.getCycleCount() < time_off_start + DSHOT_CLOCK_CYCLE_T0L) {
|
|
}
|
|
}
|
|
}
|
|
} else {
|
|
// TODO: FUTURE for TURTLE MODE
|
|
}
|
|
}
|