From 22df5bd73b3d05309cdc42fd749fc1a02f9de755 Mon Sep 17 00:00:00 2001 From: bkleiner Date: Mon, 22 Jul 2024 15:03:18 +0200 Subject: [PATCH] dshot: unify pin setup --- src/driver/at32/motor_dshot.c | 117 +++++-------------------------- src/driver/motor_dshot.c | 86 ++++++++++++++++++++++- src/driver/motor_dshot.h | 27 +++++++- src/driver/stm32/motor_dshot.c | 121 ++++----------------------------- 4 files changed, 140 insertions(+), 211 deletions(-) diff --git a/src/driver/at32/motor_dshot.c b/src/driver/at32/motor_dshot.c index cf504fda6..609ba41e4 100644 --- a/src/driver/at32/motor_dshot.c +++ b/src/driver/at32/motor_dshot.c @@ -8,40 +8,17 @@ #include "driver/gpio.h" #include "driver/interrupt.h" #include "driver/rcc.h" -#include "driver/spi.h" #ifdef USE_MOTOR_DSHOT -#define DSHOT_TIME profile.motor.dshot_time -#define DSHOT_SYMBOL_TIME (PWM_CLOCK_FREQ_HZ / (3 * DSHOT_TIME * 1000) - 1) - -#define DSHOT_MAX_PORT_COUNT 3 -#define DSHOT_DMA_BUFFER_SIZE (3 * 16) - -typedef struct { - gpio_port_t *port; - uint32_t pin; - - 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 timer_channel; - dma_device_t dma_device; -} dshot_gpio_port_t; - +extern volatile uint32_t dshot_dma_phase; extern uint16_t dshot_packet[MOTOR_PIN_MAX]; -extern motor_direction_t motor_dir; +extern dshot_pin_t dshot_pins[MOTOR_PIN_MAX]; -volatile uint32_t dshot_dma_phase = 0; // 0: idle, 1 - (gpio_port_count + 1): handle port n +extern motor_direction_t motor_dir; -static uint8_t gpio_port_count = 0; -static dshot_gpio_port_t gpio_ports[DSHOT_MAX_PORT_COUNT] = { +extern uint8_t dshot_gpio_port_count; +dshot_gpio_port_t dshot_gpio_ports[DSHOT_MAX_PORT_COUNT] = { { .timer_channel = TMR_SELECT_CHANNEL_1, .dma_device = DMA_DEVICE_TIM1_CH1, @@ -55,44 +32,11 @@ static dshot_gpio_port_t gpio_ports[DSHOT_MAX_PORT_COUNT] = { .dma_device = DMA_DEVICE_TIM1_CH4, }, }; -static volatile DMA_RAM uint32_t port_dma_buffer[DSHOT_MAX_PORT_COUNT][DSHOT_DMA_BUFFER_SIZE]; -static dshot_pin_t dshot_pins[MOTOR_PIN_MAX]; -static const dshot_gpio_port_t *dshot_gpio_for_device(const dma_device_t dev) { - return &gpio_ports[dev - DMA_DEVICE_TIM1_CH1]; -} +extern volatile DMA_RAM uint32_t port_dma_buffer[DSHOT_MAX_PORT_COUNT][DSHOT_DMA_BUFFER_SIZE]; -static 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); - gpio_pin_set(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; - - for (uint8_t i = 0; i < DSHOT_MAX_PORT_COUNT; i++) { - if (gpio_ports[i].gpio == dshot_pins[index].port || i == gpio_port_count) { - // we already got a matching port in our array - // or we reached the first empty spot - gpio_ports[i].gpio = dshot_pins[index].port; - gpio_ports[i].port_high |= dshot_pins[index].pin; - gpio_ports[i].port_low |= (dshot_pins[index].pin << 16); - - dshot_pins[index].dshot_port = i; - - if (i + 1 > gpio_port_count) { - gpio_port_count = i + 1; - } - - break; - } - } -} +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); @@ -145,7 +89,7 @@ static void dshot_enable_dma_request(uint32_t timer_channel, confirm_state new_s } void motor_dshot_init() { - gpio_port_count = 0; + dshot_gpio_port_count = 0; rcc_enable(RCC_ENCODE(TMR1)); @@ -159,13 +103,13 @@ void motor_dshot_init() { dshot_init_motor_pin(i); } - for (uint32_t j = 0; j < gpio_port_count; j++) { - dshot_init_gpio_port(&gpio_ports[j]); + 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 < 16; i++) { - port_dma_buffer[j][i * 3 + 0] = 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] = gpio_ports[j].port_low; // return line to low + 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 } } @@ -173,8 +117,8 @@ void motor_dshot_init() { motor_dir = MOTOR_FORWARD; } -static void dshot_dma_setup_port(uint32_t index) { - const dshot_gpio_port_t *port = &gpio_ports[index]; +void dshot_dma_setup_port(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); @@ -187,35 +131,6 @@ static void dshot_dma_setup_port(uint32_t index) { dshot_enable_dma_request(port->timer_channel, TRUE); } -// make dshot dma packet, then fire -void dshot_dma_start() { - for (uint8_t i = 0; i < 16; i++) { - for (uint32_t j = 0; j < gpio_port_count; j++) { - port_dma_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; - - dshot_packet[motor] <<= 1; - } - } - - dma_prepare_tx_memory((void *)port_dma_buffer, sizeof(port_dma_buffer)); - - dshot_dma_phase = gpio_port_count; - for (uint32_t j = 0; j < gpio_port_count; j++) { - dshot_dma_setup_port(j); - } -} - void motor_dshot_wait_for_ready() { while (dshot_dma_phase != 0) __NOP(); diff --git a/src/driver/motor_dshot.c b/src/driver/motor_dshot.c index a1eb5df76..34ee235cf 100644 --- a/src/driver/motor_dshot.c +++ b/src/driver/motor_dshot.c @@ -2,6 +2,9 @@ #include "core/profile.h" #include "core/project.h" +#include "driver/dma.h" +#include "driver/gpio.h" +#include "driver/spi.h" #include "driver/time.h" #include "flight/control.h" #include "util/util.h" @@ -15,12 +18,55 @@ typedef enum { DIR_CHANGE_STOP, } dir_change_state_t; -uint16_t dshot_packet[MOTOR_PIN_MAX]; // 16bits dshot data for 4 motors -motor_direction_t motor_dir = MOTOR_FORWARD; +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]; + +uint8_t dshot_gpio_port_count = 0; +extern dshot_gpio_port_t dshot_gpio_ports[DSHOT_MAX_PORT_COUNT]; +motor_direction_t motor_dir = MOTOR_FORWARD; static bool dir_change_done = true; -extern void dshot_dma_start(); +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); + +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); + gpio_pin_set(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; + + 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_pins[index].dshot_port = i; + + if (i + 1 > dshot_gpio_port_count) { + dshot_gpio_port_count = i + 1; + } + + break; + } + } +} void dshot_make_packet(uint8_t number, uint16_t value, bool telemetry) { const uint16_t packet = (value << 1) | (telemetry ? 1 : 0); @@ -41,6 +87,40 @@ void dshot_make_packet_all(uint16_t value, bool telemetry) { } } +// make dshot dma packet, then fire +void dshot_dma_start() { + 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 + } + 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; + + dshot_packet[motor] <<= 1; + } + } + + dma_prepare_tx_memory((void *)port_dma_buffer, sizeof(port_dma_buffer)); + +#ifdef STM32F4 + while (spi_dma_is_ready(SPI_PORT1) == 0) + __NOP(); +#endif + + dshot_dma_phase = dshot_gpio_port_count; + for (uint32_t j = 0; j < dshot_gpio_port_count; j++) { + dshot_dma_setup_port(j); + } +} + void motor_dshot_write(float *values) { if (dir_change_done) { for (uint32_t i = 0; i < MOTOR_PIN_MAX; i++) { diff --git a/src/driver/motor_dshot.h b/src/driver/motor_dshot.h index 40bfda018..078d589b4 100644 --- a/src/driver/motor_dshot.h +++ b/src/driver/motor_dshot.h @@ -1,7 +1,26 @@ #pragma once +#include "driver/dma.h" +#include "driver/gpio.h" #include "driver/motor.h" +typedef struct { + gpio_port_t *port; + uint32_t pin; + + 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 timer_channel; + dma_device_t dma_device; +} dshot_gpio_port_t; + #define DSHOT_CMD_BEEP1 1 #define DSHOT_CMD_BEEP2 2 #define DSHOT_CMD_BEEP3 3 @@ -12,4 +31,10 @@ #define DSHOT_CMD_ROTATE_REVERSE 21 #define DSHOT_DIR_CHANGE_IDLE_TIME_US 10000 -#define DSHOT_DIR_CHANGE_CMD_TIME_US 1000 \ No newline at end of file +#define DSHOT_DIR_CHANGE_CMD_TIME_US 1000 + +#define DSHOT_TIME profile.motor.dshot_time +#define DSHOT_SYMBOL_TIME (PWM_CLOCK_FREQ_HZ / (3 * DSHOT_TIME * 1000) - 1) + +#define DSHOT_MAX_PORT_COUNT 3 +#define DSHOT_DMA_BUFFER_SIZE (3 * 16) \ No newline at end of file diff --git a/src/driver/stm32/motor_dshot.c b/src/driver/stm32/motor_dshot.c index 6c3047802..c3c00f5e3 100644 --- a/src/driver/stm32/motor_dshot.c +++ b/src/driver/stm32/motor_dshot.c @@ -8,40 +8,17 @@ #include "driver/gpio.h" #include "driver/interrupt.h" #include "driver/rcc.h" -#include "driver/spi.h" #ifdef USE_MOTOR_DSHOT -#define DSHOT_TIME profile.motor.dshot_time -#define DSHOT_SYMBOL_TIME (PWM_CLOCK_FREQ_HZ / (3 * DSHOT_TIME * 1000) - 1) - -#define DSHOT_MAX_PORT_COUNT 3 -#define DSHOT_DMA_BUFFER_SIZE (3 * 16) - -typedef struct { - gpio_port_t *port; - uint32_t pin; - - 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 timer_channel; - dma_device_t dma_device; -} dshot_gpio_port_t; - +extern volatile uint32_t dshot_dma_phase; extern uint16_t dshot_packet[MOTOR_PIN_MAX]; -extern motor_direction_t motor_dir; +extern dshot_pin_t dshot_pins[MOTOR_PIN_MAX]; -volatile uint32_t dshot_dma_phase = 0; // 0: idle, 1 - (gpio_port_count + 1): handle port n +extern motor_direction_t motor_dir; -static uint8_t gpio_port_count = 0; -static dshot_gpio_port_t gpio_ports[DSHOT_MAX_PORT_COUNT] = { +extern uint8_t dshot_gpio_port_count; +dshot_gpio_port_t dshot_gpio_ports[DSHOT_MAX_PORT_COUNT] = { { .timer_channel = LL_TIM_CHANNEL_CH1, .dma_device = DMA_DEVICE_TIM1_CH1, @@ -56,43 +33,9 @@ static dshot_gpio_port_t gpio_ports[DSHOT_MAX_PORT_COUNT] = { }, }; static volatile DMA_RAM uint32_t port_dma_buffer[DSHOT_MAX_PORT_COUNT][DSHOT_DMA_BUFFER_SIZE]; -static dshot_pin_t dshot_pins[MOTOR_PIN_MAX]; -static const dshot_gpio_port_t *dshot_gpio_for_device(const dma_device_t dev) { - return &gpio_ports[dev - DMA_DEVICE_TIM1_CH1]; -} - -static 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); - gpio_pin_set(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; - - for (uint8_t i = 0; i < DSHOT_MAX_PORT_COUNT; i++) { - if (gpio_ports[i].gpio == dshot_pins[index].port || i == gpio_port_count) { - // we already got a matching port in our array - // or we reached the first empty spot - gpio_ports[i].gpio = dshot_pins[index].port; - gpio_ports[i].port_high |= dshot_pins[index].pin; - gpio_ports[i].port_low |= (dshot_pins[index].pin << 16); - - dshot_pins[index].dshot_port = i; - - if (i + 1 > gpio_port_count) { - gpio_port_count = i + 1; - } - - break; - } - } -} +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; @@ -172,7 +115,7 @@ static void dshot_disable_dma_request(uint32_t timer_channel) { } void motor_dshot_init() { - gpio_port_count = 0; + dshot_gpio_port_count = 0; rcc_enable(RCC_APB2_GRP1(TIM1)); @@ -191,13 +134,13 @@ void motor_dshot_init() { dshot_init_motor_pin(i); } - for (uint32_t j = 0; j < gpio_port_count; j++) { - dshot_init_gpio_port(&gpio_ports[j]); + 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 < 16; i++) { - port_dma_buffer[j][i * 3 + 0] = 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] = gpio_ports[j].port_low; // return line to low + 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 } } @@ -205,8 +148,8 @@ void motor_dshot_init() { motor_dir = MOTOR_FORWARD; } -static void dshot_dma_setup_port(uint32_t index) { - const dshot_gpio_port_t *port = &gpio_ports[index]; +void dshot_dma_setup_port(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); @@ -219,40 +162,6 @@ static void dshot_dma_setup_port(uint32_t index) { dshot_enable_dma_request(port->timer_channel); } -// make dshot dma packet, then fire -void dshot_dma_start() { - for (uint8_t i = 0; i < 16; i++) { - for (uint32_t j = 0; j < gpio_port_count; j++) { - port_dma_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; - - dshot_packet[motor] <<= 1; - } - } - - dma_prepare_tx_memory((void *)port_dma_buffer, sizeof(port_dma_buffer)); - -#ifdef STM32F4 - while (spi_dma_is_ready(SPI_PORT1) == 0) - __NOP(); -#endif - - dshot_dma_phase = gpio_port_count; - for (uint32_t j = 0; j < gpio_port_count; j++) { - dshot_dma_setup_port(j); - } -} - void motor_dshot_wait_for_ready() { while (dshot_dma_phase != 0) __NOP();