diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
index 8e378fe125..3e28f8a4a4 100644
--- a/src/main/fc/fc_core.c
+++ b/src/main/fc/fc_core.c
@@ -992,11 +992,6 @@ static FAST_CODE_NOINLINE void subTaskRcCommand(timeUs_t currentTimeUs)
 
     processRcCommand();
 
-#if defined(USE_GPS_RESCUE)
-    if (FLIGHT_MODE(GPS_RESCUE_MODE)) {
-        gpsRescueInjectRcCommands();
-    }
-#endif
 }
 
 // Function for loop trigger
diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c
index d634c8982f..c908ab3a8a 100644
--- a/src/main/fc/fc_rc.c
+++ b/src/main/fc/fc_rc.c
@@ -43,6 +43,7 @@
 
 #include "flight/failsafe.h"
 #include "flight/imu.h"
+#include "flight/gps_rescue.h"
 #include "flight/pid.h"
 #include "pg/rx.h"
 #include "rx/rx.h"
@@ -144,10 +145,13 @@ float applyRaceFlightRates(const int axis, float rcCommandf, const float rcComma
     return angleRate;
 }
 
-static void calculateSetpointRate(int axis)
+static void calculateSetpointRate(int axis, bool zeroAxis)
 {
-    // scale rcCommandf to range [-1.0, 1.0]
-    float rcCommandf = rcCommand[axis] / 500.0f;
+    float rcCommandf = 0;
+    if (!zeroAxis) {
+        // scale rcCommandf to range [-1.0, 1.0]
+        rcCommandf = rcCommand[axis] / 500.0f;
+    }
     rcDeflection[axis] = rcCommandf;
     const float rcCommandfAbs = ABS(rcCommandf);
     rcDeflectionAbs[axis] = rcCommandfAbs;
@@ -548,7 +552,27 @@ FAST_CODE void processRcCommand(void)
 #if defined(SIMULATOR_BUILD)
 #pragma GCC diagnostic pop
 #endif
-            calculateSetpointRate(axis);
+            bool useGpsRescueYaw = false;
+#ifdef USE_GPS_RESCUE
+            // Logic improvements to not override rcCommand when GPS Rescue is active.
+            // Instead only modify the flight control values derived from rcCommand
+            // like setpointRate[] and rcDeflection[].
+            if ((axis == FD_YAW) && FLIGHT_MODE(GPS_RESCUE_MODE)) {
+                // If GPS Rescue is active then override the yaw commanded input
+                // with a zero (centered) stick input. Otherwise the pilot's yaw inputs
+                // will get calculated into the rcDeflection values affecting downstream logic
+                useGpsRescueYaw = true;
+            }
+#endif
+            calculateSetpointRate(axis, useGpsRescueYaw);
+
+#ifdef USE_GPS_RESCUE
+            if (useGpsRescueYaw) {
+                // If GPS Rescue is active then override the setpointRate used in the
+                // pid controller with the value calculated from the desired heading logic.
+                setpointRate[axis] = gpsRescueGetYawRate();
+            }
+#endif
         }
 
         DEBUG_SET(DEBUG_RC_INTERPOLATION, 3, setpointRate[0]);
diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c
index 792344f4b6..8c47bbaeec 100644
--- a/src/main/flight/gps_rescue.c
+++ b/src/main/flight/gps_rescue.c
@@ -50,6 +50,9 @@
 
 #include "gps_rescue.h"
 
+#define GPS_RESCUE_MAX_YAW_RATE 180        // 180 deg/sec
+#define GPS_RESCUE_RATE_SCALE_DEGREES 20   // Scale the commanded yaw rate when the error is less then this angle
+
 PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 0);
 
 PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
@@ -71,8 +74,8 @@ PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
     .minSats = 8
 );
 
-static uint16_t      rescueThrottle;
-static int16_t       rescueYaw;
+static uint16_t rescueThrottle;
+static float    rescueYaw;
 
 int32_t       gpsRescueAngle[ANGLE_INDEX_COUNT] = { 0, 0 };
 uint16_t      hoverThrottle = 0;
@@ -319,6 +322,11 @@ void idleTasks()
     rescueThrottle = rcCommand[THROTTLE];
 
     //to do: have a default value for hoverThrottle
+    
+    // FIXME: GPS Rescue throttle handling should take into account min_check as the
+    // active throttle is from min_check through PWM_RANGE_MAX. Currently adjusting for this
+    // in gpsRescueGetThrottle() but it would be better handled here.
+
     float ct = getCosTiltAngle();
     if (ct > 0.5 && ct < 0.96 && throttleSamples < 1E6 && rescueThrottle > 1070) { //5 to 45 degrees tilt
         //TO DO: only sample when acceleration is low
@@ -395,25 +403,48 @@ void rescueAttainPosition()
 }
 
 // Very similar to maghold function on betaflight/cleanflight
-void setBearing(int16_t deg)
+void setBearing(int16_t desiredHeading)
 {
-    int16_t dif = DECIDEGREES_TO_DEGREES(attitude.values.yaw) - deg;
+    float errorAngle = DECIDEGREES_TO_DEGREES(attitude.values.yaw) - desiredHeading;
 
-    if (dif <= -180) {
-        dif += 360;
-    } else if (dif > 180) {
-        dif -= 360;
+    // Determine the most efficient direction to rotate
+    if (errorAngle <= -180) {
+        errorAngle += 360;
+    } else if (errorAngle > 180) {
+        errorAngle -= 360;
     }
 
-    dif *= -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed);
+    errorAngle *= -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed);
+    const int errorAngleSign = (errorAngle < 0) ? -1 : 1;
+    const float errorAngleAbs = ABS(errorAngle);
+        
+    // Calculate a desired yaw rate based on a maximum limit beyond
+    // an error window and then scale the requested rate down inside
+    // the window as error approaches 0.
+    rescueYaw = (errorAngleAbs > GPS_RESCUE_RATE_SCALE_DEGREES) ? GPS_RESCUE_MAX_YAW_RATE : errorAngleAbs;
 
-    rescueYaw = - (dif * gpsRescueConfig()->yawP / 20);
+    rescueYaw *= errorAngleSign;
 }
 
-void gpsRescueInjectRcCommands(void)
+float gpsRescueGetYawRate(void)
 {
-    rcCommand[THROTTLE] = rescueThrottle;
-    rcCommand[YAW] = rescueYaw;
+    return rescueYaw;
+}
+
+float gpsRescueGetThrottle(void)
+{
+    float commandedThrottle = 0;
+    const int minCheck = MAX(rxConfig()->mincheck, 1000);
+    
+    // We need to compensate for min_check since the throttle value set by gps rescue
+    // is based on the raw rcCommand value commanded by the pilot.
+    if (rescueThrottle > minCheck) {
+        // Calculated a desired commanded throttle scaled from 0.0 to 1.0 for use in the mixer.
+        commandedThrottle = (float)(rescueThrottle - minCheck) / (PWM_RANGE_MAX - minCheck);
+        commandedThrottle = constrainf(commandedThrottle, 0.0f, 1.0f);
+    }
+    
+    return commandedThrottle;
 }
 
 #endif
diff --git a/src/main/flight/gps_rescue.h b/src/main/flight/gps_rescue.h
index cfce5bd41c..afcb5bf0a2 100644
--- a/src/main/flight/gps_rescue.h
+++ b/src/main/flight/gps_rescue.h
@@ -109,4 +109,5 @@ void sensorUpdate(void);
 
 void rescueAttainPosition(void);
 
-void gpsRescueInjectRcCommands(void);
+float gpsRescueGetYawRate(void);
+float gpsRescueGetThrottle(void);
diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
index 70b4d5841f..e8b5f13755 100644
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@@ -54,6 +54,7 @@
 
 #include "flight/failsafe.h"
 #include "flight/imu.h"
+#include "flight/gps_rescue.h"
 #include "flight/mixer.h"
 #include "flight/mixer_tricopter.h"
 #include "flight/pid.h"
@@ -810,6 +811,14 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensa
     }
 #endif
 
+#ifdef USE_GPS_RESCUE
+    // If gps rescue is active then override the throttle. This prevents things
+    // like throttle boost or throttle limit from negatively affecting the throttle.
+    if (FLIGHT_MODE(GPS_RESCUE_MODE)) {
+        throttle = gpsRescueGetThrottle();
+    }
+#endif
+
     motorMixRange = motorMixMax - motorMixMin;
     if (motorMixRange > 1.0f) {
         for (int i = 0; i < motorCount; i++) {