Skip to content

Commit

Permalink
dshot: add telemetry support
Browse files Browse the repository at this point in the history
  • Loading branch information
@bkleiner
bkleiner authored and bkleiner committed Aug 4, 2024
1 parent 83d0796 commit 64dcd74
Show file tree
Hide file tree
Showing 7 changed files with 420 additions and 193 deletions.
1 change: 1 addition & 0 deletions src/core/profile.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -186,6 +186,7 @@ typedef enum {
DSHOT_TIME_150 = 150,
DSHOT_TIME_300 = 300,
DSHOT_TIME_600 = 600,
DSHOT_TIME_MAX = 600,
} __attribute__((__packed__)) dshot_time_t;

typedef struct {
Expand Down
28 changes: 1 addition & 27 deletions src/driver/at32/motor_dshot.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,20 +11,6 @@

#ifdef USE_MOTOR_DSHOT

extern volatile uint32_t dshot_dma_phase;
extern uint16_t dshot_packet[MOTOR_PIN_MAX];
extern dshot_pin_t dshot_pins[MOTOR_PIN_MAX];

extern motor_direction_t motor_dir;

extern uint8_t dshot_gpio_port_count;
extern dshot_gpio_port_t dshot_gpio_ports[DSHOT_MAX_PORT_COUNT];

extern volatile DMA_RAM uint32_t port_dma_buffer[DSHOT_MAX_PORT_COUNT][DSHOT_DMA_BUFFER_SIZE];

extern void dshot_init_motor_pin(uint32_t index);
extern const dshot_gpio_port_t *dshot_gpio_for_device(const dma_device_t dev);

static void dshot_init_gpio_port(dshot_gpio_port_t *port) {
dma_enable_rcc(port->dma_device);

Expand Down Expand Up @@ -59,9 +45,7 @@ static void dshot_init_gpio_port(dshot_gpio_port_t *port) {
interrupt_enable(dma->irq, DMA_PRIORITY);
}

void motor_dshot_init() {
dshot_gpio_port_count = 0;

void dshot_init_timer() {
rcc_enable(RCC_ENCODE(TMR1));

// setup timer to 1/3 of the full bit time
Expand All @@ -70,10 +54,6 @@ void motor_dshot_init() {
tmr_cnt_dir_set(TMR1, TMR_COUNT_UP);
tmr_period_buffer_enable(TMR1, TRUE);

for (uint32_t i = 0; i < MOTOR_PIN_MAX; i++) {
dshot_init_motor_pin(i);
}

for (uint32_t j = 0; j < dshot_gpio_port_count; j++) {
dshot_init_gpio_port(&dshot_gpio_ports[j]);

Expand All @@ -85,7 +65,6 @@ void motor_dshot_init() {
}

tmr_counter_enable(TMR1, TRUE);
motor_dir = MOTOR_FORWARD;
}

void dshot_dma_setup_port(uint32_t index) {
Expand All @@ -102,11 +81,6 @@ void dshot_dma_setup_port(uint32_t index) {
timer_enable_dma_request(TIMER1, port->timer_channel, true);
}

void motor_dshot_wait_for_ready() {
while (dshot_dma_phase != 0)
__NOP();
}

void dshot_dma_isr(dma_device_t dev) {
const dma_stream_def_t *dma = &dma_stream_defs[dev];
dma_clear_flag_tc(dma);
Expand Down
208 changes: 126 additions & 82 deletions src/driver/motor_dshot.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@ typedef enum {
DIR_CHANGE_STOP,
} dir_change_state_t;

bool dshot_inverted = true;
volatile uint32_t dshot_dma_phase = 0; // 0: idle, 1 - (gpio_port_count + 1): handle port n
uint16_t dshot_packet[MOTOR_PIN_MAX]; // 16bits dshot data for 4 motors
dshot_pin_t dshot_pins[MOTOR_PIN_MAX];
Expand All @@ -41,34 +42,48 @@ dshot_gpio_port_t dshot_gpio_ports[DSHOT_MAX_PORT_COUNT] = {
motor_direction_t motor_dir = MOTOR_FORWARD;
static bool dir_change_done = true;

volatile DMA_RAM uint32_t port_dma_buffer[DSHOT_MAX_PORT_COUNT][DSHOT_DMA_BUFFER_SIZE];

extern void dshot_dma_setup_port(uint32_t index);
volatile DMA_RAM uint16_t dma_input_buffer[DSHOT_MAX_PORT_COUNT][GCR_DMA_BUFFER_SIZE];
volatile DMA_RAM uint32_t dma_output_buffer[DSHOT_MAX_PORT_COUNT][DSHOT_DMA_BUFFER_SIZE];

const dshot_gpio_port_t *dshot_gpio_for_device(const dma_device_t dev) {
return &dshot_gpio_ports[dev - DMA_DEVICE_TIM1_CH1];
}

void dshot_init_motor_pin(uint32_t index) {
gpio_config_t gpio_init;
gpio_init.mode = GPIO_OUTPUT;
gpio_init.output = GPIO_PUSHPULL;
gpio_init.drive = GPIO_DRIVE_HIGH;
gpio_init.pull = GPIO_NO_PULL;
gpio_pin_init(target.motor_pins[index], gpio_init);
void dshot_gpio_init_output(gpio_pins_t pin) {
gpio_config_t config;
config.mode = GPIO_OUTPUT;
config.output = GPIO_PUSHPULL;
config.drive = GPIO_DRIVE_HIGH;
config.pull = GPIO_NO_PULL;
gpio_pin_init(pin, config);
}

void dshot_gpio_init_input(gpio_pins_t pin) {
gpio_config_t config;
config.mode = GPIO_INPUT;
config.output = GPIO_OPENDRAIN;
config.drive = GPIO_DRIVE_HIGH;
config.pull = GPIO_NO_PULL;
gpio_pin_init(pin, config);
}

static void dshot_init_motor_pin(uint32_t index) {
dshot_gpio_init_output(target.motor_pins[index]);
gpio_pin_reset(target.motor_pins[index]);

dshot_pins[index].port = gpio_pin_defs[target.motor_pins[index]].port;
dshot_pins[index].pin = gpio_pin_defs[target.motor_pins[index]].pin;
dshot_pins[index].dshot_port = 0;
dshot_pins[index].pin = gpio_pin_defs[target.motor_pins[index]].pin;
dshot_pins[index].port = gpio_pin_defs[target.motor_pins[index]].port;
dshot_pins[index].set_mask = dshot_inverted ? (dshot_pins[index].pin << 16) : dshot_pins[index].pin;
dshot_pins[index].reset_mask = dshot_inverted ? dshot_pins[index].pin : (dshot_pins[index].pin << 16);

for (uint8_t i = 0; i < DSHOT_MAX_PORT_COUNT; i++) {
if (dshot_gpio_ports[i].gpio == dshot_pins[index].port || i == dshot_gpio_port_count) {
// we already got a matching port in our array
// or we reached the first empty spot
dshot_gpio_ports[i].gpio = dshot_pins[index].port;
dshot_gpio_ports[i].port_high |= dshot_pins[index].pin;
dshot_gpio_ports[i].port_low |= (dshot_pins[index].pin << 16);
dshot_gpio_ports[i].set_mask |= dshot_pins[index].set_mask;
dshot_gpio_ports[i].reset_mask |= dshot_pins[index].reset_mask;

dshot_pins[index].dshot_port = i;

Expand All @@ -81,7 +96,7 @@ void dshot_init_motor_pin(uint32_t index) {
}
}

void dshot_make_packet(uint8_t number, uint16_t value, bool telemetry) {
static void dshot_make_packet(uint8_t number, uint16_t value, bool telemetry) {
const uint16_t packet = (value << 1) | (telemetry ? 1 : 0);

uint16_t csum = 0;
Expand All @@ -91,114 +106,143 @@ void dshot_make_packet(uint8_t number, uint16_t value, bool telemetry) {
csum_data >>= 4;
}

dshot_packet[number] = (packet << 4) | (csum & 0xf);
if (dshot_inverted) {
csum = ~csum;
}
csum &= 0xf;

dshot_packet[number] = (packet << 4) | csum;
}

void dshot_make_packet_all(uint16_t value, bool telemetry) {
static void dshot_make_packet_all(uint16_t value, bool telemetry) {
for (uint32_t i = 0; i < MOTOR_PIN_MAX; i++) {
dshot_make_packet(profile.motor.motor_pins[i], value, telemetry);
}
}

// make dshot dma packet, then fire
void dshot_dma_start() {
if (dshot_inverted) {
LL_TIM_SetAutoReload(TIM1, DSHOT_SYMBOL_TIME);
for (uint32_t i = 0; i < MOTOR_PIN_MAX; i++) {
const uint32_t port = dshot_pins[i].dshot_port;
state.dshot_rpm[i] = dshot_decode_eRPM_telemetry_value(dshot_decode_gcr((uint16_t *)dma_input_buffer[port], dshot_pins[i].pin));
dshot_gpio_init_output(i);
}
}

for (uint8_t i = 0; i < 16; i++) {
for (uint32_t j = 0; j < dshot_gpio_port_count; j++) {
port_dma_buffer[j][i * 3 + 1] = 0; // clear middle bit
dma_output_buffer[j][i * 3 + 1] = 0; // clear middle bit
}
for (uint8_t motor = 0; motor < MOTOR_PIN_MAX; motor++) {
const uint32_t port = dshot_pins[motor].dshot_port;
const uint32_t motor_high = (dshot_pins[motor].pin);
const uint32_t motor_low = (dshot_pins[motor].pin << 16);

const bool bit = dshot_packet[motor] & 0x8000;

// for 1 hold the line high for two timeunits
// first timeunit is already applied
port_dma_buffer[port][i * 3 + 1] |= bit ? motor_high : motor_low;
const bool bit = dshot_packet[motor] & 0x8000;
const uint32_t port = dshot_pins[motor].dshot_port;
dma_output_buffer[port][i * 3 + 1] |= bit ? dshot_pins[motor].set_mask : dshot_pins[motor].reset_mask;

dshot_packet[motor] <<= 1;
}
}

dma_prepare_tx_memory((void *)port_dma_buffer, sizeof(port_dma_buffer));
dma_prepare_tx_memory((void *)dma_output_buffer, sizeof(dma_output_buffer));
dma_prepare_rx_memory((void *)dma_input_buffer, sizeof(dma_input_buffer));

#ifdef STM32F4
while (spi_dma_is_ready(SPI_PORT1) == 0)
__NOP();
#endif

dshot_dma_phase = dshot_gpio_port_count;
dshot_dma_phase = dshot_gpio_port_count * (dshot_inverted ? 2 : 1);
for (uint32_t j = 0; j < dshot_gpio_port_count; j++) {
dshot_dma_setup_port(j);
dshot_dma_setup_output(j);
}
}

void motor_dshot_write(float *values) {
if (dir_change_done) {
for (uint32_t i = 0; i < MOTOR_PIN_MAX; i++) {
uint16_t value = 0;
if (values[i] >= 0.0f) {
const float pwm = constrain(values[i], 0.0f, 1.0f);
value = mapf(pwm, 0.0f, 1.0f, 48, 2047);
} else {
value = 0;
}
dshot_make_packet(profile.motor.motor_pins[i], value, false);
static void dshot_handle_dir_change() {
static uint8_t counter = 0;
static uint32_t dir_change_time = 0;
static dir_change_state_t state = DIR_CHANGE_START;

switch (state) {
case DIR_CHANGE_START: {
if (counter < 100) {
dshot_make_packet_all(0, false);
dshot_dma_start();
counter++;
} else {
state = DIR_CHANGE_DELAY;
counter = 0;
}
dir_change_time = time_micros();
break;
}

dshot_dma_start();
} else {
static uint8_t counter = 0;
static uint32_t dir_change_time = 0;
static dir_change_state_t state = DIR_CHANGE_START;

switch (state) {
case DIR_CHANGE_START: {
if (counter < 100) {
dshot_make_packet_all(0, false);
dshot_dma_start();
counter++;
} else {
state = DIR_CHANGE_DELAY;
counter = 0;
}
dir_change_time = time_micros();
case DIR_CHANGE_DELAY:
if ((time_micros() - dir_change_time) < DSHOT_DIR_CHANGE_IDLE_TIME_US) {
break;
}
state = DIR_CHANGE_CMD;
dir_change_time = time_micros();
break;

case DIR_CHANGE_DELAY:
if ((time_micros() - dir_change_time) < DSHOT_DIR_CHANGE_IDLE_TIME_US) {
break;
}
state = DIR_CHANGE_CMD;
dir_change_time = time_micros();
case DIR_CHANGE_CMD: {
if ((time_micros() - dir_change_time) < DSHOT_DIR_CHANGE_CMD_TIME_US) {
break;
}

case DIR_CHANGE_CMD: {
if ((time_micros() - dir_change_time) < DSHOT_DIR_CHANGE_CMD_TIME_US) {
break;
}

const uint16_t value = motor_dir == MOTOR_REVERSE ? DSHOT_CMD_ROTATE_REVERSE : DSHOT_CMD_ROTATE_NORMAL;
if (counter < 10) {
dshot_make_packet_all(value, true);
dshot_dma_start();
counter++;
} else {
state = DIR_CHANGE_STOP;
counter = 0;
}
dir_change_time = time_micros();
break;
const uint16_t value = motor_dir == MOTOR_REVERSE ? DSHOT_CMD_ROTATE_REVERSE : DSHOT_CMD_ROTATE_NORMAL;
if (counter < 10) {
dshot_make_packet_all(value, true);
dshot_dma_start();
counter++;
} else {
state = DIR_CHANGE_STOP;
counter = 0;
}
dir_change_time = time_micros();
break;
}

case DIR_CHANGE_STOP:
dir_change_done = true;
state = DIR_CHANGE_START;
break;
case DIR_CHANGE_STOP:
dir_change_done = true;
state = DIR_CHANGE_START;
break;
}
}

void motor_dshot_init() {
dshot_gpio_port_count = 0;
motor_dir = MOTOR_FORWARD;
for (uint32_t i = 0; i < MOTOR_PIN_MAX; i++) {
dshot_init_motor_pin(i);
}
dshot_init_timer();
}

void motor_dshot_write(float *values) {
if (!dir_change_done) {
return dshot_handle_dir_change();
}

for (uint32_t i = 0; i < MOTOR_PIN_MAX; i++) {
uint16_t value = 0;
if (values[i] >= 0.0f) {
const float pwm = constrain(values[i], 0.0f, 1.0f);
value = mapf(pwm, 0.0f, 1.0f, 48, 2047);
} else {
value = 0;
}
dshot_make_packet(profile.motor.motor_pins[i], value, false);
}

dshot_dma_start();
}

void motor_dshot_wait_for_ready() {
while (dshot_dma_phase != 0)
__NOP();
}

void motor_dshot_set_direction(motor_direction_t dir) {
Expand Down
Loading

0 comments on commit 64dcd74

Please sign in to comment.