piotr/edgetx-gx12
1
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

feat(radio): improve gimbal detection (#6161)

Browse Source
This commit is contained in:
Raphael Coeffic
2025-05-02 20:33:53 +10:00
committed by GitHub
parent 805fe05094
commit 3a413a70d6
12 changed files with 114 additions and 91 deletions
Download Patch File Download Diff File Expand all files Collapse all files
radio/src/boards/generic_stm32/analog_inputs.cpp
+26 -8
View File
@@ -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++]();
}
}
radio/src/boards/generic_stm32/analog_inputs.h
+1 -1
View File
@@ -23,4 +23,4 @@
extern const etx_hal_adc_driver_t _adc_driver;
bool sticksPwmDetect();
void gimbalsDetect();
radio/src/cli.cpp
+1 -1
View File
@@ -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
radio/src/targets/common/arm/stm32/flysky_gimbal_driver.cpp
+21 -3
View File
@@ -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;
radio/src/targets/common/arm/stm32/flysky_gimbal_driver.h
+3 -1
View File
@@ -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();
radio/src/targets/common/arm/stm32/sticks_pwm_driver.cpp
+47 -42
View File
@@ -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 <string.h>
#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;
}
}
}
radio/src/targets/common/arm/stm32/sticks_pwm_driver.h
+1
View File
@@ -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
radio/src/targets/horus/board.cpp
+5 -17
View File
@@ -49,10 +49,6 @@
#include <string.h>
#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();
radio/src/targets/horus/hal.h
+1
View File
@@ -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
radio/src/targets/pl18/board.cpp
+3 -9
View File
@@ -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();
radio/src/targets/taranis/board.cpp
+2 -8
View File
@@ -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)
radio/src/targets/taranis/hal.h
+3 -1
View File
@@ -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