From a3c4ae2a349e1894a1815dead4bb6c4e7f76fc56 Mon Sep 17 00:00:00 2001
From: hali9 <hali9@wp.pl>
Date: Tue, 3 Dec 2019 21:00:01 +0100
Subject: [PATCH] Update navigation.c

---
 src/main/navigation/navigation.c | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 929e8683a2e..b5468e0a69b 100755
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -2084,7 +2084,8 @@ float getFinalRTHAltitude(void)
 static void updateDesiredRTHAltitude(void)
 {
     if (ARMING_FLAG(ARMED)) {
-        if (!(navGetStateFlags(posControl.navState) & NAV_AUTO_RTH)) {
+        if (!((navGetStateFlags(posControl.navState) & NAV_AUTO_RTH)
+          || ((navGetStateFlags(posControl.navState) & NAV_AUTO_WP) && posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_RTH))) {
             switch (navConfig()->general.flags.rth_alt_control_mode) {
                 case NAV_RTH_NO_ALT:
                     posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z;