From fb51f6373550697df0f6b41811a5c33eb4b5c1f4 Mon Sep 17 00:00:00 2001
From: ctzsnooze <chris.thompson@sydney.edu.au>
Date: Sat, 16 Apr 2022 12:34:50 +1000
Subject: [PATCH] fix for GPS non-return

---
 src/main/fc/core.c |  2 +-
 src/main/rx/rx.c   | 16 +++++++++-------
 2 files changed, 10 insertions(+), 8 deletions(-)

diff --git a/src/main/fc/core.c b/src/main/fc/core.c
index e53629f7e..4eb9957ce 100644
--- a/src/main/fc/core.c
+++ b/src/main/fc/core.c
@@ -985,7 +985,7 @@ void processRxModes(timeUs_t currentTimeUs)
     }
 
 #ifdef USE_GPS_RESCUE
-    if (ARMING_FLAG(ARMED) && IS_RC_MODE_ACTIVE(BOXGPSRESCUE)) {
+    if (ARMING_FLAG(ARMED) && (IS_RC_MODE_ACTIVE(BOXGPSRESCUE) || (failsafeIsActive() && failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_GPS_RESCUE))) {
         if (!FLIGHT_MODE(GPS_RESCUE_MODE)) {
             ENABLE_FLIGHT_MODE(GPS_RESCUE_MODE);
         }
diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c
index 18d227278..4dcfe7957 100644
--- a/src/main/rx/rx.c
+++ b/src/main/rx/rx.c
@@ -653,15 +653,17 @@ void detectAndApplySignalLossBehaviour(void)
         }
 
        if (ARMING_FLAG(ARMED) && failsafeIsActive()) {
-            //  apply failsafe values, until failsafe ends, or disarmed, unless in GPS Return
-            if ((channel < NON_AUX_CHANNEL_COUNT) && !FLIGHT_MODE(GPS_RESCUE_MODE)) {
-                if (channel == THROTTLE ) {
-                    sample = failsafeConfig()->failsafe_throttle;
-                } else {
-                    sample = rxConfig()->midrc;
+            //  apply failsafe values, until failsafe ends, or disarmed, unless in GPS Return (where stick values should remain)
+            if (channel < NON_AUX_CHANNEL_COUNT) {
+                if (!FLIGHT_MODE(GPS_RESCUE_MODE)) {
+                    if (channel == THROTTLE ) {
+                        sample = failsafeConfig()->failsafe_throttle;
+                    } else {
+                        sample = rxConfig()->midrc;
+                    }
                 }
             } else if (!failsafeAuxSwitch) {
-                //  aux channels as Set in Configurator, unless failsafe initiated by switch
+                // aux channels as Set in Configurator, unless failsafe initiated by switch
                 sample = getRxfailValue(channel);
             }
         } else {