diff --git a/src/main/common/maths.c b/src/main/common/maths.c
index 565684be82..00a0c51ecd 100644
--- a/src/main/common/maths.c
+++ b/src/main/common/maths.c
@@ -176,6 +176,12 @@ int scaleRange(int x, int srcFrom, int srcTo, int destFrom, int destTo) {
     return (a / b) + destFrom;
 }
 
+float scaleRangef(float x, float srcFrom, float srcTo, float destFrom, float destTo) {
+    float a = (destTo - destFrom) * (x - srcFrom);
+    float b = srcTo - srcFrom;
+    return (a / b) + destFrom;
+}
+
 // Normalize a vector
 void normalizeV(struct fp_vector *src, struct fp_vector *dest)
 {
diff --git a/src/main/common/maths.h b/src/main/common/maths.h
index 957f85d81d..2cdae7ec17 100644
--- a/src/main/common/maths.h
+++ b/src/main/common/maths.h
@@ -102,6 +102,7 @@ float devStandardDeviation(stdev_t *dev);
 float degreesToRadians(int16_t degrees);
 
 int scaleRange(int x, int srcFrom, int srcTo, int destFrom, int destTo);
+float scaleRangef(float x, float srcFrom, float srcTo, float destFrom, float destTo);
 
 void normalizeV(struct fp_vector *src, struct fp_vector *dest);
 
diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c
index c908ab3a8a..4472d246b5 100644
--- a/src/main/fc/fc_rc.c
+++ b/src/main/fc/fc_rc.c
@@ -145,22 +145,33 @@ float applyRaceFlightRates(const int axis, float rcCommandf, const float rcComma
     return angleRate;
 }
 
-static void calculateSetpointRate(int axis, bool zeroAxis)
+static void calculateSetpointRate(int axis)
 {
-    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;
+    float angleRate;
+    
+#ifdef USE_GPS_RESCUE
+    if ((axis == FD_YAW) && FLIGHT_MODE(GPS_RESCUE_MODE)) {
+        // If GPS Rescue is active then override the setpointRate used in the
+        // pid controller with the value calculated from the desired heading logic.
+        angleRate = gpsRescueGetYawRate();
 
-    float angleRate = applyRates(axis, rcCommandf, rcCommandfAbs);
+        // Treat the stick input as centered to avoid any stick deflection base modifications (like acceleration limit)
+        rcDeflection[axis] = 0;
+        rcDeflectionAbs[axis] = 0;
+    } else
+#endif
+    {
+        // scale rcCommandf to range [-1.0, 1.0]
+        float rcCommandf = rcCommand[axis] / 500.0f;
+        rcDeflection[axis] = rcCommandf;
+        const float rcCommandfAbs = ABS(rcCommandf);
+        rcDeflectionAbs[axis] = rcCommandfAbs;
+
+        angleRate = applyRates(axis, rcCommandf, rcCommandfAbs);
+    }
+    setpointRate[axis] = constrainf(angleRate, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT); // Rate limit protection (deg/sec)
 
     DEBUG_SET(DEBUG_ANGLERATE, axis, angleRate);
-
-    setpointRate[axis] = constrainf(angleRate, -SETPOINT_RATE_LIMIT, SETPOINT_RATE_LIMIT); // Rate limit protection (deg/sec)
 }
 
 static void scaleRcCommandToFpvCamAngle(void)
@@ -552,27 +563,7 @@ FAST_CODE void processRcCommand(void)
 #if defined(SIMULATOR_BUILD)
 #pragma GCC diagnostic pop
 #endif
-            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
+            calculateSetpointRate(axis);
         }
 
         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 ea148a5705..d9f6eccdb4 100644
--- a/src/main/flight/gps_rescue.c
+++ b/src/main/flight/gps_rescue.c
@@ -50,7 +50,7 @@
 
 #include "gps_rescue.h"
 
-#define GPS_RESCUE_MAX_YAW_RATE       100  // 100 deg/sec max yaw
+#define GPS_RESCUE_MAX_YAW_RATE       360  // deg/sec max yaw rate
 #define GPS_RESCUE_RATE_SCALE_DEGREES 45   // 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);
@@ -405,7 +405,7 @@ void rescueAttainPosition()
 // Very similar to maghold function on betaflight/cleanflight
 void setBearing(int16_t desiredHeading)
 {
-    float errorAngle = DECIDEGREES_TO_DEGREES(attitude.values.yaw) - desiredHeading;
+    float errorAngle = (attitude.values.yaw / 10.0f) - desiredHeading;
 
     // Determine the most efficient direction to rotate
     if (errorAngle <= -180) {
@@ -415,20 +415,11 @@ void setBearing(int16_t desiredHeading)
     }
 
     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.
-    if (errorAngleAbs > GPS_RESCUE_RATE_SCALE_DEGREES) {
-        rescueYaw = GPS_RESCUE_MAX_YAW_RATE;
-    } else {
-        rescueYaw = (errorAngleAbs / GPS_RESCUE_RATE_SCALE_DEGREES) * GPS_RESCUE_MAX_YAW_RATE;
-    }
-    rescueYaw = (errorAngleAbs > GPS_RESCUE_RATE_SCALE_DEGREES) ? GPS_RESCUE_MAX_YAW_RATE : errorAngleAbs;
-
-    rescueYaw *= errorAngleSign;
+    rescueYaw = -constrainf(errorAngle / GPS_RESCUE_RATE_SCALE_DEGREES * GPS_RESCUE_MAX_YAW_RATE, -GPS_RESCUE_MAX_YAW_RATE, GPS_RESCUE_MAX_YAW_RATE);
 }
 
 float gpsRescueGetYawRate(void)
@@ -438,16 +429,11 @@ float gpsRescueGetYawRate(void)
 
 float gpsRescueGetThrottle(void)
 {
-    float commandedThrottle = 0;
-    const int minCheck = MAX(rxConfig()->mincheck, 1000);
-    
+    // Calculated a desired commanded throttle scaled from 0.0 to 1.0 for use in the mixer.
     // 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);
-    }
+    float commandedThrottle = scaleRangef(rescueThrottle, MAX(rxConfig()->mincheck, PWM_RANGE_MIN), PWM_RANGE_MAX, 0.0f, 1.0f);
+    commandedThrottle = constrainf(commandedThrottle, 0.0f, 1.0f);
     
     return commandedThrottle;
 }