From 92769bd2b299e5369e4265fb6804cbc1c7af7a79 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Tue, 18 Jan 2022 11:49:00 +0100 Subject: [PATCH] dshot: remove BOARD_DSHOT_MOTOR_ASSIGNMENT & handle timer channel gaps --- .../cubepilot/cubeorange/src/board_config.h | 1 - .../cubepilot/cubeyellow/src/board_config.h | 1 - boards/holybro/durandal-v1/src/board_config.h | 1 - boards/holybro/pix32v5/src/board_config.h | 1 - boards/modalai/fc-v1/src/board_config.h | 1 - boards/modalai/fc-v2/src/board_config.h | 1 - .../mro/ctrl-zero-h7-oem/src/board_config.h | 1 - boards/mro/ctrl-zero-h7/src/board_config.h | 1 - boards/mro/pixracerpro/src/board_config.h | 1 - boards/omnibus/f4sd/src/board_config.h | 1 - boards/px4/fmu-v2/src/board_config.h | 1 - boards/px4/fmu-v3/src/board_config.h | 1 - boards/px4/fmu-v4/src/board_config.h | 1 - boards/px4/fmu-v5/src/board_config.h | 1 - boards/px4/fmu-v5x/src/board_config.h | 1 - boards/px4/fmu-v6u/src/board_config.h | 1 - boards/px4/fmu-v6x/src/board_config.h | 1 - boards/spracing/h7extreme/src/board_config.h | 1 - boards/uvify/core/src/board_config.h | 1 - .../include/px4_platform/io_timer_init.h | 14 ++++++++++++ .../px4/nxp/imxrt/include/px4_arch/io_timer.h | 3 +++ .../nxp/kinetis/include/px4_arch/io_timer.h | 2 ++ .../nxp/s32k1xx/include/px4_arch/io_timer.h | 2 ++ .../rpi_common/include/px4_arch/io_timer.h | 2 ++ .../src/px4/stm/stm32_common/dshot/dshot.c | 22 +++++++------------ .../stm32_common/include/px4_arch/io_timer.h | 2 ++ 26 files changed, 33 insertions(+), 33 deletions(-) diff --git a/boards/cubepilot/cubeorange/src/board_config.h b/boards/cubepilot/cubeorange/src/board_config.h index 15e60d4400..b5e19ea752 100644 --- a/boards/cubepilot/cubeorange/src/board_config.h +++ b/boards/cubepilot/cubeorange/src/board_config.h @@ -149,7 +149,6 @@ #define BOARD_HAS_STATIC_MANIFEST 1 -#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5}; #define BOARD_ENABLE_CONSOLE_BUFFER diff --git a/boards/cubepilot/cubeyellow/src/board_config.h b/boards/cubepilot/cubeyellow/src/board_config.h index dfa27265c0..53fff3ac6e 100644 --- a/boards/cubepilot/cubeyellow/src/board_config.h +++ b/boards/cubepilot/cubeyellow/src/board_config.h @@ -144,7 +144,6 @@ #define BOARD_HAS_STATIC_MANIFEST 1 -#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5}; #define BOARD_ENABLE_CONSOLE_BUFFER diff --git a/boards/holybro/durandal-v1/src/board_config.h b/boards/holybro/durandal-v1/src/board_config.h index f18a2591e1..55a3b52238 100644 --- a/boards/holybro/durandal-v1/src/board_config.h +++ b/boards/holybro/durandal-v1/src/board_config.h @@ -181,7 +181,6 @@ #define BOARD_NUM_IO_TIMERS 4 -#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5, 6, 7, 9, 8}; /* Power supply control and monitoring GPIOs */ diff --git a/boards/holybro/pix32v5/src/board_config.h b/boards/holybro/pix32v5/src/board_config.h index 879a7ad168..61adf5d679 100644 --- a/boards/holybro/pix32v5/src/board_config.h +++ b/boards/holybro/pix32v5/src/board_config.h @@ -430,7 +430,6 @@ #define BOARD_NUM_IO_TIMERS 5 -#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5, 6, 7, 8, 9, 10}; __BEGIN_DECLS diff --git a/boards/modalai/fc-v1/src/board_config.h b/boards/modalai/fc-v1/src/board_config.h index 91411b9398..11e46d6586 100644 --- a/boards/modalai/fc-v1/src/board_config.h +++ b/boards/modalai/fc-v1/src/board_config.h @@ -262,7 +262,6 @@ #define BOARD_NUM_IO_TIMERS 5 -#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5, 6, 7}; __BEGIN_DECLS diff --git a/boards/modalai/fc-v2/src/board_config.h b/boards/modalai/fc-v2/src/board_config.h index caadee90b7..26f22e4b73 100644 --- a/boards/modalai/fc-v2/src/board_config.h +++ b/boards/modalai/fc-v2/src/board_config.h @@ -199,7 +199,6 @@ */ #define DIRECT_PWM_OUTPUT_CHANNELS 8 -#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5, 6, 7}; /* Power supply control and monitoring GPIOs */ #define GPIO_nVDD_USB_VALID /* PF13 */ (GPIO_INPUT |GPIO_FLOAT|GPIO_PORTF|GPIO_PIN13) /* Low for USB power, High for DC power */ diff --git a/boards/mro/ctrl-zero-h7-oem/src/board_config.h b/boards/mro/ctrl-zero-h7-oem/src/board_config.h index 7978f97427..c573b6522e 100644 --- a/boards/mro/ctrl-zero-h7-oem/src/board_config.h +++ b/boards/mro/ctrl-zero-h7-oem/src/board_config.h @@ -157,7 +157,6 @@ #define BOARD_NUM_IO_TIMERS 3 -#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5, 6, 7}; #define BOARD_ENABLE_CONSOLE_BUFFER diff --git a/boards/mro/ctrl-zero-h7/src/board_config.h b/boards/mro/ctrl-zero-h7/src/board_config.h index 15e6183d1a..cd8f5921ef 100644 --- a/boards/mro/ctrl-zero-h7/src/board_config.h +++ b/boards/mro/ctrl-zero-h7/src/board_config.h @@ -157,7 +157,6 @@ #define BOARD_NUM_IO_TIMERS 3 -#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5, 6, 7}; #define BOARD_ENABLE_CONSOLE_BUFFER diff --git a/boards/mro/pixracerpro/src/board_config.h b/boards/mro/pixracerpro/src/board_config.h index d2aeacf48a..95f5de5288 100644 --- a/boards/mro/pixracerpro/src/board_config.h +++ b/boards/mro/pixracerpro/src/board_config.h @@ -146,7 +146,6 @@ #define BOARD_ADC_BRICK_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK1_VALID)) #define BOARD_NUM_IO_TIMERS 3 -#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5, 6, 7}; #define BOARD_DMA_ALLOC_POOL_SIZE 5120 /* This board provides a DMA pool and APIs */ #define BOARD_HAS_ON_RESET 1 /* This board provides the board_on_reset interface */ #define BOARD_ENABLE_CONSOLE_BUFFER diff --git a/boards/omnibus/f4sd/src/board_config.h b/boards/omnibus/f4sd/src/board_config.h index 78c89478ce..627a42e9c0 100644 --- a/boards/omnibus/f4sd/src/board_config.h +++ b/boards/omnibus/f4sd/src/board_config.h @@ -147,7 +147,6 @@ #define BOARD_ENABLE_CONSOLE_BUFFER #define BOARD_CONSOLE_BUFFER_SIZE (1024*3) -#define BOARD_DSHOT_MOTOR_ASSIGNMENT {0, 1, 3, 2}; __BEGIN_DECLS diff --git a/boards/px4/fmu-v2/src/board_config.h b/boards/px4/fmu-v2/src/board_config.h index f3b462e42c..643e175c7a 100644 --- a/boards/px4/fmu-v2/src/board_config.h +++ b/boards/px4/fmu-v2/src/board_config.h @@ -158,7 +158,6 @@ #define BOARD_HAS_ON_RESET 1 -#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5}; __BEGIN_DECLS diff --git a/boards/px4/fmu-v3/src/board_config.h b/boards/px4/fmu-v3/src/board_config.h index c986ecb714..3c12d70e13 100644 --- a/boards/px4/fmu-v3/src/board_config.h +++ b/boards/px4/fmu-v3/src/board_config.h @@ -158,7 +158,6 @@ #define BOARD_HAS_ON_RESET 1 -#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5}; /* Internal IMU Heater * diff --git a/boards/px4/fmu-v4/src/board_config.h b/boards/px4/fmu-v4/src/board_config.h index 99b238d011..ef6571d84d 100644 --- a/boards/px4/fmu-v4/src/board_config.h +++ b/boards/px4/fmu-v4/src/board_config.h @@ -179,7 +179,6 @@ #define BOARD_HAS_ON_RESET 1 -#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5}; __BEGIN_DECLS diff --git a/boards/px4/fmu-v5/src/board_config.h b/boards/px4/fmu-v5/src/board_config.h index c6eedce45a..56e4870fbd 100644 --- a/boards/px4/fmu-v5/src/board_config.h +++ b/boards/px4/fmu-v5/src/board_config.h @@ -433,7 +433,6 @@ #define BOARD_NUM_IO_TIMERS 5 -#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5, 6, 7, 8, 9, 10}; __BEGIN_DECLS diff --git a/boards/px4/fmu-v5x/src/board_config.h b/boards/px4/fmu-v5x/src/board_config.h index b5fba07d0d..05a0d54477 100644 --- a/boards/px4/fmu-v5x/src/board_config.h +++ b/boards/px4/fmu-v5x/src/board_config.h @@ -218,7 +218,6 @@ */ #define DIRECT_PWM_OUTPUT_CHANNELS 9 -#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5, 6, 7, 8}; /* Power supply control and monitoring GPIOs */ diff --git a/boards/px4/fmu-v6u/src/board_config.h b/boards/px4/fmu-v6u/src/board_config.h index 399a1a881f..890986ea0e 100644 --- a/boards/px4/fmu-v6u/src/board_config.h +++ b/boards/px4/fmu-v6u/src/board_config.h @@ -196,7 +196,6 @@ */ #define DIRECT_PWM_OUTPUT_CHANNELS 9 -#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5, 6, 7, 8}; /* Power supply control and monitoring GPIOs */ diff --git a/boards/px4/fmu-v6x/src/board_config.h b/boards/px4/fmu-v6x/src/board_config.h index f2966995bf..9291fcdfa9 100644 --- a/boards/px4/fmu-v6x/src/board_config.h +++ b/boards/px4/fmu-v6x/src/board_config.h @@ -236,7 +236,6 @@ */ #define DIRECT_PWM_OUTPUT_CHANNELS 9 -#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5, 6, 7, 8}; /* Power supply control and monitoring GPIOs */ diff --git a/boards/spracing/h7extreme/src/board_config.h b/boards/spracing/h7extreme/src/board_config.h index 570d6e8785..9c1ea8e431 100644 --- a/boards/spracing/h7extreme/src/board_config.h +++ b/boards/spracing/h7extreme/src/board_config.h @@ -133,7 +133,6 @@ #define BOARD_NUM_IO_TIMERS 3 -#define BOARD_DSHOT_MOTOR_ASSIGNMENT {1, 2, 3, 0, 4, 5, 6, 7}; __BEGIN_DECLS diff --git a/boards/uvify/core/src/board_config.h b/boards/uvify/core/src/board_config.h index d10535d5e6..22708b23e2 100644 --- a/boards/uvify/core/src/board_config.h +++ b/boards/uvify/core/src/board_config.h @@ -178,7 +178,6 @@ #define BOARD_HAS_ON_RESET 1 -#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5}; #define BOARD_ENABLE_CONSOLE_BUFFER #define BOARD_CONSOLE_BUFFER_SIZE (1024*3) diff --git a/platforms/nuttx/src/px4/common/include/px4_platform/io_timer_init.h b/platforms/nuttx/src/px4/common/include/px4_platform/io_timer_init.h index eafe54654b..f348b6017e 100644 --- a/platforms/nuttx/src/px4/common/include/px4_platform/io_timer_init.h +++ b/platforms/nuttx/src/px4/common/include/px4_platform/io_timer_init.h @@ -58,6 +58,8 @@ static inline constexpr io_timers_channel_mapping_t initIOTimerChannelMapping(co } uint32_t first_channel = UINT32_MAX; + uint32_t min_timer_channel = UINT32_MAX; + uint32_t max_timer_channel = 0; uint32_t channel_count = 0; for (uint32_t channel = 0; channel < MAX_TIMER_IO_CHANNELS; ++channel) { @@ -74,11 +76,23 @@ static inline constexpr io_timers_channel_mapping_t initIOTimerChannelMapping(co } ++channel_count; + + if (timer_io_channels_conf[channel].timer_channel < min_timer_channel) { + min_timer_channel = timer_io_channels_conf[channel].timer_channel; + } + + if (timer_io_channels_conf[channel].timer_channel > max_timer_channel) { + max_timer_channel = timer_io_channels_conf[channel].timer_channel; + } } } if (first_channel == UINT32_MAX) { //unused timer, channel_count is 0 first_channel = 0; + + } else { + ret.element[i].lowest_timer_channel = min_timer_channel; + ret.element[i].channel_count_including_gaps = max_timer_channel - min_timer_channel + 1; } ret.element[i].first_channel_index = first_channel; diff --git a/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/io_timer.h index f54bc9bc0a..3efb8101b0 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/io_timer.h @@ -86,6 +86,8 @@ typedef struct io_timers_t { typedef struct io_timers_channel_mapping_element_t { uint32_t first_channel_index; uint32_t channel_count; + uint32_t lowest_timer_channel; + uint32_t channel_count_including_gaps; } io_timers_channel_mapping_element_t; /* mapping for each io_timers to timer_io_channels */ @@ -102,6 +104,7 @@ typedef struct timer_io_channels_t { uint8_t val_offset; /* IMXRT_FLEXPWM_SM0VAL3_OFFSET or IMXRT_FLEXPWM_SM0VAL5_OFFSET */ uint8_t sub_module; /* 0 based sub module offset */ uint8_t sub_module_bits; /* LDOK and CLDOK bits */ + uint8_t timer_channel; /* Unused */ } timer_io_channels_t; #define SM0 0 diff --git a/platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/io_timer.h index 8d24a75e3f..f6cd7427be 100644 --- a/platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/io_timer.h @@ -91,6 +91,8 @@ typedef struct io_timers_t { typedef struct io_timers_channel_mapping_element_t { uint32_t first_channel_index; uint32_t channel_count; + uint32_t lowest_timer_channel; + uint32_t channel_count_including_gaps; } io_timers_channel_mapping_element_t; /* mapping for each io_timers to timer_io_channels */ diff --git a/platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/io_timer.h index bc31b0bfd5..71a026b5c5 100644 --- a/platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/io_timer.h @@ -87,6 +87,8 @@ typedef struct io_timers_t { typedef struct io_timers_channel_mapping_element_t { uint32_t first_channel_index; uint32_t channel_count; + uint32_t lowest_timer_channel; + uint32_t channel_count_including_gaps; } io_timers_channel_mapping_element_t; /* mapping for each io_timers to timer_io_channels */ 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 e5901703a2..c9fbedd8f0 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 @@ -97,6 +97,8 @@ typedef struct io_timers_t { typedef struct io_timers_channel_mapping_element_t { uint32_t first_channel_index; uint32_t channel_count; + uint32_t lowest_timer_channel; + uint32_t channel_count_including_gaps; } io_timers_channel_mapping_element_t; /* mapping for each io_timers to timer_io_channels */ 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 00e60fe65a..d9b8d013b0 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -82,10 +82,6 @@ static uint8_t dshot_burst_buffer_array[DSHOT_TIMERS * DSHOT_BURST_BUFFER_SIZE(M px4_cache_aligned_data() = {}; static uint32_t *dshot_burst_buffer[DSHOT_TIMERS] = {}; -#ifdef BOARD_DSHOT_MOTOR_ASSIGNMENT -static const uint8_t motor_assignment[MOTORS_NUMBER] = BOARD_DSHOT_MOTOR_ASSIGNMENT; -#endif /* BOARD_DSHOT_MOTOR_ASSIGNMENT */ - int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq) { unsigned buffer_offset = 0; @@ -104,7 +100,7 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq) #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); + 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 @@ -140,9 +136,10 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq) for (uint8_t timer_index = 0; (timer_index < DSHOT_TIMERS) && (OK == ret_val); timer_index++) { if (true == dshot_handler[timer_index].init) { - dshot_handler[timer_index].dma_size = io_timers_channel_mapping.element[timer_index].channel_count * + 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); + io_timer_set_dshot_mode(timer_index, dshot_pwm_freq, + io_timers_channel_mapping.element[timer_index].channel_count_including_gaps); dshot_handler[timer_index].dma_handle = stm32_dmachannel(io_timers[timer_index].dshot.dmamap); @@ -164,7 +161,7 @@ void up_dshot_trigger(void) // 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)); + 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, @@ -192,10 +189,6 @@ void dshot_motor_data_set(unsigned motor_number, uint16_t throttle, bool telemet uint16_t packet = 0; uint16_t checksum = 0; -#ifdef BOARD_DSHOT_MOTOR_ASSIGNMENT - motor_number = motor_assignment[motor_number]; -#endif /* BOARD_DSHOT_MOTOR_ASSIGNMENT */ - packet |= throttle << DSHOT_THROTTLE_POSITION; packet |= ((uint16_t)telemetry & 0x01) << DSHOT_TELEMETRY_POSITION; @@ -213,8 +206,9 @@ void dshot_motor_data_set(unsigned motor_number, uint16_t throttle, bool telemet unsigned timer = timer_io_channels[motor_number].timer_index; uint32_t *buffer = dshot_burst_buffer[timer]; - unsigned num_motors = io_timers_channel_mapping.element[timer].channel_count; - unsigned timer_channel_index = motor_number - io_timers_channel_mapping.element[timer].first_channel_index; + 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; 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] = 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 cbf69d432a..3f5d6676b7 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 @@ -102,6 +102,8 @@ typedef struct io_timers_t { typedef struct io_timers_channel_mapping_element_t { uint32_t first_channel_index; uint32_t channel_count; + uint32_t lowest_timer_channel; + uint32_t channel_count_including_gaps; } io_timers_channel_mapping_element_t; /* mapping for each io_timers to timer_io_channels */