From e33dda884dfde477aeb06e083e69c4db0fa1b38a Mon Sep 17 00:00:00 2001
From: "Konstantin Sharlaimov (DigitalEntity)"
 <konstantin.sharlaimov@gmail.com>
Date: Tue, 24 Nov 2015 14:58:21 +1000
Subject: [PATCH] Fix a bug in RTH altitude calculation. Bug is observed as RTH
 never finishing the CLIMB stage

---
 src/main/flight/navigation_rewrite.c         |  6 ++--
 src/main/flight/navigation_rewrite.h         | 11 ------
 src/main/flight/navigation_rewrite_private.h | 38 ++++++++++----------
 3 files changed, 22 insertions(+), 33 deletions(-)

diff --git a/src/main/flight/navigation_rewrite.c b/src/main/flight/navigation_rewrite.c
index 3c67f86436..06dcaba399 100755
--- a/src/main/flight/navigation_rewrite.c
+++ b/src/main/flight/navigation_rewrite.c
@@ -1098,7 +1098,7 @@ static void updateHomePositionCompatibility(void)
 static void updateDesiredRTHAltitude(void)
 {
     if (ARMING_FLAG(ARMED)) {
-        if (!(navGetStateFlags(posControl.navState) & NAV_MODE_RTH)) {
+        if (!(navGetStateFlags(posControl.navState) & NAV_AUTO_RTH)) {
             switch (posControl.navConfig->flags.rth_alt_control_style) {
             case NAV_RTH_NO_ALT:
                 posControl.homeWaypointAbove.pos.V.Z = posControl.actualState.pos.V.Z;
@@ -1467,7 +1467,7 @@ static void setDesiredPositionToWaypointAndUpdateInitialBearing(navWaypointPosit
  *  In PH mode our waypoint is hold position */
 bool isApproachingLastWaypoint(void)
 {
-    if (navGetStateFlags(posControl.navState) & NAV_MODE_WP) {
+    if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP) {
         if (posControl.waypointCount == 0) {
             /* No waypoints, holding current position */
             return true;
@@ -1848,7 +1848,7 @@ void abortForcedRTH(void)
 
 rthState_e getStateOfForcedRTH(void)
 {
-    if (navGetStateFlags(posControl.navState) & NAV_MODE_RTH) {
+    if (navGetStateFlags(posControl.navState) & NAV_AUTO_RTH) {
 
         if (posControl.navState == NAV_STATE_RTH_2D_FINISHED || posControl.navState == NAV_STATE_RTH_3D_FINISHED) {
             return RTH_HAS_LANDED;
diff --git a/src/main/flight/navigation_rewrite.h b/src/main/flight/navigation_rewrite.h
index 915f1fd909..c2f0b6e376 100755
--- a/src/main/flight/navigation_rewrite.h
+++ b/src/main/flight/navigation_rewrite.h
@@ -38,17 +38,6 @@
 // Maximum number of waypoints, special waypoint 0 = home,
 #define NAV_MAX_WAYPOINTS       15
 
-// navigation mode
-typedef enum navigationMode_e {
-    NAV_MODE_NONE       = 0,
-    NAV_MODE_ALTHOLD,
-    NAV_MODE_POSHOLD_2D,
-    NAV_MODE_POSHOLD_3D,
-    NAV_MODE_WP,
-    NAV_MODE_RTH,
-    NAV_MODE_RTH_2D,
-} navigationMode_t;
-
 enum {
     NAV_GPS_ATTI    = 0,                    // Pitch/roll stick controls attitude (pitch/roll lean angles)
     NAV_GPS_CRUISE  = 1                     // Pitch/roll stick controls velocity (forward/right speed)
diff --git a/src/main/flight/navigation_rewrite_private.h b/src/main/flight/navigation_rewrite_private.h
index 3a71222800..8f7d814011 100755
--- a/src/main/flight/navigation_rewrite_private.h
+++ b/src/main/flight/navigation_rewrite_private.h
@@ -153,29 +153,29 @@ typedef enum {
     NAV_STATE_POSHOLD_3D_INITIALIZE,            // 6
     NAV_STATE_POSHOLD_3D_IN_PROGRESS,           // 7
 
-    NAV_STATE_RTH_INITIALIZE,
+    NAV_STATE_RTH_INITIALIZE,                   // 8
 
-    NAV_STATE_RTH_2D_INITIALIZE,
-    NAV_STATE_RTH_2D_HEAD_HOME,
-    NAV_STATE_RTH_2D_FINISHING,
-    NAV_STATE_RTH_2D_FINISHED,
+    NAV_STATE_RTH_2D_INITIALIZE,                // 9
+    NAV_STATE_RTH_2D_HEAD_HOME,                 // 10
+    NAV_STATE_RTH_2D_FINISHING,                 // 11
+    NAV_STATE_RTH_2D_FINISHED,                  // 12
 
-    NAV_STATE_RTH_3D_INITIALIZE,
-    NAV_STATE_RTH_3D_CLIMB_TO_SAFE_ALT,
-    NAV_STATE_RTH_3D_HEAD_HOME,
-    NAV_STATE_RTH_3D_HOVER_PRIOR_TO_LANDING,
-    NAV_STATE_RTH_3D_LANDING,
-    NAV_STATE_RTH_3D_FINISHING,
-    NAV_STATE_RTH_3D_FINISHED,
+    NAV_STATE_RTH_3D_INITIALIZE,                // 13
+    NAV_STATE_RTH_3D_CLIMB_TO_SAFE_ALT,         // 14
+    NAV_STATE_RTH_3D_HEAD_HOME,                 // 15
+    NAV_STATE_RTH_3D_HOVER_PRIOR_TO_LANDING,    // 16
+    NAV_STATE_RTH_3D_LANDING,                   // 17
+    NAV_STATE_RTH_3D_FINISHING,                 // 18
+    NAV_STATE_RTH_3D_FINISHED,                  // 19
 
-    NAV_STATE_WAYPOINT_INITIALIZE,
-    NAV_STATE_WAYPOINT_IN_PROGRESS,
-    NAV_STATE_WAYPOINT_REACHED,
-    NAV_STATE_WAYPOINT_FINISHED,
+    NAV_STATE_WAYPOINT_INITIALIZE,              // 20
+    NAV_STATE_WAYPOINT_IN_PROGRESS,             // 21
+    NAV_STATE_WAYPOINT_REACHED,                 // 22
+    NAV_STATE_WAYPOINT_FINISHED,                // 23
 
-    NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
-    NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS,
-    NAV_STATE_EMERGENCY_LANDING_FINISHED,
+    NAV_STATE_EMERGENCY_LANDING_INITIALIZE,     // 24
+    NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS,    // 25
+    NAV_STATE_EMERGENCY_LANDING_FINISHED,       // 26
 
     NAV_STATE_COUNT,
 } navigationFSMState_t;