Merge pull request #4702 from martinbudden/bfa_3d_iterm

Reset ITerm on motor reversal

src/main/fc/fc_core.c

@@ -421,7 +421,7 @@ void processRx(timeUs_t currentTimeUs)
     /* In airmode Iterm should be prevented to grow when Low thottle and Roll + Pitch Centered.
      This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air */
     if (throttleStatus == THROTTLE_LOW && !airmodeIsActivated) {
-        pidResetErrorGyroState();
+        pidResetITerm();
         if (currentPidProfile->pidAtMinThrottle)
             pidStabilisationState(PID_STABILISATION_ON);
         else

src/main/flight/mixer.c

@@ -541,6 +541,10 @@ static void calculateThrottleAndCurrentMotorEndpoints(void)
                 motorOutputMin = deadbandMotor3dLow;
                 motorOutputRange = motorOutputLow - deadbandMotor3dLow;
             }
+            if (motorOutputMixSign != -1) {
+                // reset ITerm on motor reversal
+                pidResetITerm();
+            }
             motorOutputMixSign = -1;
             rcThrottlePrevious = rcCommand[THROTTLE];
             throttle = rcCommand3dDeadBandLow - rcCommand[THROTTLE];
@@ -551,6 +555,10 @@ static void calculateThrottleAndCurrentMotorEndpoints(void)
             motorRangeMax = motorOutputHigh;
             motorOutputMin = deadbandMotor3dHigh;
             motorOutputRange = motorOutputHigh - deadbandMotor3dHigh;
+            if (motorOutputMixSign != 1) {
+                // reset ITerm on motor reversal
+                pidResetITerm();
+            }
             motorOutputMixSign = 1;
             rcThrottlePrevious = rcCommand[THROTTLE];
             throttle = rcCommand[THROTTLE] - rcCommand3dDeadBandHigh;
@@ -568,6 +576,10 @@ static void calculateThrottleAndCurrentMotorEndpoints(void)
                 motorOutputMin = deadbandMotor3dLow;
                 motorOutputRange = motorOutputLow - deadbandMotor3dLow;
             }
+            if (motorOutputMixSign != -1) {
+                // reset ITerm on motor reversal
+                pidResetITerm();
+            }
             motorOutputMixSign = -1;
             throttle = 0;
             currentThrottleInputRange = rcCommandThrottleRange3dLow;
@@ -577,6 +589,10 @@ static void calculateThrottleAndCurrentMotorEndpoints(void)
             motorRangeMax = motorOutputHigh;
             motorOutputMin = deadbandMotor3dHigh;
             motorOutputRange = motorOutputHigh - deadbandMotor3dHigh;
+            if (motorOutputMixSign != 1) {
+                // reset ITerm on motor reversal
+                pidResetITerm();
+            }
             motorOutputMixSign = 1;
             throttle = 0;
             currentThrottleInputRange = rcCommandThrottleRange3dHigh;

src/main/flight/pid.c

@@ -136,7 +136,7 @@ static void pidSetTargetLooptime(uint32_t pidLooptime)
     dT = (float)targetPidLooptime * 0.000001f;
 }
 
-void pidResetErrorGyroState(void)
+void pidResetITerm(void)
 {
     for (int axis = 0; axis < 3; axis++) {
         axisPID_I[axis] = 0.0f;

src/main/flight/pid.h

@@ -127,7 +127,7 @@ extern uint32_t targetPidLooptime;
 // PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction
 extern uint8_t PIDweight[3];
 
-void pidResetErrorGyroState(void);
+void pidResetITerm(void);
 void pidStabilisationState(pidStabilisationState_e pidControllerState);
 void pidSetItermAccelerator(float newItermAccelerator);
 void pidInitFilters(const pidProfile_t *pidProfile);

src/test/unit/arming_prevention_unittest.cc

@@ -642,7 +642,7 @@ extern "C" {
     void failsafeStartMonitoring(void) {}
     void failsafeUpdateState(void) {}
     bool failsafeIsActive(void) { return false; }
-    void pidResetErrorGyroState(void) {}
+    void pidResetITerm(void) {}
     void updateAdjustmentStates(void) {}
     void processRcAdjustments(controlRateConfig_t *) {}
     void updateGpsWaypointsAndMode(void) {}