Skip to content

Commit

Permalink
dshot: unify pin setup
Browse files Browse the repository at this point in the history
  • Loading branch information
bkleiner committed Jul 22, 2024
1 parent 95bd585 commit 22df5bd
Show file tree
Hide file tree
Showing 4 changed files with 140 additions and 211 deletions.
117 changes: 16 additions & 101 deletions src/driver/at32/motor_dshot.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -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);
Expand Down Expand Up @@ -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));

Expand All @@ -159,22 +103,22 @@ 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
}
}

tmr_counter_enable(TMR1, TRUE);
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);
Expand All @@ -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();
Expand Down
86 changes: 83 additions & 3 deletions src/driver/motor_dshot.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand All @@ -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);
Expand All @@ -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++) {
Expand Down
27 changes: 26 additions & 1 deletion src/driver/motor_dshot.h
View file
Original file line number Diff line number Diff line change
@@ -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
Expand All @@ -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
#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)
Loading

0 comments on commit 22df5bd

Please sign in to comment.