Changed method to set climb stage altitude

Climb stage altitude is now set when RTH is initiated. this saves on calculations in each loop.
breadoven · Aug 1, 2021 · f06288c · f06288c
1 parent 9ed0b1d
commit f06288c
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 19 deletions.
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
@@ -2158,7 +2158,14 @@ static void updateDesiredRTHAltitude(void)
     if (ARMING_FLAG(ARMED)) {
         if (!((navGetStateFlags(posControl.navState) & NAV_AUTO_RTH)
           || ((navGetStateFlags(posControl.navState) & NAV_AUTO_WP) && posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_RTH))) {
-            posControl.rthState.rthStartAltitude = posControl.actualState.abs.pos.z;
+            switch (navConfig()->general.flags.rth_climb_first_stage_mode) {
+                case NAV_RTH_CLIMB_STAGE_AT_LEAST:
+                    posControl.rthState.rthClimbStageAltitude = navConfig()->general.rth_climb_first_stage_altitude;
+                    break;
+                case NAV_RTH_CLIMB_STAGE_EXTRA:
+                    posControl.rthState.rthClimbStageAltitude = posControl.actualState.abs.pos.z + navConfig()->general.rth_climb_first_stage_altitude;
+                    break; 
+            }
 
             switch (navConfig()->general.flags.rth_alt_control_mode) {
                 case NAV_RTH_NO_ALT:
@@ -2193,7 +2200,7 @@ static void updateDesiredRTHAltitude(void)
             }
         }
     } else {
-        posControl.rthState.rthStartAltitude = posControl.actualState.abs.pos.z;
+        posControl.rthState.rthClimbStageAltitude = posControl.actualState.abs.pos.z;
         posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z;
         posControl.rthState.rthFinalAltitude = posControl.actualState.abs.pos.z;
     }
@@ -2464,17 +2471,8 @@ static bool rthAltControlStickOverrideCheck(unsigned axis)
  * --------------------------------------------------- */
  bool rthClimbStageActiveAndComplete() {
     if ((STATE(FIXED_WING_LEGACY) || STATE(AIRPLANE)) && (navConfig()->general.rth_climb_first_stage_altitude > 0)) {
-        switch (navConfig()->general.flags.rth_climb_first_stage_mode) {
-            case NAV_RTH_CLIMB_STAGE_AT_LEAST:
-                if (posControl.actualState.abs.pos.z >= navConfig()->general.rth_climb_first_stage_altitude) {
-                    return true;
-                }
-                break;
-            case NAV_RTH_CLIMB_STAGE_EXTRA:
-                if (posControl.actualState.abs.pos.z >= (posControl.rthState.rthStartAltitude + navConfig()->general.rth_climb_first_stage_altitude)) {
-                    return true;
-                }
-                break; 
+        if (posControl.actualState.abs.pos.z >= posControl.rthState.rthClimbStageAltitude) {
+            return true;
         }
     }
 

diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h
@@ -304,12 +304,12 @@ typedef struct {
 
 typedef struct {
     navigationHomeFlags_t   homeFlags;
-    navWaypointPosition_t   homePosition;       // Original home position and base altitude
-    float                   rthStartAltitude;   // Altitude at start of RTH
-    float                   rthInitialAltitude; // Altitude at start of RTH, can include added margins and extra height
-    float                   rthFinalAltitude;   // Altitude at end of RTH approach
-    float                   rthInitialDistance; // Distance when starting flight home
-    fpVector3_t             homeTmpWaypoint;    // Temporary storage for home target
+    navWaypointPosition_t   homePosition;           // Original home position and base altitude
+    float                   rthInitialAltitude;     // Altitude at start of RTH, can include added margins and extra height
+    float                   rthClimbStageAltitude;  // Altitude at end of the climb phase
+    float                   rthFinalAltitude;       // Altitude at end of RTH approach
+    float                   rthInitialDistance;     // Distance when starting flight home
+    fpVector3_t             homeTmpWaypoint;        // Temporary storage for home target
 } rthState_t;
 
 typedef enum {