diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 1f725554e..7d7ed4fc0 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -28,9 +28,6 @@ #include "timer.h" #include "drivers/pwm_output.h" -#define MULTISHOT_5US_PW (MULTISHOT_TIMER_MHZ * 5) -#define MULTISHOT_20US_MULT (MULTISHOT_TIMER_MHZ * 20 / 1000.0f) - static pwmWriteFunc *pwmWrite; static pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS]; static pwmCompleteWriteFunc *pwmCompleteWrite = NULL; @@ -99,16 +96,19 @@ static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8 #endif } -static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHardware, uint8_t mhz, uint16_t period, uint16_t value, uint8_t inversion) +static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHardware, uint32_t hz, uint16_t period, uint16_t value, uint8_t inversion) { #if defined(USE_HAL_DRIVER) TIM_HandleTypeDef* Handle = timerFindTimerHandle(timerHardware->tim); if(Handle == NULL) return; #endif - configTimeBase(timerHardware->tim, period, mhz); - pwmOCConfig(timerHardware->tim, timerHardware->channel, value, - inversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output); + configTimeBase(timerHardware->tim, period, hz); + pwmOCConfig(timerHardware->tim, + timerHardware->channel, + value, + inversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output + ); #if defined(USE_HAL_DRIVER) if(timerHardware->output & TIMER_OUTPUT_N_CHANNEL) @@ -122,7 +122,7 @@ static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHard #endif port->ccr = timerChCCR(timerHardware); - port->period = period; + port->tim = timerHardware->tim; *port->ccr = 0; @@ -134,29 +134,10 @@ static void pwmWriteUnused(uint8_t index, float value) UNUSED(value); } -static void pwmWriteBrushed(uint8_t index, float value) -{ - *motors[index].ccr = lrintf((value - 1000) * motors[index].period / 1000); -} - static void pwmWriteStandard(uint8_t index, float value) { - *motors[index].ccr = lrintf(value); -} - -static void pwmWriteOneShot125(uint8_t index, float value) -{ - *motors[index].ccr = lrintf(value * ONESHOT125_TIMER_MHZ/8.0f); -} - -static void pwmWriteOneShot42(uint8_t index, float value) -{ - *motors[index].ccr = lrintf(value * ONESHOT42_TIMER_MHZ/24.0f); -} - -static void pwmWriteMultiShot(uint8_t index, float value) -{ - *motors[index].ccr = lrintf(((value-1000) * MULTISHOT_20US_MULT) + MULTISHOT_5US_PW); + /* TODO: move value to be a number between 0-1 (i.e. percent throttle from mixer) */ + *motors[index].ccr = lrintf((value * motors[index].pulseScale) + motors[index].pulseOffset); } #ifdef USE_DSHOT @@ -246,32 +227,32 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8 { memset(motors, 0, sizeof(motors)); - uint32_t timerMhzCounter = 0; bool useUnsyncedPwm = motorConfig->useUnsyncedPwm; + float sMin = 0; + float sLen = 0; switch (motorConfig->motorPwmProtocol) { default: case PWM_TYPE_ONESHOT125: - timerMhzCounter = ONESHOT125_TIMER_MHZ; - pwmWrite = &pwmWriteOneShot125; + sMin = 125e-6f; + sLen = 125e-6f; break; case PWM_TYPE_ONESHOT42: - timerMhzCounter = ONESHOT42_TIMER_MHZ; - pwmWrite = &pwmWriteOneShot42; + sMin = 42e-6f; + sLen = 42e-6f; break; case PWM_TYPE_MULTISHOT: - timerMhzCounter = MULTISHOT_TIMER_MHZ; - pwmWrite = &pwmWriteMultiShot; + sMin = 5e-6f; + sLen = 20e-6f; break; case PWM_TYPE_BRUSHED: - timerMhzCounter = PWM_BRUSHED_TIMER_MHZ; - pwmWrite = &pwmWriteBrushed; + sMin = 0; useUnsyncedPwm = true; idlePulse = 0; break; case PWM_TYPE_STANDARD: - timerMhzCounter = PWM_TIMER_MHZ; - pwmWrite = &pwmWriteStandard; + sMin = 1e-3f; + sLen = 1e-3f; useUnsyncedPwm = true; idlePulse = 0; break; @@ -295,6 +276,7 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8 } if (!isDshot) { + pwmWrite = &pwmWriteStandard; pwmCompleteWrite = useUnsyncedPwm ? &pwmCompleteWriteUnused : &pwmCompleteOneshotMotorUpdate; } @@ -314,7 +296,9 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8 #ifdef USE_DSHOT if (isDshot) { - pwmDshotMotorHardwareConfig(timerHardware, motorIndex, motorConfig->motorPwmProtocol, + pwmDshotMotorHardwareConfig(timerHardware, + motorIndex, + motorConfig->motorPwmProtocol, motorConfig->motorPwmInversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output); motors[motorIndex].enabled = true; continue; @@ -328,12 +312,24 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8 IOConfigGPIO(motors[motorIndex].io, IOCFG_AF_PP); #endif - if (useUnsyncedPwm) { - const uint32_t hz = timerMhzCounter * 1000000; - pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, hz / motorConfig->motorPwmRate, idlePulse, motorConfig->motorPwmInversion); - } else { - pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, 0xFFFF, 0, motorConfig->motorPwmInversion); - } + /* standard PWM outputs */ + const unsigned pwmRateHz = useUnsyncedPwm ? motorConfig->motorPwmRate : ceilf(1 / (sMin + sLen)); + + const uint32_t clock = timerClock(timerHardware->tim); + /* used to find the desired timer frequency for max resolution */ + const unsigned prescaler = ((clock / pwmRateHz) + 0xffff) / 0x10000; /* rounding up */ + const uint32_t hz = clock / prescaler; + const unsigned period = hz / pwmRateHz; + + /* + if brushed then it is the entire length of the period. + TODO: this can be moved back to periodMin and periodLen + once mixer outputs a 0..1 float value. + */ + motors[motorIndex].pulseScale = ((motorConfig->motorPwmProtocol == PWM_TYPE_BRUSHED) ? period : (sLen * hz)) / 1000.0f; + motors[motorIndex].pulseOffset = (sMin * hz) - (motors[motorIndex].pulseScale * 1000); + + pwmOutConfig(&motors[motorIndex], timerHardware, hz, period, idlePulse, motorConfig->motorPwmInversion); bool timerAlreadyUsed = false; for (int i = 0; i < motorIndex; i++) { @@ -364,16 +360,16 @@ uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType) { switch (pwmProtocolType) { case(PWM_TYPE_PROSHOT1000): - return MOTOR_PROSHOT1000_MHZ * 1000000; + return MOTOR_PROSHOT1000_HZ; case(PWM_TYPE_DSHOT1200): - return MOTOR_DSHOT1200_MHZ * 1000000; + return MOTOR_DSHOT1200_HZ; case(PWM_TYPE_DSHOT600): - return MOTOR_DSHOT600_MHZ * 1000000; + return MOTOR_DSHOT600_HZ; case(PWM_TYPE_DSHOT300): - return MOTOR_DSHOT300_MHZ * 1000000; + return MOTOR_DSHOT300_HZ; default: case(PWM_TYPE_DSHOT150): - return MOTOR_DSHOT150_MHZ * 1000000; + return MOTOR_DSHOT150_HZ; } } @@ -460,8 +456,7 @@ void servoDevInit(const servoDevConfig_t *servoConfig) /* flag failure and disable ability to arm */ break; } - - pwmOutConfig(&servos[servoIndex], timer, PWM_TIMER_MHZ, 1000000 / servoConfig->servoPwmRate, servoConfig->servoCenterPulse, 0); + pwmOutConfig(&servos[servoIndex], timer, PWM_TIMER_HZ, PWM_TIMER_HZ / servoConfig->servoPwmRate, servoConfig->servoCenterPulse, 0); servos[servoIndex].enabled = true; } } @@ -474,7 +469,7 @@ void pwmWriteBeeper(bool onoffBeep) if(!beeperPwm.io) return; if(onoffBeep == true) { - *beeperPwm.ccr = (1000000/freqBeep)/2; + *beeperPwm.ccr = (PWM_TIMER_HZ / freqBeep) / 2; beeperPwm.enabled = true; } else { *beeperPwm.ccr = 0; @@ -500,7 +495,7 @@ void beeperPwmInit(IO_t io, uint16_t frequency) IOConfigGPIO(beeperPwm.io, IOCFG_AF_PP); #endif freqBeep = frequency; - pwmOutConfig(&beeperPwm, timer, PWM_TIMER_MHZ, 1000000/freqBeep, (1000000/freqBeep)/2,0); + pwmOutConfig(&beeperPwm, timer, PWM_TIMER_HZ, PWM_TIMER_HZ / freqBeep, (PWM_TIMER_HZ / freqBeep) / 2, 0); } *beeperPwm.ccr = 0; beeperPwm.enabled = false; diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index c37fa960f..34c56c69d 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -20,7 +20,9 @@ #include "drivers/io_types.h" #include "timer.h" +#ifndef MAX_SUPPORTED_MOTORS #define MAX_SUPPORTED_MOTORS 12 +#endif #if defined(USE_QUAD_MIXER_ONLY) #define MAX_SUPPORTED_SERVOS 1 @@ -75,45 +77,29 @@ typedef enum { PWM_TYPE_MAX } motorPwmProtocolTypes_e; -#define PWM_TIMER_MHZ 1 +#define PWM_TIMER_HZ MHZ_TO_HZ(1) #ifdef USE_DSHOT #define MAX_DMA_TIMERS 8 -#define MOTOR_DSHOT1200_MHZ 24 -#define MOTOR_DSHOT600_MHZ 12 -#define MOTOR_DSHOT300_MHZ 6 -#define MOTOR_DSHOT150_MHZ 3 +#define MOTOR_DSHOT1200_HZ MHZ_TO_HZ(24) +#define MOTOR_DSHOT600_HZ MHZ_TO_HZ(12) +#define MOTOR_DSHOT300_HZ MHZ_TO_HZ(6) +#define MOTOR_DSHOT150_HZ MHZ_TO_HZ(3) #define MOTOR_BIT_0 7 #define MOTOR_BIT_1 14 #define MOTOR_BITLENGTH 19 -#define MOTOR_PROSHOT1000_MHZ 24 +#define MOTOR_PROSHOT1000_HZ MHZ_TO_HZ(24) #define PROSHOT_BASE_SYMBOL 24 // 1uS #define PROSHOT_BIT_WIDTH 3 #define MOTOR_NIBBLE_LENGTH_PROSHOT 96 // 4uS #endif -#if defined(STM32F40_41xxx) // must be multiples of timer clock -#define ONESHOT125_TIMER_MHZ 12 -#define ONESHOT42_TIMER_MHZ 21 -#define MULTISHOT_TIMER_MHZ 84 -#define PWM_BRUSHED_TIMER_MHZ 21 -#elif defined(STM32F7) // must be multiples of timer clock -#define ONESHOT125_TIMER_MHZ 9 -#define ONESHOT42_TIMER_MHZ 27 -#define MULTISHOT_TIMER_MHZ 54 -#define PWM_BRUSHED_TIMER_MHZ 27 -#else -#define ONESHOT125_TIMER_MHZ 8 -#define ONESHOT42_TIMER_MHZ 24 -#define MULTISHOT_TIMER_MHZ 72 -#define PWM_BRUSHED_TIMER_MHZ 24 -#endif #define DSHOT_DMA_BUFFER_SIZE 18 /* resolution + frame reset (2us) */ -#define PROSHOT_DMA_BUFFER_SIZE 6/* resolution + frame reset (2us) */ +#define PROSHOT_DMA_BUFFER_SIZE 6 /* resolution + frame reset (2us) */ typedef struct { TIM_TypeDef *timer; @@ -149,9 +135,10 @@ typedef struct { volatile timCCR_t *ccr; TIM_TypeDef *tim; bool forceOverflow; - uint16_t period; bool enabled; IO_t io; + float pulseScale; + float pulseOffset; } pwmOutputPort_t; typedef struct motorDevConfig_s { diff --git a/src/main/drivers/rx_pwm.c b/src/main/drivers/rx_pwm.c index 97c5822ea..863e21e8e 100644 --- a/src/main/drivers/rx_pwm.c +++ b/src/main/drivers/rx_pwm.c @@ -391,7 +391,7 @@ void pwmRxInit(const pwmConfig_t *pwmConfig) IOConfigGPIO(io, IOCFG_AF_PP); #endif - timerConfigure(timer, (uint16_t)PWM_TIMER_PERIOD, PWM_TIMER_MHZ); + timerConfigure(timer, (uint16_t)PWM_TIMER_PERIOD, PWM_TIMER_HZ); timerChCCHandlerInit(&port->edgeCb, pwmEdgeCallback); timerChOvrHandlerInit(&port->overflowCb, pwmOverflowCallback); timerChConfigCallbacks(timer, &port->edgeCb, &port->overflowCb); @@ -408,7 +408,7 @@ void pwmRxInit(const pwmConfig_t *pwmConfig) #define UNUSED_PPM_TIMER_REFERENCE 0 #define FIRST_PWM_PORT 0 -void ppmAvoidPWMTimerClash(TIM_TypeDef *pwmTimer, uint8_t pwmProtocol) +void ppmAvoidPWMTimerClash(TIM_TypeDef *pwmTimer) { pwmOutputPort_t *motors = pwmGetMotors(); for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS; motorIndex++) { @@ -416,26 +416,12 @@ void ppmAvoidPWMTimerClash(TIM_TypeDef *pwmTimer, uint8_t pwmProtocol) continue; } - switch (pwmProtocol) - { - case PWM_TYPE_ONESHOT125: - ppmCountDivisor = ONESHOT125_TIMER_MHZ; - break; - case PWM_TYPE_ONESHOT42: - ppmCountDivisor = ONESHOT42_TIMER_MHZ; - break; - case PWM_TYPE_MULTISHOT: - ppmCountDivisor = MULTISHOT_TIMER_MHZ; - break; - case PWM_TYPE_BRUSHED: - ppmCountDivisor = PWM_BRUSHED_TIMER_MHZ; - break; - } + ppmCountDivisor = timerClock(pwmTimer) / (pwmTimer->PSC + 1); return; } } -void ppmRxInit(const ppmConfig_t *ppmConfig, uint8_t pwmProtocol) +void ppmRxInit(const ppmConfig_t *ppmConfig) { ppmResetDevice(); @@ -447,7 +433,7 @@ void ppmRxInit(const ppmConfig_t *ppmConfig, uint8_t pwmProtocol) return; } - ppmAvoidPWMTimerClash(timer->tim, pwmProtocol); + ppmAvoidPWMTimerClash(timer->tim); port->mode = INPUT_MODE_PPM; port->timerHardware = timer; @@ -462,7 +448,7 @@ void ppmRxInit(const ppmConfig_t *ppmConfig, uint8_t pwmProtocol) IOConfigGPIO(io, IOCFG_AF_PP); #endif - timerConfigure(timer, (uint16_t)PPM_TIMER_PERIOD, PWM_TIMER_MHZ); + timerConfigure(timer, (uint16_t)PPM_TIMER_PERIOD, PWM_TIMER_HZ); timerChCCHandlerInit(&port->edgeCb, ppmEdgeCallback); timerChOvrHandlerInit(&port->overflowCb, ppmOverflowCallback); timerChConfigCallbacks(timer, &port->edgeCb, &port->overflowCb); diff --git a/src/main/drivers/rx_pwm.h b/src/main/drivers/rx_pwm.h index a5e462918..0943af646 100644 --- a/src/main/drivers/rx_pwm.h +++ b/src/main/drivers/rx_pwm.h @@ -36,7 +36,7 @@ typedef struct pwmConfig_s { inputFilteringMode_e inputFilteringMode; } pwmConfig_t; -void ppmRxInit(const ppmConfig_t *ppmConfig, uint8_t pwmProtocol); +void ppmRxInit(const ppmConfig_t *ppmConfig); void pwmRxInit(const pwmConfig_t *pwmConfig); uint16_t pwmRead(uint8_t channel); diff --git a/src/main/drivers/serial_escserial.c b/src/main/drivers/serial_escserial.c index 5595e5c50..f55f70d54 100644 --- a/src/main/drivers/serial_escserial.c +++ b/src/main/drivers/serial_escserial.c @@ -184,8 +184,7 @@ static void serialTimerTxConfigBL(const timerHardware_t *timerHardwarePtr, uint8 } } while (isTimerPeriodTooLarge(timerPeriod)); - uint8_t mhz = clock / 1000000; - timerConfigure(timerHardwarePtr, timerPeriod, mhz); + timerConfigure(timerHardwarePtr, timerPeriod, clock); timerChCCHandlerInit(&escSerialPorts[reference].timerCb, onSerialTimerBL); timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].timerCb, NULL); } @@ -193,9 +192,8 @@ static void serialTimerTxConfigBL(const timerHardware_t *timerHardwarePtr, uint8 static void serialTimerRxConfigBL(const timerHardware_t *timerHardwarePtr, uint8_t reference, portOptions_t options) { // start bit is usually a FALLING signal - uint8_t mhz = SystemCoreClock / 2000000; TIM_DeInit(timerHardwarePtr->tim); - timerConfigure(timerHardwarePtr, 0xFFFF, mhz); + timerConfigure(timerHardwarePtr, 0xFFFF, SystemCoreClock / 2); escSerialICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, (options & SERIAL_INVERTED) ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling); timerChCCHandlerInit(&escSerialPorts[reference].edgeCb, onSerialRxPinChangeBL); timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].edgeCb, NULL); @@ -203,9 +201,9 @@ static void serialTimerRxConfigBL(const timerHardware_t *timerHardwarePtr, uint8 static void escSerialTimerTxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference) { - uint32_t timerPeriod=34; + uint32_t timerPeriod = 34; TIM_DeInit(timerHardwarePtr->tim); - timerConfigure(timerHardwarePtr, timerPeriod, 1); + timerConfigure(timerHardwarePtr, timerPeriod, MHZ_TO_HZ(1)); timerChCCHandlerInit(&escSerialPorts[reference].timerCb, onSerialTimerEsc); timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].timerCb, NULL); } @@ -228,7 +226,7 @@ static void escSerialTimerRxConfig(const timerHardware_t *timerHardwarePtr, uint { // start bit is usually a FALLING signal TIM_DeInit(timerHardwarePtr->tim); - timerConfigure(timerHardwarePtr, 0xFFFF, 1); + timerConfigure(timerHardwarePtr, 0xFFFF, MHZ_TO_HZ(1)); escSerialICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Falling); timerChCCHandlerInit(&escSerialPorts[reference].edgeCb, onSerialRxPinChangeEsc); timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].edgeCb, NULL); diff --git a/src/main/drivers/serial_softserial.c b/src/main/drivers/serial_softserial.c index 7540a521b..1d552bed8 100644 --- a/src/main/drivers/serial_softserial.c +++ b/src/main/drivers/serial_softserial.c @@ -204,7 +204,7 @@ static bool isTimerPeriodTooLarge(uint32_t timerPeriod) static void serialTimerConfigureTimebase(const timerHardware_t *timerHardwarePtr, uint32_t baud) { - uint32_t baseClock = SystemCoreClock / timerClockDivisor(timerHardwarePtr->tim); + uint32_t baseClock = timerClock(timerHardwarePtr->tim); uint32_t clock = baseClock; uint32_t timerPeriod; @@ -220,9 +220,7 @@ static void serialTimerConfigureTimebase(const timerHardware_t *timerHardwarePtr } } while (isTimerPeriodTooLarge(timerPeriod)); - uint8_t mhz = baseClock / 1000000; - - timerConfigure(timerHardwarePtr, timerPeriod, mhz); + timerConfigure(timerHardwarePtr, timerPeriod, baseClock); } static void resetBuffers(softSerial_t *softSerial) diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index 050600cb0..57a2b69f4 100755 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -229,16 +229,16 @@ void timerNVICConfigure(uint8_t irq) NVIC_Init(&NVIC_InitStructure); } -void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz) +void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint32_t hz) { TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); - TIM_TimeBaseStructure.TIM_Period = (period - 1) & 0xffff; // AKA TIMx_ARR + TIM_TimeBaseStructure.TIM_Period = (period - 1) & 0xFFFF; // AKA TIMx_ARR // "The counter clock frequency (CK_CNT) is equal to f CK_PSC / (PSC[15:0] + 1)." - STM32F10x Reference Manual 14.4.11 // Thus for 1Mhz: 72000000 / 1000000 = 72, 72 - 1 = 71 = TIM_Prescaler - TIM_TimeBaseStructure.TIM_Prescaler = (SystemCoreClock / timerClockDivisor(tim) / ((uint32_t)mhz * 1000000)) - 1; + TIM_TimeBaseStructure.TIM_Prescaler = (timerClock(tim) / hz) - 1; TIM_TimeBaseStructure.TIM_ClockDivision = 0; TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; @@ -246,9 +246,9 @@ void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz) } // old interface for PWM inputs. It should be replaced -void timerConfigure(const timerHardware_t *timerHardwarePtr, uint16_t period, uint8_t mhz) +void timerConfigure(const timerHardware_t *timerHardwarePtr, uint16_t period, uint32_t hz) { - configTimeBase(timerHardwarePtr->tim, period, mhz); + configTimeBase(timerHardwarePtr->tim, period, hz); TIM_Cmd(timerHardwarePtr->tim, ENABLE); uint8_t irq = timerInputIrq(timerHardwarePtr->tim); @@ -849,14 +849,19 @@ uint16_t timerDmaSource(uint8_t channel) uint16_t timerGetPrescalerByDesiredMhz(TIM_TypeDef *tim, uint16_t mhz) { - // protection here for desired MHZ > timerClock??? - if ((uint32_t)(mhz * 1000000) > timerClock(tim)) { - return 0; - } - return (uint16_t)(round((timerClock(tim) / (mhz * 1000000)) - 1)); + return timerGetPrescalerByDesiredHertz(tim, MHZ_TO_HZ(mhz)); } -uint16_t timerGetPeriodByPrescaler(TIM_TypeDef *tim, uint16_t prescaler, uint32_t hertz) +uint16_t timerGetPeriodByPrescaler(TIM_TypeDef *tim, uint16_t prescaler, uint32_t hz) { - return ((uint16_t)((timerClock(tim) / (prescaler + 1)) / hertz)); + return ((uint16_t)((timerClock(tim) / (prescaler + 1)) / hz)); +} + +uint16_t timerGetPrescalerByDesiredHertz(TIM_TypeDef *tim, uint32_t hz) +{ + // protection here for desired hertz > SystemCoreClock??? + if (hz > timerClock(tim)) { + return 0; + } + return (uint16_t)((timerClock(tim) + hz / 2 ) / hz) - 1; } diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index 2292ff6a7..00cf4d8bc 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -134,6 +134,8 @@ typedef enum { #define HARDWARE_TIMER_DEFINITION_COUNT 14 #endif +#define MHZ_TO_HZ(x) (x * 1000000) + extern const timerHardware_t timerHardware[]; extern const timerDef_t timerDefinitions[]; @@ -155,7 +157,7 @@ typedef enum { TYPE_TIMER } channelType_t; -void timerConfigure(const timerHardware_t *timHw, uint16_t period, uint8_t mhz); // This interface should be replaced. +void timerConfigure(const timerHardware_t *timHw, uint16_t period, uint32_t hz); // This interface should be replaced. void timerChConfigIC(const timerHardware_t *timHw, bool polarityRising, unsigned inputFilterSamples); void timerChConfigICDual(const timerHardware_t* timHw, bool polarityRising, unsigned inputFilterSamples); @@ -183,7 +185,7 @@ void timerForceOverflow(TIM_TypeDef *tim); uint8_t timerClockDivisor(TIM_TypeDef *tim); uint32_t timerClock(TIM_TypeDef *tim); -void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz); // TODO - just for migration +void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint32_t hz); // TODO - just for migration rccPeriphTag_t timerRCC(TIM_TypeDef *tim); uint8_t timerInputIrq(TIM_TypeDef *tim); @@ -200,5 +202,6 @@ void timerOCPreloadConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t preload); volatile timCCR_t *timerCCR(TIM_TypeDef *tim, uint8_t channel); uint16_t timerDmaSource(uint8_t channel); +uint16_t timerGetPrescalerByDesiredHertz(TIM_TypeDef *tim, uint32_t hz); uint16_t timerGetPrescalerByDesiredMhz(TIM_TypeDef *tim, uint16_t mhz); -uint16_t timerGetPeriodByPrescaler(TIM_TypeDef *tim, uint16_t prescaler, uint32_t hertz); +uint16_t timerGetPeriodByPrescaler(TIM_TypeDef *tim, uint16_t prescaler, uint32_t hz); diff --git a/src/main/drivers/timer_hal.c b/src/main/drivers/timer_hal.c index 350c1b432..30eb04e83 100644 --- a/src/main/drivers/timer_hal.c +++ b/src/main/drivers/timer_hal.c @@ -241,7 +241,7 @@ TIM_HandleTypeDef* timerFindTimerHandle(TIM_TypeDef *tim) return &timerHandle[timerIndex].Handle; } -void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz) +void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint32_t hz) { uint8_t timerIndex = lookupTimerIndex(tim); if (timerIndex >= USED_TIMER_COUNT) { @@ -256,7 +256,7 @@ void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz) timerHandle[timerIndex].Handle.Instance = tim; timerHandle[timerIndex].Handle.Init.Period = (period - 1) & 0xffff; // AKA TIMx_ARR - timerHandle[timerIndex].Handle.Init.Prescaler = (SystemCoreClock / timerClockDivisor(tim) / ((uint32_t)mhz * 1000000)) - 1; + timerHandle[timerIndex].Handle.Init.Prescaler = (timerClock(tim) / hz) - 1; timerHandle[timerIndex].Handle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; timerHandle[timerIndex].Handle.Init.CounterMode = TIM_COUNTERMODE_UP; @@ -287,14 +287,14 @@ void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz) } // old interface for PWM inputs. It should be replaced -void timerConfigure(const timerHardware_t *timerHardwarePtr, uint16_t period, uint8_t mhz) +void timerConfigure(const timerHardware_t *timerHardwarePtr, uint16_t period, uint32_t hz) { uint8_t timerIndex = lookupTimerIndex(timerHardwarePtr->tim); if (timerIndex >= USED_TIMER_COUNT) { return; } - configTimeBase(timerHardwarePtr->tim, period, mhz); + configTimeBase(timerHardwarePtr->tim, period, hz); HAL_TIM_Base_Start(&timerHandle[timerIndex].Handle); uint8_t irq = timerInputIrq(timerHardwarePtr->tim); @@ -898,14 +898,19 @@ uint16_t timerDmaSource(uint8_t channel) uint16_t timerGetPrescalerByDesiredMhz(TIM_TypeDef *tim, uint16_t mhz) { - // protection here for desired MHZ > SystemCoreClock??? - if (mhz * 1000000 > (SystemCoreClock / timerClockDivisor(tim))) { - return 0; - } - return (uint16_t)(round((SystemCoreClock / timerClockDivisor(tim) / (mhz * 1000000)) - 1)); + return timerGetPrescalerByDesiredHertz(tim, MHZ_TO_HZ(mhz)); } -uint16_t timerGetPeriodByPrescaler(TIM_TypeDef *tim, uint16_t prescaler, uint32_t hertz) +uint16_t timerGetPeriodByPrescaler(TIM_TypeDef *tim, uint16_t prescaler, uint32_t hz) { - return ((uint16_t)((SystemCoreClock / timerClockDivisor(tim) / (prescaler + 1)) / hertz)); + return ((uint16_t)((timerClock(tim) / (prescaler + 1)) / hz)); } + +uint16_t timerGetPrescalerByDesiredHertz(TIM_TypeDef *tim, uint32_t hz) +{ + // protection here for desired hertz > SystemCoreClock??? + if (hz > timerClock(tim)) { + return 0; + } + return (uint16_t)((timerClock(tim) + hz / 2 ) / hz) - 1; +} \ No newline at end of file diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 0e4d2f30a..783c0f956 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -384,7 +384,7 @@ void init(void) if (0) {} #if defined(USE_PPM) else if (feature(FEATURE_RX_PPM)) { - ppmRxInit(ppmConfig(), motorConfig()->dev.motorPwmProtocol); + ppmRxInit(ppmConfig()); } #endif #if defined(USE_PWM) diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h index b53a0ddb7..a99bbb0e3 100644 --- a/src/main/target/common_fc_pre.h +++ b/src/main/target/common_fc_pre.h @@ -36,6 +36,7 @@ // Using RX DMA disables the use of receive callbacks #define USE_UART1_RX_DMA #define USE_UART1_TX_DMA +#define MAX_SUPPORTED_MOTORS 8 #endif #ifdef STM32F3