fix: PWM gimbals initialisation and inversion (#3745)
* fix: PWM gimbals initialisation and inversion * feat: add periph clock support in PWM stick driver
This commit is contained in:
@@ -43,6 +43,7 @@ set(FIRMWARE_TARGET_SRC ${FIRMWARE_TARGET_SRC}
|
||||
../common/arm/stm32/module_timer_driver.cpp
|
||||
../common/arm/stm32/stm32_switch_driver.cpp
|
||||
../common/arm/stm32/stm32_adc.cpp
|
||||
../common/arm/stm32/stm32_timer.cpp
|
||||
../common/arm/stm32/stm32_gpio_driver.cpp
|
||||
../common/arm/stm32/mixer_scheduler_driver.cpp
|
||||
)
|
||||
|
||||
@@ -19,151 +19,157 @@
|
||||
* GNU General Public License for more details.
|
||||
*/
|
||||
|
||||
#include "sticks_pwm_driver.h"
|
||||
#include "stm32_gpio_driver.h"
|
||||
#include "stm32_timer.h"
|
||||
#include "hal/adc_driver.h"
|
||||
#include "stm32_hal_ll.h"
|
||||
#include "delays_driver.h"
|
||||
|
||||
#include "hal.h"
|
||||
#include "dataconstants.h"
|
||||
|
||||
static volatile uint32_t pwm_interrupt_count;
|
||||
static volatile uint32_t _pwm_interrupt_count;
|
||||
|
||||
static void sticksPwmInit()
|
||||
static void sticks_pwm_init(const stick_pwm_timer_t* tim)
|
||||
{
|
||||
stm32_gpio_enable_clock(tim->GPIOx);
|
||||
|
||||
LL_GPIO_InitTypeDef pinInit;
|
||||
pinInit.Pin = PWM_GPIOA_PINS;
|
||||
pinInit.Pin = tim->GPIO_Pin;
|
||||
pinInit.Alternate = tim->GPIO_Alternate;
|
||||
pinInit.Mode = LL_GPIO_MODE_ALTERNATE;
|
||||
pinInit.Speed = LL_GPIO_SPEED_FREQ_VERY_HIGH;
|
||||
pinInit.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
|
||||
pinInit.Pull = LL_GPIO_PULL_UP;
|
||||
LL_GPIO_Init(PWM_GPIO, &pinInit);
|
||||
LL_GPIO_Init(tim->GPIOx, &pinInit);
|
||||
|
||||
PWM_TIMER->CR1 &= ~TIM_CR1_CEN; // Stop timer
|
||||
PWM_TIMER->PSC = 80;
|
||||
PWM_TIMER->ARR = 0xffff;
|
||||
PWM_TIMER->CCMR1 = TIM_CCMR1_CC1S_0 | TIM_CCMR1_CC2S_0;
|
||||
PWM_TIMER->CCMR2 = TIM_CCMR2_CC3S_0 | TIM_CCMR2_CC4S_0;
|
||||
PWM_TIMER->CCER = TIM_CCER_CC1E | TIM_CCER_CC2E | TIM_CCER_CC3E | TIM_CCER_CC4E;
|
||||
PWM_TIMER->DIER |= TIM_DIER_CC1IE | TIM_DIER_CC2IE | TIM_DIER_CC3IE | TIM_DIER_CC4IE;
|
||||
PWM_TIMER->CR1 = TIM_CR1_CEN; // Start timer
|
||||
auto TIMx = tim->TIMx;
|
||||
|
||||
NVIC_EnableIRQ(PWM_IRQn);
|
||||
NVIC_SetPriority(PWM_IRQn, 10);
|
||||
stm32_timer_enable_clock(TIMx);
|
||||
TIMx->CR1 &= ~TIM_CR1_CEN; // Stop timer
|
||||
TIMx->PSC = 80;
|
||||
TIMx->ARR = 0xffff;
|
||||
TIMx->CCMR1 = TIM_CCMR1_CC1S_0 | TIM_CCMR1_CC2S_0;
|
||||
TIMx->CCMR2 = TIM_CCMR2_CC3S_0 | TIM_CCMR2_CC4S_0;
|
||||
TIMx->CCER = TIM_CCER_CC1E | TIM_CCER_CC2E | TIM_CCER_CC3E | TIM_CCER_CC4E;
|
||||
TIMx->DIER |= TIM_DIER_CC1IE | TIM_DIER_CC2IE | TIM_DIER_CC3IE | TIM_DIER_CC4IE;
|
||||
TIMx->CR1 = TIM_CR1_CEN; // Start timer
|
||||
|
||||
NVIC_EnableIRQ(tim->TIM_IRQn);
|
||||
NVIC_SetPriority(tim->TIM_IRQn, 10);
|
||||
}
|
||||
|
||||
static void sticksPwmDeInit()
|
||||
static void sticks_pwm_deinit(const stick_pwm_timer_t* tim)
|
||||
{
|
||||
NVIC_DisableIRQ(PWM_IRQn);
|
||||
PWM_TIMER->CR1 &= ~TIM_CR1_CEN; // Stop timer
|
||||
NVIC_DisableIRQ(tim->TIM_IRQn);
|
||||
|
||||
// Stop timer
|
||||
auto TIMx = tim->TIMx;
|
||||
TIMx->CR1 &= ~TIM_CR1_CEN;
|
||||
stm32_timer_disable_clock(TIMx);
|
||||
|
||||
LL_GPIO_InitTypeDef pinInit;
|
||||
LL_GPIO_StructInit(&pinInit);
|
||||
|
||||
pinInit.Pin = PWM_GPIOA_PINS;
|
||||
pinInit.Pin = tim->GPIO_Pin;
|
||||
pinInit.Mode = LL_GPIO_MODE_INPUT;
|
||||
LL_GPIO_Init(PWM_GPIO, &pinInit);
|
||||
LL_GPIO_Init(tim->GPIOx, &pinInit);
|
||||
}
|
||||
|
||||
bool sticksPwmDetect()
|
||||
bool sticks_pwm_detect(const stick_pwm_timer_t* timer,
|
||||
const stick_pwm_input_t* inputs,
|
||||
uint8_t n_inputs)
|
||||
{
|
||||
pwm_interrupt_count = 0;
|
||||
sticksPwmInit();
|
||||
if (!timer || !inputs || n_inputs == 0)
|
||||
return false;
|
||||
|
||||
_pwm_interrupt_count = 0;
|
||||
sticks_pwm_init(timer);
|
||||
delay_ms(20);
|
||||
if (pwm_interrupt_count < 32) {
|
||||
sticksPwmDeInit();
|
||||
|
||||
if (_pwm_interrupt_count < 32) {
|
||||
sticks_pwm_deinit(timer);
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static inline uint32_t TIM_GetCapture_Stick(uint8_t n)
|
||||
static inline uint32_t tim_get_capture(TIM_TypeDef *TIMx, uint8_t channel)
|
||||
{
|
||||
switch (n) {
|
||||
case 0:
|
||||
return PWM_TIMER->CCR1;
|
||||
case 1:
|
||||
return PWM_TIMER->CCR2;
|
||||
case 2:
|
||||
return PWM_TIMER->CCR3;
|
||||
case 3:
|
||||
return PWM_TIMER->CCR4;
|
||||
default:
|
||||
return 0;
|
||||
}
|
||||
auto* base_CCRx = &(TIMx->CCR1);
|
||||
return READ_REG(*(base_CCRx + channel));
|
||||
}
|
||||
|
||||
static inline void TIM_SetPolarityRising(uint8_t n)
|
||||
static inline void tim_set_polarity_rising(TIM_TypeDef *TIMx, uint8_t channel)
|
||||
{
|
||||
PWM_TIMER->CCER &= ~(TIM_CCER_CC1P << (n * 4));
|
||||
CLEAR_BIT(TIMx->CCER, TIM_CCER_CC1P << (channel * 4));
|
||||
}
|
||||
|
||||
static inline void TIM_SetPolarityFalling(uint8_t n)
|
||||
static inline void tim_set_polarity_falling(TIM_TypeDef *TIMx, uint8_t channel)
|
||||
{
|
||||
PWM_TIMER->CCER |= (TIM_CCER_CC1P << (n * 4));
|
||||
SET_BIT(TIMx->CCER, TIM_CCER_CC1P << (channel * 4));
|
||||
}
|
||||
|
||||
static inline void TIM_ClearITPendingBit(uint8_t n)
|
||||
static inline uint32_t tim_is_active_flag_CCx(TIM_TypeDef *TIMx, uint8_t channel)
|
||||
{
|
||||
PWM_TIMER->SR = ~(TIM_SR_CC1IF << n);
|
||||
return READ_BIT(TIMx->SR, TIM_SR_CC1IF << channel);
|
||||
}
|
||||
|
||||
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)
|
||||
if (b > a) {
|
||||
return b - a;
|
||||
else
|
||||
return b + 0xffff - a;
|
||||
}
|
||||
|
||||
return b + 0xffff - a;
|
||||
}
|
||||
|
||||
#if !defined(STICK_PWM_CHANNEL_0)
|
||||
#define STICK_PWM_CHANNEL_0 0
|
||||
#endif
|
||||
|
||||
#if !defined(STICK_PWM_CHANNEL_1)
|
||||
#define STICK_PWM_CHANNEL_1 1
|
||||
#endif
|
||||
|
||||
#if !defined(STICK_PWM_CHANNEL_2)
|
||||
#define STICK_PWM_CHANNEL_2 2
|
||||
#endif
|
||||
|
||||
#if !defined(STICK_PWM_CHANNEL_3)
|
||||
#define STICK_PWM_CHANNEL_3 3
|
||||
#endif
|
||||
|
||||
static const uint8_t _channel_map[] = {
|
||||
STICK_PWM_CHANNEL_0,
|
||||
STICK_PWM_CHANNEL_1,
|
||||
STICK_PWM_CHANNEL_2,
|
||||
STICK_PWM_CHANNEL_3,
|
||||
};
|
||||
|
||||
extern "C" void PWM_IRQHandler(void)
|
||||
void sticks_pwm_isr(const stick_pwm_timer_t* tim,
|
||||
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<MAX_STICKS; i++) {
|
||||
if (PWM_TIMER->SR & (TIM_SR_CC1IF << i)) {
|
||||
uint32_t capture = TIM_GetCapture_Stick(i);
|
||||
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);
|
||||
tim_clear_flag_CCx(TIMx, channel);
|
||||
|
||||
// overflow may happen but we only use this to detect PWM / ADC on radio startup
|
||||
pwm_interrupt_count++;
|
||||
_pwm_interrupt_count++;
|
||||
|
||||
if (timer_capture_states[i] != 0) {
|
||||
uint32_t value = diff_with_16bits_overflow(timer_capture_rising_time[i], capture);
|
||||
if (value < 10000) {
|
||||
adcValues[_channel_map[i]] = (uint16_t) value;
|
||||
|
||||
if (value <= ADC_MAX_VALUE) {
|
||||
uint16_t v16 = (uint16_t)value;
|
||||
adcValues[i] = input.inverted ? ADC_INVERT_VALUE(v16) : v16;
|
||||
}
|
||||
TIM_SetPolarityRising(i);
|
||||
|
||||
tim_set_polarity_rising(TIMx, channel);
|
||||
timer_capture_states[i] = 0;
|
||||
}
|
||||
else {
|
||||
timer_capture_rising_time[i] = capture;
|
||||
TIM_SetPolarityFalling(i);
|
||||
|
||||
tim_set_polarity_falling(TIMx, channel);
|
||||
timer_capture_states[i] = 0x80;
|
||||
}
|
||||
TIM_ClearITPendingBit(i);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -21,5 +21,29 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
#include "stm32_hal_ll.h"
|
||||
|
||||
struct stick_pwm_input_t {
|
||||
uint8_t channel;
|
||||
uint8_t inverted;
|
||||
};
|
||||
|
||||
struct stick_pwm_timer_t {
|
||||
|
||||
GPIO_TypeDef* GPIOx;
|
||||
uint32_t GPIO_Pin;
|
||||
uint32_t GPIO_Alternate;
|
||||
|
||||
TIM_TypeDef* TIMx;
|
||||
IRQn_Type TIM_IRQn;
|
||||
};
|
||||
|
||||
// returns 'true' if PWM sticks have been detected
|
||||
bool sticksPwmDetect();
|
||||
bool sticks_pwm_detect(const stick_pwm_timer_t* timer,
|
||||
const stick_pwm_input_t* inputs,
|
||||
uint8_t n_inputs);
|
||||
|
||||
void sticks_pwm_isr(const stick_pwm_timer_t* tim,
|
||||
const stick_pwm_input_t* inputs,
|
||||
uint8_t n_inputs);
|
||||
|
||||
@@ -26,7 +26,6 @@
|
||||
|
||||
|
||||
struct stm32_adc_input_t {
|
||||
const char* name;
|
||||
GPIO_TypeDef* GPIOx;
|
||||
uint32_t GPIO_Pin;
|
||||
uint32_t ADC_Channel;
|
||||
|
||||
@@ -20,6 +20,7 @@
|
||||
*/
|
||||
|
||||
#include "stm32_pulse_driver.h"
|
||||
#include "stm32_timer.h"
|
||||
#include "stm32_dma.h"
|
||||
|
||||
#include "definitions.h"
|
||||
@@ -34,21 +35,6 @@
|
||||
// - DMA IRQ prio configurable (now 7)
|
||||
// - Timer IRQ prio configurable (now 7)
|
||||
|
||||
static void enable_tim_clock(TIM_TypeDef* TIMx)
|
||||
{
|
||||
if (TIMx == TIM1) {
|
||||
LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_TIM1);
|
||||
} else if (TIMx == TIM2) {
|
||||
LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM2);
|
||||
} else if (TIMx == TIM3) {
|
||||
LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM3);
|
||||
} else if (TIMx == TIM4) {
|
||||
LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM4);
|
||||
} else if (TIMx == TIM8) {
|
||||
LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_TIM8);
|
||||
}
|
||||
}
|
||||
|
||||
void stm32_pulse_init(const stm32_pulse_timer_t* tim, uint32_t freq)
|
||||
{
|
||||
if (tim->DMA_TC_CallbackPtr) {
|
||||
@@ -72,7 +58,7 @@ void stm32_pulse_init(const stm32_pulse_timer_t* tim, uint32_t freq)
|
||||
timInit.Prescaler = __LL_TIM_CALC_PSC(tim->TIM_Freq, freq);
|
||||
timInit.Autoreload = STM32_DEFAULT_TIMER_AUTORELOAD;
|
||||
|
||||
enable_tim_clock(tim->TIMx);
|
||||
stm32_timer_enable_clock(tim->TIMx);
|
||||
LL_TIM_Init(tim->TIMx, &timInit);
|
||||
|
||||
if (tim->DMAx) {
|
||||
@@ -86,21 +72,6 @@ void stm32_pulse_init(const stm32_pulse_timer_t* tim, uint32_t freq)
|
||||
NVIC_SetPriority(tim->TIM_IRQn, 7);
|
||||
}
|
||||
|
||||
static void disable_tim_clock(TIM_TypeDef* TIMx)
|
||||
{
|
||||
if (TIMx == TIM1) {
|
||||
LL_APB2_GRP1_DisableClock(LL_APB2_GRP1_PERIPH_TIM1);
|
||||
} else if (TIMx == TIM2) {
|
||||
LL_APB1_GRP1_DisableClock(LL_APB1_GRP1_PERIPH_TIM2);
|
||||
} else if (TIMx == TIM3) {
|
||||
LL_APB1_GRP1_DisableClock(LL_APB1_GRP1_PERIPH_TIM3);
|
||||
} else if (TIMx == TIM4) {
|
||||
LL_APB1_GRP1_DisableClock(LL_APB1_GRP1_PERIPH_TIM4);
|
||||
} else if (TIMx == TIM8) {
|
||||
LL_APB2_GRP1_DisableClock(LL_APB2_GRP1_PERIPH_TIM8);
|
||||
}
|
||||
}
|
||||
|
||||
void stm32_pulse_deinit(const stm32_pulse_timer_t* tim)
|
||||
{
|
||||
// Disable IRQs
|
||||
@@ -118,7 +89,7 @@ void stm32_pulse_deinit(const stm32_pulse_timer_t* tim)
|
||||
}
|
||||
|
||||
LL_TIM_DeInit(tim->TIMx);
|
||||
disable_tim_clock(tim->TIMx);
|
||||
stm32_timer_disable_clock(tim->TIMx);
|
||||
|
||||
// Reconfigure pin as input
|
||||
LL_GPIO_InitTypeDef pinInit;
|
||||
|
||||
@@ -0,0 +1,56 @@
|
||||
/*
|
||||
* Copyright (C) EdgeTX
|
||||
*
|
||||
* Based on code named
|
||||
* opentx - https://github.com/opentx/opentx
|
||||
* th9x - http://code.google.com/p/th9x
|
||||
* er9x - http://code.google.com/p/er9x
|
||||
* gruvin9x - http://code.google.com/p/gruvin9x
|
||||
*
|
||||
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
|
||||
*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License version 2 as
|
||||
* published by the Free Software Foundation.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*/
|
||||
|
||||
#include "stm32_timer.h"
|
||||
|
||||
void stm32_timer_enable_clock(TIM_TypeDef *TIMx)
|
||||
{
|
||||
if (TIMx == TIM1) {
|
||||
LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_TIM1);
|
||||
} else if (TIMx == TIM2) {
|
||||
LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM2);
|
||||
} else if (TIMx == TIM3) {
|
||||
LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM3);
|
||||
} else if (TIMx == TIM4) {
|
||||
LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM4);
|
||||
} else if (TIMx == TIM5) {
|
||||
LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM5);
|
||||
} else if (TIMx == TIM8) {
|
||||
LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_TIM8);
|
||||
}
|
||||
}
|
||||
|
||||
void stm32_timer_disable_clock(TIM_TypeDef *TIMx)
|
||||
{
|
||||
if (TIMx == TIM1) {
|
||||
LL_APB2_GRP1_DisableClock(LL_APB2_GRP1_PERIPH_TIM1);
|
||||
} else if (TIMx == TIM2) {
|
||||
LL_APB1_GRP1_DisableClock(LL_APB1_GRP1_PERIPH_TIM2);
|
||||
} else if (TIMx == TIM3) {
|
||||
LL_APB1_GRP1_DisableClock(LL_APB1_GRP1_PERIPH_TIM3);
|
||||
} else if (TIMx == TIM4) {
|
||||
LL_APB1_GRP1_DisableClock(LL_APB1_GRP1_PERIPH_TIM4);
|
||||
} else if (TIMx == TIM5) {
|
||||
LL_APB1_GRP1_DisableClock(LL_APB1_GRP1_PERIPH_TIM5);
|
||||
} else if (TIMx == TIM8) {
|
||||
LL_APB2_GRP1_DisableClock(LL_APB2_GRP1_PERIPH_TIM8);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,27 @@
|
||||
/*
|
||||
* Copyright (C) EdgeTX
|
||||
*
|
||||
* Based on code named
|
||||
* opentx - https://github.com/opentx/opentx
|
||||
* th9x - http://code.google.com/p/th9x
|
||||
* er9x - http://code.google.com/p/er9x
|
||||
* gruvin9x - http://code.google.com/p/gruvin9x
|
||||
*
|
||||
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
|
||||
*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License version 2 as
|
||||
* published by the Free Software Foundation.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "stm32_hal_ll.h"
|
||||
|
||||
void stm32_timer_enable_clock(TIM_TypeDef *TIMx);
|
||||
void stm32_timer_disable_clock(TIM_TypeDef *TIMx);
|
||||
Reference in New Issue
Block a user