From ad067ee09a40eaab4bab6e8610b40643cbb04a3c Mon Sep 17 00:00:00 2001
From: Bruce Luckcuck <github@etracer.net>
Date: Sun, 27 May 2018 21:09:38 -0400
Subject: [PATCH] Overshoot logic updates, disable for GPS Rescue, constrain
 max setpoint Updates for finalize the flight performance.

Disable if GPS Rescue is active

Constrain the max setpoint calculated by the angle correction logic. To catch border cases where the user activates the feature while inverted and has a high gain which could result in excessively high setpoint rates.

Make sure internal states are reset when feature is activated via mode switch.
---
 src/main/flight/pid.c | 16 ++++++++++------
 1 file changed, 10 insertions(+), 6 deletions(-)

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 8b78dfba2..431172a11 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -89,8 +89,8 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
 #endif
 
 #ifdef USE_ACRO_TRAINER
-#define ACRO_TRAINER_LOOKAHEAD_MIN_TIME   0.01f
-#define ACRO_TRAINER_LOOKAHEAD_RATE_LIMIT 500.0f // Max gyro rate for lookahead time scaling
+#define ACRO_TRAINER_LOOKAHEAD_RATE_LIMIT 500.0f  // Max gyro rate for lookahead time scaling
+#define ACRO_TRAINER_SETPOINT_LIMIT       1000.0f // Limit the correcting setpoint
 #endif // USE_ACRO_TRAINER
 
 PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 3);
@@ -599,7 +599,7 @@ static FAST_CODE_NOINLINE float applyAcroTrainer(int axis, const rollAndPitchTri
 {
     float ret = setPoint;
 
-    if (!FLIGHT_MODE(ANGLE_MODE) && !FLIGHT_MODE(HORIZON_MODE)) {
+    if (!FLIGHT_MODE(ANGLE_MODE) && !FLIGHT_MODE(HORIZON_MODE) && !FLIGHT_MODE(GPS_RESCUE_MODE)) {
         bool resetIterm = false;
         float projectedAngle = 0;
         const int setpointSign = acroTrainerSign(setPoint);
@@ -619,7 +619,7 @@ static FAST_CODE_NOINLINE float applyAcroTrainer(int axis, const rollAndPitchTri
         }
 
         if (acroTrainerAxisState[axis] != 0) {
-            ret = ((acroTrainerAngleLimit * angleSign) - currentAngle) * acroTrainerGain;
+            ret = constrainf(((acroTrainerAngleLimit * angleSign) - currentAngle) * acroTrainerGain, -ACRO_TRAINER_SETPOINT_LIMIT, ACRO_TRAINER_SETPOINT_LIMIT);
         } else {
         
         // Not currently over the limit so project the angle based on current angle and
@@ -627,7 +627,6 @@ static FAST_CODE_NOINLINE float applyAcroTrainer(int axis, const rollAndPitchTri
         // If the projected angle exceeds the limit then apply limiting to minimize overshoot.
             // Calculate the lookahead window by scaling proportionally with gyro rate from 0-500dps
             float checkInterval = constrainf(fabsf(gyro.gyroADCf[axis]) / ACRO_TRAINER_LOOKAHEAD_RATE_LIMIT, 0.0f, 1.0f) * acroTrainerLookaheadTime;
-            checkInterval = MAX(checkInterval, ACRO_TRAINER_LOOKAHEAD_MIN_TIME);
             projectedAngle = (gyro.gyroADCf[axis] * checkInterval) + currentAngle;
             const int projectedAngleSign = acroTrainerSign(projectedAngle);
             if ((fabsf(projectedAngle) > acroTrainerAngleLimit) && (projectedAngleSign == setpointSign)) {
@@ -840,6 +839,11 @@ bool crashRecoveryModeActive(void)
 #ifdef USE_ACRO_TRAINER
 void pidSetAcroTrainerState(bool newState)
 {
-    acroTrainerActive = newState;
+    if (acroTrainerActive != newState) {
+        if (newState) {
+            pidAcroTrainerInit();
+        }
+        acroTrainerActive = newState;
+    }
 }
 #endif // USE_ACRO_TRAINER