From 40a2bee49201358ce8121c5da1cf40d167835a54 Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Fri, 21 Apr 2017 13:18:23 +1200 Subject: [PATCH 1/2] Made float parameter calculation static. --- src/main/flight/mixer.c | 15 ++++++++++++--- src/main/flight/mixer.h | 3 ++- src/main/flight/pid.c | 1 + 3 files changed, 15 insertions(+), 4 deletions(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index aef38122da..f02319291d 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -128,6 +128,9 @@ int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; mixerMode_e currentMixerMode; static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS]; +float pidSumLimit; +float pidSumLimitYaw; + static const motorMixer_t mixerQuadX[] = { { 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R @@ -391,6 +394,12 @@ void mixerInit(mixerMode_e mixerMode) initEscEndpoints(); } +void pidInitMixer(struct pidProfile_s *pidProfile) +{ + pidSumLimit = CONVERT_PARAMETER_TO_FLOAT(pidProfile->pidSumLimit); + pidSumLimitYaw = CONVERT_PARAMETER_TO_FLOAT(pidProfile->pidSumLimitYaw); +} + #ifndef USE_QUAD_MIXER_ONLY void mixerConfigureOutput(void) @@ -555,13 +564,13 @@ void mixTable(pidProfile_t *pidProfile) // Calculate and Limit the PIDsum scaledAxisPIDf[FD_ROLL] = constrainf((axisPID_P[FD_ROLL] + axisPID_I[FD_ROLL] + axisPID_D[FD_ROLL]) / PID_MIXER_SCALING, - -CONVERT_PARAMETER_TO_FLOAT(pidProfile->pidSumLimit), CONVERT_PARAMETER_TO_FLOAT(pidProfile->pidSumLimit)); + -pidSumLimit, pidSumLimit); scaledAxisPIDf[FD_PITCH] = constrainf((axisPID_P[FD_PITCH] + axisPID_I[FD_PITCH] + axisPID_D[FD_PITCH]) / PID_MIXER_SCALING, - -CONVERT_PARAMETER_TO_FLOAT(pidProfile->pidSumLimit), CONVERT_PARAMETER_TO_FLOAT(pidProfile->pidSumLimit)); + -pidSumLimit, pidSumLimit); scaledAxisPIDf[FD_YAW] = constrainf((axisPID_P[FD_YAW] + axisPID_I[FD_YAW]) / PID_MIXER_SCALING, - -CONVERT_PARAMETER_TO_FLOAT(pidProfile->pidSumLimit), CONVERT_PARAMETER_TO_FLOAT(pidProfile->pidSumLimitYaw)); + -pidSumLimitYaw, pidSumLimitYaw); // Calculate voltage compensation const float vbatCompensationFactor = (pidProfile->vbatPidCompensation) ? calculateVbatPidCompensation() : 1.0f; diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index ad6bcdb1c4..ff9f5fd4cd 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -128,11 +128,12 @@ bool mixerIsOutputSaturated(int axis, float errorRate); void mixerLoadMix(int index, motorMixer_t *customMixers); void mixerInit(mixerMode_e mixerMode); +struct pidProfile_s; +void pidInitMixer(struct pidProfile_s *pidProfile); void mixerConfigureOutput(void); void mixerResetDisarmedMotors(void); -struct pidProfile_s; void mixTable(struct pidProfile_s *pidProfile); void syncMotors(bool enabled); void writeMotors(void); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 819d2ae6ae..c397ef900f 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -277,6 +277,7 @@ void pidInit(const pidProfile_t *pidProfile) pidSetTargetLooptime(gyro.targetLooptime * pidConfig()->pid_process_denom); // Initialize pid looptime pidInitFilters(pidProfile); pidInitConfig(pidProfile); + pidInitMixer(pidProfile); } // calculates strength of horizon leveling; 0 = none, 1.0 = most leveling From 991e69974f49e676a85c9d67b689edbf020fc3ca Mon Sep 17 00:00:00 2001 From: mikeller Date: Sun, 23 Apr 2017 18:26:06 +1200 Subject: [PATCH 2/2] Added more mixer optimisations from #2898. --- src/main/flight/mixer.c | 61 ++++++++++++++++++++--------------------- src/main/flight/mixer.h | 2 +- 2 files changed, 31 insertions(+), 32 deletions(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index f02319291d..e7cd380dfc 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -394,7 +394,7 @@ void mixerInit(mixerMode_e mixerMode) initEscEndpoints(); } -void pidInitMixer(struct pidProfile_s *pidProfile) +void pidInitMixer(const struct pidProfile_s *pidProfile) { pidSumLimit = CONVERT_PARAMETER_TO_FLOAT(pidProfile->pidSumLimit); pidSumLimitYaw = CONVERT_PARAMETER_TO_FLOAT(pidProfile->pidSumLimitYaw); @@ -560,17 +560,13 @@ void mixTable(pidProfile_t *pidProfile) throttle = constrainf(throttle / currentThrottleInputRange, 0.0f, 1.0f); const float motorOutputRange = motorOutputMax - motorOutputMin; - float scaledAxisPIDf[3]; // Calculate and Limit the PIDsum - scaledAxisPIDf[FD_ROLL] = - constrainf((axisPID_P[FD_ROLL] + axisPID_I[FD_ROLL] + axisPID_D[FD_ROLL]) / PID_MIXER_SCALING, - -pidSumLimit, pidSumLimit); - scaledAxisPIDf[FD_PITCH] = - constrainf((axisPID_P[FD_PITCH] + axisPID_I[FD_PITCH] + axisPID_D[FD_PITCH]) / PID_MIXER_SCALING, - -pidSumLimit, pidSumLimit); - scaledAxisPIDf[FD_YAW] = - constrainf((axisPID_P[FD_YAW] + axisPID_I[FD_YAW]) / PID_MIXER_SCALING, - -pidSumLimitYaw, pidSumLimitYaw); + const float scaledAxisPidRoll = + constrainf((axisPID_P[FD_ROLL] + axisPID_I[FD_ROLL] + axisPID_D[FD_ROLL]) / PID_MIXER_SCALING, -pidSumLimit, pidSumLimit); + const float scaledAxisPidPitch = + constrainf((axisPID_P[FD_PITCH] + axisPID_I[FD_PITCH] + axisPID_D[FD_PITCH]) / PID_MIXER_SCALING, -pidSumLimit, pidSumLimit); + const float scaledAxisPidYaw = + constrainf((axisPID_P[FD_YAW] + axisPID_I[FD_YAW]) / PID_MIXER_SCALING, -pidSumLimitYaw, pidSumLimitYaw); // Calculate voltage compensation const float vbatCompensationFactor = (pidProfile->vbatPidCompensation) ? calculateVbatPidCompensation() : 1.0f; @@ -578,21 +574,23 @@ void mixTable(pidProfile_t *pidProfile) // Find roll/pitch/yaw desired output float motorMix[MAX_SUPPORTED_MOTORS]; float motorMixMax = 0, motorMixMin = 0; + const int yawDirection = GET_DIRECTION(mixerConfig()->yaw_motors_reversed); for (int i = 0; i < motorCount; i++) { - motorMix[i] = - scaledAxisPIDf[PITCH] * currentMixer[i].pitch + - scaledAxisPIDf[ROLL] * currentMixer[i].roll + - scaledAxisPIDf[YAW] * currentMixer[i].yaw * -GET_DIRECTION(mixerConfig()->yaw_motors_reversed); + float mix = + scaledAxisPidRoll * currentMixer[i].roll + + scaledAxisPidPitch * currentMixer[i].pitch + + scaledAxisPidYaw * currentMixer[i].yaw * (-yawDirection); if (vbatCompensationFactor > 1.0f) { - motorMix[i] *= vbatCompensationFactor; // Add voltage compensation + mix *= vbatCompensationFactor; // Add voltage compensation } - if (motorMix[i] > motorMixMax) { - motorMixMax = motorMix[i]; - } else if (motorMix[i] < motorMixMin) { - motorMixMin = motorMix[i]; + if (mix > motorMixMax) { + motorMixMax = mix; + } else if (mix < motorMixMin) { + motorMixMin = mix; } + motorMix[i] = mix; } motorMixRange = motorMixMax - motorMixMin; @@ -607,42 +605,43 @@ void mixTable(pidProfile_t *pidProfile) } } else { if (isAirmodeActive()) { // Only automatically adjust throttle during airmode scenario - float throttleLimitOffset = motorMixRange / 2.0f; + const float throttleLimitOffset = motorMixRange / 2.0f; throttle = constrainf(throttle, 0.0f + throttleLimitOffset, 1.0f - throttleLimitOffset); } } // Now add in the desired throttle, but keep in a range that doesn't clip adjusted // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips. - uint32_t i = 0; - for (i = 0; i < motorCount; i++) { - motor[i] = motorOutputMin + lrintf(motorOutputRange * (motorMix[i] + (throttle * currentMixer[i].throttle))); + for (uint32_t i = 0; i < motorCount; i++) { + float motorOutput = motorOutputMin + lrintf(motorOutputRange * (motorMix[i] + (throttle * currentMixer[i].throttle))); // Dshot works exactly opposite in lower 3D section. if (mixerInversion) { - motor[i] = motorOutputMin + (motorOutputMax - motor[i]); + motorOutput = motorOutputMin + (motorOutputMax - motorOutput); } if (failsafeIsActive()) { - if (isMotorProtocolDshot()) - motor[i] = (motor[i] < motorOutputMin) ? disarmMotorOutput : motor[i]; // Prevent getting into special reserved range + if (isMotorProtocolDshot()) { + motorOutput = (motorOutput < motorOutputMin) ? disarmMotorOutput : motorOutput; // Prevent getting into special reserved range + } - motor[i] = constrain(motor[i], disarmMotorOutput, motorOutputMax); + motorOutput = constrain(motorOutput, disarmMotorOutput, motorOutputMax); } else { - motor[i] = constrain(motor[i], motorOutputMin, motorOutputMax); + motorOutput = constrain(motorOutput, motorOutputMin, motorOutputMax); } // Motor stop handling if (feature(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED) && !feature(FEATURE_3D) && !isAirmodeActive()) { if (((rcData[THROTTLE]) < rxConfig()->mincheck)) { - motor[i] = disarmMotorOutput; + motorOutput = disarmMotorOutput; } } + motor[i] = motorOutput; } // Disarmed mode if (!ARMING_FLAG(ARMED)) { - for (i = 0; i < motorCount; i++) { + for (int i = 0; i < motorCount; i++) { motor[i] = motor_disarmed[i]; } } diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index ff9f5fd4cd..51733d31e7 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -129,7 +129,7 @@ bool mixerIsOutputSaturated(int axis, float errorRate); void mixerLoadMix(int index, motorMixer_t *customMixers); void mixerInit(mixerMode_e mixerMode); struct pidProfile_s; -void pidInitMixer(struct pidProfile_s *pidProfile); +void pidInitMixer(const struct pidProfile_s *pidProfile); void mixerConfigureOutput(void);