From e802e2d03257901178b57ff043cf4abb42ae4006 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sat, 6 Dec 2014 11:39:31 +0000 Subject: [PATCH] Remove usage of feature() from pwm driver code so that driver code does not have a dependency on config.c. --- src/main/drivers/pwm_mapping.c | 4 +++- src/main/drivers/pwm_mapping.h | 1 + src/main/drivers/pwm_output.c | 38 ++++++++++++++-------------------- src/main/drivers/pwm_output.h | 4 +++- src/main/flight/mixer.c | 33 +++++++++++++++-------------- src/main/flight/mixer.h | 2 +- src/main/main.c | 1 + 7 files changed, 42 insertions(+), 41 deletions(-) diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index b7fa0a5cf9..1cd4dafee1 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -370,7 +370,9 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) pwmInConfig(timerHardwarePtr, channelIndex); channelIndex++; } else if (type == MAP_TO_MOTOR_OUTPUT) { - if (init->motorPwmRate > 500) { + if (init->useOneshot) { + pwmOneshotMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->idlePulse); + } else if (init->motorPwmRate > 500) { pwmBrushedMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->motorPwmRate, init->idlePulse); } else { pwmBrushlessMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->motorPwmRate, init->idlePulse); diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index 24787f2675..c0c6d751d2 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -45,6 +45,7 @@ typedef struct drv_pwm_config_t { bool useUART2; #endif bool useVbat; + bool useOneshot; bool useSoftSerial; bool useLEDStrip; bool useServos; diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 21286cc1fb..83dd65b2da 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -27,8 +27,6 @@ #include "flight/failsafe.h" // FIXME dependency into the main code from a driver -#include "config/config.h" // FIXME dependency into the main code from a driver - #include "pwm_mapping.h" #include "pwm_output.h" @@ -37,7 +35,7 @@ typedef void (*pwmWriteFuncPtr)(uint8_t index, uint16_t value); // function poi typedef struct { volatile timCCR_t *ccr; - volatile TIM_TypeDef *tim; + TIM_TypeDef *tim; uint16_t period; pwmWriteFuncPtr pwmWritePtr; } pwmOutputPort_t; @@ -140,29 +138,23 @@ void pwmWriteMotor(uint8_t index, uint16_t value) motors[index]->pwmWritePtr(index, value); } -void pwmFinishedWritingMotors(uint8_t numberMotors) +void pwmCompleteOneshotMotorUpdate(uint8_t motorCount) { uint8_t index; - volatile TIM_TypeDef *lastTimerPtr = NULL; + TIM_TypeDef *lastTimerPtr = NULL; + for(index = 0; index < motorCount; index++){ - if(feature(FEATURE_ONESHOT125)){ + // 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; - for(index = 0; index < numberMotors; 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); - } + 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. - for(index = 0; index < numberMotors; index++){ - *motors[index]->ccr = 0; - } + *motors[index]->ccr = 0; } } @@ -183,13 +175,13 @@ void pwmBrushedMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIn void pwmBrushlessMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse) { uint32_t hz = PWM_TIMER_MHZ * 1000000; + motors[motorIndex] = pwmOutConfig(timerHardware, PWM_TIMER_MHZ, hz / motorPwmRate, idlePulse); + motors[motorIndex]->pwmWritePtr = pwmWriteStandard; +} - if(feature(FEATURE_ONESHOT125)){ - motors[motorIndex] = pwmOutConfig(timerHardware, ONESHOT125_TIMER_MHZ, 0xFFFF, idlePulse); - } else { - motors[motorIndex] = pwmOutConfig(timerHardware, PWM_TIMER_MHZ, hz / motorPwmRate, idlePulse); - } - +void pwmOneshotMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t idlePulse) +{ + motors[motorIndex] = pwmOutConfig(timerHardware, ONESHOT125_TIMER_MHZ, 0xFFFF, idlePulse); motors[motorIndex]->pwmWritePtr = pwmWriteStandard; } diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 3f91b1fec5..40689e9f9f 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -19,8 +19,10 @@ void pwmBrushedMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse); void pwmBrushlessMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse); +void pwmOneshotMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t idlePulse); + void pwmWriteMotor(uint8_t index, uint16_t value); -void pwmFinishedWritingMotors(uint8_t numberMotors); +void pwmCompleteOneshotMotorUpdate(uint8_t motorCount); void pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse); void pwmWriteServo(uint8_t index, uint16_t value); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index b2d73324e3..46e50600ec 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -42,7 +42,7 @@ #define AUX_FORWARD_CHANNEL_TO_SERVO_COUNT 4 -static uint8_t numberMotor = 0; +static uint8_t motorCount = 0; int16_t motor[MAX_SUPPORTED_MOTORS]; int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; int16_t servo[MAX_SUPPORTED_SERVOS] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 }; @@ -271,21 +271,21 @@ void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfigura if (customMixers[i].throttle == 0.0f) break; currentMixer[i] = customMixers[i]; - numberMotor++; + motorCount++; } } else { - numberMotor = mixers[currentMixerConfiguration].numberMotor; + motorCount = mixers[currentMixerConfiguration].motorCount; // copy motor-based mixers if (mixers[currentMixerConfiguration].motor) { - for (i = 0; i < numberMotor; i++) + for (i = 0; i < motorCount; i++) currentMixer[i] = mixers[currentMixerConfiguration].motor[i]; } } // in 3D mode, mixer gain has to be halved if (feature(FEATURE_3D)) { - if (numberMotor > 1) { - for (i = 0; i < numberMotor; i++) { + if (motorCount > 1) { + for (i = 0; i < motorCount; i++) { currentMixer[i].pitch *= 0.5f; currentMixer[i].roll *= 0.5f; currentMixer[i].yaw *= 0.5f; @@ -323,7 +323,7 @@ void mixerLoadMix(int index, motorMixer_t *customMixers) // do we have anything here to begin with? if (mixers[index].motor != NULL) { - for (i = 0; i < mixers[index].numberMotor; i++) + for (i = 0; i < mixers[index].motorCount; i++) customMixers[i] = mixers[index].motor[i]; } } @@ -393,10 +393,13 @@ void writeMotors(void) { uint8_t i; - for (i = 0; i < numberMotor; i++) + for (i = 0; i < motorCount; i++) pwmWriteMotor(i, motor[i]); - pwmFinishedWritingMotors(numberMotor); + + if (feature(FEATURE_ONESHOT125)) { + pwmCompleteOneshotMotorUpdate(motorCount); + } } void writeAllMotors(int16_t mc) @@ -404,7 +407,7 @@ void writeAllMotors(int16_t mc) uint8_t i; // Sends commands to all motors - for (i = 0; i < numberMotor; i++) + for (i = 0; i < motorCount; i++) motor[i] = mc; writeMotors(); } @@ -460,14 +463,14 @@ void mixTable(void) int16_t maxMotor; uint32_t i; - if (numberMotor > 3) { + if (motorCount > 3) { // prevent "yaw jump" during yaw correction axisPID[YAW] = constrain(axisPID[YAW], -100 - abs(rcCommand[YAW]), +100 + abs(rcCommand[YAW])); } // motors for non-servo mixes - if (numberMotor > 1) - for (i = 0; i < numberMotor; i++) + if (motorCount > 1) + for (i = 0; i < motorCount; i++) motor[i] = rcCommand[THROTTLE] * currentMixer[i].throttle + axisPID[PITCH] * currentMixer[i].pitch + axisPID[ROLL] * currentMixer[i].roll + -mixerConfig->yaw_direction * axisPID[YAW] * currentMixer[i].yaw; // airplane / servo mixes @@ -568,10 +571,10 @@ void mixTable(void) } maxMotor = motor[0]; - for (i = 1; i < numberMotor; i++) + for (i = 1; i < motorCount; i++) if (motor[i] > maxMotor) maxMotor = motor[i]; - for (i = 0; i < numberMotor; i++) { + for (i = 0; i < motorCount; i++) { if (maxMotor > escAndServoConfig->maxthrottle) // this is a way to still have good gyro corrections if at least one motor reaches its max. motor[i] -= maxMotor - escAndServoConfig->maxthrottle; if (feature(FEATURE_3D)) { diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index ad846dc334..a0cd547ba3 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -58,7 +58,7 @@ typedef struct motorMixer_t { // Custom mixer configuration typedef struct mixer_t { - uint8_t numberMotor; + uint8_t motorCount; uint8_t useServo; const motorMixer_t *motor; } mixer_t; diff --git a/src/main/main.c b/src/main/main.c index e77d5b3046..c772305ff0 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -269,6 +269,7 @@ void init(void) pwm_params.useCurrentMeterADC = feature(FEATURE_CURRENT_METER); pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP); pwm_params.usePPM = feature(FEATURE_RX_PPM); + pwm_params.useOneshot = feature(FEATURE_ONESHOT125); pwm_params.useServos = isMixerUsingServos(); pwm_params.extraServos = currentProfile->gimbalConfig.gimbal_flags & GIMBAL_FORWARDAUX; pwm_params.motorPwmRate = masterConfig.motor_pwm_rate;