diff --git a/boards/px4/fmu-v6x/default.px4board b/boards/px4/fmu-v6x/default.px4board index 60bb22729c8f..881cc02d0878 100644 --- a/boards/px4/fmu-v6x/default.px4board +++ b/boards/px4/fmu-v6x/default.px4board @@ -32,7 +32,7 @@ CONFIG_DRIVERS_POWER_MONITOR_INA226=y CONFIG_DRIVERS_POWER_MONITOR_INA228=y CONFIG_DRIVERS_POWER_MONITOR_INA238=y CONFIG_DRIVERS_PWM_OUT=y -CONFIG_DRIVERS_PX4IO=y +#CONFIG_DRIVERS_PX4IO=y CONFIG_DRIVERS_RC_INPUT=y CONFIG_DRIVERS_SAFETY_BUTTON=y CONFIG_DRIVERS_TONE_ALARM=y diff --git a/boards/px4/fmu-v6x/init/rc.board_sensors b/boards/px4/fmu-v6x/init/rc.board_sensors index 826b26c2e607..c35ada1d475a 100644 --- a/boards/px4/fmu-v6x/init/rc.board_sensors +++ b/boards/px4/fmu-v6x/init/rc.board_sensors @@ -58,7 +58,7 @@ else fi # Internal SPI bus ICM42688p -icm42688p -R 6 -s start +#icm42688p -R 6 -s start # disabled for DMA for Dshot if ver hwtypecmp V6X000003 V6X001003 V6X003003 V6X000004 V6X001004 V6X004003 V6X004004 V6X005003 V6X005004 then diff --git a/boards/px4/fmu-v6x/nuttx-config/include/board_dma_map.h b/boards/px4/fmu-v6x/nuttx-config/include/board_dma_map.h index 1f45efc569d1..dc5318a3982d 100644 --- a/boards/px4/fmu-v6x/nuttx-config/include/board_dma_map.h +++ b/boards/px4/fmu-v6x/nuttx-config/include/board_dma_map.h @@ -39,8 +39,8 @@ #define DMAMAP_SPI1_RX DMAMAP_DMA12_SPI1RX_0 /* 1 DMA1:37 ICM-20649 */ #define DMAMAP_SPI1_TX DMAMAP_DMA12_SPI1TX_0 /* 2 DMA1:38 ICM-20649 */ -#define DMAMAP_SPI2_RX DMAMAP_DMA12_SPI2RX_0 /* 3 DMA1:39 ICM-42688-P */ -#define DMAMAP_SPI2_TX DMAMAP_DMA12_SPI2TX_0 /* 4 DMA1:40 ICM-42688-P */ +//#define DMAMAP_SPI2_RX DMAMAP_DMA12_SPI2RX_0 /* 3 DMA1:39 ICM-42688-P */ +//#define DMAMAP_SPI2_TX DMAMAP_DMA12_SPI2TX_0 /* 4 DMA1:40 ICM-42688-P */ //#define DMAMAP_USART1_RX DMAMAP_DMA12_USART1RX_0 /* DMA1:41 GPS1 */ //#define DMAMAP_USART1_TX DMAMAP_DMA12_USART1TX_0 /* DMA1:42 GPS1 */ @@ -48,19 +48,19 @@ //#define DMAMAP_USART2_RX DMAMAP_DMA12_USART2RX_0 /* DMA1:43 Telem3 */ //#define DMAMAP_USART2_TX DMAMAP_DMA12_USART2TX_0 /* DMA1:44 Telem3 */ -//#define DMAMAP_USART3_RX DMAMAP_DMA12_USART3RX_0 /* DMA1:45 DEBUG */ -//#define DMAMAP_USART3_TX DMAMAP_DMA12_USART3TX_0 /* DMA1:46 DEBUG */ +#define DMAMAP_USART3_RX DMAMAP_DMA12_USART3RX_0 /* DMA1:45 DEBUG */ +#define DMAMAP_USART3_TX DMAMAP_DMA12_USART3TX_0 /* DMA1:46 DEBUG */ //#define DMAMAP_UART4_RX DMAMAP_DMA12_UART4RX_0 /* DMA1:63 EXT2 */ //#define DMAMAP_UART4_TX DMAMAP_DMA12_UART4TX_0 /* DMA1:64 EXT2 */ -#define DMAMAP_USART6_RX DMAMAP_DMA12_USART6RX_0 /* 5 DMA1:71 PX4IO */ -#define DMAMAP_USART6_TX DMAMAP_DMA12_USART6TX_0 /* 6 DMA1:72 PX4IO */ +//#define DMAMAP_USART6_RX DMAMAP_DMA12_USART6RX_0 /* 5 DMA1:71 PX4IO --> not used */ +//#define DMAMAP_USART6_TX DMAMAP_DMA12_USART6TX_0 /* 6 DMA1:72 PX4IO --> not used */ // Assigned in timer_config.cpp -// Timer 4 /* 7 DMA1:32 TIM4UP */ -// Timer 5 /* 8 DMA1:50 TIM5UP */ +// Timer 4 /* 7 DMA1:32 TIM4UP --> uses up to 4 channels */ +// Timer 5 /* 8 DMA1:50 TIM5UP --> not configured */ // DMAMUX2 Using at most 8 Channels on DMA2 -------- Assigned // V diff --git a/boards/px4/fmu-v6x/nuttx-config/nsh/defconfig b/boards/px4/fmu-v6x/nuttx-config/nsh/defconfig index 30fa23a61afc..45c29ec1f39f 100644 --- a/boards/px4/fmu-v6x/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v6x/nuttx-config/nsh/defconfig @@ -254,8 +254,6 @@ CONFIG_STM32H7_SPI1=y CONFIG_STM32H7_SPI1_DMA=y CONFIG_STM32H7_SPI1_DMA_BUFFER=1024 CONFIG_STM32H7_SPI2=y -CONFIG_STM32H7_SPI2_DMA=y -CONFIG_STM32H7_SPI2_DMA_BUFFER=4096 CONFIG_STM32H7_SPI3=y CONFIG_STM32H7_SPI3_DMA=y CONFIG_STM32H7_SPI3_DMA_BUFFER=1024 diff --git a/boards/px4/fmu-v6x/src/timer_config.cpp b/boards/px4/fmu-v6x/src/timer_config.cpp index 928f4916f0b8..4340e5c37690 100644 --- a/boards/px4/fmu-v6x/src/timer_config.cpp +++ b/boards/px4/fmu-v6x/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::Timer4, DMA{DMA::Index1}), + initIOTimer(Timer::Timer4), initIOTimer(Timer::Timer12), initIOTimer(Timer::Timer1), initIOTimer(Timer::Timer2), 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 404e9975c8b4..6d436f34580a 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -90,13 +90,14 @@ 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] = {}; -static uint16_t dshot_capture_buffer[32] px4_cache_aligned_data() = {}; +static dshot_handler_t dshot_capture_handlers[4] = {}; +static uint16_t dshot_capture_buffers[4][32] px4_cache_aligned_data() = {}; static struct hrt_call _call; static void do_capture(DMA_HANDLE handle, uint8_t status, void *arg); static void process_capture_results(void *arg); -static unsigned calculate_period(void); +static unsigned calculate_period(unsigned channel); static int dshot_output_timer_init(unsigned channel); static uint32_t read_ok = 0; @@ -111,12 +112,16 @@ static int _timers_init_mask = 0; static int _channels_init_mask = 0; // We only support capture on the first timer (usually 4 channels) for now. -static uint32_t _motor_to_capture = 0; static uint32_t _erpms[4] = {}; static void(*_erpm_callback)(uint32_t[], size_t, void *) = NULL; static void *_erpm_callback_context = NULL; +// We use this state to make sure the steps are never executed out of order which +// can lead to segfaults if a dma_handle is suddenly NULL. +enum State {None, Trigger, Capture, Collect}; +static enum State _state = None; + uint8_t nibbles_from_mapped(uint8_t mapped) { switch (mapped) { @@ -174,7 +179,7 @@ uint8_t nibbles_from_mapped(uint8_t mapped) } } -unsigned calculate_period(void) +unsigned calculate_period(unsigned channel) { uint32_t value = 0; @@ -182,32 +187,38 @@ unsigned calculate_period(void) uint32_t high = 1; unsigned shifted = 0; - unsigned previous = 0; - for (unsigned i = 1; i < (32); ++i) { + for (unsigned i = 1; i < sizeof(dshot_capture_buffers[channel]) / sizeof(dshot_capture_buffers[channel][0]); ++i) { - // We can ignore the very first data point as it's the pulse before it starts. - if (i > 1) { + unsigned previous = dshot_capture_buffers[channel][i - 1]; - if (dshot_capture_buffer[i] == 0) { - // Once we get zeros we're through. - break; - } + // TODO: Doesn't cope with DShot frequency other than 600 + const uint32_t bits = (dshot_capture_buffers[channel][i] - previous + 5) / 20; - // TODO: Doesn't cope with DShot frequency other than 600 - const uint32_t bits = (dshot_capture_buffer[i] - previous + 5) / 20; + if (shifted == 0 && (bits > 5 || bits == 0)) { + // Too long or too short, lets ignore the beginning until it's usable. + continue; + } - for (unsigned bit = 0; bit < bits; ++bit) { - value = value << 1; - value |= high; - ++shifted; - } + if (dshot_capture_buffers[channel][i] == 0) { + // Once we get zeros we're through. + break; + } - // The next edge toggles. - high = !high; + if (bits > 10) { + // Something is not right, let's not waste cycles and get out of here. + ++read_fail_zero; + return 0; } - previous = dshot_capture_buffer[i]; + for (unsigned bit = 0; bit < bits; ++bit) { + value = value << 1; + value |= high; + ++shifted; + } + + // The next edge toggles. + high = !high; } if (shifted == 0) { @@ -344,6 +355,11 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq) void up_dshot_trigger(void) { + if (_state != None) { + return; + } + + _state = Trigger; for (unsigned channel = 0; channel < MAX_TIMER_IO_CHANNELS; channel++) { if (_channels_init_mask & (1 << channel)) { @@ -356,6 +372,9 @@ void up_dshot_trigger(void) } } + io_timer_set_enable(true, enable_bidirectional_dshot ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot, + IO_TIMER_ALL_MODES_CHANNELS); + for (unsigned timer = 0; timer < DSHOT_TIMERS; ++timer) { if (_timers_init_mask & (1 << timer)) { @@ -390,12 +409,11 @@ void up_dshot_trigger(void) io_timer_update_dma_req(timer, false); // Trigger DMA (DShot Outputs) stm32_dmastart(dshot_handler[timer].dma_handle, do_capture, NULL, false); + io_timer_update_dma_req(timer, true); } } - io_timer_set_enable(true, enable_bidirectional_dshot ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot, - IO_TIMER_ALL_MODES_CHANNELS); } void do_capture(DMA_HANDLE handle, uint8_t status, void *arg) @@ -404,6 +422,12 @@ void do_capture(DMA_HANDLE handle, uint8_t status, void *arg) (void)status; (void)arg; + if (_state != Trigger) { + return; + } + + _state = Capture; + for (unsigned timer = 0; timer < DSHOT_TIMERS; ++timer) { if (_timers_init_mask & (1 << timer)) { if (dshot_handler[timer].dma_handle != NULL) { @@ -412,54 +436,76 @@ void do_capture(DMA_HANDLE handle, uint8_t status, void *arg) dshot_handler[timer].dma_handle = NULL; } - // TODO: this doesn't scale to more than 4 motors yet - unsigned capture_channel = _motor_to_capture; + memset(dshot_capture_buffers, 0, sizeof(dshot_capture_buffers)); + up_clean_dcache((uintptr_t) dshot_capture_buffers, + (uintptr_t) dshot_capture_buffers + + sizeof(dshot_capture_buffers)); - dshot_handler[timer].dma_size = sizeof(dshot_capture_buffer); + io_timer_capture_update_dma_req(timer, false); - // TODO: We should probably do this at another level board specific. - // However, right now the dma handles are all hard-coded to the UP(date) source - // rather than the capture compare one. - unsigned timer_channel = timer_io_channels[capture_channel].timer_channel; + for (unsigned channel = 0; channel < 4; ++channel) { + dshot_capture_handlers[channel].dma_size = sizeof(dshot_capture_buffers[channel]); - switch (timer_channel) { - case 1: - dshot_handler[timer].dma_handle = stm32_dmachannel(DMAMAP_DMA12_TIM5CH1_0); - break; + // TODO: We should probably do this at another level board specific. + // However, right now the dma handles are all hard-coded to the UP(date) source + // rather than the capture compare one. + unsigned timer_channel = timer_io_channels[channel].timer_channel; - case 2: - dshot_handler[timer].dma_handle = stm32_dmachannel(DMAMAP_DMA12_TIM5CH2_0); - break; + switch (timer_channel) { + case 1: + dshot_capture_handlers[channel].dma_handle = stm32_dmachannel(DMAMAP_DMA12_TIM5CH1_0); + break; - case 3: - dshot_handler[timer].dma_handle = stm32_dmachannel(DMAMAP_DMA12_TIM5CH3_0); - break; + case 2: + dshot_capture_handlers[channel].dma_handle = stm32_dmachannel(DMAMAP_DMA12_TIM5CH2_0); + break; - case 4: - dshot_handler[timer].dma_handle = stm32_dmachannel(DMAMAP_DMA12_TIM5CH4_0); - break; - } + case 3: + dshot_capture_handlers[channel].dma_handle = stm32_dmachannel(DMAMAP_DMA12_TIM5CH3_0); + break; - memset(dshot_capture_buffer, 0, sizeof(dshot_capture_buffer)); - up_clean_dcache((uintptr_t) dshot_capture_buffer, - (uintptr_t) dshot_capture_buffer + - sizeof(dshot_capture_buffer)); + case 4: + dshot_capture_handlers[channel].dma_handle = stm32_dmachannel(DMAMAP_DMA12_TIM5CH4_0); + break; + } - px4_stm32_dmasetup(dshot_handler[timer].dma_handle, - io_timers[timer].base + STM32_GTIM_DMAR_OFFSET, - (uint32_t) dshot_capture_buffer, - sizeof(dshot_capture_buffer), - DSHOT_TELEMETRY_DMA_SCR); + io_timer_unallocate_channel(channel); - io_timer_unallocate_channel(capture_channel); - io_timer_channel_init(capture_channel, IOTimerChanMode_CaptureDMA, NULL, NULL); - io_timer_set_enable(true, IOTimerChanMode_CaptureDMA, 1 << capture_channel); + unsigned offset = 0; - up_input_capture_set(capture_channel, Both, 0, NULL, NULL); + switch (timer_io_channels[channel].timer_channel) { + case 1: + offset = STM32_GTIM_CCR1_OFFSET; + break; + + case 2: + offset = STM32_GTIM_CCR2_OFFSET; + break; + + case 3: + offset = STM32_GTIM_CCR3_OFFSET; + break; + + case 4: + offset = STM32_GTIM_CCR4_OFFSET; + break; + } + + px4_stm32_dmasetup(dshot_capture_handlers[channel].dma_handle, + io_timers[timer].base + offset, + (uint32_t) dshot_capture_buffers[channel], + sizeof(dshot_capture_buffers[channel]), + DSHOT_TELEMETRY_DMA_SCR); + + io_timer_channel_init(channel, IOTimerChanMode_CaptureDMA, NULL, NULL); + io_timer_set_enable(true, IOTimerChanMode_CaptureDMA, 1 << channel); + + up_input_capture_set(channel, Both, 0, NULL, NULL); + + io_timer_set_capture_mode(timer, _dshot_frequency, channel); + stm32_dmastart(dshot_capture_handlers[channel].dma_handle, NULL, NULL, false); + } - io_timer_capture_update_dma_req(timer, false); - io_timer_set_capture_mode(timer, _dshot_frequency, capture_channel); - stm32_dmastart(dshot_handler[timer].dma_handle, NULL, NULL, false); io_timer_capture_update_dma_req(timer, true); } } @@ -472,26 +518,38 @@ void process_capture_results(void *arg) { (void)arg; + if (_state != Capture) { + return; + } + + _state = Collect; + + + up_invalidate_dcache((uintptr_t)dshot_capture_buffers, + (uintptr_t)dshot_capture_buffers + + sizeof(dshot_capture_buffers)); + // In case DMA is still set up from the last capture, we clear that. for (unsigned timer = 0; timer < DSHOT_TIMERS; ++timer) { if (_timers_init_mask & (1 << timer)) { - if (dshot_handler[timer].dma_handle != NULL) { - stm32_dmastop(dshot_handler[timer].dma_handle); - stm32_dmafree(dshot_handler[timer].dma_handle); - dshot_handler[timer].dma_handle = NULL; + + for (unsigned channel = 0; channel < 4; ++channel) { + if (dshot_capture_handlers[channel].dma_handle != NULL) { + stm32_dmastop(dshot_capture_handlers[channel].dma_handle); + stm32_dmafree(dshot_capture_handlers[channel].dma_handle); + dshot_capture_handlers[channel].dma_handle = NULL; + } } } } - up_invalidate_dcache((uintptr_t)dshot_capture_buffer, - (uintptr_t)dshot_capture_buffer + - sizeof(dshot_capture_buffer)); - // TODO: fix order // TODO: convert from period to erpm - _erpms[_motor_to_capture] = calculate_period(); - for (unsigned channel = 0; channel < MAX_TIMER_IO_CHANNELS; channel++) { + for (unsigned channel = 0; channel < 4; channel++) { + + _erpms[channel] = calculate_period(channel); + if (_channels_init_mask & (1 << channel)) { io_timer_unallocate_channel(channel); io_timer_channel_init(channel, @@ -503,7 +561,7 @@ void process_capture_results(void *arg) _erpm_callback(_erpms, 4, _erpm_callback_context); } - _motor_to_capture = (_motor_to_capture + 1) % 4; + _state = None; } 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 427a23bf2e37..08240dd07ce0 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 @@ -594,6 +594,7 @@ int io_timer_set_capture_mode(uint8_t timer, unsigned dshot_pwm_freq, unsigned c rPSC(timer) = ((int)(io_timers[timer].clock_freq / (dshot_pwm_freq * 5 / 4)) / DSHOT_MOTOR_PWM_BIT_WIDTH) - 1; + rDCR(timer) = 0; switch (timer_io_channels[channel].timer_channel) { @@ -603,8 +604,6 @@ int io_timer_set_capture_mode(uint8_t timer, unsigned dshot_pwm_freq, unsigned c rCCMR1(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR1_CC1S_SHIFT); rCR1(timer) |= GTIM_CR1_CEN; rCCER(timer) |= (GTIM_CCER_CC1E | GTIM_CCER_CC1P | GTIM_CCER_CC1NP); -// We need to pass the offset of the register to read by DMA divided by 4. - rDCR(timer) = 0xD; // 0x34 / 4, offset for CC1 break; case 2: @@ -612,7 +611,6 @@ int io_timer_set_capture_mode(uint8_t timer, unsigned dshot_pwm_freq, unsigned c rCCMR1(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR1_CC2S_SHIFT); rCR1(timer) |= GTIM_CR1_CEN; rCCER(timer) |= (GTIM_CCER_CC2E | GTIM_CCER_CC2P | GTIM_CCER_CC2NP); - rDCR(timer) = 0xE; // 0x38 / 4, offset for CC2 break; case 3: @@ -620,7 +618,6 @@ int io_timer_set_capture_mode(uint8_t timer, unsigned dshot_pwm_freq, unsigned c rCCMR2(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR2_CC3S_SHIFT); rCR1(timer) |= GTIM_CR1_CEN; rCCER(timer) |= (GTIM_CCER_CC3E | GTIM_CCER_CC3P | GTIM_CCER_CC3NP); - rDCR(timer) = 0xF; // 0x3C / 4, offset for CC3 break; case 4: @@ -628,7 +625,6 @@ int io_timer_set_capture_mode(uint8_t timer, unsigned dshot_pwm_freq, unsigned c rCCMR2(timer) |= (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR2_CC4S_SHIFT); rCR1(timer) |= GTIM_CR1_CEN; rCCER(timer) |= (GTIM_CCER_CC4E | GTIM_CCER_CC4P | GTIM_CCER_CC4NP); - rDCR(timer) = 0x10; // 0x40 / 4, offset for CC4 break; }