Merge pull request #6227 from etracer65/gps_rescue_controls_fix

Revise GPS Rescue throttle and yaw handling to not modify rcCommand

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

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

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"
@@ -146,17 +147,31 @@ float applyRaceFlightRates(const int axis, float rcCommandf, const float rcComma
 
 static void calculateSetpointRate(int axis)
 {
+    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();
+
+        // 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;
 
-    float angleRate = applyRates(axis, rcCommandf, 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)

src/main/flight/gps_rescue.c

@@ -50,6 +50,9 @@
 
 #include "gps_rescue.h"
 
+#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);
 
 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,39 @@ 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 = (attitude.values.yaw / 10.0f) - 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);
         
-    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 = -constrainf(errorAngle / GPS_RESCUE_RATE_SCALE_DEGREES * GPS_RESCUE_MAX_YAW_RATE, -GPS_RESCUE_MAX_YAW_RATE, GPS_RESCUE_MAX_YAW_RATE);
 }
 
-void gpsRescueInjectRcCommands(void)
+float gpsRescueGetYawRate(void)
 {
-    rcCommand[THROTTLE] = rescueThrottle;
-    rcCommand[YAW] = rescueYaw;
+    return rescueYaw;
+}
+
+float gpsRescueGetThrottle(void)
+{
+    // 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.
+    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;
 }
 
 #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++) {