From 4ee3d2778e0d68764e73dd151722664cdc44da61 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Thu, 8 Jan 2026 20:51:04 -0900 Subject: [PATCH] sequential dshot burst/capture timer triggering, it works! --- .../src/px4/stm/stm32_common/dshot/dshot.c | 204 +++++++++++------- src/drivers/dshot/DShot.cpp | 45 +--- 2 files changed, 131 insertions(+), 118 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 3b03b9aad1..069352c888 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -99,6 +99,10 @@ static void process_capture_results(uint8_t timer_index, uint8_t channel_index); static uint32_t convert_edge_intervals_to_bitstream(uint8_t timer_index, uint8_t channel_index); static void decode_dshot_telemetry(uint32_t payload, struct BDShotTelemetry *packet); +static int8_t dshot_get_next_timer(int8_t current); +static void dshot_start_timer_burst(uint8_t timer_index); +static void dshot_advance_to_next_timer(void); + // Timer configuration struct typedef struct timer_config_t { DMA_HANDLE dma_handle; // DMA stream for DMA update and eRPM Capture Compare @@ -140,6 +144,9 @@ static bool _edt_enabled = false; // Extended DShot Telemetry static bool _bdshot_cycle_complete[MAX_IO_TIMERS] = { [0 ...(MAX_IO_TIMERS - 1)] = true }; +// Sequential timer processing: -1 = sequence complete/idle, otherwise timer index being processed +static int8_t _current_timer_index = -1; + // Online flags, set if ESC is reponding with valid BDShot frames #define BDSHOT_OFFLINE_COUNT 200 static bool _bdshot_online[MAX_TIMER_IO_CHANNELS] = {}; @@ -359,89 +366,125 @@ int up_dshot_init(uint32_t channel_mask, uint32_t bdshot_channel_mask, unsigned return channels_init_mask; } -// Kicks off a DMA transmit for each configured timer and the associated channels -void up_dshot_trigger() +// Find next enabled+initialized timer after current index, or -1 if none +static int8_t dshot_get_next_timer(int8_t current) { - if (_bdshot_channel_mask) { - bool all_complete = true; + for (int8_t i = current + 1; i < MAX_IO_TIMERS; i++) { + if (timer_configs[i].enabled && timer_configs[i].initialized) { + return i; + } + } - for (int i = 0; i < MAX_IO_TIMERS; i++) { - if (!_bdshot_cycle_complete[i]) { - all_complete = false; - } + return -1; +} + +// ESC boot delay - wait for ESCs to initialize before enabling BDShot capture +static const uint64_t ESC_BOOT_DELAY_US = 3000000; + +// Start burst DMA for a single timer +static void dshot_start_timer_burst(uint8_t timer_index) +{ + timer_config_t *timer = &timer_configs[timer_index]; + io_timer_channel_mode_t mode = timer->bidirectional ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot; + + io_timer_set_enable(true, mode, io_timer_get_group(timer_index)); + + uint32_t channel_count = io_timers_channel_mapping.element[timer_index].channel_count_including_gaps; + io_timer_set_dshot_burst_mode(timer_index, _dshot_frequency, channel_count); + + if (timer->bidirectional) { + // Deallocate DMA from previous transaction + if (timer->dma_handle != NULL) { + stm32_dmastop(timer->dma_handle); + stm32_dmafree(timer->dma_handle); + timer->dma_handle = NULL; } - if (!all_complete) { - PX4_WARN("BDShot cycle not complete! Check system jitter"); + // Allocate DMA + timer->dma_handle = stm32_dmachannel(io_timers[timer_index].dshot.dma_map_up); + + if (timer->dma_handle == NULL) { + PX4_WARN("DMA allocation for timer %u failed", timer_index); + dshot_advance_to_next_timer(); return; } } - // For each timer, begin DMA transmit - for (uint8_t timer_index = 0; timer_index < MAX_IO_TIMERS; timer_index++) { + // Flush cache so DMA sees the data + up_clean_dcache((uintptr_t)dshot_output_buffer[timer_index], + (uintptr_t)dshot_output_buffer[timer_index] + + DSHOT_OUTPUT_BUFFER_SIZE(channel_count)); - timer_config_t *timer = &timer_configs[timer_index]; + px4_stm32_dmasetup(timer->dma_handle, + io_timers[timer_index].base + STM32_GTIM_DMAR_OFFSET, + (uint32_t)(dshot_output_buffer[timer_index]), + channel_count * CHANNEL_OUTPUT_BUFF_SIZE, + DSHOT_DMA_SCR); - io_timer_channel_mode_t mode = timer->bidirectional ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot; + // Clean UDE flag before DMA is started + io_timer_update_dma_req(timer_index, false); - io_timer_set_enable(true, mode, io_timer_get_group(timer_index)); + // Trigger DMA. For BDShot after boot delay, use callback for capture setup. + // For standard DShot or during boot, use callback to advance to next timer. + if (timer->bidirectional && (hrt_absolute_time() > ESC_BOOT_DELAY_US)) { + perf_begin(capture_cycle_perf); + stm32_dmastart(timer->dma_handle, dma_burst_finished_callback, &timer->timer_index, false); - if (timer->enabled && timer->initialized) { + } else { + // Standard DShot or BDShot during boot - no capture, just advance to next timer + stm32_dmastart(timer->dma_handle, dma_burst_finished_callback, &timer->timer_index, false); - uint32_t channel_count = io_timers_channel_mapping.element[timer_index].channel_count_including_gaps; - - io_timer_set_dshot_burst_mode(timer_index, _dshot_frequency, channel_count); - - if (timer->bidirectional) { - // Deallocate DMA from previous transaction - if (timer->dma_handle != NULL) { - stm32_dmastop(timer->dma_handle); - stm32_dmafree(timer->dma_handle); - timer->dma_handle = NULL; - } - - // Allocate DMA - timer->dma_handle = stm32_dmachannel(io_timers[timer_index].dshot.dma_map_up); - - if (timer->dma_handle == NULL) { - PX4_WARN("DMA allocation for timer %u failed", timer_index); - continue; - } - } - - // Flush cache so DMA sees the data - 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(timer->dma_handle, - io_timers[timer_index].base + STM32_GTIM_DMAR_OFFSET, - (uint32_t)(dshot_output_buffer[timer_index]), - channel_count * CHANNEL_OUTPUT_BUFF_SIZE, - DSHOT_DMA_SCR); - - // Clean UDE flag before DMA is started - io_timer_update_dma_req(timer_index, false); - - // Trigger DMA (DShot Outputs). Only capture compare after the system has had time to boot. - static const uint64_t ESC_BOOT_DELAY_US = 3000000; - - if (timer->bidirectional && (hrt_absolute_time() > ESC_BOOT_DELAY_US)) { - perf_begin(capture_cycle_perf); - stm32_dmastart(timer->dma_handle, dma_burst_finished_callback, &timer->timer_index, false); - - } else { - stm32_dmastart(timer->dma_handle, NULL, NULL, false); - - if (timer->bidirectional) { - _bdshot_cycle_complete[timer_index] = true; - } - } - - // Enable DMA update request - io_timer_update_dma_req(timer_index, true); + if (timer->bidirectional) { + // During boot, mark BDShot cycle complete immediately + _bdshot_cycle_complete[timer_index] = true; } } + + // Enable DMA update request + io_timer_update_dma_req(timer_index, true); +} + +// Advance to next timer in sequence, or complete the sequence +static void dshot_advance_to_next_timer(void) +{ + int8_t next = dshot_get_next_timer(_current_timer_index); + + if (next < 0) { + // All timers done + _current_timer_index = -1; + return; + } + + _current_timer_index = next; + dshot_start_timer_burst(next); +} + +// Kicks off DMA transmit sequence for all configured timers (sequentially) +void up_dshot_trigger() +{ + // Check if previous sequence is complete + if (_current_timer_index >= 0) { + PX4_WARN("DShot cycle not complete! Check system jitter"); + return; + } + + // Find first enabled timer + int8_t first = dshot_get_next_timer(-1); + + if (first < 0) { + return; // No timers configured + } + + // Mark all BDShot timers as cycle incomplete + for (uint8_t i = 0; i < MAX_IO_TIMERS; i++) { + if (timer_configs[i].enabled && timer_configs[i].initialized && timer_configs[i].bidirectional) { + _bdshot_cycle_complete[i] = false; + } + } + + // Start sequence with first timer + _current_timer_index = first; + dshot_start_timer_burst(first); } static void select_next_capture_channel(uint8_t timer_index) @@ -484,11 +527,19 @@ void dma_burst_finished_callback(DMA_HANDLE handle, uint8_t status, void *arg) { uint8_t timer_index = *((uint8_t *)arg); + timer_config_t *timer = &timer_configs[timer_index]; + + // For standard DShot or BDShot during boot: no capture needed, advance to next timer + if (!timer->bidirectional || (hrt_absolute_time() <= ESC_BOOT_DELAY_US)) { + dshot_advance_to_next_timer(); + return; + } + // Clean DMA UP configuration - if (timer_configs[timer_index].dma_handle != NULL) { - stm32_dmastop(timer_configs[timer_index].dma_handle); - stm32_dmafree(timer_configs[timer_index].dma_handle); - timer_configs[timer_index].dma_handle = NULL; + if (timer->dma_handle != NULL) { + stm32_dmastop(timer->dma_handle); + stm32_dmafree(timer->dma_handle); + timer->dma_handle = NULL; } // Disable DMA update request @@ -524,9 +575,11 @@ void dma_burst_finished_callback(DMA_HANDLE handle, uint8_t status, void *arg) uint8_t capture_channel = timer_configs[timer_index].capture_channel; timer_configs[timer_index].dma_handle = stm32_dmachannel(io_timers[timer_index].dshot.dma_map_ch[capture_channel]); - // If DMA handler is valid, start DMA - if (timer_configs[timer_index].dma_handle == NULL) { + // If DMA handler is invalid, skip capture and advance to next timer + if (timer->dma_handle == NULL) { PX4_WARN("failed to allocate dma for timer %u channel %u", timer_index, capture_channel); + _bdshot_cycle_complete[timer_index] = true; + dshot_advance_to_next_timer(); return; } @@ -606,6 +659,9 @@ static void capture_complete_callback(void *arg) perf_end(hrt_callback_perf); _bdshot_cycle_complete[timer_index] = true; + + // Chain to next timer in sequence + dshot_advance_to_next_timer(); } void process_capture_results(uint8_t timer_index, uint8_t channel_index) diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index 12c53e1b85..9fbb8fa855 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -945,12 +945,9 @@ void DShot::mixerChanged() bool DShot::initialize_dshot() { - PX4_INFO("initialize_dshot: motor_mask=0x%lx", _output_mask); - uint32_t dshot_timer_channels = 0; // Channels on DShot-enabled timers // Iterate through timers to determine DShot frequency and BDShot channels - // _output_mask already contains only motor channels (set by mixerChanged) for (uint8_t timer_index = 0; timer_index < MAX_IO_TIMERS; timer_index++) { // Get mask of actuator channels associated with this timer group @@ -981,57 +978,17 @@ bool DShot::initialize_dshot() dshot_timer_channels |= timer_channels; } - // Bidirectional DShot (tim_config < -5 means -6, -7, or -8) + // Bidirectional DShot if (tim_config < -5) { - // Add timer channels that are also motors to BDShot mask - // _bdshot_output_mask |= (timer_channels & _output_mask); - // _bdshot_output_mask |= timer_channels; _bdshot_timer_channels |= timer_channels; } - - PX4_INFO("timer_index %u, channels_mask %lu", timer_index, dshot_timer_channels); } - PX4_INFO("_bdshot_output_mask %lu", _bdshot_output_mask); - if (dshot_timer_channels == 0) { PX4_WARN("No channels configured"); return false; } - - // _output_mask &= dshot_timer_channels; - - // PX4_INFO("dshot_timer_channels=0x%lx, _output_mask=0x%lx, _bdshot_output_mask=0x%lx", - // dshot_timer_channels, _output_mask, _bdshot_output_mask); - - - // if (ret < 0) { - // PX4_ERR("up_dshot_init failed (%i)", ret); - // return false; - // } - - // if ((uint32_t)ret != _output_mask) { - // PX4_INFO("Failed to configure some channels"); - // PX4_INFO("requested: 0x%lx", _output_mask); - // PX4_INFO("configured: 0x%lx", (uint32_t)ret); - // _output_mask = ret; - // } - - // Set our mixer to explicitly disable channels we do not control - // for (uint8_t i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) { - // if (((1 << i) & _output_mask) == 0) { - // _mixing_output.disableFunction(i); - // } - // } - - // if (_output_mask == 0) { - // PX4_WARN("No channels configured"); - // return false; - // } - - // up_dshot_arm(true); - return true; }