Revise GPS Rescue throttle and yaw handling to not modify rcCommand

Injecting new values directly into rcCommand to override pilot inputs does not work correctly when rc interpolation or smoothing is enabled. This is because the smoothing functions maintain an internal state that is used to produce the final rcCommand values. So in effect it re-overrides the values set by GPS Rescue.

Additionally, modifying rcCommand directly is undesirable because:
- It happens before rates are applied. So the pilot's rates will effect the control commanded by GPS Rescue.
- rcCommand values are used for more than input in the flight control. They also are used for stick commands, etc.
- In the case of throttle, various modifications can be additionally applied like throttle boost and throttle limit that may negatively effect GPS Rescue.

These changes revise the logic to only modify the commanded values used in the PID controller (yaw) and mixer (throttle) rather than attempting to override rcCommand.

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

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)
 {
+    float rcCommandf = 0;
+    if (!zeroAxis) {
         // scale rcCommandf to range [-1.0, 1.0]
-    float rcCommandf = rcCommand[axis] / 500.0f;
+        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]);

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,
@@ -72,7 +75,7 @@ PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
 );
 
 static uint16_t rescueThrottle;
-static int16_t       rescueYaw;
+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) {
+    // Determine the most efficient direction to rotate
-        dif += 360;
+    if (errorAngle <= -180) {
-    } else if (dif > 180) {
+        errorAngle += 360;
-        dif -= 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);
         
-    rescueYaw = - (dif * gpsRescueConfig()->yawP / 20);
+    // 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 *= errorAngleSign;
 }
 
-void gpsRescueInjectRcCommands(void)
+float gpsRescueGetYawRate(void)
 {
-    rcCommand[THROTTLE] = rescueThrottle;
+    return rescueYaw;
-    rcCommand[YAW] = 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

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);

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++) {