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

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
@@ -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;
@@ -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();
@@ -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;
}
}
}
@@ -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