Merge pull request #8661 from breadoven/abo_fw_alt_control_fix

Fixed wing climb rate fool proofing fix
iNavFlight · Dec 31, 2022 · 5937ba7 · 5937ba7
2 parents 1582b21 + e04032c
commit 5937ba7
Showing 1 changed file with 3 additions and 1 deletion.
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
@@ -2906,9 +2906,9 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti
         if (STATE(FIXED_WING_LEGACY)) {
             // Fixed wing climb rate controller is open-loop. We simply move the known altitude target
             float timeDelta = US2S(currentTimeUs - lastUpdateTimeUs);
+            static bool targetHoldActive = false;
 
             if (timeDelta <= HZ2S(MIN_POSITION_UPDATE_RATE_HZ) && desiredClimbRate) {
-                static bool targetHoldActive = false;
                 // Update target altitude only if actual altitude moving in same direction and lagging by < 5 m, otherwise hold target
                 if (navGetCurrentActualPositionAndVelocity()->vel.z * desiredClimbRate >= 0 && fabsf(posControl.desiredState.pos.z - altitudeToUse) < 500) {
                     posControl.desiredState.pos.z += desiredClimbRate * timeDelta;
@@ -2917,6 +2917,8 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti
                     posControl.desiredState.pos.z = altitudeToUse + desiredClimbRate;
                     targetHoldActive = true;
                 }
+            } else {
+                targetHoldActive = false;
             }
         }
         else {