From 9ba79d3b2a659599866b937ae482bd06c70b7984 Mon Sep 17 00:00:00 2001 From: bkleiner Date: Sun, 28 Jan 2024 12:56:34 +0100 Subject: [PATCH] dshot: add telemetry support --- src/core/profile.h | 1 + src/driver/mcu/at32/motor_dshot.c | 28 +--- src/driver/mcu/stm32/motor_dshot.c | 69 +++++----- src/driver/motor_dshot.c | 208 +++++++++++++++++------------ src/driver/motor_dshot.h | 44 +++++- src/driver/motor_dshot_gcr.c | 166 +++++++++++++++++++++++ src/flight/control.h | 97 +++++++------- 7 files changed, 420 insertions(+), 193 deletions(-) create mode 100644 src/driver/motor_dshot_gcr.c diff --git a/src/core/profile.h b/src/core/profile.h index ad4d653ce..5fe6d72cc 100644 --- a/src/core/profile.h +++ b/src/core/profile.h @@ -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 { diff --git a/src/driver/mcu/at32/motor_dshot.c b/src/driver/mcu/at32/motor_dshot.c index beb8c2537..3e4ca6ddc 100644 --- a/src/driver/mcu/at32/motor_dshot.c +++ b/src/driver/mcu/at32/motor_dshot.c @@ -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); @@ -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 @@ -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]); @@ -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) { @@ -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); diff --git a/src/driver/mcu/stm32/motor_dshot.c b/src/driver/mcu/stm32/motor_dshot.c index c42d37ed1..9a2d63c82 100644 --- a/src/driver/mcu/stm32/motor_dshot.c +++ b/src/driver/mcu/stm32/motor_dshot.c @@ -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) { LL_TIM_OC_InitTypeDef tim_oc_init; LL_TIM_OC_StructInit(&tim_oc_init); @@ -48,8 +34,8 @@ static void dshot_init_gpio_port(dshot_gpio_port_t *port) { #else DMA_InitStructure.Channel = dma->channel; #endif - DMA_InitStructure.PeriphOrM2MSrcAddress = (uint32_t)&port->gpio->BSRR; - DMA_InitStructure.MemoryOrM2MDstAddress = (uint32_t)port_dma_buffer[0]; + DMA_InitStructure.PeriphOrM2MSrcAddress = 0x0; // overwritten later + DMA_InitStructure.MemoryOrM2MDstAddress = 0x0; // overwritten later DMA_InitStructure.Direction = LL_DMA_DIRECTION_MEMORY_TO_PERIPH; DMA_InitStructure.NbData = DSHOT_DMA_BUFFER_SIZE; DMA_InitStructure.PeriphOrM2MSrcIncMode = LL_DMA_PERIPH_NOINCREMENT; @@ -70,9 +56,7 @@ static void dshot_init_gpio_port(dshot_gpio_port_t *port) { LL_DMA_EnableIT_TC(dma->port, dma->stream_index); } -void motor_dshot_init() { - dshot_gpio_port_count = 0; - +void dshot_init_timer() { rcc_enable(RCC_APB2_GRP1(TIM1)); // setup timer to 1/3 of the full bit time @@ -86,41 +70,47 @@ void motor_dshot_init() { LL_TIM_EnableARRPreload(TIM1); LL_TIM_DisableMasterSlaveMode(TIM1); - 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]); for (uint8_t i = 0; i < DSHOT_DMA_SYMBOLS; i++) { - port_dma_buffer[j][i * 3 + 0] = dshot_gpio_ports[j].port_high; // start bit - port_dma_buffer[j][i * 3 + 1] = 0; // actual bit, set below - port_dma_buffer[j][i * 3 + 2] = dshot_gpio_ports[j].port_low; // return line to low + dma_output_buffer[j][i * 3 + 0] = dshot_gpio_ports[j].set_mask; // start bit + dma_output_buffer[j][i * 3 + 1] = 0; // actual bit, set below + dma_output_buffer[j][i * 3 + 2] = dshot_gpio_ports[j].reset_mask; // return line to low } } LL_TIM_EnableCounter(TIM1); - motor_dir = MOTOR_FORWARD; } -void dshot_dma_setup_port(uint32_t index) { +void dshot_dma_setup_output(uint32_t index) { const dshot_gpio_port_t *port = &dshot_gpio_ports[index]; const dma_stream_def_t *dma = &dma_stream_defs[port->dma_device]; - dma_clear_flag_tc(dma); - LL_DMA_SetPeriphAddress(dma->port, dma->stream_index, (uint32_t)&port->gpio->BSRR); - LL_DMA_SetMemoryAddress(dma->port, dma->stream_index, (uint32_t)&port_dma_buffer[index][0]); + LL_DMA_SetMemoryAddress(dma->port, dma->stream_index, (uint32_t)&dma_output_buffer[index][0]); LL_DMA_SetDataLength(dma->port, dma->stream_index, DSHOT_DMA_BUFFER_SIZE); + LL_DMA_SetDataTransferDirection(dma->port, dma->stream_index, LL_DMA_DIRECTION_MEMORY_TO_PERIPH); + LL_DMA_SetPeriphSize(dma->port, dma->stream_index, LL_DMA_PDATAALIGN_WORD); + LL_DMA_SetMemorySize(dma->port, dma->stream_index, LL_DMA_MDATAALIGN_WORD); LL_DMA_EnableStream(dma->port, dma->stream_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_setup_input(uint32_t index) { + const dshot_gpio_port_t *port = &dshot_gpio_ports[index]; + const dma_stream_def_t *dma = &dma_stream_defs[port->dma_device]; + + LL_DMA_SetPeriphAddress(dma->port, dma->stream_index, (uint32_t)&port->gpio->IDR); + LL_DMA_SetMemoryAddress(dma->port, dma->stream_index, (uint32_t)&dma_input_buffer[index][0]); + LL_DMA_SetDataLength(dma->port, dma->stream_index, GCR_DMA_BUFFER_SIZE); + LL_DMA_SetDataTransferDirection(dma->port, dma->stream_index, LL_DMA_DIRECTION_PERIPH_TO_MEMORY); + LL_DMA_SetPeriphSize(dma->port, dma->stream_index, LL_DMA_PDATAALIGN_HALFWORD); + LL_DMA_SetMemorySize(dma->port, dma->stream_index, LL_DMA_MDATAALIGN_HALFWORD); + + LL_DMA_EnableStream(dma->port, dma->stream_index); + timer_enable_dma_request(TIMER1, port->timer_channel, true); } void dshot_dma_isr(dma_device_t dev) { @@ -132,5 +122,16 @@ void dshot_dma_isr(dma_device_t dev) { timer_enable_dma_request(TIMER1, port->timer_channel, false); dshot_dma_phase--; + + if (dshot_dma_phase == dshot_gpio_port_count) { + // output phase done, lets swap to input + LL_TIM_SetAutoReload(TIM1, GCR_SYMBOL_TIME); + for (uint32_t i = 0; i < MOTOR_PIN_MAX; i++) { + dshot_gpio_init_input(i); + } + for (uint32_t j = 0; j < dshot_gpio_port_count; j++) { + dshot_dma_setup_input(j); + } + } } #endif \ No newline at end of file diff --git a/src/driver/motor_dshot.c b/src/driver/motor_dshot.c index 8f1df301f..e1495fac6 100644 --- a/src/driver/motor_dshot.c +++ b/src/driver/motor_dshot.c @@ -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]; @@ -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; @@ -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; @@ -91,10 +106,15 @@ 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); } @@ -102,103 +122,127 @@ void dshot_make_packet_all(uint16_t value, bool 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) { diff --git a/src/driver/motor_dshot.h b/src/driver/motor_dshot.h index 67c6a61a7..4b58c3c5b 100644 --- a/src/driver/motor_dshot.h +++ b/src/driver/motor_dshot.h @@ -1,5 +1,6 @@ #pragma once +#include "core/profile.h" #include "driver/dma.h" #include "driver/gpio.h" #include "driver/motor.h" @@ -8,14 +9,17 @@ typedef struct { gpio_port_t *port; uint32_t pin; + uint32_t set_mask; + uint32_t reset_mask; + uint32_t dshot_port; } dshot_pin_t; typedef struct { gpio_port_t *gpio; - uint32_t port_low; // motor pins for BSRRL, for setting pins low - uint32_t port_high; // motor pins for BSRRH, for setting pins high + uint32_t set_mask; + uint32_t reset_mask; timer_channel_t timer_channel; dma_device_t dma_device; @@ -37,5 +41,39 @@ typedef struct { #define DSHOT_SYMBOL_TIME (PWM_CLOCK_FREQ_HZ / (3 * DSHOT_TIME * 1000) - 1) #define DSHOT_MAX_PORT_COUNT 3 + #define DSHOT_DMA_SYMBOLS (16) -#define DSHOT_DMA_BUFFER_SIZE (3 * DSHOT_DMA_SYMBOLS) \ No newline at end of file +#define DSHOT_DMA_BUFFER_SIZE (3 * DSHOT_DMA_SYMBOLS) + +#define GCR_TIME ((DSHOT_TIME * 5) / 4) +#define GCR_SYMBOL_TIME (PWM_CLOCK_FREQ_HZ / (3 * GCR_TIME * 1000 - 1)) + +// 30us delay, 5us buffer, 21 bits for worst case +#define GCR_SYMBOL_TIME_MAX (((DSHOT_TIME_MAX * 5) / 4) * 3) +#define GCR_DMA_BUFFER_SIZE (((30 + 5) * GCR_SYMBOL_TIME_MAX) / 1000 + 21 * 3) + +extern bool dshot_inverted; +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 uint16_t dma_input_buffer[DSHOT_MAX_PORT_COUNT][GCR_DMA_BUFFER_SIZE]; +extern 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); + +void dshot_init_timer(); + +void dshot_gpio_init_output(gpio_pins_t pin); +void dshot_dma_setup_output(uint32_t index); + +void dshot_gpio_init_input(gpio_pins_t pin); +void dshot_dma_setup_input(uint32_t index); + +uint32_t dshot_decode_eRPM_telemetry_value(uint16_t value); +uint32_t dshot_decode_gcr(uint16_t *dma_buffer, uint32_t pin_mask); diff --git a/src/driver/motor_dshot_gcr.c b/src/driver/motor_dshot_gcr.c new file mode 100644 index 000000000..cb24bbf65 --- /dev/null +++ b/src/driver/motor_dshot_gcr.c @@ -0,0 +1,166 @@ +#include "motor_dshot.h" + +#define DSHOT_BITBAND + +typedef struct { + uint32_t value; + uint32_t _other[15]; +} dshot_bitband_t; + +#define BITBAND_SRAM_REF 0x20000000 +#define BITBAND_SRAM_BASE 0x22000000 +#define BITBAND_SRAM(a, b) ((BITBAND_SRAM_BASE + (((a) - BITBAND_SRAM_REF) << 5) + ((b) << 2))) + +#define GCR_INVALID 0xffffffff +#define GCR_MIN_VALID_COUNT ((21 - 2) * 3) // 57 +#define GCR_MAX_VALID_COUNT ((21 + 2) * 3) // 69 + +static const uint32_t gcr_dict[32] = { + GCR_INVALID, + GCR_INVALID, + GCR_INVALID, + GCR_INVALID, + GCR_INVALID, + GCR_INVALID, + GCR_INVALID, + GCR_INVALID, + GCR_INVALID, + 9, + 10, + 11, + GCR_INVALID, + 13, + 14, + 15, + GCR_INVALID, + GCR_INVALID, + 2, + 3, + GCR_INVALID, + 5, + 6, + 7, + GCR_INVALID, + 0, + 8, + 1, + GCR_INVALID, + 4, + 12, + GCR_INVALID, +}; + +uint32_t dshot_decode_eRPM_telemetry_value(uint16_t value) { + // eRPM range + if (value == 0x0fff) { + return 0; + } + + // Convert value to 16 bit from the GCR telemetry format (eeem mmmm mmmm) + value = (value & 0x01ff) << ((value & 0xfe00) >> 9); + if (!value) { + return 0; + } + + // Convert period to erpm * 100 + return (1000000 * 60 / 100 + value / 2) / value; +} + +uint32_t dshot_decode_gcr(uint16_t *dma_buffer, uint32_t pin_mask) { +#ifdef DSHOT_BITBAND + const dshot_bitband_t *buf = (dshot_bitband_t *)BITBAND_SRAM((uint32_t)dma_buffer, POSITION_VAL(pin_mask)); +#define pin_type dshot_bitband_t +#define pin_value ((ptr++)->value) +#else + const uint16_t *buf = dma_buffer; +#define pin_type uint16_t +#define pin_value (*ptr++ & pin_mask) +#endif + + const pin_type *ptr = buf + 10; + const pin_type *ptr_end = buf + (GCR_DMA_BUFFER_SIZE - GCR_MIN_VALID_COUNT); + + // find falling edge + while (ptr < ptr_end) { + if (pin_value == 0 || + pin_value == 0 || + pin_value == 0 || + pin_value == 0) { + break; + } + } + if (ptr >= ptr_end) { + return 0; + } + + ptr_end = ptr + GCR_MAX_VALID_COUNT; + + uint32_t value = 0; + uint32_t bit_len = 0; + const pin_type *last_ptr = ptr; + while (ptr < ptr_end) { + // find rising edge + while (ptr < ptr_end) { + if (pin_value != 0 || + pin_value != 0 || + pin_value != 0 || + pin_value != 0) { + break; + } + } + if (ptr >= ptr_end) { + break; + } + { + const uint32_t len = max((ptr - last_ptr + 1) / 3, 1); + value <<= len; + last_ptr = ptr; + bit_len += len; + } + + // find falling edge + while (ptr < ptr_end) { + if (pin_value == 0 || + pin_value == 0 || + pin_value == 0 || + pin_value == 0) { + break; + } + } + if (ptr >= ptr_end) { + break; + } + { + const uint32_t len = max((ptr - last_ptr + 1) / 3, 1); + value <<= len; + value |= (0x1 << len) - 1; + last_ptr = ptr; + bit_len += len; + } + } + + if (bit_len < 18 || bit_len > 21) { + return 0; + } + + const uint32_t fill_len = (21 - bit_len); + value <<= fill_len; + value |= (0x1 << fill_len) - 1; + value = (value ^ (value >> 1)); + + const uint32_t decoded = + gcr_dict[(value >> 0) & 0x1f] | + gcr_dict[(value >> 5) & 0x1f] << 4 | + gcr_dict[(value >> 10) & 0x1f] << 8 | + gcr_dict[(value >> 15) & 0x1f] << 12; + + uint32_t csum = decoded; + csum = csum ^ (csum >> 8); + csum = csum ^ (csum >> 4); + + if ((csum & 0xf) != 0xf || decoded > 0xffff) { + return 0; + } + + return decoded >> 4; +} \ No newline at end of file diff --git a/src/flight/control.h b/src/flight/control.h index ade71fd0d..228927956 100644 --- a/src/flight/control.h +++ b/src/flight/control.h @@ -107,55 +107,58 @@ typedef struct { vec4_t motor_mix; float angleerror[ANGLE_PID_SIZE]; + + uint32_t dshot_rpm[4]; } control_state_t; -#define STATE_MEMBERS \ - MEMBER(failloop, uint8_t) \ - MEMBER(looptime, float) \ - MEMBER(looptime_us, float) \ - MEMBER(looptime_autodetect, float) \ - MEMBER(timefactor, float) \ - MEMBER(loop_counter, uint32_t) \ - MEMBER(uptime, float) \ - MEMBER(armtime, float) \ - MEMBER(cpu_load, uint32_t) \ - MEMBER(failsafe_time_ms, uint32_t) \ - MEMBER(lipo_cell_count, uint8_t) \ - MEMBER(cpu_temp, float) \ - MEMBER(vbat, float) \ - MEMBER(vbat_filtered, float) \ - MEMBER(vbat_filtered_decay, float) \ - MEMBER(vbat_cell_avg, float) \ - MEMBER(vbat_compensated, float) \ - MEMBER(vbat_compensated_cell_avg, float) \ - MEMBER(ibat, float) \ - MEMBER(ibat_filtered, float) \ - MEMBER(ibat_drawn, float) \ - MEMBER(rx, vec4_t) \ - MEMBER(rx_filtered, vec4_t) \ - MEMBER(rx_override, vec4_t) \ - MEMBER(stick_calibration_wizard, uint8_t) \ - MEMBER(rx_rssi, float) \ - MEMBER(rx_status, uint32_t) \ - MEMBER(throttle, float) \ - MEMBER(thrsum, float) \ - ARRAY_MEMBER(aux, AUX_CHANNEL_MAX, uint8_t) \ - MEMBER(accel_raw, vec3_t) \ - MEMBER(accel, vec3_t) \ - MEMBER(gyro_temp, float) \ - MEMBER(gyro_raw, vec3_t) \ - MEMBER(gyro, vec3_t) \ - MEMBER(gyro_delta_angle, vec3_t) \ - MEMBER(GEstG, vec3_t) \ - MEMBER(attitude, vec3_t) \ - MEMBER(setpoint, vec3_t) \ - MEMBER(error, vec3_t) \ - MEMBER(pid_p_term, vec3_t) \ - MEMBER(pid_i_term, vec3_t) \ - MEMBER(pid_d_term, vec3_t) \ - MEMBER(pidoutput, vec3_t) \ - MEMBER(motor_mix, vec4_t) \ - ARRAY_MEMBER(angleerror, ANGLE_PID_SIZE, float) +#define STATE_MEMBERS \ + MEMBER(failloop, uint8_t) \ + MEMBER(looptime, float) \ + MEMBER(looptime_us, float) \ + MEMBER(looptime_autodetect, float) \ + MEMBER(timefactor, float) \ + MEMBER(loop_counter, uint32_t) \ + MEMBER(uptime, float) \ + MEMBER(armtime, float) \ + MEMBER(cpu_load, uint32_t) \ + MEMBER(failsafe_time_ms, uint32_t) \ + MEMBER(lipo_cell_count, uint8_t) \ + MEMBER(cpu_temp, float) \ + MEMBER(vbat, float) \ + MEMBER(vbat_filtered, float) \ + MEMBER(vbat_filtered_decay, float) \ + MEMBER(vbat_cell_avg, float) \ + MEMBER(vbat_compensated, float) \ + MEMBER(vbat_compensated_cell_avg, float) \ + MEMBER(ibat, float) \ + MEMBER(ibat_filtered, float) \ + MEMBER(ibat_drawn, float) \ + MEMBER(rx, vec4_t) \ + MEMBER(rx_filtered, vec4_t) \ + MEMBER(rx_override, vec4_t) \ + MEMBER(stick_calibration_wizard, uint8_t) \ + MEMBER(rx_rssi, float) \ + MEMBER(rx_status, uint32_t) \ + MEMBER(throttle, float) \ + MEMBER(thrsum, float) \ + ARRAY_MEMBER(aux, AUX_CHANNEL_MAX, uint8_t) \ + MEMBER(accel_raw, vec3_t) \ + MEMBER(accel, vec3_t) \ + MEMBER(gyro_temp, float) \ + MEMBER(gyro_raw, vec3_t) \ + MEMBER(gyro, vec3_t) \ + MEMBER(gyro_delta_angle, vec3_t) \ + MEMBER(GEstG, vec3_t) \ + MEMBER(attitude, vec3_t) \ + MEMBER(setpoint, vec3_t) \ + MEMBER(error, vec3_t) \ + MEMBER(pid_p_term, vec3_t) \ + MEMBER(pid_i_term, vec3_t) \ + MEMBER(pid_d_term, vec3_t) \ + MEMBER(pidoutput, vec3_t) \ + MEMBER(motor_mix, vec4_t) \ + ARRAY_MEMBER(angleerror, ANGLE_PID_SIZE, float) \ + ARRAY_MEMBER(dshot_rpm, 4, uint32_t) typedef struct { uint8_t active;