From 543851db50231197060b57ada50f84518565bece Mon Sep 17 00:00:00 2001 From: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> Date: Wed, 12 Mar 2025 10:55:15 -0800 Subject: [PATCH] [Sponsored by ARK] Bidirectional DShot (#23863) * Bidirectional DShot Co-authored-by: Julian Oes * f4/f1 support, not supported * fix f1 build target * sanity check timer_channel value, fix CCxNP ifdef, debug stuff * removed debug code, added define for H7 HAVE_GTIM_CCXNP * round robin sampling for less than 4 DMA * unlimited esc_status logging * dshot: fix formatting * 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. * ARK: enable 4 DMA channels for DShot on 6X * 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. * 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. * boards: add ark fpv and pi6x BOARD_DMA_NUM_DSHOT_CHANNELS * dshot: turn off debug build --------- Co-authored-by: Julian Oes Co-authored-by: alexklimaj --- boards/ark/fmu-v6x/src/board_config.h | 3 + boards/ark/fpv/src/board_config.h | 3 + boards/ark/fpv/src/timer_config.cpp | 2 +- boards/ark/pi6x/src/board_config.h | 3 + boards/holybro/kakutef7/default.px4board | 5 +- .../nuttx/src/px4/nxp/imxrt/dshot/dshot.c | 15 + .../rpi_common/include/px4_arch/io_timer.h | 3 - .../src/px4/stm/stm32_common/dshot/dshot.c | 963 +++++++++++++++--- .../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 | 182 +++- .../stm32f1/include/px4_arch/hw_description.h | 5 + .../stm32f4/include/px4_arch/hw_description.h | 5 + .../stm32f7/include/px4_arch/hw_description.h | 41 + .../stm32h7/include/px4_arch/hw_description.h | 192 +++- .../stm/stm32h7/include/px4_arch/io_timer.h | 2 + .../px4_arch/io_timer_hw_description.h | 11 +- src/drivers/drv_dshot.h | 15 +- src/drivers/dshot/DShot.cpp | 106 +- src/drivers/dshot/DShot.h | 18 +- src/drivers/dshot/DShotTelemetry.cpp | 12 +- src/drivers/dshot/DShotTelemetry.h | 9 +- src/modules/logger/logged_topics.cpp | 3 +- 25 files changed, 1362 insertions(+), 268 deletions(-) diff --git a/boards/ark/fmu-v6x/src/board_config.h b/boards/ark/fmu-v6x/src/board_config.h index 04b84631c7..f5e9daeb97 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 diff --git a/boards/ark/fpv/src/board_config.h b/boards/ark/fpv/src/board_config.h index 7b3c54183e..6611e2e734 100644 --- a/boards/ark/fpv/src/board_config.h +++ b/boards/ark/fpv/src/board_config.h @@ -307,6 +307,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 623aad1f06..711ad62289 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 5e123db351..8988aa619d 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 diff --git a/boards/holybro/kakutef7/default.px4board b/boards/holybro/kakutef7/default.px4board index 82a3261e87..280641c940 100644 --- a/boards/holybro/kakutef7/default.px4board +++ b/boards/holybro/kakutef7/default.px4board @@ -17,7 +17,6 @@ CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y CONFIG_DRIVERS_OSD_ATXXXX=y CONFIG_DRIVERS_PWM_OUT=y CONFIG_DRIVERS_RC_INPUT=y -CONFIG_DRIVERS_TELEMETRY_FRSKY_TELEMETRY=y CONFIG_MODULES_BATTERY_STATUS=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y @@ -47,6 +46,6 @@ CONFIG_MODULES_SENSORS=y # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set # CONFIG_SENSORS_VEHICLE_MAGNETOMETER is not set # CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW is not set -CONFIG_SYSTEMCMDS_DMESG=y +# CONFIG_SYSTEMCMDS_DMESG is not set CONFIG_SYSTEMCMDS_PARAM=y -CONFIG_SYSTEMCMDS_TOP=y +# CONFIG_SYSTEMCMDS_TOP is not set diff --git a/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c b/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c index 22a27427f9..233d4fc288 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c +++ b/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c @@ -418,6 +418,21 @@ void up_bdshot_erpm(void) } +int up_bdshot_num_erpm_ready(void) +{ + int num_ready = 0; + + for (unsigned i = 0; i < DSHOT_TIMERS; ++i) { + // We only check that data has been received, rather than if it's valid. + // This ensures data is published even if one channel has bit errors. + if (bdshot_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/rpi/rpi_common/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/io_timer.h index 0d042d85ce..978b87e9dc 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 @@ -151,9 +151,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/dshot.c b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c index 00491e9768..4a04194613 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,32 @@ #include #include +#include +#include +#include -#define MOTOR_PWM_BIT_1 14u -#define MOTOR_PWM_BIT_0 7u -#define DSHOT_TIMERS MAX_IO_TIMERS -#define MOTORS_NUMBER DIRECT_PWM_OUTPUT_CHANNELS -#define ONE_MOTOR_DATA_SIZE 16u -#define 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 +// 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 +#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 +77,641 @@ 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 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); + +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 + 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 +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] = {}; +static bool _erpms_ready[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; - for (int timer_index = 0; timer_index < DSHOT_TIMERS; timer_index++) { - dshot_handler[timer_index].init = false; - } + uint8_t timer_channel = timer_io_channels[output_channel].timer_channel; - for (unsigned timer = 0; timer < DSHOT_TIMERS; ++timer) { - if (io_timers[timer].base == 0) { // no more timers configured - break; - } - - // we know the uint8_t* cast to uint32_t* is fine, since we're aligned to cache line size -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wcast-align" - dshot_burst_buffer[timer] = (uint32_t *)&dshot_burst_buffer_array[buffer_offset]; -#pragma GCC diagnostic pop - buffer_offset += DSHOT_BURST_BUFFER_SIZE(io_timers_channel_mapping.element[timer].channel_count_including_gaps); - - if (buffer_offset > sizeof(dshot_burst_buffer_array)) { - return -EINVAL; // something is wrong with the board configuration or some other logic - } - } - - /* Init channels */ - int ret_val = OK; - int channels_init_mask = 0; - - for (unsigned channel = 0; (channel_mask != 0) && (channel < MAX_TIMER_IO_CHANNELS) && (OK == ret_val); channel++) { - if (channel_mask & (1 << channel)) { - uint8_t timer = timer_io_channels[channel].timer_index; - - if (io_timers[timer].dshot.dma_base == 0) { // board does not configure dshot on this timer + if ((timer_channel <= 0) || (timer_channel >= 5)) { + // invalid channel, only 1 - 4 continue; } - ret_val = io_timer_channel_init(channel, IOTimerChanMode_Dshot, NULL, NULL); - channel_mask &= ~(1 << channel); + uint8_t timer_channel_index = timer_channel - 1; - if (OK == ret_val) { - dshot_handler[timer].init = true; - channels_init_mask |= 1 << channel; + if (io_timers[timer_index].dshot.dma_base == 0) { + // board does not configure dshot on this timer + continue; + } - } else if (ret_val == -EBUSY) { - /* either timer or channel already used - this is not fatal */ - ret_val = 0; + // 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; + } + } + } +} + +// 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; + } + + 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); + } + } +} + +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++) { + + 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_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; + } + + 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); + + 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; + _num_dma_available++; + + if (_num_dma_available >= BOARD_DMA_NUM_DSHOT_CHANNELS) { + PX4_INFO("Limiting DMA channels to %u", _num_dma_available); + break; + } + } + } + + // 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 = 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]); + 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); + } + } + } + } +} + +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; + } + + io_timer_channel_mode_t mode = timer_configs[timer_index].bidirectional ? IOTimerChanMode_DshotInverted : + IOTimerChanMode_Dshot; + + 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; + } + + 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]; + + 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; + } + + timer_configs[timer_index].initialized_channels[timer_channel_index] = true; + channels_init_mask |= (1 << output_channel); + + if (timer_configs[timer_index].bidirectional) { + PX4_DEBUG("DShot initialized OutputChannel %u (bidirectional)", output_channel); + + } 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; +} - 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); +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; - dshot_handler[timer_index].dma_handle = stm32_dmachannel(io_timers[timer_index].dshot.dmamap); + if (_bidirectional) { + PX4_INFO("Bidirectional DShot enabled, only one timer will be used"); + } - if (NULL == dshot_handler[timer_index].dma_handle) { - ret_val = ERROR; + // 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); + + // 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; + + for (uint8_t timer_index = 0; timer_index < MAX_IO_TIMERS; timer_index++) { + channels_init_mask |= init_timer_channels(timer_index); + } + + unsigned output_buffer_offset = 0; + + 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); -int up_bdshot_channel_status(uint8_t channel) -{ - // Not implemented - return -1; -} + // 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) { -void up_bdshot_status(void) -{ -} + uint32_t channel_count = io_timers_channel_mapping.element[timer_index].channel_count_including_gaps; -void up_dshot_trigger(void) -{ - for (uint8_t timer = 0; (timer < DSHOT_TIMERS); timer++) { + io_timer_set_dshot_burst_mode(timer_index, _dshot_frequency, channel_count); - if (true == dshot_handler[timer].init) { + // 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_burst_buffer[timer], - (uintptr_t)dshot_burst_buffer[timer] + - DSHOT_BURST_BUFFER_SIZE(io_timers_channel_mapping.element[timer].channel_count_including_gaps)); + up_clean_dcache((uintptr_t) dshot_output_buffer[timer_index], + (uintptr_t) dshot_output_buffer[timer_index] + + DSHOT_OUTPUT_BUFFER_SIZE(channel_count)); - 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, + 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, false); - // Trigger DMA (DShot Outputs) - stm32_dmastart(dshot_handler[timer].dma_handle, NULL, NULL, false); - io_timer_update_dma_req(timer, true); + 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); } } } +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); + + // 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)); + + // 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++) { + + 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; + } + + 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]; + + 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); +} + +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 = 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]; + + 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 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); + + if (period == 0) { + // If the parsing failed, set the eRPM to 0 + _erpms[channel_index] = 0; + + } 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; + } + + // We set it ready anyway, not to hold up other channels when used in round robin. + _erpms_ready[channel_index] = true; + } +} + /** -* 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 +723,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); - 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; + } else { + packet |= ((checksum) & 0x0F); + } - 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] = + + 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 (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 +749,214 @@ 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_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]; + + if (channel_initialized) { + *erpm = _erpms[timer_channel_index]; + _erpms_ready[timer_channel_index] = false; + 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:"); + + 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++) { + 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; + + // We can ignore the very first data point as it's the pulse before it starts. + unsigned previous = dshot_capture_buffer[channel_index][1]; + + // Loop through the capture buffer for the specified channel + for (unsigned i = 2; i < CHANNEL_CAPTURE_BUFF_SIZE; ++i) { + + 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; + + // 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]; + } + + 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 93811a898c..64a86d13a6 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 8f64a825b6..009207a279 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 @@ -81,6 +81,8 @@ typedef enum io_timer_channel_mode_t { IOTimerChanMode_PPS = 8, IOTimerChanMode_RPM = 9, IOTimerChanMode_Other = 10, + IOTimerChanMode_DshotInverted = 11, + IOTimerChanMode_CaptureDMA = 12, IOTimerChanModeSize } io_timer_channel_mode_t; @@ -159,7 +161,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 @@ -169,7 +176,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 55ea439ef7..bdd6feec25 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 8d43bfc735..349393c28d 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 4246ea3585..0ab43a1d1e 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 @@ -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); @@ -73,8 +72,14 @@ 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]))) @@ -96,41 +101,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 _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 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 +147,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 +167,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 +525,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 +601,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 +630,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 | HW_GTIM_CCER_CC1NP); + rCCMR1(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR1_CC1S_SHIFT); + 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 | HW_GTIM_CCER_CC2NP); + rCCMR1(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR1_CC2S_SHIFT); + 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 | HW_GTIM_CCER_CC3NP); + rCCMR2(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR2_CC3S_SHIFT); + 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 | HW_GTIM_CCER_CC4NP); + rCCMR2(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR2_CC4S_SHIFT); + rCCER(timer) |= (GTIM_CCER_CC4E | GTIM_CCER_CC4P | HW_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 +870,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 +884,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 +1001,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 +1052,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 +1092,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 +1113,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/stm32f1/include/px4_arch/hw_description.h b/platforms/nuttx/src/px4/stm/stm32f1/include/px4_arch/hw_description.h index aa2c0699ac..86badcfe65 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,11 @@ #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 +} + 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 975467dd70..1dd3bfd92d 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,11 @@ #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 +} + 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/stm32f7/include/px4_arch/hw_description.h b/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/hw_description.h index 975467dd70..9234406ad5 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 a93b9538be..10113e3059 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 uint32_t getTimerUpdateDMAMap(Timer::Timer timer, const DMA &dma) +static inline constexpr void getTimerChannelDMAMap(Timer::Timer timer, const DMA &dma, uint32_t *dma_map_ch) { - uint32_t dma_map = 0; - switch (timer) { case Timer::Timer1: - dma_map = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM1UP_0 : DMAMAP_DMA12_TIM1UP_1; + 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: - dma_map = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM2UP_0 : DMAMAP_DMA12_TIM2UP_1; + 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: - dma_map = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM3UP_0 : DMAMAP_DMA12_TIM3UP_1; + 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: - dma_map = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM4UP_0 : DMAMAP_DMA12_TIM4UP_1; + 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: - dma_map = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM5UP_0 : DMAMAP_DMA12_TIM5UP_1; + 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: - dma_map = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM6UP_0 : DMAMAP_DMA12_TIM6UP_1; + // 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_up = 0; + + switch (timer) { + case Timer::Timer1: + dma_map_up = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM1UP_0 : DMAMAP_DMA12_TIM1UP_1; + break; + + case Timer::Timer2: + dma_map_up = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM2UP_0 : DMAMAP_DMA12_TIM2UP_1; + + break; + + case Timer::Timer3: + dma_map_up = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM3UP_0 : DMAMAP_DMA12_TIM3UP_1; + + break; + + case Timer::Timer4: + dma_map_up = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM4UP_0 : DMAMAP_DMA12_TIM4UP_1; + + break; + + case Timer::Timer5: + dma_map_up = (dma.index == DMA::Index1) ? DMAMAP_DMA12_TIM5UP_0 : DMAMAP_DMA12_TIM5UP_1; + + break; + + case Timer::Timer6: + 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.h b/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/io_timer.h index 6b565c3847..32e5ab6c37 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 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 b1299b04e3..20886b995c 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 431a56787d..792d6076fc 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). @@ -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 4bd9bdef19..10c356ade1 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -167,10 +167,6 @@ void DShot::enable_dshot_outputs(const bool enabled) } _outputs_initialized = true; - - if (_bidirectional_dshot_enabled) { - init_telemetry(NULL); - } } if (_outputs_initialized) { @@ -179,28 +175,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"); @@ -208,32 +200,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); @@ -241,34 +236,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; @@ -287,8 +282,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; @@ -299,13 +298,18 @@ 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++; 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() < _num_motors) { + return 0; + } + for (unsigned i = 0; i < _num_outputs; i++) { if (_mixing_output.isFunctionSet(i)) { if (up_bdshot_get_erpm(i, &erpm) == 0) { @@ -313,7 +317,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; @@ -389,7 +393,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; @@ -405,7 +409,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], @@ -420,11 +425,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(); } } @@ -534,8 +539,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) { @@ -545,20 +549,24 @@ 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(); - } - - if (need_to_publish > 0) { - // ESC telem wrap around or bdshot update - publish_esc_status(); + // 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 (_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(); @@ -794,7 +802,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 4fc312c3e9..9b4d8cfb48 100644 --- a/src/drivers/dshot/DShot.h +++ b/src/drivers/dshot/DShot.h @@ -118,18 +118,14 @@ private: 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); @@ -141,14 +137,16 @@ private: 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; @@ -165,6 +163,8 @@ private: 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 0132a9b049..70992b1703 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 1164d1e200..8672e85f7f 100644 --- a/src/drivers/dshot/DShotTelemetry.h +++ b/src/drivers/dshot/DShotTelemetry.h @@ -64,14 +64,12 @@ public: 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 @@ private: */ int setBaudrate(unsigned baud); - void requestNextMotor(); + void requestNextMotor(int num_motors); /** * Decode a single byte from an ESC feedback frame @@ -126,7 +124,6 @@ private: 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}; diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 85dd467c33..7949b95509 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -61,7 +61,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);