From b74b80cd4d6124c4db82eff848938e6b5a385fa7 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 31 Oct 2022 17:06:30 +1300 Subject: [PATCH 01/12] dshot: bidirectional dshot working on one motor This still includes a couple of hacks and debug statements. --- .../src/px4/stm/stm32_common/dshot/dshot.c | 334 +++++++++++++++++- .../stm32_common/include/px4_arch/io_timer.h | 5 + .../stm/stm32_common/io_pins/input_capture.c | 4 +- .../px4/stm/stm32_common/io_pins/io_timer.c | 57 ++- src/drivers/drv_dshot.h | 4 +- 5 files changed, 383 insertions(+), 21 deletions(-) diff --git a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c index 5b501850668a..90bda45ba834 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -1,7 +1,8 @@ /**************************************************************************** * - * Copyright (C) 2019-2021 PX4 Development Team. All rights reserved. + * Copyright (C) 2019-2023 PX4 Development Team. All rights reserved. * Author: Igor Misic + * Author: Julian Oes * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -45,6 +46,10 @@ #include #include +#include +#include +#include + #define MOTOR_PWM_BIT_1 14u #define MOTOR_PWM_BIT_0 7u @@ -63,6 +68,9 @@ #define DSHOT_DMA_SCR (DMA_SCR_PRIHI | DMA_SCR_MSIZE_32BITS | DMA_SCR_PSIZE_32BITS | DMA_SCR_MINC | \ DMA_SCR_DIR_M2P | DMA_SCR_TCIE | DMA_SCR_HTIE | DMA_SCR_TEIE | DMA_SCR_DMEIE) +#define DSHOT_TELEMETRY_DMA_SCR (DMA_SCR_PRIHI | DMA_SCR_MSIZE_16BITS | DMA_SCR_PSIZE_16BITS | DMA_SCR_MINC | \ + DMA_SCR_DIR_P2M | DMA_SCR_TCIE | DMA_SCR_TEIE | DMA_SCR_DMEIE) + typedef struct dshot_handler_t { bool init; DMA_HANDLE dma_handle; @@ -82,8 +90,167 @@ static uint8_t dshot_burst_buffer_array[DSHOT_TIMERS * DSHOT_BURST_BUFFER_SIZE(M px4_cache_aligned_data() = {}; static uint32_t *dshot_burst_buffer[DSHOT_TIMERS] = {}; +static uint16_t dshot_capture_buffer[32] px4_cache_aligned_data() = {}; +static size_t dshot_capture_buffer_size = sizeof(dshot_capture_buffer) * sizeof(dshot_capture_buffer[0]); + +static struct hrt_call _call; + +static void do_capture(DMA_HANDLE handle, uint8_t status, void *arg); +static void process_capture_results(void *arg); +static unsigned calculate_period(void); + +static uint32_t read_ok = 0; +static uint32_t read_fail_nibble = 0; +static uint32_t read_fail_crc = 0; +static uint32_t read_fail_zero = 0; + +static bool enable_bidirectional_dshot = true; + +static uint32_t _dshot_frequency = 0; + +uint8_t nibbles_from_mapped(uint8_t mapped) +{ + switch (mapped) { + case 0x19: + return 0x0; + + case 0x1B: + return 0x1; + + case 0x12: + return 0x2; + + case 0x13: + return 0x3; + + case 0x1D: + return 0x4; + + case 0x15: + return 0x5; + + case 0x16: + return 6; + + case 0x17: + return 7; + + case 0x1a: + return 8; + + case 0x09: + return 9; + + case 0x0A: + return 0x0A; + + case 0x0B: + return 0x0B; + + case 0x1E: + return 0x0C; + + case 0x0D: + return 0x0D; + + case 0x0E: + return 0x0E; + + case 0x0F: + return 0x0F; + + default: + // Unknown mapped + return 0xff; + } +} + +unsigned calculate_period(void) +{ + uint32_t value = 0; + + // We start off with high + uint32_t high = 1; + + unsigned shifted = 0; + unsigned previous = 0; + + for (unsigned i = 1; i < (32); ++i) { + + // We can ignore the very first data point as it's the pulse before it starts. + if (i > 1) { + + if (dshot_capture_buffer[i] == 0) { + // Once we get zeros we're through. + break; + } + + const uint32_t bits = (dshot_capture_buffer[i] - previous + 5) / 20; + + for (unsigned bit = 0; bit < bits; ++bit) { + value = value << 1; + value |= high; + ++shifted; + } + + // The next edge toggles. + high = !high; + } + + previous = dshot_capture_buffer[i]; + } + + if (shifted == 0) { + // no data yet, or this time + ++read_fail_zero; + return 0; + } + + // We need to make sure we shifted 21 times. We might have missed some low "pulses" at the very end. + value = value << (21 - shifted); + + // Note: At 0 throttle, the value is 0x1AD6AE, so 0b110101101011010101110 + + // From GCR to eRPM according to: + // https://brushlesswhoop.com/dshot-and-bidirectional-dshot/#erpm-transmission + unsigned gcr = (value ^ (value >> 1)); + + uint32_t data = 0; + + // 20bits -> 5 mapped -> 4 nibbles + for (unsigned i = 0; i < 4; ++i) { + uint32_t nibble = nibbles_from_mapped(gcr & (0x1F)) << (4 * i); + + if (nibble == 0xff) { + ++read_fail_nibble; + return 0; + } + + data |= nibble; + gcr = gcr >> 5; + } + + unsigned shift = (data & 0xE000) >> 13; + unsigned period = ((data & 0x1FF0) >> 4) << shift; + unsigned crc = (data & 0xf); + + unsigned payload = (data & 0xFFF0) >> 4; + unsigned calculated_crc = (~(payload ^ (payload >> 4) ^ (payload >> 8))) & 0x0F; + + if (crc != calculated_crc) { + ++read_fail_crc; + return 0; + } + + ++read_ok; + return period; +} + + int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq) { + _dshot_frequency = dshot_pwm_freq; + unsigned buffer_offset = 0; for (int timer_index = 0; timer_index < DSHOT_TIMERS; timer_index++) { @@ -107,8 +274,23 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq) } } + int channels_init_mask = 0xf; + int ret_val = 0; + + return ret_val == OK ? channels_init_mask : ret_val; +} + +void up_dshot_trigger(void) +{ + static uint64_t counter = 0; + + if (counter++ % 800 == 0) { + printf("ok: %lu, nibble: %lu, crc: %lu, zero: %lu\n", read_ok, read_fail_nibble, read_fail_crc, read_fail_zero); + } + /* Init channels */ int ret_val = OK; + int channel_mask = 0xf; int channels_init_mask = 0; for (unsigned channel = 0; (channel_mask != 0) && (channel < MAX_TIMER_IO_CHANNELS) && (OK == ret_val); channel++) { @@ -119,7 +301,9 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq) continue; } - ret_val = io_timer_channel_init(channel, IOTimerChanMode_Dshot, NULL, NULL); + ret_val = io_timer_unallocate_channel(channel); + ret_val = io_timer_channel_init(channel, + enable_bidirectional_dshot ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot, NULL, NULL); channel_mask &= ~(1 << channel); if (OK == ret_val) { @@ -133,28 +317,29 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq) } } - for (uint8_t timer_index = 0; (timer_index < DSHOT_TIMERS) && (OK == ret_val); timer_index++) { + for (uint8_t timer_index = 0; (timer_index < 1) && (OK == ret_val); timer_index++) { if (true == dshot_handler[timer_index].init) { dshot_handler[timer_index].dma_size = io_timers_channel_mapping.element[timer_index].channel_count_including_gaps * ONE_MOTOR_BUFF_SIZE; - io_timer_set_dshot_mode(timer_index, dshot_pwm_freq, + io_timer_set_dshot_mode(timer_index, _dshot_frequency, io_timers_channel_mapping.element[timer_index].channel_count_including_gaps); - dshot_handler[timer_index].dma_handle = stm32_dmachannel(io_timers[timer_index].dshot.dmamap); - if (NULL == dshot_handler[timer_index].dma_handle) { - ret_val = ERROR; + if (dshot_handler[timer_index].dma_handle != NULL) { + stm32_dmafree(dshot_handler[timer_index].dma_handle); } + + dshot_handler[timer_index].dma_handle = stm32_dmachannel(io_timers[timer_index].dshot.dmamap); } - } - return ret_val == OK ? channels_init_mask : ret_val; -} -void up_dshot_trigger(void) -{ - for (uint8_t timer = 0; (timer < DSHOT_TIMERS); timer++) { + if (NULL == dshot_handler[timer_index].dma_handle) { + ret_val = ERROR; + } + } + + for (uint8_t timer = 0; (timer < 1); timer++) { if (true == dshot_handler[timer].init) { @@ -172,13 +357,124 @@ void up_dshot_trigger(void) // Clean UDE flag before DMA is started io_timer_update_dma_req(timer, false); // Trigger DMA (DShot Outputs) - stm32_dmastart(dshot_handler[timer].dma_handle, NULL, NULL, false); + stm32_dmastart(dshot_handler[timer].dma_handle, do_capture, NULL, false); io_timer_update_dma_req(timer, true); + } + } + + io_timer_set_enable(true, enable_bidirectional_dshot ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot, + IO_TIMER_ALL_MODES_CHANNELS); +} +void do_capture(DMA_HANDLE handle, uint8_t status, void *arg) +{ + (void)handle; + (void)status; + (void)arg; + + const int capture_timer = 0; + const int capture_channel = 3; + + for (unsigned timer = 0; timer < DSHOT_TIMERS; ++timer) { + if (dshot_handler[timer].dma_handle != NULL) { + stm32_dmastop(dshot_handler[timer].dma_handle); } } + + + dshot_handler[capture_timer].dma_size = 32; + + + if (dshot_handler[0].dma_handle != NULL) { + stm32_dmafree(dshot_handler[0].dma_handle); + } + + dshot_handler[0].dma_handle = stm32_dmachannel(DMAMAP_DMA12_TIM5CH1_0); + + memset(dshot_capture_buffer, 0, sizeof(dshot_capture_buffer)); + up_clean_dcache((uintptr_t)dshot_capture_buffer, + (uintptr_t)dshot_capture_buffer + + dshot_capture_buffer_size); + + up_invalidate_dcache((uintptr_t)dshot_capture_buffer, + (uintptr_t)dshot_capture_buffer + + dshot_capture_buffer_size); + + px4_stm32_dmasetup(dshot_handler[0].dma_handle, + io_timers[capture_timer].base + STM32_GTIM_DMAR_OFFSET, + (uint32_t)(&dshot_capture_buffer[0]), + 32, + DSHOT_TELEMETRY_DMA_SCR); + + // TODO: check retval? + io_timer_unallocate_channel(capture_channel); + io_timer_channel_init(capture_channel, IOTimerChanMode_CaptureDMA, NULL, NULL); + io_timer_set_enable(true, IOTimerChanMode_CaptureDMA, 1 << capture_channel); + + up_input_capture_set(capture_channel, Both, 0, NULL, NULL); + + io_timer_capture_update_dma_req(capture_timer, false); + io_timer_set_capture_mode(capture_timer, _dshot_frequency); + + stm32_dmastart(dshot_handler[0].dma_handle, NULL, NULL, false); + io_timer_capture_update_dma_req(capture_timer, true); + + // It takes around 85 us for the ESC to respond, so we should have a result after 150 us, surely. + hrt_call_after(&_call, 150, process_capture_results, NULL); } +void process_capture_results(void *arg) +{ + (void)arg; + + up_invalidate_dcache((uintptr_t)dshot_capture_buffer, + (uintptr_t)dshot_capture_buffer + + dshot_capture_buffer_size); + + if (dshot_handler[0].dma_handle != NULL) { + + stm32_dmastop(dshot_handler[0].dma_handle); + } + + /* Init channels */ + int ret_val = OK; + int channel_mask = 0xf; + int channels_init_mask = 0; + + for (unsigned channel = 0; (channel_mask != 0) && (channel < MAX_TIMER_IO_CHANNELS) && (OK == ret_val); channel++) { + if (channel_mask & (1 << channel)) { + uint8_t timer = timer_io_channels[channel].timer_index; + + if (io_timers[timer].dshot.dma_base == 0) { // board does not configure dshot on this timer + continue; + } + + ret_val = io_timer_unallocate_channel(channel); + ret_val = io_timer_channel_init(channel, + enable_bidirectional_dshot ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot, NULL, NULL); + channel_mask &= ~(1 << channel); + + if (OK == ret_val) { + dshot_handler[timer].init = true; + channels_init_mask |= 1 << channel; + + } else if (ret_val == -EBUSY) { + /* either timer or channel already used - this is not fatal */ + ret_val = 0; + } + } + } + + const unsigned period = calculate_period(); + + static uint64_t counter = 0; + + if (counter++ % 800 == 0) { + printf("period: %u\n", period); + } +} + + /** * bits 1-11 - throttle value (0-47 are reserved, 48-2047 give 2000 steps of throttle resolution) * bit 12 - dshot telemetry enable/disable @@ -202,7 +498,12 @@ void dshot_motor_data_set(unsigned motor_number, uint16_t throttle, bool telemet csum_data >>= NIBBLES_SIZE; } - packet |= (checksum & 0x0F); + if (enable_bidirectional_dshot) { + packet |= ((~checksum) & 0x0F); + + } else { + packet |= ((checksum) & 0x0F); + } unsigned timer = timer_io_channels[motor_number].timer_index; uint32_t *buffer = dshot_burst_buffer[timer]; @@ -219,7 +520,8 @@ void dshot_motor_data_set(unsigned motor_number, uint16_t throttle, bool telemet int up_dshot_arm(bool armed) { - return io_timer_set_enable(armed, IOTimerChanMode_Dshot, IO_TIMER_ALL_MODES_CHANNELS); + return io_timer_set_enable(armed, enable_bidirectional_dshot ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot, + IO_TIMER_ALL_MODES_CHANNELS); } #endif diff --git a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h index 31d6e9b1c7e4..67554d796af9 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h @@ -80,6 +80,8 @@ typedef enum io_timer_channel_mode_t { IOTimerChanMode_LED = 7, IOTimerChanMode_PPS = 8, IOTimerChanMode_Other = 9, + IOTimerChanMode_DshotInverted = 10, + IOTimerChanMode_CaptureDMA = 11, IOTimerChanModeSize } io_timer_channel_mode_t; @@ -159,6 +161,9 @@ __EXPORT int io_timer_get_channel_mode(unsigned channel); __EXPORT int io_timer_get_mode_channels(io_timer_channel_mode_t mode); __EXPORT extern void io_timer_trigger(unsigned channels_mask); __EXPORT void io_timer_update_dma_req(uint8_t timer, bool enable); +__EXPORT void io_timer_capture_update_dma_req(uint8_t timer, bool enable); +__EXPORT int io_timer_set_enable_capture_dma(bool state, io_timer_channel_allocation_t masks); +__EXPORT int io_timer_set_capture_mode(uint8_t timer, unsigned dshot_pwm_rate); /** * Reserve a timer diff --git a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/input_capture.c b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/input_capture.c index 61fd70ee886c..8aa990dc3133 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/input_capture.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/input_capture.c @@ -365,8 +365,10 @@ int up_input_capture_set_trigger(unsigned channel, input_capture_edge edge) rv = -ENXIO; /* Any pins in capture mode */ + int mode = io_timer_get_channel_mode(channel); - if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) { + if (mode == IOTimerChanMode_Capture || + mode == IOTimerChanMode_CaptureDMA) { uint16_t edge_bits = 0xffff; diff --git a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c index 5a72cc9f5e30..a6e840c4fe7c 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012, 2017 PX4 Development Team. All rights reserved. + * Copyright (C) 2012, 2017, 2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -131,6 +131,7 @@ static int io_timer_handler7(int irq, void *context, void *arg); (GTIM_CCMR_ICF_NOFILT << GTIM_CCMR1_IC1F_SHIFT) #define CCMR_C1_PWMOUT_INIT (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC1M_SHIFT) | GTIM_CCMR1_OC1PE +#define CCMR_C1_PWMOUT_INVERTED_INIT (GTIM_CCMR_MODE_PWM2 << GTIM_CCMR1_OC1M_SHIFT) | GTIM_CCMR1_OC1PE #define CCMR_C1_PWMIN_INIT 0 // TBD @@ -519,6 +520,18 @@ void io_timer_update_dma_req(uint8_t timer, bool enable) } } +void io_timer_capture_update_dma_req(uint8_t timer, bool enable) +{ + if (enable) { + rDIER(timer) |= ATIM_DIER_CC1DE; + rEGR(timer) |= (ATIM_EGR_UG | ATIM_EGR_CC1G | ATIM_EGR_CC2G); + + } else { + rEGR(timer) &= ~(ATIM_EGR_UG | ATIM_EGR_CC1G | ATIM_EGR_CC2G); + rDIER(timer) &= ~(ATIM_DIER_CC1DE); + } +} + int io_timer_set_dshot_mode(uint8_t timer, unsigned dshot_pwm_freq, uint8_t dma_burst_length) { int ret_val = OK; @@ -545,7 +558,7 @@ int io_timer_set_dshot_mode(uint8_t timer, unsigned dshot_pwm_freq, uint8_t dma_ rPSC(timer) = ((int)(io_timers[timer].clock_freq / dshot_pwm_freq) / DSHOT_MOTOR_PWM_BIT_WIDTH) - 1; rEGR(timer) = ATIM_EGR_UG; - // find the lowest channel index for the timer (they are not necesarily in ascending order) + // find the lowest channel index for the timer (they are not necessarily in ascending order) unsigned lowest_timer_channel = 4; uint32_t first_channel_index = io_timers_channel_mapping.element[timer].first_channel_index; uint32_t last_channel_index = first_channel_index + io_timers_channel_mapping.element[timer].channel_count; @@ -574,6 +587,28 @@ int io_timer_set_dshot_mode(uint8_t timer, unsigned dshot_pwm_freq, uint8_t dma_ return ret_val; } +int io_timer_set_capture_mode(uint8_t timer, unsigned dshot_pwm_freq) +{ + rARR(timer) = -1; + rEGR(timer) = ATIM_EGR_UG | ATIM_EGR_CC1G | ATIM_EGR_CC2G; + + rPSC(timer) = ((int)(io_timers[timer].clock_freq / (dshot_pwm_freq * 5 / 4)) / DSHOT_MOTOR_PWM_BIT_WIDTH) - 1; + + // We need to disable CC1E before we can switch to CC1S to input + rCCER(timer) &= ~(GTIM_CCER_CC1E | GTIM_CCER_CC1P | GTIM_CCER_CC1NP); + + rCCMR1(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR1_CC1S_SHIFT); + + // Enable the timer + rCR1(timer) |= GTIM_CR1_CEN; + + rCCER(timer) |= (GTIM_CCER_CC1E | GTIM_CCER_CC1P | GTIM_CCER_CC1NP); + + rDCR(timer) = 0xd; // 0xD to read CC1, 0xe to read CC2 + + return 0; +} + static inline void io_timer_set_PWM_mode(unsigned timer) { rPSC(timer) = (io_timers[timer].clock_freq / BOARD_PWM_FREQ) - 1; @@ -773,6 +808,12 @@ int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode, setbits = CCMR_C1_PWMOUT_INIT; break; + case IOTimerChanMode_DshotInverted: + ccer_setbits = 0; + dier_setbits = 0; + setbits = CCMR_C1_PWMOUT_INVERTED_INIT; + break; + case IOTimerChanMode_PWMIn: setbits = CCMR_C1_PWMIN_INIT; gpio = timer_io_channels[channel].gpio_in; @@ -781,6 +822,7 @@ int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode, #if !defined(BOARD_HAS_NO_CAPTURE) case IOTimerChanMode_Capture: + case IOTimerChanMode_CaptureDMA: setbits = CCMR_C1_CAPTURE_INIT; gpio = timer_io_channels[channel].gpio_in; break; @@ -805,6 +847,13 @@ int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode, rv = io_timer_init_timer(timer, mode); + if (rv == -16) { + // FIXME: I don't understand why exactly this is the way it is. + // With this hack I'm able to to toggle the dshot pins from output + // to input without problem. But there should be a nicer way. + rv = 0; + } + if (rv != 0 && previous_mode == IOTimerChanMode_NotUsed) { /* free the channel if it was not used before */ io_timer_unallocate_channel(channel); @@ -897,12 +946,14 @@ int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_chann break; case IOTimerChanMode_Dshot: + case IOTimerChanMode_DshotInverted: dier_bit = 0; cr1_bit = state ? GTIM_CR1_CEN : 0; break; case IOTimerChanMode_PWMIn: case IOTimerChanMode_Capture: + case IOTimerChanMode_CaptureDMA: break; default: @@ -944,6 +995,7 @@ int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_chann (mode == IOTimerChanMode_PWMOut || mode == IOTimerChanMode_OneShot || mode == IOTimerChanMode_Dshot || + mode == IOTimerChanMode_DshotInverted || mode == IOTimerChanMode_Trigger))) { action_cache[timer].gpio[shifts] = timer_io_channels[chan_index].gpio_out; } @@ -1004,6 +1056,7 @@ int io_timer_set_ccr(unsigned channel, uint16_t value) if ((mode != IOTimerChanMode_PWMOut) && (mode != IOTimerChanMode_OneShot) && (mode != IOTimerChanMode_Dshot) && + (mode != IOTimerChanMode_DshotInverted) && (mode != IOTimerChanMode_Trigger)) { rv = -EIO; diff --git a/src/drivers/drv_dshot.h b/src/drivers/drv_dshot.h index 2e8dab2d6db9..5df5f3b469d6 100644 --- a/src/drivers/drv_dshot.h +++ b/src/drivers/drv_dshot.h @@ -88,10 +88,10 @@ typedef enum { * @param channel_mask Bitmask of channels (LSB = channel 0) to enable. * This allows some of the channels to remain configured * as GPIOs or as another function. Already used channels/timers will not be configured as DShot - * @param dshot_pwm_freq Frequency of DSHOT signal. Usually DSHOT150, DSHOT300, DSHOT600 or DSHOT1200 + * @param dshot_freq Frequency of DSHOT signal. Usually DSHOT150, DSHOT300, DSHOT600 or DSHOT1200 * @return <0 on error, the initialized channels mask. */ -__EXPORT extern int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq); +__EXPORT extern int up_dshot_init(uint32_t channel_mask, unsigned dshot_freq); /** * Set Dshot motor data, used by up_dshot_motor_data_set() and up_dshot_motor_command() (internal method) From 87d74fcba9be714debc1c50227c717e4e374508c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 31 Oct 2022 22:13:00 +1300 Subject: [PATCH 02/12] dshot: scale to 4 motors, move debug out of IRQ --- .../src/px4/stm/stm32_common/dshot/dshot.c | 82 +++++++++++++++---- .../stm32_common/include/px4_arch/io_timer.h | 2 +- .../px4/stm/stm32_common/io_pins/io_timer.c | 52 +++++++++--- src/drivers/drv_dshot.h | 2 + src/drivers/dshot/DShot.cpp | 13 +++ 5 files changed, 120 insertions(+), 31 deletions(-) diff --git a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c index 90bda45ba834..d237c32a4cdb 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -108,6 +108,10 @@ static bool enable_bidirectional_dshot = true; static uint32_t _dshot_frequency = 0; +static uint32_t _motor_to_capture = 0; +static uint32_t _periods[4] = {0, 0, 0, 0}; +static bool _periods_ready = false; + uint8_t nibbles_from_mapped(uint8_t mapped) { switch (mapped) { @@ -282,12 +286,6 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq) void up_dshot_trigger(void) { - static uint64_t counter = 0; - - if (counter++ % 800 == 0) { - printf("ok: %lu, nibble: %lu, crc: %lu, zero: %lu\n", read_ok, read_fail_nibble, read_fail_crc, read_fail_zero); - } - /* Init channels */ int ret_val = OK; int channel_mask = 0xf; @@ -372,8 +370,20 @@ void do_capture(DMA_HANDLE handle, uint8_t status, void *arg) (void)status; (void)arg; + if (_periods_ready) { + // The periods need to be collected first, so we have to skip it this time. + return; + } + + if (_motor_to_capture >= 4) { + // We only support the first 4 for now. + return; + } + + // TODO: this things are still somewhat hard-coded. And I'm probably confused + // regarding channels and indexes. const int capture_timer = 0; - const int capture_channel = 3; + const int capture_channel = _motor_to_capture; for (unsigned timer = 0; timer < DSHOT_TIMERS; ++timer) { if (dshot_handler[timer].dma_handle != NULL) { @@ -381,15 +391,32 @@ void do_capture(DMA_HANDLE handle, uint8_t status, void *arg) } } - dshot_handler[capture_timer].dma_size = 32; - if (dshot_handler[0].dma_handle != NULL) { stm32_dmafree(dshot_handler[0].dma_handle); } - dshot_handler[0].dma_handle = stm32_dmachannel(DMAMAP_DMA12_TIM5CH1_0); + // TODO: We should probably do this at another level board specific. + // However, right now the dma handles are all hard-coded to the UP(date) source + // rather than the capture compare one. + switch (timer_io_channels[_motor_to_capture].timer_channel) { + case 1: + dshot_handler[capture_timer].dma_handle = stm32_dmachannel(DMAMAP_DMA12_TIM5CH1_0); + break; + + case 2: + dshot_handler[capture_timer].dma_handle = stm32_dmachannel(DMAMAP_DMA12_TIM5CH2_0); + break; + + case 3: + dshot_handler[capture_timer].dma_handle = stm32_dmachannel(DMAMAP_DMA12_TIM5CH3_0); + break; + + case 4: + dshot_handler[capture_timer].dma_handle = stm32_dmachannel(DMAMAP_DMA12_TIM5CH4_0); + break; + } memset(dshot_capture_buffer, 0, sizeof(dshot_capture_buffer)); up_clean_dcache((uintptr_t)dshot_capture_buffer, @@ -414,9 +441,9 @@ void do_capture(DMA_HANDLE handle, uint8_t status, void *arg) up_input_capture_set(capture_channel, Both, 0, NULL, NULL); io_timer_capture_update_dma_req(capture_timer, false); - io_timer_set_capture_mode(capture_timer, _dshot_frequency); + io_timer_set_capture_mode(capture_timer, _dshot_frequency, capture_channel); - stm32_dmastart(dshot_handler[0].dma_handle, NULL, NULL, false); + stm32_dmastart(dshot_handler[capture_timer].dma_handle, NULL, NULL, false); io_timer_capture_update_dma_req(capture_timer, true); // It takes around 85 us for the ESC to respond, so we should have a result after 150 us, surely. @@ -465,13 +492,14 @@ void process_capture_results(void *arg) } } - const unsigned period = calculate_period(); - - static uint64_t counter = 0; + // TODO: fix order + _periods[_motor_to_capture] = calculate_period(); - if (counter++ % 800 == 0) { - printf("period: %u\n", period); + if (_motor_to_capture == 3) { + _periods_ready = true; } + + _motor_to_capture = (_motor_to_capture + 1) % 4; } @@ -524,4 +552,24 @@ int up_dshot_arm(bool armed) IO_TIMER_ALL_MODES_CHANNELS); } +bool up_dshot_get_periods(uint32_t periods[], size_t num_periods) +{ + // TODO: hardcoded for now. + if (num_periods != 4) { + return false; + } + + if (!_periods_ready) { + return false; + } + + for (unsigned i = 0; i < 4; ++i) { + periods[i] = _periods[i]; + } + + _periods_ready = false; + + return true; +} + #endif diff --git a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h index 67554d796af9..ee844eb2f3a4 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h @@ -163,7 +163,7 @@ __EXPORT extern void io_timer_trigger(unsigned channels_mask); __EXPORT void io_timer_update_dma_req(uint8_t timer, bool enable); __EXPORT void io_timer_capture_update_dma_req(uint8_t timer, bool enable); __EXPORT int io_timer_set_enable_capture_dma(bool state, io_timer_channel_allocation_t masks); -__EXPORT int io_timer_set_capture_mode(uint8_t timer, unsigned dshot_pwm_rate); +__EXPORT int io_timer_set_capture_mode(uint8_t timer, unsigned dshot_pwm_rate, unsigned channel); /** * Reserve a timer diff --git a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c index a6e840c4fe7c..427a23bf2e37 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c @@ -523,12 +523,12 @@ void io_timer_update_dma_req(uint8_t timer, bool enable) void io_timer_capture_update_dma_req(uint8_t timer, bool enable) { if (enable) { - rDIER(timer) |= ATIM_DIER_CC1DE; - rEGR(timer) |= (ATIM_EGR_UG | ATIM_EGR_CC1G | ATIM_EGR_CC2G); + rDIER(timer) |= ATIM_DIER_CC1DE | ATIM_DIER_CC2DE | ATIM_DIER_CC3DE | ATIM_DIER_CC4DE; + rEGR(timer) |= (ATIM_EGR_UG | ATIM_EGR_CC1G | ATIM_EGR_CC2G | ATIM_EGR_CC3G | ATIM_EGR_CC4G); } else { - rEGR(timer) &= ~(ATIM_EGR_UG | ATIM_EGR_CC1G | ATIM_EGR_CC2G); - rDIER(timer) &= ~(ATIM_DIER_CC1DE); + rEGR(timer) &= ~(ATIM_EGR_UG | ATIM_EGR_CC1G | ATIM_EGR_CC2G | ATIM_EGR_CC3G | ATIM_EGR_CC4G); + rDIER(timer) &= ~(ATIM_DIER_CC1DE | ATIM_DIER_CC2DE | ATIM_DIER_CC3DE | ATIM_DIER_CC4DE); } } @@ -587,24 +587,50 @@ int io_timer_set_dshot_mode(uint8_t timer, unsigned dshot_pwm_freq, uint8_t dma_ return ret_val; } -int io_timer_set_capture_mode(uint8_t timer, unsigned dshot_pwm_freq) +int io_timer_set_capture_mode(uint8_t timer, unsigned dshot_pwm_freq, unsigned channel) { rARR(timer) = -1; - rEGR(timer) = ATIM_EGR_UG | ATIM_EGR_CC1G | ATIM_EGR_CC2G; + rEGR(timer) = ATIM_EGR_UG | GTIM_EGR_CC1G | GTIM_EGR_CC2G | GTIM_EGR_CC3G | GTIM_EGR_CC4G; rPSC(timer) = ((int)(io_timers[timer].clock_freq / (dshot_pwm_freq * 5 / 4)) / DSHOT_MOTOR_PWM_BIT_WIDTH) - 1; - // We need to disable CC1E before we can switch to CC1S to input - rCCER(timer) &= ~(GTIM_CCER_CC1E | GTIM_CCER_CC1P | GTIM_CCER_CC1NP); - rCCMR1(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR1_CC1S_SHIFT); - // Enable the timer - rCR1(timer) |= GTIM_CR1_CEN; + switch (timer_io_channels[channel].timer_channel) { + case 1: + // We need to disable CC1E before we can switch to CC1S to input + rCCER(timer) &= ~(GTIM_CCER_CC1E | GTIM_CCER_CC1P | GTIM_CCER_CC1NP); + rCCMR1(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR1_CC1S_SHIFT); + rCR1(timer) |= GTIM_CR1_CEN; + rCCER(timer) |= (GTIM_CCER_CC1E | GTIM_CCER_CC1P | GTIM_CCER_CC1NP); +// We need to pass the offset of the register to read by DMA divided by 4. + rDCR(timer) = 0xD; // 0x34 / 4, offset for CC1 + break; + + case 2: + rCCER(timer) &= ~(GTIM_CCER_CC2E | GTIM_CCER_CC2P | GTIM_CCER_CC2NP); + rCCMR1(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR1_CC2S_SHIFT); + rCR1(timer) |= GTIM_CR1_CEN; + rCCER(timer) |= (GTIM_CCER_CC2E | GTIM_CCER_CC2P | GTIM_CCER_CC2NP); + rDCR(timer) = 0xE; // 0x38 / 4, offset for CC2 + break; - rCCER(timer) |= (GTIM_CCER_CC1E | GTIM_CCER_CC1P | GTIM_CCER_CC1NP); + case 3: + rCCER(timer) &= ~(GTIM_CCER_CC3E | GTIM_CCER_CC3P | GTIM_CCER_CC3NP); + rCCMR2(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR2_CC3S_SHIFT); + rCR1(timer) |= GTIM_CR1_CEN; + rCCER(timer) |= (GTIM_CCER_CC3E | GTIM_CCER_CC3P | GTIM_CCER_CC3NP); + rDCR(timer) = 0xF; // 0x3C / 4, offset for CC3 + break; - rDCR(timer) = 0xd; // 0xD to read CC1, 0xe to read CC2 + case 4: + rCCER(timer) &= ~(GTIM_CCER_CC4E | GTIM_CCER_CC4P | GTIM_CCER_CC4NP); + rCCMR2(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR2_CC4S_SHIFT); + rCR1(timer) |= GTIM_CR1_CEN; + rCCER(timer) |= (GTIM_CCER_CC4E | GTIM_CCER_CC4P | GTIM_CCER_CC4NP); + rDCR(timer) = 0x10; // 0x40 / 4, offset for CC4 + break; + } return 0; } diff --git a/src/drivers/drv_dshot.h b/src/drivers/drv_dshot.h index 5df5f3b469d6..ef8d8938b107 100644 --- a/src/drivers/drv_dshot.h +++ b/src/drivers/drv_dshot.h @@ -137,4 +137,6 @@ __EXPORT extern void up_dshot_trigger(void); */ __EXPORT extern int up_dshot_arm(bool armed); +__EXPORT extern bool up_dshot_get_periods(uint32_t periods[], size_t num_periods); + __END_DECLS diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index 5ae0150efba9..0c9e77771400 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -435,8 +435,21 @@ bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], _current_command.clear(); } + // We need to copy out the periods from the previous measurements before triggering again. + // Otherwise, we can't capture more data in this run. + uint32_t periods[4]; + bool periods_ok = up_dshot_get_periods(periods, 4); + up_dshot_trigger(); + if (periods_ok) { + static unsigned counter = 0; + + if (counter++ % 10 == 0) { + printf("periods: % 6ld % 6ld % 6ld % 6ld\n", periods[0], periods[1], periods[2], periods[3]); + } + } + return true; } From 58a2334424fc241b72a6555cffc5297b40b9321c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 3 Nov 2022 15:01:30 +1300 Subject: [PATCH 03/12] dshot: publish erpms via uORB --- .../src/px4/stm/stm32_common/dshot/dshot.c | 38 +++++----------- src/drivers/drv_dshot.h | 2 + src/drivers/dshot/DShot.cpp | 44 +++++++++++++------ src/drivers/dshot/DShot.h | 6 +++ 4 files changed, 51 insertions(+), 39 deletions(-) diff --git a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c index d237c32a4cdb..03efaef592a0 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -109,8 +109,10 @@ static bool enable_bidirectional_dshot = true; static uint32_t _dshot_frequency = 0; static uint32_t _motor_to_capture = 0; -static uint32_t _periods[4] = {0, 0, 0, 0}; -static bool _periods_ready = false; +static uint32_t _erpms[4] = {0, 0, 0, 0}; + +static void(*_erpm_callback)(uint32_t[], size_t, void *) = NULL; +static void *_erpm_callback_context = NULL; uint8_t nibbles_from_mapped(uint8_t mapped) { @@ -370,11 +372,6 @@ void do_capture(DMA_HANDLE handle, uint8_t status, void *arg) (void)status; (void)arg; - if (_periods_ready) { - // The periods need to be collected first, so we have to skip it this time. - return; - } - if (_motor_to_capture >= 4) { // We only support the first 4 for now. return; @@ -493,10 +490,13 @@ void process_capture_results(void *arg) } // TODO: fix order - _periods[_motor_to_capture] = calculate_period(); + // TODO: convert from period to erpm + _erpms[_motor_to_capture] = calculate_period(); if (_motor_to_capture == 3) { - _periods_ready = true; + if (_erpm_callback != NULL) { + _erpm_callback(_erpms, 4, _erpm_callback_context); + } } _motor_to_capture = (_motor_to_capture + 1) % 4; @@ -552,24 +552,10 @@ int up_dshot_arm(bool armed) IO_TIMER_ALL_MODES_CHANNELS); } -bool up_dshot_get_periods(uint32_t periods[], size_t num_periods) +void up_dshot_set_erpm_callback(void(*callback)(uint32_t[], size_t, void *), void *context) { - // TODO: hardcoded for now. - if (num_periods != 4) { - return false; - } - - if (!_periods_ready) { - return false; - } - - for (unsigned i = 0; i < 4; ++i) { - periods[i] = _periods[i]; - } - - _periods_ready = false; - - return true; + _erpm_callback = callback; + _erpm_callback_context = context; } #endif diff --git a/src/drivers/drv_dshot.h b/src/drivers/drv_dshot.h index ef8d8938b107..ca13b8fb55f0 100644 --- a/src/drivers/drv_dshot.h +++ b/src/drivers/drv_dshot.h @@ -139,4 +139,6 @@ __EXPORT extern int up_dshot_arm(bool armed); __EXPORT extern bool up_dshot_get_periods(uint32_t periods[], size_t num_periods); +__EXPORT extern void up_dshot_set_erpm_callback(void(*callback)(uint32_t[], size_t, void *), void *context); + __END_DECLS diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index 0c9e77771400..67dccb3d2270 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -67,6 +67,13 @@ int DShot::init() // Getting initial parameter values update_params(); + // We can't advertise in interrupt context later. + _esc_status_pub.advertise(); + esc_status_s &esc_status = _esc_status_pub.get(); + esc_status = {}; + _esc_status_pub.update(); + up_dshot_set_erpm_callback(&DShot::erpm_trampoline, this); + ScheduleNow(); return OK; @@ -435,21 +442,8 @@ bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], _current_command.clear(); } - // We need to copy out the periods from the previous measurements before triggering again. - // Otherwise, we can't capture more data in this run. - uint32_t periods[4]; - bool periods_ok = up_dshot_get_periods(periods, 4); - up_dshot_trigger(); - if (periods_ok) { - static unsigned counter = 0; - - if (counter++ % 10 == 0) { - printf("periods: % 6ld % 6ld % 6ld % 6ld\n", periods[0], periods[1], periods[2], periods[3]); - } - } - return true; } @@ -620,6 +614,30 @@ void DShot::update_params() } } +void DShot::erpm_trampoline(uint32_t erpms[], size_t num_erpms, void *context) +{ + DShot *self = static_cast(context); + self->erpm(erpms, num_erpms); +} + +void DShot::erpm(uint32_t erpms[], size_t num_erpms) +{ + esc_status_s &esc_status = _esc_status_pub.get(); + esc_status = {}; + esc_status.timestamp = hrt_absolute_time(); + esc_status.counter = _esc_status_counter++; + esc_status.esc_count = 4; + esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_DSHOT; + esc_status.esc_armed_flags = _outputs_on; + + for (unsigned i = 0; i < 4 && i < esc_status_s::CONNECTED_ESC_MAX; ++i) { + esc_status.esc[i].timestamp = hrt_absolute_time(); + esc_status.esc[i].esc_rpm = erpms[i]; + } + + _esc_status_pub.update(); +} + int DShot::custom_command(int argc, char *argv[]) { const char *verb = argv[0]; diff --git a/src/drivers/dshot/DShot.h b/src/drivers/dshot/DShot.h index f0cb4458f404..2e0357055a04 100644 --- a/src/drivers/dshot/DShot.h +++ b/src/drivers/dshot/DShot.h @@ -143,6 +143,9 @@ class DShot final : public ModuleBase, public OutputModuleInterface void handle_vehicle_commands(); + static void erpm_trampoline(uint32_t erpms[], size_t num_erpms, void *context); + void erpm(uint32_t erpms[], size_t num_erpms); + MixingOutput _mixing_output {PARAM_PREFIX, DIRECT_PWM_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; uint32_t _reversible_outputs{}; @@ -170,6 +173,9 @@ class DShot final : public ModuleBase, public OutputModuleInterface uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; uORB::Publication _command_ack_pub{ORB_ID(vehicle_command_ack)}; + uORB::PublicationData _esc_status_pub{ORB_ID(esc_status)}; + uint16_t _esc_status_counter{0}; + DEFINE_PARAMETERS( (ParamFloat) _param_dshot_min, (ParamBool) _param_dshot_3d_enable, From 8526f27e886698bd983b2ac8591bbceb7b4566e9 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 7 Nov 2022 21:24:50 +1300 Subject: [PATCH 04/12] dshot: cleanup, reorg, however still limited to 4 --- .../src/px4/stm/stm32_common/dshot/dshot.c | 325 +++++++++--------- 1 file changed, 167 insertions(+), 158 deletions(-) diff --git a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c index 03efaef592a0..1823bf65f6d2 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -72,7 +72,6 @@ DMA_SCR_DIR_P2M | DMA_SCR_TCIE | DMA_SCR_TEIE | DMA_SCR_DMEIE) typedef struct dshot_handler_t { - bool init; DMA_HANDLE dma_handle; uint32_t dma_size; } dshot_handler_t; @@ -91,13 +90,13 @@ px4_cache_aligned_data() = {}; static uint32_t *dshot_burst_buffer[DSHOT_TIMERS] = {}; static uint16_t dshot_capture_buffer[32] px4_cache_aligned_data() = {}; -static size_t dshot_capture_buffer_size = sizeof(dshot_capture_buffer) * sizeof(dshot_capture_buffer[0]); static struct hrt_call _call; static void do_capture(DMA_HANDLE handle, uint8_t status, void *arg); static void process_capture_results(void *arg); static unsigned calculate_period(void); +static int dshot_output_timer_init(unsigned channel); static uint32_t read_ok = 0; static uint32_t read_fail_nibble = 0; @@ -107,9 +106,12 @@ static uint32_t read_fail_zero = 0; static bool enable_bidirectional_dshot = true; static uint32_t _dshot_frequency = 0; +static int _timers_init_mask = 0; +static int _channels_init_mask = 0; +// We only support capture on the first timer (usually 4 channels) for now. static uint32_t _motor_to_capture = 0; -static uint32_t _erpms[4] = {0, 0, 0, 0}; +static uint32_t _erpms[4] = {}; static void(*_erpm_callback)(uint32_t[], size_t, void *) = NULL; static void *_erpm_callback_context = NULL; @@ -191,6 +193,7 @@ unsigned calculate_period(void) break; } + // TODO: Doesn't cope with DShot frequency other than 600 const uint32_t bits = (dshot_capture_buffer[i] - previous + 5) / 20; for (unsigned bit = 0; bit < bits; ++bit) { @@ -252,101 +255,129 @@ unsigned calculate_period(void) return period; } +int dshot_output_timer_init(unsigned channel) +{ + io_timer_unallocate_channel(channel); + int ret = io_timer_channel_init(channel, + enable_bidirectional_dshot ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot, NULL, NULL); + + if (ret == -EBUSY) { + // either timer or channel already used - this is not fatal + return OK; + + } else { + return ret; + } +} int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq) { _dshot_frequency = dshot_pwm_freq; - unsigned buffer_offset = 0; + for (unsigned channel = 0; channel < MAX_TIMER_IO_CHANNELS; channel++) { + if (channel_mask & (1 << channel)) { + uint8_t timer = timer_io_channels[channel].timer_index; - for (int timer_index = 0; timer_index < DSHOT_TIMERS; timer_index++) { - dshot_handler[timer_index].init = false; - } + if (io_timers[timer].dshot.dma_base == 0) { // board does not configure dshot on this timer + continue; + } - for (unsigned timer = 0; timer < DSHOT_TIMERS; ++timer) { - if (io_timers[timer].base == 0) { // no more timers configured - break; - } + int ret = dshot_output_timer_init(channel); - // we know the uint8_t* cast to uint32_t* is fine, since we're aligned to cache line size -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wcast-align" - dshot_burst_buffer[timer] = (uint32_t *)&dshot_burst_buffer_array[buffer_offset]; -#pragma GCC diagnostic pop - buffer_offset += DSHOT_BURST_BUFFER_SIZE(io_timers_channel_mapping.element[timer].channel_count_including_gaps); + if (ret != OK) { + return ret; + } - if (buffer_offset > sizeof(dshot_burst_buffer_array)) { - return -EINVAL; // something is wrong with the board configuration or some other logic + _channels_init_mask |= (1 << channel); + _timers_init_mask |= (1 << timer); } } - int channels_init_mask = 0xf; - int ret_val = 0; + for (uint8_t timer = 0; timer < MAX_IO_TIMERS; ++timer) { + if (_timers_init_mask & (1 << timer)) { + if (dshot_handler[timer].dma_handle == NULL) { + dshot_handler[timer].dma_size = io_timers_channel_mapping.element[timer].channel_count_including_gaps * + ONE_MOTOR_BUFF_SIZE; - return ret_val == OK ? channels_init_mask : ret_val; -} + io_timer_set_dshot_mode(timer, _dshot_frequency, + io_timers_channel_mapping.element[timer].channel_count_including_gaps); -void up_dshot_trigger(void) -{ - /* Init channels */ - int ret_val = OK; - int channel_mask = 0xf; - int channels_init_mask = 0; + dshot_handler[timer].dma_handle = stm32_dmachannel(io_timers[timer].dshot.dmamap); - for (unsigned channel = 0; (channel_mask != 0) && (channel < MAX_TIMER_IO_CHANNELS) && (OK == ret_val); channel++) { - if (channel_mask & (1 << channel)) { - uint8_t timer = timer_io_channels[channel].timer_index; + if (NULL == dshot_handler[timer].dma_handle) { + // TODO: how to log this? + return -ENOSR; + } - if (io_timers[timer].dshot.dma_base == 0) { // board does not configure dshot on this timer - continue; + // We have tested this now but will anyway initialize it again during a trigger, so we can free it again. + stm32_dmafree(dshot_handler[timer].dma_handle); + dshot_handler[timer].dma_handle = NULL; } + } + } - ret_val = io_timer_unallocate_channel(channel); - ret_val = io_timer_channel_init(channel, - enable_bidirectional_dshot ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot, NULL, NULL); - channel_mask &= ~(1 << channel); + unsigned buffer_offset = 0; - if (OK == ret_val) { - dshot_handler[timer].init = true; - channels_init_mask |= 1 << channel; + for (unsigned timer = 0; timer < DSHOT_TIMERS; ++timer) { + if (_timers_init_mask & (1 << timer)) { + if (io_timers[timer].base == 0) { // no more timers configured + break; + } - } else if (ret_val == -EBUSY) { - /* either timer or channel already used - this is not fatal */ - ret_val = 0; + // we know the uint8_t* cast to uint32_t* is fine, since we're aligned to cache line size +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wcast-align" + dshot_burst_buffer[timer] = (uint32_t *) &dshot_burst_buffer_array[buffer_offset]; +#pragma GCC diagnostic pop + buffer_offset += DSHOT_BURST_BUFFER_SIZE( + io_timers_channel_mapping.element[timer].channel_count_including_gaps); + + if (buffer_offset > sizeof(dshot_burst_buffer_array)) { + return -EINVAL; // something is wrong with the board configuration or some other logic } } } - for (uint8_t timer_index = 0; (timer_index < 1) && (OK == ret_val); timer_index++) { + return _channels_init_mask; +} - if (true == dshot_handler[timer_index].init) { - dshot_handler[timer_index].dma_size = io_timers_channel_mapping.element[timer_index].channel_count_including_gaps * - ONE_MOTOR_BUFF_SIZE; - io_timer_set_dshot_mode(timer_index, _dshot_frequency, - io_timers_channel_mapping.element[timer_index].channel_count_including_gaps); +void up_dshot_trigger(void) +{ + for (unsigned channel = 0; channel < MAX_TIMER_IO_CHANNELS; channel++) { + if (_channels_init_mask & (1 << channel)) { + int ret = dshot_output_timer_init(channel); - if (dshot_handler[timer_index].dma_handle != NULL) { - stm32_dmafree(dshot_handler[timer_index].dma_handle); + if (ret != OK) { + // TODO: what to do here? + return; } - - dshot_handler[timer_index].dma_handle = stm32_dmachannel(io_timers[timer_index].dshot.dmamap); } + } + for (unsigned timer = 0; timer < DSHOT_TIMERS; ++timer) { + if (_timers_init_mask & (1 << timer)) { - if (NULL == dshot_handler[timer_index].dma_handle) { - ret_val = ERROR; - } - } + if (dshot_handler[timer].dma_handle == NULL) { + dshot_handler[timer].dma_size = io_timers_channel_mapping.element[timer].channel_count_including_gaps * + ONE_MOTOR_BUFF_SIZE; - for (uint8_t timer = 0; (timer < 1); timer++) { + io_timer_set_dshot_mode(timer, _dshot_frequency, + io_timers_channel_mapping.element[timer].channel_count_including_gaps); - if (true == dshot_handler[timer].init) { + dshot_handler[timer].dma_handle = stm32_dmachannel(io_timers[timer].dshot.dmamap); + + if (NULL == dshot_handler[timer].dma_handle) { + // TODO: how to log this? + return; + } + } // Flush cache so DMA sees the data - up_clean_dcache((uintptr_t)dshot_burst_buffer[timer], - (uintptr_t)dshot_burst_buffer[timer] + - DSHOT_BURST_BUFFER_SIZE(io_timers_channel_mapping.element[timer].channel_count_including_gaps)); + up_clean_dcache((uintptr_t) dshot_burst_buffer[timer], + (uintptr_t) dshot_burst_buffer[timer] + + DSHOT_BURST_BUFFER_SIZE( + io_timers_channel_mapping.element[timer].channel_count_including_gaps)); px4_stm32_dmasetup(dshot_handler[timer].dma_handle, io_timers[timer].base + STM32_GTIM_DMAR_OFFSET, @@ -372,76 +403,65 @@ void do_capture(DMA_HANDLE handle, uint8_t status, void *arg) (void)status; (void)arg; - if (_motor_to_capture >= 4) { - // We only support the first 4 for now. - return; - } + for (unsigned timer = 0; timer < DSHOT_TIMERS; ++timer) { + if (_timers_init_mask & (1 << timer)) { + if (dshot_handler[timer].dma_handle != NULL) { + stm32_dmastop(dshot_handler[timer].dma_handle); + stm32_dmafree(dshot_handler[timer].dma_handle); + dshot_handler[timer].dma_handle = NULL; + } - // TODO: this things are still somewhat hard-coded. And I'm probably confused - // regarding channels and indexes. - const int capture_timer = 0; - const int capture_channel = _motor_to_capture; + // TODO: this doesn't scale to more than 4 motors yet + unsigned capture_channel = _motor_to_capture; - for (unsigned timer = 0; timer < DSHOT_TIMERS; ++timer) { - if (dshot_handler[timer].dma_handle != NULL) { - stm32_dmastop(dshot_handler[timer].dma_handle); - } - } + dshot_handler[timer].dma_size = sizeof(dshot_capture_buffer); - dshot_handler[capture_timer].dma_size = 32; + // TODO: We should probably do this at another level board specific. + // However, right now the dma handles are all hard-coded to the UP(date) source + // rather than the capture compare one. + unsigned timer_channel = timer_io_channels[capture_channel].timer_channel; - if (dshot_handler[0].dma_handle != NULL) { - stm32_dmafree(dshot_handler[0].dma_handle); - } + switch (timer_channel) { + case 1: + dshot_handler[timer].dma_handle = stm32_dmachannel(DMAMAP_DMA12_TIM5CH1_0); + break; - // TODO: We should probably do this at another level board specific. - // However, right now the dma handles are all hard-coded to the UP(date) source - // rather than the capture compare one. - switch (timer_io_channels[_motor_to_capture].timer_channel) { - case 1: - dshot_handler[capture_timer].dma_handle = stm32_dmachannel(DMAMAP_DMA12_TIM5CH1_0); - break; - - case 2: - dshot_handler[capture_timer].dma_handle = stm32_dmachannel(DMAMAP_DMA12_TIM5CH2_0); - break; - - case 3: - dshot_handler[capture_timer].dma_handle = stm32_dmachannel(DMAMAP_DMA12_TIM5CH3_0); - break; - - case 4: - dshot_handler[capture_timer].dma_handle = stm32_dmachannel(DMAMAP_DMA12_TIM5CH4_0); - break; - } + case 2: + dshot_handler[timer].dma_handle = stm32_dmachannel(DMAMAP_DMA12_TIM5CH2_0); + break; - memset(dshot_capture_buffer, 0, sizeof(dshot_capture_buffer)); - up_clean_dcache((uintptr_t)dshot_capture_buffer, - (uintptr_t)dshot_capture_buffer + - dshot_capture_buffer_size); + case 3: + dshot_handler[timer].dma_handle = stm32_dmachannel(DMAMAP_DMA12_TIM5CH3_0); + break; - up_invalidate_dcache((uintptr_t)dshot_capture_buffer, - (uintptr_t)dshot_capture_buffer + - dshot_capture_buffer_size); + case 4: + dshot_handler[timer].dma_handle = stm32_dmachannel(DMAMAP_DMA12_TIM5CH4_0); + break; + } - px4_stm32_dmasetup(dshot_handler[0].dma_handle, - io_timers[capture_timer].base + STM32_GTIM_DMAR_OFFSET, - (uint32_t)(&dshot_capture_buffer[0]), - 32, - DSHOT_TELEMETRY_DMA_SCR); + memset(dshot_capture_buffer, 0, sizeof(dshot_capture_buffer)); + up_clean_dcache((uintptr_t) dshot_capture_buffer, + (uintptr_t) dshot_capture_buffer + + sizeof(dshot_capture_buffer)); - // TODO: check retval? - io_timer_unallocate_channel(capture_channel); - io_timer_channel_init(capture_channel, IOTimerChanMode_CaptureDMA, NULL, NULL); - io_timer_set_enable(true, IOTimerChanMode_CaptureDMA, 1 << capture_channel); + px4_stm32_dmasetup(dshot_handler[timer].dma_handle, + io_timers[timer].base + STM32_GTIM_DMAR_OFFSET, + (uint32_t) dshot_capture_buffer, + sizeof(dshot_capture_buffer), + DSHOT_TELEMETRY_DMA_SCR); - up_input_capture_set(capture_channel, Both, 0, NULL, NULL); + io_timer_unallocate_channel(capture_channel); + io_timer_channel_init(capture_channel, IOTimerChanMode_CaptureDMA, NULL, NULL); + io_timer_set_enable(true, IOTimerChanMode_CaptureDMA, 1 << capture_channel); - io_timer_capture_update_dma_req(capture_timer, false); - io_timer_set_capture_mode(capture_timer, _dshot_frequency, capture_channel); + up_input_capture_set(capture_channel, Both, 0, NULL, NULL); - stm32_dmastart(dshot_handler[capture_timer].dma_handle, NULL, NULL, false); - io_timer_capture_update_dma_req(capture_timer, true); + io_timer_capture_update_dma_req(timer, false); + io_timer_set_capture_mode(timer, _dshot_frequency, capture_channel); + stm32_dmastart(dshot_handler[timer].dma_handle, NULL, NULL, false); + io_timer_capture_update_dma_req(timer, true); + } + } // It takes around 85 us for the ESC to respond, so we should have a result after 150 us, surely. hrt_call_after(&_call, 150, process_capture_results, NULL); @@ -451,54 +471,37 @@ void process_capture_results(void *arg) { (void)arg; - up_invalidate_dcache((uintptr_t)dshot_capture_buffer, - (uintptr_t)dshot_capture_buffer + - dshot_capture_buffer_size); - - if (dshot_handler[0].dma_handle != NULL) { - - stm32_dmastop(dshot_handler[0].dma_handle); - } - - /* Init channels */ - int ret_val = OK; - int channel_mask = 0xf; - int channels_init_mask = 0; - - for (unsigned channel = 0; (channel_mask != 0) && (channel < MAX_TIMER_IO_CHANNELS) && (OK == ret_val); channel++) { - if (channel_mask & (1 << channel)) { - uint8_t timer = timer_io_channels[channel].timer_index; - - if (io_timers[timer].dshot.dma_base == 0) { // board does not configure dshot on this timer - continue; - } - - ret_val = io_timer_unallocate_channel(channel); - ret_val = io_timer_channel_init(channel, - enable_bidirectional_dshot ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot, NULL, NULL); - channel_mask &= ~(1 << channel); - - if (OK == ret_val) { - dshot_handler[timer].init = true; - channels_init_mask |= 1 << channel; - - } else if (ret_val == -EBUSY) { - /* either timer or channel already used - this is not fatal */ - ret_val = 0; + // In case DMA is still set up from the last capture, we clear that. + for (unsigned timer = 0; timer < DSHOT_TIMERS; ++timer) { + if (_timers_init_mask & (1 << timer)) { + if (dshot_handler[timer].dma_handle != NULL) { + stm32_dmastop(dshot_handler[timer].dma_handle); + stm32_dmafree(dshot_handler[timer].dma_handle); + dshot_handler[timer].dma_handle = NULL; } } } + up_invalidate_dcache((uintptr_t)dshot_capture_buffer, + (uintptr_t)dshot_capture_buffer + + sizeof(dshot_capture_buffer)); + // TODO: fix order // TODO: convert from period to erpm _erpms[_motor_to_capture] = calculate_period(); - if (_motor_to_capture == 3) { - if (_erpm_callback != NULL) { - _erpm_callback(_erpms, 4, _erpm_callback_context); + for (unsigned channel = 0; channel < MAX_TIMER_IO_CHANNELS; channel++) { + if (_channels_init_mask & (1 << channel)) { + io_timer_unallocate_channel(channel); + io_timer_channel_init(channel, + enable_bidirectional_dshot ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot, NULL, NULL); } } + if (_erpm_callback != NULL) { + _erpm_callback(_erpms, 4, _erpm_callback_context); + } + _motor_to_capture = (_motor_to_capture + 1) % 4; } @@ -534,6 +537,12 @@ void dshot_motor_data_set(unsigned motor_number, uint16_t throttle, bool telemet } unsigned timer = timer_io_channels[motor_number].timer_index; + + // If this timer is not initialized, we give up here. + if (!(_timers_init_mask & (1 << timer))) { + return; + } + uint32_t *buffer = dshot_burst_buffer[timer]; const io_timers_channel_mapping_element_t *mapping = &io_timers_channel_mapping.element[timer]; unsigned num_motors = mapping->channel_count_including_gaps; From 38b7c3c8a4b18bcd243939431979549db4804c2b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 7 Nov 2022 21:25:59 +1300 Subject: [PATCH 05/12] dshot: add driver stats to dshot status command --- platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c | 7 +++++++ src/drivers/drv_dshot.h | 2 ++ src/drivers/dshot/DShot.cpp | 1 + 3 files changed, 10 insertions(+) diff --git a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c index 1823bf65f6d2..c8eab5e95ca2 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -40,6 +40,7 @@ #include #include +#include #include #include #include @@ -567,4 +568,10 @@ void up_dshot_set_erpm_callback(void(*callback)(uint32_t[], size_t, void *), voi _erpm_callback_context = context; } +void print_driver_stats() +{ + PX4_INFO("dshot driver stats: %lu read, %lu failed nibble, %lu failed CRC, %lu invalid/zero", + read_ok, read_fail_nibble, read_fail_crc, read_fail_zero); +} + #endif diff --git a/src/drivers/drv_dshot.h b/src/drivers/drv_dshot.h index ca13b8fb55f0..2f4ca9903a31 100644 --- a/src/drivers/drv_dshot.h +++ b/src/drivers/drv_dshot.h @@ -141,4 +141,6 @@ __EXPORT extern bool up_dshot_get_periods(uint32_t periods[], size_t num_periods __EXPORT extern void up_dshot_set_erpm_callback(void(*callback)(uint32_t[], size_t, void *), void *context); +__EXPORT extern void print_driver_stats(void); + __END_DECLS diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index 67dccb3d2270..75f32b9c2266 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -738,6 +738,7 @@ int DShot::print_status() PX4_INFO("Outputs on: %s", _outputs_on ? "yes" : "no"); perf_print_counter(_cycle_perf); _mixing_output.printStatus(); + print_driver_stats(); if (_telemetry) { PX4_INFO("telemetry on: %s", _telemetry_device); From 358bb2a23e8369865c6dd5436649c7f6ffd5aa64 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 12 Mar 2023 16:47:44 +1300 Subject: [PATCH 06/12] dshot: update copyright header year Signed-off-by: Julian Oes --- .../nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h | 2 +- .../nuttx/src/px4/stm/stm32_common/io_pins/input_capture.c | 2 +- platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c | 2 +- src/drivers/drv_dshot.h | 2 +- src/drivers/dshot/DShot.cpp | 2 +- src/drivers/dshot/DShot.h | 2 +- 6 files changed, 6 insertions(+), 6 deletions(-) diff --git a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h index ee844eb2f3a4..5ac86c8586ee 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012, 2017 PX4 Development Team. All rights reserved. + * Copyright (C) 2012-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/input_capture.c b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/input_capture.c index 8aa990dc3133..342020bd62de 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/input_capture.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/input_capture.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012-2016 PX4 Development Team. All rights reserved. + * Copyright (C) 2012-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c index 427a23bf2e37..700ee5f2c4e9 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012, 2017, 2022 PX4 Development Team. All rights reserved. + * Copyright (C) 2012-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/drivers/drv_dshot.h b/src/drivers/drv_dshot.h index 2f4ca9903a31..1f7a0aac5228 100644 --- a/src/drivers/drv_dshot.h +++ b/src/drivers/drv_dshot.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2017-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index 75f32b9c2266..c24231b6aa18 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/drivers/dshot/DShot.h b/src/drivers/dshot/DShot.h index 2e0357055a04..90323e682d46 100644 --- a/src/drivers/dshot/DShot.h +++ b/src/drivers/dshot/DShot.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions From 602115d6a5030230d9e1aed261bb4a0e57ce41b6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 26 Apr 2023 16:25:46 +1200 Subject: [PATCH 07/12] dshot: add param to enable bidirectional dshot This adds the param DSHOT_BIDIR_EN to enable bidirectional dshot. Signed-off-by: Julian Oes --- .../src/px4/stm/stm32_common/dshot/dshot.c | 64 +++++++++++-------- src/drivers/drv_dshot.h | 5 +- src/drivers/dshot/DShot.cpp | 2 +- src/drivers/dshot/DShot.h | 3 +- src/drivers/dshot/module.yaml | 10 +++ 5 files changed, 54 insertions(+), 30 deletions(-) diff --git a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c index c8eab5e95ca2..a5e47c7e8418 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -69,8 +69,8 @@ #define DSHOT_DMA_SCR (DMA_SCR_PRIHI | DMA_SCR_MSIZE_32BITS | DMA_SCR_PSIZE_32BITS | DMA_SCR_MINC | \ DMA_SCR_DIR_M2P | DMA_SCR_TCIE | DMA_SCR_HTIE | DMA_SCR_TEIE | DMA_SCR_DMEIE) -#define DSHOT_TELEMETRY_DMA_SCR (DMA_SCR_PRIHI | DMA_SCR_MSIZE_16BITS | DMA_SCR_PSIZE_16BITS | DMA_SCR_MINC | \ - DMA_SCR_DIR_P2M | DMA_SCR_TCIE | DMA_SCR_TEIE | DMA_SCR_DMEIE) +#define DSHOT_BIDIRECTIONAL_DMA_SCR (DMA_SCR_PRIHI | DMA_SCR_MSIZE_16BITS | DMA_SCR_PSIZE_16BITS | DMA_SCR_MINC | \ + DMA_SCR_DIR_P2M | DMA_SCR_TCIE | DMA_SCR_TEIE | DMA_SCR_DMEIE) typedef struct dshot_handler_t { DMA_HANDLE dma_handle; @@ -98,13 +98,14 @@ static void do_capture(DMA_HANDLE handle, uint8_t status, void *arg); static void process_capture_results(void *arg); static unsigned calculate_period(void); static int dshot_output_timer_init(unsigned channel); +static int dshot_output_timer_deinit(unsigned channel); static uint32_t read_ok = 0; static uint32_t read_fail_nibble = 0; static uint32_t read_fail_crc = 0; static uint32_t read_fail_zero = 0; -static bool enable_bidirectional_dshot = true; +static bool bidirectional_dshot_enabled = true; static uint32_t _dshot_frequency = 0; static int _timers_init_mask = 0; @@ -121,34 +122,34 @@ uint8_t nibbles_from_mapped(uint8_t mapped) { switch (mapped) { case 0x19: - return 0x0; + return 0x00; case 0x1B: - return 0x1; + return 0x01; case 0x12: - return 0x2; + return 0x02; case 0x13: - return 0x3; + return 0x03; case 0x1D: - return 0x4; + return 0x04; case 0x15: - return 0x5; + return 0x05; case 0x16: - return 6; + return 0x06; case 0x17: - return 7; + return 0x07; case 0x1a: - return 8; + return 0x08; case 0x09: - return 9; + return 0x09; case 0x0A: return 0x0A; @@ -170,7 +171,7 @@ uint8_t nibbles_from_mapped(uint8_t mapped) default: // Unknown mapped - return 0xff; + return 0xFF; } } @@ -256,11 +257,15 @@ unsigned calculate_period(void) return period; } +int dshot_output_timer_deinit(unsigned channel) +{ + return io_timer_unallocate_channel(channel); +} + int dshot_output_timer_init(unsigned channel) { - io_timer_unallocate_channel(channel); int ret = io_timer_channel_init(channel, - enable_bidirectional_dshot ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot, NULL, NULL); + bidirectional_dshot_enabled ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot, NULL, NULL); if (ret == -EBUSY) { // either timer or channel already used - this is not fatal @@ -271,8 +276,9 @@ int dshot_output_timer_init(unsigned channel) } } -int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq) +int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bidirectional_dshot) { + bidirectional_dshot_enabled = enable_bidirectional_dshot; _dshot_frequency = dshot_pwm_freq; for (unsigned channel = 0; channel < MAX_TIMER_IO_CHANNELS; channel++) { @@ -347,7 +353,15 @@ void up_dshot_trigger(void) for (unsigned channel = 0; channel < MAX_TIMER_IO_CHANNELS; channel++) { if (_channels_init_mask & (1 << channel)) { - int ret = dshot_output_timer_init(channel); + + // For bidirectional dshot we need to re-initialize the timers every time here. + // In normal mode, we just do it once. + int ret = OK; + + if (bidirectional_dshot_enabled) { + dshot_output_timer_deinit(channel); + ret = dshot_output_timer_init(channel); + } if (ret != OK) { // TODO: what to do here? @@ -389,12 +403,12 @@ void up_dshot_trigger(void) // Clean UDE flag before DMA is started io_timer_update_dma_req(timer, false); // Trigger DMA (DShot Outputs) - stm32_dmastart(dshot_handler[timer].dma_handle, do_capture, NULL, false); + stm32_dmastart(dshot_handler[timer].dma_handle, bidirectional_dshot_enabled ? do_capture : NULL, NULL, false); io_timer_update_dma_req(timer, true); } } - io_timer_set_enable(true, enable_bidirectional_dshot ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot, + io_timer_set_enable(true, bidirectional_dshot_enabled ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot, IO_TIMER_ALL_MODES_CHANNELS); } @@ -449,7 +463,7 @@ void do_capture(DMA_HANDLE handle, uint8_t status, void *arg) io_timers[timer].base + STM32_GTIM_DMAR_OFFSET, (uint32_t) dshot_capture_buffer, sizeof(dshot_capture_buffer), - DSHOT_TELEMETRY_DMA_SCR); + DSHOT_BIDIRECTIONAL_DMA_SCR); io_timer_unallocate_channel(capture_channel); io_timer_channel_init(capture_channel, IOTimerChanMode_CaptureDMA, NULL, NULL); @@ -494,8 +508,7 @@ void process_capture_results(void *arg) for (unsigned channel = 0; channel < MAX_TIMER_IO_CHANNELS; channel++) { if (_channels_init_mask & (1 << channel)) { io_timer_unallocate_channel(channel); - io_timer_channel_init(channel, - enable_bidirectional_dshot ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot, NULL, NULL); + io_timer_channel_init(channel, IOTimerChanMode_DshotInverted, NULL, NULL); } } @@ -506,7 +519,6 @@ void process_capture_results(void *arg) _motor_to_capture = (_motor_to_capture + 1) % 4; } - /** * bits 1-11 - throttle value (0-47 are reserved, 48-2047 give 2000 steps of throttle resolution) * bit 12 - dshot telemetry enable/disable @@ -530,7 +542,7 @@ void dshot_motor_data_set(unsigned motor_number, uint16_t throttle, bool telemet csum_data >>= NIBBLES_SIZE; } - if (enable_bidirectional_dshot) { + if (bidirectional_dshot_enabled) { packet |= ((~checksum) & 0x0F); } else { @@ -558,7 +570,7 @@ void dshot_motor_data_set(unsigned motor_number, uint16_t throttle, bool telemet int up_dshot_arm(bool armed) { - return io_timer_set_enable(armed, enable_bidirectional_dshot ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot, + return io_timer_set_enable(armed, bidirectional_dshot_enabled ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot, IO_TIMER_ALL_MODES_CHANNELS); } diff --git a/src/drivers/drv_dshot.h b/src/drivers/drv_dshot.h index 1f7a0aac5228..1a304725faa6 100644 --- a/src/drivers/drv_dshot.h +++ b/src/drivers/drv_dshot.h @@ -88,10 +88,11 @@ typedef enum { * @param channel_mask Bitmask of channels (LSB = channel 0) to enable. * This allows some of the channels to remain configured * as GPIOs or as another function. Already used channels/timers will not be configured as DShot - * @param dshot_freq Frequency of DSHOT signal. Usually DSHOT150, DSHOT300, DSHOT600 or DSHOT1200 + * @param dshot_freq Frequency of DSHOT signal. Usually DSHOT150, DSHOT300, DSHOT600 or DSHOT1200 + * @param enable_bidirectional_dshot Whether to use bidirectional dshot * @return <0 on error, the initialized channels mask. */ -__EXPORT extern int up_dshot_init(uint32_t channel_mask, unsigned dshot_freq); +__EXPORT extern int up_dshot_init(uint32_t channel_mask, unsigned dshot_freq, bool enable_bidirectional_dshot); /** * Set Dshot motor data, used by up_dshot_motor_data_set() and up_dshot_motor_command() (internal method) diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index c24231b6aa18..2cd046dde684 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -151,7 +151,7 @@ void DShot::enable_dshot_outputs(const bool enabled) } } - int ret = up_dshot_init(_output_mask, dshot_frequency); + int ret = up_dshot_init(_output_mask, dshot_frequency, _param_bidirectional_enable.get()); if (ret < 0) { PX4_ERR("up_dshot_init failed (%i)", ret); diff --git a/src/drivers/dshot/DShot.h b/src/drivers/dshot/DShot.h index 90323e682d46..45a948b2b520 100644 --- a/src/drivers/dshot/DShot.h +++ b/src/drivers/dshot/DShot.h @@ -181,6 +181,7 @@ class DShot final : public ModuleBase, public OutputModuleInterface (ParamBool) _param_dshot_3d_enable, (ParamInt) _param_dshot_3d_dead_h, (ParamInt) _param_dshot_3d_dead_l, - (ParamInt) _param_mot_pole_count + (ParamInt) _param_mot_pole_count, + (ParamBool) _param_bidirectional_enable ) }; diff --git a/src/drivers/dshot/module.yaml b/src/drivers/dshot/module.yaml index edd38edd23f1..e2dcf8bc97e8 100644 --- a/src/drivers/dshot/module.yaml +++ b/src/drivers/dshot/module.yaml @@ -33,6 +33,16 @@ parameters: When mixer outputs 1000 or value inside DSHOT 3D deadband, DShot 0 is sent. type: boolean default: 0 + DSHOT_BIDIR_EN: + description: + short: Enable bidirectional DShot + long: | + This parameter enables bidirectional DShot which provides RPM feedback. + Note that this requires ESCs that support bidirectional DSHot, e.g. BlHeli32. + This is not the same as DShot telemetry which requires an additional serial connection. + type: boolean + default: 0 + reboot_required: true DSHOT_3D_DEAD_H: description: short: DSHOT 3D deadband high From e0f1608843d2ceb9ef7a3743e50371b558a41354 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 27 Apr 2023 08:48:39 +1200 Subject: [PATCH 08/12] dshot: try to make DMA channels more general This could potentially enable other H7 based platforms. Signed-off-by: Julian Oes --- .../nuttx/src/px4/stm/stm32_common/dshot/dshot.c | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c index a5e47c7e8418..30d23672cb66 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -431,26 +431,25 @@ void do_capture(DMA_HANDLE handle, uint8_t status, void *arg) dshot_handler[timer].dma_size = sizeof(dshot_capture_buffer); - // TODO: We should probably do this at another level board specific. - // However, right now the dma handles are all hard-coded to the UP(date) source - // rather than the capture compare one. + // Instead of using the UP DMA channel, we need to use the CH1-4 channels. + // It turns out that we can infer the DMA channel index by starting from the UP channel -5. unsigned timer_channel = timer_io_channels[capture_channel].timer_channel; switch (timer_channel) { case 1: - dshot_handler[timer].dma_handle = stm32_dmachannel(DMAMAP_DMA12_TIM5CH1_0); + dshot_handler[timer].dma_handle = stm32_dmachannel(io_timers[timer].dshot.dmamap - 5 + 1); break; case 2: - dshot_handler[timer].dma_handle = stm32_dmachannel(DMAMAP_DMA12_TIM5CH2_0); + dshot_handler[timer].dma_handle = stm32_dmachannel(io_timers[timer].dshot.dmamap - 5 + 2); break; case 3: - dshot_handler[timer].dma_handle = stm32_dmachannel(DMAMAP_DMA12_TIM5CH3_0); + dshot_handler[timer].dma_handle = stm32_dmachannel(io_timers[timer].dshot.dmamap - 5 + 3); break; case 4: - dshot_handler[timer].dma_handle = stm32_dmachannel(DMAMAP_DMA12_TIM5CH4_0); + dshot_handler[timer].dma_handle = stm32_dmachannel(io_timers[timer].dshot.dmamap - 5 + 4); break; } From 3fcc64e56d0898ca859035ce3f5704dee205b504 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 27 Apr 2023 12:47:19 +1200 Subject: [PATCH 09/12] dshot: only publish when all motor RPM is there Otherwise we publish a lot of duplicate data for little gain. Signed-off-by: Julian Oes --- platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c | 5 ++++- src/drivers/dshot/DShot.cpp | 1 + 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c index 30d23672cb66..757c8d28a8eb 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -512,7 +512,10 @@ void process_capture_results(void *arg) } if (_erpm_callback != NULL) { - _erpm_callback(_erpms, 4, _erpm_callback_context); + // Only publish every 4th time once all measurements have come in. + if (_motor_to_capture == 3) { + _erpm_callback(_erpms, 4, _erpm_callback_context); + } } _motor_to_capture = (_motor_to_capture + 1) % 4; diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index 2cd046dde684..3cd14b4787d1 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -622,6 +622,7 @@ void DShot::erpm_trampoline(uint32_t erpms[], size_t num_erpms, void *context) void DShot::erpm(uint32_t erpms[], size_t num_erpms) { + // TODO: this is hard-coded to 4 motors esc_status_s &esc_status = _esc_status_pub.get(); esc_status = {}; esc_status.timestamp = hrt_absolute_time(); From 7eb919a78c4c288a2fa60544f8cc721e8c2c45c7 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 27 Apr 2023 15:00:08 +1200 Subject: [PATCH 10/12] dshot: convert period to RPM This calculates the actual RPM from the period transmitted via dshot to based on the pole count. Signed-off-by: Julian Oes --- .../src/px4/stm/stm32_common/dshot/dshot.c | 23 ++++++++++++++----- src/drivers/drv_dshot.h | 2 +- src/drivers/dshot/DShot.cpp | 11 ++++----- src/drivers/dshot/DShot.h | 4 ++-- 4 files changed, 25 insertions(+), 15 deletions(-) diff --git a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c index 757c8d28a8eb..b873b882a210 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -113,9 +113,9 @@ static int _channels_init_mask = 0; // We only support capture on the first timer (usually 4 channels) for now. static uint32_t _motor_to_capture = 0; -static uint32_t _erpms[4] = {}; +static int32_t _erpms[4] = {}; -static void(*_erpm_callback)(uint32_t[], size_t, void *) = NULL; +static void(*_erpm_callback)(int32_t[], size_t, void *) = NULL; static void *_erpm_callback_context = NULL; uint8_t nibbles_from_mapped(uint8_t mapped) @@ -500,9 +500,20 @@ void process_capture_results(void *arg) (uintptr_t)dshot_capture_buffer + sizeof(dshot_capture_buffer)); - // TODO: fix order - // TODO: convert from period to erpm - _erpms[_motor_to_capture] = calculate_period(); + const unsigned period = calculate_period(); + + if (period == 0) { + // If the parsing failed, we get 0. + _erpms[_motor_to_capture] = 0; + + } else if (period == 65408) { + // For still, we get this magic 65408 value. + _erpms[_motor_to_capture] = 0; + + } else { + // from period in us to eRPM + _erpms[_motor_to_capture] = 1000000 * 60 / period; + } for (unsigned channel = 0; channel < MAX_TIMER_IO_CHANNELS; channel++) { if (_channels_init_mask & (1 << channel)) { @@ -576,7 +587,7 @@ int up_dshot_arm(bool armed) IO_TIMER_ALL_MODES_CHANNELS); } -void up_dshot_set_erpm_callback(void(*callback)(uint32_t[], size_t, void *), void *context) +void up_dshot_set_erpm_callback(void(*callback)(int32_t[], size_t, void *), void *context) { _erpm_callback = callback; _erpm_callback_context = context; diff --git a/src/drivers/drv_dshot.h b/src/drivers/drv_dshot.h index 1a304725faa6..0c03a1b043d4 100644 --- a/src/drivers/drv_dshot.h +++ b/src/drivers/drv_dshot.h @@ -140,7 +140,7 @@ __EXPORT extern int up_dshot_arm(bool armed); __EXPORT extern bool up_dshot_get_periods(uint32_t periods[], size_t num_periods); -__EXPORT extern void up_dshot_set_erpm_callback(void(*callback)(uint32_t[], size_t, void *), void *context); +__EXPORT extern void up_dshot_set_erpm_callback(void(*callback)(int32_t[], size_t, void *), void *context); __EXPORT extern void print_driver_stats(void); diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index 3cd14b4787d1..f799aed8a854 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -614,26 +614,25 @@ void DShot::update_params() } } -void DShot::erpm_trampoline(uint32_t erpms[], size_t num_erpms, void *context) +void DShot::erpm_trampoline(int32_t erpms[], size_t num_erpms, void *context) { DShot *self = static_cast(context); self->erpm(erpms, num_erpms); } -void DShot::erpm(uint32_t erpms[], size_t num_erpms) +void DShot::erpm(int32_t erpms[], size_t num_erpms) { - // TODO: this is hard-coded to 4 motors esc_status_s &esc_status = _esc_status_pub.get(); esc_status = {}; esc_status.timestamp = hrt_absolute_time(); esc_status.counter = _esc_status_counter++; - esc_status.esc_count = 4; + esc_status.esc_count = num_erpms; esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_DSHOT; esc_status.esc_armed_flags = _outputs_on; - for (unsigned i = 0; i < 4 && i < esc_status_s::CONNECTED_ESC_MAX; ++i) { + for (unsigned i = 0; i < num_erpms && i < esc_status_s::CONNECTED_ESC_MAX; ++i) { esc_status.esc[i].timestamp = hrt_absolute_time(); - esc_status.esc[i].esc_rpm = erpms[i]; + esc_status.esc[i].esc_rpm = erpms[i] / (_param_mot_pole_count.get() / 2); } _esc_status_pub.update(); diff --git a/src/drivers/dshot/DShot.h b/src/drivers/dshot/DShot.h index 45a948b2b520..36069aae7126 100644 --- a/src/drivers/dshot/DShot.h +++ b/src/drivers/dshot/DShot.h @@ -143,8 +143,8 @@ class DShot final : public ModuleBase, public OutputModuleInterface void handle_vehicle_commands(); - static void erpm_trampoline(uint32_t erpms[], size_t num_erpms, void *context); - void erpm(uint32_t erpms[], size_t num_erpms); + static void erpm_trampoline(int32_t erpms[], size_t num_erpms, void *context); + void erpm(int32_t erpms[], size_t num_erpms); MixingOutput _mixing_output {PARAM_PREFIX, DIRECT_PWM_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; uint32_t _reversible_outputs{}; From 833be7ec7b7404dab030291d69032a617f5941ad Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 19 May 2023 09:36:01 +1200 Subject: [PATCH 11/12] dshot: fix spikes/parse errors Somehow shifting the timer enable around made a big difference and got rid of the spikes. Signed-off-by: Julian Oes --- platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c index b873b882a210..16e8809a2789 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -370,6 +370,9 @@ void up_dshot_trigger(void) } } + io_timer_set_enable(true, bidirectional_dshot_enabled ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot, + IO_TIMER_ALL_MODES_CHANNELS); + for (unsigned timer = 0; timer < DSHOT_TIMERS; ++timer) { if (_timers_init_mask & (1 << timer)) { @@ -407,9 +410,6 @@ void up_dshot_trigger(void) io_timer_update_dma_req(timer, true); } } - - io_timer_set_enable(true, bidirectional_dshot_enabled ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot, - IO_TIMER_ALL_MODES_CHANNELS); } void do_capture(DMA_HANDLE handle, uint8_t status, void *arg) From b4412e9fbc35d81a794b1eb994529d8ef01a3f3a Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 19 May 2023 09:55:16 +1200 Subject: [PATCH 12/12] dshot: note regarding different DShot timings Signed-off-by: Julian Oes --- platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c index 16e8809a2789..57550fe1cf08 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -195,7 +195,8 @@ unsigned calculate_period(void) break; } - // TODO: Doesn't cope with DShot frequency other than 600 + // This seemss to work with dshot 150, 300, 600, 1200 + // The values were found by trial and error to get the quantization just right. const uint32_t bits = (dshot_capture_buffer[i] - previous + 5) / 20; for (unsigned bit = 0; bit < bits; ++bit) {