From 6d1c1838c0823efcda36c692308f9ded84ddd18c Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Tue, 29 Oct 2024 14:52:32 -0800 Subject: [PATCH 01/14] Bidirectional DShot Co-authored-by: Julian Oes --- .../rpi_common/include/px4_arch/io_timer.h | 3 - .../px4/stm/stm32_common/dshot/CMakeLists.txt | 1 + .../src/px4/stm/stm32_common/dshot/dshot.c | 780 +++++++++++++++--- .../stm/stm32_common/include/px4_arch/dshot.h | 5 +- .../stm32_common/include/px4_arch/io_timer.h | 10 +- .../px4_arch/io_timer_hw_description.h | 11 +- .../stm/stm32_common/io_pins/input_capture.c | 6 +- .../px4/stm/stm32_common/io_pins/io_timer.c | 177 +++- .../stm32f7/include/px4_arch/hw_description.h | 41 + .../stm32h7/include/px4_arch/hw_description.h | 188 ++++- .../px4_arch/io_timer_hw_description.h | 11 +- src/drivers/drv_dshot.h | 4 +- 12 files changed, 1055 insertions(+), 182 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..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 @@ -147,9 +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); /** * Returns the pin configuration for a specific channel, to be used as GPIO output. 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..2c315a0ca409 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,5 @@ px4_add_library(arch_dshot dshot.c ) +# target_compile_options(arch_dshot PRIVATE ${MAX_CUSTOM_OPT_LEVEL} -DDEBUG_BUILD) target_compile_options(arch_dshot PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) 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..ad2ba17e620d 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2019-2021 PX4 Development Team. All rights reserved. + * Copyright (C) 2024 PX4 Development Team. All rights reserved. * Author: Igor Misic * * Redistribution and use in source and binary forms, with or without @@ -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 @@ -32,7 +32,6 @@ * ****************************************************************************/ - #if (CONFIG_STM32_HAVE_IP_DMA_V1) //Do nothing. IP DMA V1 MCUs are not supported. #else @@ -45,29 +44,27 @@ #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 ONE_MOTOR_BUFF_SIZE 17u -#define ALL_MOTORS_BUF_SIZE (MOTORS_NUMBER * ONE_MOTOR_BUFF_SIZE) -#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 - +#include +#include +#include + +// DShot protocol definitions +#define ONE_MOTOR_DATA_SIZE 16u +#define MOTOR_PWM_BIT_1 14u +#define MOTOR_PWM_BIT_0 7u +#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_HTIE | DMA_SCR_TEIE | DMA_SCR_DMEIE) + DMA_SCR_DIR_M2P | 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; +// 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) #if defined(CONFIG_ARMV7M_DCACHE) # define DMA_BUFFER_MASK (ARMV7M_DCACHE_LINESIZE - 1) @@ -75,133 +72,502 @@ 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)) -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)] +#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 void init_timer_config(uint32_t channel_mask); +static void init_timers_dma_up(void); +static void init_timers_dma_capt_comp(uint8_t timer_index); +static int32_t init_timer_channels(uint8_t timer_index); + +static void dma_burst_finished_callback(DMA_HANDLE handle, uint8_t status, void *arg); +static void capture_complete_callback(void *arg); + +static void process_capture_results(uint8_t timer_index); +static unsigned calculate_period(uint8_t timer_index, uint8_t channel_index); + +// Timer configuration struct +typedef struct timer_config_t { + DMA_HANDLE dma_up_handle; // DMA stream for DMA update + DMA_HANDLE dma_ch_handle[4]; // DMA streams for bidi CaptComp + bool enabled; // Timer enabled + bool enabled_channels[4]; // Timer Channels enabled (requested) + bool initialized; // Timer initialized + bool initialized_channels[4]; // Timer channels initialized (successfully started) + bool bidirectional; // Timer in bidi (inverted) mode + bool captcomp_channels[4]; // Channels configured for CaptComp + uint8_t timer_index; // Timer index. Necessary to have memory for passing pointer to hrt callback +} timer_config_t; + +static timer_config_t timer_configs[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_burst_buffer[DSHOT_TIMERS] = {}; +static uint32_t *dshot_output_buffer[MAX_IO_TIMERS] = {}; -int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bidirectional_dshot) +// Buffer containing channel capture data for a single timer +static uint16_t dshot_capture_buffer[MAX_NUM_CHANNELS_PER_TIMER][CHANNEL_CAPTURE_BUFF_SIZE] +px4_cache_aligned_data() = {}; + +static bool _bidirectional = false; +static uint8_t _bidi_timer_index = 0; // TODO: BDSHOT_TIM param to select timer index? +static uint32_t _dshot_frequency = 0; + +// eRPM data for channels on the singular timer +static int32_t _erpms[MAX_NUM_CHANNELS_PER_TIMER] = {}; + +// hrt callback handle for captcomp post dma processing +static struct hrt_call _cc_call; + +// decoding status for each channel +static uint32_t read_ok[MAX_NUM_CHANNELS_PER_TIMER] = {}; +static uint32_t read_fail_nibble[MAX_NUM_CHANNELS_PER_TIMER] = {}; +static uint32_t read_fail_crc[MAX_NUM_CHANNELS_PER_TIMER] = {}; +static uint32_t read_fail_zero[MAX_NUM_CHANNELS_PER_TIMER] = {}; + +static void init_timer_config(uint32_t channel_mask) { - unsigned buffer_offset = 0; + // Mark timers in use, channels in use, and timers for bidi dshot + 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; + uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1; + + if (io_timers[timer_index].dshot.dma_base == 0) { + // board does not configure dshot on this timer + continue; + } - for (int timer_index = 0; timer_index < DSHOT_TIMERS; timer_index++) { - dshot_handler[timer_index].init = false; + // NOTE: only 1 timer can be used if Bidirectional DShot is enabled + if (_bidirectional && (timer_index != _bidi_timer_index)) { + continue; + } + + // NOTE: this is necessary to pass timer_index to DMA callback + timer_configs[timer_index].timer_index = timer_index; + // Mark timer as enabled + timer_configs[timer_index].enabled = true; + // Mark channel as enabled + timer_configs[timer_index].enabled_channels[timer_channel_index] = true; + + // Mark timer as bidirectional + if (_bidirectional && timer_index == _bidi_timer_index) { + timer_configs[timer_index].bidirectional = true; + } + } } +} - for (unsigned timer = 0; timer < DSHOT_TIMERS; ++timer) { - if (io_timers[timer].base == 0) { // no more timers configured - break; +// Initializes dshot on configured timers if DShot mode is enabled and DMA is available. +static void init_timers_dma_up(void) +{ + for (uint8_t timer_index = 0; timer_index < MAX_IO_TIMERS; timer_index++) { + + if (!timer_configs[timer_index].enabled) { + // timer is not enabled for dshot + continue; } - // we know the uint8_t* cast to uint32_t* is fine, since we're aligned to cache line size -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wcast-align" - dshot_burst_buffer[timer] = (uint32_t *)&dshot_burst_buffer_array[buffer_offset]; -#pragma GCC diagnostic pop - buffer_offset += DSHOT_BURST_BUFFER_SIZE(io_timers_channel_mapping.element[timer].channel_count_including_gaps); + if (io_timers[timer_index].dshot.dma_map_up == 0) { + // timer does not have DMA UP mapped + continue; + } + + // NOTE: only 1 timer can be used if Bidirectional DShot is enabled + if (_bidirectional && (timer_index != _bidi_timer_index)) { + continue; + } + + // Attempt to allocate DMA for Burst + timer_configs[timer_index].dma_up_handle = stm32_dmachannel(io_timers[timer_index].dshot.dma_map_up); + + if (timer_configs[timer_index].dma_up_handle == NULL) { + PX4_DEBUG("Failed to allocate Timer %u DMA UP", timer_index); + continue; + } + + PX4_DEBUG("Allocated DMA UP Timer Index %u", timer_index); + timer_configs[timer_index].initialized = true; + } + + // Free the allocated DMA channels + for (uint8_t timer_index = 0; timer_index < MAX_IO_TIMERS; timer_index++) { + if (timer_configs[timer_index].dma_up_handle != NULL) { + stm32_dmafree(timer_configs[timer_index].dma_up_handle); + timer_configs[timer_index].dma_up_handle = NULL; + PX4_DEBUG("Freed DMA UP Timer Index %u", timer_index); + } + } +} - if (buffer_offset > sizeof(dshot_burst_buffer_array)) { - return -EINVAL; // something is wrong with the board configuration or some other logic +static void init_timers_dma_capt_comp(uint8_t timer_index) +{ + if (timer_configs[timer_index].enabled && timer_configs[timer_index].initialized) { + + // Allocate DMA for each enabled channel + for (uint8_t output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) { + // For each enabled channel on this timer, try allocating DMA + uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1; + bool this_timer = timer_index == timer_io_channels[output_channel].timer_index; + bool channel_enabled = timer_configs[timer_index].enabled_channels[timer_channel_index]; + + if (this_timer && channel_enabled) { + uint32_t dma_map_ch = io_timers[timer_index].dshot.dma_map_ch[timer_channel_index]; + + if (dma_map_ch == 0) { + // Timer channel is not mapped + PX4_WARN("Error! Timer %u Channel %u DMA is unmapped", timer_index, timer_channel_index); + continue; + } + + // Allocate DMA + timer_configs[timer_index].dma_ch_handle[timer_channel_index] = stm32_dmachannel(dma_map_ch); + + if (timer_configs[timer_index].dma_ch_handle[timer_channel_index] == NULL) { + PX4_WARN("Failed to allocate Timer %u Channel DMA CH %u, output_channel %u", timer_index, timer_channel_index, + output_channel); + continue; + } + + PX4_DEBUG("Allocated DMA CH Timer Index %u Channel %u", timer_index, timer_channel_index); + // Mark this timer channel as bidirectional + timer_configs[timer_index].captcomp_channels[timer_channel_index] = true; + } + } + + // De-allocate DMA for each channel + for (uint8_t output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) { + if (timer_index == timer_io_channels[output_channel].timer_index) { + uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1; + + if (timer_configs[timer_index].dma_ch_handle[timer_channel_index]) { + stm32_dmafree(timer_configs[timer_index].dma_ch_handle[timer_channel_index]); + timer_configs[timer_index].dma_ch_handle[timer_channel_index] = NULL; + PX4_DEBUG("Freed DMA CH Timer Index %u Channel %u", timer_index, timer_channel_index); + } + } } } +} - /* Init channels */ - int ret_val = OK; - int channels_init_mask = 0; +static int32_t init_timer_channels(uint8_t timer_index) +{ + int32_t channels_init_mask = 0; + + if (!timer_configs[timer_index].enabled || !timer_configs[timer_index].initialized) { + // timer is not enabled or could not be initialized + return 0; + } - for (unsigned channel = 0; (channel_mask != 0) && (channel < MAX_TIMER_IO_CHANNELS) && (OK == ret_val); channel++) { - if (channel_mask & (1 << channel)) { - uint8_t timer = timer_io_channels[channel].timer_index; + io_timer_channel_mode_t mode = timer_configs[timer_index].bidirectional ? IOTimerChanMode_DshotInverted : + IOTimerChanMode_Dshot; - if (io_timers[timer].dshot.dma_base == 0) { // board does not configure dshot on this timer + for (uint8_t output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) { + uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1; + bool this_timer = timer_index == timer_io_channels[output_channel].timer_index; + bool channel_enabled = timer_configs[timer_index].enabled_channels[timer_channel_index]; + + if (this_timer && channel_enabled) { + int ret = io_timer_channel_init(output_channel, mode, NULL, NULL); + + if (ret != OK) { + PX4_WARN("io_timer_channel_init %u failed", output_channel); continue; } - ret_val = io_timer_channel_init(channel, IOTimerChanMode_Dshot, NULL, NULL); - channel_mask &= ~(1 << channel); + timer_configs[timer_index].initialized_channels[timer_channel_index] = true; + channels_init_mask |= (1 << output_channel); - if (OK == ret_val) { - dshot_handler[timer].init = true; - channels_init_mask |= 1 << channel; + if (timer_configs[timer_index].bidirectional) { + PX4_DEBUG("DShot initialized OutputChannel %u (bidirectional)", output_channel); - } else if (ret_val == -EBUSY) { - /* either timer or channel already used - this is not fatal */ - ret_val = 0; + } else { + PX4_DEBUG("DShot initialized OutputChannel %u", output_channel); } } } - for (uint8_t timer_index = 0; (timer_index < DSHOT_TIMERS) && (OK == ret_val); timer_index++) { + return channels_init_mask; +} + +int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bidirectional_dshot) +{ + _dshot_frequency = dshot_pwm_freq; + _bidirectional = enable_bidirectional_dshot; + + if (_bidirectional) { + PX4_INFO("Bidirectional DShot enabled, only one timer will be used"); + } + + // NOTE: if bidirectional is enabled only 1 timer can be used. This is because Burst mode uses 1 DMA channel per timer + // and CaptureCompare (for reading ESC eRPM) uses 4 DMA. Even if we only used CaptureCompare on a single timer + // we would still need 5 DMA; 1 DMA for the second timer burst, and 4 DMA for the first timer CaptureCompare. The only + // way to support more than 1 timer is to burst/captcomp sequentially for each timer enabled for dshot. The code is + // structured in a way to allow extending support for this in the future. + + // Initialize timer_config data based on enabled channels + init_timer_config(channel_mask); + + // Initializes dshot on each timer if DShot mode is enabled and DMA is available + init_timers_dma_up(); + + // Initializes a single timer in Bidirectional DShot mode + if (_bidirectional) { + // Use first configured DShot timer (Timer index 0) + // TODO: BDSHOT_TIM param to select timer index? + init_timers_dma_capt_comp(_bidi_timer_index); + } + + int32_t channels_init_mask = 0; - 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); + for (uint8_t timer_index = 0; timer_index < MAX_IO_TIMERS; timer_index++) { + channels_init_mask |= init_timer_channels(timer_index); + } - dshot_handler[timer_index].dma_handle = stm32_dmachannel(io_timers[timer_index].dshot.dmamap); + unsigned output_buffer_offset = 0; - if (NULL == dshot_handler[timer_index].dma_handle) { - ret_val = ERROR; + for (unsigned timer_index = 0; timer_index < MAX_IO_TIMERS; timer_index++) { + if (timer_configs[timer_index].initialized) { + 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_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; + output_buffer_offset += DSHOT_OUTPUT_BUFFER_SIZE(channel_count); + + if (output_buffer_offset > sizeof(dshot_burst_buffer_array)) { + return -EINVAL; // something is wrong with the board configuration or some other logic } } } - return ret_val == OK ? channels_init_mask : ret_val; + return channels_init_mask; } -int up_bdshot_get_erpm(uint8_t channel, int *erpm) +// Kicks off a DMA transmit for each configured timer and the associated channels +void up_dshot_trigger() { - // Not implemented - return -1; + // Enable DShot inverted on all channels + 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 < MAX_IO_TIMERS; timer_index++) { + if (timer_configs[timer_index].enabled && timer_configs[timer_index].initialized) { + + 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); + + // Allocate DMA + if (timer_configs[timer_index].dma_up_handle == NULL) { + timer_configs[timer_index].dma_up_handle = stm32_dmachannel(io_timers[timer_index].dshot.dma_map_up); + + if (timer_configs[timer_index].dma_up_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_configs[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, + DSHOT_DMA_SCR); + + // Clean UDE flag before DMA is started + io_timer_update_dma_req(timer_index, false); + + // Trigger DMA (DShot Outputs) + if (timer_configs[timer_index].bidirectional) { + stm32_dmastart(timer_configs[timer_index].dma_up_handle, dma_burst_finished_callback, + &timer_configs[timer_index].timer_index, + false); + + } else { + stm32_dmastart(timer_configs[timer_index].dma_up_handle, NULL, NULL, false); + } + + // Enable DMA update request + io_timer_update_dma_req(timer_index, true); + } + } } -int up_bdshot_channel_status(uint8_t channel) +void dma_burst_finished_callback(DMA_HANDLE handle, uint8_t status, void *arg) { - // Not implemented - return -1; + uint8_t timer_index = *((uint8_t *)arg); + + // Clean DMA UP configuration + if (timer_configs[timer_index].dma_up_handle != NULL) { + stm32_dmastop(timer_configs[timer_index].dma_up_handle); + stm32_dmafree(timer_configs[timer_index].dma_up_handle); + timer_configs[timer_index].dma_up_handle = NULL; + } + + // Disable DMA update request + io_timer_update_dma_req(timer_index, false); + + // De-allocate timer + io_timer_unallocate_timer(timer_index); + + // Flush cache so DMA sees the data + memset(dshot_capture_buffer, 0, sizeof(dshot_capture_buffer)); + up_clean_dcache((uintptr_t) dshot_capture_buffer, + (uintptr_t) dshot_capture_buffer + DSHOT_CAPTURE_BUFFER_SIZE(MAX_NUM_CHANNELS_PER_TIMER)); + + // Allocate DMA for all enabled 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; + uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1; + bool channel_initialized = timer_configs[timer_index].initialized_channels[timer_channel_index]; + bool captcomp_enabled = timer_configs[timer_index].captcomp_channels[timer_channel_index]; + + if (is_this_timer && channel_initialized && captcomp_enabled) { + + // Re-initialize output for CaptureDMA + io_timer_unallocate_channel(output_channel); + io_timer_channel_init(output_channel, IOTimerChanMode_CaptureDMA, NULL, NULL); + + // Allocate DMA + if (timer_configs[timer_index].dma_ch_handle[timer_channel_index] == NULL) { + timer_configs[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 (timer_configs[timer_index].dma_ch_handle[timer_channel_index] == NULL) { + PX4_WARN("failed to allocate dma for timer %u channel %u", timer_index, timer_channel_index); + return; + } + + // Set Capture mode for this channel + io_timer_set_dshot_capture_mode(timer_index, timer_channel_index, _dshot_frequency); + io_timer_capture_dma_req(timer_index, timer_channel_index, true); + + // 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(timer_configs[timer_index].dma_ch_handle[timer_channel_index], + periph_addr, + (uint32_t) dshot_capture_buffer[timer_channel_index], + CHANNEL_CAPTURE_BUFF_SIZE, + DSHOT_BIDIRECTIONAL_DMA_SCR); + + // NOTE: we can't use DMA callback since GCR encoding creates a variable length pulse train. Instead + // we use an hrt callback to schedule the processing of the received and DMAd eRPM frames. + stm32_dmastart(timer_configs[timer_index].dma_ch_handle[timer_channel_index], NULL, NULL, false); + } + } + + // Enable CaptureDMA and on all configured channels + io_timer_set_enable(true, IOTimerChanMode_CaptureDMA, IO_TIMER_ALL_MODES_CHANNELS); + + // 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; + hrt_call_after(&_cc_call, delay, capture_complete_callback, arg); } -void up_bdshot_status(void) +static void capture_complete_callback(void *arg) { + uint8_t timer_index = *((uint8_t *)arg); + + // Unallocate the timer as CaptureDMA + io_timer_unallocate_timer(timer_index); + + 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; + uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1; + bool channel_initialized = timer_configs[timer_index].initialized_channels[timer_channel_index]; + bool captcomp_enabled = timer_configs[timer_index].captcomp_channels[timer_channel_index]; + + if (is_this_timer && channel_initialized) { + + if (captcomp_enabled) { + // Disable capture DMA + io_timer_capture_dma_req(timer_index, timer_channel_index, false); + + if (timer_configs[timer_index].dma_ch_handle[timer_channel_index] != NULL) { + stm32_dmastop(timer_configs[timer_index].dma_ch_handle[timer_channel_index]); + stm32_dmafree(timer_configs[timer_index].dma_ch_handle[timer_channel_index]); + timer_configs[timer_index].dma_ch_handle[timer_channel_index] = NULL; + } + } + + io_timer_unallocate_channel(output_channel); + // Initialize back to DShotInverted to bring IO back to the expected idle state + io_timer_channel_init(output_channel, IOTimerChanMode_DshotInverted, NULL, NULL); + } + } + + // Invalidate the dcache to ensure most recent data is available + up_invalidate_dcache((uintptr_t) dshot_capture_buffer, + (uintptr_t) dshot_capture_buffer + DSHOT_CAPTURE_BUFFER_SIZE(MAX_NUM_CHANNELS_PER_TIMER)); + + // Process eRPM frames from all channels on this timer + process_capture_results(timer_index); + + // Enable all channels configured as DShotInverted + io_timer_set_enable(true, IOTimerChanMode_DshotInverted, IO_TIMER_ALL_MODES_CHANNELS); } -void up_dshot_trigger(void) +void process_capture_results(uint8_t timer_index) { - for (uint8_t timer = 0; (timer < DSHOT_TIMERS); timer++) { + for (uint8_t channel_index = 0; channel_index < MAX_NUM_CHANNELS_PER_TIMER; channel_index++) { - if (true == dshot_handler[timer].init) { + // Calculate the period for each channel + const unsigned period = calculate_period(timer_index, channel_index); - // 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, - DSHOT_DMA_SCR); + if (period == 0) { + // If the parsing failed, set the eRPM to 0 + _erpms[channel_index] = 0; - // 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); - io_timer_update_dma_req(timer, true); + } else if (period == 65408) { + // Special case for zero motion (e.g., stationary motor) + _erpms[channel_index] = 0; + } else { + // Convert the period to eRPM + _erpms[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 motor_number, uint16_t throttle, bool telemetry) +void dshot_motor_data_set(unsigned channel, uint16_t throttle, bool telemetry) { + uint8_t timer_index = timer_io_channels[channel].timer_index; + uint8_t timer_channel_index = timer_io_channels[channel].timer_channel - 1; + bool channel_initialized = timer_configs[timer_index].initialized_channels[timer_channel_index]; + + if (!channel_initialized) { + return; + } + uint16_t packet = 0; uint16_t checksum = 0; @@ -213,21 +579,25 @@ void dshot_motor_data_set(unsigned motor_number, uint16_t throttle, bool telemet /* XOR checksum calculation */ csum_data >>= NIBBLES_SIZE; - for (unsigned i = 0; i < DSHOT_NUMBER_OF_NIBBLES; i++) { + for (uint8_t i = 0; i < DSHOT_NUMBER_OF_NIBBLES; i++) { checksum ^= (csum_data & 0x0F); // XOR data by nibbles csum_data >>= NIBBLES_SIZE; } - packet |= (checksum & 0x0F); + if (_bidirectional) { + packet |= ((~checksum) & 0x0F); + + } else { + packet |= ((checksum) & 0x0F); + } + - 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; + const io_timers_channel_mapping_element_t *mapping = &io_timers_channel_mapping.element[timer_index]; + uint8_t num_motors = mapping->channel_count_including_gaps; + uint8_t 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] = + for (uint8_t motor_data_index = 0; motor_data_index < ONE_MOTOR_DATA_SIZE; motor_data_index++) { + dshot_output_buffer[timer_index][motor_data_index * num_motors + timer_channel] = (packet & 0x8000) ? MOTOR_PWM_BIT_1 : MOTOR_PWM_BIT_0; // MSB first packet <<= 1; } @@ -235,7 +605,193 @@ 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 ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot, + IO_TIMER_ALL_MODES_CHANNELS); +} + +int up_bdshot_get_erpm(uint8_t output_channel, int *erpm) +{ + uint8_t timer_index = timer_io_channels[output_channel].timer_index; + uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1; + bool channel_initialized = timer_configs[timer_index].initialized_channels[timer_channel_index]; + + if (channel_initialized) { + *erpm = _erpms[timer_channel_index]; + return PX4_OK; + } + + // this channel is not configured for dshot + return PX4_ERROR; +} + +int up_bdshot_channel_status(uint8_t channel) +{ + uint8_t timer_index = timer_io_channels[channel].timer_index; + uint8_t timer_channel_index = timer_io_channels[channel].timer_channel - 1; + bool channel_initialized = timer_configs[timer_index].initialized_channels[timer_channel_index]; + + // TODO: track that each channel is communicating using the decode stats + if (channel_initialized) { + return 1; + } + + return 0; +} + +void up_bdshot_status(void) +{ + PX4_INFO("dshot driver stats:"); + + uint8_t timer_index = _bidi_timer_index; + + for (uint8_t timer_channel_index = 0; timer_channel_index < MAX_NUM_CHANNELS_PER_TIMER; timer_channel_index++) { + bool channel_initialized = timer_configs[timer_index].initialized_channels[timer_channel_index]; + + if (channel_initialized) { + PX4_INFO("Timer %u, Channel %u: read %lu, failed nibble %lu, failed CRC %lu, invalid/zero %lu", + timer_index, timer_channel_index, + read_ok[timer_channel_index], + read_fail_nibble[timer_channel_index], + read_fail_crc[timer_channel_index], + read_fail_zero[timer_channel_index]); + } + } +} + +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(uint8_t timer_index, uint8_t channel_index) +{ + uint32_t value = 0; + uint32_t high = 1; // We start off with high + unsigned shifted = 0; + unsigned previous = 0; + + // 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. + if (i > 1) { + + if (dshot_capture_buffer[channel_index][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[channel_index][i] - previous + 5) / 20; + + // Shift the bits into the value + for (unsigned bit = 0; bit < bits; ++bit) { + value = (value << 1) | high; + ++shifted; + } + + // The next edge toggles. + high = !high; + } + + previous = dshot_capture_buffer[channel_index][i]; + } + + if (shifted == 0) { + // no data yet, or this time + ++read_fail_zero[channel_index]; + return 0; + } + + // We need to make sure we shifted 21 times. We might have missed some low "pulses" at the very end. + value <<= (21 - shifted); + + // 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[channel_index];; + return 0; + } + + data |= nibble; + 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[channel_index];; + return 0; + } + + ++read_ok[channel_index];; + return period; } #endif 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..64a86d13a62f 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 @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2019 PX4 Development Team. All rights reserved. + * Copyright (C) 2024 PX4 Development Team. All rights reserved. * Author: Igor Misic * * Redistribution and use in source and binary forms, with or without @@ -58,5 +58,6 @@ */ typedef struct dshot_conf_t { uint32_t dma_base; - uint32_t dmamap; + uint32_t dma_map_up; + uint32_t dma_map_ch[4]; } dshot_conf_t; 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..08a6bcad2b7d 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012, 2017 PX4 Development Team. All rights reserved. + * Copyright (C) 2024 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -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; @@ -158,7 +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 int io_timer_set_dshot_burst_mode(uint8_t timer, unsigned dshot_pwm_rate, uint8_t dma_burst_length); + +__EXPORT void io_timer_capture_dma_req(uint8_t timer, uint8_t timer_channel_index, bool enable); +__EXPORT int io_timer_set_dshot_capture_mode(uint8_t timer, uint8_t timer_channel_index, unsigned dshot_pwm_freq); /** * Reserve a timer @@ -168,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/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..bdd6feec25e7 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 @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2019 PX4 Development Team. All rights reserved. + * Copyright (C) 2024 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -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/stm32_common/io_pins/input_capture.c b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/input_capture.c index 8d43bfc7352b..349393c28dce 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/input_capture.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/input_capture.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012-2016 PX4 Development Team. All rights reserved. + * Copyright (C) 2024 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -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..828ecaa01bee 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012, 2017 PX4 Development Team. All rights reserved. + * Copyright (C) 2024 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -96,41 +96,42 @@ 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 +#define CCMR_C1_PWMOUT_INVERTED_INIT (GTIM_CCMR_MODE_PWM2 << GTIM_CCMR1_OC1M_SHIFT) | GTIM_CCMR1_OC1PE #define CCMR_C1_PWMIN_INIT 0 // TBD @@ -141,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 */ @@ -161,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) @@ -519,7 +520,57 @@ void io_timer_update_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) +void io_timer_capture_dma_req(uint8_t timer, uint8_t timer_channel_index, bool enable) +{ + if (enable) { + switch (timer_channel_index) { + case 0: + rDIER(timer) |= ATIM_DIER_CC1DE; + rEGR(timer) |= (ATIM_EGR_UG | ATIM_EGR_CC1G); + break; + + case 1: + rDIER(timer) |= ATIM_DIER_CC2DE; + rEGR(timer) |= (ATIM_EGR_UG | ATIM_EGR_CC2G); + break; + + case 2: + rDIER(timer) |= ATIM_DIER_CC3DE; + rEGR(timer) |= (ATIM_EGR_UG | ATIM_EGR_CC3G); + break; + + case 3: + rDIER(timer) |= ATIM_DIER_CC4DE; + rEGR(timer) |= (ATIM_EGR_UG | ATIM_EGR_CC4G); + break; + } + + } else { + switch (timer_channel_index) { + case 0: + rEGR(timer) &= ~(ATIM_EGR_UG | ATIM_EGR_CC1G); + rDIER(timer) &= ~ATIM_DIER_CC1DE; + break; + + case 1: + rEGR(timer) &= ~(ATIM_EGR_UG | ATIM_EGR_CC2G); + rDIER(timer) &= ~ATIM_DIER_CC2DE; + break; + + case 2: + rEGR(timer) &= ~(ATIM_EGR_UG | ATIM_EGR_CC3G); + rDIER(timer) &= ~ATIM_DIER_CC3DE; + break; + + case 3: + rEGR(timer) &= ~(ATIM_EGR_UG | ATIM_EGR_CC4G); + rDIER(timer) &= ~ATIM_DIER_CC4DE; + break; + } + } +} + +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; @@ -545,7 +596,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 +625,47 @@ 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, uint8_t timer_channel_index, unsigned dshot_pwm_freq) +{ + // Timer Autor Reload Register max value + rARR(timer) = 0xFFFFFFFF; + // Timer Prescalar + rPSC(timer) = ((int)(io_timers[timer].clock_freq / (dshot_pwm_freq * 5 / 4)) / DSHOT_MOTOR_PWM_BIT_WIDTH) - 1; + + switch (timer_channel_index) { + case 0: + rEGR(timer) |= ATIM_EGR_UG | GTIM_EGR_CC1G; + rCCER(timer) &= ~(GTIM_CCER_CC1E | GTIM_CCER_CC1P | GTIM_CCER_CC1NP); + rCCMR1(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR1_CC1S_SHIFT); + rCCER(timer) |= (GTIM_CCER_CC1E | GTIM_CCER_CC1P | GTIM_CCER_CC1NP); + + break; + + case 1: + rEGR(timer) |= ATIM_EGR_UG | GTIM_EGR_CC2G; + rCCER(timer) &= ~(GTIM_CCER_CC2E | GTIM_CCER_CC2P | GTIM_CCER_CC2NP); + rCCMR1(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR1_CC2S_SHIFT); + rCCER(timer) |= (GTIM_CCER_CC2E | GTIM_CCER_CC2P | GTIM_CCER_CC2NP); + break; + + case 2: + rEGR(timer) |= ATIM_EGR_UG | GTIM_EGR_CC3G; + rCCER(timer) &= ~(GTIM_CCER_CC3E | GTIM_CCER_CC3P | GTIM_CCER_CC3NP); + rCCMR2(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR2_CC3S_SHIFT); + rCCER(timer) |= (GTIM_CCER_CC3E | GTIM_CCER_CC3P | GTIM_CCER_CC3NP); + break; + + case 3: + rEGR(timer) |= ATIM_EGR_UG | GTIM_EGR_CC4G; + rCCER(timer) &= ~(GTIM_CCER_CC4E | GTIM_CCER_CC4P | GTIM_CCER_CC4NP); + rCCMR2(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR2_CC4S_SHIFT); + rCCER(timer) |= (GTIM_CCER_CC4E | GTIM_CCER_CC4P | GTIM_CCER_CC4NP); + 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 +865,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 +879,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; @@ -897,10 +996,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 +1047,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; } @@ -985,7 +1087,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; } @@ -1006,6 +1108,7 @@ int io_timer_set_ccr(unsigned channel, uint16_t value) if ((mode != IOTimerChanMode_PWMOut) && (mode != IOTimerChanMode_OneShot) && (mode != IOTimerChanMode_Dshot) && + (mode != IOTimerChanMode_DshotInverted) && (mode != IOTimerChanMode_Trigger)) { rv = -EIO; diff --git a/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/hw_description.h b/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/hw_description.h index 975467dd70e4..9234406ad567 100644 --- a/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/hw_description.h +++ b/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/hw_description.h @@ -35,6 +35,47 @@ #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::Index2) { + dma_map_ch[0] = DMAMAP_TIM1_CH1; + dma_map_ch[1] = DMAMAP_TIM1_CH2_1; + dma_map_ch[2] = DMAMAP_TIM1_CH3_2; + dma_map_ch[3] = DMAMAP_TIM1_CH4; + } + + break; + + case Timer::Timer2: + case Timer::Timer3: + break; + + case Timer::Timer4: + if (dma.index == DMA::Index1) { + dma_map_ch[0] = DMAMAP_TIM4_CH1; + dma_map_ch[1] = DMAMAP_TIM4_CH2; + dma_map_ch[2] = DMAMAP_TIM4_CH3; + dma_map_ch[3] = 0; + } + + break; + + case Timer::Timer5: + case Timer::Timer6: + case Timer::Timer7: + case Timer::Timer8: + case Timer::Timer9: + case Timer::Timer10: + case Timer::Timer11: + case Timer::Timer12: + case Timer::Timer13: + case Timer::Timer14: + break; + } +} + static inline constexpr uint32_t getTimerUpdateDMAMap(Timer::Timer timer, const DMA &dma) { uint32_t dma_map = 0; 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..10113e305920 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 @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2024 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,47 +35,211 @@ #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; + dma_map_ch[3] = 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; + dma_map_ch[3] = 0; + } + + 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-existent + break; + + case Timer::Timer10: + // Non-existent + break; + + case Timer::Timer11: + // Non-existent + break; + + case Timer::Timer12: + // Non-existent + break; + + case Timer::Timer13: + // Non-existent + break; + + case Timer::Timer14: + // Non-existent + 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 +255,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..20886b995ca7 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 @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2024 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -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; diff --git a/src/drivers/drv_dshot.h b/src/drivers/drv_dshot.h index 9c36dfd52d8b..ef297ff3b345 100644 --- a/src/drivers/drv_dshot.h +++ b/src/drivers/drv_dshot.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2024 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -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 e7f9e5866794d3550f2ccd9c32b1cf9a3bb9f09c Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Tue, 29 Oct 2024 15:05:25 -0800 Subject: [PATCH 02/14] f4/f1 support, not supported --- .../stm32f1/include/px4_arch/hw_description.h | 22 +++++++++++++++++++ .../stm32f4/include/px4_arch/hw_description.h | 22 +++++++++++++++++++ 2 files changed, 44 insertions(+) diff --git a/platforms/nuttx/src/px4/stm/stm32f1/include/px4_arch/hw_description.h b/platforms/nuttx/src/px4/stm/stm32f1/include/px4_arch/hw_description.h index aa2c0699ac4f..2097d0f5ade0 100644 --- a/platforms/nuttx/src/px4/stm/stm32f1/include/px4_arch/hw_description.h +++ b/platforms/nuttx/src/px4/stm/stm32f1/include/px4_arch/hw_description.h @@ -35,6 +35,28 @@ #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) +{ + // Not supported + switch (timer) { + case Timer::Timer1: + case Timer::Timer2: + case Timer::Timer3: + case Timer::Timer4: + case Timer::Timer5: + case Timer::Timer6: + case Timer::Timer7: + case Timer::Timer8: + case Timer::Timer9: + case Timer::Timer10: + case Timer::Timer11: + case Timer::Timer12: + case Timer::Timer13: + case Timer::Timer14: + break; + } +} + static inline constexpr uint32_t getTimerUpdateDMAMap(Timer::Timer timer, const DMA &dma) { // not used on STM32F1 diff --git a/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/hw_description.h b/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/hw_description.h index 975467dd70e4..10f416a702cd 100644 --- a/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/hw_description.h +++ b/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/hw_description.h @@ -35,6 +35,28 @@ #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) +{ + // Not supported + switch (timer) { + case Timer::Timer1: + case Timer::Timer2: + case Timer::Timer3: + case Timer::Timer4: + case Timer::Timer5: + case Timer::Timer6: + case Timer::Timer7: + case Timer::Timer8: + case Timer::Timer9: + case Timer::Timer10: + case Timer::Timer11: + case Timer::Timer12: + case Timer::Timer13: + case Timer::Timer14: + break; + } +} + static inline constexpr uint32_t getTimerUpdateDMAMap(Timer::Timer timer, const DMA &dma) { uint32_t dma_map = 0; From 1aa216ef070999af4537abf2ae36d5e5ac4f1f13 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Tue, 29 Oct 2024 15:29:46 -0800 Subject: [PATCH 03/14] fix f1 build target --- .../px4/stm/stm32_common/io_pins/io_timer.c | 23 ++++++++++++------- .../stm32f1/include/px4_arch/hw_description.h | 17 -------------- .../stm32f4/include/px4_arch/hw_description.h | 17 -------------- 3 files changed, 15 insertions(+), 42 deletions(-) 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 828ecaa01bee..bb685f0ace45 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 @@ -73,8 +73,15 @@ static int io_timer_handler7(int irq, void *context, void *arg); #if defined(HAVE_GTIM_CCXNP) #define HW_GTIM_CCER_CC1NP GTIM_CCER_CC1NP +#define HW_GTIM_CCER_CC2NP GTIM_CCER_CC2NP +#define HW_GTIM_CCER_CC3NP GTIM_CCER_CC3NP +#define HW_GTIM_CCER_CC4NP GTIM_CCER_CC4NP + #else # define HW_GTIM_CCER_CC1NP 0 +# define HW_GTIM_CCER_CC2NP 0 +# define HW_GTIM_CCER_CC3NP 0 +# define HW_GTIM_CCER_CC4NP 0 #endif #define arraySize(a) (sizeof((a))/sizeof(((a)[0]))) @@ -635,31 +642,31 @@ int io_timer_set_dshot_capture_mode(uint8_t timer, uint8_t timer_channel_index, switch (timer_channel_index) { case 0: rEGR(timer) |= ATIM_EGR_UG | GTIM_EGR_CC1G; - rCCER(timer) &= ~(GTIM_CCER_CC1E | GTIM_CCER_CC1P | GTIM_CCER_CC1NP); + rCCER(timer) &= ~(GTIM_CCER_CC1E | GTIM_CCER_CC1P | HW_GTIM_CCER_CC1NP); rCCMR1(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR1_CC1S_SHIFT); - rCCER(timer) |= (GTIM_CCER_CC1E | GTIM_CCER_CC1P | GTIM_CCER_CC1NP); + rCCER(timer) |= (GTIM_CCER_CC1E | GTIM_CCER_CC1P | HW_GTIM_CCER_CC1NP); break; case 1: rEGR(timer) |= ATIM_EGR_UG | GTIM_EGR_CC2G; - rCCER(timer) &= ~(GTIM_CCER_CC2E | GTIM_CCER_CC2P | GTIM_CCER_CC2NP); + rCCER(timer) &= ~(GTIM_CCER_CC2E | GTIM_CCER_CC2P | HW_GTIM_CCER_CC2NP); rCCMR1(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR1_CC2S_SHIFT); - rCCER(timer) |= (GTIM_CCER_CC2E | GTIM_CCER_CC2P | GTIM_CCER_CC2NP); + rCCER(timer) |= (GTIM_CCER_CC2E | GTIM_CCER_CC2P | HW_GTIM_CCER_CC2NP); break; case 2: rEGR(timer) |= ATIM_EGR_UG | GTIM_EGR_CC3G; - rCCER(timer) &= ~(GTIM_CCER_CC3E | GTIM_CCER_CC3P | GTIM_CCER_CC3NP); + rCCER(timer) &= ~(GTIM_CCER_CC3E | GTIM_CCER_CC3P | HW_GTIM_CCER_CC3NP); rCCMR2(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR2_CC3S_SHIFT); - rCCER(timer) |= (GTIM_CCER_CC3E | GTIM_CCER_CC3P | GTIM_CCER_CC3NP); + rCCER(timer) |= (GTIM_CCER_CC3E | GTIM_CCER_CC3P | HW_GTIM_CCER_CC3NP); break; case 3: rEGR(timer) |= ATIM_EGR_UG | GTIM_EGR_CC4G; - rCCER(timer) &= ~(GTIM_CCER_CC4E | GTIM_CCER_CC4P | GTIM_CCER_CC4NP); + rCCER(timer) &= ~(GTIM_CCER_CC4E | GTIM_CCER_CC4P | HW_GTIM_CCER_CC4NP); rCCMR2(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR2_CC4S_SHIFT); - rCCER(timer) |= (GTIM_CCER_CC4E | GTIM_CCER_CC4P | GTIM_CCER_CC4NP); + rCCER(timer) |= (GTIM_CCER_CC4E | GTIM_CCER_CC4P | HW_GTIM_CCER_CC4NP); break; } diff --git a/platforms/nuttx/src/px4/stm/stm32f1/include/px4_arch/hw_description.h b/platforms/nuttx/src/px4/stm/stm32f1/include/px4_arch/hw_description.h index 2097d0f5ade0..86badcfe655f 100644 --- a/platforms/nuttx/src/px4/stm/stm32f1/include/px4_arch/hw_description.h +++ b/platforms/nuttx/src/px4/stm/stm32f1/include/px4_arch/hw_description.h @@ -38,23 +38,6 @@ static inline constexpr void getTimerChannelDMAMap(Timer::Timer timer, const DMA &dma, uint32_t *dma_map_ch) { // Not supported - switch (timer) { - case Timer::Timer1: - case Timer::Timer2: - case Timer::Timer3: - case Timer::Timer4: - case Timer::Timer5: - case Timer::Timer6: - case Timer::Timer7: - case Timer::Timer8: - case Timer::Timer9: - case Timer::Timer10: - case Timer::Timer11: - case Timer::Timer12: - case Timer::Timer13: - case Timer::Timer14: - break; - } } static inline constexpr uint32_t getTimerUpdateDMAMap(Timer::Timer timer, const DMA &dma) diff --git a/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/hw_description.h b/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/hw_description.h index 10f416a702cd..1dd3bfd92d31 100644 --- a/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/hw_description.h +++ b/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/hw_description.h @@ -38,23 +38,6 @@ static inline constexpr void getTimerChannelDMAMap(Timer::Timer timer, const DMA &dma, uint32_t *dma_map_ch) { // Not supported - switch (timer) { - case Timer::Timer1: - case Timer::Timer2: - case Timer::Timer3: - case Timer::Timer4: - case Timer::Timer5: - case Timer::Timer6: - case Timer::Timer7: - case Timer::Timer8: - case Timer::Timer9: - case Timer::Timer10: - case Timer::Timer11: - case Timer::Timer12: - case Timer::Timer13: - case Timer::Timer14: - break; - } } static inline constexpr uint32_t getTimerUpdateDMAMap(Timer::Timer timer, const DMA &dma) From 87d6851b08128fc9901b701bcf10361680bfc5a2 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Fri, 15 Nov 2024 14:35:33 -0900 Subject: [PATCH 04/14] sanity check timer_channel value, fix CCxNP ifdef, debug stuff --- .../px4/stm/stm32_common/dshot/CMakeLists.txt | 4 +- .../src/px4/stm/stm32_common/dshot/dshot.c | 62 +++++++++++++++++-- .../px4/stm/stm32_common/io_pins/io_timer.c | 21 ++++--- 3 files changed, 71 insertions(+), 16 deletions(-) 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 2c315a0ca409..c256ce43d26c 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/CMakeLists.txt +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/CMakeLists.txt @@ -34,5 +34,5 @@ px4_add_library(arch_dshot dshot.c ) -# target_compile_options(arch_dshot PRIVATE ${MAX_CUSTOM_OPT_LEVEL} -DDEBUG_BUILD) -target_compile_options(arch_dshot PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) +target_compile_options(arch_dshot PRIVATE ${MAX_CUSTOM_OPT_LEVEL} -DDEBUG_BUILD) +# target_compile_options(arch_dshot PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) 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 ad2ba17e620d..fd1ba33d52f4 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -114,6 +114,8 @@ static uint32_t *dshot_output_buffer[MAX_IO_TIMERS] = {}; static uint16_t dshot_capture_buffer[MAX_NUM_CHANNELS_PER_TIMER][CHANNEL_CAPTURE_BUFF_SIZE] px4_cache_aligned_data() = {}; +static uint16_t test_buffer[MAX_NUM_CHANNELS_PER_TIMER][CHANNEL_CAPTURE_BUFF_SIZE]; + static bool _bidirectional = false; static uint8_t _bidi_timer_index = 0; // TODO: BDSHOT_TIM param to select timer index? static uint32_t _dshot_frequency = 0; @@ -136,7 +138,14 @@ static void init_timer_config(uint32_t channel_mask) 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; - uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1; + + uint8_t timer_channel = timer_io_channels[output_channel].timer_channel; + if ((timer_channel <= 0) || (timer_channel >= 5)) { + // invalid channel, only 1 - 4 + continue; + } + + uint8_t timer_channel_index = timer_channel - 1; if (io_timers[timer_index].dshot.dma_base == 0) { // board does not configure dshot on this timer @@ -211,8 +220,15 @@ static void init_timers_dma_capt_comp(uint8_t timer_index) // Allocate DMA for each enabled channel for (uint8_t output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) { + + uint8_t timer_channel = timer_io_channels[output_channel].timer_channel; + if ((timer_channel <= 0) || (timer_channel >= 5)) { + // invalid channel, only 1 - 4 + continue; + } + // For each enabled channel on this timer, try allocating DMA - uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1; + uint8_t timer_channel_index = timer_channel - 1; bool this_timer = timer_index == timer_io_channels[output_channel].timer_index; bool channel_enabled = timer_configs[timer_index].enabled_channels[timer_channel_index]; @@ -225,6 +241,7 @@ static void init_timers_dma_capt_comp(uint8_t timer_index) continue; } + PX4_DEBUG("Allocating DMA CH for Timer Index %u Channel %u", timer_index, timer_channel_index); // Allocate DMA timer_configs[timer_index].dma_ch_handle[timer_channel_index] = stm32_dmachannel(dma_map_ch); @@ -243,7 +260,14 @@ static void init_timers_dma_capt_comp(uint8_t timer_index) // De-allocate DMA for each channel for (uint8_t output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) { if (timer_index == timer_io_channels[output_channel].timer_index) { - uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1; + + uint8_t timer_channel = timer_io_channels[output_channel].timer_channel; + if ((timer_channel <= 0) || (timer_channel >= 5)) { + // invalid channel, only 1 - 4 + continue; + } + + uint8_t timer_channel_index = timer_channel - 1; if (timer_configs[timer_index].dma_ch_handle[timer_channel_index]) { stm32_dmafree(timer_configs[timer_index].dma_ch_handle[timer_channel_index]); @@ -268,7 +292,14 @@ static int32_t init_timer_channels(uint8_t timer_index) IOTimerChanMode_Dshot; for (uint8_t output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) { - uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1; + + uint8_t timer_channel = timer_io_channels[output_channel].timer_channel; + if ((timer_channel <= 0) || (timer_channel >= 5)) { + // invalid channel, only 1 - 4 + continue; + } + + uint8_t timer_channel_index = timer_channel - 1; bool this_timer = timer_index == timer_io_channels[output_channel].timer_index; bool channel_enabled = timer_configs[timer_index].enabled_channels[timer_channel_index]; @@ -436,7 +467,13 @@ void dma_burst_finished_callback(DMA_HANDLE handle, uint8_t status, void *arg) 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; - uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1; + uint8_t timer_channel = timer_io_channels[output_channel].timer_channel; + if ((timer_channel <= 0) || (timer_channel >= 5)) { + // invalid channel, only 1 - 4 + continue; + } + + uint8_t timer_channel_index = timer_channel - 1; bool channel_initialized = timer_configs[timer_index].initialized_channels[timer_channel_index]; bool captcomp_enabled = timer_configs[timer_index].captcomp_channels[timer_channel_index]; @@ -497,7 +534,13 @@ static void capture_complete_callback(void *arg) 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; - uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1; + uint8_t timer_channel = timer_io_channels[output_channel].timer_channel; + if ((timer_channel <= 0) || (timer_channel >= 5)) { + // invalid channel, only 1 - 4 + continue; + } + + uint8_t timer_channel_index = timer_channel - 1; bool channel_initialized = timer_configs[timer_index].initialized_channels[timer_channel_index]; bool captcomp_enabled = timer_configs[timer_index].captcomp_channels[timer_channel_index]; @@ -655,6 +698,11 @@ void up_bdshot_status(void) read_fail_crc[timer_channel_index], read_fail_zero[timer_channel_index]); } + + for (uint8_t i = 0; i < CHANNEL_CAPTURE_BUFF_SIZE; i++){ + PX4_INFO_RAW("%d ", test_buffer[timer_channel_index][i]); + } + PX4_INFO_RAW("\n"); } } @@ -722,6 +770,8 @@ unsigned calculate_period(uint8_t timer_index, uint8_t channel_index) unsigned shifted = 0; unsigned previous = 0; + memcpy(test_buffer[channel_index], dshot_capture_buffer[channel_index], CHANNEL_CAPTURE_BUFF_SIZE); + // Loop through the capture buffer for the specified channel for (unsigned i = 1; i < CHANNEL_CAPTURE_BUFF_SIZE; ++i) { 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 bb685f0ace45..86ed4685adb9 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 @@ -71,19 +71,24 @@ static int io_timer_handler5(int irq, void *context, void *arg); static int io_timer_handler6(int irq, void *context, void *arg); static int io_timer_handler7(int irq, void *context, void *arg); -#if defined(HAVE_GTIM_CCXNP) +// #if defined(HAVE_GTIM_CCXNP) +// #define HW_GTIM_CCER_CC1NP GTIM_CCER_CC1NP +// #define HW_GTIM_CCER_CC2NP GTIM_CCER_CC2NP +// #define HW_GTIM_CCER_CC3NP GTIM_CCER_CC3NP +// #define HW_GTIM_CCER_CC4NP GTIM_CCER_CC4NP + +// #else +// # define HW_GTIM_CCER_CC1NP 0 +// # define HW_GTIM_CCER_CC2NP 0 +// # define HW_GTIM_CCER_CC3NP 0 +// # define HW_GTIM_CCER_CC4NP 0 +// #endif + #define HW_GTIM_CCER_CC1NP GTIM_CCER_CC1NP #define HW_GTIM_CCER_CC2NP GTIM_CCER_CC2NP #define HW_GTIM_CCER_CC3NP GTIM_CCER_CC3NP #define HW_GTIM_CCER_CC4NP GTIM_CCER_CC4NP -#else -# define HW_GTIM_CCER_CC1NP 0 -# define HW_GTIM_CCER_CC2NP 0 -# define HW_GTIM_CCER_CC3NP 0 -# define HW_GTIM_CCER_CC4NP 0 -#endif - #define arraySize(a) (sizeof((a))/sizeof(((a)[0]))) /* If the timer clock source provided as clock_freq is the STM32_APBx_TIMx_CLKIN From ff6430baef456483be5fd1c8fd36cdef08bda091 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Sun, 17 Nov 2024 12:54:57 -0900 Subject: [PATCH 05/14] removed debug code, added define for H7 HAVE_GTIM_CCXNP --- .../src/px4/stm/stm32_common/dshot/dshot.c | 9 -------- .../px4/stm/stm32_common/io_pins/io_timer.c | 21 +++++++------------ .../stm/stm32h7/include/px4_arch/io_timer.h | 2 ++ 3 files changed, 9 insertions(+), 23 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 fd1ba33d52f4..f0158901be73 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -114,8 +114,6 @@ static uint32_t *dshot_output_buffer[MAX_IO_TIMERS] = {}; static uint16_t dshot_capture_buffer[MAX_NUM_CHANNELS_PER_TIMER][CHANNEL_CAPTURE_BUFF_SIZE] px4_cache_aligned_data() = {}; -static uint16_t test_buffer[MAX_NUM_CHANNELS_PER_TIMER][CHANNEL_CAPTURE_BUFF_SIZE]; - static bool _bidirectional = false; static uint8_t _bidi_timer_index = 0; // TODO: BDSHOT_TIM param to select timer index? static uint32_t _dshot_frequency = 0; @@ -698,11 +696,6 @@ void up_bdshot_status(void) read_fail_crc[timer_channel_index], read_fail_zero[timer_channel_index]); } - - for (uint8_t i = 0; i < CHANNEL_CAPTURE_BUFF_SIZE; i++){ - PX4_INFO_RAW("%d ", test_buffer[timer_channel_index][i]); - } - PX4_INFO_RAW("\n"); } } @@ -770,8 +763,6 @@ unsigned calculate_period(uint8_t timer_index, uint8_t channel_index) unsigned shifted = 0; unsigned previous = 0; - memcpy(test_buffer[channel_index], dshot_capture_buffer[channel_index], CHANNEL_CAPTURE_BUFF_SIZE); - // Loop through the capture buffer for the specified channel for (unsigned i = 1; i < CHANNEL_CAPTURE_BUFF_SIZE; ++i) { 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 86ed4685adb9..0ab43a1d1e58 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 @@ -61,7 +61,6 @@ #include #include - static int io_timer_handler0(int irq, void *context, void *arg); static int io_timer_handler1(int irq, void *context, void *arg); static int io_timer_handler2(int irq, void *context, void *arg); @@ -71,23 +70,17 @@ static int io_timer_handler5(int irq, void *context, void *arg); static int io_timer_handler6(int irq, void *context, void *arg); static int io_timer_handler7(int irq, void *context, void *arg); -// #if defined(HAVE_GTIM_CCXNP) -// #define HW_GTIM_CCER_CC1NP GTIM_CCER_CC1NP -// #define HW_GTIM_CCER_CC2NP GTIM_CCER_CC2NP -// #define HW_GTIM_CCER_CC3NP GTIM_CCER_CC3NP -// #define HW_GTIM_CCER_CC4NP GTIM_CCER_CC4NP - -// #else -// # define HW_GTIM_CCER_CC1NP 0 -// # define HW_GTIM_CCER_CC2NP 0 -// # define HW_GTIM_CCER_CC3NP 0 -// # define HW_GTIM_CCER_CC4NP 0 -// #endif - +#if defined(HAVE_GTIM_CCXNP) #define HW_GTIM_CCER_CC1NP GTIM_CCER_CC1NP #define HW_GTIM_CCER_CC2NP GTIM_CCER_CC2NP #define HW_GTIM_CCER_CC3NP GTIM_CCER_CC3NP #define HW_GTIM_CCER_CC4NP GTIM_CCER_CC4NP +#else +# define HW_GTIM_CCER_CC1NP 0 +# define HW_GTIM_CCER_CC2NP 0 +# define HW_GTIM_CCER_CC3NP 0 +# define HW_GTIM_CCER_CC4NP 0 +#endif #define arraySize(a) (sizeof((a))/sizeof(((a)[0]))) diff --git a/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/io_timer.h index 6b565c384730..32e5ab6c3763 100644 --- a/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/io_timer.h @@ -33,3 +33,5 @@ #pragma once #include "../../../stm32_common/include/px4_arch/io_timer.h" + +#define HAVE_GTIM_CCXNP From 07b7dc80a69e6688de6110bec4ede1cee24ceba8 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Sun, 17 Nov 2024 15:21:45 -0900 Subject: [PATCH 06/14] round robin sampling for less than 4 DMA --- .../src/px4/stm/stm32_common/dshot/dshot.c | 126 +++++++++++++++--- 1 file changed, 105 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 f0158901be73..e51877c5f74e 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -48,6 +48,9 @@ #include #include +#define TEST_LIMITED_DMA 1 +#define LIMIT_DMA_CHANNELS 1 + // DShot protocol definitions #define ONE_MOTOR_DATA_SIZE 16u #define MOTOR_PWM_BIT_1 14u @@ -84,6 +87,8 @@ static void init_timers_dma_up(void); static void init_timers_dma_capt_comp(uint8_t timer_index); static int32_t init_timer_channels(uint8_t timer_index); +static void configure_channels_round_robin(uint8_t timer_index); + static void dma_burst_finished_callback(DMA_HANDLE handle, uint8_t status, void *arg); static void capture_complete_callback(void *arg); @@ -100,9 +105,12 @@ typedef struct timer_config_t { bool initialized_channels[4]; // Timer channels initialized (successfully started) bool bidirectional; // Timer in bidi (inverted) mode bool captcomp_channels[4]; // Channels configured for CaptComp + bool round_robin_enabled; uint8_t timer_index; // Timer index. Necessary to have memory for passing pointer to hrt callback } timer_config_t; +static uint8_t _num_dma_available = 0; + static timer_config_t timer_configs[MAX_IO_TIMERS] = {}; // Output buffer of interleaved motor output bytes @@ -252,6 +260,14 @@ static void init_timers_dma_capt_comp(uint8_t timer_index) PX4_DEBUG("Allocated DMA CH Timer Index %u Channel %u", timer_index, timer_channel_index); // Mark this timer channel as bidirectional timer_configs[timer_index].captcomp_channels[timer_channel_index] = true; + _num_dma_available++; + +#if defined(TEST_LIMITED_DMA) + if (_num_dma_available >= LIMIT_DMA_CHANNELS) { + PX4_INFO("Limiting DMA channels to %u", _num_dma_available); + break; + } +#endif } } @@ -350,6 +366,11 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bi // Use first configured DShot timer (Timer index 0) // TODO: BDSHOT_TIM param to select timer index? init_timers_dma_capt_comp(_bidi_timer_index); + + // Enable round robin if we have 1 - 3 DMA + if ((_num_dma_available < 4) && _num_dma_available > 0) { + timer_configs[_bidi_timer_index].round_robin_enabled = true; + } } int32_t channels_init_mask = 0; @@ -439,6 +460,57 @@ void up_dshot_trigger() } } +static void configure_channels_round_robin(uint8_t timer_index) +{ + switch (_num_dma_available) { + case 1: { + for (uint8_t i = 0; i < 4; i++) { + if (timer_configs[timer_index].captcomp_channels[i]) { + timer_configs[timer_index].captcomp_channels[i] = false; + if (i == 3) { + timer_configs[timer_index].captcomp_channels[0] = true; + } else { + timer_configs[timer_index].captcomp_channels[i + 1] = true; + } + break; + } + } + break; + } + case 2: { + if (timer_configs[timer_index].captcomp_channels[0]) { + timer_configs[timer_index].captcomp_channels[0] = false; + timer_configs[timer_index].captcomp_channels[1] = true; + timer_configs[timer_index].captcomp_channels[2] = false; + timer_configs[timer_index].captcomp_channels[3] = true; + + } else { + timer_configs[timer_index].captcomp_channels[0] = true; + timer_configs[timer_index].captcomp_channels[1] = false; + timer_configs[timer_index].captcomp_channels[2] = true; + timer_configs[timer_index].captcomp_channels[3] = false; + } + break; + } + case 3: { + for (uint8_t i = 0; i < 4; i++) { + if (!timer_configs[timer_index].captcomp_channels[i]) { + timer_configs[timer_index].captcomp_channels[i] = true; + if (i == 3) { + timer_configs[timer_index].captcomp_channels[0] = false; + } else { + timer_configs[timer_index].captcomp_channels[i + 1] = false; + } + break; + } + } + break; + } + default: + break; + } +} + void dma_burst_finished_callback(DMA_HANDLE handle, uint8_t status, void *arg) { uint8_t timer_index = *((uint8_t *)arg); @@ -461,6 +533,11 @@ void dma_burst_finished_callback(DMA_HANDLE handle, uint8_t status, void *arg) up_clean_dcache((uintptr_t) dshot_capture_buffer, (uintptr_t) dshot_capture_buffer + DSHOT_CAPTURE_BUFFER_SIZE(MAX_NUM_CHANNELS_PER_TIMER)); + // If round robin is enabled reconfigure which channels we capture on + if (timer_configs[timer_index].round_robin_enabled) { + configure_channels_round_robin(timer_index); + } + // Allocate DMA for all enabled channels on this timer for (uint8_t output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) { @@ -655,8 +732,9 @@ int up_bdshot_get_erpm(uint8_t output_channel, int *erpm) uint8_t timer_index = timer_io_channels[output_channel].timer_index; uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1; bool channel_initialized = timer_configs[timer_index].initialized_channels[timer_channel_index]; + bool captcomp_enabled = timer_configs[timer_index].captcomp_channels[timer_channel_index]; - if (channel_initialized) { + if (channel_initialized && captcomp_enabled) { *erpm = _erpms[timer_channel_index]; return PX4_OK; } @@ -683,6 +761,14 @@ void up_bdshot_status(void) { PX4_INFO("dshot driver stats:"); + if (_bidirectional) { + PX4_INFO("Bidirectional DShot enabled"); + PX4_INFO("Available DMA: %u", _num_dma_available); + if (_num_dma_available < 4) { + PX4_INFO("Round robin enabled"); + } + } + uint8_t timer_index = _bidi_timer_index; for (uint8_t timer_channel_index = 0; timer_channel_index < MAX_NUM_CHANNELS_PER_TIMER; timer_channel_index++) { @@ -761,33 +847,31 @@ unsigned calculate_period(uint8_t timer_index, uint8_t channel_index) uint32_t value = 0; uint32_t high = 1; // We start off with high unsigned shifted = 0; - unsigned previous = 0; - // 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. + unsigned previous = dshot_capture_buffer[channel_index][1]; - // We can ignore the very first data point as it's the pulse before it starts. - if (i > 1) { - - if (dshot_capture_buffer[channel_index][i] == 0) { - // Once we get zeros we're through - break; - } + // Loop through the capture buffer for the specified channel + for (unsigned i = 2; i < CHANNEL_CAPTURE_BUFF_SIZE; ++i) { - // 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[channel_index][i] - previous + 5) / 20; + if (dshot_capture_buffer[channel_index][i] == 0) { + // Once we get zeros we're through + break; + } - // Shift the bits into the value - for (unsigned bit = 0; bit < bits; ++bit) { - value = (value << 1) | high; - ++shifted; - } + // 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[channel_index][i] - previous + 5) / 20; - // The next edge toggles. - high = !high; + // Convert GCR encoded pulse train into value + for (unsigned bit = 0; bit < bits; ++bit) { + value = (value << 1) | high; + ++shifted; } + // The next edge toggles. + high = !high; + previous = dshot_capture_buffer[channel_index][i]; } From 6cde017f97cab67c291e5a87cc2289e34a7f5b62 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Sun, 17 Nov 2024 15:22:13 -0900 Subject: [PATCH 07/14] unlimited esc_status logging --- src/modules/logger/logged_topics.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 3ff1c321e79b..de77a745ccfa 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -62,7 +62,8 @@ 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", 250); + add_topic("esc_status"); add_topic("failure_detector_status", 100); add_topic("failsafe_flags"); add_optional_topic("follow_target", 500); From e628db8726d4620506a26deeb0c036a1bdf604c7 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 19 Nov 2024 11:42:02 +1300 Subject: [PATCH 08/14] dshot: fix formatting --- .../src/px4/stm/stm32_common/dshot/dshot.c | 83 ++++++++++++------- 1 file changed, 51 insertions(+), 32 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 e51877c5f74e..9bc0a72e7c50 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -146,6 +146,7 @@ static void init_timer_config(uint32_t channel_mask) uint8_t timer_index = timer_io_channels[output_channel].timer_index; uint8_t timer_channel = timer_io_channels[output_channel].timer_channel; + if ((timer_channel <= 0) || (timer_channel >= 5)) { // invalid channel, only 1 - 4 continue; @@ -228,6 +229,7 @@ static void init_timers_dma_capt_comp(uint8_t timer_index) for (uint8_t output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) { uint8_t timer_channel = timer_io_channels[output_channel].timer_channel; + if ((timer_channel <= 0) || (timer_channel >= 5)) { // invalid channel, only 1 - 4 continue; @@ -276,6 +278,7 @@ static void init_timers_dma_capt_comp(uint8_t timer_index) if (timer_index == timer_io_channels[output_channel].timer_index) { uint8_t timer_channel = timer_io_channels[output_channel].timer_channel; + if ((timer_channel <= 0) || (timer_channel >= 5)) { // invalid channel, only 1 - 4 continue; @@ -308,6 +311,7 @@ static int32_t init_timer_channels(uint8_t timer_index) for (uint8_t output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) { uint8_t timer_channel = timer_io_channels[output_channel].timer_channel; + if ((timer_channel <= 0) || (timer_channel >= 5)) { // invalid channel, only 1 - 4 continue; @@ -464,48 +468,60 @@ static void configure_channels_round_robin(uint8_t timer_index) { switch (_num_dma_available) { case 1: { - for (uint8_t i = 0; i < 4; i++) { - if (timer_configs[timer_index].captcomp_channels[i]) { - timer_configs[timer_index].captcomp_channels[i] = false; - if (i == 3) { - timer_configs[timer_index].captcomp_channels[0] = true; - } else { - timer_configs[timer_index].captcomp_channels[i + 1] = true; + for (uint8_t i = 0; i < 4; i++) { + if (timer_configs[timer_index].captcomp_channels[i]) { + timer_configs[timer_index].captcomp_channels[i] = false; + + if (i == 3) { + timer_configs[timer_index].captcomp_channels[0] = true; + + } else { + timer_configs[timer_index].captcomp_channels[i + 1] = true; + } + + break; } - break; } + + break; } - break; - } + case 2: { - if (timer_configs[timer_index].captcomp_channels[0]) { - timer_configs[timer_index].captcomp_channels[0] = false; - timer_configs[timer_index].captcomp_channels[1] = true; - timer_configs[timer_index].captcomp_channels[2] = false; - timer_configs[timer_index].captcomp_channels[3] = true; + if (timer_configs[timer_index].captcomp_channels[0]) { + timer_configs[timer_index].captcomp_channels[0] = false; + timer_configs[timer_index].captcomp_channels[1] = true; + timer_configs[timer_index].captcomp_channels[2] = false; + timer_configs[timer_index].captcomp_channels[3] = true; - } else { - timer_configs[timer_index].captcomp_channels[0] = true; - timer_configs[timer_index].captcomp_channels[1] = false; - timer_configs[timer_index].captcomp_channels[2] = true; - timer_configs[timer_index].captcomp_channels[3] = false; + } else { + timer_configs[timer_index].captcomp_channels[0] = true; + timer_configs[timer_index].captcomp_channels[1] = false; + timer_configs[timer_index].captcomp_channels[2] = true; + timer_configs[timer_index].captcomp_channels[3] = false; + } + + break; } - break; - } + case 3: { - for (uint8_t i = 0; i < 4; i++) { - if (!timer_configs[timer_index].captcomp_channels[i]) { - timer_configs[timer_index].captcomp_channels[i] = true; - if (i == 3) { - timer_configs[timer_index].captcomp_channels[0] = false; - } else { - timer_configs[timer_index].captcomp_channels[i + 1] = false; + for (uint8_t i = 0; i < 4; i++) { + if (!timer_configs[timer_index].captcomp_channels[i]) { + timer_configs[timer_index].captcomp_channels[i] = true; + + if (i == 3) { + timer_configs[timer_index].captcomp_channels[0] = false; + + } else { + timer_configs[timer_index].captcomp_channels[i + 1] = false; + } + + break; } - break; } + + break; } - break; - } + default: break; } @@ -543,6 +559,7 @@ void dma_burst_finished_callback(DMA_HANDLE handle, uint8_t status, void *arg) bool is_this_timer = timer_index == timer_io_channels[output_channel].timer_index; uint8_t timer_channel = timer_io_channels[output_channel].timer_channel; + if ((timer_channel <= 0) || (timer_channel >= 5)) { // invalid channel, only 1 - 4 continue; @@ -610,6 +627,7 @@ static void capture_complete_callback(void *arg) bool is_this_timer = timer_index == timer_io_channels[output_channel].timer_index; uint8_t timer_channel = timer_io_channels[output_channel].timer_channel; + if ((timer_channel <= 0) || (timer_channel >= 5)) { // invalid channel, only 1 - 4 continue; @@ -764,6 +782,7 @@ void up_bdshot_status(void) if (_bidirectional) { PX4_INFO("Bidirectional DShot enabled"); PX4_INFO("Available DMA: %u", _num_dma_available); + if (_num_dma_available < 4) { PX4_INFO("Round robin enabled"); } From 345739187aa7a7ee367a43af16dac14fa4b49aeb Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 19 Nov 2024 11:42:29 +1300 Subject: [PATCH 09/14] dshot: add define for number of DMA channels to use This allows individual boards to override the number of DShot channels and hence avoid round robin capture of the RPM feedback. --- platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 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 9bc0a72e7c50..3ed46a7d5a5b 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -48,8 +48,10 @@ #include #include -#define TEST_LIMITED_DMA 1 -#define LIMIT_DMA_CHANNELS 1 +// This can be overriden for a specific board. +#ifndef BOARD_DMA_NUM_DSHOT_CHANNELS +#define BOARD_DMA_NUM_DSHOT_CHANNELS 1 +#endif // DShot protocol definitions #define ONE_MOTOR_DATA_SIZE 16u @@ -264,12 +266,10 @@ static void init_timers_dma_capt_comp(uint8_t timer_index) timer_configs[timer_index].captcomp_channels[timer_channel_index] = true; _num_dma_available++; -#if defined(TEST_LIMITED_DMA) - if (_num_dma_available >= LIMIT_DMA_CHANNELS) { + if (_num_dma_available >= BOARD_DMA_NUM_DSHOT_CHANNELS) { PX4_INFO("Limiting DMA channels to %u", _num_dma_available); break; } -#endif } } From c3d1c91b4b4288d9ea0e8a92b6063d7256f56e81 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 19 Nov 2024 11:43:23 +1300 Subject: [PATCH 10/14] ARK: enable 4 DMA channels for DShot on 6X --- boards/ark/fmu-v6x/src/board_config.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/boards/ark/fmu-v6x/src/board_config.h b/boards/ark/fmu-v6x/src/board_config.h index 04b84631c760..f5e9daeb978a 100644 --- a/boards/ark/fmu-v6x/src/board_config.h +++ b/boards/ark/fmu-v6x/src/board_config.h @@ -420,6 +420,9 @@ /* This board provides a DMA pool and APIs */ #define BOARD_DMA_ALLOC_POOL_SIZE 5120 +/* This board has 4 DMA channels available for bidirectional dshot */ +#define BOARD_DMA_NUM_DSHOT_CHANNELS 4 + /* This board provides the board_on_reset interface */ #define BOARD_HAS_ON_RESET 1 From 4cf2f242dc991ddd6527c2adf22c26fe255352ba Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 19 Nov 2024 12:59:22 +1300 Subject: [PATCH 11/14] dshot: publish when all channels are updated This slows down the ESC_STATUS publication in the case of round robin capture. E.g. for 800 Hz output with one DMA channel, the ESC_STATUS is now published at 200 Hz. --- .../nuttx/src/px4/nxp/imxrt/dshot/dshot.c | 13 ++++++++++ .../src/px4/stm/stm32_common/dshot/dshot.c | 25 +++++++++++++++++-- src/drivers/drv_dshot.h | 11 ++++++++ src/drivers/dshot/DShot.cpp | 5 ++++ 4 files changed, 52 insertions(+), 2 deletions(-) diff --git a/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c b/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c index 22a27427f9dc..a32bf5dff165 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c +++ b/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c @@ -418,6 +418,19 @@ void up_bdshot_erpm(void) } +int up_bdshot_num_erpm_ready(void) +{ + int num_ready = 0; + + for (unsigned i = 0; i < DSHOT_TIMERS; ++i) { + if (bdshot_parsed_recv_mask & (1 << i)) { + ++num_ready; + } + } + + return num_ready; +} + int up_bdshot_get_erpm(uint8_t channel, int *erpm) { 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 3ed46a7d5a5b..4a04194613c5 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -130,6 +130,7 @@ static uint32_t _dshot_frequency = 0; // eRPM data for channels on the singular timer static int32_t _erpms[MAX_NUM_CHANNELS_PER_TIMER] = {}; +static bool _erpms_ready[MAX_NUM_CHANNELS_PER_TIMER] = {}; // hrt callback handle for captcomp post dma processing static struct hrt_call _cc_call; @@ -671,6 +672,10 @@ void process_capture_results(uint8_t timer_index) { for (uint8_t channel_index = 0; channel_index < MAX_NUM_CHANNELS_PER_TIMER; channel_index++) { + if (!timer_configs[timer_index].captcomp_channels[channel_index]) { + continue; + } + // Calculate the period for each channel const unsigned period = calculate_period(timer_index, channel_index); @@ -686,6 +691,9 @@ void process_capture_results(uint8_t timer_index) // Convert the period to eRPM _erpms[channel_index] = (1000000 * 60) / period; } + + // We set it ready anyway, not to hold up other channels when used in round robin. + _erpms_ready[channel_index] = true; } } @@ -745,15 +753,28 @@ int up_dshot_arm(bool armed) IO_TIMER_ALL_MODES_CHANNELS); } +int up_bdshot_num_erpm_ready(void) +{ + int num_ready = 0; + + for (unsigned i = 0; i < MAX_NUM_CHANNELS_PER_TIMER; ++i) { + if (_erpms_ready[i]) { + ++num_ready; + } + } + + return num_ready; +} + int up_bdshot_get_erpm(uint8_t output_channel, int *erpm) { uint8_t timer_index = timer_io_channels[output_channel].timer_index; uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1; bool channel_initialized = timer_configs[timer_index].initialized_channels[timer_channel_index]; - bool captcomp_enabled = timer_configs[timer_index].captcomp_channels[timer_channel_index]; - if (channel_initialized && captcomp_enabled) { + if (channel_initialized) { *erpm = _erpms[timer_channel_index]; + _erpms_ready[timer_channel_index] = false; return PX4_OK; } diff --git a/src/drivers/drv_dshot.h b/src/drivers/drv_dshot.h index ef297ff3b345..dba58c3105e0 100644 --- a/src/drivers/drv_dshot.h +++ b/src/drivers/drv_dshot.h @@ -143,6 +143,17 @@ __EXPORT extern int up_dshot_arm(bool armed); __EXPORT extern void up_bdshot_status(void); +/** + * Get how many bidirectional erpm channels are ready + * + * When we get the erpm round-robin style, we need to get + * and publish the erpms less often. + * + * @return <0 on error, OK on succes + */ +__EXPORT extern int up_bdshot_num_erpm_ready(void); + + /** * Get bidrectional dshot erpm for a channel * @param channel Dshot channel diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index 940019d9bf04..a898948d61ff 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -309,6 +309,11 @@ int DShot::handle_new_bdshot_erpm(void) esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_DSHOT; esc_status.esc_armed_flags = _outputs_on; + // We wait until all are ready. + if (up_bdshot_num_erpm_ready() < (int)popcount(_output_mask)) { + return 0; + } + for (unsigned i = 0; i < _num_outputs; i++) { if (_mixing_output.isFunctionSet(i)) { if (up_bdshot_get_erpm(i, &erpm) == 0) { From fdb94f938c03b93a3166cf8dbf3b79c079f287a3 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 19 Nov 2024 15:16:44 +1300 Subject: [PATCH 12/14] dshot: avoid duplicate publications for bidir and telem Instead of publishing both bidirectional dshot updates as well as telemetry updates, we now combine the data from both streams, and publish whenever we get RPM updates, as the latter arrives with higher rate, e.g. 200 Hz with round robin, or faster otherwise. When combining the data, we take RPM from bidirectional dshot, and the rest from telemetry. When we have only one of the two, either telemetry or bidirectional dshot, we just publish that one. --- src/drivers/dshot/DShot.cpp | 99 ++++++++++++++-------------- src/drivers/dshot/DShot.h | 18 ++--- src/drivers/dshot/DShotTelemetry.cpp | 12 ++-- src/drivers/dshot/DShotTelemetry.h | 9 +-- 4 files changed, 69 insertions(+), 69 deletions(-) diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index a898948d61ff..66cdeee98954 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -170,10 +170,6 @@ void DShot::enable_dshot_outputs(const bool enabled) } _outputs_initialized = true; - - if (_bidirectional_dshot_enabled) { - init_telemetry(NULL); - } } if (_outputs_initialized) { @@ -182,28 +178,24 @@ void DShot::enable_dshot_outputs(const bool enabled) } } -void DShot::update_telemetry_num_motors() +void DShot::update_num_motors() { - if (!_telemetry) { - return; - } - int motor_count = 0; for (unsigned i = 0; i < _num_outputs; ++i) { if (_mixing_output.isFunctionSet(i)) { - _telemetry->actuator_functions[motor_count] = (uint8_t)_mixing_output.outputFunction(i); + _actuator_functions[motor_count] = (uint8_t)_mixing_output.outputFunction(i); ++motor_count; } } - _telemetry->handler.setNumMotors(motor_count); + _num_motors = motor_count; } void DShot::init_telemetry(const char *device) { if (!_telemetry) { - _telemetry = new Telemetry{}; + _telemetry = new DShotTelemetry{}; if (!_telemetry) { PX4_ERR("alloc failed"); @@ -211,32 +203,35 @@ void DShot::init_telemetry(const char *device) } } - _telemetry->esc_status_pub.advertise(); - if (device != NULL) { - int ret = _telemetry->handler.init(device); + int ret = _telemetry->init(device); if (ret != 0) { PX4_ERR("telemetry init failed (%i)", ret); } } - update_telemetry_num_motors(); + update_num_motors(); } -int DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data) +int DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data, bool ignore_rpm) { int ret = 0; // fill in new motor data - esc_status_s &esc_status = _telemetry->esc_status_pub.get(); + esc_status_s &esc_status = esc_status_pub.get(); if (telemetry_index < esc_status_s::CONNECTED_ESC_MAX) { esc_status.esc_online_flags |= 1 << telemetry_index; - esc_status.esc[telemetry_index].actuator_function = _telemetry->actuator_functions[telemetry_index]; - esc_status.esc[telemetry_index].timestamp = data.time; - esc_status.esc[telemetry_index].esc_rpm = (static_cast(data.erpm) * 100) / - (_param_mot_pole_count.get() / 2); + esc_status.esc[telemetry_index].actuator_function = _actuator_functions[telemetry_index]; + + if (!ignore_rpm) { + // If we also have bidirectional dshot, we use rpm and timestamps from there. + esc_status.esc[telemetry_index].timestamp = data.time; + esc_status.esc[telemetry_index].esc_rpm = (static_cast(data.erpm) * 100) / + (_param_mot_pole_count.get() / 2); + } + esc_status.esc[telemetry_index].esc_voltage = static_cast(data.voltage) * 0.01f; esc_status.esc[telemetry_index].esc_current = static_cast(data.current) * 0.01f; esc_status.esc[telemetry_index].esc_temperature = static_cast(data.temperature); @@ -244,34 +239,34 @@ int DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTelem } // publish when motor index wraps (which is robust against motor timeouts) - if (telemetry_index <= _telemetry->last_telemetry_index) { + if (telemetry_index <= _last_telemetry_index) { esc_status.timestamp = hrt_absolute_time(); esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_DSHOT; - esc_status.esc_count = _telemetry->handler.numMotors(); + esc_status.esc_count = _num_motors; ++esc_status.counter; ret = 1; // Indicate we wrapped, so we publish data } - _telemetry->last_telemetry_index = telemetry_index; + _last_telemetry_index = telemetry_index; return ret; } void DShot::publish_esc_status(void) { - esc_status_s &esc_status = _telemetry->esc_status_pub.get(); + esc_status_s &esc_status = esc_status_pub.get(); int telemetry_index = 0; // clear data of the esc that are offline - for (int index = 0; (index < _telemetry->last_telemetry_index); index++) { + for (int index = 0; (index < _last_telemetry_index); index++) { if ((esc_status.esc_online_flags & (1 << index)) == 0) { memset(&esc_status.esc[index], 0, sizeof(struct esc_report_s)); } } // FIXME: mark all UART Telemetry ESC's as online, otherwise commander complains even for a single dropout - esc_status.esc_count = _telemetry->handler.numMotors(); + esc_status.esc_count = _num_motors; esc_status.esc_online_flags = (1 << esc_status.esc_count) - 1; esc_status.esc_armed_flags = (1 << esc_status.esc_count) - 1; @@ -290,8 +285,12 @@ void DShot::publish_esc_status(void) } } - // ESC telem wrap around or bdshot update - _telemetry->esc_status_pub.update(); + if (!esc_status_pub.advertised()) { + esc_status_pub.advertise(); + + } else { + esc_status_pub.update(); + } // reset esc online flags esc_status.esc_online_flags = 0; @@ -302,7 +301,7 @@ int DShot::handle_new_bdshot_erpm(void) int num_erpms = 0; int telemetry_index = 0; int erpm; - esc_status_s &esc_status = _telemetry->esc_status_pub.get(); + esc_status_s &esc_status = esc_status_pub.get(); esc_status.timestamp = hrt_absolute_time(); esc_status.counter = _esc_status_counter++; @@ -310,7 +309,7 @@ int DShot::handle_new_bdshot_erpm(void) esc_status.esc_armed_flags = _outputs_on; // We wait until all are ready. - if (up_bdshot_num_erpm_ready() < (int)popcount(_output_mask)) { + if (up_bdshot_num_erpm_ready() < _num_motors) { return 0; } @@ -321,7 +320,7 @@ int DShot::handle_new_bdshot_erpm(void) esc_status.esc_online_flags |= 1 << telemetry_index; esc_status.esc[telemetry_index].timestamp = hrt_absolute_time(); esc_status.esc[telemetry_index].esc_rpm = (erpm * 100) / (_param_mot_pole_count.get() / 2); - esc_status.esc[telemetry_index].actuator_function = _telemetry->actuator_functions[telemetry_index]; + esc_status.esc[telemetry_index].actuator_function = _actuator_functions[telemetry_index]; } ++telemetry_index; @@ -397,7 +396,7 @@ void DShot::retrieve_and_print_esc_info_thread_safe(const int motor_index) int DShot::request_esc_info() { - _telemetry->handler.redirectOutput(*_request_esc_info.load()); + _telemetry->redirectOutput(*_request_esc_info.load()); _waiting_for_esc_info = true; int motor_index = _request_esc_info.load()->motor_index; @@ -413,7 +412,8 @@ int DShot::request_esc_info() void DShot::mixerChanged() { - update_telemetry_num_motors(); + update_num_motors(); + } bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], @@ -428,11 +428,11 @@ bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], if (_telemetry) { // check for an ESC info request. We only process it when we're not expecting other telemetry data if (_request_esc_info.load() != nullptr && !_waiting_for_esc_info && stop_motors - && !_telemetry->handler.expectingData() && !_current_command.valid()) { + && !_telemetry->expectingData() && !_current_command.valid()) { requested_telemetry_index = request_esc_info(); } else { - requested_telemetry_index = _telemetry->handler.getRequestMotorIndex(); + requested_telemetry_index = _telemetry->getRequestMotorIndex(); } } @@ -542,8 +542,7 @@ void DShot::Run() } if (_telemetry) { - int telem_update = _telemetry->handler.update(); - int need_to_publish = 0; + const int telem_update = _telemetry->update(_num_motors); // Are we waiting for ESC info? if (_waiting_for_esc_info) { @@ -553,21 +552,25 @@ void DShot::Run() } } else if (telem_update >= 0) { - need_to_publish = handle_new_telemetry_data(telem_update, _telemetry->handler.latestESCData()); - } + const int need_to_publish = handle_new_telemetry_data(telem_update, _telemetry->latestESCData(), + _bidirectional_dshot_enabled); - if (_bidirectional_dshot_enabled) { - // Add bdshot data to esc status - need_to_publish += handle_new_bdshot_erpm(); + // We don't want to publish twice, once by telemetry and once by bidirectional dishot. + if (!_bidirectional_dshot_enabled && need_to_publish) { + publish_esc_status(); + } } + } - if (need_to_publish > 0) { - // ESC telem wrap around or bdshot update + if (_bidirectional_dshot_enabled) { + // Add bdshot data to esc status + const int need_to_publish = handle_new_bdshot_erpm(); + + if (need_to_publish) { publish_esc_status(); } } - if (_parameter_update_sub.updated()) { update_params(); } @@ -802,7 +805,7 @@ int DShot::print_status() if (_telemetry) { PX4_INFO("telemetry on: %s", _telemetry_device); - _telemetry->handler.printStatus(); + _telemetry->printStatus(); } /* Print dshot status */ diff --git a/src/drivers/dshot/DShot.h b/src/drivers/dshot/DShot.h index e60b33b86279..e744c27f2601 100644 --- a/src/drivers/dshot/DShot.h +++ b/src/drivers/dshot/DShot.h @@ -120,18 +120,14 @@ class DShot final : public ModuleBase, public OutputModuleInterface void clear() { num_repetitions = 0; } }; - struct Telemetry { - DShotTelemetry handler{}; - uORB::PublicationMultiData esc_status_pub{ORB_ID(esc_status)}; - int last_telemetry_index{-1}; - uint8_t actuator_functions[esc_status_s::CONNECTED_ESC_MAX] {}; - }; + int _last_telemetry_index{-1}; + uint8_t _actuator_functions[esc_status_s::CONNECTED_ESC_MAX] {}; void enable_dshot_outputs(const bool enabled); void init_telemetry(const char *device); - int handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data); + int handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data, bool ignore_rpm); void publish_esc_status(void); @@ -143,14 +139,16 @@ class DShot final : public ModuleBase, public OutputModuleInterface void update_params(); - void update_telemetry_num_motors(); + void update_num_motors(); void handle_vehicle_commands(); MixingOutput _mixing_output{PARAM_PREFIX, DIRECT_PWM_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; uint32_t _reversible_outputs{}; - Telemetry *_telemetry{nullptr}; + DShotTelemetry *_telemetry{nullptr}; + + uORB::PublicationMultiData esc_status_pub{ORB_ID(esc_status)}; static char _telemetry_device[20]; static px4::atomic_bool _request_telemetry_init; @@ -167,6 +165,8 @@ class DShot final : public ModuleBase, public OutputModuleInterface static constexpr unsigned _num_outputs{DIRECT_PWM_OUTPUT_CHANNELS}; uint32_t _output_mask{0}; + int _num_motors{0}; + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; Command _current_command{}; diff --git a/src/drivers/dshot/DShotTelemetry.cpp b/src/drivers/dshot/DShotTelemetry.cpp index 0132a9b04941..70992b1703ca 100644 --- a/src/drivers/dshot/DShotTelemetry.cpp +++ b/src/drivers/dshot/DShotTelemetry.cpp @@ -87,7 +87,7 @@ int DShotTelemetry::redirectOutput(OutputBuffer &buffer) return 0; } -int DShotTelemetry::update() +int DShotTelemetry::update(int num_motors) { if (_uart_fd < 0) { return -1; @@ -120,7 +120,7 @@ int DShotTelemetry::update() ++_num_timeouts; } - requestNextMotor(); + requestNextMotor(num_motors); return -2; } @@ -142,7 +142,7 @@ int DShotTelemetry::update() _redirect_output = nullptr; ret = _current_motor_index_request; _current_motor_index_request = -1; - requestNextMotor(); + requestNextMotor(num_motors); } } else { @@ -153,7 +153,7 @@ int DShotTelemetry::update() ret = _current_motor_index_request; } - requestNextMotor(); + requestNextMotor(num_motors); } } } @@ -225,9 +225,9 @@ uint8_t DShotTelemetry::crc8(const uint8_t *buf, uint8_t len) } -void DShotTelemetry::requestNextMotor() +void DShotTelemetry::requestNextMotor(int num_motors) { - _current_motor_index_request = (_current_motor_index_request + 1) % _num_motors; + _current_motor_index_request = (_current_motor_index_request + 1) % num_motors; _current_request_start = 0; _frame_position = 0; } diff --git a/src/drivers/dshot/DShotTelemetry.h b/src/drivers/dshot/DShotTelemetry.h index 1164d1e2008c..8672e85f7f48 100644 --- a/src/drivers/dshot/DShotTelemetry.h +++ b/src/drivers/dshot/DShotTelemetry.h @@ -64,14 +64,12 @@ class DShotTelemetry void deinit(); - void setNumMotors(int num_motors) { _num_motors = num_motors; } - int numMotors() const { return _num_motors; } - /** * Read telemetry from the UART (non-blocking) and handle timeouts. + * @param num_motors How many DShot enabled motors * @return -1 if no update, -2 timeout, >= 0 for the motor index. Use @latestESCData() to get the data. */ - int update(); + int update(int num_motors); /** * Redirect everything that is read into a different buffer. @@ -112,7 +110,7 @@ class DShotTelemetry */ int setBaudrate(unsigned baud); - void requestNextMotor(); + void requestNextMotor(int num_motors); /** * Decode a single byte from an ESC feedback frame @@ -126,7 +124,6 @@ class DShotTelemetry static uint8_t crc8(const uint8_t *buf, uint8_t len); int _uart_fd{-1}; - int _num_motors{0}; uint8_t _frame_buffer[ESC_FRAME_SIZE]; int _frame_position{0}; From ee3f7244ba9cbc8932f35f28d9593a428375a582 Mon Sep 17 00:00:00 2001 From: alexklimaj Date: Tue, 26 Nov 2024 14:20:13 -0700 Subject: [PATCH 13/14] boards: add ark fpv and pi6x BOARD_DMA_NUM_DSHOT_CHANNELS --- boards/ark/fpv/src/board_config.h | 3 +++ boards/ark/fpv/src/timer_config.cpp | 2 +- boards/ark/pi6x/src/board_config.h | 3 +++ 3 files changed, 7 insertions(+), 1 deletion(-) diff --git a/boards/ark/fpv/src/board_config.h b/boards/ark/fpv/src/board_config.h index 081dbb56698f..d2f2968a6fc4 100644 --- a/boards/ark/fpv/src/board_config.h +++ b/boards/ark/fpv/src/board_config.h @@ -304,6 +304,9 @@ /* This board provides a DMA pool and APIs */ #define BOARD_DMA_ALLOC_POOL_SIZE 5120 +/* This board has 3 DMA channels available for bidirectional dshot */ +#define BOARD_DMA_NUM_DSHOT_CHANNELS 3 + /* This board provides the board_on_reset interface */ #define BOARD_HAS_ON_RESET 1 diff --git a/boards/ark/fpv/src/timer_config.cpp b/boards/ark/fpv/src/timer_config.cpp index 623aad1f0601..711ad62289cd 100644 --- a/boards/ark/fpv/src/timer_config.cpp +++ b/boards/ark/fpv/src/timer_config.cpp @@ -58,7 +58,7 @@ constexpr io_timers_t io_timers[MAX_IO_TIMERS] = { initIOTimer(Timer::Timer5, DMA{DMA::Index1}), initIOTimer(Timer::Timer8, DMA{DMA::Index1}), - initIOTimer(Timer::Timer4, DMA{DMA::Index1}), + initIOTimer(Timer::Timer4), }; constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { diff --git a/boards/ark/pi6x/src/board_config.h b/boards/ark/pi6x/src/board_config.h index 5e123db35124..8988aa619d30 100644 --- a/boards/ark/pi6x/src/board_config.h +++ b/boards/ark/pi6x/src/board_config.h @@ -309,6 +309,9 @@ /* This board provides a DMA pool and APIs */ #define BOARD_DMA_ALLOC_POOL_SIZE 5120 +/* This board has 4 DMA channels available for bidirectional dshot */ +#define BOARD_DMA_NUM_DSHOT_CHANNELS 4 + /* This board provides the board_on_reset interface */ #define BOARD_HAS_ON_RESET 1 From 03528b3201c2c97267bcb79bcc354920b47d83cc Mon Sep 17 00:00:00 2001 From: alexklimaj Date: Wed, 27 Nov 2024 11:41:50 -0700 Subject: [PATCH 14/14] dshot: turn off debug build --- platforms/nuttx/src/px4/stm/stm32_common/dshot/CMakeLists.txt | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) 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 c256ce43d26c..5221ae896a63 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/CMakeLists.txt +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/CMakeLists.txt @@ -34,5 +34,4 @@ px4_add_library(arch_dshot dshot.c ) -target_compile_options(arch_dshot PRIVATE ${MAX_CUSTOM_OPT_LEVEL} -DDEBUG_BUILD) -# target_compile_options(arch_dshot PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) +target_compile_options(arch_dshot PRIVATE ${MAX_CUSTOM_OPT_LEVEL})