Rework maxing out motors

src/main/flight/mixer.c

@@ -69,7 +69,7 @@ static rxConfig_t *rxConfig;
 static mixerMode_e currentMixerMode;
 static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
 
-float totalErrorRatioLimit = 1.0f;
+bool motorLimitReached = false;
 
 #ifdef USE_SERVOS
 static uint8_t servoRuleCount = 0;
@@ -751,7 +751,7 @@ void mixTable(void)
     }
 
     if (!(IS_RC_MODE_ACTIVE(BOXAIRMODE)) && !(feature(FEATURE_3D))) {
-        totalErrorRatioLimit = 1.0f; // It  always needs to be full ratio so it can't get stuck when flipping back and fourth
+        motorLimitReached = false; // It  always needs to be reset so it can't get stuck when flipping back and fourth
         // motors for non-servo mixes
         for (i = 0; i < motorCount; i++) {
             motor[i] =
@@ -761,6 +761,7 @@ void mixTable(void)
                 -mixerConfig->yaw_motor_direction * axisPID[YAW] * currentMixer[i].yaw;
         }
     } else {
+        // Initial mixer concept by bdoiron74 reused and optimized for Air Mode
         int16_t rollPitchYawMix[MAX_SUPPORTED_MOTORS];
         int16_t rollPitchYawMixMax = 0; // assumption: symetrical about zero.
         int16_t rollPitchYawMixMin = 0;
@@ -782,25 +783,23 @@ void mixTable(void)
         int16_t throttleMin, throttleMax;
 
         if (rollPitchYawMixRange > throttleRange) {
+            motorLimitReached = true;
             for (i = 0; i < motorCount; i++) {
                 rollPitchYawMix[i] = (rollPitchYawMix[i] * throttleRange) / rollPitchYawMixRange;
             }
-            throttleMin = throttleMax = escAndServoConfig->minthrottle + (throttleRange / 2);
+            throttleMin = escAndServoConfig->minthrottle;
+            throttleMax = escAndServoConfig->maxthrottle;
         } else {
+            motorLimitReached = false;
             throttleMin = escAndServoConfig->minthrottle + (rollPitchYawMixRange / 2);
             throttleMax = escAndServoConfig->maxthrottle - (rollPitchYawMixRange / 2);
         }
 
         // 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.
-        //
-        // TODO: handle the case when motors don't all get the same throttle factor...
         for (i = 0; i < motorCount; i++) {
             motor[i] = rollPitchYawMix[i] + constrainf(rcCommand[THROTTLE] * currentMixer[i].throttle, throttleMin, throttleMax);
         }
-
-        // adjust feedback to scale PID error inputs to our limitations.
-        totalErrorRatioLimit = constrainf(((float)throttleRange / rollPitchYawMixRange), 0.4f, 1.0f);
     }
 
     if (ARMING_FLAG(ARMED)) {

src/main/flight/pid.c

@@ -48,7 +48,7 @@
 #include "config/runtime_config.h"
 
 extern float dT;
-extern float totalErrorRatioLimit;
+extern bool motorLimitReached;
 extern bool allowITermShrinkOnly;
 
 int16_t axisPID[3];
@@ -161,10 +161,6 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
         // multiplication of rcCommand corresponds to changing the sticks scaling here
         RateError = AngleRate - gyroRate;
 
-        if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
-            RateError = RateError * totalErrorRatioLimit;
-        }
-
         // -----calculate P component
         PTerm = RateError * pidProfile->P_f[axis] * PIDweight[axis] / 100;
 
@@ -175,7 +171,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
         // -----calculate I component.
         errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * 10, -250.0f, 250.0f);
 
-        if (allowITermShrinkOnly || totalErrorRatioLimit < 0.98f) {
+        if (allowITermShrinkOnly || motorLimitReached) {
             if (ABS(errorGyroIf[axis]) < ABS(previousErrorGyroIf[axis])) {
                 previousErrorGyroIf[axis] = errorGyroIf[axis];
             } else {
@@ -302,10 +298,6 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
         // multiplication of rcCommand corresponds to changing the sticks scaling here
         RateError = AngleRateTmp - (gyroADC[axis] / 4);
 
-        if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
-            RateError = RateError * totalErrorRatioLimit;
-        }
-
         // -----calculate P component
         PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7;
 
@@ -326,7 +318,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
 
         ITerm = errorGyroI[axis] >> 13;
 
-        if (allowITermShrinkOnly || totalErrorRatioLimit < 0.98f) {
+        if (allowITermShrinkOnly || motorLimitReached) {
             if (ABS(errorGyroI[axis]) < ABS(previousErrorGyroI[axis])) {
                 previousErrorGyroI[axis] = errorGyroI[axis];
             } else {