englebert
/
ExpressFC-OLD
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Fixed the DSHOT by generating all signal all at once

master
Englebert 3 years ago
parent
74b57d113f
commit
08403fafa3
6 changed files with 251 additions and 42 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      ExpressFC.h
  2. 16
      ExpressFC.ino
  3. 22
      Motors.cpp
  4. 2
      Motors.h
  5. 240
      dshot.cpp
  6. 11
      dshot.h

2
ExpressFC.h
View File

@ -10,7 +10,7 @@
#define MOTOR3 14
#define MOTOR4 27
#define LED_BUILTIN 4
#define LED_BUILTIN 2
// Task scheduler
#if CONFIG_FREERTOS_UNICORE

16
ExpressFC.ino
View File

@ -38,6 +38,7 @@ void setup() {
// Delay for a while to let the CPU settle down
delay(10);
pinMode(LED_BUILTIN, OUTPUT);
// Serial.print(F("Initializing Motors: "));
// Initialize motors
@ -144,24 +145,27 @@ void taskMotor(void *pvParameters) {
(void) pvParameters;
for(;;) {
if(ESP.getCycleCount() > last_dshot + DSHOT_TIMEFRAME) {
/***
motors.write_motor(MOTOR1, 1046);
motors.write_motor(MOTOR2, 1346);
motors.write_motor(MOTOR3, 1146);
motors.write_motor(MOTOR4, 1246);
last_dshot = ESP.getCycleCount();
}
***/
motors.write_motor(1046, 1346, 1146, 2040);
//motors.write_motor(1046, 1046, 1046, 1046);
vTaskDelay(1);
}
}
void taskBlink(void *pvParameters) {
(void) pvParameters;
pinMode(LED_BUILTIN, OUTPUT);
for(;;) {
digitalWrite(LED_BUILTIN, HIGH);
vTaskDelay(100);
delay(100);
digitalWrite(LED_BUILTIN, LOW);
vTaskDelay(100);
delay(100);
vTaskDelay(10);
}
}

22
Motors.cpp
View File

@ -17,25 +17,17 @@ Motors::Motors(uint8_t motor1_pin, uint8_t motor2_pin, uint8_t motor3_pin, uint8
pinMode(_motor2_pin, OUTPUT);
pinMode(_motor3_pin, OUTPUT);
pinMode(_motor4_pin, OUTPUT);
// Setting pins to dshot protocol
d.set_pins(_motor1_pin, _motor2_pin, _motor3_pin, _motor4_pin);
}
void Motors::initialize(void) {
// Initializing motors
// PIN NUMBER | VAL | TELEMETRY
d.send_cmd(_motor1_pin, 0, false);
d.send_cmd(_motor2_pin, 0, false);
d.send_cmd(_motor3_pin, 0, false);
d.send_cmd(_motor4_pin, 0, false);
// 1| 2| 3| 4| VAL | TELEMETRY
d.send_cmd(0, 0, 0, 0, false);
}
void Motors::write_motor(uint8_t id, uint16_t val) {
if(id == MOTOR1) {
d.send_cmd(_motor1_pin, val, false);
} else if(id == MOTOR2) {
d.send_cmd(_motor2_pin, val, false);
} else if(id == MOTOR3) {
d.send_cmd(_motor3_pin, val, false);
} else if(id == MOTOR4) {
d.send_cmd(_motor4_pin, val, false);
}
void Motors::write_motor(uint16_t val1, uint16_t val2, uint16_t val3, uint16_t val4) {
d.send_cmd(val1, val2, val3, val4, false);
}

2
Motors.h
View File

@ -7,7 +7,7 @@ class Motors {
public:
Motors(uint8_t motor1_pin, uint8_t motor2_pin, uint8_t motor3_pin, uint8_t motor4_pin);
void initialize(void);
void write_motor(uint8_t id, uint16_t val);
void write_motor(uint16_t val1, uint16_t val2, uint16_t val3, uint16_t val4);
private:
uint8_t _motor1_pin, _motor2_pin, _motor3_pin, _motor4_pin;

240
dshot.cpp
View File

@ -4,6 +4,13 @@
dshot::dshot(void) {
}
void dshot::set_pins(uint8_t motor1_pin, uint8_t motor2_pin, uint8_t motor3_pin, uint8_t motor4_pin) {
_motor1_pin = motor1_pin;
_motor2_pin = motor2_pin;
_motor3_pin = motor3_pin;
_motor4_pin = motor4_pin;
}
/***
(>>4) = 000010000010 # right shift value by 4
(^) = 100010101110 # XOR with value
@ -17,25 +24,45 @@ uint8_t dshot::crc(uint16_t val) {
return (a ^ (a >> 8)) & 0x0F;
}
void dshot::send_cmd(uint8_t pin, uint16_t speed, bool telemetry) {
void dshot::send_cmd(uint16_t motor1_val, uint16_t motor2_val, uint16_t motor3_val, uint16_t motor4_val, bool telemetry) {
bool all_channels_done = false;
// Over limit protection
if(speed > 2047)
speed = 2047;
if(motor1_val > 2047) motor1_val = 2047;
if(motor2_val > 2047) motor2_val = 2047;
if(motor3_val > 2047) motor3_val = 2047;
if(motor4_val > 2047) motor4_val = 2047;
// Speed left shift by 1 and add the telemetry bit
uint16_t val = (speed << 1) | telemetry;
uint16_t val1 = (motor1_val << 1) | telemetry;
uint16_t val2 = (motor2_val << 1) | telemetry;
uint16_t val3 = (motor3_val << 1) | telemetry;
uint16_t val4 = (motor4_val << 1) | telemetry;
// Preparing packets
if(!telemetry) {
uint16_t packet = (val << 4) | crc(val);
uint16_t packet1 = (val1 << 4) | crc(val1);
uint16_t packet2 = (val2 << 4) | crc(val2);
uint16_t packet3 = (val3 << 4) | crc(val3);
uint16_t packet4 = (val4 << 4) | crc(val4);
// Serial.print(F("PACKET: "));
// Serial.println(packet, BIN);
uint8_t bits = 16, duration_on, duration_off;
uint8_t bits = 16, counter = 0;
uint16_t duration_on, duration_off;
uint32_t time_on_start, time_off_start, time_on_end, time_off_end;
uint32_t current_clock;
uint32_t time_on_start_1, time_on_start_2, time_on_start_3, time_on_start_4;
uint32_t time_off_start_1, time_off_start_2, time_off_start_3, time_off_start_4;
uint16_t duration_on_1, duration_on_2, duration_on_3, duration_on_4;
uint16_t duration_off_1, duration_off_2, duration_off_3, duration_off_4;
bool bits_done_1, bits_done_2, bits_done_3, bits_done_4;
while(bits != 0) {
bits--;
/***
if((packet >> bits) & 0x0001) { // HIGH PULSE
if((packet1 >> bits) & 0x0001) { // HIGH PULSE
duration_on = DSHOT_CLOCK_CYCLE_T1H;
duration_off = DSHOT_CLOCK_CYCLE_T1L;
} else { // LOW PULSE
@ -43,28 +70,38 @@ void dshot::send_cmd(uint8_t pin, uint16_t speed, bool telemetry) {
duration_off = DSHOT_CLOCK_CYCLE_T0L;
}
GPIO.out_w1ts = ((uint32_t)1 << pin);
GPIO.out_w1ts = ((uint32_t)1 << _motor1_pin);
time_on_start = ESP.getCycleCount();
// Wait till the T1H time reached
while(time_on_end < time_on_start + duration_on) {
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);
GPIO.out_w1tc = ((uint32_t)1 << _motor1_pin);
time_off_start = ESP.getCycleCount();
// Wait till the T1L time reached
while(time_off_end < time_off_start + duration_off) {
while(time_off_end <= time_off_start + duration_off) {
time_off_end = ESP.getCycleCount();
}
// Making sure total clock...
while(time_off_end < time_on_start + DSHOT_CLOCK_CYCLE_TOTAL) {
time_off_end = ESP.getCycleCount();
}
***/
if((packet >> bits) & 0x0001) { // HIGH PULSE
/***
if((packet1 >> bits) & 0x0001) { // HIGH PULSE
// __
// If high then generate high signal pulse _| |_|
// Setting pin high for T1H
GPIO.out_w1ts = ((uint32_t)1 << pin);
GPIO.out_w1ts = ((uint32_t)1 << _motor1_pin);
GPIO.out_w1ts = ((uint32_t)1 << _motor2_pin);
GPIO.out_w1ts = ((uint32_t)1 << _motor3_pin);
GPIO.out_w1ts = ((uint32_t)1 << _motor4_pin);
time_on_start = ESP.getCycleCount();
// Wait till the T1H time reached
@ -72,7 +109,10 @@ void dshot::send_cmd(uint8_t pin, uint16_t speed, bool telemetry) {
}
// Setting pin low for T1L
GPIO.out_w1tc = ((uint32_t)1 << pin);
GPIO.out_w1tc = ((uint32_t)1 << _motor1_pin);
GPIO.out_w1tc = ((uint32_t)1 << _motor2_pin);
GPIO.out_w1tc = ((uint32_t)1 << _motor3_pin);
GPIO.out_w1tc = ((uint32_t)1 << _motor4_pin);
time_off_start = ESP.getCycleCount();
// Wait till the T1L time reached
@ -82,7 +122,10 @@ void dshot::send_cmd(uint8_t pin, uint16_t speed, bool telemetry) {
} else { // LOW PULSE
// _
// If low then generate high signal pulse _| |__|
GPIO.out_w1ts = ((uint32_t)1 << pin);
GPIO.out_w1ts = ((uint32_t)1 << _motor1_pin);
GPIO.out_w1ts = ((uint32_t)1 << _motor2_pin);
GPIO.out_w1ts = ((uint32_t)1 << _motor3_pin);
GPIO.out_w1ts = ((uint32_t)1 << _motor4_pin);
time_on_start = ESP.getCycleCount();
// Wait till the T1H time reached
@ -90,14 +133,177 @@ void dshot::send_cmd(uint8_t pin, uint16_t speed, bool telemetry) {
}
// Setting pin low for T1L
GPIO.out_w1tc = ((uint32_t)1 << pin);
GPIO.out_w1tc = ((uint32_t)1 << _motor1_pin);
GPIO.out_w1tc = ((uint32_t)1 << _motor2_pin);
GPIO.out_w1tc = ((uint32_t)1 << _motor3_pin);
GPIO.out_w1tc = ((uint32_t)1 << _motor4_pin);
time_off_start = ESP.getCycleCount();
// Wait till the T1L time reached
while(ESP.getCycleCount() < time_off_start + DSHOT_CLOCK_CYCLE_T0L) {
}
}
}
***/
// TODO: NEED TO RETHINK THE LOGIC!!
// All channels set to false
bits_done_1 = false;
bits_done_2 = false;
bits_done_3 = false;
bits_done_4 = false;
all_channels_done = false;
if((packet1 >> bits) & 0x0001) { // HIGH PULSE
duration_on_1 = DSHOT_CLOCK_CYCLE_T1H;
duration_off_1 = DSHOT_CLOCK_CYCLE_T1L;
} else { // LOW PULSE
duration_on_1 = DSHOT_CLOCK_CYCLE_T0H;
duration_off_1 = DSHOT_CLOCK_CYCLE_T0L;
}
if((packet2 >> bits) & 0x0001) { // HIGH PULSE
duration_on_2 = DSHOT_CLOCK_CYCLE_T1H;
duration_off_2 = DSHOT_CLOCK_CYCLE_T1L;
} else { // LOW PULSE
duration_on_2 = DSHOT_CLOCK_CYCLE_T0H;
duration_off_2 = DSHOT_CLOCK_CYCLE_T0L;
}
if((packet3 >> bits) & 0x0001) { // HIGH PULSE
duration_on_3 = DSHOT_CLOCK_CYCLE_T1H;
duration_off_3 = DSHOT_CLOCK_CYCLE_T1L;
} else { // LOW PULSE
duration_on_3 = DSHOT_CLOCK_CYCLE_T0H;
duration_off_3 = DSHOT_CLOCK_CYCLE_T0L;
}
if((packet4 >> bits) & 0x0001) { // HIGH PULSE
duration_on_4 = DSHOT_CLOCK_CYCLE_T1H;
duration_off_4 = DSHOT_CLOCK_CYCLE_T1L;
} else { // LOW PULSE
duration_on_4 = DSHOT_CLOCK_CYCLE_T0H;
duration_off_4 = DSHOT_CLOCK_CYCLE_T0L;
}
// Start of the bit
GPIO.out_w1ts = ((uint32_t)1 << _motor1_pin);
GPIO.out_w1ts = ((uint32_t)1 << _motor2_pin);
GPIO.out_w1ts = ((uint32_t)1 << _motor3_pin);
GPIO.out_w1ts = ((uint32_t)1 << _motor4_pin);
time_on_start_1 = ESP.getCycleCount();
time_on_start_2 = time_on_start_1;
time_on_start_3 = time_on_start_1;
time_on_start_4 = time_on_start_1;
// Due to each of the bits may have different on time and off time. We need to track it
// counter = 0;
while(!all_channels_done){
// while(
// counter != 4
// ) {
current_clock = ESP.getCycleCount();
if(
!bits_done_1 &&
current_clock > time_on_start_1 + duration_on_1
) {
GPIO.out_w1tc = ((uint32_t)1 << _motor1_pin);
// time_off_start_1 = ESP.getCycleCount();
time_off_start_1 = current_clock;
bits_done_1 = true;
// counter++;
}
if(
!bits_done_2 &&
current_clock > time_on_start_2 + duration_on_2
) {
GPIO.out_w1tc = ((uint32_t)1 << _motor2_pin);
// time_off_start_2 = ESP.getCycleCount();
time_off_start_2 = current_clock;
bits_done_2 = true;
// counter++;
}
if(
!bits_done_3 &&
current_clock > time_on_start_3 + duration_on_3
) {
GPIO.out_w1tc = ((uint32_t)1 << _motor3_pin);
// time_off_start_3 = ESP.getCycleCount();
time_off_start_3 = current_clock;
bits_done_3 = true;
// counter++;
}
if(
!bits_done_4 &&
current_clock > time_on_start_4 + duration_on_4
) {
GPIO.out_w1tc = ((uint32_t)1 << _motor4_pin);
// time_off_start_4 = ESP.getCycleCount();
time_off_start_4 = current_clock;
bits_done_4 = true;
// counter++;
}
if(bits_done_1 && bits_done_2 && bits_done_3 && bits_done_4)
all_channels_done = true;
} // End of while
// Reset flags
all_channels_done = false;
bits_done_1 = false;
bits_done_2 = false;
bits_done_3 = false;
bits_done_4 = false;
// Waiting for each of the motor bits done...
while(
!bits_done_1 &&
!bits_done_2 &&
!bits_done_3 &&
!bits_done_4
) {
current_clock = ESP.getCycleCount();
if(
!bits_done_1 &&
current_clock > time_off_start_1 + duration_off_1
) {
bits_done_1 = true;
}
if(
!bits_done_2 &&
current_clock > time_off_start_2 + duration_off_2
) {
bits_done_2 = true;
}
if(
!bits_done_3 &&
current_clock > time_off_start_3 + duration_off_3
) {
bits_done_3 = true;
}
if(
!bits_done_4 &&
current_clock > time_off_start_4 + duration_off_4
) {
bits_done_4 = true;
}
/*
if(bits_done_1 && bits_done_2 && bits_done_3 && bits_done_4)
all_channels_done = true;
*/
} // End of while
} // End of while
} else {
// TODO: FUTURE for TURTLE MODE
}

11
dshot.h
View File

@ -30,18 +30,25 @@
// DSHOT600 time length
#define DSHOT_TIMEFRAME 6416 // Total DSHOT time frame
/***
#define DSHOT_CLOCK_CYCLE_TOTAL 401 // Based on calculation, each cycle is about 4.167nS
#define DSHOT_CLOCK_CYCLE_T0H 150 // Precisely at 150.0024000384006 based on 240MHz
#define DSHOT_CLOCK_CYCLE_T0L 251 // Precisely at 250.8040128642058 based on 240MHz
#define DSHOT_CLOCK_CYCLE_T1H 300 // Precisely at 300.0048000768012 based on 240MHz
#define DSHOT_CLOCK_CYCLE_T1L 101 // Precisely at 100.8016128258052 based on 240MHz
***/
#define DSHOT_CLOCK_CYCLE_T0H 70 // Precisely at 150.0024000384006 based on 240MHz
#define DSHOT_CLOCK_CYCLE_T0L 171 // Precisely at 250.8040128642058 based on 240MHz
#define DSHOT_CLOCK_CYCLE_T1H 220 // Precisely at 300.0048000768012 based on 240MHz
#define DSHOT_CLOCK_CYCLE_T1L 21 // Precisely at 100.8016128258052 based on 240MHz
class dshot {
public:
dshot(void);
uint8_t crc(uint16_t val);
void send_cmd(uint8_t pin, uint16_t val, bool telemetry);
void set_pins(uint8_t motor1_pin, uint8_t motor2_pin, uint8_t motor3_pin, uint8_t motor4_pin);
void send_cmd(uint16_t motor1_val, uint16_t motor2_val, uint16_t motor3_val, uint16_t motor4_val, bool telemetry);
uint8_t _motor1_pin, _motor2_pin, _motor3_pin, _motor4_pin;
private:
};

Write Preview
Loading…
Cancel
Save