From a5b84c32e153826d945d46916a911ca7d4a2dd46 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Tue, 15 Oct 2024 16:54:43 -0800 Subject: [PATCH 01/19] added function to get DMA map for Timer Channel --- .../src/px4/stm/stm32_common/dshot/dshot.c | 2 +- .../stm/stm32_common/include/px4_arch/dshot.h | 5 +- .../px4_arch/io_timer_hw_description.h | 9 +- .../stm32h7/include/px4_arch/hw_description.h | 165 ++++++++++++++++-- .../px4_arch/io_timer_hw_description.h | 9 +- 5 files changed, 169 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 00491e976831..1156f5ecc6bf 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -141,7 +141,7 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bi io_timer_set_dshot_mode(timer_index, dshot_pwm_freq, 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); + dshot_handler[timer_index].dma_handle = stm32_dmachannel(io_timers[timer_index].dshot.dma_map_up); if (NULL == dshot_handler[timer_index].dma_handle) { ret_val = ERROR; diff --git a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/dshot.h b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/dshot.h index 93811a898cf7..da828aeb5729 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/dshot.h +++ b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/dshot.h @@ -58,5 +58,8 @@ */ typedef struct dshot_conf_t { uint32_t dma_base; - uint32_t dmamap; + uint32_t dma_map_up; + uint32_t dma_map_ch[4]; + // TODO: we just check for non-zero to determine if channel is available + // uint32_t dma_ch_count; } dshot_conf_t; diff --git a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer_hw_description.h b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer_hw_description.h index 55ea439ef73f..b8ada773dcf1 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer_hw_description.h +++ b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer_hw_description.h @@ -103,7 +103,7 @@ static inline constexpr timer_io_channels_t initIOTimerChannelOutputClear(const } -static inline constexpr io_timers_t initIOTimer(Timer::Timer timer, DMA dshot_dma = {}) +static inline constexpr io_timers_t initIOTimer(Timer::Timer timer, DMA dma = {}) { bool nuttx_config_timer_enabled = false; io_timers_t ret{}; @@ -268,9 +268,10 @@ static inline constexpr io_timers_t initIOTimer(Timer::Timer timer, DMA dshot_dm constexpr_assert(!nuttx_config_timer_enabled, "IO Timer requires NuttX timer config to be disabled (STM32_TIMx)"); // DShot - if (dshot_dma.index != DMA::Invalid) { - ret.dshot.dma_base = getDMABaseRegister(dshot_dma); - ret.dshot.dmamap = getTimerUpdateDMAMap(timer, dshot_dma); + if (dma.index != DMA::Invalid) { + ret.dshot.dma_base = getDMABaseRegister(dma); + ret.dshot.dma_map_up = getTimerUpdateDMAMap(timer, dma); + getTimerChannelDMAMap(timer, dma, ret.dshot.dma_map_ch); } return ret; diff --git a/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/hw_description.h b/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/hw_description.h index a93b9538bee6..40bc8a8ceed6 100644 --- a/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/hw_description.h +++ b/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/hw_description.h @@ -35,47 +35,190 @@ #include "../../../stm32_common/include/px4_arch/hw_description.h" +static inline constexpr void getTimerChannelDMAMap(Timer::Timer timer, const DMA &dma, uint32_t* dma_map_ch) +{ + switch (timer) { + case Timer::Timer1: + if (dma.index == DMA::Index1) { + dma_map_ch[0] = DMAMAP_DMA12_TIM1CH1_0; + dma_map_ch[1] = DMAMAP_DMA12_TIM1CH2_0; + dma_map_ch[2] = DMAMAP_DMA12_TIM1CH3_0; + dma_map_ch[3] = DMAMAP_DMA12_TIM1CH4_0; + } else { + dma_map_ch[0] = DMAMAP_DMA12_TIM1CH1_1; + dma_map_ch[1] = DMAMAP_DMA12_TIM1CH2_1; + dma_map_ch[2] = DMAMAP_DMA12_TIM1CH3_1; + dma_map_ch[3] = DMAMAP_DMA12_TIM1CH4_1; + } + break; + + case Timer::Timer2: + if (dma.index == DMA::Index1) { + dma_map_ch[0] = DMAMAP_DMA12_TIM2CH1_0; + dma_map_ch[1] = DMAMAP_DMA12_TIM2CH2_0; + dma_map_ch[2] = DMAMAP_DMA12_TIM2CH3_0; + dma_map_ch[3] = DMAMAP_DMA12_TIM2CH4_0; + } else { + dma_map_ch[0] = DMAMAP_DMA12_TIM2CH1_1; + dma_map_ch[1] = DMAMAP_DMA12_TIM2CH2_1; + dma_map_ch[2] = DMAMAP_DMA12_TIM2CH3_1; + dma_map_ch[3] = DMAMAP_DMA12_TIM2CH4_1; + } + break; + + case Timer::Timer3: + if (dma.index == DMA::Index1) { + dma_map_ch[0] = DMAMAP_DMA12_TIM3CH1_0; + dma_map_ch[1] = DMAMAP_DMA12_TIM3CH2_0; + dma_map_ch[2] = DMAMAP_DMA12_TIM3CH3_0; + dma_map_ch[3] = DMAMAP_DMA12_TIM3CH4_0; + } else { + dma_map_ch[0] = DMAMAP_DMA12_TIM3CH1_1; + dma_map_ch[1] = DMAMAP_DMA12_TIM3CH2_1; + dma_map_ch[2] = DMAMAP_DMA12_TIM3CH3_1; + dma_map_ch[3] = DMAMAP_DMA12_TIM3CH4_1; + } + break; + + case Timer::Timer4: + if (dma.index == DMA::Index1) { + dma_map_ch[0] = DMAMAP_DMA12_TIM4CH1_0; + dma_map_ch[1] = DMAMAP_DMA12_TIM4CH2_0; + dma_map_ch[2] = DMAMAP_DMA12_TIM4CH3_0; + } else { + dma_map_ch[0] = DMAMAP_DMA12_TIM4CH1_1; + dma_map_ch[1] = DMAMAP_DMA12_TIM4CH2_1; + dma_map_ch[2] = DMAMAP_DMA12_TIM4CH3_1; + } + break; + + case Timer::Timer5: + if (dma.index == DMA::Index1) { + dma_map_ch[0] = DMAMAP_DMA12_TIM5CH1_0; + dma_map_ch[1] = DMAMAP_DMA12_TIM5CH2_0; + dma_map_ch[2] = DMAMAP_DMA12_TIM5CH3_0; + dma_map_ch[3] = DMAMAP_DMA12_TIM5CH4_0; + } else { + dma_map_ch[0] = DMAMAP_DMA12_TIM5CH1_1; + dma_map_ch[1] = DMAMAP_DMA12_TIM5CH2_1; + dma_map_ch[2] = DMAMAP_DMA12_TIM5CH3_1; + dma_map_ch[3] = DMAMAP_DMA12_TIM5CH4_1; + } + break; + + case Timer::Timer6: + // No channels available + break; + + case Timer::Timer7: + // No channels available + break; + + case Timer::Timer8: + if (dma.index == DMA::Index1) { + dma_map_ch[0] = DMAMAP_DMA12_TIM8CH1_0; + dma_map_ch[1] = DMAMAP_DMA12_TIM8CH2_0; + dma_map_ch[2] = DMAMAP_DMA12_TIM8CH3_0; + dma_map_ch[3] = DMAMAP_DMA12_TIM8CH4_0; + } else { + dma_map_ch[0] = DMAMAP_DMA12_TIM8CH1_1; + dma_map_ch[1] = DMAMAP_DMA12_TIM8CH2_1; + dma_map_ch[2] = DMAMAP_DMA12_TIM8CH3_1; + dma_map_ch[3] = DMAMAP_DMA12_TIM8CH4_1; + } + break; + + case Timer::Timer9: + // Non-existant + break; + + case Timer::Timer10: + // Non-existant + break; + + case Timer::Timer11: + // Non-existant + break; + + case Timer::Timer12: + // Non-existant + break; + + case Timer::Timer13: + // Non-existant + break; + + case Timer::Timer14: + // Non-existant + break; + + case Timer::Timer15: + if (dma.index == DMA::Index1) { + dma_map_ch[0] = DMAMAP_DMA12_TIM15CH1_0; + } else { + dma_map_ch[0] = DMAMAP_DMA12_TIM15CH1_1; + } + break; + + case Timer::Timer16: + if (dma.index == DMA::Index1) { + dma_map_ch[0] = DMAMAP_DMA12_TIM16CH1_0; + } else { + dma_map_ch[0] = DMAMAP_DMA12_TIM16CH1_1; + } + break; + + case Timer::Timer17: + if (dma.index == DMA::Index1) { + dma_map_ch[0] = DMAMAP_DMA12_TIM17CH1_0; + } else { + dma_map_ch[0] = DMAMAP_DMA12_TIM17CH1_1; + } + break; + } +} + static inline constexpr uint32_t getTimerUpdateDMAMap(Timer::Timer timer, const DMA &dma) { - uint32_t dma_map = 0; + uint32_t dma_map_up = 0; switch (timer) { case Timer::Timer1: - dma_map = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM1UP_0 : DMAMAP_DMA12_TIM1UP_1; + dma_map_up = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM1UP_0 : DMAMAP_DMA12_TIM1UP_1; break; case Timer::Timer2: - dma_map = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM2UP_0 : DMAMAP_DMA12_TIM2UP_1; + dma_map_up = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM2UP_0 : DMAMAP_DMA12_TIM2UP_1; break; case Timer::Timer3: - dma_map = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM3UP_0 : DMAMAP_DMA12_TIM3UP_1; + dma_map_up = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM3UP_0 : DMAMAP_DMA12_TIM3UP_1; break; case Timer::Timer4: - dma_map = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM4UP_0 : DMAMAP_DMA12_TIM4UP_1; + dma_map_up = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM4UP_0 : DMAMAP_DMA12_TIM4UP_1; break; case Timer::Timer5: - dma_map = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM5UP_0 : DMAMAP_DMA12_TIM5UP_1; + dma_map_up = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM5UP_0 : DMAMAP_DMA12_TIM5UP_1; break; case Timer::Timer6: - dma_map = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM6UP_0 : DMAMAP_DMA12_TIM6UP_1; + dma_map_up = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM6UP_0 : DMAMAP_DMA12_TIM6UP_1; break; case Timer::Timer7: - dma_map = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM7UP_0 : DMAMAP_DMA12_TIM7UP_1; + dma_map_up = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM7UP_0 : DMAMAP_DMA12_TIM7UP_1; break; case Timer::Timer8: - dma_map = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM8UP_0 : DMAMAP_DMA12_TIM8UP_1; + dma_map_up = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM8UP_0 : DMAMAP_DMA12_TIM8UP_1; break; @@ -91,6 +234,6 @@ static inline constexpr uint32_t getTimerUpdateDMAMap(Timer::Timer timer, const break; } - constexpr_assert(dma_map != 0, "Invalid DMA config for given timer"); - return dma_map; + constexpr_assert(dma_map_up != 0, "Invalid DMA config for given timer"); + return dma_map_up; } diff --git a/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/io_timer_hw_description.h b/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/io_timer_hw_description.h index b1299b04e336..aa8cf8d44442 100644 --- a/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/io_timer_hw_description.h +++ b/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/io_timer_hw_description.h @@ -135,7 +135,7 @@ static inline constexpr timer_io_channels_t initIOTimerChannel(const io_timers_t return ret; } -static inline constexpr io_timers_t initIOTimer(Timer::Timer timer, DMA dshot_dma = {}) +static inline constexpr io_timers_t initIOTimer(Timer::Timer timer, DMA dma = {}) { bool nuttx_config_timer_enabled = false; io_timers_t ret{}; @@ -306,9 +306,10 @@ static inline constexpr io_timers_t initIOTimer(Timer::Timer timer, DMA dshot_dm constexpr_assert(!nuttx_config_timer_enabled, "IO Timer requires NuttX timer config to be disabled (STM32_TIMx)"); // DShot - if (dshot_dma.index != DMA::Invalid) { - ret.dshot.dma_base = getDMABaseRegister(dshot_dma); - ret.dshot.dmamap = getTimerUpdateDMAMap(timer, dshot_dma); + if (dma.index != DMA::Invalid) { + ret.dshot.dma_base = getDMABaseRegister(dma); + ret.dshot.dma_map_up = getTimerUpdateDMAMap(timer, dma); + getTimerChannelDMAMap(timer, dma, ret.dshot.dma_map_ch); } return ret; From 08eb6b4a6ed52d75fcc4f63db47387fc866171bd Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Wed, 16 Oct 2024 14:06:46 -0800 Subject: [PATCH 02/19] brought over julians work --- .../src/px4/stm/stm32_common/dshot/dshot.c | 480 +++++++++++++++--- .../stm32_common/include/px4_arch/io_timer.h | 6 +- .../stm/stm32_common/io_pins/input_capture.c | 4 +- .../px4/stm/stm32_common/io_pins/io_timer.c | 82 ++- 4 files changed, 510 insertions(+), 62 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 1156f5ecc6bf..319e285db98e 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -45,6 +45,10 @@ #include #include +#include +#include +#include +#include #define MOTOR_PWM_BIT_1 14u #define MOTOR_PWM_BIT_0 7u @@ -63,8 +67,11 @@ #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) +// TODO: why 16bit? +#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 { - bool init; DMA_HANDLE dma_handle; uint32_t dma_size; } dshot_handler_t; @@ -82,36 +89,200 @@ 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] = {}; -int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bidirectional_dshot) +static uint16_t dshot_capture_buffer[32] px4_cache_aligned_data() = {}; + +// TODO: audit local variables +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 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 bidirectional_dshot_enabled = false; + +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 int32_t _erpms[16] = {}; + +// static void(*_erpm_callback)(int32_t[], size_t, void *) = NULL; +// static void *_erpm_callback_context = NULL; + +uint8_t nibbles_from_mapped(uint8_t mapped) { - unsigned buffer_offset = 0; + switch (mapped) { + case 0x19: + return 0x00; + + case 0x1B: + return 0x01; + + case 0x12: + return 0x02; + + case 0x13: + return 0x03; + + case 0x1D: + return 0x04; + + case 0x15: + return 0x05; + + case 0x16: + return 0x06; + + case 0x17: + return 0x07; + + case 0x1a: + return 0x08; + + case 0x09: + return 0x09; + + case 0x0A: + return 0x0A; + + case 0x0B: + return 0x0B; - for (int timer_index = 0; timer_index < DSHOT_TIMERS; timer_index++) { - dshot_handler[timer_index].init = false; + case 0x1E: + return 0x0C; + + case 0x0D: + return 0x0D; + + case 0x0E: + return 0x0E; + + case 0x0F: + return 0x0F; + + default: + // Unknown mapped + return 0xFF; } +} - for (unsigned timer = 0; timer < DSHOT_TIMERS; ++timer) { - if (io_timers[timer].base == 0) { // no more timers configured - break; +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; + } + + // 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) { + value = value << 1; + value |= high; + ++shifted; + } + + // The next edge toggles. + high = !high; } - // 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); + previous = dshot_capture_buffer[i]; + } - if (buffer_offset > sizeof(dshot_burst_buffer_array)) { - return -EINVAL; // something is wrong with the board configuration or some other logic + 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; } - /* Init channels */ - int ret_val = OK; - int channels_init_mask = 0; + ++read_ok; + return period; +} + +int dshot_output_timer_deinit(unsigned channel) +{ + return io_timer_unallocate_channel(channel); +} + +int dshot_output_timer_init(unsigned channel) +{ + int ret = io_timer_channel_init(channel, + bidirectional_dshot_enabled ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot, NULL, NULL); + + if (ret == -EBUSY) { + // either timer or channel already used - this is not fatal + return OK; + + } else { + return ret; + } +} - for (unsigned channel = 0; (channel_mask != 0) && (channel < MAX_TIMER_IO_CHANNELS) && (OK == ret_val); channel++) { +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++) { if (channel_mask & (1 << channel)) { uint8_t timer = timer_io_channels[channel].timer_index; @@ -119,65 +290,113 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bi continue; } - ret_val = io_timer_channel_init(channel, IOTimerChanMode_Dshot, NULL, NULL); - channel_mask &= ~(1 << channel); + int ret = dshot_output_timer_init(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; + if (ret != OK) { + return ret; } + + _channels_init_mask |= (1 << channel); + _timers_init_mask |= (1 << timer); } } - for (uint8_t timer_index = 0; (timer_index < DSHOT_TIMERS) && (OK == ret_val); timer_index++) { + 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; + + io_timer_set_dshot_mode(timer, _dshot_frequency, + io_timers_channel_mapping.element[timer].channel_count_including_gaps); - 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_timers_channel_mapping.element[timer_index].channel_count_including_gaps); + dshot_handler[timer].dma_handle = stm32_dmachannel(io_timers[timer].dshot.dma_map_up); - dshot_handler[timer_index].dma_handle = stm32_dmachannel(io_timers[timer_index].dshot.dma_map_up); + if (NULL == dshot_handler[timer].dma_handle) { + // TODO: how to log this? + return -ENOSR; + } - if (NULL == dshot_handler[timer_index].dma_handle) { - ret_val = ERROR; + // 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; } } } - return ret_val == OK ? channels_init_mask : ret_val; -} + unsigned buffer_offset = 0; -int up_bdshot_get_erpm(uint8_t channel, int *erpm) -{ - // Not implemented - return -1; -} + 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; + } -int up_bdshot_channel_status(uint8_t channel) -{ - // Not implemented - return -1; -} + // 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); -void up_bdshot_status(void) -{ + if (buffer_offset > sizeof(dshot_burst_buffer_array)) { + return -EINVAL; // something is wrong with the board configuration or some other logic + } + } + } + + return _channels_init_mask; } void up_dshot_trigger(void) { - for (uint8_t timer = 0; (timer < DSHOT_TIMERS); timer++) { - if (true == dshot_handler[timer].init) { + for (unsigned channel = 0; channel < MAX_TIMER_IO_CHANNELS; channel++) { + if (_channels_init_mask & (1 << 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? + return; + } + } + } + + 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)) { + + 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; + + io_timer_set_dshot_mode(timer, _dshot_frequency, + io_timers_channel_mapping.element[timer].channel_count_including_gaps); + + dshot_handler[timer].dma_handle = stm32_dmachannel(io_timers[timer].dshot.dma_map_up); + + 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, @@ -188,11 +407,132 @@ 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, bidirectional_dshot_enabled ? do_capture : NULL, NULL, false); io_timer_update_dma_req(timer, true); + } + } +} + +void do_capture(DMA_HANDLE handle, uint8_t status, void *arg) +{ + (void)handle; + (void)status; + (void)arg; + + 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 doesn't scale to more than 4 motors yet + unsigned capture_channel = _motor_to_capture; + dshot_handler[timer].dma_size = sizeof(dshot_capture_buffer); + + // 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(io_timers[timer].dshot.dma_map_up - 5 + 1); + // dshot_handler[timer].dma_handle = stm32_dmachannel(io_timers[timer].dshot.dma_map_ch[0]); + + break; + + case 2: + dshot_handler[timer].dma_handle = stm32_dmachannel(io_timers[timer].dshot.dma_map_up - 5 + 2); + break; + + case 3: + dshot_handler[timer].dma_handle = stm32_dmachannel(io_timers[timer].dshot.dma_map_up - 5 + 3); + break; + + case 4: + dshot_handler[timer].dma_handle = stm32_dmachannel(io_timers[timer].dshot.dma_map_up - 5 + 4); + break; + } + + 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)); + + 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_BIDIRECTIONAL_DMA_SCR); + + 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(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); +} + +void process_capture_results(void *arg) +{ + (void)arg; + + // 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)); + + 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)) { + io_timer_unallocate_channel(channel); + io_timer_channel_init(channel, IOTimerChanMode_DshotInverted, NULL, NULL); } } + + // if (_erpm_callback != NULL) { + // // 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; } /** @@ -235,7 +575,29 @@ 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, bidirectional_dshot_enabled ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot, + IO_TIMER_ALL_MODES_CHANNELS); +} + +int up_bdshot_get_erpm(uint8_t channel, int *erpm) +{ + // Not implemented + // *erpm = _erpms[channel]; + // return 0; + return -1; +} + +int up_bdshot_channel_status(uint8_t channel) +{ + // return <0 on error / not supported, 0 on offline, 1 on online + // return 1; + + // Not implemented + return -1; +} + +void up_bdshot_status(void) +{ } #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..1c28b149cdd2 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,7 +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, unsigned channel); /** * Reserve a timer * @return 0 on success (if not used yet, or already set to the mode) 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 8d43bfc7352b..c16c8bc7398a 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 4246ea3585e5..dda6904570ee 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 @@ -131,6 +131,8 @@ 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 +// TODO: PWM mode2? +#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 +521,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 | 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 | ATIM_EGR_CC3G | ATIM_EGR_CC4G); + rDIER(timer) &= ~(ATIM_DIER_CC1DE | ATIM_DIER_CC2DE | ATIM_DIER_CC3DE | ATIM_DIER_CC4DE); + } +} + int io_timer_set_dshot_mode(uint8_t timer, unsigned dshot_pwm_freq, uint8_t dma_burst_length) { int ret_val = OK; @@ -545,7 +559,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 +588,53 @@ 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, unsigned channel) +{ + rARR(timer) = -1; + 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; + + // TODO: hardcoded for v6x? + 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; + + 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; + + 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; +} + static inline void io_timer_set_PWM_mode(unsigned timer) { rPSC(timer) = (io_timers[timer].clock_freq / BOARD_PWM_FREQ) - 1; @@ -773,6 +834,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 +848,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 +873,14 @@ int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode, rv = io_timer_init_timer(timer, mode); + if (rv == -16) { + // TODO: + // 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,10 +973,12 @@ 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; /* fallthrough */ case IOTimerChanMode_Capture: + case IOTimerChanMode_CaptureDMA: cr1_bit = state ? GTIM_CR1_CEN : 0; /* fallthrough */ @@ -946,6 +1024,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; } @@ -1006,6 +1085,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; From e1fa5558aef2a935d9935def5282277d089e9c34 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Wed, 16 Oct 2024 16:37:40 -0800 Subject: [PATCH 03/19] fix dshot impl, guard against unused channels --- .../src/px4/stm/stm32_common/dshot/dshot.c | 41 +++++++++++++------ src/drivers/drv_dshot.h | 2 +- 2 files changed, 29 insertions(+), 14 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 319e285db98e..39d9259c323b 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -351,7 +351,6 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bi void up_dshot_trigger(void) { - for (unsigned channel = 0; channel < MAX_TIMER_IO_CHANNELS; channel++) { if (_channels_init_mask & (1 << channel)) { @@ -540,8 +539,13 @@ void process_capture_results(void *arg) * bit 12 - dshot telemetry enable/disable * bits 13-16 - XOR checksum **/ -void dshot_motor_data_set(unsigned motor_number, uint16_t throttle, bool telemetry) +void dshot_motor_data_set(unsigned channel, uint16_t throttle, bool telemetry) { + if (!(_channels_init_mask & (1 << channel))) { + // this channel is not configured for dshot + return; + } + uint16_t packet = 0; uint16_t checksum = 0; @@ -558,13 +562,19 @@ void dshot_motor_data_set(unsigned motor_number, uint16_t throttle, bool telemet csum_data >>= NIBBLES_SIZE; } - packet |= (checksum & 0x0F); + if (bidirectional_dshot_enabled) { + packet |= ((~checksum) & 0x0F); + + } else { + packet |= ((checksum) & 0x0F); + } + + unsigned timer = timer_io_channels[channel].timer_index; - unsigned timer = timer_io_channels[motor_number].timer_index; 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; - unsigned timer_channel_index = timer_io_channels[motor_number].timer_channel - mapping->lowest_timer_channel; + unsigned timer_channel_index = timer_io_channels[channel].timer_channel - mapping->lowest_timer_channel; for (unsigned motor_data_index = 0; motor_data_index < ONE_MOTOR_DATA_SIZE; motor_data_index++) { buffer[motor_data_index * num_motors + timer_channel_index] = @@ -581,23 +591,28 @@ int up_dshot_arm(bool armed) int up_bdshot_get_erpm(uint8_t channel, int *erpm) { - // Not implemented - // *erpm = _erpms[channel]; - // return 0; - return -1; + if (_channels_init_mask & (1 << channel)) { + *erpm = _erpms[channel]; + return PX4_OK; + } + // this channel is not configured for dshot + return PX4_ERROR; } int up_bdshot_channel_status(uint8_t channel) { - // return <0 on error / not supported, 0 on offline, 1 on online - // return 1; + // TODO: track that each channel is communicating + if (_channels_init_mask & (1 << channel)) { + return 1; + } - // Not implemented - return -1; + return 0; } void up_bdshot_status(void) { + PX4_INFO("dshot driver stats: read %lu, failed nibble %lu, failed CRC %lu, invalid/zero %lu", + 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 9c36dfd52d8b..7ae47dfbe065 100644 --- a/src/drivers/drv_dshot.h +++ b/src/drivers/drv_dshot.h @@ -96,7 +96,7 @@ __EXPORT extern int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq /** * Set Dshot motor data, used by up_dshot_motor_data_set() and up_dshot_motor_command() (internal method) */ -__EXPORT extern void dshot_motor_data_set(unsigned motor_number, uint16_t throttle, bool telemetry); +__EXPORT extern void dshot_motor_data_set(unsigned channel, uint16_t throttle, bool telemetry); /** * Set the current dshot throttle value for a channel (motor). From 0b700c767dee4b2a6a907f5267b1ad0446687a03 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Wed, 16 Oct 2024 16:37:54 -0800 Subject: [PATCH 04/19] increase esc_status logging rate --- src/modules/logger/logged_topics.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 8bd55e62fec3..31126c573cdc 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -62,7 +62,7 @@ void LoggedTopics::add_default_topics() add_optional_topic("external_ins_attitude"); add_optional_topic("external_ins_global_position"); add_optional_topic("external_ins_local_position"); - add_optional_topic("esc_status", 250); + add_optional_topic("esc_status", 10); add_topic("failure_detector_status", 100); add_topic("failsafe_flags"); add_optional_topic("follow_target", 500); From 700fbccd4141052219b50be57dd813f980510b8f Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Thu, 17 Oct 2024 19:46:12 -0800 Subject: [PATCH 05/19] staging massive rewrite --- .../rpi_common/include/px4_arch/io_timer.h | 1 - .../px4/stm/stm32_common/dshot/CMakeLists.txt | 2 +- .../src/px4/stm/stm32_common/dshot/dshot.c | 348 ++++++++++-------- .../stm32_common/include/px4_arch/io_timer.h | 10 +- .../px4/stm/stm32_common/io_pins/io_timer.c | 30 +- src/drivers/dshot/DShot.cpp | 2 + 6 files changed, 221 insertions(+), 172 deletions(-) diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/io_timer.h index c9fbedd8f0bc..96de3fe9e122 100644 --- a/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/io_timer.h @@ -147,7 +147,6 @@ __EXPORT int io_timer_free_channel(unsigned channel); __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(void); -__EXPORT void io_timer_update_dma_req(uint8_t timer, bool enable); __EXPORT extern int io_timer_set_dshot_mode(uint8_t timer, unsigned dshot_pwm_rate, uint8_t dma_burst_length); diff --git a/platforms/nuttx/src/px4/stm/stm32_common/dshot/CMakeLists.txt b/platforms/nuttx/src/px4/stm/stm32_common/dshot/CMakeLists.txt index 5221ae896a63..743644c42e43 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/CMakeLists.txt +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/CMakeLists.txt @@ -34,4 +34,4 @@ px4_add_library(arch_dshot dshot.c ) -target_compile_options(arch_dshot PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) +target_compile_options(arch_dshot PRIVATE ${MAX_CUSTOM_OPT_LEVEL} -Wno-error=unused-function -Wno-error=unused-variable) 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 39d9259c323b..ad100c47d246 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -55,8 +55,8 @@ #define DSHOT_TIMERS MAX_IO_TIMERS #define MOTORS_NUMBER DIRECT_PWM_OUTPUT_CHANNELS #define ONE_MOTOR_DATA_SIZE 16u -#define ONE_MOTOR_BUFF_SIZE 17u -#define ALL_MOTORS_BUF_SIZE (MOTORS_NUMBER * ONE_MOTOR_BUFF_SIZE) +#define CHANNEL_OUTPUT_BUFF_SIZE 17u +#define ALL_MOTORS_BUF_SIZE (MOTORS_NUMBER * CHANNEL_OUTPUT_BUFF_SIZE) #define DSHOT_THROTTLE_POSITION 5u #define DSHOT_TELEMETRY_POSITION 4u #define NIBBLES_SIZE 4u @@ -72,7 +72,9 @@ DMA_SCR_DIR_P2M | DMA_SCR_TCIE | DMA_SCR_TEIE | DMA_SCR_DMEIE) typedef struct dshot_handler_t { - DMA_HANDLE dma_handle; + DMA_HANDLE dma_up_handle; // DMA stream for DMA update + DMA_HANDLE dma_ch_handle[4]; // DMA streams for bidi capture compare + uint32_t dma_ch_count; uint32_t dma_size; } dshot_handler_t; @@ -82,7 +84,7 @@ typedef struct dshot_handler_t { #else #define DMA_ALIGN_UP(n) (n) #endif -#define DSHOT_BURST_BUFFER_SIZE(motors_number) (DMA_ALIGN_UP(sizeof(uint32_t)*ONE_MOTOR_BUFF_SIZE*motors_number)) +#define DSHOT_BURST_BUFFER_SIZE(channel_count) (DMA_ALIGN_UP(sizeof(uint32_t)*CHANNEL_OUTPUT_BUFF_SIZE*channel_count)) static dshot_handler_t dshot_handler[DSHOT_TIMERS] = {}; static uint8_t dshot_burst_buffer_array[DSHOT_TIMERS * DSHOT_BURST_BUFFER_SIZE(MAX_NUM_CHANNELS_PER_TIMER)] @@ -94,19 +96,22 @@ static uint16_t dshot_capture_buffer[32] px4_cache_aligned_data() = {}; // TODO: audit local variables static struct hrt_call _call; -static void do_capture(DMA_HANDLE handle, uint8_t status, void *arg); +static void dma_callback_begin_capture(DMA_HANDLE handle, uint8_t status, void *arg); +static void dma_callback_capture_complete(DMA_HANDLE handle, uint8_t status, void *arg); +static void reinitialize_timers_callback(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 bidirectional_dshot_enabled = false; +static uint32_t jakechannelcounter[8] = {}; + +static bool _birectional = false; static uint32_t _dshot_frequency = 0; static int _timers_init_mask = 0; static int _channels_init_mask = 0; @@ -115,8 +120,6 @@ static int _channels_init_mask = 0; static uint32_t _motor_to_capture = 0; static int32_t _erpms[16] = {}; -// static void(*_erpm_callback)(int32_t[], size_t, void *) = NULL; -// static void *_erpm_callback_context = NULL; uint8_t nibbles_from_mapped(uint8_t mapped) { @@ -258,87 +261,110 @@ 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) -{ - int ret = io_timer_channel_init(channel, - bidirectional_dshot_enabled ? 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, bool enable_bidirectional_dshot) { - bidirectional_dshot_enabled = enable_bidirectional_dshot; + _birectional = enable_bidirectional_dshot; _dshot_frequency = dshot_pwm_freq; - for (unsigned channel = 0; channel < MAX_TIMER_IO_CHANNELS; channel++) { - if (channel_mask & (1 << channel)) { - uint8_t timer = timer_io_channels[channel].timer_index; + // Initialize the timer channels + for (unsigned output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) { + if (channel_mask & (1 << output_channel)) { + uint8_t timer_index = timer_io_channels[output_channel].timer_index; - if (io_timers[timer].dshot.dma_base == 0) { // board does not configure dshot on this timer + // TODO: is there a smarter way to do this? + if (io_timers[timer_index].dshot.dma_base == 0) { + // board does not configure dshot on this timer continue; } - int ret = dshot_output_timer_init(channel); - + io_timer_channel_mode_t mode = _birectional ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot; + int ret = io_timer_channel_init(output_channel, mode, NULL, NULL); if (ret != OK) { + PX4_INFO("io_timer_channel_init %u failed", output_channel); return ret; } - _channels_init_mask |= (1 << channel); - _timers_init_mask |= (1 << timer); + _channels_init_mask |= (1 << output_channel); + _timers_init_mask |= (1 << timer_index); } } - 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; + // For each dshot timer: + // - enable dshot mode + // - allocate DMA for Update (transmit) + // - allocate DMA for Capture Compare (receive) + for (uint8_t timer_index = 0; timer_index < MAX_IO_TIMERS; timer_index++) { + if (_timers_init_mask & (1 << timer_index)) { + if (dshot_handler[timer_index].dma_up_handle == NULL) { + + // Set the DMA buffer size to hold all DMA data + uint32_t channel_count = io_timers_channel_mapping.element[timer_index].channel_count_including_gaps; + dshot_handler[timer_index].dma_size = channel_count * CHANNEL_OUTPUT_BUFF_SIZE; - io_timer_set_dshot_mode(timer, _dshot_frequency, - io_timers_channel_mapping.element[timer].channel_count_including_gaps); + // Configure timer in dshot mode + io_timer_set_dshot_mode(timer_index, _dshot_frequency, channel_count); - dshot_handler[timer].dma_handle = stm32_dmachannel(io_timers[timer].dshot.dma_map_up); + // Configure DMA UP handle + dshot_handler[timer_index].dma_up_handle = stm32_dmachannel(io_timers[timer_index].dshot.dma_map_up); - if (NULL == dshot_handler[timer].dma_handle) { + if (dshot_handler[timer_index].dma_up_handle == NULL) { // TODO: how to log this? + PX4_INFO("could not allocate timer %u DMA UP handle", timer_index); return -ENOSR; } - // 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; + // This was just for testing. We will initialize again at run time. + stm32_dmafree(dshot_handler[timer_index].dma_up_handle); + dshot_handler[timer_index].dma_up_handle = NULL; + PX4_INFO("allocated and freed DMA for timer %u UP", timer_index); } } } + // For each dshot channel: + // - allocate DMA for Capture Compare (receive) + for (uint8_t output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) { + if (_channels_init_mask & (1 << output_channel)) { + uint8_t timer_index = timer_io_channels[output_channel].timer_index; + uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1; + + DMA_HANDLE* dma_handle = &dshot_handler[timer_index].dma_ch_handle[timer_channel_index]; + uint32_t dma_map_ch = io_timers[timer_index].dshot.dma_map_ch[timer_channel_index]; + if (dma_map_ch) { + PX4_INFO("allocating timer %u DMA CH%u handle, output_channel %u", timer_index, timer_channel_index, output_channel); + *dma_handle = stm32_dmachannel(dma_map_ch); + if (*dma_handle == NULL) { + PX4_INFO("could not allocate timer %u DMA CH%u handle, output_channel %u", timer_index, timer_channel_index, output_channel); + return -ENOSR; + } + + // This was just for testing. We will initialize again at run time. + PX4_INFO("allocated timer %u DMA CH%u handle, output_channel %u", timer_index, timer_channel_index, output_channel); + stm32_dmafree(*dma_handle); + *dma_handle = NULL; + PX4_INFO("FREEEEE"); + } + } + } + + px4_usleep(500000); + + // TODO: what are we doing here and why can't this be defined at compile time? unsigned buffer_offset = 0; - for (unsigned timer = 0; timer < DSHOT_TIMERS; ++timer) { - if (_timers_init_mask & (1 << timer)) { - if (io_timers[timer].base == 0) { // no more timers configured + for (unsigned timer_index = 0; timer_index < DSHOT_TIMERS; timer_index++) { + if (_timers_init_mask & (1 << timer_index)) { + if (io_timers[timer_index].base == 0) { // no more timers configured break; } // 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]; + dshot_burst_buffer[timer_index] = (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); + uint32_t channel_count = io_timers_channel_mapping.element[timer_index].channel_count_including_gaps; + buffer_offset += DSHOT_BURST_BUFFER_SIZE(channel_count); if (buffer_offset > sizeof(dshot_burst_buffer_array)) { return -EINVAL; // something is wrong with the board configuration or some other logic @@ -349,138 +375,156 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bi return _channels_init_mask; } +// Kicks off a DMA transmit for each configured timer and the associated channels void up_dshot_trigger(void) { - for (unsigned channel = 0; channel < MAX_TIMER_IO_CHANNELS; channel++) { - if (_channels_init_mask & (1 << 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? - return; + // For bidirectional dshot we need to re-initialize the timers every time here. + // In normal mode, we just do it once. + if (_birectional) { + for (uint8_t output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) { + if (_channels_init_mask & (1 << output_channel)) { + io_timer_unallocate_channel(output_channel); + int ret = io_timer_channel_init(output_channel, IOTimerChanMode_DshotInverted, NULL, NULL); + if (ret != OK) { + PX4_INFO("io_timer_channel_init %u failed", output_channel); + return; + } } } } - io_timer_set_enable(true, bidirectional_dshot_enabled ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot, + // Enable all timers configured as dshot (weird way of doing this...) + io_timer_set_enable(true, _birectional ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot, IO_TIMER_ALL_MODES_CHANNELS); - for (unsigned timer = 0; timer < DSHOT_TIMERS; ++timer) { - if (_timers_init_mask & (1 << timer)) { + // Clear capture_buffer cache + if (_birectional) { + uintptr_t buf = (uintptr_t)dshot_capture_buffer; + size_t size = sizeof(dshot_capture_buffer); + memset(dshot_capture_buffer, 0, size); + up_clean_dcache(buf, buf + size); + } - 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 each timer, begin DMA transmit + for (uint8_t timer_index = 0; timer_index < DSHOT_TIMERS; timer_index++) { + if (_timers_init_mask & (1 << timer_index)) { - io_timer_set_dshot_mode(timer, _dshot_frequency, - io_timers_channel_mapping.element[timer].channel_count_including_gaps); + uint32_t channel_count = io_timers_channel_mapping.element[timer_index].channel_count_including_gaps; - dshot_handler[timer].dma_handle = stm32_dmachannel(io_timers[timer].dshot.dma_map_up); + // Allocate DMA if necessary + if (dshot_handler[timer_index].dma_up_handle == NULL) { + dshot_handler[timer_index].dma_size = channel_count * CHANNEL_OUTPUT_BUFF_SIZE; - if (NULL == dshot_handler[timer].dma_handle) { - // TODO: how to log this? + io_timer_set_dshot_mode(timer_index, _dshot_frequency, channel_count); + + dshot_handler[timer_index].dma_up_handle = stm32_dmachannel(io_timers[timer_index].dshot.dma_map_up); + + if (dshot_handler[timer_index].dma_up_handle == NULL) { + PX4_INFO("DMA allocation for timer %u failed", timer_index); 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)); - - px4_stm32_dmasetup(dshot_handler[timer].dma_handle, - io_timers[timer].base + STM32_GTIM_DMAR_OFFSET, - (uint32_t)(dshot_burst_buffer[timer]), - dshot_handler[timer].dma_size, + up_clean_dcache((uintptr_t) dshot_burst_buffer[timer_index], + (uintptr_t) dshot_burst_buffer[timer_index] + + DSHOT_BURST_BUFFER_SIZE(channel_count)); + + px4_stm32_dmasetup(dshot_handler[timer_index].dma_up_handle, + io_timers[timer_index].base + STM32_GTIM_DMAR_OFFSET, + (uint32_t)(dshot_burst_buffer[timer_index]), + dshot_handler[timer_index].dma_size, DSHOT_DMA_SCR); // Clean UDE flag before DMA is started - io_timer_update_dma_req(timer, false); + io_timer_disable_update_dma_req(timer_index); + // Trigger DMA (DShot Outputs) - stm32_dmastart(dshot_handler[timer].dma_handle, bidirectional_dshot_enabled ? do_capture : NULL, NULL, false); - io_timer_update_dma_req(timer, true); + stm32_dmastart(dshot_handler[timer_index].dma_up_handle, _birectional ? dma_callback_begin_capture : NULL, &timer_index, false); + + // Enable DMA update request + io_timer_enable_update_dma_req(timer_index); } } } -void do_capture(DMA_HANDLE handle, uint8_t status, void *arg) +void dma_callback_begin_capture(DMA_HANDLE handle, uint8_t status, void *arg) { (void)handle; (void)status; - (void)arg; - 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 doesn't scale to more than 4 motors yet - unsigned capture_channel = _motor_to_capture; + uint8_t timer_index = *((uint8_t*)arg); - dshot_handler[timer].dma_size = sizeof(dshot_capture_buffer); - - // 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(io_timers[timer].dshot.dma_map_up - 5 + 1); - // dshot_handler[timer].dma_handle = stm32_dmachannel(io_timers[timer].dshot.dma_map_ch[0]); + // Clear DMA UP configuration + if (dshot_handler[timer_index].dma_up_handle != NULL) { + stm32_dmastop(dshot_handler[timer_index].dma_up_handle); + stm32_dmafree(dshot_handler[timer_index].dma_up_handle); + dshot_handler[timer_index].dma_up_handle = NULL; + } - break; + // Disable DMA capture request + io_timer_disable_capture_dma_req(timer_index); - case 2: - dshot_handler[timer].dma_handle = stm32_dmachannel(io_timers[timer].dshot.dma_map_up - 5 + 2); - break; + // Allocate DMA for all channels on this timer - case 3: - dshot_handler[timer].dma_handle = stm32_dmachannel(io_timers[timer].dshot.dma_map_up - 5 + 3); - break; + for (uint8_t output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) { - case 4: - dshot_handler[timer].dma_handle = stm32_dmachannel(io_timers[timer].dshot.dma_map_up - 5 + 4); - break; - } + bool is_this_timer = timer_index == timer_io_channels[output_channel].timer_index; + bool channel_enabled = _channels_init_mask & (1 << output_channel); - 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)); + if (is_this_timer && channel_enabled) { + uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel -1; + dshot_handler[timer_index].dma_ch_handle[timer_channel_index] = stm32_dmachannel(io_timers[timer_index].dshot.dma_map_ch[timer_channel_index]); - px4_stm32_dmasetup(dshot_handler[timer].dma_handle, - io_timers[timer].base + STM32_GTIM_DMAR_OFFSET, + // Setup DMA for this channel + px4_stm32_dmasetup(dshot_handler[timer_index].dma_ch_handle[timer_channel_index], + io_timers[timer_index].base + STM32_GTIM_DMAR_OFFSET, // DMA address for burst mode (16-bit, TIM2-5 only) (uint32_t) dshot_capture_buffer, sizeof(dshot_capture_buffer), DSHOT_BIDIRECTIONAL_DMA_SCR); - 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); + // Reset / Enable timer channel + io_timer_unallocate_channel(output_channel); + io_timer_channel_init(output_channel, IOTimerChanMode_CaptureDMA, NULL, NULL); + io_timer_set_enable(true, IOTimerChanMode_CaptureDMA, 1 << output_channel); + + // Enable input capture on this channel + up_input_capture_set(output_channel, Both, 0, NULL, NULL); - up_input_capture_set(capture_channel, Both, 0, NULL, NULL); + // Setup dshot capture for this channel + io_timer_set_dshot_capture_mode(timer_index, _dshot_frequency, output_channel); - 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); + // Start DMA with callback if bidi dshot is enabled + stm32_dmastart(dshot_handler[timer_index].dma_up_handle, dma_callback_capture_complete, &output_channel, false); } } - // 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); + // Enable DMA capture compare on this timer + io_timer_enable_capture_dma_req(timer_index); +} + +static void dma_callback_capture_complete(DMA_HANDLE handle, uint8_t status, void *arg) +{ + unsigned output_channel = *((unsigned*)arg); + + jakechannelcounter[output_channel]++; + + hrt_call_after(&_call, 1, reinitialize_timers_callback, &output_channel); +} + +void reinitialize_timers_callback(void *arg) +{ + unsigned output_channel = *((unsigned*)arg); + + uint8_t timer_index = timer_io_channels[output_channel].timer_index; + uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1; + + stm32_dmastop(dshot_handler[timer_index].dma_ch_handle[timer_channel_index]); + stm32_dmafree(dshot_handler[timer_index].dma_ch_handle[timer_channel_index]); + dshot_handler[timer_index].dma_ch_handle[timer_channel_index] = NULL; + + io_timer_unallocate_channel(output_channel); + io_timer_channel_init(output_channel, IOTimerChanMode_DshotInverted, NULL, NULL); } void process_capture_results(void *arg) @@ -490,10 +534,10 @@ void process_capture_results(void *arg) // 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; + if (dshot_handler[timer].dma_up_handle != NULL) { + stm32_dmastop(dshot_handler[timer].dma_up_handle); + stm32_dmafree(dshot_handler[timer].dma_up_handle); + dshot_handler[timer].dma_up_handle = NULL; } } } @@ -562,7 +606,7 @@ void dshot_motor_data_set(unsigned channel, uint16_t throttle, bool telemetry) csum_data >>= NIBBLES_SIZE; } - if (bidirectional_dshot_enabled) { + if (_birectional) { packet |= ((~checksum) & 0x0F); } else { @@ -574,10 +618,10 @@ void dshot_motor_data_set(unsigned channel, uint16_t throttle, bool telemetry) 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; - unsigned timer_channel_index = timer_io_channels[channel].timer_channel - mapping->lowest_timer_channel; + unsigned timer_channel = timer_io_channels[channel].timer_channel - mapping->lowest_timer_channel; for (unsigned motor_data_index = 0; motor_data_index < ONE_MOTOR_DATA_SIZE; motor_data_index++) { - buffer[motor_data_index * num_motors + timer_channel_index] = + buffer[motor_data_index * num_motors + timer_channel] = (packet & 0x8000) ? MOTOR_PWM_BIT_1 : MOTOR_PWM_BIT_0; // MSB first packet <<= 1; } @@ -585,7 +629,7 @@ void dshot_motor_data_set(unsigned channel, uint16_t throttle, bool telemetry) int up_dshot_arm(bool armed) { - return io_timer_set_enable(armed, bidirectional_dshot_enabled ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot, + return io_timer_set_enable(armed, _birectional ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot, IO_TIMER_ALL_MODES_CHANNELS); } @@ -613,6 +657,8 @@ void up_bdshot_status(void) { PX4_INFO("dshot driver stats: read %lu, failed nibble %lu, failed CRC %lu, invalid/zero %lu", read_ok, read_fail_nibble, read_fail_crc, read_fail_zero); + + PX4_INFO("ch0 %lu, ch1 %lu, ch2 %lu, ch3 %lu,", jakechannelcounter[0], jakechannelcounter[1], jakechannelcounter[2], jakechannelcounter[3]); } #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 1c28b149cdd2..0c5b10840012 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 @@ -160,10 +160,12 @@ __EXPORT int io_timer_unallocate_channel(unsigned channel); __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, unsigned channel); +__EXPORT void io_timer_enable_update_dma_req(uint8_t timer); +__EXPORT void io_timer_disable_update_dma_req(uint8_t timer); +__EXPORT void io_timer_enable_capture_dma_req(uint8_t timer); +__EXPORT void io_timer_disable_capture_dma_req(uint8_t timer); + +__EXPORT int io_timer_set_dshot_capture_mode(uint8_t timer, unsigned dshot_pwm_rate, unsigned channel); /** * Reserve a timer * @return 0 on success (if not used yet, or already set to the mode) 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 dda6904570ee..181b8b0ad7c4 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 @@ -511,26 +511,26 @@ static inline void io_timer_set_oneshot_mode(unsigned timer) rEGR(timer) = GTIM_EGR_UG; } -void io_timer_update_dma_req(uint8_t timer, bool enable) +void io_timer_enable_update_dma_req(uint8_t timer) { - if (enable) { - rDIER(timer) |= ATIM_DIER_UDE; + rDIER(timer) |= ATIM_DIER_UDE; +} - } else { - rDIER(timer) &= ~ATIM_DIER_UDE; - } +void io_timer_disable_update_dma_req(uint8_t timer) +{ + rDIER(timer) &= ~ATIM_DIER_UDE; } -void io_timer_capture_update_dma_req(uint8_t timer, bool enable) +void io_timer_enable_capture_dma_req(uint8_t timer) { - if (enable) { - 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); + 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 | ATIM_EGR_CC3G | ATIM_EGR_CC4G); - rDIER(timer) &= ~(ATIM_DIER_CC1DE | ATIM_DIER_CC2DE | ATIM_DIER_CC3DE | ATIM_DIER_CC4DE); - } +void io_timer_disable_capture_dma_req(uint8_t timer) +{ + 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); } int io_timer_set_dshot_mode(uint8_t timer, unsigned dshot_pwm_freq, uint8_t dma_burst_length) @@ -588,7 +588,7 @@ 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, unsigned channel) +int io_timer_set_dshot_capture_mode(uint8_t timer, unsigned dshot_pwm_freq, unsigned channel) { rARR(timer) = -1; rEGR(timer) = ATIM_EGR_UG | GTIM_EGR_CC1G | GTIM_EGR_CC2G | GTIM_EGR_CC3G | GTIM_EGR_CC4G; diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index 940019d9bf04..9e9d91c905e7 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -534,6 +534,8 @@ void DShot::Run() if (_outputs_on != outputs_on) { enable_dshot_outputs(outputs_on); + PX4_INFO("DSHOT OUTPUTS ENABLED"); + px4_usleep(100'000); } if (_telemetry) { From 90818e8971dd19415ada43ac63ffe73d571d1066 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Fri, 18 Oct 2024 13:42:00 -0800 Subject: [PATCH 06/19] all channel dma callbacks are working but capture_buffer data is all zero. Need to check callback timing and erpm frame timing --- .../src/px4/stm/stm32_common/dshot/dshot.c | 500 ++++++++---------- src/drivers/dshot/DShot.cpp | 2 - 2 files changed, 235 insertions(+), 267 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 ad100c47d246..4d1d104f0143 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -48,28 +48,23 @@ #include #include #include -#include #define MOTOR_PWM_BIT_1 14u #define MOTOR_PWM_BIT_0 7u -#define DSHOT_TIMERS MAX_IO_TIMERS -#define MOTORS_NUMBER DIRECT_PWM_OUTPUT_CHANNELS #define ONE_MOTOR_DATA_SIZE 16u -#define CHANNEL_OUTPUT_BUFF_SIZE 17u -#define ALL_MOTORS_BUF_SIZE (MOTORS_NUMBER * CHANNEL_OUTPUT_BUFF_SIZE) +#define CHANNEL_OUTPUT_BUFF_SIZE 17u #define DSHOT_THROTTLE_POSITION 5u #define DSHOT_TELEMETRY_POSITION 4u #define NIBBLES_SIZE 4u #define DSHOT_NUMBER_OF_NIBBLES 3u -#define DSHOT_END_OF_STREAM 16u #define MAX_NUM_CHANNELS_PER_TIMER 4u // CCR1-CCR4 #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) + DMA_SCR_DIR_M2P | DMA_SCR_TCIE | DMA_SCR_HTIE | DMA_SCR_TEIE | DMA_SCR_DMEIE) -// TODO: why 16bit? +// 16-bit because not all of the General Purpose Timers support 32-bit #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) + DMA_SCR_DIR_P2M | DMA_SCR_TCIE | DMA_SCR_TEIE | DMA_SCR_DMEIE) typedef struct dshot_handler_t { DMA_HANDLE dma_up_handle; // DMA stream for DMA update @@ -84,186 +79,46 @@ typedef struct dshot_handler_t { #else #define DMA_ALIGN_UP(n) (n) #endif -#define DSHOT_BURST_BUFFER_SIZE(channel_count) (DMA_ALIGN_UP(sizeof(uint32_t)*CHANNEL_OUTPUT_BUFF_SIZE*channel_count)) +#define DSHOT_OUTPUT_BUFFER_SIZE(channel_count) (DMA_ALIGN_UP(sizeof(uint32_t)*CHANNEL_OUTPUT_BUFF_SIZE*channel_count)) -static dshot_handler_t dshot_handler[DSHOT_TIMERS] = {}; -static uint8_t dshot_burst_buffer_array[DSHOT_TIMERS * DSHOT_BURST_BUFFER_SIZE(MAX_NUM_CHANNELS_PER_TIMER)] +static dshot_handler_t dshot_handler[MAX_IO_TIMERS] = {}; +static uint8_t dshot_burst_buffer_array[MAX_IO_TIMERS * DSHOT_OUTPUT_BUFFER_SIZE(MAX_NUM_CHANNELS_PER_TIMER)] px4_cache_aligned_data() = {}; -static uint32_t *dshot_burst_buffer[DSHOT_TIMERS] = {}; +static uint32_t *dshot_output_buffer[MAX_IO_TIMERS] = {}; -static uint16_t dshot_capture_buffer[32] px4_cache_aligned_data() = {}; +// static uint16_t dshot_capture_buffer[32] px4_cache_aligned_data() = {}; +static uint16_t dshot_capture_buffer[MAX_TIMER_IO_CHANNELS][32] px4_cache_aligned_data() = {}; -// TODO: audit local variables -static struct hrt_call _call; -static void dma_callback_begin_capture(DMA_HANDLE handle, uint8_t status, void *arg); +static void dma_callback_capture_start(DMA_HANDLE handle, uint8_t status, void *arg); static void dma_callback_capture_complete(DMA_HANDLE handle, uint8_t status, void *arg); -static void reinitialize_timers_callback(void *arg); -static void process_capture_results(void *arg); -static unsigned calculate_period(void); +static void process_capture_results(int8_t output_channel); + +static unsigned calculate_period_from_erpm_frame(uint8_t* buffer, size_t buffer_size); 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 uint32_t jakechannelcounter[8] = {}; +static int32_t jakechannelcounter[8] = { -7, -7, -7, -7, -7, -7, -7 , -7 }; +static uint32_t jakecounter = 0; +// We need local permanent memory for indices, channels, and callbacks for each output +static uint8_t timer_index_array[MAX_IO_TIMERS] = {}; +static uint8_t output_channel_array[MAX_TIMER_IO_CHANNELS] = {}; -static bool _birectional = false; +static bool _bidirectional = false; 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 int32_t _erpms[16] = {}; - - -uint8_t nibbles_from_mapped(uint8_t mapped) -{ - switch (mapped) { - case 0x19: - return 0x00; - - case 0x1B: - return 0x01; - - case 0x12: - return 0x02; - - case 0x13: - return 0x03; - - case 0x1D: - return 0x04; - - case 0x15: - return 0x05; - - case 0x16: - return 0x06; - - case 0x17: - return 0x07; - - case 0x1a: - return 0x08; - - case 0x09: - return 0x09; - - 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; - } - - // 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) { - 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; -} +static int32_t _erpms[MAX_TIMER_IO_CHANNELS] = {}; int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bidirectional_dshot) { - _birectional = enable_bidirectional_dshot; + _bidirectional = enable_bidirectional_dshot; _dshot_frequency = dshot_pwm_freq; // Initialize the timer channels @@ -277,7 +132,7 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bi continue; } - io_timer_channel_mode_t mode = _birectional ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot; + io_timer_channel_mode_t mode = _bidirectional ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot; int ret = io_timer_channel_init(output_channel, mode, NULL, NULL); if (ret != OK) { PX4_INFO("io_timer_channel_init %u failed", output_channel); @@ -316,7 +171,6 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bi // This was just for testing. We will initialize again at run time. stm32_dmafree(dshot_handler[timer_index].dma_up_handle); dshot_handler[timer_index].dma_up_handle = NULL; - PX4_INFO("allocated and freed DMA for timer %u UP", timer_index); } } } @@ -331,7 +185,6 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bi DMA_HANDLE* dma_handle = &dshot_handler[timer_index].dma_ch_handle[timer_channel_index]; uint32_t dma_map_ch = io_timers[timer_index].dshot.dma_map_ch[timer_channel_index]; if (dma_map_ch) { - PX4_INFO("allocating timer %u DMA CH%u handle, output_channel %u", timer_index, timer_channel_index, output_channel); *dma_handle = stm32_dmachannel(dma_map_ch); if (*dma_handle == NULL) { PX4_INFO("could not allocate timer %u DMA CH%u handle, output_channel %u", timer_index, timer_channel_index, output_channel); @@ -339,20 +192,16 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bi } // This was just for testing. We will initialize again at run time. - PX4_INFO("allocated timer %u DMA CH%u handle, output_channel %u", timer_index, timer_channel_index, output_channel); stm32_dmafree(*dma_handle); *dma_handle = NULL; - PX4_INFO("FREEEEE"); } } } - px4_usleep(500000); - // TODO: what are we doing here and why can't this be defined at compile time? unsigned buffer_offset = 0; - for (unsigned timer_index = 0; timer_index < DSHOT_TIMERS; timer_index++) { + for (unsigned timer_index = 0; timer_index < MAX_IO_TIMERS; timer_index++) { if (_timers_init_mask & (1 << timer_index)) { if (io_timers[timer_index].base == 0) { // no more timers configured break; @@ -361,10 +210,10 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bi // 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_index] = (uint32_t *) &dshot_burst_buffer_array[buffer_offset]; + dshot_output_buffer[timer_index] = (uint32_t *) &dshot_burst_buffer_array[buffer_offset]; #pragma GCC diagnostic pop uint32_t channel_count = io_timers_channel_mapping.element[timer_index].channel_count_including_gaps; - buffer_offset += DSHOT_BURST_BUFFER_SIZE(channel_count); + buffer_offset += DSHOT_OUTPUT_BUFFER_SIZE(channel_count); if (buffer_offset > sizeof(dshot_burst_buffer_array)) { return -EINVAL; // something is wrong with the board configuration or some other logic @@ -380,7 +229,7 @@ void up_dshot_trigger(void) { // For bidirectional dshot we need to re-initialize the timers every time here. // In normal mode, we just do it once. - if (_birectional) { + if (_bidirectional) { for (uint8_t output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) { if (_channels_init_mask & (1 << output_channel)) { io_timer_unallocate_channel(output_channel); @@ -394,19 +243,11 @@ void up_dshot_trigger(void) } // Enable all timers configured as dshot (weird way of doing this...) - io_timer_set_enable(true, _birectional ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot, - IO_TIMER_ALL_MODES_CHANNELS); - - // Clear capture_buffer cache - if (_birectional) { - uintptr_t buf = (uintptr_t)dshot_capture_buffer; - size_t size = sizeof(dshot_capture_buffer); - memset(dshot_capture_buffer, 0, size); - up_clean_dcache(buf, buf + size); - } + io_timer_set_enable(true, _bidirectional ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot, + IO_TIMER_ALL_MODES_CHANNELS); // For each timer, begin DMA transmit - for (uint8_t timer_index = 0; timer_index < DSHOT_TIMERS; timer_index++) { + for (uint8_t timer_index = 0; timer_index < MAX_IO_TIMERS; timer_index++) { if (_timers_init_mask & (1 << timer_index)) { uint32_t channel_count = io_timers_channel_mapping.element[timer_index].channel_count_including_gaps; @@ -415,8 +256,6 @@ void up_dshot_trigger(void) if (dshot_handler[timer_index].dma_up_handle == NULL) { dshot_handler[timer_index].dma_size = channel_count * CHANNEL_OUTPUT_BUFF_SIZE; - io_timer_set_dshot_mode(timer_index, _dshot_frequency, channel_count); - dshot_handler[timer_index].dma_up_handle = stm32_dmachannel(io_timers[timer_index].dshot.dma_map_up); if (dshot_handler[timer_index].dma_up_handle == NULL) { @@ -425,14 +264,17 @@ void up_dshot_trigger(void) } } + // Set timer in dshot mode + io_timer_set_dshot_mode(timer_index, _dshot_frequency, channel_count); + // Flush cache so DMA sees the data - up_clean_dcache((uintptr_t) dshot_burst_buffer[timer_index], - (uintptr_t) dshot_burst_buffer[timer_index] + - DSHOT_BURST_BUFFER_SIZE(channel_count)); + up_clean_dcache((uintptr_t) dshot_output_buffer[timer_index], + (uintptr_t) dshot_output_buffer[timer_index] + + DSHOT_OUTPUT_BUFFER_SIZE(channel_count)); px4_stm32_dmasetup(dshot_handler[timer_index].dma_up_handle, io_timers[timer_index].base + STM32_GTIM_DMAR_OFFSET, - (uint32_t)(dshot_burst_buffer[timer_index]), + (uint32_t)(dshot_output_buffer[timer_index]), dshot_handler[timer_index].dma_size, DSHOT_DMA_SCR); @@ -440,7 +282,8 @@ void up_dshot_trigger(void) io_timer_disable_update_dma_req(timer_index); // Trigger DMA (DShot Outputs) - stm32_dmastart(dshot_handler[timer_index].dma_up_handle, _birectional ? dma_callback_begin_capture : NULL, &timer_index, false); + timer_index_array[timer_index] = timer_index; + stm32_dmastart(dshot_handler[timer_index].dma_up_handle, _bidirectional ? dma_callback_capture_start : NULL, &timer_index_array[timer_index], false); // Enable DMA update request io_timer_enable_update_dma_req(timer_index); @@ -448,39 +291,56 @@ void up_dshot_trigger(void) } } -void dma_callback_begin_capture(DMA_HANDLE handle, uint8_t status, void *arg) +void dma_callback_capture_start(DMA_HANDLE handle, uint8_t status, void *arg) { - (void)handle; (void)status; uint8_t timer_index = *((uint8_t*)arg); - // Clear DMA UP configuration - if (dshot_handler[timer_index].dma_up_handle != NULL) { - stm32_dmastop(dshot_handler[timer_index].dma_up_handle); - stm32_dmafree(dshot_handler[timer_index].dma_up_handle); - dshot_handler[timer_index].dma_up_handle = NULL; - } + // // Clear DMA UP configuration + stm32_dmastop(handle); + stm32_dmafree(handle); + + // NOTE: The dma_up_handle is now invalid // Disable DMA capture request io_timer_disable_capture_dma_req(timer_index); // Allocate DMA for all channels on this timer - for (uint8_t output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) { bool is_this_timer = timer_index == timer_io_channels[output_channel].timer_index; bool channel_enabled = _channels_init_mask & (1 << output_channel); + // if ((output_channel != 1) && (output_channel != 2) && (output_channel != 3)) { + // continue; + // } + if (is_this_timer && channel_enabled) { - uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel -1; - dshot_handler[timer_index].dma_ch_handle[timer_channel_index] = stm32_dmachannel(io_timers[timer_index].dshot.dma_map_ch[timer_channel_index]); + uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1; + + // Allocate DMA + if (dshot_handler[timer_index].dma_ch_handle[timer_channel_index] == NULL) { + dshot_handler[timer_index].dma_ch_handle[timer_channel_index] = stm32_dmachannel(io_timers[timer_index].dshot.dma_map_ch[timer_channel_index]); + } + + // If DMA handler is valid, start DMA + if (dshot_handler[timer_index].dma_ch_handle[timer_channel_index] == NULL) { + PX4_INFO("failed to allocate dma for timer %u channel %u", timer_index, timer_channel_index); + continue; + } + + // Flush cache so DMA sees the data + memset(dshot_capture_buffer[output_channel], 0, sizeof(dshot_capture_buffer[output_channel])); + up_clean_dcache((uintptr_t)dshot_capture_buffer[output_channel], + (uintptr_t)dshot_capture_buffer[output_channel] + + sizeof(dshot_capture_buffer[output_channel])); // Setup DMA for this channel px4_stm32_dmasetup(dshot_handler[timer_index].dma_ch_handle[timer_channel_index], io_timers[timer_index].base + STM32_GTIM_DMAR_OFFSET, // DMA address for burst mode (16-bit, TIM2-5 only) - (uint32_t) dshot_capture_buffer, - sizeof(dshot_capture_buffer), + (uint32_t) dshot_capture_buffer[output_channel], + sizeof(dshot_capture_buffer[output_channel]), DSHOT_BIDIRECTIONAL_DMA_SCR); // Reset / Enable timer channel @@ -495,7 +355,8 @@ void dma_callback_begin_capture(DMA_HANDLE handle, uint8_t status, void *arg) io_timer_set_dshot_capture_mode(timer_index, _dshot_frequency, output_channel); // Start DMA with callback if bidi dshot is enabled - stm32_dmastart(dshot_handler[timer_index].dma_up_handle, dma_callback_capture_complete, &output_channel, false); + output_channel_array[output_channel] = output_channel; + stm32_dmastart(dshot_handler[timer_index].dma_ch_handle[timer_channel_index], dma_callback_capture_complete, &output_channel_array[output_channel], false); } } @@ -505,77 +366,42 @@ void dma_callback_begin_capture(DMA_HANDLE handle, uint8_t status, void *arg) static void dma_callback_capture_complete(DMA_HANDLE handle, uint8_t status, void *arg) { - unsigned output_channel = *((unsigned*)arg); - - jakechannelcounter[output_channel]++; - - hrt_call_after(&_call, 1, reinitialize_timers_callback, &output_channel); -} + // TODO: capture complete callback firing too early? Data is zero... + uint8_t output_channel = *((uint8_t*)arg); -void reinitialize_timers_callback(void *arg) -{ - unsigned output_channel = *((unsigned*)arg); + stm32_dmastop(handle); + stm32_dmafree(handle); - uint8_t timer_index = timer_io_channels[output_channel].timer_index; - uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1; + up_invalidate_dcache((uintptr_t)dshot_capture_buffer[output_channel], + (uintptr_t)dshot_capture_buffer[output_channel] + + sizeof(dshot_capture_buffer[output_channel])); - stm32_dmastop(dshot_handler[timer_index].dma_ch_handle[timer_channel_index]); - stm32_dmafree(dshot_handler[timer_index].dma_ch_handle[timer_channel_index]); - dshot_handler[timer_index].dma_ch_handle[timer_channel_index] = NULL; + process_capture_results(output_channel); + // TODO: do we need to unallocate / reinit timers here? It's done at the start.. io_timer_unallocate_channel(output_channel); io_timer_channel_init(output_channel, IOTimerChanMode_DshotInverted, NULL, NULL); } -void process_capture_results(void *arg) +void process_capture_results(int8_t output_channel) { - (void)arg; - - // 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_up_handle != NULL) { - stm32_dmastop(dshot_handler[timer].dma_up_handle); - stm32_dmafree(dshot_handler[timer].dma_up_handle); - dshot_handler[timer].dma_up_handle = NULL; - } - } - } + jakechannelcounter[output_channel] = output_channel; + jakecounter++; - up_invalidate_dcache((uintptr_t)dshot_capture_buffer, - (uintptr_t)dshot_capture_buffer + - sizeof(dshot_capture_buffer)); - - const unsigned period = calculate_period(); + const unsigned period = calculate_period_from_erpm_frame((uint8_t*)dshot_capture_buffer[output_channel], sizeof(dshot_capture_buffer[output_channel])); if (period == 0) { // If the parsing failed, we get 0. - _erpms[_motor_to_capture] = 0; + _erpms[output_channel] = 0; } else if (period == 65408) { // For still, we get this magic 65408 value. - _erpms[_motor_to_capture] = 0; + _erpms[output_channel] = 0; } else { // from period in us to eRPM - _erpms[_motor_to_capture] = 1000000 * 60 / period; + _erpms[output_channel] = 1000000 * 60 / period; } - - 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, IOTimerChanMode_DshotInverted, NULL, NULL); - } - } - - // if (_erpm_callback != NULL) { - // // 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; } /** @@ -606,7 +432,7 @@ void dshot_motor_data_set(unsigned channel, uint16_t throttle, bool telemetry) csum_data >>= NIBBLES_SIZE; } - if (_birectional) { + if (_bidirectional) { packet |= ((~checksum) & 0x0F); } else { @@ -615,7 +441,7 @@ void dshot_motor_data_set(unsigned channel, uint16_t throttle, bool telemetry) unsigned timer = timer_io_channels[channel].timer_index; - uint32_t *buffer = dshot_burst_buffer[timer]; + uint32_t *buffer = dshot_output_buffer[timer]; const io_timers_channel_mapping_element_t *mapping = &io_timers_channel_mapping.element[timer]; unsigned num_motors = mapping->channel_count_including_gaps; unsigned timer_channel = timer_io_channels[channel].timer_channel - mapping->lowest_timer_channel; @@ -629,7 +455,7 @@ void dshot_motor_data_set(unsigned channel, uint16_t throttle, bool telemetry) int up_dshot_arm(bool armed) { - return io_timer_set_enable(armed, _birectional ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot, + return io_timer_set_enable(armed, _bidirectional ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot, IO_TIMER_ALL_MODES_CHANNELS); } @@ -658,7 +484,151 @@ void up_bdshot_status(void) PX4_INFO("dshot driver stats: read %lu, failed nibble %lu, failed CRC %lu, invalid/zero %lu", read_ok, read_fail_nibble, read_fail_crc, read_fail_zero); - PX4_INFO("ch0 %lu, ch1 %lu, ch2 %lu, ch3 %lu,", jakechannelcounter[0], jakechannelcounter[1], jakechannelcounter[2], jakechannelcounter[3]); + // PX4_INFO("ch0 %ld, ch1 %ld, ch2 %ld, ch3 %ld,", _erpms[0], _erpms[1], _erpms[2], _erpms[3]); + + int32_t* jc = jakechannelcounter; + PX4_INFO("ch0 %ld, ch1 %ld, ch2 %ld, ch3 %ld, ch4 %ld, ch5 %ld, ch6 %ld, ch7 %ld,", jc[0], jc[1], jc[2], jc[3], jc[4], jc[5], jc[6], jc[7]); + PX4_INFO("jakecounter %lu", jakecounter); +} + +uint8_t nibbles_from_mapped(uint8_t mapped) +{ + switch (mapped) { + case 0x19: + return 0x00; + + case 0x1B: + return 0x01; + + case 0x12: + return 0x02; + + case 0x13: + return 0x03; + + case 0x1D: + return 0x04; + + case 0x15: + return 0x05; + + case 0x16: + return 0x06; + + case 0x17: + return 0x07; + + case 0x1a: + return 0x08; + + case 0x09: + return 0x09; + + 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_from_erpm_frame(uint8_t* buffer, size_t buffer_size) +{ + uint32_t value = 0; + + // We start off with high + uint32_t high = 1; + + unsigned shifted = 0; + unsigned previous = 0; + + for (unsigned i = 1; i < buffer_size; ++i) { + + // We can ignore the very first data point as it's the pulse before it starts. + if (i > 1) { + + if (buffer[i] == 0) { + // Once we get zeros we're through. + break; + } + + // 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 = (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 = 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; } #endif diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index 9e9d91c905e7..940019d9bf04 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -534,8 +534,6 @@ void DShot::Run() if (_outputs_on != outputs_on) { enable_dshot_outputs(outputs_on); - PX4_INFO("DSHOT OUTPUTS ENABLED"); - px4_usleep(100'000); } if (_telemetry) { From d5a5f613125c30254552b1b987519ba814a738e9 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Sat, 19 Oct 2024 11:53:24 -0800 Subject: [PATCH 07/19] it seems that freeing DMA in the dma callback context does not work --- .../src/px4/stm/stm32_common/dshot/dshot.c | 27 ++++++++++++++----- 1 file changed, 20 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 4d1d104f0143..b41da024d7dd 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -104,6 +104,9 @@ static uint32_t read_fail_zero = 0; static int32_t jakechannelcounter[8] = { -7, -7, -7, -7, -7, -7, -7 , -7 }; static uint32_t jakecounter = 0; +static uint32_t jakedmaupallocated = 0; +static uint32_t jakedmaupallocatedarray[MAX_TIMER_IO_CHANNELS] = {}; + // We need local permanent memory for indices, channels, and callbacks for each output static uint8_t timer_index_array[MAX_IO_TIMERS] = {}; @@ -262,6 +265,8 @@ void up_dshot_trigger(void) PX4_INFO("DMA allocation for timer %u failed", timer_index); return; } + + jakedmaupallocated++; } // Set timer in dshot mode @@ -283,7 +288,10 @@ void up_dshot_trigger(void) // Trigger DMA (DShot Outputs) timer_index_array[timer_index] = timer_index; + + // TODO: this is for sure screwing everything up stm32_dmastart(dshot_handler[timer_index].dma_up_handle, _bidirectional ? dma_callback_capture_start : NULL, &timer_index_array[timer_index], false); + // stm32_dmastart(dshot_handler[timer_index].dma_up_handle, NULL, NULL, false); // Enable DMA update request io_timer_enable_update_dma_req(timer_index); @@ -297,11 +305,11 @@ void dma_callback_capture_start(DMA_HANDLE handle, uint8_t status, void *arg) uint8_t timer_index = *((uint8_t*)arg); - // // Clear DMA UP configuration + // Clean DMA UP configuration stm32_dmastop(handle); stm32_dmafree(handle); - - // NOTE: The dma_up_handle is now invalid + handle = NULL; + // dshot_handler[timer_index].dma_up_handle = NULL; // Disable DMA capture request io_timer_disable_capture_dma_req(timer_index); @@ -312,16 +320,13 @@ void dma_callback_capture_start(DMA_HANDLE handle, uint8_t status, void *arg) bool is_this_timer = timer_index == timer_io_channels[output_channel].timer_index; bool channel_enabled = _channels_init_mask & (1 << output_channel); - // if ((output_channel != 1) && (output_channel != 2) && (output_channel != 3)) { - // continue; - // } - if (is_this_timer && channel_enabled) { uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1; // Allocate DMA if (dshot_handler[timer_index].dma_ch_handle[timer_channel_index] == NULL) { dshot_handler[timer_index].dma_ch_handle[timer_channel_index] = stm32_dmachannel(io_timers[timer_index].dshot.dma_map_ch[timer_channel_index]); + jakedmaupallocatedarray[output_channel]++; } // If DMA handler is valid, start DMA @@ -371,6 +376,10 @@ static void dma_callback_capture_complete(DMA_HANDLE handle, uint8_t status, voi stm32_dmastop(handle); stm32_dmafree(handle); + handle = NULL; + + // TODO: how do we set the handle to null? + // dshot_handler[timer_index].dma_ch_handle[timer_channel_index] up_invalidate_dcache((uintptr_t)dshot_capture_buffer[output_channel], (uintptr_t)dshot_capture_buffer[output_channel] + @@ -489,6 +498,10 @@ void up_bdshot_status(void) int32_t* jc = jakechannelcounter; PX4_INFO("ch0 %ld, ch1 %ld, ch2 %ld, ch3 %ld, ch4 %ld, ch5 %ld, ch6 %ld, ch7 %ld,", jc[0], jc[1], jc[2], jc[3], jc[4], jc[5], jc[6], jc[7]); PX4_INFO("jakecounter %lu", jakecounter); + PX4_INFO("jakedmaupallocated %lu", jakedmaupallocated); + uint32_t* jca = jakedmaupallocatedarray; + PX4_INFO("alloc %ld, alloc %ld, alloc %ld, alloc %ld, alloc %ld, alloc %ld, alloc %ld, alloc %ld,", jca[0], jca[1], jca[2], jca[3], jca[4], jca[5], jca[6], jca[7]); + } uint8_t nibbles_from_mapped(uint8_t mapped) From d56fcc477081b28e17349c688d63e9fd622472bb Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Mon, 21 Oct 2024 10:59:07 -0800 Subject: [PATCH 08/19] a bunch of debugging --- .../src/px4/stm/stm32_common/dshot/dshot.c | 161 +++++++++++++----- 1 file changed, 122 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 b41da024d7dd..3ef93724c787 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -60,7 +60,8 @@ #define MAX_NUM_CHANNELS_PER_TIMER 4u // CCR1-CCR4 #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) + DMA_SCR_DIR_M2P | DMA_SCR_TCIE | DMA_SCR_TEIE | DMA_SCR_DMEIE) + // DMA_SCR_DIR_M2P | DMA_SCR_TCIE | DMA_SCR_HTIE | DMA_SCR_TEIE | DMA_SCR_DMEIE) // 16-bit because not all of the General Purpose Timers support 32-bit #define DSHOT_BIDIRECTIONAL_DMA_SCR (DMA_SCR_PRIHI | DMA_SCR_MSIZE_16BITS | DMA_SCR_PSIZE_16BITS | DMA_SCR_MINC | \ @@ -69,8 +70,6 @@ typedef struct dshot_handler_t { DMA_HANDLE dma_up_handle; // DMA stream for DMA update DMA_HANDLE dma_ch_handle[4]; // DMA streams for bidi capture compare - uint32_t dma_ch_count; - uint32_t dma_size; } dshot_handler_t; #if defined(CONFIG_ARMV7M_DCACHE) @@ -97,15 +96,28 @@ static void process_capture_results(int8_t output_channel); static unsigned calculate_period_from_erpm_frame(uint8_t* buffer, size_t buffer_size); +// decoding status 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 int32_t jakechannelcounter[8] = { -7, -7, -7, -7, -7, -7, -7 , -7 }; -static uint32_t jakecounter = 0; -static uint32_t jakedmaupallocated = 0; -static uint32_t jakedmaupallocatedarray[MAX_TIMER_IO_CHANNELS] = {}; +// begin debug variables +static int32_t capturecompletechannel[8] = { -7, -7, -7, -7, -7, -7, -7 , -7 }; +static uint32_t capturecomplete = 0; +static uint32_t dmaupallocatedcount = 0; +static uint32_t dmaupdeallocatedcount = 0; +static uint32_t dmaupallocatedcountarray[MAX_TIMER_IO_CHANNELS] = {}; +static uint32_t dmanotallocated = 0; +static uint32_t capturestart_incomplete = 0; +static uint32_t capturefinished_incomplete = 0; +static uint32_t cycles = 0; +static uint32_t interruptcounter = 0; +static uint32_t fullcapturecounter = 0; +static uint8_t dmastatus_dma_up[MAX_IO_TIMERS] = { [0 ... MAX_IO_TIMERS - 1] = 69 }; +static uint8_t dmastatus_dma_ch[MAX_TIMER_IO_CHANNELS] = { [0 ... MAX_TIMER_IO_CHANNELS - 1] = 69 }; + +// end debug variables // We need local permanent memory for indices, channels, and callbacks for each output @@ -157,7 +169,6 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bi // Set the DMA buffer size to hold all DMA data uint32_t channel_count = io_timers_channel_mapping.element[timer_index].channel_count_including_gaps; - dshot_handler[timer_index].dma_size = channel_count * CHANNEL_OUTPUT_BUFF_SIZE; // Configure timer in dshot mode io_timer_set_dshot_mode(timer_index, _dshot_frequency, channel_count); @@ -228,11 +239,12 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bi } // Kicks off a DMA transmit for each configured timer and the associated channels -void up_dshot_trigger(void) +void up_dshot_trigger() { - // For bidirectional dshot we need to re-initialize the timers every time here. - // In normal mode, we just do it once. + cycles++; + if (_bidirectional) { + // For each channel, bidirectional dshot needs to initialize each timer every time for (uint8_t output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) { if (_channels_init_mask & (1 << output_channel)) { io_timer_unallocate_channel(output_channel); @@ -242,6 +254,27 @@ void up_dshot_trigger(void) return; } } + + // We must free DMA from previous incomplete DMA CH callback + uint8_t timer_index = timer_io_channels[output_channel].timer_index; + uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1; + + if (dshot_handler[timer_index].dma_ch_handle[timer_channel_index] != NULL) { + stm32_dmastop(dshot_handler[timer_index].dma_ch_handle[timer_channel_index]); + stm32_dmafree(dshot_handler[timer_index].dma_ch_handle[timer_channel_index]); + dshot_handler[timer_index].dma_ch_handle[timer_channel_index] = NULL; + } + } + + // For each timer, bidirectional dshot needs to initialize DMA UP each time + for (uint8_t timer_index = 0; timer_index < MAX_IO_TIMERS; timer_index++) { + if (_timers_init_mask & (1 << timer_index)) { + if (dshot_handler[timer_index].dma_up_handle != NULL) { + stm32_dmastop(dshot_handler[timer_index].dma_up_handle); + stm32_dmafree(dshot_handler[timer_index].dma_up_handle); + dshot_handler[timer_index].dma_up_handle = NULL; + } + } } } @@ -252,12 +285,8 @@ void up_dshot_trigger(void) // For each timer, begin DMA transmit for (uint8_t timer_index = 0; timer_index < MAX_IO_TIMERS; timer_index++) { if (_timers_init_mask & (1 << timer_index)) { - - uint32_t channel_count = io_timers_channel_mapping.element[timer_index].channel_count_including_gaps; - // Allocate DMA if necessary if (dshot_handler[timer_index].dma_up_handle == NULL) { - dshot_handler[timer_index].dma_size = channel_count * CHANNEL_OUTPUT_BUFF_SIZE; dshot_handler[timer_index].dma_up_handle = stm32_dmachannel(io_timers[timer_index].dshot.dma_map_up); @@ -266,9 +295,11 @@ void up_dshot_trigger(void) return; } - jakedmaupallocated++; + dmaupallocatedcount++; } + uint32_t channel_count = io_timers_channel_mapping.element[timer_index].channel_count_including_gaps; + // Set timer in dshot mode io_timer_set_dshot_mode(timer_index, _dshot_frequency, channel_count); @@ -280,7 +311,7 @@ void up_dshot_trigger(void) px4_stm32_dmasetup(dshot_handler[timer_index].dma_up_handle, io_timers[timer_index].base + STM32_GTIM_DMAR_OFFSET, (uint32_t)(dshot_output_buffer[timer_index]), - dshot_handler[timer_index].dma_size, + channel_count * CHANNEL_OUTPUT_BUFF_SIZE, DSHOT_DMA_SCR); // Clean UDE flag before DMA is started @@ -289,9 +320,7 @@ void up_dshot_trigger(void) // Trigger DMA (DShot Outputs) timer_index_array[timer_index] = timer_index; - // TODO: this is for sure screwing everything up stm32_dmastart(dshot_handler[timer_index].dma_up_handle, _bidirectional ? dma_callback_capture_start : NULL, &timer_index_array[timer_index], false); - // stm32_dmastart(dshot_handler[timer_index].dma_up_handle, NULL, NULL, false); // Enable DMA update request io_timer_enable_update_dma_req(timer_index); @@ -301,15 +330,33 @@ void up_dshot_trigger(void) void dma_callback_capture_start(DMA_HANDLE handle, uint8_t status, void *arg) { - (void)status; - uint8_t timer_index = *((uint8_t*)arg); + dmastatus_dma_up[timer_index] = status; + + interruptcounter++; + + if (status & DMA_STATUS_ERROR) { + // incomplete + capturestart_incomplete++; + return; + } + + // TODO: half transfer flag is always set.... + // check for half transfer + if (status & DMA_STATUS_HTIF) { + return; + } + + fullcapturecounter++; + // Clean DMA UP configuration - stm32_dmastop(handle); - stm32_dmafree(handle); - handle = NULL; - // dshot_handler[timer_index].dma_up_handle = NULL; + if (dshot_handler[timer_index].dma_up_handle != NULL) { + stm32_dmastop(dshot_handler[timer_index].dma_up_handle); + stm32_dmafree(dshot_handler[timer_index].dma_up_handle); + dshot_handler[timer_index].dma_up_handle = NULL; + dmaupdeallocatedcount++; + } // Disable DMA capture request io_timer_disable_capture_dma_req(timer_index); @@ -317,6 +364,10 @@ void dma_callback_capture_start(DMA_HANDLE handle, uint8_t status, void *arg) // Allocate DMA for all channels on this timer for (uint8_t output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) { + if (output_channel != 0) { + continue; + } + bool is_this_timer = timer_index == timer_io_channels[output_channel].timer_index; bool channel_enabled = _channels_init_mask & (1 << output_channel); @@ -326,7 +377,9 @@ void dma_callback_capture_start(DMA_HANDLE handle, uint8_t status, void *arg) // Allocate DMA if (dshot_handler[timer_index].dma_ch_handle[timer_channel_index] == NULL) { dshot_handler[timer_index].dma_ch_handle[timer_channel_index] = stm32_dmachannel(io_timers[timer_index].dshot.dma_map_ch[timer_channel_index]); - jakedmaupallocatedarray[output_channel]++; + dmaupallocatedcountarray[output_channel]++; + } else { + dmanotallocated++; } // If DMA handler is valid, start DMA @@ -371,15 +424,33 @@ void dma_callback_capture_start(DMA_HANDLE handle, uint8_t status, void *arg) static void dma_callback_capture_complete(DMA_HANDLE handle, uint8_t status, void *arg) { - // TODO: capture complete callback firing too early? Data is zero... uint8_t output_channel = *((uint8_t*)arg); - stm32_dmastop(handle); - stm32_dmafree(handle); - handle = NULL; + dmastatus_dma_ch[output_channel] = status; + + if (status & DMA_STATUS_ERROR) { + // incomplete + capturefinished_incomplete++; + return; + } + + // check for half transfer + if (status & DMA_STATUS_HTIF) { + return; + } + + // TODO: capture complete callback firing too early? Data is zero... + uint8_t timer_index = timer_io_channels[output_channel].timer_index; + uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1; + + capturecompletechannel[output_channel] = output_channel; + capturecomplete++; - // TODO: how do we set the handle to null? - // dshot_handler[timer_index].dma_ch_handle[timer_channel_index] + if (dshot_handler[timer_index].dma_ch_handle[timer_channel_index] != NULL) { + stm32_dmastop(dshot_handler[timer_index].dma_ch_handle[timer_channel_index]); + stm32_dmafree(dshot_handler[timer_index].dma_ch_handle[timer_channel_index]); + dshot_handler[timer_index].dma_ch_handle[timer_channel_index] = NULL; + } up_invalidate_dcache((uintptr_t)dshot_capture_buffer[output_channel], (uintptr_t)dshot_capture_buffer[output_channel] + @@ -394,9 +465,6 @@ static void dma_callback_capture_complete(DMA_HANDLE handle, uint8_t status, voi void process_capture_results(int8_t output_channel) { - jakechannelcounter[output_channel] = output_channel; - jakecounter++; - const unsigned period = calculate_period_from_erpm_frame((uint8_t*)dshot_capture_buffer[output_channel], sizeof(dshot_capture_buffer[output_channel])); if (period == 0) { @@ -495,13 +563,28 @@ void up_bdshot_status(void) // PX4_INFO("ch0 %ld, ch1 %ld, ch2 %ld, ch3 %ld,", _erpms[0], _erpms[1], _erpms[2], _erpms[3]); - int32_t* jc = jakechannelcounter; + int32_t* jc = capturecompletechannel; PX4_INFO("ch0 %ld, ch1 %ld, ch2 %ld, ch3 %ld, ch4 %ld, ch5 %ld, ch6 %ld, ch7 %ld,", jc[0], jc[1], jc[2], jc[3], jc[4], jc[5], jc[6], jc[7]); - PX4_INFO("jakecounter %lu", jakecounter); - PX4_INFO("jakedmaupallocated %lu", jakedmaupallocated); - uint32_t* jca = jakedmaupallocatedarray; + PX4_INFO("capturecomplete %lu", capturecomplete); + + PX4_INFO("dmaupallocatedcount %lu", dmaupallocatedcount); + PX4_INFO("dmaupdeallocatedcount %lu", dmaupdeallocatedcount); + PX4_INFO("dmanotallocated %lu", dmanotallocated); + + PX4_INFO("capturestart_incomplete %lu", capturestart_incomplete); + PX4_INFO("capturefinished_incomplete %lu", capturefinished_incomplete); + + PX4_INFO("cycles %lu", cycles); + PX4_INFO("interruptcounter %lu", interruptcounter); + PX4_INFO("fullcapturecounter %lu", fullcapturecounter); + + uint32_t* jca = dmaupallocatedcountarray; PX4_INFO("alloc %ld, alloc %ld, alloc %ld, alloc %ld, alloc %ld, alloc %ld, alloc %ld, alloc %ld,", jca[0], jca[1], jca[2], jca[3], jca[4], jca[5], jca[6], jca[7]); + uint8_t* stup = dmastatus_dma_up; + PX4_INFO("st0 %u, st1 %u, st2 %u", stup[0], stup[1], stup[2]); + uint8_t* st = dmastatus_dma_ch; + PX4_INFO("st0 %u, st1 %u, st2 %u, st3 %u, st4 %u, st5 %u, st6 %u, st7 %u,", st[0], st[1], st[2], st[3], st[4], st[5], st[6], st[7]); } uint8_t nibbles_from_mapped(uint8_t mapped) From e6bf230985df27236dd1e9e565a31b80f1c3e255 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Mon, 21 Oct 2024 18:06:01 -0800 Subject: [PATCH 09/19] it's working now --- boards/ark/fmu-v6x/src/board_config.h | 10 +- boards/ark/fmu-v6x/src/timer_config.cpp | 4 +- .../src/px4/stm/stm32_common/dshot/dshot.c | 262 +++++++++++++----- .../stm32_common/include/px4_arch/io_timer.h | 6 +- .../px4/stm/stm32_common/io_pins/io_timer.c | 29 +- 5 files changed, 212 insertions(+), 99 deletions(-) diff --git a/boards/ark/fmu-v6x/src/board_config.h b/boards/ark/fmu-v6x/src/board_config.h index 04b84631c760..10e20f81def4 100644 --- a/boards/ark/fmu-v6x/src/board_config.h +++ b/boards/ark/fmu-v6x/src/board_config.h @@ -238,7 +238,7 @@ /* PWM */ -#define DIRECT_PWM_OUTPUT_CHANNELS 8 +#define DIRECT_PWM_OUTPUT_CHANNELS 6 #define GPIO_FMU_CH1 /* PI0 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTI|GPIO_PIN0) #define GPIO_FMU_CH2 /* PH12 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTH|GPIO_PIN12) @@ -246,8 +246,12 @@ #define GPIO_FMU_CH4 /* PH10 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTH|GPIO_PIN10) #define GPIO_FMU_CH5 /* PD13 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTD|GPIO_PIN13) #define GPIO_FMU_CH6 /* PD14 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTD|GPIO_PIN14) -#define GPIO_FMU_CH7 /* PH6 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTH|GPIO_PIN6) -#define GPIO_FMU_CH8 /* PH9 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTH|GPIO_PIN9) +// #define GPIO_FMU_CH7 /* PH6 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTH|GPIO_PIN6) +// #define GPIO_FMU_CH8 /* PH9 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTH|GPIO_PIN9) + +// JAKE: debugging logic analyzer +#define GPIO_FMU_CH7 /* PH6 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTH|GPIO_PIN6) +#define GPIO_FMU_CH8 /* PH9 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTH|GPIO_PIN9) #define GPIO_FMU_CAP /* PE11 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTE|GPIO_PIN11) #define GPIO_SPIX_SYNC /* PE9 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTE|GPIO_PIN9) diff --git a/boards/ark/fmu-v6x/src/timer_config.cpp b/boards/ark/fmu-v6x/src/timer_config.cpp index 3504c3569dc0..6cb2415dc8a6 100644 --- a/boards/ark/fmu-v6x/src/timer_config.cpp +++ b/boards/ark/fmu-v6x/src/timer_config.cpp @@ -71,8 +71,8 @@ constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel1}, {GPIO::PortH, GPIO::Pin10}), initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel2}, {GPIO::PortD, GPIO::Pin13}), initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel3}, {GPIO::PortD, GPIO::Pin14}), - initIOTimerChannel(io_timers, {Timer::Timer12, Timer::Channel1}, {GPIO::PortH, GPIO::Pin6}), - initIOTimerChannel(io_timers, {Timer::Timer12, Timer::Channel2}, {GPIO::PortH, GPIO::Pin9}), + // initIOTimerChannel(io_timers, {Timer::Timer12, Timer::Channel1}, {GPIO::PortH, GPIO::Pin6}), + // initIOTimerChannel(io_timers, {Timer::Timer12, Timer::Channel2}, {GPIO::PortH, GPIO::Pin9}), //initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel2}, {GPIO::PortE, GPIO::Pin11}), //initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel1}, {GPIO::PortE, GPIO::Pin9}), }; 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 3ef93724c787..d19edacf2af2 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -52,7 +52,12 @@ #define MOTOR_PWM_BIT_1 14u #define MOTOR_PWM_BIT_0 7u #define ONE_MOTOR_DATA_SIZE 16u -#define CHANNEL_OUTPUT_BUFF_SIZE 17u +// #define CHANNEL_OUTPUT_BUFF_SIZE 17u +#define CHANNEL_OUTPUT_BUFF_SIZE 18u + +#define CHANNEL_CAPTURE_BUFF_SIZE 16u + + #define DSHOT_THROTTLE_POSITION 5u #define DSHOT_TELEMETRY_POSITION 4u #define NIBBLES_SIZE 4u @@ -86,15 +91,20 @@ px4_cache_aligned_data() = {}; static uint32_t *dshot_output_buffer[MAX_IO_TIMERS] = {}; // static uint16_t dshot_capture_buffer[32] px4_cache_aligned_data() = {}; -static uint16_t dshot_capture_buffer[MAX_TIMER_IO_CHANNELS][32] px4_cache_aligned_data() = {}; +// static uint16_t dshot_capture_buffer[MAX_TIMER_IO_CHANNELS][32] px4_cache_aligned_data() = {}; +static uint16_t dshot_capture_buffer[MAX_TIMER_IO_CHANNELS][16] px4_cache_aligned_data() = {}; static void dma_callback_capture_start(DMA_HANDLE handle, uint8_t status, void *arg); -static void dma_callback_capture_complete(DMA_HANDLE handle, uint8_t status, void *arg); +// static void dma_callback_capture_complete(DMA_HANDLE handle, uint8_t status, void *arg); + +static void dma_callback_capture_complete(void *arg); + +// static void dma_callback_capture_complete_timeout(void *arg); static void process_capture_results(int8_t output_channel); -static unsigned calculate_period_from_erpm_frame(uint8_t* buffer, size_t buffer_size); +static unsigned calculate_period_from_erpm_frame(uint16_t* buffer, size_t buffer_size); // decoding status static uint32_t read_ok = 0; @@ -102,21 +112,23 @@ static uint32_t read_fail_nibble = 0; static uint32_t read_fail_crc = 0; static uint32_t read_fail_zero = 0; +static struct hrt_call _dma_capture_callback_call[MAX_TIMER_IO_CHANNELS]; + // begin debug variables static int32_t capturecompletechannel[8] = { -7, -7, -7, -7, -7, -7, -7 , -7 }; -static uint32_t capturecomplete = 0; static uint32_t dmaupallocatedcount = 0; static uint32_t dmaupdeallocatedcount = 0; static uint32_t dmaupallocatedcountarray[MAX_TIMER_IO_CHANNELS] = {}; static uint32_t dmanotallocated = 0; -static uint32_t capturestart_incomplete = 0; -static uint32_t capturefinished_incomplete = 0; + static uint32_t cycles = 0; static uint32_t interruptcounter = 0; -static uint32_t fullcapturecounter = 0; static uint8_t dmastatus_dma_up[MAX_IO_TIMERS] = { [0 ... MAX_IO_TIMERS - 1] = 69 }; static uint8_t dmastatus_dma_ch[MAX_TIMER_IO_CHANNELS] = { [0 ... MAX_TIMER_IO_CHANNELS - 1] = 69 }; +static uint32_t capturecompletecounter = 0; +static uint32_t capturecompletetimeout[MAX_TIMER_IO_CHANNELS] = {}; + // end debug variables @@ -243,39 +255,69 @@ void up_dshot_trigger() { cycles++; + // uint8_t timer_index = timer_io_channels[output_channel].timer_index; + // uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1; + + // if (dshot_handler[timer_index].dma_ch_handle[timer_channel_index] != NULL) { + // stm32_dmastop(dshot_handler[timer_index].dma_ch_handle[timer_channel_index]); + // stm32_dmafree(dshot_handler[timer_index].dma_ch_handle[timer_channel_index]); + // dshot_handler[timer_index].dma_ch_handle[timer_channel_index] = NULL; + // } + + // up_invalidate_dcache((uintptr_t)dshot_capture_buffer[output_channel], + // (uintptr_t)dshot_capture_buffer[output_channel] + + // sizeof(dshot_capture_buffer[output_channel])); + + + // // TODO: do we need to unallocate / reinit timers here? It's done at the start.. + // io_timer_unallocate_channel(output_channel); + // io_timer_channel_init(output_channel, IOTimerChanMode_DshotInverted, NULL, NULL); + + if (_bidirectional) { // For each channel, bidirectional dshot needs to initialize each timer every time for (uint8_t output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) { - if (_channels_init_mask & (1 << output_channel)) { - io_timer_unallocate_channel(output_channel); - int ret = io_timer_channel_init(output_channel, IOTimerChanMode_DshotInverted, NULL, NULL); - if (ret != OK) { - PX4_INFO("io_timer_channel_init %u failed", output_channel); - return; - } - } - // We must free DMA from previous incomplete DMA CH callback + // Get timer indicies and timer channel indices uint8_t timer_index = timer_io_channels[output_channel].timer_index; uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1; + // Free DMA if still allocated from previous attempt if (dshot_handler[timer_index].dma_ch_handle[timer_channel_index] != NULL) { stm32_dmastop(dshot_handler[timer_index].dma_ch_handle[timer_channel_index]); stm32_dmafree(dshot_handler[timer_index].dma_ch_handle[timer_channel_index]); dshot_handler[timer_index].dma_ch_handle[timer_channel_index] = NULL; } - } - // For each timer, bidirectional dshot needs to initialize DMA UP each time - for (uint8_t timer_index = 0; timer_index < MAX_IO_TIMERS; timer_index++) { - if (_timers_init_mask & (1 << timer_index)) { - if (dshot_handler[timer_index].dma_up_handle != NULL) { - stm32_dmastop(dshot_handler[timer_index].dma_up_handle); - stm32_dmafree(dshot_handler[timer_index].dma_up_handle); - dshot_handler[timer_index].dma_up_handle = NULL; + // Invalidate dcache from previous attempt + up_invalidate_dcache((uintptr_t)dshot_capture_buffer[output_channel], + (uintptr_t)dshot_capture_buffer[output_channel] + + sizeof(dshot_capture_buffer[output_channel])); + + // Re-initialize timer channels + if (_channels_init_mask & (1 << output_channel)) { + io_timer_unallocate_channel(output_channel); + int ret = io_timer_channel_init(output_channel, IOTimerChanMode_DshotInverted, NULL, NULL); + if (ret != OK) { + PX4_INFO("io_timer_channel_init %u failed", output_channel); + return; } } + + + // io_timer_capture_dma_req(timer_index, false); } + + // For each timer, bidirectional dshot needs to initialize DMA UP each time + // for (uint8_t timer_index = 0; timer_index < MAX_IO_TIMERS; timer_index++) { + // if (_timers_init_mask & (1 << timer_index)) { + // if (dshot_handler[timer_index].dma_up_handle != NULL) { + // stm32_dmastop(dshot_handler[timer_index].dma_up_handle); + // stm32_dmafree(dshot_handler[timer_index].dma_up_handle); + // dshot_handler[timer_index].dma_up_handle = NULL; + // } + // } + // } } // Enable all timers configured as dshot (weird way of doing this...) @@ -285,6 +327,11 @@ void up_dshot_trigger() // For each timer, begin DMA transmit for (uint8_t timer_index = 0; timer_index < MAX_IO_TIMERS; timer_index++) { if (_timers_init_mask & (1 << timer_index)) { + + // Set timer in dshot mode + uint32_t channel_count = io_timers_channel_mapping.element[timer_index].channel_count_including_gaps; + io_timer_set_dshot_mode(timer_index, _dshot_frequency, channel_count); + // Allocate DMA if necessary if (dshot_handler[timer_index].dma_up_handle == NULL) { @@ -298,11 +345,6 @@ void up_dshot_trigger() dmaupallocatedcount++; } - uint32_t channel_count = io_timers_channel_mapping.element[timer_index].channel_count_including_gaps; - - // Set timer in dshot mode - io_timer_set_dshot_mode(timer_index, _dshot_frequency, channel_count); - // Flush cache so DMA sees the data up_clean_dcache((uintptr_t) dshot_output_buffer[timer_index], (uintptr_t) dshot_output_buffer[timer_index] + @@ -315,41 +357,36 @@ void up_dshot_trigger() DSHOT_DMA_SCR); // Clean UDE flag before DMA is started - io_timer_disable_update_dma_req(timer_index); + io_timer_update_dma_req(timer_index, false); // Trigger DMA (DShot Outputs) timer_index_array[timer_index] = timer_index; - stm32_dmastart(dshot_handler[timer_index].dma_up_handle, _bidirectional ? dma_callback_capture_start : NULL, &timer_index_array[timer_index], false); + + if (_bidirectional) { + stm32_dmastart(dshot_handler[timer_index].dma_up_handle, dma_callback_capture_start, &timer_index_array[timer_index], false); + + } else { + stm32_dmastart(dshot_handler[timer_index].dma_up_handle, NULL, NULL, false); + } // Enable DMA update request - io_timer_enable_update_dma_req(timer_index); + io_timer_update_dma_req(timer_index, true); + px4_arch_gpiowrite(GPIO_FMU_CH8, true); } } } void dma_callback_capture_start(DMA_HANDLE handle, uint8_t status, void *arg) { + px4_arch_gpiowrite(GPIO_FMU_CH8, false); + uint8_t timer_index = *((uint8_t*)arg); dmastatus_dma_up[timer_index] = status; interruptcounter++; - if (status & DMA_STATUS_ERROR) { - // incomplete - capturestart_incomplete++; - return; - } - - // TODO: half transfer flag is always set.... - // check for half transfer - if (status & DMA_STATUS_HTIF) { - return; - } - - fullcapturecounter++; - // Clean DMA UP configuration if (dshot_handler[timer_index].dma_up_handle != NULL) { stm32_dmastop(dshot_handler[timer_index].dma_up_handle); @@ -359,14 +396,14 @@ void dma_callback_capture_start(DMA_HANDLE handle, uint8_t status, void *arg) } // Disable DMA capture request - io_timer_disable_capture_dma_req(timer_index); + io_timer_capture_dma_req(timer_index, false); // Allocate DMA for all channels on this timer for (uint8_t output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) { - if (output_channel != 0) { - continue; - } + // if (output_channel != 0) { + // continue; + // } bool is_this_timer = timer_index == timer_io_channels[output_channel].timer_index; bool channel_enabled = _channels_init_mask & (1 << output_channel); @@ -398,7 +435,7 @@ void dma_callback_capture_start(DMA_HANDLE handle, uint8_t status, void *arg) px4_stm32_dmasetup(dshot_handler[timer_index].dma_ch_handle[timer_channel_index], io_timers[timer_index].base + STM32_GTIM_DMAR_OFFSET, // DMA address for burst mode (16-bit, TIM2-5 only) (uint32_t) dshot_capture_buffer[output_channel], - sizeof(dshot_capture_buffer[output_channel]), + CHANNEL_CAPTURE_BUFF_SIZE, DSHOT_BIDIRECTIONAL_DMA_SCR); // Reset / Enable timer channel @@ -414,37 +451,63 @@ void dma_callback_capture_start(DMA_HANDLE handle, uint8_t status, void *arg) // Start DMA with callback if bidi dshot is enabled output_channel_array[output_channel] = output_channel; - stm32_dmastart(dshot_handler[timer_index].dma_ch_handle[timer_channel_index], dma_callback_capture_complete, &output_channel_array[output_channel], false); + // stm32_dmastart(dshot_handler[timer_index].dma_ch_handle[timer_channel_index], dma_callback_capture_complete, &output_channel_array[output_channel], false); + + + // NOTE: we can't use DMA callback since GCR encoding means the number of pulses in the pulse train (eRPM frame) + // is variable. The number of pulses (which are decoded into uint16_t) is variable and depends on the frame. + // Instead we must use a timer callback since dshot frames have fixed timings. + stm32_dmastart(dshot_handler[timer_index].dma_ch_handle[timer_channel_index], NULL, NULL, false); + + // It takes ~50us, we we'll wait 75us + hrt_call_after(&_dma_capture_callback_call[output_channel], 75, dma_callback_capture_complete, &output_channel_array[output_channel]); + + if (output_channel == 0) { + px4_arch_gpiowrite(GPIO_FMU_CH8, true); + } } } // Enable DMA capture compare on this timer - io_timer_enable_capture_dma_req(timer_index); + io_timer_capture_dma_req(timer_index, true); } -static void dma_callback_capture_complete(DMA_HANDLE handle, uint8_t status, void *arg) -{ - uint8_t output_channel = *((uint8_t*)arg); +// static void dma_callback_capture_complete_timeout(void *arg) +// { +// uint8_t output_channel = *((uint8_t*)arg); - dmastatus_dma_ch[output_channel] = status; +// capturecompletetimeout[output_channel]++; - if (status & DMA_STATUS_ERROR) { - // incomplete - capturefinished_incomplete++; - return; - } +// uint8_t timer_index = timer_io_channels[output_channel].timer_index; +// uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1; - // check for half transfer - if (status & DMA_STATUS_HTIF) { - return; - } +// if (dshot_handler[timer_index].dma_ch_handle[timer_channel_index] != NULL) { +// stm32_dmastop(dshot_handler[timer_index].dma_ch_handle[timer_channel_index]); +// stm32_dmafree(dshot_handler[timer_index].dma_ch_handle[timer_channel_index]); +// dshot_handler[timer_index].dma_ch_handle[timer_channel_index] = NULL; +// } + +// up_invalidate_dcache((uintptr_t)dshot_capture_buffer[output_channel], +// (uintptr_t)dshot_capture_buffer[output_channel] + +// sizeof(dshot_capture_buffer[output_channel])); + +// io_timer_unallocate_channel(output_channel); +// io_timer_channel_init(output_channel, IOTimerChanMode_DshotInverted, NULL, NULL); +// } + +static void dma_callback_capture_complete(void *arg) +{ + irqstate_t flags = px4_enter_critical_section(); + + uint8_t output_channel = *((uint8_t*)arg); + + capturecompletecounter++; // TODO: capture complete callback firing too early? Data is zero... uint8_t timer_index = timer_io_channels[output_channel].timer_index; uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1; capturecompletechannel[output_channel] = output_channel; - capturecomplete++; if (dshot_handler[timer_index].dma_ch_handle[timer_channel_index] != NULL) { stm32_dmastop(dshot_handler[timer_index].dma_ch_handle[timer_channel_index]); @@ -461,11 +524,59 @@ static void dma_callback_capture_complete(DMA_HANDLE handle, uint8_t status, voi // TODO: do we need to unallocate / reinit timers here? It's done at the start.. io_timer_unallocate_channel(output_channel); io_timer_channel_init(output_channel, IOTimerChanMode_DshotInverted, NULL, NULL); + + if (output_channel == 0) { + px4_arch_gpiowrite(GPIO_FMU_CH8, false); + } + + px4_leave_critical_section(flags); } +// static void dma_callback_capture_complete(DMA_HANDLE handle, uint8_t status, void *arg) +// { +// uint8_t output_channel = *((uint8_t*)arg); + +// // cancel watchdog +// hrt_cancel(&_dma_capture_callback_call[output_channel]); + +// capturecompletecounter++; + +// if (output_channel == 0) { +// px4_arch_gpiowrite(GPIO_FMU_CH8, false); +// } + +// dmastatus_dma_ch[output_channel] = status; + +// // TODO: capture complete callback firing too early? Data is zero... +// uint8_t timer_index = timer_io_channels[output_channel].timer_index; +// uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1; + +// capturecompletechannel[output_channel] = output_channel; + +// if (dshot_handler[timer_index].dma_ch_handle[timer_channel_index] != NULL) { +// stm32_dmastop(dshot_handler[timer_index].dma_ch_handle[timer_channel_index]); +// stm32_dmafree(dshot_handler[timer_index].dma_ch_handle[timer_channel_index]); +// dshot_handler[timer_index].dma_ch_handle[timer_channel_index] = NULL; +// } + +// up_invalidate_dcache((uintptr_t)dshot_capture_buffer[output_channel], +// (uintptr_t)dshot_capture_buffer[output_channel] + +// sizeof(dshot_capture_buffer[output_channel])); + +// process_capture_results(output_channel); + +// // TODO: do we need to unallocate / reinit timers here? It's done at the start.. +// io_timer_unallocate_channel(output_channel); +// io_timer_channel_init(output_channel, IOTimerChanMode_DshotInverted, NULL, NULL); + +// px4_arch_gpiowrite(GPIO_FMU_CH8, true); +// } + void process_capture_results(int8_t output_channel) { - const unsigned period = calculate_period_from_erpm_frame((uint8_t*)dshot_capture_buffer[output_channel], sizeof(dshot_capture_buffer[output_channel])); + // const unsigned period = calculate_period_from_erpm_frame((uint8_t*)dshot_capture_buffer[output_channel], sizeof(dshot_capture_buffer[output_channel])); + const unsigned period = calculate_period_from_erpm_frame(dshot_capture_buffer[output_channel], CHANNEL_CAPTURE_BUFF_SIZE); + if (period == 0) { // If the parsing failed, we get 0. @@ -565,18 +676,13 @@ void up_bdshot_status(void) int32_t* jc = capturecompletechannel; PX4_INFO("ch0 %ld, ch1 %ld, ch2 %ld, ch3 %ld, ch4 %ld, ch5 %ld, ch6 %ld, ch7 %ld,", jc[0], jc[1], jc[2], jc[3], jc[4], jc[5], jc[6], jc[7]); - PX4_INFO("capturecomplete %lu", capturecomplete); PX4_INFO("dmaupallocatedcount %lu", dmaupallocatedcount); PX4_INFO("dmaupdeallocatedcount %lu", dmaupdeallocatedcount); PX4_INFO("dmanotallocated %lu", dmanotallocated); - PX4_INFO("capturestart_incomplete %lu", capturestart_incomplete); - PX4_INFO("capturefinished_incomplete %lu", capturefinished_incomplete); - PX4_INFO("cycles %lu", cycles); PX4_INFO("interruptcounter %lu", interruptcounter); - PX4_INFO("fullcapturecounter %lu", fullcapturecounter); uint32_t* jca = dmaupallocatedcountarray; PX4_INFO("alloc %ld, alloc %ld, alloc %ld, alloc %ld, alloc %ld, alloc %ld, alloc %ld, alloc %ld,", jca[0], jca[1], jca[2], jca[3], jca[4], jca[5], jca[6], jca[7]); @@ -585,6 +691,12 @@ void up_bdshot_status(void) PX4_INFO("st0 %u, st1 %u, st2 %u", stup[0], stup[1], stup[2]); uint8_t* st = dmastatus_dma_ch; PX4_INFO("st0 %u, st1 %u, st2 %u, st3 %u, st4 %u, st5 %u, st6 %u, st7 %u,", st[0], st[1], st[2], st[3], st[4], st[5], st[6], st[7]); + + + PX4_INFO("capturecompletecounter %lu", capturecompletecounter); + + uint32_t* ccto = capturecompletetimeout; + PX4_INFO("ccto0 %lu, ccto1 %lu, ccto2 %lu, ccto3 %lu, ccto4 %lu, ccto5 %lu, ccto6 %lu, ccto7 %lu,", ccto[0], ccto[1], ccto[2], ccto[3], ccto[4], ccto[5], ccto[6], ccto[7]); } uint8_t nibbles_from_mapped(uint8_t mapped) @@ -644,7 +756,7 @@ uint8_t nibbles_from_mapped(uint8_t mapped) } } -unsigned calculate_period_from_erpm_frame(uint8_t* buffer, size_t buffer_size) +unsigned calculate_period_from_erpm_frame(uint16_t* buffer, size_t buffer_size) { uint32_t value = 0; 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 0c5b10840012..3ca0e93dfd57 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 @@ -160,10 +160,8 @@ __EXPORT int io_timer_unallocate_channel(unsigned channel); __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_enable_update_dma_req(uint8_t timer); -__EXPORT void io_timer_disable_update_dma_req(uint8_t timer); -__EXPORT void io_timer_enable_capture_dma_req(uint8_t timer); -__EXPORT void io_timer_disable_capture_dma_req(uint8_t timer); +__EXPORT void io_timer_update_dma_req(uint8_t timer, bool enable); +__EXPORT void io_timer_capture_dma_req(uint8_t timer, bool enable); __EXPORT int io_timer_set_dshot_capture_mode(uint8_t timer, unsigned dshot_pwm_rate, unsigned channel); /** 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 181b8b0ad7c4..a4793dca1163 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 @@ -511,26 +511,25 @@ static inline void io_timer_set_oneshot_mode(unsigned timer) rEGR(timer) = GTIM_EGR_UG; } -void io_timer_enable_update_dma_req(uint8_t timer) +void io_timer_update_dma_req(uint8_t timer, bool enable) { - rDIER(timer) |= ATIM_DIER_UDE; -} + if (enable) { + rDIER(timer) |= ATIM_DIER_UDE; -void io_timer_disable_update_dma_req(uint8_t timer) -{ - rDIER(timer) &= ~ATIM_DIER_UDE; -} - -void io_timer_enable_capture_dma_req(uint8_t timer) -{ - 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 { + rDIER(timer) &= ~ATIM_DIER_UDE; + } } -void io_timer_disable_capture_dma_req(uint8_t timer) +void io_timer_capture_dma_req(uint8_t timer, bool enable) { - 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); + if (enable) { + 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 | ATIM_EGR_CC3G | ATIM_EGR_CC4G); + rDIER(timer) &= ~(ATIM_DIER_CC1DE | ATIM_DIER_CC2DE | ATIM_DIER_CC3DE | ATIM_DIER_CC4DE); + } } int io_timer_set_dshot_mode(uint8_t timer, unsigned dshot_pwm_freq, uint8_t dma_burst_length) From 3c1f984f183d25849ebe2ee330b039e08130cd7a Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Mon, 21 Oct 2024 22:39:35 -0800 Subject: [PATCH 10/19] clean up, also tried adding separate callback functions for hrt callback for each output_channel, but I am still seeing a weird race condition where one of the hrt callbacks hangs causing a stall on the IO output lines and all ESCs reinitialize --- .../src/px4/stm/stm32_common/dshot/dshot.c | 104 ++++++++++-------- 1 file changed, 56 insertions(+), 48 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 d19edacf2af2..c180e4e64743 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -100,7 +100,12 @@ static void dma_callback_capture_start(DMA_HANDLE handle, uint8_t status, void * static void dma_callback_capture_complete(void *arg); -// static void dma_callback_capture_complete_timeout(void *arg); +static void capture_complete_callback_tim0_out1(void *arg); +static void capture_complete_callback_tim0_out2(void *arg); +static void capture_complete_callback_tim0_out3(void *arg); +static void capture_complete_callback_tim0_out4(void *arg); + +// static void capture_complete_callback_timeout(void *arg); static void process_capture_results(int8_t output_channel); @@ -114,20 +119,25 @@ static uint32_t read_fail_zero = 0; static struct hrt_call _dma_capture_callback_call[MAX_TIMER_IO_CHANNELS]; +typedef void (*hrt_callback_t)(void *arg); +static hrt_callback_t _cc_callbacks[4] = { + capture_complete_callback_tim0_out1, + capture_complete_callback_tim0_out2, + capture_complete_callback_tim0_out3, + capture_complete_callback_tim0_out4 +}; + // begin debug variables -static int32_t capturecompletechannel[8] = { -7, -7, -7, -7, -7, -7, -7 , -7 }; +static uint32_t capturecompletechannel[8] = {}; static uint32_t dmaupallocatedcount = 0; static uint32_t dmaupdeallocatedcount = 0; static uint32_t dmaupallocatedcountarray[MAX_TIMER_IO_CHANNELS] = {}; -static uint32_t dmanotallocated = 0; static uint32_t cycles = 0; static uint32_t interruptcounter = 0; static uint8_t dmastatus_dma_up[MAX_IO_TIMERS] = { [0 ... MAX_IO_TIMERS - 1] = 69 }; -static uint8_t dmastatus_dma_ch[MAX_TIMER_IO_CHANNELS] = { [0 ... MAX_TIMER_IO_CHANNELS - 1] = 69 }; static uint32_t capturecompletecounter = 0; -static uint32_t capturecompletetimeout[MAX_TIMER_IO_CHANNELS] = {}; // end debug variables @@ -273,7 +283,6 @@ void up_dshot_trigger() // io_timer_unallocate_channel(output_channel); // io_timer_channel_init(output_channel, IOTimerChanMode_DshotInverted, NULL, NULL); - if (_bidirectional) { // For each channel, bidirectional dshot needs to initialize each timer every time for (uint8_t output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) { @@ -303,21 +312,7 @@ void up_dshot_trigger() return; } } - - - // io_timer_capture_dma_req(timer_index, false); } - - // For each timer, bidirectional dshot needs to initialize DMA UP each time - // for (uint8_t timer_index = 0; timer_index < MAX_IO_TIMERS; timer_index++) { - // if (_timers_init_mask & (1 << timer_index)) { - // if (dshot_handler[timer_index].dma_up_handle != NULL) { - // stm32_dmastop(dshot_handler[timer_index].dma_up_handle); - // stm32_dmafree(dshot_handler[timer_index].dma_up_handle); - // dshot_handler[timer_index].dma_up_handle = NULL; - // } - // } - // } } // Enable all timers configured as dshot (weird way of doing this...) @@ -401,7 +396,7 @@ void dma_callback_capture_start(DMA_HANDLE handle, uint8_t status, void *arg) // Allocate DMA for all channels on this timer for (uint8_t output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) { - // if (output_channel != 0) { + // if (output_channel != 3) { // continue; // } @@ -415,8 +410,6 @@ void dma_callback_capture_start(DMA_HANDLE handle, uint8_t status, void *arg) if (dshot_handler[timer_index].dma_ch_handle[timer_channel_index] == NULL) { dshot_handler[timer_index].dma_ch_handle[timer_channel_index] = stm32_dmachannel(io_timers[timer_index].dshot.dma_map_ch[timer_channel_index]); dmaupallocatedcountarray[output_channel]++; - } else { - dmanotallocated++; } // If DMA handler is valid, start DMA @@ -451,16 +444,14 @@ void dma_callback_capture_start(DMA_HANDLE handle, uint8_t status, void *arg) // Start DMA with callback if bidi dshot is enabled output_channel_array[output_channel] = output_channel; - // stm32_dmastart(dshot_handler[timer_index].dma_ch_handle[timer_channel_index], dma_callback_capture_complete, &output_channel_array[output_channel], false); - - // NOTE: we can't use DMA callback since GCR encoding means the number of pulses in the pulse train (eRPM frame) + // NOTE: we can't use DMA callback since GCR encoding creates a variable length pulse train. // is variable. The number of pulses (which are decoded into uint16_t) is variable and depends on the frame. // Instead we must use a timer callback since dshot frames have fixed timings. stm32_dmastart(dshot_handler[timer_index].dma_ch_handle[timer_channel_index], NULL, NULL, false); - // It takes ~50us, we we'll wait 75us - hrt_call_after(&_dma_capture_callback_call[output_channel], 75, dma_callback_capture_complete, &output_channel_array[output_channel]); + // It takes ~50us, we we'll wait 60us + hrt_call_after(&_dma_capture_callback_call[output_channel], 60, _cc_callbacks[output_channel], &output_channel_array[output_channel]); if (output_channel == 0) { px4_arch_gpiowrite(GPIO_FMU_CH8, true); @@ -472,7 +463,7 @@ void dma_callback_capture_start(DMA_HANDLE handle, uint8_t status, void *arg) io_timer_capture_dma_req(timer_index, true); } -// static void dma_callback_capture_complete_timeout(void *arg) +// static void capture_complete_callback_timeout(void *arg) // { // uint8_t output_channel = *((uint8_t*)arg); @@ -495,19 +486,20 @@ void dma_callback_capture_start(DMA_HANDLE handle, uint8_t status, void *arg) // io_timer_channel_init(output_channel, IOTimerChanMode_DshotInverted, NULL, NULL); // } -static void dma_callback_capture_complete(void *arg) -{ - irqstate_t flags = px4_enter_critical_section(); - uint8_t output_channel = *((uint8_t*)arg); +// TODO: use a single callback for processing data from all 4 channels!!! +// - iterate over all output_channels + +static void capture_complete(uint8_t output_channel) +{ capturecompletecounter++; // TODO: capture complete callback firing too early? Data is zero... uint8_t timer_index = timer_io_channels[output_channel].timer_index; uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1; - capturecompletechannel[output_channel] = output_channel; + capturecompletechannel[output_channel]++; if (dshot_handler[timer_index].dma_ch_handle[timer_channel_index] != NULL) { stm32_dmastop(dshot_handler[timer_index].dma_ch_handle[timer_channel_index]); @@ -528,10 +520,33 @@ static void dma_callback_capture_complete(void *arg) if (output_channel == 0) { px4_arch_gpiowrite(GPIO_FMU_CH8, false); } +} + +static void capture_complete_callback_tim0_out1(void *arg) +{ + uint8_t output_channel = *((uint8_t*)arg); + capture_complete(output_channel); +} - px4_leave_critical_section(flags); +static void capture_complete_callback_tim0_out2(void *arg) +{ + uint8_t output_channel = *((uint8_t*)arg); + capture_complete(output_channel); } +static void capture_complete_callback_tim0_out3(void *arg) +{ + uint8_t output_channel = *((uint8_t*)arg); + capture_complete(output_channel); +} + +static void capture_complete_callback_tim0_out4(void *arg) +{ + uint8_t output_channel = *((uint8_t*)arg); + capture_complete(output_channel); +} + + // static void dma_callback_capture_complete(DMA_HANDLE handle, uint8_t status, void *arg) // { // uint8_t output_channel = *((uint8_t*)arg); @@ -674,29 +689,22 @@ void up_bdshot_status(void) // PX4_INFO("ch0 %ld, ch1 %ld, ch2 %ld, ch3 %ld,", _erpms[0], _erpms[1], _erpms[2], _erpms[3]); - int32_t* jc = capturecompletechannel; - PX4_INFO("ch0 %ld, ch1 %ld, ch2 %ld, ch3 %ld, ch4 %ld, ch5 %ld, ch6 %ld, ch7 %ld,", jc[0], jc[1], jc[2], jc[3], jc[4], jc[5], jc[6], jc[7]); + PX4_INFO("cycles %lu", cycles); + PX4_INFO("interruptcounter %lu", interruptcounter); + PX4_INFO("capturecompletecounter %lu", capturecompletecounter); + + uint32_t* jc = capturecompletechannel; + PX4_INFO("ch0 %lu, ch1 %lu, ch2 %lu, ch3 %lu, ch4 %lu, ch5 %lu, ch6 %lu, ch7 %lu,", jc[0], jc[1], jc[2], jc[3], jc[4], jc[5], jc[6], jc[7]); PX4_INFO("dmaupallocatedcount %lu", dmaupallocatedcount); PX4_INFO("dmaupdeallocatedcount %lu", dmaupdeallocatedcount); - PX4_INFO("dmanotallocated %lu", dmanotallocated); - - PX4_INFO("cycles %lu", cycles); - PX4_INFO("interruptcounter %lu", interruptcounter); uint32_t* jca = dmaupallocatedcountarray; PX4_INFO("alloc %ld, alloc %ld, alloc %ld, alloc %ld, alloc %ld, alloc %ld, alloc %ld, alloc %ld,", jca[0], jca[1], jca[2], jca[3], jca[4], jca[5], jca[6], jca[7]); uint8_t* stup = dmastatus_dma_up; PX4_INFO("st0 %u, st1 %u, st2 %u", stup[0], stup[1], stup[2]); - uint8_t* st = dmastatus_dma_ch; - PX4_INFO("st0 %u, st1 %u, st2 %u, st3 %u, st4 %u, st5 %u, st6 %u, st7 %u,", st[0], st[1], st[2], st[3], st[4], st[5], st[6], st[7]); - - - PX4_INFO("capturecompletecounter %lu", capturecompletecounter); - uint32_t* ccto = capturecompletetimeout; - PX4_INFO("ccto0 %lu, ccto1 %lu, ccto2 %lu, ccto3 %lu, ccto4 %lu, ccto5 %lu, ccto6 %lu, ccto7 %lu,", ccto[0], ccto[1], ccto[2], ccto[3], ccto[4], ccto[5], ccto[6], ccto[7]); } uint8_t nibbles_from_mapped(uint8_t mapped) From c7283560701260e6399dc13b33b04c46d0b39e60 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Tue, 22 Oct 2024 18:40:49 -0800 Subject: [PATCH 11/19] remove commented out stuff, this works for spurts at a time and then the outputs reset. Sometimes one or two channels won't respond or function, and will spuriously work --- .../src/px4/stm/stm32_common/dshot/dshot.c | 88 ------------------- 1 file changed, 88 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 c180e4e64743..e1ffb0510082 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -265,24 +265,6 @@ void up_dshot_trigger() { cycles++; - // uint8_t timer_index = timer_io_channels[output_channel].timer_index; - // uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1; - - // if (dshot_handler[timer_index].dma_ch_handle[timer_channel_index] != NULL) { - // stm32_dmastop(dshot_handler[timer_index].dma_ch_handle[timer_channel_index]); - // stm32_dmafree(dshot_handler[timer_index].dma_ch_handle[timer_channel_index]); - // dshot_handler[timer_index].dma_ch_handle[timer_channel_index] = NULL; - // } - - // up_invalidate_dcache((uintptr_t)dshot_capture_buffer[output_channel], - // (uintptr_t)dshot_capture_buffer[output_channel] + - // sizeof(dshot_capture_buffer[output_channel])); - - - // // TODO: do we need to unallocate / reinit timers here? It's done at the start.. - // io_timer_unallocate_channel(output_channel); - // io_timer_channel_init(output_channel, IOTimerChanMode_DshotInverted, NULL, NULL); - if (_bidirectional) { // For each channel, bidirectional dshot needs to initialize each timer every time for (uint8_t output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) { @@ -357,7 +339,6 @@ void up_dshot_trigger() // Trigger DMA (DShot Outputs) timer_index_array[timer_index] = timer_index; - if (_bidirectional) { stm32_dmastart(dshot_handler[timer_index].dma_up_handle, dma_callback_capture_start, &timer_index_array[timer_index], false); @@ -463,34 +444,6 @@ void dma_callback_capture_start(DMA_HANDLE handle, uint8_t status, void *arg) io_timer_capture_dma_req(timer_index, true); } -// static void capture_complete_callback_timeout(void *arg) -// { -// uint8_t output_channel = *((uint8_t*)arg); - -// capturecompletetimeout[output_channel]++; - -// uint8_t timer_index = timer_io_channels[output_channel].timer_index; -// uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1; - -// if (dshot_handler[timer_index].dma_ch_handle[timer_channel_index] != NULL) { -// stm32_dmastop(dshot_handler[timer_index].dma_ch_handle[timer_channel_index]); -// stm32_dmafree(dshot_handler[timer_index].dma_ch_handle[timer_channel_index]); -// dshot_handler[timer_index].dma_ch_handle[timer_channel_index] = NULL; -// } - -// up_invalidate_dcache((uintptr_t)dshot_capture_buffer[output_channel], -// (uintptr_t)dshot_capture_buffer[output_channel] + -// sizeof(dshot_capture_buffer[output_channel])); - -// io_timer_unallocate_channel(output_channel); -// io_timer_channel_init(output_channel, IOTimerChanMode_DshotInverted, NULL, NULL); -// } - - - -// TODO: use a single callback for processing data from all 4 channels!!! -// - iterate over all output_channels - static void capture_complete(uint8_t output_channel) { capturecompletecounter++; @@ -546,47 +499,6 @@ static void capture_complete_callback_tim0_out4(void *arg) capture_complete(output_channel); } - -// static void dma_callback_capture_complete(DMA_HANDLE handle, uint8_t status, void *arg) -// { -// uint8_t output_channel = *((uint8_t*)arg); - -// // cancel watchdog -// hrt_cancel(&_dma_capture_callback_call[output_channel]); - -// capturecompletecounter++; - -// if (output_channel == 0) { -// px4_arch_gpiowrite(GPIO_FMU_CH8, false); -// } - -// dmastatus_dma_ch[output_channel] = status; - -// // TODO: capture complete callback firing too early? Data is zero... -// uint8_t timer_index = timer_io_channels[output_channel].timer_index; -// uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1; - -// capturecompletechannel[output_channel] = output_channel; - -// if (dshot_handler[timer_index].dma_ch_handle[timer_channel_index] != NULL) { -// stm32_dmastop(dshot_handler[timer_index].dma_ch_handle[timer_channel_index]); -// stm32_dmafree(dshot_handler[timer_index].dma_ch_handle[timer_channel_index]); -// dshot_handler[timer_index].dma_ch_handle[timer_channel_index] = NULL; -// } - -// up_invalidate_dcache((uintptr_t)dshot_capture_buffer[output_channel], -// (uintptr_t)dshot_capture_buffer[output_channel] + -// sizeof(dshot_capture_buffer[output_channel])); - -// process_capture_results(output_channel); - -// // TODO: do we need to unallocate / reinit timers here? It's done at the start.. -// io_timer_unallocate_channel(output_channel); -// io_timer_channel_init(output_channel, IOTimerChanMode_DshotInverted, NULL, NULL); - -// px4_arch_gpiowrite(GPIO_FMU_CH8, true); -// } - void process_capture_results(int8_t output_channel) { // const unsigned period = calculate_period_from_erpm_frame((uint8_t*)dshot_capture_buffer[output_channel], sizeof(dshot_capture_buffer[output_channel])); From a0d42b8293cabc5df7c9af855bef28856f431156 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Wed, 23 Oct 2024 23:14:32 -0800 Subject: [PATCH 12/19] everything seems to be working except for cache --- .../src/px4/stm/stm32_common/dshot/dshot.c | 324 ++++++++++-------- 1 file changed, 182 insertions(+), 142 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 e1ffb0510082..d18aca737b44 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -54,6 +54,8 @@ #define ONE_MOTOR_DATA_SIZE 16u // #define CHANNEL_OUTPUT_BUFF_SIZE 17u #define CHANNEL_OUTPUT_BUFF_SIZE 18u +// #define CHANNEL_OUTPUT_BUFF_SIZE 28u + #define CHANNEL_CAPTURE_BUFF_SIZE 16u @@ -92,24 +94,23 @@ static uint32_t *dshot_output_buffer[MAX_IO_TIMERS] = {}; // static uint16_t dshot_capture_buffer[32] px4_cache_aligned_data() = {}; // static uint16_t dshot_capture_buffer[MAX_TIMER_IO_CHANNELS][32] px4_cache_aligned_data() = {}; -static uint16_t dshot_capture_buffer[MAX_TIMER_IO_CHANNELS][16] px4_cache_aligned_data() = {}; +// static uint16_t dshot_capture_buffer[MAX_TIMER_IO_CHANNELS][CHANNEL_CAPTURE_BUFF_SIZE] px4_cache_aligned_data() = {}; +static uint16_t dshot_capture_buffer[MAX_TIMER_IO_CHANNELS][32] px4_cache_aligned_data() = {}; -static void dma_callback_capture_start(DMA_HANDLE handle, uint8_t status, void *arg); -// static void dma_callback_capture_complete(DMA_HANDLE handle, uint8_t status, void *arg); -static void dma_callback_capture_complete(void *arg); +static void dma_callback_capture_start(DMA_HANDLE handle, uint8_t status, void *arg); -static void capture_complete_callback_tim0_out1(void *arg); -static void capture_complete_callback_tim0_out2(void *arg); -static void capture_complete_callback_tim0_out3(void *arg); -static void capture_complete_callback_tim0_out4(void *arg); +static void capture_complete_callback_tim0(void *arg); +static void capture_complete_callback_tim1(void *arg); +static void capture_complete_callback_tim2(void *arg); // static void capture_complete_callback_timeout(void *arg); static void process_capture_results(int8_t output_channel); -static unsigned calculate_period_from_erpm_frame(uint16_t* buffer, size_t buffer_size); +static unsigned calculate_period(uint8_t output_channel); +// static unsigned calculate_period_from_erpm_frame(uint16_t* buffer, size_t buffer_size); // decoding status static uint32_t read_ok = 0; @@ -120,31 +121,23 @@ static uint32_t read_fail_zero = 0; static struct hrt_call _dma_capture_callback_call[MAX_TIMER_IO_CHANNELS]; typedef void (*hrt_callback_t)(void *arg); -static hrt_callback_t _cc_callbacks[4] = { - capture_complete_callback_tim0_out1, - capture_complete_callback_tim0_out2, - capture_complete_callback_tim0_out3, - capture_complete_callback_tim0_out4 +static hrt_callback_t _cc_callbacks[3] = { + capture_complete_callback_tim0, + capture_complete_callback_tim1, + capture_complete_callback_tim2, }; // begin debug variables -static uint32_t capturecompletechannel[8] = {}; -static uint32_t dmaupallocatedcount = 0; -static uint32_t dmaupdeallocatedcount = 0; -static uint32_t dmaupallocatedcountarray[MAX_TIMER_IO_CHANNELS] = {}; - static uint32_t cycles = 0; static uint32_t interruptcounter = 0; -static uint8_t dmastatus_dma_up[MAX_IO_TIMERS] = { [0 ... MAX_IO_TIMERS - 1] = 69 }; - static uint32_t capturecompletecounter = 0; + // end debug variables // We need local permanent memory for indices, channels, and callbacks for each output static uint8_t timer_index_array[MAX_IO_TIMERS] = {}; -static uint8_t output_channel_array[MAX_TIMER_IO_CHANNELS] = {}; static bool _bidirectional = false; static uint32_t _dshot_frequency = 0; @@ -265,38 +258,6 @@ void up_dshot_trigger() { cycles++; - if (_bidirectional) { - // For each channel, bidirectional dshot needs to initialize each timer every time - for (uint8_t output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) { - - // Get timer indicies and timer channel indices - uint8_t timer_index = timer_io_channels[output_channel].timer_index; - uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1; - - // Free DMA if still allocated from previous attempt - if (dshot_handler[timer_index].dma_ch_handle[timer_channel_index] != NULL) { - stm32_dmastop(dshot_handler[timer_index].dma_ch_handle[timer_channel_index]); - stm32_dmafree(dshot_handler[timer_index].dma_ch_handle[timer_channel_index]); - dshot_handler[timer_index].dma_ch_handle[timer_channel_index] = NULL; - } - - // Invalidate dcache from previous attempt - up_invalidate_dcache((uintptr_t)dshot_capture_buffer[output_channel], - (uintptr_t)dshot_capture_buffer[output_channel] + - sizeof(dshot_capture_buffer[output_channel])); - - // Re-initialize timer channels - if (_channels_init_mask & (1 << output_channel)) { - io_timer_unallocate_channel(output_channel); - int ret = io_timer_channel_init(output_channel, IOTimerChanMode_DshotInverted, NULL, NULL); - if (ret != OK) { - PX4_INFO("io_timer_channel_init %u failed", output_channel); - return; - } - } - } - } - // Enable all timers configured as dshot (weird way of doing this...) io_timer_set_enable(true, _bidirectional ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot, IO_TIMER_ALL_MODES_CHANNELS); @@ -318,8 +279,6 @@ void up_dshot_trigger() PX4_INFO("DMA allocation for timer %u failed", timer_index); return; } - - dmaupallocatedcount++; } // Flush cache so DMA sees the data @@ -359,8 +318,6 @@ void dma_callback_capture_start(DMA_HANDLE handle, uint8_t status, void *arg) uint8_t timer_index = *((uint8_t*)arg); - dmastatus_dma_up[timer_index] = status; - interruptcounter++; // Clean DMA UP configuration @@ -368,18 +325,28 @@ void dma_callback_capture_start(DMA_HANDLE handle, uint8_t status, void *arg) stm32_dmastop(dshot_handler[timer_index].dma_up_handle); stm32_dmafree(dshot_handler[timer_index].dma_up_handle); dshot_handler[timer_index].dma_up_handle = NULL; - dmaupdeallocatedcount++; } - // Disable DMA capture request - io_timer_capture_dma_req(timer_index, false); + // Disable DMA update request + io_timer_update_dma_req(timer_index, false); - // Allocate DMA for all channels on this timer + // De-allocate timer + io_timer_unallocate_timer(timer_index); + + // Disable / Init timer channels first for (uint8_t output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) { - // if (output_channel != 3) { - // continue; - // } + bool is_this_timer = timer_index == timer_io_channels[output_channel].timer_index; + bool channel_enabled = _channels_init_mask & (1 << output_channel); + + if (is_this_timer && channel_enabled) { + io_timer_unallocate_channel(output_channel); + io_timer_channel_init(output_channel, IOTimerChanMode_CaptureDMA, NULL, NULL); + } + } + + // Allocate DMA for all channels on this timer + for (uint8_t output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) { bool is_this_timer = timer_index == timer_io_channels[output_channel].timer_index; bool channel_enabled = _channels_init_mask & (1 << output_channel); @@ -390,7 +357,6 @@ void dma_callback_capture_start(DMA_HANDLE handle, uint8_t status, void *arg) // Allocate DMA if (dshot_handler[timer_index].dma_ch_handle[timer_channel_index] == NULL) { dshot_handler[timer_index].dma_ch_handle[timer_channel_index] = stm32_dmachannel(io_timers[timer_index].dshot.dma_map_ch[timer_channel_index]); - dmaupallocatedcountarray[output_channel]++; } // If DMA handler is valid, start DMA @@ -401,109 +367,113 @@ void dma_callback_capture_start(DMA_HANDLE handle, uint8_t status, void *arg) // Flush cache so DMA sees the data memset(dshot_capture_buffer[output_channel], 0, sizeof(dshot_capture_buffer[output_channel])); - up_clean_dcache((uintptr_t)dshot_capture_buffer[output_channel], - (uintptr_t)dshot_capture_buffer[output_channel] + - sizeof(dshot_capture_buffer[output_channel])); + // up_clean_dcache((uintptr_t) dshot_capture_buffer[output_channel], + // (uintptr_t) dshot_capture_buffer[output_channel] + + // sizeof(dshot_capture_buffer[output_channel])); // Setup DMA for this channel px4_stm32_dmasetup(dshot_handler[timer_index].dma_ch_handle[timer_channel_index], io_timers[timer_index].base + STM32_GTIM_DMAR_OFFSET, // DMA address for burst mode (16-bit, TIM2-5 only) (uint32_t) dshot_capture_buffer[output_channel], - CHANNEL_CAPTURE_BUFF_SIZE, + sizeof(dshot_capture_buffer[output_channel]), DSHOT_BIDIRECTIONAL_DMA_SCR); - // Reset / Enable timer channel - io_timer_unallocate_channel(output_channel); - io_timer_channel_init(output_channel, IOTimerChanMode_CaptureDMA, NULL, NULL); - io_timer_set_enable(true, IOTimerChanMode_CaptureDMA, 1 << output_channel); - - // Enable input capture on this channel - up_input_capture_set(output_channel, Both, 0, NULL, NULL); - // Setup dshot capture for this channel + /////////////////////////////////// + // THIS IS IT!!! This function causes the IO to idle FLOAT, when they should idle HIGH io_timer_set_dshot_capture_mode(timer_index, _dshot_frequency, output_channel); - - // Start DMA with callback if bidi dshot is enabled - output_channel_array[output_channel] = output_channel; + /////////////////////////////////// // NOTE: we can't use DMA callback since GCR encoding creates a variable length pulse train. - // is variable. The number of pulses (which are decoded into uint16_t) is variable and depends on the frame. - // Instead we must use a timer callback since dshot frames have fixed timings. + // Instead we must use a timer callback since we can rely on dshot frame fixed timing. stm32_dmastart(dshot_handler[timer_index].dma_ch_handle[timer_channel_index], NULL, NULL, false); - - // It takes ~50us, we we'll wait 60us - hrt_call_after(&_dma_capture_callback_call[output_channel], 60, _cc_callbacks[output_channel], &output_channel_array[output_channel]); - - if (output_channel == 0) { - px4_arch_gpiowrite(GPIO_FMU_CH8, true); - } } } - // Enable DMA capture compare on this timer + // It takes ~50us, we we'll wait 60us + hrt_call_after(&_dma_capture_callback_call[timer_index], 100, _cc_callbacks[timer_index], arg); + + // Enable CaptureDMA and on all channels + io_timer_set_enable(true, IOTimerChanMode_CaptureDMA, IO_TIMER_ALL_MODES_CHANNELS); + + // Enable CaptureDMA on this timer io_timer_capture_dma_req(timer_index, true); + + px4_arch_gpiowrite(GPIO_FMU_CH8, true); } -static void capture_complete(uint8_t output_channel) +static void capture_complete(uint8_t timer_index) { capturecompletecounter++; - // TODO: capture complete callback firing too early? Data is zero... - uint8_t timer_index = timer_io_channels[output_channel].timer_index; - uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1; + for (uint8_t output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) { + + bool is_this_timer = timer_index == timer_io_channels[output_channel].timer_index; + bool channel_enabled = _channels_init_mask & (1 << output_channel); - capturecompletechannel[output_channel]++; + if (is_this_timer && channel_enabled) { + uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1; + if (dshot_handler[timer_index].dma_ch_handle[timer_channel_index] != NULL) { + stm32_dmastop(dshot_handler[timer_index].dma_ch_handle[timer_channel_index]); + stm32_dmafree(dshot_handler[timer_index].dma_ch_handle[timer_channel_index]); + dshot_handler[timer_index].dma_ch_handle[timer_channel_index] = NULL; + } + } - if (dshot_handler[timer_index].dma_ch_handle[timer_channel_index] != NULL) { - stm32_dmastop(dshot_handler[timer_index].dma_ch_handle[timer_channel_index]); - stm32_dmafree(dshot_handler[timer_index].dma_ch_handle[timer_channel_index]); - dshot_handler[timer_index].dma_ch_handle[timer_channel_index] = NULL; + // up_invalidate_dcache((uintptr_t)dshot_capture_buffer[output_channel], + // (uintptr_t)dshot_capture_buffer[output_channel] + + // sizeof(dshot_capture_buffer[output_channel])); + + process_capture_results(output_channel); + io_timer_unallocate_channel(output_channel); } - up_invalidate_dcache((uintptr_t)dshot_capture_buffer[output_channel], - (uintptr_t)dshot_capture_buffer[output_channel] + - sizeof(dshot_capture_buffer[output_channel])); + // Disable capture DMA + io_timer_capture_dma_req(timer_index, false); + // Unallocate the timer as CaptureDMA + io_timer_unallocate_timer(timer_index); - process_capture_results(output_channel); + // Re-init the channels (and timer as a consequence) + for (uint8_t output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) { - // TODO: do we need to unallocate / reinit timers here? It's done at the start.. - io_timer_unallocate_channel(output_channel); - io_timer_channel_init(output_channel, IOTimerChanMode_DshotInverted, NULL, NULL); + bool is_this_timer = timer_index == timer_io_channels[output_channel].timer_index; + bool channel_enabled = _channels_init_mask & (1 << output_channel); - if (output_channel == 0) { - px4_arch_gpiowrite(GPIO_FMU_CH8, false); + if (is_this_timer && channel_enabled) { + io_timer_channel_init(output_channel, IOTimerChanMode_DshotInverted, NULL, NULL); + } } -} -static void capture_complete_callback_tim0_out1(void *arg) -{ - uint8_t output_channel = *((uint8_t*)arg); - capture_complete(output_channel); + io_timer_set_enable(true, IOTimerChanMode_DshotInverted, IO_TIMER_ALL_MODES_CHANNELS); + + // DEBUGGING + px4_arch_gpiowrite(GPIO_FMU_CH8, false); } -static void capture_complete_callback_tim0_out2(void *arg) +static void capture_complete_callback_tim0(void *arg) { - uint8_t output_channel = *((uint8_t*)arg); - capture_complete(output_channel); + uint8_t timer_index = *((uint8_t*)arg); + capture_complete(timer_index); } -static void capture_complete_callback_tim0_out3(void *arg) +static void capture_complete_callback_tim1(void *arg) { - uint8_t output_channel = *((uint8_t*)arg); - capture_complete(output_channel); + uint8_t timer_index = *((uint8_t*)arg); + capture_complete(timer_index); } -static void capture_complete_callback_tim0_out4(void *arg) +static void capture_complete_callback_tim2(void *arg) { - uint8_t output_channel = *((uint8_t*)arg); - capture_complete(output_channel); + uint8_t timer_index = *((uint8_t*)arg); + capture_complete(timer_index); } void process_capture_results(int8_t output_channel) { // const unsigned period = calculate_period_from_erpm_frame((uint8_t*)dshot_capture_buffer[output_channel], sizeof(dshot_capture_buffer[output_channel])); - const unsigned period = calculate_period_from_erpm_frame(dshot_capture_buffer[output_channel], CHANNEL_CAPTURE_BUFF_SIZE); + // const unsigned period = calculate_period_from_erpm_frame(dshot_capture_buffer[output_channel], CHANNEL_CAPTURE_BUFF_SIZE); + const unsigned period = calculate_period(output_channel); if (period == 0) { // If the parsing failed, we get 0. @@ -604,19 +574,6 @@ void up_bdshot_status(void) PX4_INFO("cycles %lu", cycles); PX4_INFO("interruptcounter %lu", interruptcounter); PX4_INFO("capturecompletecounter %lu", capturecompletecounter); - - uint32_t* jc = capturecompletechannel; - PX4_INFO("ch0 %lu, ch1 %lu, ch2 %lu, ch3 %lu, ch4 %lu, ch5 %lu, ch6 %lu, ch7 %lu,", jc[0], jc[1], jc[2], jc[3], jc[4], jc[5], jc[6], jc[7]); - - PX4_INFO("dmaupallocatedcount %lu", dmaupallocatedcount); - PX4_INFO("dmaupdeallocatedcount %lu", dmaupdeallocatedcount); - - uint32_t* jca = dmaupallocatedcountarray; - PX4_INFO("alloc %ld, alloc %ld, alloc %ld, alloc %ld, alloc %ld, alloc %ld, alloc %ld, alloc %ld,", jca[0], jca[1], jca[2], jca[3], jca[4], jca[5], jca[6], jca[7]); - - uint8_t* stup = dmastatus_dma_up; - PX4_INFO("st0 %u, st1 %u, st2 %u", stup[0], stup[1], stup[2]); - } uint8_t nibbles_from_mapped(uint8_t mapped) @@ -676,7 +633,7 @@ uint8_t nibbles_from_mapped(uint8_t mapped) } } -unsigned calculate_period_from_erpm_frame(uint16_t* buffer, size_t buffer_size) +unsigned calculate_period(uint8_t output_channel) { uint32_t value = 0; @@ -686,19 +643,19 @@ unsigned calculate_period_from_erpm_frame(uint16_t* buffer, size_t buffer_size) unsigned shifted = 0; unsigned previous = 0; - for (unsigned i = 1; i < buffer_size; ++i) { + 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 (buffer[i] == 0) { + if (dshot_capture_buffer[output_channel][i] == 0) { // Once we get zeros we're through. break; } // 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 = (buffer[i] - previous + 5) / 20; + const uint32_t bits = (dshot_capture_buffer[output_channel][i] - previous + 5) / 20; for (unsigned bit = 0; bit < bits; ++bit) { value = value << 1; @@ -710,7 +667,7 @@ unsigned calculate_period_from_erpm_frame(uint16_t* buffer, size_t buffer_size) high = !high; } - previous = buffer[i]; + previous = dshot_capture_buffer[output_channel][i]; } if (shifted == 0) { @@ -759,4 +716,87 @@ unsigned calculate_period_from_erpm_frame(uint16_t* buffer, size_t buffer_size) return period; } +// unsigned calculate_period_from_erpm_frame(uint16_t* buffer, size_t buffer_size) +// { +// uint32_t value = 0; + +// // We start off with high +// uint32_t high = 1; + +// unsigned shifted = 0; +// unsigned previous = 0; + +// for (unsigned i = 1; i < buffer_size; ++i) { + +// // We can ignore the very first data point as it's the pulse before it starts. +// if (i > 1) { + +// if (buffer[i] == 0) { +// // Once we get zeros we're through. +// break; +// } + +// // 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 = (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 = 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; +// } + #endif From a7d7f7a1ce02dda55a0da674f83a10dfd3a963db Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Thu, 24 Oct 2024 16:55:43 -0800 Subject: [PATCH 13/19] 4 channel DMA transfer doesn't work, but any one of the single channels does --- .../src/px4/stm/stm32_common/dshot/dshot.c | 316 +++++++----------- .../stm32_common/include/px4_arch/io_timer.h | 3 +- .../px4/stm/stm32_common/io_pins/io_timer.c | 60 ++-- 3 files changed, 145 insertions(+), 234 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 d18aca737b44..f130e14c88cf 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -8,14 +8,14 @@ * are met: * * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. + * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. + * used to endorse or promote products derived from this software + * without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT @@ -49,22 +49,21 @@ #include #include -#define MOTOR_PWM_BIT_1 14u -#define MOTOR_PWM_BIT_0 7u -#define ONE_MOTOR_DATA_SIZE 16u -// #define CHANNEL_OUTPUT_BUFF_SIZE 17u -#define CHANNEL_OUTPUT_BUFF_SIZE 18u -// #define CHANNEL_OUTPUT_BUFF_SIZE 28u +#define MOTOR_PWM_BIT_1 14u +#define MOTOR_PWM_BIT_0 7u +#define ONE_MOTOR_DATA_SIZE 16u +#define CHANNEL_OUTPUT_BUFF_SIZE 18u +// #define CHANNEL_CAPTURE_BUFF_SIZE 16u +#define CHANNEL_CAPTURE_BUFF_SIZE 32u -#define CHANNEL_CAPTURE_BUFF_SIZE 16u -#define DSHOT_THROTTLE_POSITION 5u -#define DSHOT_TELEMETRY_POSITION 4u -#define NIBBLES_SIZE 4u -#define DSHOT_NUMBER_OF_NIBBLES 3u -#define MAX_NUM_CHANNELS_PER_TIMER 4u // CCR1-CCR4 +#define DSHOT_THROTTLE_POSITION 5u +#define DSHOT_TELEMETRY_POSITION 4u +#define NIBBLES_SIZE 4u +#define DSHOT_NUMBER_OF_NIBBLES 3u +#define MAX_NUM_CHANNELS_PER_TIMER 4u // CCR1-CCR4 #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_TEIE | DMA_SCR_DMEIE) @@ -75,8 +74,8 @@ DMA_SCR_DIR_P2M | DMA_SCR_TCIE | DMA_SCR_TEIE | DMA_SCR_DMEIE) typedef struct dshot_handler_t { - DMA_HANDLE dma_up_handle; // DMA stream for DMA update - DMA_HANDLE dma_ch_handle[4]; // DMA streams for bidi capture compare + DMA_HANDLE dma_up_handle; // DMA stream for DMA update + DMA_HANDLE dma_ch_handle[4]; // DMA streams for bidi capture compare } dshot_handler_t; #if defined(CONFIG_ARMV7M_DCACHE) @@ -85,19 +84,16 @@ typedef struct dshot_handler_t { #else #define DMA_ALIGN_UP(n) (n) #endif -#define DSHOT_OUTPUT_BUFFER_SIZE(channel_count) (DMA_ALIGN_UP(sizeof(uint32_t)*CHANNEL_OUTPUT_BUFF_SIZE*channel_count)) +#define DSHOT_OUTPUT_BUFFER_SIZE(channel_count) (DMA_ALIGN_UP(sizeof(uint32_t) * CHANNEL_OUTPUT_BUFF_SIZE * channel_count)) +#define DSHOT_CAPTURE_BUFFER_SIZE(channel_count) (DMA_ALIGN_UP(sizeof(uint16_t)* CHANNEL_CAPTURE_BUFF_SIZE * channel_count)) static dshot_handler_t dshot_handler[MAX_IO_TIMERS] = {}; + static uint8_t dshot_burst_buffer_array[MAX_IO_TIMERS * DSHOT_OUTPUT_BUFFER_SIZE(MAX_NUM_CHANNELS_PER_TIMER)] px4_cache_aligned_data() = {}; static uint32_t *dshot_output_buffer[MAX_IO_TIMERS] = {}; -// static uint16_t dshot_capture_buffer[32] px4_cache_aligned_data() = {}; -// static uint16_t dshot_capture_buffer[MAX_TIMER_IO_CHANNELS][32] px4_cache_aligned_data() = {}; -// static uint16_t dshot_capture_buffer[MAX_TIMER_IO_CHANNELS][CHANNEL_CAPTURE_BUFF_SIZE] px4_cache_aligned_data() = {}; -static uint16_t dshot_capture_buffer[MAX_TIMER_IO_CHANNELS][32] px4_cache_aligned_data() = {}; - - +static uint16_t dshot_capture_buffer[MAX_IO_TIMERS][MAX_NUM_CHANNELS_PER_TIMER][CHANNEL_CAPTURE_BUFF_SIZE] px4_cache_aligned_data() = {}; static void dma_callback_capture_start(DMA_HANDLE handle, uint8_t status, void *arg); @@ -105,12 +101,10 @@ static void capture_complete_callback_tim0(void *arg); static void capture_complete_callback_tim1(void *arg); static void capture_complete_callback_tim2(void *arg); -// static void capture_complete_callback_timeout(void *arg); -static void process_capture_results(int8_t output_channel); +static void process_capture_results(uint8_t timer_index); -static unsigned calculate_period(uint8_t output_channel); -// static unsigned calculate_period_from_erpm_frame(uint16_t* buffer, size_t buffer_size); +static unsigned calculate_period(uint8_t timer_index, uint8_t channel_index); // decoding status static uint32_t read_ok = 0; @@ -122,9 +116,9 @@ static struct hrt_call _dma_capture_callback_call[MAX_TIMER_IO_CHANNELS]; typedef void (*hrt_callback_t)(void *arg); static hrt_callback_t _cc_callbacks[3] = { - capture_complete_callback_tim0, - capture_complete_callback_tim1, - capture_complete_callback_tim2, + capture_complete_callback_tim0, + capture_complete_callback_tim1, + capture_complete_callback_tim2, }; // begin debug variables @@ -144,7 +138,7 @@ static uint32_t _dshot_frequency = 0; static int _timers_init_mask = 0; static int _channels_init_mask = 0; -static int32_t _erpms[MAX_TIMER_IO_CHANNELS] = {}; +static int32_t _erpms[MAX_IO_TIMERS][MAX_NUM_CHANNELS_PER_TIMER] = {}; int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bidirectional_dshot) { @@ -228,7 +222,7 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bi } // TODO: what are we doing here and why can't this be defined at compile time? - unsigned buffer_offset = 0; + unsigned output_buffer_offset = 0; for (unsigned timer_index = 0; timer_index < MAX_IO_TIMERS; timer_index++) { if (_timers_init_mask & (1 << timer_index)) { @@ -239,12 +233,13 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bi // 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_output_buffer[timer_index] = (uint32_t *) &dshot_burst_buffer_array[buffer_offset]; + dshot_output_buffer[timer_index] = (uint32_t *) &dshot_burst_buffer_array[output_buffer_offset]; + #pragma GCC diagnostic pop uint32_t channel_count = io_timers_channel_mapping.element[timer_index].channel_count_including_gaps; - buffer_offset += DSHOT_OUTPUT_BUFFER_SIZE(channel_count); + output_buffer_offset += DSHOT_OUTPUT_BUFFER_SIZE(channel_count); - if (buffer_offset > sizeof(dshot_burst_buffer_array)) { + if (output_buffer_offset > sizeof(dshot_burst_buffer_array)) { return -EINVAL; // something is wrong with the board configuration or some other logic } } @@ -350,9 +345,9 @@ void dma_callback_capture_start(DMA_HANDLE handle, uint8_t status, void *arg) bool is_this_timer = timer_index == timer_io_channels[output_channel].timer_index; bool channel_enabled = _channels_init_mask & (1 << output_channel); + uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1; if (is_this_timer && channel_enabled) { - uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1; // Allocate DMA if (dshot_handler[timer_index].dma_ch_handle[timer_channel_index] == NULL) { @@ -366,40 +361,47 @@ void dma_callback_capture_start(DMA_HANDLE handle, uint8_t status, void *arg) } // Flush cache so DMA sees the data - memset(dshot_capture_buffer[output_channel], 0, sizeof(dshot_capture_buffer[output_channel])); - // up_clean_dcache((uintptr_t) dshot_capture_buffer[output_channel], - // (uintptr_t) dshot_capture_buffer[output_channel] + - // sizeof(dshot_capture_buffer[output_channel])); + memset(dshot_capture_buffer[timer_index][timer_channel_index], 0, sizeof(dshot_capture_buffer[timer_index][timer_channel_index])); // Setup DMA for this channel px4_stm32_dmasetup(dshot_handler[timer_index].dma_ch_handle[timer_channel_index], io_timers[timer_index].base + STM32_GTIM_DMAR_OFFSET, // DMA address for burst mode (16-bit, TIM2-5 only) - (uint32_t) dshot_capture_buffer[output_channel], - sizeof(dshot_capture_buffer[output_channel]), + (uint32_t) dshot_capture_buffer[timer_index][timer_channel_index], + // sizeof(dshot_capture_buffer[timer_index][timer_channel_index]), + 32, // number of transfer (each transfer is 16bits, we have 16 32bit values so 32 16 bit transers) DSHOT_BIDIRECTIONAL_DMA_SCR); - - - /////////////////////////////////// - // THIS IS IT!!! This function causes the IO to idle FLOAT, when they should idle HIGH - io_timer_set_dshot_capture_mode(timer_index, _dshot_frequency, output_channel); - /////////////////////////////////// - - // NOTE: we can't use DMA callback since GCR encoding creates a variable length pulse train. - // Instead we must use a timer callback since we can rely on dshot frame fixed timing. - stm32_dmastart(dshot_handler[timer_index].dma_ch_handle[timer_channel_index], NULL, NULL, false); } } - // It takes ~50us, we we'll wait 60us - hrt_call_after(&_dma_capture_callback_call[timer_index], 100, _cc_callbacks[timer_index], arg); + up_clean_dcache((uintptr_t) dshot_capture_buffer[timer_index], + (uintptr_t) dshot_capture_buffer[timer_index] + DSHOT_CAPTURE_BUFFER_SIZE(MAX_NUM_CHANNELS_PER_TIMER)); - // Enable CaptureDMA and on all channels - io_timer_set_enable(true, IOTimerChanMode_CaptureDMA, IO_TIMER_ALL_MODES_CHANNELS); + io_timer_set_dshot_capture_mode(timer_index, _dshot_frequency); // Enable CaptureDMA on this timer io_timer_capture_dma_req(timer_index, true); + // Enable CaptureDMA and on all channels + io_timer_set_enable(true, IOTimerChanMode_CaptureDMA, IO_TIMER_ALL_MODES_CHANNELS); + + // Enable DMA on the channels + for (uint8_t output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) { + + bool is_this_timer = timer_index == timer_io_channels[output_channel].timer_index; + bool channel_enabled = _channels_init_mask & (1 << output_channel); + uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1; + + if (is_this_timer && channel_enabled) { + stm32_dmastart(dshot_handler[timer_index].dma_ch_handle[timer_channel_index], NULL, NULL, false); + } + } + px4_arch_gpiowrite(GPIO_FMU_CH8, true); + + // NOTE: we can't use DMA callback since GCR encoding creates a variable length pulse train. + // Instead we must use a timer callback since we can rely on dshot frame fixed timing. + // It takes ~50us, we we'll wait 60us + hrt_call_after(&_dma_capture_callback_call[timer_index], 100, _cc_callbacks[timer_index], arg); } static void capture_complete(uint8_t timer_index) @@ -410,9 +412,9 @@ static void capture_complete(uint8_t timer_index) bool is_this_timer = timer_index == timer_io_channels[output_channel].timer_index; bool channel_enabled = _channels_init_mask & (1 << output_channel); + uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1; if (is_this_timer && channel_enabled) { - uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1; if (dshot_handler[timer_index].dma_ch_handle[timer_channel_index] != NULL) { stm32_dmastop(dshot_handler[timer_index].dma_ch_handle[timer_channel_index]); stm32_dmafree(dshot_handler[timer_index].dma_ch_handle[timer_channel_index]); @@ -420,19 +422,22 @@ static void capture_complete(uint8_t timer_index) } } - // up_invalidate_dcache((uintptr_t)dshot_capture_buffer[output_channel], - // (uintptr_t)dshot_capture_buffer[output_channel] + - // sizeof(dshot_capture_buffer[output_channel])); + up_invalidate_dcache((uintptr_t) dshot_capture_buffer[timer_index], + // (uintptr_t) dshot_capture_buffer[timer_index] + 64); // clean 16 32bit regions x 4 + (uintptr_t) dshot_capture_buffer[timer_index] + DSHOT_CAPTURE_BUFFER_SIZE(MAX_NUM_CHANNELS_PER_TIMER)); - process_capture_results(output_channel); io_timer_unallocate_channel(output_channel); } // Disable capture DMA io_timer_capture_dma_req(timer_index, false); + // Unallocate the timer as CaptureDMA io_timer_unallocate_timer(timer_index); + // Process all channels on this timer + process_capture_results(timer_index); + // Re-init the channels (and timer as a consequence) for (uint8_t output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) { @@ -468,31 +473,30 @@ static void capture_complete_callback_tim2(void *arg) capture_complete(timer_index); } -void process_capture_results(int8_t output_channel) +void process_capture_results(uint8_t timer_index) { - // const unsigned period = calculate_period_from_erpm_frame((uint8_t*)dshot_capture_buffer[output_channel], sizeof(dshot_capture_buffer[output_channel])); - // const unsigned period = calculate_period_from_erpm_frame(dshot_capture_buffer[output_channel], CHANNEL_CAPTURE_BUFF_SIZE); - - const unsigned period = calculate_period(output_channel); - - if (period == 0) { - // If the parsing failed, we get 0. - _erpms[output_channel] = 0; - - } else if (period == 65408) { - // For still, we get this magic 65408 value. - _erpms[output_channel] = 0; - - } else { - // from period in us to eRPM - _erpms[output_channel] = 1000000 * 60 / period; + for (uint8_t channel_index = 0; channel_index < MAX_NUM_CHANNELS_PER_TIMER; channel_index++) { + + // Calculate the period for each channel + const unsigned period = calculate_period(timer_index, channel_index); + + if (period == 0) { + // If the parsing failed, set the eRPM to 0 + _erpms[timer_index][channel_index] = 0; + } else if (period == 65408) { + // Special case for zero motion (e.g., stationary motor) + _erpms[timer_index][channel_index] = 0; + } else { + // Convert the period to eRPM + _erpms[timer_index][channel_index] = (1000000 * 60) / period; + } } } /** -* bits 1-11 - throttle value (0-47 are reserved, 48-2047 give 2000 steps of throttle resolution) -* bit 12 - dshot telemetry enable/disable -* bits 13-16 - XOR checksum +* bits 1-11 - throttle value (0-47 are reserved, 48-2047 give 2000 steps of throttle resolution) +* bit 12 - dshot telemetry enable/disable +* bits 13-16 - XOR checksum **/ void dshot_motor_data_set(unsigned channel, uint16_t throttle, bool telemetry) { @@ -544,10 +548,13 @@ int up_dshot_arm(bool armed) IO_TIMER_ALL_MODES_CHANNELS); } -int up_bdshot_get_erpm(uint8_t channel, int *erpm) +int up_bdshot_get_erpm(uint8_t output_channel, int *erpm) { - if (_channels_init_mask & (1 << channel)) { - *erpm = _erpms[channel]; + uint8_t timer_index = timer_io_channels[output_channel].timer_index; + uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1; + + if (_channels_init_mask & (1 << output_channel)) { + *erpm = _erpms[timer_index][timer_channel_index]; return PX4_OK; } // this channel is not configured for dshot @@ -633,76 +640,70 @@ uint8_t nibbles_from_mapped(uint8_t mapped) } } -unsigned calculate_period(uint8_t output_channel) +unsigned calculate_period(uint8_t timer_index, uint8_t channel_index) { uint32_t value = 0; - - // We start off with high - uint32_t high = 1; - + uint32_t high = 1; // We start off with high unsigned shifted = 0; unsigned previous = 0; - for (unsigned i = 1; i < (32); ++i) { + // Loop through the capture buffer for the specified channel + for (unsigned i = 1; i < CHANNEL_CAPTURE_BUFF_SIZE; ++i) { - // We can ignore the very first data point as it's the pulse before it starts. + // Ignore the first data point, which is the pulse before it starts if (i > 1) { - if (dshot_capture_buffer[output_channel][i] == 0) { - // Once we get zeros we're through. + if (dshot_capture_buffer[timer_index][channel_index][i] == 0) { + // Once we hit zeros, the data has ended break; } - // 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[output_channel][i] - previous + 5) / 20; + // Quantization logic for DShot 150, 300, 600, 1200 + const uint32_t bits = (dshot_capture_buffer[timer_index][channel_index][i] - previous + 5) / 20; + // Shift the bits into the value for (unsigned bit = 0; bit < bits; ++bit) { - value = value << 1; - value |= high; + value = (value << 1) | high; ++shifted; } - // The next edge toggles. + // Toggle the high/low state on the next edge high = !high; } - previous = dshot_capture_buffer[output_channel][i]; + previous = dshot_capture_buffer[timer_index][channel_index][i]; } if (shifted == 0) { - // no data yet, or this time + // If no data was shifted, the read failed ++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 + // Ensure we've shifted 21 times; otherwise, we pad with zeros + value <<= (21 - shifted); - // From GCR to eRPM according to: - // https://brushlesswhoop.com/dshot-and-bidirectional-dshot/#erpm-transmission + // Gray code (GCR) to eRPM conversion logic unsigned gcr = (value ^ (value >> 1)); uint32_t data = 0; - // 20bits -> 5 mapped -> 4 nibbles + // Decode 4 nibbles (5-bit gray code -> 4-bit nibbles) for (unsigned i = 0; i < 4; ++i) { - uint32_t nibble = nibbles_from_mapped(gcr & (0x1F)) << (4 * i); + uint32_t nibble = nibbles_from_mapped(gcr & 0x1F) << (4 * i); - if (nibble == 0xff) { + if (nibble == 0xFF) { ++read_fail_nibble; return 0; } data |= nibble; - gcr = gcr >> 5; + gcr >>= 5; } unsigned shift = (data & 0xE000) >> 13; unsigned period = ((data & 0x1FF0) >> 4) << shift; - unsigned crc = (data & 0xf); + unsigned crc = data & 0xF; unsigned payload = (data & 0xFFF0) >> 4; unsigned calculated_crc = (~(payload ^ (payload >> 4) ^ (payload >> 8))) & 0x0F; @@ -716,87 +717,4 @@ unsigned calculate_period(uint8_t output_channel) return period; } -// unsigned calculate_period_from_erpm_frame(uint16_t* buffer, size_t buffer_size) -// { -// uint32_t value = 0; - -// // We start off with high -// uint32_t high = 1; - -// unsigned shifted = 0; -// unsigned previous = 0; - -// for (unsigned i = 1; i < buffer_size; ++i) { - -// // We can ignore the very first data point as it's the pulse before it starts. -// if (i > 1) { - -// if (buffer[i] == 0) { -// // Once we get zeros we're through. -// break; -// } - -// // 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 = (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 = 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; -// } - #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 3ca0e93dfd57..25b065194098 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,8 @@ __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_dma_req(uint8_t timer, bool enable); -__EXPORT int io_timer_set_dshot_capture_mode(uint8_t timer, unsigned dshot_pwm_rate, unsigned channel); +__EXPORT int io_timer_set_dshot_capture_mode(uint8_t timer, unsigned dshot_pwm_rate); + /** * Reserve a timer * @return 0 on success (if not used yet, or already set to the mode) 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 a4793dca1163..c3f9a792c953 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 @@ -587,49 +587,41 @@ int io_timer_set_dshot_mode(uint8_t timer, unsigned dshot_pwm_freq, uint8_t dma_ return ret_val; } -int io_timer_set_dshot_capture_mode(uint8_t timer, unsigned dshot_pwm_freq, unsigned channel) +// TODO: pass channel_mask as arg to choose which channels to trigger CC +int io_timer_set_dshot_capture_mode(uint8_t timer, unsigned dshot_pwm_freq) { + // Timer Autor Reload Register -- why -1? rARR(timer) = -1; + + // Timer Event Generation Register -- update generation, captcomp 1 - 4 rEGR(timer) = ATIM_EGR_UG | GTIM_EGR_CC1G | GTIM_EGR_CC2G | GTIM_EGR_CC3G | GTIM_EGR_CC4G; + // Timer Prescalar rPSC(timer) = ((int)(io_timers[timer].clock_freq / (dshot_pwm_freq * 5 / 4)) / DSHOT_MOTOR_PWM_BIT_WIDTH) - 1; - // TODO: hardcoded for v6x? - 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; + // 4 transfers, CCR1 to CCR4 + rDCR(timer) = TIM_DMABASE_CCR1 | TIM_DMABURSTLENGTH_4TRANSFERS; + // rDCR(timer) = TIM_DMABASE_CCR1 | TIM_DMABURSTLENGTH_1TRANSFER; + // rDCR(timer) = TIM_DMABASE_CCR1 | TIM_DMABURSTLENGTH_2TRANSFERS; - 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; - 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; + rCCER(timer) &= ~(GTIM_CCER_CC1E | GTIM_CCER_CC2E | GTIM_CCER_CC3E | GTIM_CCER_CC4E); + rCCER(timer) &= ~(GTIM_CCER_CC1P | GTIM_CCER_CC2P | GTIM_CCER_CC3P | GTIM_CCER_CC4P); + rCCER(timer) &= ~(GTIM_CCER_CC1NP | GTIM_CCER_CC2NP | GTIM_CCER_CC3NP | GTIM_CCER_CC4NP); - 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; - } + // Set all channels to input capture mode + rCCMR1(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR1_CC1S_SHIFT); // CC1 input + rCCMR1(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR1_CC2S_SHIFT); // CC2 input + rCCMR2(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR2_CC3S_SHIFT); // CC3 input + rCCMR2(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR2_CC4S_SHIFT); // CC4 input + + // Enable channels + rCCER(timer) |= (GTIM_CCER_CC1E | GTIM_CCER_CC2E | GTIM_CCER_CC3E | GTIM_CCER_CC4E); + rCCER(timer) |= (GTIM_CCER_CC1P | GTIM_CCER_CC2P | GTIM_CCER_CC3P | GTIM_CCER_CC4P); + rCCER(timer) |= (GTIM_CCER_CC1NP | GTIM_CCER_CC2NP | GTIM_CCER_CC3NP | GTIM_CCER_CC4NP); + + // Enable the timer + rCR1(timer) |= GTIM_CR1_CEN; return 0; } From d0fc1587c99906dc6e765b61eff207ca3e8d79b3 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Thu, 24 Oct 2024 19:55:08 -0800 Subject: [PATCH 14/19] it works! Switched from burst DMA to DMA on each channel, cleaned up --- .../src/px4/stm/stm32_common/dshot/dshot.c | 73 ++++++++----------- .../px4/stm/stm32_common/io_pins/io_timer.c | 28 +++---- 2 files changed, 43 insertions(+), 58 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 f130e14c88cf..8b0da3f54465 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -54,11 +54,8 @@ #define ONE_MOTOR_DATA_SIZE 16u #define CHANNEL_OUTPUT_BUFF_SIZE 18u -// #define CHANNEL_CAPTURE_BUFF_SIZE 16u #define CHANNEL_CAPTURE_BUFF_SIZE 32u - - #define DSHOT_THROTTLE_POSITION 5u #define DSHOT_TELEMETRY_POSITION 4u #define NIBBLES_SIZE 4u @@ -114,6 +111,7 @@ static uint32_t read_fail_zero = 0; static struct hrt_call _dma_capture_callback_call[MAX_TIMER_IO_CHANNELS]; +// Separate HRT callbacks for each timer typedef void (*hrt_callback_t)(void *arg); static hrt_callback_t _cc_callbacks[3] = { capture_complete_callback_tim0, @@ -121,14 +119,7 @@ static hrt_callback_t _cc_callbacks[3] = { capture_complete_callback_tim2, }; -// begin debug variables -static uint32_t cycles = 0; -static uint32_t interruptcounter = 0; -static uint32_t capturecompletecounter = 0; - - -// end debug variables - +static uint32_t delay_hrt = 0; // We need local permanent memory for indices, channels, and callbacks for each output static uint8_t timer_index_array[MAX_IO_TIMERS] = {}; @@ -251,8 +242,6 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bi // Kicks off a DMA transmit for each configured timer and the associated channels void up_dshot_trigger() { - cycles++; - // Enable all timers configured as dshot (weird way of doing this...) io_timer_set_enable(true, _bidirectional ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot, IO_TIMER_ALL_MODES_CHANNELS); @@ -309,12 +298,11 @@ void up_dshot_trigger() void dma_callback_capture_start(DMA_HANDLE handle, uint8_t status, void *arg) { + // DEBUGGING px4_arch_gpiowrite(GPIO_FMU_CH8, false); uint8_t timer_index = *((uint8_t*)arg); - interruptcounter++; - // Clean DMA UP configuration if (dshot_handler[timer_index].dma_up_handle != NULL) { stm32_dmastop(dshot_handler[timer_index].dma_up_handle); @@ -360,24 +348,27 @@ void dma_callback_capture_start(DMA_HANDLE handle, uint8_t status, void *arg) continue; } - // Flush cache so DMA sees the data - memset(dshot_capture_buffer[timer_index][timer_channel_index], 0, sizeof(dshot_capture_buffer[timer_index][timer_channel_index])); + uint32_t periph_addr = io_timers[timer_index].base + STM32_GTIM_CCR1_OFFSET + (4 * timer_channel_index); // Setup DMA for this channel px4_stm32_dmasetup(dshot_handler[timer_index].dma_ch_handle[timer_channel_index], - io_timers[timer_index].base + STM32_GTIM_DMAR_OFFSET, // DMA address for burst mode (16-bit, TIM2-5 only) + periph_addr, (uint32_t) dshot_capture_buffer[timer_index][timer_channel_index], - // sizeof(dshot_capture_buffer[timer_index][timer_channel_index]), - 32, // number of transfer (each transfer is 16bits, we have 16 32bit values so 32 16 bit transers) + CHANNEL_CAPTURE_BUFF_SIZE, DSHOT_BIDIRECTIONAL_DMA_SCR); } } + // Flush cache so DMA sees the data + memset(dshot_capture_buffer[timer_index], 0, sizeof(dshot_capture_buffer[timer_index])); up_clean_dcache((uintptr_t) dshot_capture_buffer[timer_index], (uintptr_t) dshot_capture_buffer[timer_index] + DSHOT_CAPTURE_BUFFER_SIZE(MAX_NUM_CHANNELS_PER_TIMER)); + // TODO: timer channels mask + // Enable CaptComp for the channels io_timer_set_dshot_capture_mode(timer_index, _dshot_frequency); + // TODO: timer channels mask // Enable CaptureDMA on this timer io_timer_capture_dma_req(timer_index, true); @@ -392,21 +383,25 @@ void dma_callback_capture_start(DMA_HANDLE handle, uint8_t status, void *arg) uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1; if (is_this_timer && channel_enabled) { + // NOTE: we can't use DMA callback since GCR encoding creates a variable length pulse train stm32_dmastart(dshot_handler[timer_index].dma_ch_handle[timer_channel_index], NULL, NULL, false); } } - px4_arch_gpiowrite(GPIO_FMU_CH8, true); + // 30us to switch regardless of DShot frequency + eRPM frame time + 10us for good measure + hrt_abstime frame_us = (16 * 1000000) / _dshot_frequency; // 16 bits * us_per_s / bits_per_s + hrt_abstime delay = 30 + frame_us + 10; + delay_hrt = delay; + hrt_call_after(&_dma_capture_callback_call[timer_index], delay, _cc_callbacks[timer_index], arg); - // NOTE: we can't use DMA callback since GCR encoding creates a variable length pulse train. - // Instead we must use a timer callback since we can rely on dshot frame fixed timing. - // It takes ~50us, we we'll wait 60us - hrt_call_after(&_dma_capture_callback_call[timer_index], 100, _cc_callbacks[timer_index], arg); + // DEBUGGING + px4_arch_gpiowrite(GPIO_FMU_CH8, true); } static void capture_complete(uint8_t timer_index) { - capturecompletecounter++; + // DEBUGGING + px4_arch_gpiowrite(GPIO_FMU_CH8, false); for (uint8_t output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) { @@ -420,13 +415,9 @@ static void capture_complete(uint8_t timer_index) stm32_dmafree(dshot_handler[timer_index].dma_ch_handle[timer_channel_index]); dshot_handler[timer_index].dma_ch_handle[timer_channel_index] = NULL; } - } - up_invalidate_dcache((uintptr_t) dshot_capture_buffer[timer_index], - // (uintptr_t) dshot_capture_buffer[timer_index] + 64); // clean 16 32bit regions x 4 - (uintptr_t) dshot_capture_buffer[timer_index] + DSHOT_CAPTURE_BUFFER_SIZE(MAX_NUM_CHANNELS_PER_TIMER)); - - io_timer_unallocate_channel(output_channel); + io_timer_unallocate_channel(output_channel); + } } // Disable capture DMA @@ -435,10 +426,14 @@ static void capture_complete(uint8_t timer_index) // Unallocate the timer as CaptureDMA io_timer_unallocate_timer(timer_index); - // Process all channels on this timer + // Invalidate the dcache to ensure most recent data is available + up_invalidate_dcache((uintptr_t) dshot_capture_buffer[timer_index], + (uintptr_t) dshot_capture_buffer[timer_index] + DSHOT_CAPTURE_BUFFER_SIZE(MAX_NUM_CHANNELS_PER_TIMER)); + + // Process eRPM frames from all channels on this timer process_capture_results(timer_index); - // Re-init the channels (and timer as a consequence) + // Initialize back to DShotInverted to bring IO back to the expected idle state for (uint8_t output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) { bool is_this_timer = timer_index == timer_io_channels[output_channel].timer_index; @@ -449,10 +444,8 @@ static void capture_complete(uint8_t timer_index) } } + // Enable all channels on this timer configured as DShotInverted io_timer_set_enable(true, IOTimerChanMode_DshotInverted, IO_TIMER_ALL_MODES_CHANNELS); - - // DEBUGGING - px4_arch_gpiowrite(GPIO_FMU_CH8, false); } static void capture_complete_callback_tim0(void *arg) @@ -576,11 +569,7 @@ void up_bdshot_status(void) PX4_INFO("dshot driver stats: read %lu, failed nibble %lu, failed CRC %lu, invalid/zero %lu", read_ok, read_fail_nibble, read_fail_crc, read_fail_zero); - // PX4_INFO("ch0 %ld, ch1 %ld, ch2 %ld, ch3 %ld,", _erpms[0], _erpms[1], _erpms[2], _erpms[3]); - - PX4_INFO("cycles %lu", cycles); - PX4_INFO("interruptcounter %lu", interruptcounter); - PX4_INFO("capturecompletecounter %lu", capturecompletecounter); + PX4_INFO("delay %lu", delay_hrt); } uint8_t nibbles_from_mapped(uint8_t mapped) 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 c3f9a792c953..9afea089905b 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 @@ -587,33 +587,29 @@ int io_timer_set_dshot_mode(uint8_t timer, unsigned dshot_pwm_freq, uint8_t dma_ return ret_val; } -// TODO: pass channel_mask as arg to choose which channels to trigger CC int io_timer_set_dshot_capture_mode(uint8_t timer, unsigned dshot_pwm_freq) { - // Timer Autor Reload Register -- why -1? - rARR(timer) = -1; + // Timer Autor Reload Register max value + rARR(timer) = 0xFFFFFFFF; - // Timer Event Generation Register -- update generation, captcomp 1 - 4 - rEGR(timer) = ATIM_EGR_UG | GTIM_EGR_CC1G | GTIM_EGR_CC2G | GTIM_EGR_CC3G | GTIM_EGR_CC4G; + // Timer Event Generation Register -- update generation, CC1 - CC4 + rEGR(timer) = ATIM_EGR_UG | GTIM_EGR_CC1G | GTIM_EGR_CC2G | GTIM_EGR_CC3G | GTIM_EGR_CC4G; // Timer Prescalar + // https://brushlesswhoop.com/dshot-and-bidirectional-dshot/ rPSC(timer) = ((int)(io_timers[timer].clock_freq / (dshot_pwm_freq * 5 / 4)) / DSHOT_MOTOR_PWM_BIT_WIDTH) - 1; - // 4 transfers, CCR1 to CCR4 - rDCR(timer) = TIM_DMABASE_CCR1 | TIM_DMABURSTLENGTH_4TRANSFERS; - // rDCR(timer) = TIM_DMABASE_CCR1 | TIM_DMABURSTLENGTH_1TRANSFER; - // rDCR(timer) = TIM_DMABASE_CCR1 | TIM_DMABURSTLENGTH_2TRANSFERS; - - + // Disable CaptComp on all channels rCCER(timer) &= ~(GTIM_CCER_CC1E | GTIM_CCER_CC2E | GTIM_CCER_CC3E | GTIM_CCER_CC4E); rCCER(timer) &= ~(GTIM_CCER_CC1P | GTIM_CCER_CC2P | GTIM_CCER_CC3P | GTIM_CCER_CC4P); rCCER(timer) &= ~(GTIM_CCER_CC1NP | GTIM_CCER_CC2NP | GTIM_CCER_CC3NP | GTIM_CCER_CC4NP); - // Set all channels to input capture mode - rCCMR1(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR1_CC1S_SHIFT); // CC1 input - rCCMR1(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR1_CC2S_SHIFT); // CC2 input - rCCMR2(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR2_CC3S_SHIFT); // CC3 input - rCCMR2(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR2_CC4S_SHIFT); // CC4 input + // TODO: channels_mask arg + // Enable CaptComp input on all channels + rCCMR1(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR1_CC1S_SHIFT); + rCCMR1(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR1_CC2S_SHIFT); + rCCMR2(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR2_CC3S_SHIFT); + rCCMR2(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR2_CC4S_SHIFT); // Enable channels rCCER(timer) |= (GTIM_CCER_CC1E | GTIM_CCER_CC2E | GTIM_CCER_CC3E | GTIM_CCER_CC4E); From a37702c953108e3d519e83a1abcad3849b209c2c Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Thu, 24 Oct 2024 20:08:18 -0800 Subject: [PATCH 15/19] minor clean up --- platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c | 1 + .../nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c | 8 -------- 2 files changed, 1 insertion(+), 8 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 8b0da3f54465..f9056511142a 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -324,6 +324,7 @@ void dma_callback_capture_start(DMA_HANDLE handle, uint8_t status, void *arg) if (is_this_timer && channel_enabled) { io_timer_unallocate_channel(output_channel); + // TODO: this needs to do what io_timer_set_dshot_capture_mode is currently doing io_timer_channel_init(output_channel, IOTimerChanMode_CaptureDMA, NULL, NULL); } } 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 9afea089905b..8e25b47f3820 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 @@ -860,14 +860,6 @@ int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode, rv = io_timer_init_timer(timer, mode); - if (rv == -16) { - // TODO: - // 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); From 90c77e5dd08146b4cd747e980ca6e9f3d0a15d70 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Thu, 24 Oct 2024 23:57:02 -0800 Subject: [PATCH 16/19] clean up and formatting --- .../rpi_common/include/px4_arch/io_timer.h | 2 - .../src/px4/stm/stm32_common/dshot/dshot.c | 159 +++++++++--------- .../stm32_common/include/px4_arch/io_timer.h | 3 +- .../px4/stm/stm32_common/io_pins/io_timer.c | 129 +++++++------- .../stm32h7/include/px4_arch/hw_description.h | 12 +- 5 files changed, 148 insertions(+), 157 deletions(-) diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/io_timer.h index 96de3fe9e122..6bc702266fb8 100644 --- a/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/io_timer.h @@ -148,8 +148,6 @@ __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(void); -__EXPORT extern int io_timer_set_dshot_mode(uint8_t timer, unsigned dshot_pwm_rate, uint8_t dma_burst_length); - /** * Returns the pin configuration for a specific channel, to be used as GPIO output. * 0 is returned if the channel is not valid. 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 f9056511142a..53e6722a18fb 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -49,88 +49,87 @@ #include #include +// DShot protocol definitions +#define ONE_MOTOR_DATA_SIZE 16u #define MOTOR_PWM_BIT_1 14u #define MOTOR_PWM_BIT_0 7u -#define ONE_MOTOR_DATA_SIZE 16u -#define CHANNEL_OUTPUT_BUFF_SIZE 18u - -#define CHANNEL_CAPTURE_BUFF_SIZE 32u - #define DSHOT_THROTTLE_POSITION 5u #define DSHOT_TELEMETRY_POSITION 4u #define NIBBLES_SIZE 4u #define DSHOT_NUMBER_OF_NIBBLES 3u #define MAX_NUM_CHANNELS_PER_TIMER 4u // CCR1-CCR4 +// DMA stream configuration registers #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_TEIE | DMA_SCR_DMEIE) - // DMA_SCR_DIR_M2P | DMA_SCR_TCIE | DMA_SCR_HTIE | DMA_SCR_TEIE | DMA_SCR_DMEIE) // 16-bit because not all of the General Purpose Timers support 32-bit #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_up_handle; // DMA stream for DMA update - DMA_HANDLE dma_ch_handle[4]; // DMA streams for bidi capture compare -} dshot_handler_t; - #if defined(CONFIG_ARMV7M_DCACHE) # define DMA_BUFFER_MASK (ARMV7M_DCACHE_LINESIZE - 1) # define DMA_ALIGN_UP(n) (((n) + DMA_BUFFER_MASK) & ~DMA_BUFFER_MASK) #else #define DMA_ALIGN_UP(n) (n) #endif + +#define CHANNEL_OUTPUT_BUFF_SIZE 17u +#define CHANNEL_CAPTURE_BUFF_SIZE 32u + #define DSHOT_OUTPUT_BUFFER_SIZE(channel_count) (DMA_ALIGN_UP(sizeof(uint32_t) * CHANNEL_OUTPUT_BUFF_SIZE * channel_count)) #define DSHOT_CAPTURE_BUFFER_SIZE(channel_count) (DMA_ALIGN_UP(sizeof(uint16_t)* CHANNEL_CAPTURE_BUFF_SIZE * channel_count)) -static dshot_handler_t dshot_handler[MAX_IO_TIMERS] = {}; +// Holds DMA handles (UP / CH1-4) +typedef struct timer_dma_handles_t { + DMA_HANDLE dma_up_handle; // DMA stream for DMA update + DMA_HANDLE dma_ch_handle[4]; // DMA streams for bidi capture compare +} timer_dma_handles_t; +static timer_dma_handles_t timer_dma_handles[MAX_IO_TIMERS] = {}; + +// Output buffer of interleaved motor output bytes static uint8_t dshot_burst_buffer_array[MAX_IO_TIMERS * DSHOT_OUTPUT_BUFFER_SIZE(MAX_NUM_CHANNELS_PER_TIMER)] px4_cache_aligned_data() = {}; static uint32_t *dshot_output_buffer[MAX_IO_TIMERS] = {}; +// Array of buffers for timer/channel erpm data static uint16_t dshot_capture_buffer[MAX_IO_TIMERS][MAX_NUM_CHANNELS_PER_TIMER][CHANNEL_CAPTURE_BUFF_SIZE] px4_cache_aligned_data() = {}; -static void dma_callback_capture_start(DMA_HANDLE handle, uint8_t status, void *arg); - -static void capture_complete_callback_tim0(void *arg); -static void capture_complete_callback_tim1(void *arg); -static void capture_complete_callback_tim2(void *arg); - +static void dma_start_callback(DMA_HANDLE handle, uint8_t status, void *arg); +static void cc_callback_timer_index_0(void *arg); +static void cc_callback_timer_index_1(void *arg); +static void cc_callback_timer_index_2(void *arg); static void process_capture_results(uint8_t timer_index); - static unsigned calculate_period(uint8_t timer_index, uint8_t channel_index); -// decoding status -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 _bidirectional = false; +static uint32_t _dshot_frequency = 0; +static int _timers_init_mask = 0; +static int _channels_init_mask = 0; + +static uint8_t _timer_index_array[MAX_IO_TIMERS] = {}; +static int32_t _erpms[MAX_IO_TIMERS][MAX_NUM_CHANNELS_PER_TIMER] = {}; +// We need local permanent memory for indices, channels, and callbacks for each output static struct hrt_call _dma_capture_callback_call[MAX_TIMER_IO_CHANNELS]; // Separate HRT callbacks for each timer typedef void (*hrt_callback_t)(void *arg); static hrt_callback_t _cc_callbacks[3] = { - capture_complete_callback_tim0, - capture_complete_callback_tim1, - capture_complete_callback_tim2, + cc_callback_timer_index_0, + cc_callback_timer_index_1, + cc_callback_timer_index_2, }; +// decoding status +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 uint32_t delay_hrt = 0; -// We need local permanent memory for indices, channels, and callbacks for each output -static uint8_t timer_index_array[MAX_IO_TIMERS] = {}; - -static bool _bidirectional = false; -static uint32_t _dshot_frequency = 0; -static int _timers_init_mask = 0; -static int _channels_init_mask = 0; - -static int32_t _erpms[MAX_IO_TIMERS][MAX_NUM_CHANNELS_PER_TIMER] = {}; - int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bidirectional_dshot) { _bidirectional = enable_bidirectional_dshot; @@ -141,7 +140,6 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bi if (channel_mask & (1 << output_channel)) { uint8_t timer_index = timer_io_channels[output_channel].timer_index; - // TODO: is there a smarter way to do this? if (io_timers[timer_index].dshot.dma_base == 0) { // board does not configure dshot on this timer continue; @@ -159,44 +157,39 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bi } } - // For each dshot timer: - // - enable dshot mode - // - allocate DMA for Update (transmit) - // - allocate DMA for Capture Compare (receive) + // For each dshot timer, test the DMA UP configuration. for (uint8_t timer_index = 0; timer_index < MAX_IO_TIMERS; timer_index++) { if (_timers_init_mask & (1 << timer_index)) { - if (dshot_handler[timer_index].dma_up_handle == NULL) { + if (timer_dma_handles[timer_index].dma_up_handle == NULL) { // Set the DMA buffer size to hold all DMA data uint32_t channel_count = io_timers_channel_mapping.element[timer_index].channel_count_including_gaps; // Configure timer in dshot mode - io_timer_set_dshot_mode(timer_index, _dshot_frequency, channel_count); + io_timer_set_dshot_burst_mode(timer_index, _dshot_frequency, channel_count); // Configure DMA UP handle - dshot_handler[timer_index].dma_up_handle = stm32_dmachannel(io_timers[timer_index].dshot.dma_map_up); + timer_dma_handles[timer_index].dma_up_handle = stm32_dmachannel(io_timers[timer_index].dshot.dma_map_up); - if (dshot_handler[timer_index].dma_up_handle == NULL) { - // TODO: how to log this? + if (timer_dma_handles[timer_index].dma_up_handle == NULL) { PX4_INFO("could not allocate timer %u DMA UP handle", timer_index); return -ENOSR; } // This was just for testing. We will initialize again at run time. - stm32_dmafree(dshot_handler[timer_index].dma_up_handle); - dshot_handler[timer_index].dma_up_handle = NULL; + stm32_dmafree(timer_dma_handles[timer_index].dma_up_handle); + timer_dma_handles[timer_index].dma_up_handle = NULL; } } } - // For each dshot channel: - // - allocate DMA for Capture Compare (receive) + // For each dshot channel, test the input DMA Capture Compare configuration for (uint8_t output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) { if (_channels_init_mask & (1 << output_channel)) { uint8_t timer_index = timer_io_channels[output_channel].timer_index; uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1; - DMA_HANDLE* dma_handle = &dshot_handler[timer_index].dma_ch_handle[timer_channel_index]; + DMA_HANDLE* dma_handle = &timer_dma_handles[timer_index].dma_ch_handle[timer_channel_index]; uint32_t dma_map_ch = io_timers[timer_index].dshot.dma_map_ch[timer_channel_index]; if (dma_map_ch) { *dma_handle = stm32_dmachannel(dma_map_ch); @@ -212,7 +205,7 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bi } } - // TODO: what are we doing here and why can't this be defined at compile time? + // TODO: can we define this differently? unsigned output_buffer_offset = 0; for (unsigned timer_index = 0; timer_index < MAX_IO_TIMERS; timer_index++) { @@ -242,7 +235,7 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bi // Kicks off a DMA transmit for each configured timer and the associated channels void up_dshot_trigger() { - // Enable all timers configured as dshot (weird way of doing this...) + // Enable DShot inverted on all channels io_timer_set_enable(true, _bidirectional ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot, IO_TIMER_ALL_MODES_CHANNELS); @@ -252,14 +245,14 @@ void up_dshot_trigger() // Set timer in dshot mode uint32_t channel_count = io_timers_channel_mapping.element[timer_index].channel_count_including_gaps; - io_timer_set_dshot_mode(timer_index, _dshot_frequency, channel_count); + io_timer_set_dshot_burst_mode(timer_index, _dshot_frequency, channel_count); // Allocate DMA if necessary - if (dshot_handler[timer_index].dma_up_handle == NULL) { + if (timer_dma_handles[timer_index].dma_up_handle == NULL) { - dshot_handler[timer_index].dma_up_handle = stm32_dmachannel(io_timers[timer_index].dshot.dma_map_up); + timer_dma_handles[timer_index].dma_up_handle = stm32_dmachannel(io_timers[timer_index].dshot.dma_map_up); - if (dshot_handler[timer_index].dma_up_handle == NULL) { + if (timer_dma_handles[timer_index].dma_up_handle == NULL) { PX4_INFO("DMA allocation for timer %u failed", timer_index); return; } @@ -270,7 +263,7 @@ void up_dshot_trigger() (uintptr_t) dshot_output_buffer[timer_index] + DSHOT_OUTPUT_BUFFER_SIZE(channel_count)); - px4_stm32_dmasetup(dshot_handler[timer_index].dma_up_handle, + px4_stm32_dmasetup(timer_dma_handles[timer_index].dma_up_handle, io_timers[timer_index].base + STM32_GTIM_DMAR_OFFSET, (uint32_t)(dshot_output_buffer[timer_index]), channel_count * CHANNEL_OUTPUT_BUFF_SIZE, @@ -280,13 +273,13 @@ void up_dshot_trigger() io_timer_update_dma_req(timer_index, false); // Trigger DMA (DShot Outputs) - timer_index_array[timer_index] = timer_index; + _timer_index_array[timer_index] = timer_index; if (_bidirectional) { - stm32_dmastart(dshot_handler[timer_index].dma_up_handle, dma_callback_capture_start, &timer_index_array[timer_index], false); + stm32_dmastart(timer_dma_handles[timer_index].dma_up_handle, dma_start_callback, &_timer_index_array[timer_index], false); } else { - stm32_dmastart(dshot_handler[timer_index].dma_up_handle, NULL, NULL, false); + stm32_dmastart(timer_dma_handles[timer_index].dma_up_handle, NULL, NULL, false); } // Enable DMA update request @@ -296,7 +289,7 @@ void up_dshot_trigger() } } -void dma_callback_capture_start(DMA_HANDLE handle, uint8_t status, void *arg) +void dma_start_callback(DMA_HANDLE handle, uint8_t status, void *arg) { // DEBUGGING px4_arch_gpiowrite(GPIO_FMU_CH8, false); @@ -304,10 +297,10 @@ void dma_callback_capture_start(DMA_HANDLE handle, uint8_t status, void *arg) uint8_t timer_index = *((uint8_t*)arg); // Clean DMA UP configuration - if (dshot_handler[timer_index].dma_up_handle != NULL) { - stm32_dmastop(dshot_handler[timer_index].dma_up_handle); - stm32_dmafree(dshot_handler[timer_index].dma_up_handle); - dshot_handler[timer_index].dma_up_handle = NULL; + if (timer_dma_handles[timer_index].dma_up_handle != NULL) { + stm32_dmastop(timer_dma_handles[timer_index].dma_up_handle); + stm32_dmafree(timer_dma_handles[timer_index].dma_up_handle); + timer_dma_handles[timer_index].dma_up_handle = NULL; } // Disable DMA update request @@ -324,7 +317,6 @@ void dma_callback_capture_start(DMA_HANDLE handle, uint8_t status, void *arg) if (is_this_timer && channel_enabled) { io_timer_unallocate_channel(output_channel); - // TODO: this needs to do what io_timer_set_dshot_capture_mode is currently doing io_timer_channel_init(output_channel, IOTimerChanMode_CaptureDMA, NULL, NULL); } } @@ -339,20 +331,21 @@ void dma_callback_capture_start(DMA_HANDLE handle, uint8_t status, void *arg) if (is_this_timer && channel_enabled) { // Allocate DMA - if (dshot_handler[timer_index].dma_ch_handle[timer_channel_index] == NULL) { - dshot_handler[timer_index].dma_ch_handle[timer_channel_index] = stm32_dmachannel(io_timers[timer_index].dshot.dma_map_ch[timer_channel_index]); + if (timer_dma_handles[timer_index].dma_ch_handle[timer_channel_index] == NULL) { + timer_dma_handles[timer_index].dma_ch_handle[timer_channel_index] = stm32_dmachannel(io_timers[timer_index].dshot.dma_map_ch[timer_channel_index]); } // If DMA handler is valid, start DMA - if (dshot_handler[timer_index].dma_ch_handle[timer_channel_index] == NULL) { + if (timer_dma_handles[timer_index].dma_ch_handle[timer_channel_index] == NULL) { PX4_INFO("failed to allocate dma for timer %u channel %u", timer_index, timer_channel_index); continue; } + // Choose which CC register for this DMA stream uint32_t periph_addr = io_timers[timer_index].base + STM32_GTIM_CCR1_OFFSET + (4 * timer_channel_index); // Setup DMA for this channel - px4_stm32_dmasetup(dshot_handler[timer_index].dma_ch_handle[timer_channel_index], + px4_stm32_dmasetup(timer_dma_handles[timer_index].dma_ch_handle[timer_channel_index], periph_addr, (uint32_t) dshot_capture_buffer[timer_index][timer_channel_index], CHANNEL_CAPTURE_BUFF_SIZE, @@ -365,18 +358,18 @@ void dma_callback_capture_start(DMA_HANDLE handle, uint8_t status, void *arg) up_clean_dcache((uintptr_t) dshot_capture_buffer[timer_index], (uintptr_t) dshot_capture_buffer[timer_index] + DSHOT_CAPTURE_BUFFER_SIZE(MAX_NUM_CHANNELS_PER_TIMER)); - // TODO: timer channels mask + // TODO: timer channels mask, right now we enable all 4 channels // Enable CaptComp for the channels io_timer_set_dshot_capture_mode(timer_index, _dshot_frequency); - // TODO: timer channels mask + // TODO: timer channels mask, right now we enable all 4 channels // Enable CaptureDMA on this timer io_timer_capture_dma_req(timer_index, true); // Enable CaptureDMA and on all channels io_timer_set_enable(true, IOTimerChanMode_CaptureDMA, IO_TIMER_ALL_MODES_CHANNELS); - // Enable DMA on the channels + // Start DMA on all the channels for (uint8_t output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) { bool is_this_timer = timer_index == timer_io_channels[output_channel].timer_index; @@ -385,7 +378,7 @@ void dma_callback_capture_start(DMA_HANDLE handle, uint8_t status, void *arg) if (is_this_timer && channel_enabled) { // NOTE: we can't use DMA callback since GCR encoding creates a variable length pulse train - stm32_dmastart(dshot_handler[timer_index].dma_ch_handle[timer_channel_index], NULL, NULL, false); + stm32_dmastart(timer_dma_handles[timer_index].dma_ch_handle[timer_channel_index], NULL, NULL, false); } } @@ -411,10 +404,10 @@ static void capture_complete(uint8_t timer_index) uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1; if (is_this_timer && channel_enabled) { - if (dshot_handler[timer_index].dma_ch_handle[timer_channel_index] != NULL) { - stm32_dmastop(dshot_handler[timer_index].dma_ch_handle[timer_channel_index]); - stm32_dmafree(dshot_handler[timer_index].dma_ch_handle[timer_channel_index]); - dshot_handler[timer_index].dma_ch_handle[timer_channel_index] = NULL; + if (timer_dma_handles[timer_index].dma_ch_handle[timer_channel_index] != NULL) { + stm32_dmastop(timer_dma_handles[timer_index].dma_ch_handle[timer_channel_index]); + stm32_dmafree(timer_dma_handles[timer_index].dma_ch_handle[timer_channel_index]); + timer_dma_handles[timer_index].dma_ch_handle[timer_channel_index] = NULL; } io_timer_unallocate_channel(output_channel); @@ -449,19 +442,19 @@ static void capture_complete(uint8_t timer_index) io_timer_set_enable(true, IOTimerChanMode_DshotInverted, IO_TIMER_ALL_MODES_CHANNELS); } -static void capture_complete_callback_tim0(void *arg) +static void cc_callback_timer_index_0(void *arg) { uint8_t timer_index = *((uint8_t*)arg); capture_complete(timer_index); } -static void capture_complete_callback_tim1(void *arg) +static void cc_callback_timer_index_1(void *arg) { uint8_t timer_index = *((uint8_t*)arg); capture_complete(timer_index); } -static void capture_complete_callback_tim2(void *arg) +static void cc_callback_timer_index_2(void *arg) { uint8_t timer_index = *((uint8_t*)arg); capture_complete(timer_index); 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 25b065194098..fba66e34f111 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,6 +163,8 @@ __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_dma_req(uint8_t timer, bool enable); + +__EXPORT extern int io_timer_set_dshot_burst_mode(uint8_t timer, unsigned dshot_pwm_rate, uint8_t dma_burst_length); __EXPORT int io_timer_set_dshot_capture_mode(uint8_t timer, unsigned dshot_pwm_rate); /** @@ -173,7 +175,6 @@ __EXPORT int io_timer_allocate_timer(unsigned timer, io_timer_channel_mode_t mod __EXPORT int io_timer_unallocate_timer(unsigned timer); -__EXPORT extern int io_timer_set_dshot_mode(uint8_t timer, unsigned dshot_pwm_rate, uint8_t dma_burst_length); /** * Returns the pin configuration for a specific channel, to be used as GPIO output. 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 8e25b47f3820..2b44112fc6ae 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 @@ -96,42 +96,41 @@ static int io_timer_handler7(int irq, void *context, void *arg); #define MAX_CHANNELS_PER_TIMER 4 -#define _REG32(_base, _reg) (*(volatile uint32_t *)(_base + _reg)) -#define REG(_tmr, _reg) _REG32(io_timers[_tmr].base, _reg) - -#define rCR1(_tmr) REG(_tmr, STM32_GTIM_CR1_OFFSET) -#define rCR2(_tmr) REG(_tmr, STM32_GTIM_CR2_OFFSET) -#define rSMCR(_tmr) REG(_tmr, STM32_GTIM_SMCR_OFFSET) -#define rDIER(_tmr) REG(_tmr, STM32_GTIM_DIER_OFFSET) -#define rSR(_tmr) REG(_tmr, STM32_GTIM_SR_OFFSET) -#define rEGR(_tmr) REG(_tmr, STM32_GTIM_EGR_OFFSET) -#define rCCMR1(_tmr) REG(_tmr, STM32_GTIM_CCMR1_OFFSET) -#define rCCMR2(_tmr) REG(_tmr, STM32_GTIM_CCMR2_OFFSET) -#define rCCER(_tmr) REG(_tmr, STM32_GTIM_CCER_OFFSET) -#define rCNT(_tmr) REG(_tmr, STM32_GTIM_CNT_OFFSET) -#define rPSC(_tmr) REG(_tmr, STM32_GTIM_PSC_OFFSET) -#define rARR(_tmr) REG(_tmr, STM32_GTIM_ARR_OFFSET) -#define rCCR1(_tmr) REG(_tmr, STM32_GTIM_CCR1_OFFSET) -#define rCCR2(_tmr) REG(_tmr, STM32_GTIM_CCR2_OFFSET) -#define rCCR3(_tmr) REG(_tmr, STM32_GTIM_CCR3_OFFSET) -#define rCCR4(_tmr) REG(_tmr, STM32_GTIM_CCR4_OFFSET) -#define rDCR(_tmr) REG(_tmr, STM32_GTIM_DCR_OFFSET) -#define rDMAR(_tmr) REG(_tmr, STM32_GTIM_DMAR_OFFSET) -#define rBDTR(_tmr) REG(_tmr, STM32_ATIM_BDTR_OFFSET) +#define _REG32(_base, _reg) (*(volatile uint32_t *)(_base + _reg)) +#define REG(_tmr, _reg) _REG32(io_timers[_tmr].base, _reg) + +#define rCR1(_tmr) REG(_tmr, STM32_GTIM_CR1_OFFSET) +#define rCR2(_tmr) REG(_tmr, STM32_GTIM_CR2_OFFSET) +#define rSMCR(_tmr) REG(_tmr, STM32_GTIM_SMCR_OFFSET) +#define rDIER(_tmr) REG(_tmr, STM32_GTIM_DIER_OFFSET) +#define rSR(_tmr) REG(_tmr, STM32_GTIM_SR_OFFSET) +#define rEGR(_tmr) REG(_tmr, STM32_GTIM_EGR_OFFSET) +#define rCCMR1(_tmr) REG(_tmr, STM32_GTIM_CCMR1_OFFSET) +#define rCCMR2(_tmr) REG(_tmr, STM32_GTIM_CCMR2_OFFSET) +#define rCCER(_tmr) REG(_tmr, STM32_GTIM_CCER_OFFSET) +#define rCNT(_tmr) REG(_tmr, STM32_GTIM_CNT_OFFSET) +#define rPSC(_tmr) REG(_tmr, STM32_GTIM_PSC_OFFSET) +#define rARR(_tmr) REG(_tmr, STM32_GTIM_ARR_OFFSET) +#define rCCR1(_tmr) REG(_tmr, STM32_GTIM_CCR1_OFFSET) +#define rCCR2(_tmr) REG(_tmr, STM32_GTIM_CCR2_OFFSET) +#define rCCR3(_tmr) REG(_tmr, STM32_GTIM_CCR3_OFFSET) +#define rCCR4(_tmr) REG(_tmr, STM32_GTIM_CCR4_OFFSET) +#define rDCR(_tmr) REG(_tmr, STM32_GTIM_DCR_OFFSET) +#define rDMAR(_tmr) REG(_tmr, STM32_GTIM_DMAR_OFFSET) +#define rBDTR(_tmr) REG(_tmr, STM32_ATIM_BDTR_OFFSET) #define GTIM_SR_CCIF (GTIM_SR_CC4IF|GTIM_SR_CC3IF|GTIM_SR_CC2IF|GTIM_SR_CC1IF) #define GTIM_SR_CCOF (GTIM_SR_CC4OF|GTIM_SR_CC3OF|GTIM_SR_CC2OF|GTIM_SR_CC1OF) -#define CCMR_C1_RESET 0x00ff -#define CCMR_C1_NUM_BITS 8 -#define CCER_C1_NUM_BITS 4 +#define CCMR_C1_RESET 0x00ff +#define CCMR_C1_NUM_BITS 8 +#define CCER_C1_NUM_BITS 4 #define CCMR_C1_CAPTURE_INIT (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR1_CC1S_SHIFT) | \ (GTIM_CCMR_ICPSC_NOPSC << GTIM_CCMR1_IC1PSC_SHIFT) | \ (GTIM_CCMR_ICF_NOFILT << GTIM_CCMR1_IC1F_SHIFT) #define CCMR_C1_PWMOUT_INIT (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC1M_SHIFT) | GTIM_CCMR1_OC1PE -// TODO: PWM mode2? #define CCMR_C1_PWMOUT_INVERTED_INIT (GTIM_CCMR_MODE_PWM2 << GTIM_CCMR1_OC1M_SHIFT) | GTIM_CCMR1_OC1PE #define CCMR_C1_PWMIN_INIT 0 // TBD @@ -143,15 +142,15 @@ static int io_timer_handler7(int irq, void *context, void *arg); #endif /* The transfer is done to 1 register starting from TIMx_CR1 + TIMx_DCR.DBA */ -#define TIM_DMABURSTLENGTH_1TRANSFER 0x00000000U +#define TIM_DMABURSTLENGTH_1TRANSFER 0x00000000U /* The transfer is done to 2 registers starting from TIMx_CR1 + TIMx_DCR.DBA */ -#define TIM_DMABURSTLENGTH_2TRANSFERS 0x00000100U +#define TIM_DMABURSTLENGTH_2TRANSFERS 0x00000100U /* The transfer is done to 3 registers starting from TIMx_CR1 + TIMx_DCR.DBA */ -#define TIM_DMABURSTLENGTH_3TRANSFERS 0x00000200U +#define TIM_DMABURSTLENGTH_3TRANSFERS 0x00000200U /* The transfer is done to 4 registers starting from TIMx_CR1 + TIMx_DCR.DBA */ -#define TIM_DMABURSTLENGTH_4TRANSFERS 0x00000300U +#define TIM_DMABURSTLENGTH_4TRANSFERS 0x00000300U -// NotUsed PWMOut PWMIn Capture OneShot Trigger Dshot LED PPS Other +// NotUsed PWMOut PWMIn Capture OneShot Trigger Dshot LED PPS Other io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT16_MAX, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; typedef uint8_t io_timer_allocation_t; /* big enough to hold MAX_IO_TIMERS */ @@ -163,15 +162,15 @@ io_timer_channel_allocation_t timer_allocations[MAX_IO_TIMERS] = { }; /* Stats and handlers are only useful for Capture */ typedef struct channel_stat_t { - uint32_t isr_cout; - uint32_t overflows; + uint32_t isr_cout; + uint32_t overflows; } channel_stat_t; static channel_stat_t io_timer_channel_stats[MAX_TIMER_IO_CHANNELS]; static struct channel_handler_entry { channel_handler_t callback; - void *context; + void *context; } channel_handlers[MAX_TIMER_IO_CHANNELS]; #endif // !defined(BOARD_HAS_NO_CAPTURE) @@ -216,8 +215,8 @@ static int io_timer_handler(uint16_t timer_index) if (channel_handlers[chan_index].callback) { channel_handlers[chan_index].callback(channel_handlers[chan_index].context, tmr, - chan_index, &timer_io_channels[chan_index], - now, count); + chan_index, &timer_io_channels[chan_index], + now, count); } } @@ -356,8 +355,8 @@ int io_timer_validate_channel_index(unsigned channel) /* test timer for validity */ if ((io_timers[timer].base != 0) && - (timer_io_channels[channel].gpio_out != 0) && - (timer_io_channels[channel].gpio_in != 0)) { + (timer_io_channels[channel].gpio_out != 0) && + (timer_io_channels[channel].gpio_in != 0)) { rv = 0; } } @@ -373,10 +372,10 @@ uint32_t io_timer_channel_get_gpio_output(unsigned channel) #ifdef CONFIG_STM32_STM32F10XX return (timer_io_channels[channel].gpio_out & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | - (GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_CLEAR); + (GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_CLEAR); #else return (timer_io_channels[channel].gpio_out & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | - (GPIO_OUTPUT | GPIO_PUSHPULL | GPIO_SPEED_2MHz | GPIO_OUTPUT_CLEAR); + (GPIO_OUTPUT | GPIO_PUSHPULL | GPIO_SPEED_2MHz | GPIO_OUTPUT_CLEAR); #endif } @@ -532,7 +531,7 @@ void io_timer_capture_dma_req(uint8_t timer, bool enable) } } -int io_timer_set_dshot_mode(uint8_t timer, unsigned dshot_pwm_freq, uint8_t dma_burst_length) +int io_timer_set_dshot_burst_mode(uint8_t timer, unsigned dshot_pwm_freq, uint8_t dma_burst_length) { int ret_val = OK; uint32_t tim_dma_burst_length; @@ -605,16 +604,16 @@ int io_timer_set_dshot_capture_mode(uint8_t timer, unsigned dshot_pwm_freq) rCCER(timer) &= ~(GTIM_CCER_CC1NP | GTIM_CCER_CC2NP | GTIM_CCER_CC3NP | GTIM_CCER_CC4NP); // TODO: channels_mask arg - // Enable CaptComp input on all channels - rCCMR1(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR1_CC1S_SHIFT); - rCCMR1(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR1_CC2S_SHIFT); - rCCMR2(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR2_CC3S_SHIFT); - rCCMR2(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR2_CC4S_SHIFT); + // Enable CaptComp input on all channels + rCCMR1(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR1_CC1S_SHIFT); + rCCMR1(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR1_CC2S_SHIFT); + rCCMR2(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR2_CC3S_SHIFT); + rCCMR2(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR2_CC4S_SHIFT); - // Enable channels - rCCER(timer) |= (GTIM_CCER_CC1E | GTIM_CCER_CC2E | GTIM_CCER_CC3E | GTIM_CCER_CC4E); - rCCER(timer) |= (GTIM_CCER_CC1P | GTIM_CCER_CC2P | GTIM_CCER_CC3P | GTIM_CCER_CC4P); - rCCER(timer) |= (GTIM_CCER_CC1NP | GTIM_CCER_CC2NP | GTIM_CCER_CC3NP | GTIM_CCER_CC4NP); + // Enable channels + rCCER(timer) |= (GTIM_CCER_CC1E | GTIM_CCER_CC2E | GTIM_CCER_CC3E | GTIM_CCER_CC4E); + rCCER(timer) |= (GTIM_CCER_CC1P | GTIM_CCER_CC2P | GTIM_CCER_CC3P | GTIM_CCER_CC4P); + rCCER(timer) |= (GTIM_CCER_CC1NP | GTIM_CCER_CC2NP | GTIM_CCER_CC3NP | GTIM_CCER_CC4NP); // Enable the timer rCR1(timer) |= GTIM_CR1_CEN; @@ -693,9 +692,9 @@ int io_timer_init_timer(unsigned timer, io_timer_channel_mode_t mode) rDCR(timer) = 0; if ((io_timers[timer].base == STM32_TIM1_BASE) - || (io_timers[timer].base == STM32_TIM8_BASE) + || (io_timers[timer].base == STM32_TIM8_BASE) #ifdef STM32_TIM15_BASE - || (io_timers[timer].base == STM32_TIM15_BASE) + || (io_timers[timer].base == STM32_TIM15_BASE) #endif ) { @@ -764,7 +763,7 @@ int io_timer_set_pwm_rate(unsigned timer, unsigned rate) /* Get the channel bits that belong to the timer and are in PWM or OneShot mode */ uint32_t channels = get_timer_channels(timer) & (io_timer_get_mode_channels(IOTimerChanMode_OneShot) | - io_timer_get_mode_channels(IOTimerChanMode_PWMOut)); + io_timer_get_mode_channels(IOTimerChanMode_PWMOut)); /* Request to use OneShot ?*/ @@ -1000,11 +999,11 @@ int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_chann action_cache[timer].dier_setbits |= dier_bit << shifts; if ((state && - (mode == IOTimerChanMode_PWMOut || - mode == IOTimerChanMode_OneShot || - mode == IOTimerChanMode_Dshot || - mode == IOTimerChanMode_DshotInverted || - mode == IOTimerChanMode_Trigger))) { + (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; } } @@ -1043,7 +1042,7 @@ int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_chann /* arm requires the timer be enabled */ rCR1(actions) |= cr1_bit; - } else { + } else { rCR1(actions) = 0; } @@ -1062,10 +1061,10 @@ int io_timer_set_ccr(unsigned channel, uint16_t value) if (rv == 0) { if ((mode != IOTimerChanMode_PWMOut) && - (mode != IOTimerChanMode_OneShot) && - (mode != IOTimerChanMode_Dshot) && - (mode != IOTimerChanMode_DshotInverted) && - (mode != IOTimerChanMode_Trigger)) { + (mode != IOTimerChanMode_OneShot) && + (mode != IOTimerChanMode_Dshot) && + (mode != IOTimerChanMode_DshotInverted) && + (mode != IOTimerChanMode_Trigger)) { rv = -EIO; @@ -1088,8 +1087,8 @@ uint16_t io_channel_get_ccr(unsigned channel) int mode = io_timer_get_channel_mode(channel); if ((mode == IOTimerChanMode_PWMOut) || - (mode == IOTimerChanMode_OneShot) || - (mode == IOTimerChanMode_Trigger)) { + (mode == IOTimerChanMode_OneShot) || + (mode == IOTimerChanMode_Trigger)) { value = REG(channels_timer(channel), timer_io_channels[channel].ccr_offset); } } diff --git a/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/hw_description.h b/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/hw_description.h index 40bc8a8ceed6..0b79bda3420e 100644 --- a/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/hw_description.h +++ b/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/hw_description.h @@ -129,27 +129,27 @@ static inline constexpr void getTimerChannelDMAMap(Timer::Timer timer, const DMA break; case Timer::Timer9: - // Non-existant + // Non-existent break; case Timer::Timer10: - // Non-existant + // Non-existent break; case Timer::Timer11: - // Non-existant + // Non-existent break; case Timer::Timer12: - // Non-existant + // Non-existent break; case Timer::Timer13: - // Non-existant + // Non-existent break; case Timer::Timer14: - // Non-existant + // Non-existent break; case Timer::Timer15: From df73a841249b7e5f31f2596f61cc48c6847bf3c6 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Fri, 25 Oct 2024 00:17:41 -0800 Subject: [PATCH 17/19] make format --- .../src/px4/stm/stm32_common/dshot/dshot.c | 40 +++++++++++------- .../px4/stm/stm32_common/io_pins/io_timer.c | 41 ++++++++++--------- .../stm32h7/include/px4_arch/hw_description.h | 20 ++++++++- 3 files changed, 65 insertions(+), 36 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 53e6722a18fb..a90679958b53 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -61,11 +61,11 @@ // DMA stream configuration registers #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_TEIE | DMA_SCR_DMEIE) + DMA_SCR_DIR_M2P | DMA_SCR_TCIE | DMA_SCR_TEIE | DMA_SCR_DMEIE) // 16-bit because not all of the General Purpose Timers support 32-bit #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) + DMA_SCR_DIR_P2M | DMA_SCR_TCIE | DMA_SCR_TEIE | DMA_SCR_DMEIE) #if defined(CONFIG_ARMV7M_DCACHE) # define DMA_BUFFER_MASK (ARMV7M_DCACHE_LINESIZE - 1) @@ -94,7 +94,8 @@ px4_cache_aligned_data() = {}; static uint32_t *dshot_output_buffer[MAX_IO_TIMERS] = {}; // Array of buffers for timer/channel erpm data -static uint16_t dshot_capture_buffer[MAX_IO_TIMERS][MAX_NUM_CHANNELS_PER_TIMER][CHANNEL_CAPTURE_BUFF_SIZE] px4_cache_aligned_data() = {}; +static uint16_t dshot_capture_buffer[MAX_IO_TIMERS][MAX_NUM_CHANNELS_PER_TIMER][CHANNEL_CAPTURE_BUFF_SIZE] +px4_cache_aligned_data() = {}; static void dma_start_callback(DMA_HANDLE handle, uint8_t status, void *arg); static void cc_callback_timer_index_0(void *arg); @@ -147,6 +148,7 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bi io_timer_channel_mode_t mode = _bidirectional ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot; int ret = io_timer_channel_init(output_channel, mode, NULL, NULL); + if (ret != OK) { PX4_INFO("io_timer_channel_init %u failed", output_channel); return ret; @@ -189,12 +191,15 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bi uint8_t timer_index = timer_io_channels[output_channel].timer_index; uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1; - DMA_HANDLE* dma_handle = &timer_dma_handles[timer_index].dma_ch_handle[timer_channel_index]; + DMA_HANDLE *dma_handle = &timer_dma_handles[timer_index].dma_ch_handle[timer_channel_index]; uint32_t dma_map_ch = io_timers[timer_index].dshot.dma_map_ch[timer_channel_index]; + if (dma_map_ch) { *dma_handle = stm32_dmachannel(dma_map_ch); + if (*dma_handle == NULL) { - PX4_INFO("could not allocate timer %u DMA CH%u handle, output_channel %u", timer_index, timer_channel_index, output_channel); + PX4_INFO("could not allocate timer %u DMA CH%u handle, output_channel %u", timer_index, timer_channel_index, + output_channel); return -ENOSR; } @@ -237,7 +242,7 @@ void up_dshot_trigger() { // Enable DShot inverted on all channels io_timer_set_enable(true, _bidirectional ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot, - IO_TIMER_ALL_MODES_CHANNELS); + IO_TIMER_ALL_MODES_CHANNELS); // For each timer, begin DMA transmit for (uint8_t timer_index = 0; timer_index < MAX_IO_TIMERS; timer_index++) { @@ -276,7 +281,8 @@ void up_dshot_trigger() _timer_index_array[timer_index] = timer_index; if (_bidirectional) { - stm32_dmastart(timer_dma_handles[timer_index].dma_up_handle, dma_start_callback, &_timer_index_array[timer_index], false); + stm32_dmastart(timer_dma_handles[timer_index].dma_up_handle, dma_start_callback, &_timer_index_array[timer_index], + false); } else { stm32_dmastart(timer_dma_handles[timer_index].dma_up_handle, NULL, NULL, false); @@ -294,7 +300,7 @@ void dma_start_callback(DMA_HANDLE handle, uint8_t status, void *arg) // DEBUGGING px4_arch_gpiowrite(GPIO_FMU_CH8, false); - uint8_t timer_index = *((uint8_t*)arg); + uint8_t timer_index = *((uint8_t *)arg); // Clean DMA UP configuration if (timer_dma_handles[timer_index].dma_up_handle != NULL) { @@ -332,7 +338,8 @@ void dma_start_callback(DMA_HANDLE handle, uint8_t status, void *arg) // Allocate DMA if (timer_dma_handles[timer_index].dma_ch_handle[timer_channel_index] == NULL) { - timer_dma_handles[timer_index].dma_ch_handle[timer_channel_index] = stm32_dmachannel(io_timers[timer_index].dshot.dma_map_ch[timer_channel_index]); + timer_dma_handles[timer_index].dma_ch_handle[timer_channel_index] = stm32_dmachannel( + io_timers[timer_index].dshot.dma_map_ch[timer_channel_index]); } // If DMA handler is valid, start DMA @@ -356,7 +363,7 @@ void dma_start_callback(DMA_HANDLE handle, uint8_t status, void *arg) // Flush cache so DMA sees the data memset(dshot_capture_buffer[timer_index], 0, sizeof(dshot_capture_buffer[timer_index])); up_clean_dcache((uintptr_t) dshot_capture_buffer[timer_index], - (uintptr_t) dshot_capture_buffer[timer_index] + DSHOT_CAPTURE_BUFFER_SIZE(MAX_NUM_CHANNELS_PER_TIMER)); + (uintptr_t) dshot_capture_buffer[timer_index] + DSHOT_CAPTURE_BUFFER_SIZE(MAX_NUM_CHANNELS_PER_TIMER)); // TODO: timer channels mask, right now we enable all 4 channels // Enable CaptComp for the channels @@ -422,7 +429,7 @@ static void capture_complete(uint8_t timer_index) // Invalidate the dcache to ensure most recent data is available up_invalidate_dcache((uintptr_t) dshot_capture_buffer[timer_index], - (uintptr_t) dshot_capture_buffer[timer_index] + DSHOT_CAPTURE_BUFFER_SIZE(MAX_NUM_CHANNELS_PER_TIMER)); + (uintptr_t) dshot_capture_buffer[timer_index] + DSHOT_CAPTURE_BUFFER_SIZE(MAX_NUM_CHANNELS_PER_TIMER)); // Process eRPM frames from all channels on this timer process_capture_results(timer_index); @@ -444,19 +451,19 @@ static void capture_complete(uint8_t timer_index) static void cc_callback_timer_index_0(void *arg) { - uint8_t timer_index = *((uint8_t*)arg); + uint8_t timer_index = *((uint8_t *)arg); capture_complete(timer_index); } static void cc_callback_timer_index_1(void *arg) { - uint8_t timer_index = *((uint8_t*)arg); + uint8_t timer_index = *((uint8_t *)arg); capture_complete(timer_index); } static void cc_callback_timer_index_2(void *arg) { - uint8_t timer_index = *((uint8_t*)arg); + uint8_t timer_index = *((uint8_t *)arg); capture_complete(timer_index); } @@ -470,9 +477,11 @@ void process_capture_results(uint8_t timer_index) if (period == 0) { // If the parsing failed, set the eRPM to 0 _erpms[timer_index][channel_index] = 0; + } else if (period == 65408) { // Special case for zero motion (e.g., stationary motor) _erpms[timer_index][channel_index] = 0; + } else { // Convert the period to eRPM _erpms[timer_index][channel_index] = (1000000 * 60) / period; @@ -544,6 +553,7 @@ int up_bdshot_get_erpm(uint8_t output_channel, int *erpm) *erpm = _erpms[timer_index][timer_channel_index]; return PX4_OK; } + // this channel is not configured for dshot return PX4_ERROR; } @@ -561,7 +571,7 @@ int up_bdshot_channel_status(uint8_t channel) void up_bdshot_status(void) { PX4_INFO("dshot driver stats: read %lu, failed nibble %lu, failed CRC %lu, invalid/zero %lu", - read_ok, read_fail_nibble, read_fail_crc, read_fail_zero); + read_ok, read_fail_nibble, read_fail_crc, read_fail_zero); PX4_INFO("delay %lu", delay_hrt); } 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 2b44112fc6ae..29145ec8193c 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 @@ -215,8 +215,8 @@ static int io_timer_handler(uint16_t timer_index) if (channel_handlers[chan_index].callback) { channel_handlers[chan_index].callback(channel_handlers[chan_index].context, tmr, - chan_index, &timer_io_channels[chan_index], - now, count); + chan_index, &timer_io_channels[chan_index], + now, count); } } @@ -355,8 +355,8 @@ int io_timer_validate_channel_index(unsigned channel) /* test timer for validity */ if ((io_timers[timer].base != 0) && - (timer_io_channels[channel].gpio_out != 0) && - (timer_io_channels[channel].gpio_in != 0)) { + (timer_io_channels[channel].gpio_out != 0) && + (timer_io_channels[channel].gpio_in != 0)) { rv = 0; } } @@ -372,10 +372,10 @@ uint32_t io_timer_channel_get_gpio_output(unsigned channel) #ifdef CONFIG_STM32_STM32F10XX return (timer_io_channels[channel].gpio_out & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | - (GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_CLEAR); + (GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_CLEAR); #else return (timer_io_channels[channel].gpio_out & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | - (GPIO_OUTPUT | GPIO_PUSHPULL | GPIO_SPEED_2MHz | GPIO_OUTPUT_CLEAR); + (GPIO_OUTPUT | GPIO_PUSHPULL | GPIO_SPEED_2MHz | GPIO_OUTPUT_CLEAR); #endif } @@ -525,6 +525,7 @@ void io_timer_capture_dma_req(uint8_t timer, bool enable) if (enable) { 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 | ATIM_EGR_CC3G | ATIM_EGR_CC4G); rDIER(timer) &= ~(ATIM_DIER_CC1DE | ATIM_DIER_CC2DE | ATIM_DIER_CC3DE | ATIM_DIER_CC4DE); @@ -692,9 +693,9 @@ int io_timer_init_timer(unsigned timer, io_timer_channel_mode_t mode) rDCR(timer) = 0; if ((io_timers[timer].base == STM32_TIM1_BASE) - || (io_timers[timer].base == STM32_TIM8_BASE) + || (io_timers[timer].base == STM32_TIM8_BASE) #ifdef STM32_TIM15_BASE - || (io_timers[timer].base == STM32_TIM15_BASE) + || (io_timers[timer].base == STM32_TIM15_BASE) #endif ) { @@ -763,7 +764,7 @@ int io_timer_set_pwm_rate(unsigned timer, unsigned rate) /* Get the channel bits that belong to the timer and are in PWM or OneShot mode */ uint32_t channels = get_timer_channels(timer) & (io_timer_get_mode_channels(IOTimerChanMode_OneShot) | - io_timer_get_mode_channels(IOTimerChanMode_PWMOut)); + io_timer_get_mode_channels(IOTimerChanMode_PWMOut)); /* Request to use OneShot ?*/ @@ -999,11 +1000,11 @@ int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_chann action_cache[timer].dier_setbits |= dier_bit << shifts; if ((state && - (mode == IOTimerChanMode_PWMOut || - mode == IOTimerChanMode_OneShot || - mode == IOTimerChanMode_Dshot || - mode == IOTimerChanMode_DshotInverted || - mode == IOTimerChanMode_Trigger))) { + (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; } } @@ -1061,10 +1062,10 @@ int io_timer_set_ccr(unsigned channel, uint16_t value) if (rv == 0) { if ((mode != IOTimerChanMode_PWMOut) && - (mode != IOTimerChanMode_OneShot) && - (mode != IOTimerChanMode_Dshot) && - (mode != IOTimerChanMode_DshotInverted) && - (mode != IOTimerChanMode_Trigger)) { + (mode != IOTimerChanMode_OneShot) && + (mode != IOTimerChanMode_Dshot) && + (mode != IOTimerChanMode_DshotInverted) && + (mode != IOTimerChanMode_Trigger)) { rv = -EIO; @@ -1087,8 +1088,8 @@ uint16_t io_channel_get_ccr(unsigned channel) int mode = io_timer_get_channel_mode(channel); if ((mode == IOTimerChanMode_PWMOut) || - (mode == IOTimerChanMode_OneShot) || - (mode == IOTimerChanMode_Trigger)) { + (mode == IOTimerChanMode_OneShot) || + (mode == IOTimerChanMode_Trigger)) { value = REG(channels_timer(channel), timer_io_channels[channel].ccr_offset); } } diff --git a/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/hw_description.h b/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/hw_description.h index 0b79bda3420e..ee02fbaeecc4 100644 --- a/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/hw_description.h +++ b/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/hw_description.h @@ -35,7 +35,7 @@ #include "../../../stm32_common/include/px4_arch/hw_description.h" -static inline constexpr void getTimerChannelDMAMap(Timer::Timer timer, const DMA &dma, uint32_t* dma_map_ch) +static inline constexpr void getTimerChannelDMAMap(Timer::Timer timer, const DMA &dma, uint32_t *dma_map_ch) { switch (timer) { case Timer::Timer1: @@ -44,12 +44,14 @@ static inline constexpr void getTimerChannelDMAMap(Timer::Timer timer, const DMA dma_map_ch[1] = DMAMAP_DMA12_TIM1CH2_0; dma_map_ch[2] = DMAMAP_DMA12_TIM1CH3_0; dma_map_ch[3] = DMAMAP_DMA12_TIM1CH4_0; + } else { dma_map_ch[0] = DMAMAP_DMA12_TIM1CH1_1; dma_map_ch[1] = DMAMAP_DMA12_TIM1CH2_1; dma_map_ch[2] = DMAMAP_DMA12_TIM1CH3_1; dma_map_ch[3] = DMAMAP_DMA12_TIM1CH4_1; } + break; case Timer::Timer2: @@ -58,12 +60,14 @@ static inline constexpr void getTimerChannelDMAMap(Timer::Timer timer, const DMA dma_map_ch[1] = DMAMAP_DMA12_TIM2CH2_0; dma_map_ch[2] = DMAMAP_DMA12_TIM2CH3_0; dma_map_ch[3] = DMAMAP_DMA12_TIM2CH4_0; + } else { dma_map_ch[0] = DMAMAP_DMA12_TIM2CH1_1; dma_map_ch[1] = DMAMAP_DMA12_TIM2CH2_1; dma_map_ch[2] = DMAMAP_DMA12_TIM2CH3_1; dma_map_ch[3] = DMAMAP_DMA12_TIM2CH4_1; } + break; case Timer::Timer3: @@ -72,12 +76,14 @@ static inline constexpr void getTimerChannelDMAMap(Timer::Timer timer, const DMA dma_map_ch[1] = DMAMAP_DMA12_TIM3CH2_0; dma_map_ch[2] = DMAMAP_DMA12_TIM3CH3_0; dma_map_ch[3] = DMAMAP_DMA12_TIM3CH4_0; + } else { dma_map_ch[0] = DMAMAP_DMA12_TIM3CH1_1; dma_map_ch[1] = DMAMAP_DMA12_TIM3CH2_1; dma_map_ch[2] = DMAMAP_DMA12_TIM3CH3_1; dma_map_ch[3] = DMAMAP_DMA12_TIM3CH4_1; } + break; case Timer::Timer4: @@ -85,11 +91,13 @@ static inline constexpr void getTimerChannelDMAMap(Timer::Timer timer, const DMA dma_map_ch[0] = DMAMAP_DMA12_TIM4CH1_0; dma_map_ch[1] = DMAMAP_DMA12_TIM4CH2_0; dma_map_ch[2] = DMAMAP_DMA12_TIM4CH3_0; + } else { dma_map_ch[0] = DMAMAP_DMA12_TIM4CH1_1; dma_map_ch[1] = DMAMAP_DMA12_TIM4CH2_1; dma_map_ch[2] = DMAMAP_DMA12_TIM4CH3_1; } + break; case Timer::Timer5: @@ -98,12 +106,14 @@ static inline constexpr void getTimerChannelDMAMap(Timer::Timer timer, const DMA dma_map_ch[1] = DMAMAP_DMA12_TIM5CH2_0; dma_map_ch[2] = DMAMAP_DMA12_TIM5CH3_0; dma_map_ch[3] = DMAMAP_DMA12_TIM5CH4_0; + } else { dma_map_ch[0] = DMAMAP_DMA12_TIM5CH1_1; dma_map_ch[1] = DMAMAP_DMA12_TIM5CH2_1; dma_map_ch[2] = DMAMAP_DMA12_TIM5CH3_1; dma_map_ch[3] = DMAMAP_DMA12_TIM5CH4_1; } + break; case Timer::Timer6: @@ -120,12 +130,14 @@ static inline constexpr void getTimerChannelDMAMap(Timer::Timer timer, const DMA dma_map_ch[1] = DMAMAP_DMA12_TIM8CH2_0; dma_map_ch[2] = DMAMAP_DMA12_TIM8CH3_0; dma_map_ch[3] = DMAMAP_DMA12_TIM8CH4_0; + } else { dma_map_ch[0] = DMAMAP_DMA12_TIM8CH1_1; dma_map_ch[1] = DMAMAP_DMA12_TIM8CH2_1; dma_map_ch[2] = DMAMAP_DMA12_TIM8CH3_1; dma_map_ch[3] = DMAMAP_DMA12_TIM8CH4_1; } + break; case Timer::Timer9: @@ -155,25 +167,31 @@ static inline constexpr void getTimerChannelDMAMap(Timer::Timer timer, const DMA case Timer::Timer15: if (dma.index == DMA::Index1) { dma_map_ch[0] = DMAMAP_DMA12_TIM15CH1_0; + } else { dma_map_ch[0] = DMAMAP_DMA12_TIM15CH1_1; } + break; case Timer::Timer16: if (dma.index == DMA::Index1) { dma_map_ch[0] = DMAMAP_DMA12_TIM16CH1_0; + } else { dma_map_ch[0] = DMAMAP_DMA12_TIM16CH1_1; } + break; case Timer::Timer17: if (dma.index == DMA::Index1) { dma_map_ch[0] = DMAMAP_DMA12_TIM17CH1_0; + } else { dma_map_ch[0] = DMAMAP_DMA12_TIM17CH1_1; } + break; } } From a158b758f7c05bdeffac3c40b00408c4025e58a3 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Fri, 25 Oct 2024 00:34:38 -0800 Subject: [PATCH 18/19] individual timer channel status reporting --- .../src/px4/stm/stm32_common/dshot/dshot.c | 38 +++++++++++-------- 1 file changed, 23 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 a90679958b53..8bf2ded7afe0 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -113,7 +113,6 @@ static int _channels_init_mask = 0; static uint8_t _timer_index_array[MAX_IO_TIMERS] = {}; static int32_t _erpms[MAX_IO_TIMERS][MAX_NUM_CHANNELS_PER_TIMER] = {}; -// We need local permanent memory for indices, channels, and callbacks for each output static struct hrt_call _dma_capture_callback_call[MAX_TIMER_IO_CHANNELS]; // Separate HRT callbacks for each timer @@ -125,11 +124,10 @@ static hrt_callback_t _cc_callbacks[3] = { }; // decoding status -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 uint32_t delay_hrt = 0; +static uint32_t read_ok[MAX_IO_TIMERS][MAX_NUM_CHANNELS_PER_TIMER] = {}; +static uint32_t read_fail_nibble[MAX_IO_TIMERS][MAX_NUM_CHANNELS_PER_TIMER] = {}; +static uint32_t read_fail_crc[MAX_IO_TIMERS][MAX_NUM_CHANNELS_PER_TIMER] = {}; +static uint32_t read_fail_zero[MAX_IO_TIMERS][MAX_NUM_CHANNELS_PER_TIMER] = {}; int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bidirectional_dshot) { @@ -392,7 +390,6 @@ void dma_start_callback(DMA_HANDLE handle, uint8_t status, void *arg) // 30us to switch regardless of DShot frequency + eRPM frame time + 10us for good measure hrt_abstime frame_us = (16 * 1000000) / _dshot_frequency; // 16 bits * us_per_s / bits_per_s hrt_abstime delay = 30 + frame_us + 10; - delay_hrt = delay; hrt_call_after(&_dma_capture_callback_call[timer_index], delay, _cc_callbacks[timer_index], arg); // DEBUGGING @@ -570,10 +567,21 @@ int up_bdshot_channel_status(uint8_t channel) void up_bdshot_status(void) { - PX4_INFO("dshot driver stats: read %lu, failed nibble %lu, failed CRC %lu, invalid/zero %lu", - read_ok, read_fail_nibble, read_fail_crc, read_fail_zero); - - PX4_INFO("delay %lu", delay_hrt); + PX4_INFO("dshot driver stats:"); + + for (uint8_t timer_index = 0; timer_index < MAX_IO_TIMERS; timer_index++) { + for (uint8_t channel_index = 0; channel_index < MAX_NUM_CHANNELS_PER_TIMER; channel_index++) { + + if (_channels_init_mask & (1 << (timer_index * MAX_NUM_CHANNELS_PER_TIMER + channel_index))) { + PX4_INFO("Timer %u, Channel %u: read %lu, failed nibble %lu, failed CRC %lu, invalid/zero %lu", + timer_index, channel_index, + read_ok[timer_index][channel_index], + read_fail_nibble[timer_index][channel_index], + read_fail_crc[timer_index][channel_index], + read_fail_zero[timer_index][channel_index]); + } + } + } } uint8_t nibbles_from_mapped(uint8_t mapped) @@ -669,7 +677,7 @@ unsigned calculate_period(uint8_t timer_index, uint8_t channel_index) if (shifted == 0) { // If no data was shifted, the read failed - ++read_fail_zero; + ++read_fail_zero[timer_index][channel_index]; return 0; } @@ -686,7 +694,7 @@ unsigned calculate_period(uint8_t timer_index, uint8_t channel_index) uint32_t nibble = nibbles_from_mapped(gcr & 0x1F) << (4 * i); if (nibble == 0xFF) { - ++read_fail_nibble; + ++read_fail_nibble[timer_index][channel_index];; return 0; } @@ -702,11 +710,11 @@ unsigned calculate_period(uint8_t timer_index, uint8_t channel_index) unsigned calculated_crc = (~(payload ^ (payload >> 4) ^ (payload >> 8))) & 0x0F; if (crc != calculated_crc) { - ++read_fail_crc; + ++read_fail_crc[timer_index][channel_index];; return 0; } - ++read_ok; + ++read_ok[timer_index][channel_index];; return period; } From 43dc0867874772e14314aabfff34e12ec5b885cd Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Fri, 25 Oct 2024 14:42:04 -0800 Subject: [PATCH 19/19] remove debug definitions --- boards/ark/fmu-v6x/src/board_config.h | 10 +++++----- boards/ark/fmu-v6x/src/timer_config.cpp | 4 ++-- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/boards/ark/fmu-v6x/src/board_config.h b/boards/ark/fmu-v6x/src/board_config.h index 10e20f81def4..052693d01437 100644 --- a/boards/ark/fmu-v6x/src/board_config.h +++ b/boards/ark/fmu-v6x/src/board_config.h @@ -238,7 +238,7 @@ /* PWM */ -#define DIRECT_PWM_OUTPUT_CHANNELS 6 +#define DIRECT_PWM_OUTPUT_CHANNELS 8 #define GPIO_FMU_CH1 /* PI0 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTI|GPIO_PIN0) #define GPIO_FMU_CH2 /* PH12 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTH|GPIO_PIN12) @@ -246,12 +246,12 @@ #define GPIO_FMU_CH4 /* PH10 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTH|GPIO_PIN10) #define GPIO_FMU_CH5 /* PD13 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTD|GPIO_PIN13) #define GPIO_FMU_CH6 /* PD14 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTD|GPIO_PIN14) -// #define GPIO_FMU_CH7 /* PH6 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTH|GPIO_PIN6) -// #define GPIO_FMU_CH8 /* PH9 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTH|GPIO_PIN9) +#define GPIO_FMU_CH7 /* PH6 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTH|GPIO_PIN6) +#define GPIO_FMU_CH8 /* PH9 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTH|GPIO_PIN9) // JAKE: debugging logic analyzer -#define GPIO_FMU_CH7 /* PH6 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTH|GPIO_PIN6) -#define GPIO_FMU_CH8 /* PH9 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTH|GPIO_PIN9) +// #define GPIO_FMU_CH7 /* PH6 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTH|GPIO_PIN6) +// #define GPIO_FMU_CH8 /* PH9 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTH|GPIO_PIN9) #define GPIO_FMU_CAP /* PE11 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTE|GPIO_PIN11) #define GPIO_SPIX_SYNC /* PE9 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTE|GPIO_PIN9) diff --git a/boards/ark/fmu-v6x/src/timer_config.cpp b/boards/ark/fmu-v6x/src/timer_config.cpp index 6cb2415dc8a6..3504c3569dc0 100644 --- a/boards/ark/fmu-v6x/src/timer_config.cpp +++ b/boards/ark/fmu-v6x/src/timer_config.cpp @@ -71,8 +71,8 @@ constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel1}, {GPIO::PortH, GPIO::Pin10}), initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel2}, {GPIO::PortD, GPIO::Pin13}), initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel3}, {GPIO::PortD, GPIO::Pin14}), - // initIOTimerChannel(io_timers, {Timer::Timer12, Timer::Channel1}, {GPIO::PortH, GPIO::Pin6}), - // initIOTimerChannel(io_timers, {Timer::Timer12, Timer::Channel2}, {GPIO::PortH, GPIO::Pin9}), + initIOTimerChannel(io_timers, {Timer::Timer12, Timer::Channel1}, {GPIO::PortH, GPIO::Pin6}), + initIOTimerChannel(io_timers, {Timer::Timer12, Timer::Channel2}, {GPIO::PortH, GPIO::Pin9}), //initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel2}, {GPIO::PortE, GPIO::Pin11}), //initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel1}, {GPIO::PortE, GPIO::Pin9}), };