diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 3112ccdae..b28fb8613 100644 --- a/src/main/fc/fc_core.c +++ b/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 diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index a80fca4c1..bb5dfcd28 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -545,6 +545,10 @@ 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]; @@ -555,6 +559,10 @@ 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; @@ -572,6 +580,10 @@ void calculateThrottleAndCurrentMotorEndpoints(void) motorOutputMin = deadbandMotor3dLow; motorOutputRange = motorOutputLow - deadbandMotor3dLow; } + if (motorOutputMixSign != -1) { + // reset ITerm on motor reversal + pidResetITerm(); + } motorOutputMixSign = -1; throttle = 0; currentThrottleInputRange = rcCommandThrottleRange3dLow; @@ -581,6 +593,10 @@ 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; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 64c91b2e3..26761a6be 100644 --- a/src/main/flight/pid.c +++ b/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; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 9cb1f58d8..ba6f99415 100644 --- a/src/main/flight/pid.h +++ b/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); diff --git a/src/test/unit/arming_prevention_unittest.cc b/src/test/unit/arming_prevention_unittest.cc index 0f5e2d3f0..49cb53885 100644 --- a/src/test/unit/arming_prevention_unittest.cc +++ b/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) {}