diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 13f01eac4..59e1167a2 100755 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -30,11 +30,6 @@ #include "pwm_rx.h" #include "pwm_mapping.h" -void pwmBrushedMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate); -void pwmBrushlessMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse); -void pwmFastPwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t fastPwmProtocolType, uint16_t motorPwmRate, uint16_t idlePulse); -void pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse); - /* Configuration maps @@ -313,14 +308,14 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) } #endif if (init->useFastPwm) { - pwmFastPwmMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->pwmProtocolType, init->motorPwmRate, init->idlePulse); - pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_ONESHOT|PWM_PF_OUTPUT_PROTOCOL_PWM ; + pwmFastPwmMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->motorPwmRate, init->idlePulse, init->pwmProtocolType); + pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_PWM | PWM_PF_OUTPUT_PROTOCOL_ONESHOT; } else if (init->pwmProtocolType == PWM_TYPE_BRUSHED) { pwmBrushedMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->motorPwmRate); - pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_MOTOR | PWM_PF_MOTOR_MODE_BRUSHED | PWM_PF_OUTPUT_PROTOCOL_PWM; + pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_PWM | PWM_PF_MOTOR_MODE_BRUSHED; } else { pwmBrushlessMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->motorPwmRate, init->idlePulse); - pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_PWM ; + pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_PWM; } pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].index = pwmOutputConfiguration.motorCount; pwmOutputConfiguration.portConfigurations[pwmOutputConfiguration.outputCount].timerHardware = timerHardwarePtr; diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index ffb554099..ffd16e74d 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -30,24 +30,8 @@ #error Invalid motor/servo/port configuration #endif -#define PULSE_1MS (1000) // 1ms pulse width -#define MAX_INPUTS 8 #define PWM_TIMER_MHZ 1 -#if defined(STM32F40_41xxx) // must be multiples of timer clock -#define ONESHOT42_TIMER_MHZ 21 -#define ONESHOT125_TIMER_MHZ 12 -#define PWM_BRUSHED_TIMER_MHZ 21 -#define MULTISHOT_TIMER_MHZ 84 -#else -#define PWM_BRUSHED_TIMER_MHZ 24 -#define MULTISHOT_TIMER_MHZ 72 -#define ONESHOT42_TIMER_MHZ 24 -#define ONESHOT125_TIMER_MHZ 8 -#endif - -#define MULTISHOT_5US_PW (MULTISHOT_TIMER_MHZ * 5) -#define MULTISHOT_20US_MULT (MULTISHOT_TIMER_MHZ * 20 / 1000.0f) typedef struct sonarIOConfig_s { ioTag_t triggerTag; diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 056ee17fa..c5ebfcd17 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -17,21 +17,31 @@ #include #include - -#include #include #include "platform.h" #include "io.h" #include "timer.h" - -#include "flight/failsafe.h" // FIXME dependency into the main code from a driver - #include "pwm_mapping.h" - #include "pwm_output.h" +#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 +#else +#define ONESHOT125_TIMER_MHZ 8 +#define ONESHOT42_TIMER_MHZ 24 +#define MULTISHOT_TIMER_MHZ 72 +#define PWM_BRUSHED_TIMER_MHZ 24 +#endif + +#define MULTISHOT_5US_PW (MULTISHOT_TIMER_MHZ * 5) +#define MULTISHOT_20US_MULT (MULTISHOT_TIMER_MHZ * 20 / 1000.0f) + + typedef void (*pwmWriteFuncPtr)(uint8_t index, uint16_t value); // function pointer used to write motors typedef struct { @@ -52,6 +62,8 @@ static pwmOutputPort_t *servos[MAX_PWM_SERVOS]; static uint8_t allocatedOutputPortCount = 0; static bool pwmMotorsEnabled = true; + + static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8_t ouputPolarity) { TIM_OCInitTypeDef TIM_OCInitStructure; @@ -65,22 +77,22 @@ static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8 TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set; switch (channel) { - case TIM_Channel_1: - TIM_OC1Init(tim, &TIM_OCInitStructure); - TIM_OC1PreloadConfig(tim, TIM_OCPreload_Enable); - break; - case TIM_Channel_2: - TIM_OC2Init(tim, &TIM_OCInitStructure); - TIM_OC2PreloadConfig(tim, TIM_OCPreload_Enable); - break; - case TIM_Channel_3: - TIM_OC3Init(tim, &TIM_OCInitStructure); - TIM_OC3PreloadConfig(tim, TIM_OCPreload_Enable); - break; - case TIM_Channel_4: - TIM_OC4Init(tim, &TIM_OCInitStructure); - TIM_OC4PreloadConfig(tim, TIM_OCPreload_Enable); - break; + case TIM_Channel_1: + TIM_OC1Init(tim, &TIM_OCInitStructure); + TIM_OC1PreloadConfig(tim, TIM_OCPreload_Enable); + break; + case TIM_Channel_2: + TIM_OC2Init(tim, &TIM_OCInitStructure); + TIM_OC2PreloadConfig(tim, TIM_OCPreload_Enable); + break; + case TIM_Channel_3: + TIM_OC3Init(tim, &TIM_OCInitStructure); + TIM_OC3PreloadConfig(tim, TIM_OCPreload_Enable); + break; + case TIM_Channel_4: + TIM_OC4Init(tim, &TIM_OCInitStructure); + TIM_OC4PreloadConfig(tim, TIM_OCPreload_Enable); + break; } } @@ -90,7 +102,7 @@ static pwmOutputPort_t *pwmOutConfig(const timerHardware_t *timerHardware, uint8 configTimeBase(timerHardware->tim, period, mhz); - IO_t io = IOGetByTag(timerHardware->tag); + const IO_t io = IOGetByTag(timerHardware->tag); IOInit(io, OWNER_MOTOR, RESOURCE_OUTPUT, allocatedOutputPortCount); IOConfigGPIO(io, IOCFG_AF_PP); @@ -102,18 +114,18 @@ static pwmOutputPort_t *pwmOutConfig(const timerHardware_t *timerHardware, uint8 TIM_Cmd(timerHardware->tim, ENABLE); switch (timerHardware->channel) { - case TIM_Channel_1: - p->ccr = &timerHardware->tim->CCR1; - break; - case TIM_Channel_2: - p->ccr = &timerHardware->tim->CCR2; - break; - case TIM_Channel_3: - p->ccr = &timerHardware->tim->CCR3; - break; - case TIM_Channel_4: - p->ccr = &timerHardware->tim->CCR4; - break; + case TIM_Channel_1: + p->ccr = &timerHardware->tim->CCR1; + break; + case TIM_Channel_2: + p->ccr = &timerHardware->tim->CCR2; + break; + case TIM_Channel_3: + p->ccr = &timerHardware->tim->CCR3; + break; + case TIM_Channel_4: + p->ccr = &timerHardware->tim->CCR4; + break; } p->period = period; p->tim = timerHardware->tim; @@ -133,16 +145,16 @@ static void pwmWriteStandard(uint8_t index, uint16_t value) *motors[index]->ccr = value; } -static void pwmWriteOneShot42(uint8_t index, uint16_t value) -{ - *motors[index]->ccr = lrintf((float)(value * ONESHOT42_TIMER_MHZ/24.0f)); -} - static void pwmWriteOneShot125(uint8_t index, uint16_t value) { *motors[index]->ccr = lrintf((float)(value * ONESHOT125_TIMER_MHZ/8.0f)); } +static void pwmWriteOneShot42(uint8_t index, uint16_t value) +{ + *motors[index]->ccr = lrintf((float)(value * ONESHOT42_TIMER_MHZ/24.0f)); +} + static void pwmWriteMultiShot(uint8_t index, uint16_t value) { *motors[index]->ccr = lrintf(((float)(value-1000) * MULTISHOT_20US_MULT) + MULTISHOT_5US_PW); @@ -150,15 +162,14 @@ static void pwmWriteMultiShot(uint8_t index, uint16_t value) void pwmWriteMotor(uint8_t index, uint16_t value) { - if (motors[index] && index < MAX_MOTORS && pwmMotorsEnabled) + if (motors[index] && index < MAX_MOTORS && pwmMotorsEnabled) { motors[index]->pwmWritePtr(index, value); + } } void pwmShutdownPulsesForAllMotors(uint8_t motorCount) { - uint8_t index; - - for(index = 0; index < motorCount; index++){ + for(int index = 0; index < motorCount; index++){ // Set the compare register to 0, which stops the output pulsing if the timer overflows *motors[index]->ccr = 0; } @@ -176,17 +187,14 @@ void pwmEnableMotors(void) void pwmCompleteOneshotMotorUpdate(uint8_t motorCount) { - uint8_t index; TIM_TypeDef *lastTimerPtr = NULL; - for (index = 0; index < motorCount; index++) { - + for (int index = 0; index < motorCount; index++) { // Force the timer to overflow if it's the first motor to output, or if we change timers if (motors[index]->tim != lastTimerPtr) { lastTimerPtr = motors[index]->tim; timerForceOverflow(motors[index]->tim); } - // Set the compare register to 0, which stops the output pulsing if the timer overflows before the main loop completes again. // This compare register will be set to the output value on the next main loop. *motors[index]->ccr = 0; @@ -195,44 +203,46 @@ void pwmCompleteOneshotMotorUpdate(uint8_t motorCount) void pwmBrushedMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate) { - uint32_t hz = PWM_BRUSHED_TIMER_MHZ * 1000000; + const uint32_t hz = PWM_BRUSHED_TIMER_MHZ * 1000000; motors[motorIndex] = pwmOutConfig(timerHardware, PWM_BRUSHED_TIMER_MHZ, hz / motorPwmRate, 0); motors[motorIndex]->pwmWritePtr = pwmWriteBrushed; } void pwmBrushlessMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse) { - uint32_t hz = PWM_TIMER_MHZ * 1000000; + const uint32_t hz = PWM_TIMER_MHZ * 1000000; motors[motorIndex] = pwmOutConfig(timerHardware, PWM_TIMER_MHZ, hz / motorPwmRate, idlePulse); motors[motorIndex]->pwmWritePtr = pwmWriteStandard; } -void pwmFastPwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t fastPwmProtocolType, uint16_t motorPwmRate, uint16_t idlePulse) +void pwmFastPwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse, uint8_t fastPwmProtocolType) { uint32_t timerMhzCounter; + pwmWriteFuncPtr pwmWritePtr; switch (fastPwmProtocolType) { - default: - case (PWM_TYPE_ONESHOT125): - timerMhzCounter = ONESHOT125_TIMER_MHZ; - break; - case (PWM_TYPE_ONESHOT42): - timerMhzCounter = ONESHOT42_TIMER_MHZ; - break; - case (PWM_TYPE_MULTISHOT): - timerMhzCounter = MULTISHOT_TIMER_MHZ; + default: + case (PWM_TYPE_ONESHOT125): + timerMhzCounter = ONESHOT125_TIMER_MHZ; + pwmWritePtr = pwmWriteOneShot125; + break; + case (PWM_TYPE_ONESHOT42): + timerMhzCounter = ONESHOT42_TIMER_MHZ; + pwmWritePtr = pwmWriteOneShot42; + break; + case (PWM_TYPE_MULTISHOT): + timerMhzCounter = MULTISHOT_TIMER_MHZ; + pwmWritePtr = pwmWriteMultiShot; + break; } if (motorPwmRate > 0) { - uint32_t hz = timerMhzCounter * 1000000; + const uint32_t hz = timerMhzCounter * 1000000; motors[motorIndex] = pwmOutConfig(timerHardware, timerMhzCounter, hz / motorPwmRate, idlePulse); } else { motors[motorIndex] = pwmOutConfig(timerHardware, timerMhzCounter, 0xFFFF, 0); } - - motors[motorIndex]->pwmWritePtr = (fastPwmProtocolType == PWM_TYPE_MULTISHOT) ? pwmWriteMultiShot : - ((fastPwmProtocolType == PWM_TYPE_ONESHOT125) ? pwmWriteOneShot125 : - pwmWriteOneShot42); + motors[motorIndex]->pwmWritePtr = pwmWritePtr; } #ifdef USE_SERVOS @@ -243,7 +253,8 @@ void pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, ui void pwmWriteServo(uint8_t index, uint16_t value) { - if (servos[index] && index < MAX_SERVOS) + if (servos[index] && index < MAX_SERVOS) { *servos[index]->ccr = value; + } } #endif diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index c89e7fa80..b9bc62c68 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -25,6 +25,12 @@ typedef enum { PWM_TYPE_BRUSHED } motorPwmProtocolTypes_e; +struct timerHardware_s; +void pwmBrushedMotorConfig(const struct timerHardware_s *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate); +void pwmBrushlessMotorConfig(const struct timerHardware_s *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse); +void pwmFastPwmMotorConfig(const struct timerHardware_s *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse, uint8_t fastPwmProtocolType); +void pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse); + void pwmWriteMotor(uint8_t index, uint16_t value); void pwmShutdownPulsesForAllMotors(uint8_t motorCount); void pwmCompleteOneshotMotorUpdate(uint8_t motorCount);