From ccb4f77ae260dee1cb3a1a6de160264024d2c6b7 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Fri, 1 Dec 2017 06:04:35 +0000 Subject: [PATCH] Tidy of mixer and servo code --- src/main/flight/mixer.c | 47 +++++++++++++++------------------------- src/main/flight/servos.c | 24 ++++++++++---------- 2 files changed, 28 insertions(+), 43 deletions(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index a80fca4c1..0be3ae39c 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -416,8 +416,9 @@ void mixerConfigureOutput(void) // load custom mixer into currentMixer for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) { // check if done - if (customMotorMixer(i)->throttle == 0.0f) + if (customMotorMixer(i)->throttle == 0.0f) { break; + } currentMixer[i] = *customMotorMixer(i); motorCount++; } @@ -455,7 +456,6 @@ void mixerLoadMix(int index, motorMixer_t *customMixers) for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) { customMixers[i].throttle = 0.0f; } - // do we have anything here to begin with? if (mixers[index].motor != NULL) { for (int i = 0; i < mixers[index].motorCount; i++) { @@ -467,14 +467,12 @@ void mixerLoadMix(int index, motorMixer_t *customMixers) void mixerConfigureOutput(void) { motorCount = QUAD_MOTOR_COUNT; - for (int i = 0; i < motorCount; i++) { currentMixer[i] = mixerQuadX[i]; } - mixerResetDisarmedMotors(); } -#endif +#endif // USE_QUAD_MIXER_ONLY void mixerResetDisarmedMotors(void) { @@ -500,14 +498,12 @@ static void writeAllMotors(int16_t mc) for (int i = 0; i < motorCount; i++) { motor[i] = mc; } - writeMotors(); } void stopMotors(void) { writeAllMotors(disarmMotorOutput); - delay(50); // give the timers and ESCs a chance to react. } @@ -524,21 +520,21 @@ static float motorRangeMax; static float motorOutputRange; static int8_t motorOutputMixSign; -void calculateThrottleAndCurrentMotorEndpoints(void) +static void calculateThrottleAndCurrentMotorEndpoints(void) { static uint16_t rcThrottlePrevious = 0; // Store the last throttle direction for deadband transitions float currentThrottleInputRange = 0; - if(feature(FEATURE_3D)) { + if (feature(FEATURE_3D)) { if (!ARMING_FLAG(ARMED)) { rcThrottlePrevious = rxConfig()->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming. } - if(rcCommand[THROTTLE] <= rcCommand3dDeadBandLow) { + if (rcCommand[THROTTLE] <= rcCommand3dDeadBandLow) { // INVERTED motorRangeMin = motorOutputLow; motorRangeMax = deadbandMotor3dLow; - if(isMotorProtocolDshot()) { + if (isMotorProtocolDshot()) { motorOutputMin = motorOutputLow; motorOutputRange = deadbandMotor3dLow - motorOutputLow; } else { @@ -549,7 +545,7 @@ void calculateThrottleAndCurrentMotorEndpoints(void) rcThrottlePrevious = rcCommand[THROTTLE]; throttle = rcCommand3dDeadBandLow - rcCommand[THROTTLE]; currentThrottleInputRange = rcCommandThrottleRange3dLow; - } else if(rcCommand[THROTTLE] >= rcCommand3dDeadBandHigh) { + } else if (rcCommand[THROTTLE] >= rcCommand3dDeadBandHigh) { // NORMAL motorRangeMin = deadbandMotor3dHigh; motorRangeMax = motorOutputHigh; @@ -559,13 +555,13 @@ void calculateThrottleAndCurrentMotorEndpoints(void) rcThrottlePrevious = rcCommand[THROTTLE]; throttle = rcCommand[THROTTLE] - rcCommand3dDeadBandHigh; currentThrottleInputRange = rcCommandThrottleRange3dHigh; - } else if((rcThrottlePrevious <= rcCommand3dDeadBandLow && - !isModeActivationConditionPresent(BOX3DONASWITCH)) || + } else if ((rcThrottlePrevious <= rcCommand3dDeadBandLow && + !isModeActivationConditionPresent(BOX3DONASWITCH)) || isMotorsReversed()) { // INVERTED_TO_DEADBAND motorRangeMin = motorOutputLow; motorRangeMax = deadbandMotor3dLow; - if(isMotorProtocolDshot()) { + if (isMotorProtocolDshot()) { motorOutputMin = motorOutputLow; motorOutputRange = deadbandMotor3dLow - motorOutputLow; } else { @@ -602,21 +598,18 @@ void calculateThrottleAndCurrentMotorEndpoints(void) static void applyFlipOverAfterCrashModeToMotors(void) { - float motorMix[MAX_SUPPORTED_MOTORS]; - if (ARMING_FLAG(ARMED)) { + float motorMix[MAX_SUPPORTED_MOTORS]; for (int i = 0; i < motorCount; i++) { if (getRcDeflectionAbs(FD_ROLL) > getRcDeflectionAbs(FD_PITCH)) { motorMix[i] = getRcDeflection(FD_ROLL) * currentMixer[i].roll * -1; } else { motorMix[i] = getRcDeflection(FD_PITCH) * currentMixer[i].pitch * -1; } - // Apply the mix to motor endpoints float motorOutput = motorOutputMin + motorOutputRange * motorMix[i]; - //Add a little bit to the motorOutputMin so props aren't spinning when sticks are centered + // Add a little bit to the motorOutputMin so props aren't spinning when sticks are centered motorOutput = (motorOutput < motorOutputMin + CRASH_FLIP_DEADBAND ) ? disarmMotorOutput : motorOutput - CRASH_FLIP_DEADBAND; - motor[i] = motorOutput; } } else { @@ -633,17 +626,14 @@ static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS]) // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips. for (uint32_t i = 0; i < motorCount; i++) { float motorOutput = motorOutputMin + (motorOutputRange * (motorOutputMixSign * motorMix[i] + throttle * currentMixer[i].throttle)); - if (failsafeIsActive()) { if (isMotorProtocolDshot()) { motorOutput = (motorOutput < motorRangeMin) ? disarmMotorOutput : motorOutput; // Prevent getting into special reserved range } - motorOutput = constrain(motorOutput, disarmMotorOutput, motorRangeMax); } else { motorOutput = constrain(motorOutput, motorRangeMin, motorRangeMax); } - // Motor stop handling if (feature(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED) && !feature(FEATURE_3D) && !isAirmodeActive()) { if (((rcData[THROTTLE]) < rxConfig()->mincheck)) { @@ -664,7 +654,7 @@ static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS]) void mixTable(uint8_t vbatPidCompensation) { if (isFlipOverAfterCrashMode()) { - applyFlipOverAfterCrashModeToMotors(); + applyFlipOverAfterCrashModeToMotors(); return; } @@ -672,9 +662,9 @@ void mixTable(uint8_t vbatPidCompensation) calculateThrottleAndCurrentMotorEndpoints(); // Calculate and Limit the PIDsum - float scaledAxisPidRoll = + const float scaledAxisPidRoll = constrainf(axisPID_P[FD_ROLL] + axisPID_I[FD_ROLL] + axisPID_D[FD_ROLL], -currentPidProfile->pidSumLimit, currentPidProfile->pidSumLimit) / PID_MIXER_SCALING; - float scaledAxisPidPitch = + const float scaledAxisPidPitch = constrainf(axisPID_P[FD_PITCH] + axisPID_I[FD_PITCH] + axisPID_D[FD_PITCH], -currentPidProfile->pidSumLimit, currentPidProfile->pidSumLimit) / PID_MIXER_SCALING; float scaledAxisPidYaw = constrainf(axisPID_P[FD_YAW] + axisPID_I[FD_YAW], -currentPidProfile->pidSumLimitYaw, currentPidProfile->pidSumLimitYaw) / PID_MIXER_SCALING; @@ -683,7 +673,7 @@ void mixTable(uint8_t vbatPidCompensation) } // Calculate voltage compensation - const float vbatCompensationFactor = (vbatPidCompensation) ? calculateVbatPidCompensation() : 1.0f; + const float vbatCompensationFactor = vbatPidCompensation ? calculateVbatPidCompensation() : 1.0f; // Find roll/pitch/yaw desired output float motorMix[MAX_SUPPORTED_MOTORS]; @@ -750,7 +740,6 @@ float convertExternalToMotor(uint16_t externalValue) #endif default: motorValue = externalValue; - break; } @@ -774,13 +763,11 @@ uint16_t convertMotorToExternal(float motorValue) } else { externalValue = (motorValue < DSHOT_MIN_THROTTLE) ? PWM_RANGE_MIN : scaleRange(motorValue, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE, PWM_RANGE_MIN + 1, PWM_RANGE_MAX); } - break; case false: #endif default: externalValue = motorValue; - break; } diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index aea88cb10..c8b4be36a 100644 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -213,10 +213,11 @@ int16_t determineServoMiddleOrForwardFromChannel(servoIndex_e servoIndex) int servoDirection(int servoIndex, int inputSource) { // determine the direction (reversed or not) from the direction bitfield of the servo - if (servoParams(servoIndex)->reversedSources & (1 << inputSource)) + if (servoParams(servoIndex)->reversedSources & (1 << inputSource)) { return -1; - else + } else { return 1; + } } void servosInit(void) @@ -224,8 +225,9 @@ void servosInit(void) // enable servos for mixes that require them. note, this shifts motor counts. useServo = mixers[currentMixerMode].useServo; // if we want camstab/trig, that also enables servos, even if mixer doesn't - if (feature(FEATURE_SERVO_TILT) || feature(FEATURE_CHANNEL_FORWARDING)) + if (feature(FEATURE_SERVO_TILT) || feature(FEATURE_CHANNEL_FORWARDING)) { useServo = 1; + } // give all servos a default command for (uint8_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) { @@ -242,9 +244,9 @@ void loadCustomServoMixer(void) // load custom mixer into currentServoMixer for (int i = 0; i < MAX_SERVO_RULES; i++) { // check if done - if (customServoMixers(i)->rate == 0) + if (customServoMixers(i)->rate == 0) { break; - + } currentServoMixer[i] = *customServoMixers(i); servoRuleCount++; } @@ -266,13 +268,11 @@ void servoConfigureOutput(void) currentMixerMode == MIXER_CUSTOM_AIRPLANE ) { ENABLE_STATE(FIXED_WING); - if (currentMixerMode == MIXER_CUSTOM_AIRPLANE) { loadCustomServoMixer(); } } else { DISABLE_STATE(FIXED_WING); - if (currentMixerMode == MIXER_CUSTOM_TRI) { loadCustomServoMixer(); } @@ -288,7 +288,6 @@ void servoMixerLoadMix(int index) for (int i = 0; i < MAX_SERVO_RULES; i++) { customServoMixersMutable(i)->targetChannel = customServoMixersMutable(i)->inputSource = customServoMixersMutable(i)->rate = customServoMixersMutable(i)->box = 0; } - for (int i = 0; i < servoMixers[index].servoRuleCount; i++) { *customServoMixersMutable(i) = servoMixers[index].rule[i]; } @@ -297,8 +296,8 @@ void servoMixerLoadMix(int index) STATIC_UNIT_TESTED void forwardAuxChannelsToServos(uint8_t firstServoIndex) { // start forwarding from this channel - uint8_t channelOffset = servoConfig()->channelForwardingStartChannel; - for (uint8_t servoOffset = 0; servoOffset < MAX_AUX_CHANNEL_COUNT && channelOffset < MAX_SUPPORTED_RC_CHANNEL_COUNT; servoOffset++) { + int channelOffset = servoConfig()->channelForwardingStartChannel; + for (int servoOffset = 0; servoOffset < MAX_AUX_CHANNEL_COUNT && channelOffset < MAX_SUPPORTED_RC_CHANNEL_COUNT; servoOffset++) { pwmWriteServo(firstServoIndex + servoOffset, rcData[channelOffset++]); } } @@ -329,7 +328,7 @@ void writeServos(void) pwmWriteServo(servoIndex++, servo[SERVO_HELI_RIGHT]); pwmWriteServo(servoIndex++, servo[SERVO_HELI_TOP]); pwmWriteServo(servoIndex++, servo[SERVO_HELI_RUD]); - break; + break; case MIXER_TRI: case MIXER_CUSTOM_TRI: @@ -535,7 +534,6 @@ static void filterServos(void) #if defined(MIXER_DEBUG) uint32_t startTime = micros(); #endif - if (servoConfig()->servo_lowpass_freq) { for (int servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { servo[servoIdx] = lrintf(biquadFilterApply(&servoFilter[servoIdx], (float)servo[servoIdx])); @@ -547,4 +545,4 @@ static void filterServos(void) debug[0] = (int16_t)(micros() - startTime); #endif } -#endif +#endif // USE_SERVOS