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 11, 2024
1 parent 8020d0e commit 7d8b599
Show file tree
Hide file tree
Showing 8 changed files with 463 additions and 203 deletions.
1 change: 1 addition & 0 deletions src/core/profile.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -140,6 +140,7 @@ const profile_t default_profile = {
.motor_limit = MOTOR_LIMIT,
.digital_idle = DIGITAL_IDLE,
.dshot_time = DSHOT_TIME_600,
.dshot_inverted = false,

#ifdef TORQUE_BOOST
.torque_boost = TORQUE_BOOST,
Expand Down
3 changes: 3 additions & 0 deletions src/core/profile.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -186,12 +186,14 @@ 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 {
float digital_idle;
float motor_limit;
dshot_time_t dshot_time;
bool dshot_inverted;
uint8_t invert_yaw;
uint8_t gyro_orientation;
float torque_boost;
Expand All @@ -205,6 +207,7 @@ typedef struct {
MEMBER(digital_idle, float) \
MEMBER(motor_limit, float) \
MEMBER(dshot_time, uint16_t) \
MEMBER(dshot_inverted, bool) \
MEMBER(invert_yaw, uint8_t) \
MEMBER(gyro_orientation, uint8_t) \
MEMBER(torque_boost, float) \
Expand Down
74 changes: 39 additions & 35 deletions src/driver/mcu/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 All @@ -43,8 +29,8 @@ static void dshot_init_gpio_port(dshot_gpio_port_t *port) {
dmamux_init(dma->mux, dma->request);

dma_init_type init;
init.peripheral_base_addr = (uint32_t)(&port->gpio->scr);
init.memory_base_addr = (uint32_t)port_dma_buffer[0];
init.peripheral_base_addr = 0x0; // overwritten later
init.memory_base_addr = 0x0; // overwritten later
init.direction = DMA_DIR_MEMORY_TO_PERIPHERAL;
init.buffer_size = DSHOT_DMA_BUFFER_SIZE;
init.peripheral_inc_enable = FALSE;
Expand All @@ -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,41 +54,51 @@ 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]);

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
}
}

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

void dshot_dma_setup_port(uint32_t index) {
void dshot_dma_setup_output(uint32_t index) {
tmr_period_value_set(TMR1, DSHOT_SYMBOL_TIME);

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);

dma->channel->ctrl_bit.dtd = 1; // DMA_DIR_MEMORY_TO_PERIPHERAL
dma->channel->ctrl_bit.mwidth = DMA_MEMORY_DATA_WIDTH_WORD;
dma->channel->ctrl_bit.pwidth = DMA_PERIPHERAL_DATA_WIDTH_WORD;
dma->channel->paddr = (uint32_t)(&port->gpio->scr);
dma->channel->maddr = (uint32_t)&port_dma_buffer[index][0];
dma->channel->dtcnt = DSHOT_DMA_BUFFER_SIZE;
dma->channel->maddr = (uint32_t)(&dma_output_buffer[index][0]);
dma->channel->dtcnt_bit.cnt = DSHOT_DMA_BUFFER_SIZE;

dma_channel_enable(dma->channel, TRUE);
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) {
tmr_period_value_set(TMR1, GCR_SYMBOL_TIME);

const dshot_gpio_port_t *port = &dshot_gpio_ports[index];
const dma_stream_def_t *dma = &dma_stream_defs[port->dma_device];

dma->channel->ctrl_bit.dtd = 0; // DMA_DIR_PERIPHERAL_TO_MEMORY
dma->channel->ctrl_bit.mwidth = DMA_MEMORY_DATA_WIDTH_HALFWORD;
dma->channel->ctrl_bit.pwidth = DMA_PERIPHERAL_DATA_WIDTH_HALFWORD;
dma->channel->paddr = (uint32_t)(&port->gpio->idt);
dma->channel->maddr = (uint32_t)(&dma_input_buffer[index][0]);
dma->channel->dtcnt_bit.cnt = GCR_DMA_BUFFER_SIZE;

dma_channel_enable(dma->channel, TRUE);
timer_enable_dma_request(TIMER1, port->timer_channel, true);
}

void dshot_dma_isr(dma_device_t dev) {
Expand All @@ -116,5 +110,15 @@ void dshot_dma_isr(dma_device_t dev) {
timer_enable_dma_request(TIMER1, port->timer_channel, false);

dshot_dma_phase--;

if (profile.motor.dshot_inverted && dshot_dma_phase == dshot_gpio_port_count) {
// output phase done, lets swap to input
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
75 changes: 39 additions & 36 deletions src/driver/mcu/stm32/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) {
LL_TIM_OC_InitTypeDef tim_oc_init;
LL_TIM_OC_StructInit(&tim_oc_init);
Expand All @@ -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;
Expand All @@ -64,15 +50,12 @@ static void dshot_init_gpio_port(dshot_gpio_port_t *port) {
DMA_InitStructure.PeriphBurst = LL_DMA_PBURST_SINGLE;
#endif
LL_DMA_Init(dma->port, dma->stream_index, &DMA_InitStructure);
LL_DMA_EnableIT_TC(dma->port, dma->stream_index);

interrupt_enable(dma->irq, DMA_PRIORITY);

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
Expand All @@ -86,41 +69,51 @@ 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) {
LL_TIM_SetAutoReload(TIM1, DSHOT_SYMBOL_TIME);

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) {
LL_TIM_SetAutoReload(TIM1, GCR_SYMBOL_TIME);

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) {
Expand All @@ -132,5 +125,15 @@ void dshot_dma_isr(dma_device_t dev) {
timer_enable_dma_request(TIMER1, port->timer_channel, false);

dshot_dma_phase--;

if (profile.motor.dshot_inverted && dshot_dma_phase == dshot_gpio_port_count) {
// output phase done, lets swap to input
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
Loading

0 comments on commit 7d8b599

Please sign in to comment.