From 3a413a70d69961fb3cfc933222f2399cdfaf2252 Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Fri, 2 May 2025 12:33:53 +0200 Subject: [PATCH] feat(radio): improve gimbal detection (#6161) --- .../boards/generic_stm32/analog_inputs.cpp | 34 +++++-- .../src/boards/generic_stm32/analog_inputs.h | 2 +- radio/src/cli.cpp | 2 +- .../common/arm/stm32/flysky_gimbal_driver.cpp | 24 ++++- .../common/arm/stm32/flysky_gimbal_driver.h | 4 +- .../common/arm/stm32/sticks_pwm_driver.cpp | 89 ++++++++++--------- .../common/arm/stm32/sticks_pwm_driver.h | 1 + radio/src/targets/horus/board.cpp | 22 ++--- radio/src/targets/horus/hal.h | 1 + radio/src/targets/pl18/board.cpp | 12 +-- radio/src/targets/taranis/board.cpp | 10 +-- radio/src/targets/taranis/hal.h | 4 +- 12 files changed, 114 insertions(+), 91 deletions(-) diff --git a/radio/src/boards/generic_stm32/analog_inputs.cpp b/radio/src/boards/generic_stm32/analog_inputs.cpp index 2c0c5053c..bfd99db1e 100644 --- a/radio/src/boards/generic_stm32/analog_inputs.cpp +++ b/radio/src/boards/generic_stm32/analog_inputs.cpp @@ -23,6 +23,7 @@ #include "stm32_adc.h" #include "stm32_spi_adc.h" #include "sticks_pwm_driver.h" +#include "flysky_gimbal_driver.h" #include "hal.h" @@ -99,27 +100,44 @@ static const stick_pwm_timer_t _sticks_timer = { .GPIO_Alternate = PWM_GPIO_AF, .TIMx = PWM_TIMER, .TIM_IRQn = PWM_IRQn, + .TIM_Freq = PWM_TIMER_FREQ, }; #if !defined(PWM_IRQHandler) #error "Missing PWM_IRQHandler" #endif +static_assert(DIM(_PWM_inputs) <= MAX_STICKS, "too many PWM inputs"); + extern "C" void PWM_IRQHandler(void) { sticks_pwm_isr(&_sticks_timer, _PWM_inputs, DIM(_PWM_inputs)); } -bool sticksPwmDetect() +static bool pwm_gimbal_init() { return sticks_pwm_detect(&_sticks_timer, _PWM_inputs, DIM(_PWM_inputs)); } -#else - -bool sticksPwmDetect() -{ - return false; -} - #endif + +typedef bool (*gimbal_driver_t)(); + +const gimbal_driver_t gimbal_drivers[] = { +#if defined(PWM_STICKS) + pwm_gimbal_init, +#endif +#if defined(FLYSKY_GIMBAL) + flysky_gimbal_init, +#endif +}; + +void gimbalsDetect() +{ + unsigned idx = 0; + bool detected = false; + + while (idx < DIM(gimbal_drivers) && !detected) { + detected = gimbal_drivers[idx++](); + } +} diff --git a/radio/src/boards/generic_stm32/analog_inputs.h b/radio/src/boards/generic_stm32/analog_inputs.h index 811f69dfa..37d22ba22 100644 --- a/radio/src/boards/generic_stm32/analog_inputs.h +++ b/radio/src/boards/generic_stm32/analog_inputs.h @@ -23,4 +23,4 @@ extern const etx_hal_adc_driver_t _adc_driver; -bool sticksPwmDetect(); +void gimbalsDetect(); diff --git a/radio/src/cli.cpp b/radio/src/cli.cpp index 108b9dc9b..67500952f 100644 --- a/radio/src/cli.cpp +++ b/radio/src/cli.cpp @@ -1115,7 +1115,7 @@ static void spGimbalDeInit(int port_n) { (void)port_n; _sp_drv->deinit(_sp_ctx); - flysky_gimbal_init(true); + flysky_gimbal_force_init(); } #endif diff --git a/radio/src/targets/common/arm/stm32/flysky_gimbal_driver.cpp b/radio/src/targets/common/arm/stm32/flysky_gimbal_driver.cpp index e1443a884..738abc09c 100644 --- a/radio/src/targets/common/arm/stm32/flysky_gimbal_driver.cpp +++ b/radio/src/targets/common/arm/stm32/flysky_gimbal_driver.cpp @@ -168,7 +168,7 @@ void flysky_gimbal_deinit() _fs_usart_ctx = 0; } -bool flysky_gimbal_init(bool force) +static int flysky_gimbal_init_uart() { etx_serial_init cfg = { .baudrate = FLYSKY_HALL_BAUDRATE, @@ -177,11 +177,21 @@ bool flysky_gimbal_init(bool force) .polarity = ETX_Pol_Normal, }; - _fs_gimbal_detected = force; _fs_usart_ctx = STM32SerialDriver.init(REF_STM32_SERIAL_PORT(FSGimbal), &cfg); + if (!_fs_usart_ctx) return -1; + STM32SerialDriver.setIdleCb(_fs_usart_ctx, flysky_gimbal_loop, 0); + return 0; +} + +bool flysky_gimbal_init() +{ + if (flysky_gimbal_init_uart() != 0) return false; + + // Wait 70ms for FlySky gimbals to respond. According to LA trace, minimally + // 23ms is required + _fs_gimbal_detected = false; - // Wait 70ms for FlySky gimbals to respond. According to LA trace, minimally 23ms is required for (uint8_t i = 0; i < 70; i++) { delay_ms(1); if (_fs_gimbal_detected) { @@ -195,6 +205,14 @@ bool flysky_gimbal_init(bool force) return false; } +void flysky_gimbal_force_init() +{ + if (flysky_gimbal_init_uart() != 0) return; + + _fs_gimbal_detected = true; + stm32_hal_set_inputs_mask(0xF); +} + const etx_serial_port_t* flysky_gimbal_get_port() { return &_fs_gimbal_serial_port; diff --git a/radio/src/targets/common/arm/stm32/flysky_gimbal_driver.h b/radio/src/targets/common/arm/stm32/flysky_gimbal_driver.h index 9c6c18456..1f920aac9 100644 --- a/radio/src/targets/common/arm/stm32/flysky_gimbal_driver.h +++ b/radio/src/targets/common/arm/stm32/flysky_gimbal_driver.h @@ -128,7 +128,9 @@ extern signed short hall_raw_values[FLYSKY_HALL_CHANNEL_COUNT]; extern unsigned short hall_adc_values[FLYSKY_HALL_CHANNEL_COUNT]; // returns true if the gimbals were detected properly -bool flysky_gimbal_init(bool force = false); +bool flysky_gimbal_init(); + +void flysky_gimbal_force_init(); void flysky_gimbal_deinit(); const etx_serial_port_t* flysky_gimbal_get_port(); diff --git a/radio/src/targets/common/arm/stm32/sticks_pwm_driver.cpp b/radio/src/targets/common/arm/stm32/sticks_pwm_driver.cpp index 87f955093..69acddc70 100644 --- a/radio/src/targets/common/arm/stm32/sticks_pwm_driver.cpp +++ b/radio/src/targets/common/arm/stm32/sticks_pwm_driver.cpp @@ -20,15 +20,28 @@ */ #include "sticks_pwm_driver.h" + #include "stm32_gpio_driver.h" #include "stm32_timer.h" +#include "timers_driver.h" + #include "hal/adc_driver.h" -#include "stm32_hal_ll.h" -#include "delays_driver.h" #include "dataconstants.h" -static volatile uint32_t _pwm_interrupt_count; +#include + +#define PWM_DETECT_MS 20 +#define PWM_MIN_SAMPLES 3 + +// we assume the sensors output ~250Hz +#define PWM_MIN_PERIOD 3000 +#define PWM_MAX_PERIOD 5000 + +#define PWM_PRESCALER_FREQ 1038000 + +// detected PWM samples per input +static uint8_t _pwm_samples[MAX_STICKS]; static void sticks_pwm_init(const stick_pwm_timer_t* tim) { @@ -47,7 +60,7 @@ static void sticks_pwm_init(const stick_pwm_timer_t* tim) stm32_timer_enable_clock(TIMx); TIMx->CR1 &= ~TIM_CR1_CEN; // Stop timer - TIMx->PSC = 80; + TIMx->PSC = __LL_TIM_CALC_PSC(tim->TIM_Freq, PWM_PRESCALER_FREQ); TIMx->ARR = 0xffff; TIMx->CCMR1 = TIM_CCMR1_CC1S_0 | TIM_CCMR1_CC2S_0; TIMx->CCMR2 = TIM_CCMR2_CC3S_0 | TIM_CCMR2_CC4S_0; @@ -77,22 +90,27 @@ static void sticks_pwm_deinit(const stick_pwm_timer_t* tim) } bool sticks_pwm_detect(const stick_pwm_timer_t* timer, - const stick_pwm_input_t* inputs, - uint8_t n_inputs) + const stick_pwm_input_t* inputs, uint8_t n_inputs) { if (!timer || !inputs || n_inputs == 0) return false; - _pwm_interrupt_count = 0; + memset(&_pwm_samples, 0, sizeof(_pwm_samples)); sticks_pwm_init(timer); - delay_ms(20); - if (_pwm_interrupt_count < 32) { - sticks_pwm_deinit(timer); - return false; + uint32_t timeout = timersGetMsTick() + PWM_DETECT_MS; + while (timersGetMsTick() - timeout > 0) {} + + bool valid_input = true; + for (unsigned i = 0; i < n_inputs; i++) { + valid_input = valid_input && (_pwm_samples[i] >= PWM_MIN_SAMPLES); } - return true; + if (!valid_input) { + sticks_pwm_deinit(timer); + } + + return valid_input; } static inline uint32_t tim_get_capture(TIM_TypeDef *TIMx, uint8_t channel) @@ -101,6 +119,11 @@ static inline uint32_t tim_get_capture(TIM_TypeDef *TIMx, uint8_t channel) return READ_REG(*(base_CCRx + channel)); } +static inline bool tim_polarity_is_falling(TIM_TypeDef *TIMx, uint8_t channel) +{ + return READ_BIT(TIMx->CCER, TIM_CCER_CC1P << (channel * 4)); +} + static inline void tim_set_polarity_rising(TIM_TypeDef *TIMx, uint8_t channel) { CLEAR_BIT(TIMx->CCER, TIM_CCER_CC1P << (channel * 4)); @@ -121,54 +144,36 @@ static inline void tim_clear_flag_CCx(TIM_TypeDef *TIMx, uint8_t channel) CLEAR_BIT(TIMx->SR, TIM_SR_CC1IF << channel); } -static inline uint32_t diff_with_16bits_overflow(uint32_t a, uint32_t b) -{ - if (b > a) { - return b - a; - } - - return b + 0xffff - a; -} - void sticks_pwm_isr(const stick_pwm_timer_t* tim, - const stick_pwm_input_t* inputs, - uint8_t n_inputs) + const stick_pwm_input_t* inputs, uint8_t n_inputs) { - static uint8_t timer_capture_states[MAX_STICKS]; static uint32_t timer_capture_rising_time[MAX_STICKS]; auto TIMx = tim->TIMx; auto adcValues = getAnalogValues(); for (uint8_t i = 0; i < n_inputs; i++) { - const auto& input = inputs[i]; auto channel = input.channel; - - if (tim_is_active_flag_CCx(TIMx, channel)) { - uint32_t capture = tim_get_capture(TIMx, channel); + if (tim_is_active_flag_CCx(TIMx, channel)) { + uint16_t capture = (uint16_t)tim_get_capture(TIMx, channel); tim_clear_flag_CCx(TIMx, channel); - // overflow may happen but we only use this to detect PWM / ADC on radio startup - _pwm_interrupt_count++; - - if (timer_capture_states[i] != 0) { - uint32_t value = diff_with_16bits_overflow(timer_capture_rising_time[i], capture); - + if (tim_polarity_is_falling(TIMx, channel)) { + uint16_t value = capture - timer_capture_rising_time[i]; if (value <= ADC_MAX_VALUE) { - uint16_t v16 = (uint16_t)value; - adcValues[i] = input.inverted ? ADC_INVERT_VALUE(v16) : v16; + adcValues[i] = input.inverted ? ADC_INVERT_VALUE(value) : value; + _pwm_samples[i]++; } - tim_set_polarity_rising(TIMx, channel); - timer_capture_states[i] = 0; - } - else { + } else { + uint16_t period = capture - timer_capture_rising_time[i]; + if (period < PWM_MIN_PERIOD || period > PWM_MAX_PERIOD) { + _pwm_samples[i] = 0; + } timer_capture_rising_time[i] = capture; - tim_set_polarity_falling(TIMx, channel); - timer_capture_states[i] = 0x80; } } } diff --git a/radio/src/targets/common/arm/stm32/sticks_pwm_driver.h b/radio/src/targets/common/arm/stm32/sticks_pwm_driver.h index 6a520500a..ea3dab77c 100644 --- a/radio/src/targets/common/arm/stm32/sticks_pwm_driver.h +++ b/radio/src/targets/common/arm/stm32/sticks_pwm_driver.h @@ -37,6 +37,7 @@ struct stick_pwm_timer_t { TIM_TypeDef* TIMx; IRQn_Type TIM_IRQn; + uint32_t TIM_Freq; }; // returns 'true' if PWM sticks have been detected diff --git a/radio/src/targets/horus/board.cpp b/radio/src/targets/horus/board.cpp index acd93ef5b..aa4ad26a8 100644 --- a/radio/src/targets/horus/board.cpp +++ b/radio/src/targets/horus/board.cpp @@ -49,10 +49,6 @@ #include -#if defined(FLYSKY_GIMBAL) - #include "flysky_gimbal_driver.h" -#endif - #if defined(CSD203_SENSOR) #include "csd203_sensor.h" #endif @@ -183,8 +179,9 @@ void boardInit() board_trainer_init(); pwrOn(); - delaysInit(); + delaysInit(); + timersInit(); __enable_irq(); TRACE("\nHorus board started :)"); @@ -194,21 +191,12 @@ void boardInit() keysInit(); switchInit(); rotaryEncoderInit(); + gimbalsDetect(); -#if defined(FLYSKY_GIMBAL) && defined(PWM_STICKS) - if (!flysky_gimbal_init()) { - sticksPwmDetect(); - } -#elif defined(FLYSKY_GIMBAL) - flysky_gimbal_init(); -#elif defined(PWM_STICKS) - sticksPwmDetect(); -#endif - - if (!adcInit(&_adc_driver)) + if (!adcInit(&_adc_driver)) { TRACE("adcInit failed"); + } - timersInit(); #if defined(HARDWARE_TOUCH) && !defined(SIMU) touchPanelInit(); diff --git a/radio/src/targets/horus/hal.h b/radio/src/targets/horus/hal.h index 2a1e0055f..d98bc7691 100644 --- a/radio/src/targets/horus/hal.h +++ b/radio/src/targets/horus/hal.h @@ -480,6 +480,7 @@ #if !(defined(RADIO_TX16S) || defined(RADIO_F16) || defined(RADIO_T15) || defined(RADIO_V16)) #define PWM_STICKS #define PWM_TIMER TIM5 + #define PWM_TIMER_FREQ (PERI1_FREQUENCY * TIMER_MULT_APB1) #define PWM_GPIO GPIOA #define PWM_GPIO_AF GPIO_AF2 #define PWM_IRQHandler TIM5_IRQHandler diff --git a/radio/src/targets/pl18/board.cpp b/radio/src/targets/pl18/board.cpp index d72721281..f8f6fccb6 100644 --- a/radio/src/targets/pl18/board.cpp +++ b/radio/src/targets/pl18/board.cpp @@ -34,6 +34,7 @@ #include "hal/rotary_encoder.h" #include "board.h" +#include "boards/generic_stm32/analog_inputs.h" #include "boards/generic_stm32/module_ports.h" #include "boards/generic_stm32/rgb_leds.h" @@ -46,11 +47,7 @@ #include "vs1053b.h" #endif -#if defined(FLYSKY_GIMBAL) - #include "flysky_gimbal_driver.h" -#endif #include "timers_driver.h" - #include "battery_driver.h" #include "touch_driver.h" @@ -224,11 +221,8 @@ void boardInit() board_trainer_init(); battery_charge_init(); - - #if defined(FLYSKY_GIMBAL) - flysky_gimbal_init(); - #endif - + + gimbalsDetect(); timersInit(); touchPanelInit(); usbInit(); diff --git a/radio/src/targets/taranis/board.cpp b/radio/src/targets/taranis/board.cpp index 871aafc7e..f6791914e 100644 --- a/radio/src/targets/taranis/board.cpp +++ b/radio/src/targets/taranis/board.cpp @@ -214,6 +214,7 @@ void boardInit() #endif delaysInit(); + timersInit(); __enable_irq(); keysInit(); @@ -223,20 +224,13 @@ void boardInit() rotaryEncoderInit(); #endif -#if defined(PWM_STICKS) - sticksPwmDetect(); -#endif - -#if defined(FLYSKY_GIMBAL) - flysky_gimbal_init(); -#endif + gimbalsDetect(); if (!adcInit(&_adc_driver)) TRACE("adcInit failed"); lcdInit(); // delaysInit() must be called before audioInit(); - timersInit(); usbInit(); #if defined(LED_STRIP_GPIO) diff --git a/radio/src/targets/taranis/hal.h b/radio/src/targets/taranis/hal.h index 7b9624f75..4638863a0 100644 --- a/radio/src/targets/taranis/hal.h +++ b/radio/src/targets/taranis/hal.h @@ -1417,6 +1417,7 @@ #define ADC_GPIO_PIN_BATT LL_GPIO_PIN_0 // PC.00 #define PWM_STICKS #define PWM_TIMER TIM5 + #define PWM_TIMER_FREQ (PERI1_FREQUENCY * TIMER_MULT_APB1) #define PWM_GPIO GPIOA #define PWM_GPIO_AF GPIO_AF2 #define PWM_IRQHandler TIM5_IRQHandler @@ -1468,11 +1469,12 @@ #define ADC_VREF_PREC2 320 #define ADC_GPIOA_PINS (ADC_GPIO_PIN_STICK_RV | ADC_GPIO_PIN_STICK_RH | ADC_GPIO_PIN_STICK_LH | ADC_GPIO_PIN_STICK_LV) #define PWM_STICKS - #define PWM_IRQHandler TIM5_IRQHandler #define PWM_TIMER TIM5 + #define PWM_TIMER_FREQ (PERI1_FREQUENCY * TIMER_MULT_APB1) #define PWM_GPIO GPIOA #define PWM_GPIO_AF GPIO_AF2 #define PWM_IRQn TIM5_IRQn + #define PWM_IRQHandler TIM5_IRQHandler #define PWM_GPIOA_PINS (ADC_GPIO_PIN_STICK_RV | ADC_GPIO_PIN_STICK_RH | ADC_GPIO_PIN_STICK_LH | ADC_GPIO_PIN_STICK_LV) #define STICK_PWM_CHANNEL_LH 1 #define STICK_PWM_CHANNEL_LV 0