Skip to content

Commit 93beb56

Browse files
qqqlabqqqlab
committed
add Dshot RP2040
1 parent 8a7a82b commit 93beb56

File tree

19 files changed

+1197
-50
lines changed

19 files changed

+1197
-50
lines changed

‎examples/01.Quadcopter/01.Quadcopter.ino‎

Lines changed: 9 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -97,15 +97,16 @@ void setup() {
9797
//setup madflight components: Serial.begin(115200), imu, rcin, led, etc. See src/madflight/interface.h for full interface description of each component.
9898
madflight_setup();
9999

100-
// Uncomment ONE line - select output type
101-
int out_hz = 400; int min_us = 950; int max_us = 2000; //Standard PWM: 400Hz, 950-2000 us
102-
//int out_hz = 2000; int min_us = 125; int max_us = 250; //Oneshot125: 2000Hz, 125-250 us
103-
104100
// Setup 4 motors for the quadcopter
105-
out.setupMotor(0, cfg.pin_out0, out_hz, min_us, max_us);
106-
out.setupMotor(1, cfg.pin_out1, out_hz, min_us, max_us);
107-
out.setupMotor(2, cfg.pin_out2, out_hz, min_us, max_us);
108-
out.setupMotor(3, cfg.pin_out3, out_hz, min_us, max_us);
101+
int motor_idxs[] = {0, 1, 2, 3}; //motor indexes
102+
int motor_pins[] = {cfg.pin_out0, cfg.pin_out1, cfg.pin_out2, cfg.pin_out3}; //motor pins
103+
104+
// Uncomment ONE line - select output type
105+
bool success = out.setupMotors(4, motor_idxs, motor_pins, 400, 950, 2000); // Standard PWM: 400Hz, 950-2000 us
106+
//bool success = out.setupMotors(4, motor_idxs, motor_pins, 2000, 125, 250); // Oneshot125: 2000Hz, 125-250 us
107+
//bool success = out.setupDshot(4, motor_idxs, motor_pins, 300); // Dshot300
108+
//bool success = out.setupDshotBidir(4, motor_idxs, motor_pins, 300); // Dshot300 Bi-Directional
109+
if(!success) madflight_die("Motor init failed.");
109110

110111
//set initial desired yaw
111112
yaw_desired = ahr.yaw;

‎examples/02.QuadcopterAdvanced/02.QuadcopterAdvanced.ino‎

Lines changed: 9 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -106,15 +106,16 @@ void setup() {
106106
// STOP if imu is not installed
107107
if(!imu.installed()) madflight_die("This program needs an IMU.");
108108

109-
// Uncomment ONE line - select output type
110-
int out_hz = 400; int min_us = 950; int max_us = 2000; //Standard PWM: 400Hz, 950-2000 us
111-
//int out_hz = 2000; int min_us = 125; int max_us = 250; //Oneshot125: 2000Hz, 125-250 us
112-
113109
// Setup 4 motors for the quadcopter
114-
out.setupMotor(0, cfg.pin_out0, out_hz, min_us, max_us);
115-
out.setupMotor(1, cfg.pin_out1, out_hz, min_us, max_us);
116-
out.setupMotor(2, cfg.pin_out2, out_hz, min_us, max_us);
117-
out.setupMotor(3, cfg.pin_out3, out_hz, min_us, max_us);
110+
int motor_idxs[] = {0, 1, 2, 3}; //motor indexes
111+
int motor_pins[] = {cfg.pin_out0, cfg.pin_out1, cfg.pin_out2, cfg.pin_out3}; //motor pins
112+
113+
// Uncomment ONE line - select output type
114+
bool success = out.setupMotors(4, motor_idxs, motor_pins, 400, 950, 2000); // Standard PWM: 400Hz, 950-2000 us
115+
//bool success = out.setupMotors(4, motor_idxs, motor_pins, 2000, 125, 250); // Oneshot125: 2000Hz, 125-250 us
116+
//bool success = out.setupDshot(4, motor_idxs, motor_pins, 300); // Dshot300
117+
//bool success = out.setupDshotBidir(4, motor_idxs, motor_pins, 300); // Dshot300 Bi-Directional
118+
if(!success) madflight_die("Motor init failed.");
118119

119120
// Set initial desired yaw
120121
yaw_desired = ahr.yaw;

‎src/hal/ESP32/Dshot/Dshot.h‎

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,9 @@
1+
class Dshot {
2+
public:
3+
bool setup( int* pins, uint8_t cnt, int freq_khz = 300) {
4+
Serial.printf("ERROR: Dshot not implemented");
5+
return false;
6+
}
7+
8+
void set_throttle( uint16_t* throttle) {}
9+
};

‎src/hal/ESP32/DshotBidir/DshotBidir.h‎

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,12 @@
1+
class DshotBidir {
2+
public:
3+
bool setup( int* pins, uint8_t cnt, int freq_khz = 300) {
4+
Serial.printf("ERROR: DshotBidir not implemented");
5+
return false;
6+
}
7+
8+
void set_throttle( uint16_t* throttle) {}
9+
10+
//get erpm values, call before set_throttle. Returns negative values on error.
11+
void get_erpm( int* erpm) {}
12+
};

‎src/hal/ESP32/hal_ESP32.h‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -5,5 +5,7 @@
55

66
#include <Arduino.h>
77
#include "ESP32_PWM.h" //Servo and onshot
8+
#include "Dshot/Dshot.h"
9+
#include "DshotBidir/DshotBidir.h"
810

911
#define MF_FREERTOS_DEFAULT_STACK_SIZE 2048 //stack size on ESP32 is in bytes, not in 32bit words

‎src/hal/RP2040/Dshot/Dshot.h‎

Lines changed: 26 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,26 @@
1+
#include "DshotParallel.h"
2+
3+
class Dshot {
4+
private:
5+
DshotParallel dshot_impl;
6+
public:
7+
bool setup( int* pins, uint8_t cnt, int freq_khz = 300) {
8+
//check pins
9+
for(int i = 0; i < cnt - 1; i++) {
10+
if(pins[i] + 1 != pins[i + 1]) {
11+
Serial.printf("OUT: ERROR dshot pins should be sequential");
12+
return false;
13+
}
14+
}
15+
16+
if(!dshot_impl.begin(pins[0], cnt, freq_khz)) {
17+
Serial.printf("OUT: ERROR dshot init failed");
18+
return false;
19+
}
20+
return true;
21+
}
22+
23+
void set_throttle( uint16_t* throttle) {
24+
dshot_impl.set_throttle(throttle);
25+
}
26+
};

‎src/hal/RP2040/Dshot/DshotParallel.h‎

Lines changed: 153 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,153 @@
1+
/*==========================================================================================
2+
MIT License
3+
4+
Copyright (c) 2025 https://github.com/qqqlab
5+
6+
Permission is hereby granted, free of charge, to any person obtaining a copy
7+
of this software and associated documentation files (the "Software"), to deal
8+
in the Software without restriction, including without limitation the rights
9+
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
10+
copies of the Software, and to permit persons to whom the Software is
11+
furnished to do so, subject to the following conditions:
12+
13+
The above copyright notice and this permission notice shall be included in all
14+
copies or substantial portions of the Software.
15+
16+
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
17+
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
18+
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
19+
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
20+
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
21+
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
22+
SOFTWARE.
23+
==========================================================================================*/
24+
25+
/* DSHOT PROTOCOL
26+
27+
Available modes: Dshot150, Dshot300, Dshot600, Dshot1200. The number is the bit rate in kHz.
28+
29+
Frame Structure
30+
31+
11 bit throttle: 2048 possible values. 0 is reserved for disarmed. 1-47 are reserved for special commands. Leaving 48 to 2047 (2000 steps) for the actual throttle value
32+
1 bit telemetry request - if this is set, telemetry data is sent back via a separate channel
33+
4 bit CRC: (Cyclic Redundancy) Check to validate data (throttle and telemetry request bit)
34+
35+
Resulting in a frame with the following structure:
36+
37+
SSSSSSSSSSSTCCCC (16 bits, MSB transmitted first)
38+
39+
Pause (output low) between frames: 21 bits recommended, 2 minimum
40+
41+
Bit Structure
42+
43+
A bit period (6.667 us for Dshot150) is split in 8 equal parts. A zero-bit is encoded as 3H 5L, and a one-bit is encoded as 6H 2L.
44+
45+
*/
46+
47+
48+
#pragma once
49+
50+
#include "dshot_parallel.pio.h"
51+
52+
class DshotParallel {
53+
private:
54+
bool setup_done = false;
55+
uint8_t pin_count;
56+
PIO pio;
57+
uint sm;
58+
uint offset;
59+
uint32_t write_ts = 0; //last write timestamp
60+
61+
public:
62+
uint32_t interval_us = 0; //minimal interval between two writes in us (16 bits transmission + 21 bits pause)
63+
64+
~DshotParallel() {
65+
end();
66+
}
67+
68+
bool begin(int pin, uint8_t pin_count, int rate_khz) {
69+
end();
70+
71+
if(pin < 0 || pin_count < 1 || pin_count > 8 || rate_khz < 0) return false;
72+
73+
this->pin_count = pin_count;
74+
this->interval_us = (16 + 21) * 1000 / rate_khz;
75+
76+
// This will find a free pio and state machine for our program and load it for us
77+
// We use pio_claim_free_sm_and_add_program_for_gpio_range (for_gpio_range variant)
78+
// so we will get a PIO instance suitable for addressing gpios >= 32 if needed and supported by the hardware
79+
bool success = pio_claim_free_sm_and_add_program_for_gpio_range(&dshot_parallel_program, &pio, &sm, &offset, pin, pin_count, true);
80+
if(!success) return false;
81+
82+
dshot_parallel_program_init(pio, sm, offset, pin, pin_count, rate_khz * 1000);
83+
84+
setup_done = true;
85+
return true;
86+
}
87+
88+
void end() {
89+
if(setup_done) {
90+
setup_done = false;
91+
92+
// This will free resources and unload our program
93+
pio_remove_program_and_unclaim_sm(&dshot_parallel_program, pio, sm, offset);
94+
}
95+
}
96+
97+
//set throttle for all ESCs
98+
bool set_throttle(uint16_t *throttle) {
99+
uint16_t values[8];
100+
for(int i = 0; i < pin_count; i++) {
101+
values[i] = ( throttle[i] == 0 ? 0 : throttle[i] + 48);
102+
}
103+
return write(values);
104+
}
105+
106+
//only write if last write was more than interval_us in the past
107+
bool write(uint16_t *values) {
108+
if(micros() - write_ts < interval_us) return false;
109+
if(!write_raw(values)) return false;
110+
write_ts = micros();
111+
return true;
112+
}
113+
114+
//immediately write, don't check interval
115+
bool write_raw(uint16_t *values) {
116+
if(!setup_done) return false;
117+
118+
//convert values into frames (i.e. shift bits, add T=0 no telemetry bit, add CRC)
119+
uint16_t frames[8];
120+
for(int ch = 0; ch < pin_count; ch++) {
121+
uint16_t f = values[ch];
122+
if(f > 2047) f = 2047; //clip to max throttle
123+
f <<= 5; //shift and add T=0 no telemetry bit
124+
uint16_t crc = (f >> 4) ^ (f >> 8) ^ (f >> 12);
125+
f |= (crc & 0x0F); //add CRC
126+
frames[ch] = f;
127+
}
128+
129+
//convert frames into fifo stream
130+
uint32_t d[4] = {};
131+
for(int bit = 15; bit >= 0; bit--) {
132+
uint32_t word = (15 - bit) / 4;
133+
uint32_t shift = ((15 - bit) % 4) * 8;
134+
for(int ch=0; ch < pin_count; ch++) {
135+
uint32_t chmask = (1u << ch);
136+
if( frames[ch] & (1u << bit) ) {
137+
d[word] |= (chmask << shift);
138+
}
139+
}
140+
}
141+
142+
//send the frames to the PIO
143+
pio_sm_set_enabled(pio, sm, false); //disable PIO while filling fifos
144+
pio_sm_clear_fifos(pio, sm); //just to be sure
145+
for(int i = 0; i < 4; i++) {
146+
pio_sm_put(pio, sm, d[i]);
147+
}
148+
pio_sm_set_enabled(pio, sm, true); //enable PIO to send our frames in parallel
149+
150+
return true;
151+
}
152+
153+
};

‎src/hal/RP2040/Dshot/dshot_parallel.pio‎

Lines changed: 62 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,62 @@
1+
/*==========================================================================================
2+
MIT License
3+
4+
Copyright (c) 2025 https://github.com/qqqlab
5+
6+
Permission is hereby granted, free of charge, to any person obtaining a copy
7+
of this software and associated documentation files (the "Software"), to deal
8+
in the Software without restriction, including without limitation the rights
9+
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
10+
copies of the Software, and to permit persons to whom the Software is
11+
furnished to do so, subject to the following conditions:
12+
13+
The above copyright notice and this permission notice shall be included in all
14+
copies or substantial portions of the Software.
15+
16+
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
17+
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
18+
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
19+
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
20+
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
21+
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
22+
SOFTWARE.
23+
==========================================================================================*/
24+
25+
.pio_version 0 // only requires PIO version 0
26+
27+
.program dshot_parallel
28+
29+
.define public T1 6
30+
.define public T2 6
31+
.define public T3 4
32+
33+
.wrap_target
34+
out x, 8
35+
mov pins, !null [T1-1]
36+
mov pins, x [T2-1]
37+
mov pins, null [T3-2]
38+
.wrap
39+
40+
% c-sdk {
41+
#include "hardware/clocks.h"
42+
43+
//pin_count 1-8
44+
static inline void dshot_parallel_program_init(PIO pio, uint sm, uint offset, uint pin_base, uint pin_count, float freq) {
45+
for(uint i=pin_base; i<pin_base+pin_count; i++) {
46+
pio_gpio_init(pio, i);
47+
}
48+
pio_sm_set_consecutive_pindirs(pio, sm, pin_base, pin_count, true);
49+
50+
pio_sm_config c = dshot_parallel_program_get_default_config(offset);
51+
sm_config_set_out_shift(&c, true, true, 32);
52+
sm_config_set_out_pins(&c, pin_base, pin_count);
53+
sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_TX);
54+
55+
int cycles_per_bit = dshot_parallel_T1 + dshot_parallel_T2 + dshot_parallel_T3;
56+
float div = clock_get_hz(clk_sys) / (freq * cycles_per_bit);
57+
sm_config_set_clkdiv(&c, div);
58+
59+
pio_sm_init(pio, sm, offset, &c);
60+
pio_sm_set_enabled(pio, sm, true);
61+
}
62+
%}

‎src/hal/RP2040/Dshot/dshot_parallel.pio.h‎

Lines changed: 68 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,68 @@
1+
// -------------------------------------------------- //
2+
// This file is autogenerated by pioasm; do not edit! //
3+
// -------------------------------------------------- //
4+
5+
#pragma once
6+
7+
#if !PICO_NO_HARDWARE
8+
#include "hardware/pio.h"
9+
#endif
10+
11+
// -------------- //
12+
// dshot_parallel //
13+
// -------------- //
14+
15+
#define dshot_parallel_wrap_target 0
16+
#define dshot_parallel_wrap 3
17+
#define dshot_parallel_pio_version 0
18+
19+
#define dshot_parallel_T1 6
20+
#define dshot_parallel_T2 6
21+
#define dshot_parallel_T3 4
22+
23+
static const uint16_t dshot_parallel_program_instructions[] = {
24+
// .wrap_target
25+
0x6028, // 0: out x, 8
26+
0xa50b, // 1: mov pins, !null [5]
27+
0xa501, // 2: mov pins, x [5]
28+
0xa203, // 3: mov pins, null [2]
29+
// .wrap
30+
};
31+
32+
#if !PICO_NO_HARDWARE
33+
static const struct pio_program dshot_parallel_program = {
34+
.instructions = dshot_parallel_program_instructions,
35+
.length = 4,
36+
.origin = -1,
37+
.pio_version = 0,
38+
#if PICO_PIO_VERSION > 0
39+
.used_gpio_ranges = 0x0
40+
#endif
41+
};
42+
43+
static inline pio_sm_config dshot_parallel_program_get_default_config(uint offset) {
44+
pio_sm_config c = pio_get_default_sm_config();
45+
sm_config_set_wrap(&c, offset + dshot_parallel_wrap_target, offset + dshot_parallel_wrap);
46+
return c;
47+
}
48+
49+
#include "hardware/clocks.h"
50+
//pin_count 1-8
51+
static inline void dshot_parallel_program_init(PIO pio, uint sm, uint offset, uint pin_base, uint pin_count, float freq) {
52+
for(uint i=pin_base; i<pin_base+pin_count; i++) {
53+
pio_gpio_init(pio, i);
54+
}
55+
pio_sm_set_consecutive_pindirs(pio, sm, pin_base, pin_count, true);
56+
pio_sm_config c = dshot_parallel_program_get_default_config(offset);
57+
sm_config_set_out_shift(&c, true, true, 32);
58+
sm_config_set_out_pins(&c, pin_base, pin_count);
59+
sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_TX);
60+
int cycles_per_bit = dshot_parallel_T1 + dshot_parallel_T2 + dshot_parallel_T3;
61+
float div = clock_get_hz(clk_sys) / (freq * cycles_per_bit);
62+
sm_config_set_clkdiv(&c, div);
63+
pio_sm_init(pio, sm, offset, &c);
64+
pio_sm_set_enabled(pio, sm, true);
65+
}
66+
67+
#endif
68+

0 commit comments

Comments
 (0)