Merge pull request #6132 from mikeller/fix_pid_loop_slowness

Remove GPS rescue update from PID loop.

src/main/fc/fc_core.c

@@ -923,10 +923,6 @@ static FAST_CODE_NOINLINE void subTaskMainSubprocesses(timeUs_t currentTimeUs)
     }
 #endif
 
-#ifdef USE_GPS_RESCUE
-    updateGPSRescueState();
-#endif
-
 #ifdef USE_SDCARD
     afatfs_poll();
 #endif
@@ -977,6 +973,7 @@ static FAST_CODE void subTaskMotorUpdate(timeUs_t currentTimeUs)
 
 static FAST_CODE_NOINLINE void subTaskRcCommand(timeUs_t currentTimeUs)
 {
+    UNUSED(currentTimeUs);
 
     // If we're armed, at minimum throttle, and we do arming via the
     // sticks, do not process yaw input from the rx.  We do this so the
@@ -999,7 +996,12 @@ static FAST_CODE_NOINLINE void subTaskRcCommand(timeUs_t currentTimeUs)
     }
 
     processRcCommand();
-    UNUSED(currentTimeUs);
+
+#if defined(USE_GPS_RESCUE)
+    if (FLIGHT_MODE(GPS_RESCUE_MODE)) {
+        gpsRescueInjectRcCommands();
+    }
+#endif
 }
 
 // Function for loop trigger

src/main/fc/fc_rc.c

@@ -43,7 +43,6 @@
 
 #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"
@@ -528,10 +527,6 @@ FAST_CODE FAST_CODE_NOINLINE void updateRcCommands(void)
             rcCommand[YAW] = rcCommandBuff.Z;
         }
     }
-
-    if (FLIGHT_MODE(GPS_RESCUE_MODE)) {
-        rcCommand[THROTTLE] = rescueThrottle;
-    }
 }
 
 void resetYawAxis(void)

src/main/flight/gps_rescue.c

@@ -71,6 +71,9 @@ PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
     .minSats = 8
 );
 
+static uint16_t      rescueThrottle;
+static uint16_t      rescueYaw;
+
 int32_t       gpsRescueAngle[ANGLE_INDEX_COUNT] = { 0, 0 };
 uint16_t      hoverThrottle = 0;
 float         averageThrottle = 0.0;
@@ -109,79 +112,81 @@ void updateGPSRescueState(void)
 
     switch (rescueState.phase) {
     case RESCUE_IDLE:
-	idleTasks();
+        idleTasks();
-	break;
+        break;
     case RESCUE_INITIALIZE:
-	if (hoverThrottle == 0) { //no actual throttle data yet, let's use the default.
+        if (hoverThrottle == 0) { //no actual throttle data yet, let's use the default.
-	    hoverThrottle = gpsRescueConfig()->throttleHover;
+            hoverThrottle = gpsRescueConfig()->throttleHover;
-	}
+        }
 
-	rescueState.phase = RESCUE_ATTAIN_ALT;
+        rescueState.phase = RESCUE_ATTAIN_ALT;
-	FALLTHROUGH;
+        FALLTHROUGH;
     case RESCUE_ATTAIN_ALT:
-	// Get to a safe altitude at a low velocity ASAP
+        // Get to a safe altitude at a low velocity ASAP
-	if (ABS(rescueState.intent.targetAltitude - rescueState.sensor.currentAltitude) < 1000) {
+        if (ABS(rescueState.intent.targetAltitude - rescueState.sensor.currentAltitude) < 1000) {
-	    rescueState.phase = RESCUE_CROSSTRACK;
+            rescueState.phase = RESCUE_CROSSTRACK;
-	}
+        }
 
-	rescueState.intent.targetGroundspeed = 500;
+        rescueState.intent.targetGroundspeed = 500;
-	rescueState.intent.targetAltitude = MAX(gpsRescueConfig()->initialAltitude * 100, rescueState.sensor.maxAltitude + 1500);
+        rescueState.intent.targetAltitude = MAX(gpsRescueConfig()->initialAltitude * 100, rescueState.sensor.maxAltitude + 1500);
-	rescueState.intent.crosstrack = true;
+        rescueState.intent.crosstrack = true;
-	rescueState.intent.minAngleDeg = 10;
+        rescueState.intent.minAngleDeg = 10;
-	rescueState.intent.maxAngleDeg = 15;
+        rescueState.intent.maxAngleDeg = 15;
-	break;
+        break;
     case RESCUE_CROSSTRACK:
-	if (rescueState.sensor.distanceToHome < gpsRescueConfig()->descentDistance) {
+        if (rescueState.sensor.distanceToHome < gpsRescueConfig()->descentDistance) {
-	    rescueState.phase = RESCUE_LANDING_APPROACH;
+            rescueState.phase = RESCUE_LANDING_APPROACH;
-	}
+        }
 
-	// We can assume at this point that we are at or above our RTH height, so we need to try and point to home and tilt while maintaining alt
+        // We can assume at this point that we are at or above our RTH height, so we need to try and point to home and tilt while maintaining alt
-	// Is our altitude way off?  We should probably kick back to phase RESCUE_ATTAIN_ALT
+        // Is our altitude way off?  We should probably kick back to phase RESCUE_ATTAIN_ALT
-	rescueState.intent.targetGroundspeed = gpsRescueConfig()->rescueGroundspeed;
+        rescueState.intent.targetGroundspeed = gpsRescueConfig()->rescueGroundspeed;
-	rescueState.intent.targetAltitude = MAX(gpsRescueConfig()->initialAltitude * 100, rescueState.sensor.maxAltitude + 1500);
+        rescueState.intent.targetAltitude = MAX(gpsRescueConfig()->initialAltitude * 100, rescueState.sensor.maxAltitude + 1500);
-	rescueState.intent.crosstrack = true;
+        rescueState.intent.crosstrack = true;
-	rescueState.intent.minAngleDeg = 15;
+        rescueState.intent.minAngleDeg = 15;
-	rescueState.intent.maxAngleDeg = gpsRescueConfig()->angle;
+        rescueState.intent.maxAngleDeg = gpsRescueConfig()->angle;
-	break;
+        break;
     case RESCUE_LANDING_APPROACH:
-	// We are getting close to home in the XY plane, get Z where it needs to be to move to landing phase
+        // We are getting close to home in the XY plane, get Z where it needs to be to move to landing phase
-	if (rescueState.sensor.distanceToHome < 10 && rescueState.sensor.currentAltitude <= 1000) {
+        if (rescueState.sensor.distanceToHome < 10 && rescueState.sensor.currentAltitude <= 1000) {
-	    rescueState.phase = RESCUE_LANDING;
+            rescueState.phase = RESCUE_LANDING;
-	}
+        }
 
-	// Only allow new altitude and new speed to be equal or lower than the current values (to prevent parabolic movement on overshoot)
+        // Only allow new altitude and new speed to be equal or lower than the current values (to prevent parabolic movement on overshoot)
-	int32_t newAlt = gpsRescueConfig()->initialAltitude * 100  * rescueState.sensor.distanceToHome / gpsRescueConfig()->descentDistance;
+        int32_t newAlt = gpsRescueConfig()->initialAltitude * 100  * rescueState.sensor.distanceToHome / gpsRescueConfig()->descentDistance;
-	int32_t newSpeed = gpsRescueConfig()->rescueGroundspeed * rescueState.sensor.distanceToHome / gpsRescueConfig()->descentDistance;
+        int32_t newSpeed = gpsRescueConfig()->rescueGroundspeed * rescueState.sensor.distanceToHome / gpsRescueConfig()->descentDistance;
 
-	rescueState.intent.targetAltitude = constrain(newAlt, 100, rescueState.intent.targetAltitude);
+        rescueState.intent.targetAltitude = constrain(newAlt, 100, rescueState.intent.targetAltitude);
-	rescueState.intent.targetGroundspeed = constrain(newSpeed, 100, rescueState.intent.targetGroundspeed);
+        rescueState.intent.targetGroundspeed = constrain(newSpeed, 100, rescueState.intent.targetGroundspeed);
-	rescueState.intent.crosstrack = true;
+        rescueState.intent.crosstrack = true;
-	rescueState.intent.minAngleDeg = 10;
+        rescueState.intent.minAngleDeg = 10;
-	rescueState.intent.maxAngleDeg = 20;
+        rescueState.intent.maxAngleDeg = 20;
-	break;
+        break;
     case RESCUE_LANDING:
-	// We have reached the XYZ envelope to be considered at "home".  We need to land gently and check our accelerometer for abnormal data.
+        // We have reached the XYZ envelope to be considered at "home".  We need to land gently and check our accelerometer for abnormal data.
-	// At this point, do not let the target altitude go up anymore, so if we overshoot, we dont' move in a parabolic trajectory
+        // At this point, do not let the target altitude go up anymore, so if we overshoot, we dont' move in a parabolic trajectory
 
-	// If we are over 120% of average magnitude, just disarm since we're pretty much home
+        // If we are over 120% of average magnitude, just disarm since we're pretty much home
-	if (rescueState.sensor.accMagnitude > rescueState.sensor.accMagnitudeAvg * 1.5) {
+        if (rescueState.sensor.accMagnitude > rescueState.sensor.accMagnitudeAvg * 1.5) {
-	    disarm();
+            disarm();
-	    rescueState.phase = RESCUE_COMPLETE;
+            rescueState.phase = RESCUE_COMPLETE;
-	}
+        }
 
-	rescueState.intent.targetGroundspeed = 0;
+        rescueState.intent.targetGroundspeed = 0;
-	rescueState.intent.targetAltitude = 0;
+        rescueState.intent.targetAltitude = 0;
-	rescueState.intent.crosstrack = true;
+        rescueState.intent.crosstrack = true;
-	rescueState.intent.minAngleDeg = 0;
+        rescueState.intent.minAngleDeg = 0;
-	rescueState.intent.maxAngleDeg = 15;
+        rescueState.intent.maxAngleDeg = 15;
-	break;
+        break;
     case RESCUE_COMPLETE:
-	rescueStop();
+        rescueStop();
-	break;
+        break;
     case RESCUE_ABORT:
-	disarm();
+        disarm();
-	rescueStop();
+        rescueStop();
-	break;
+        break;
+    default:
+        break;
     }
 
     performSanityChecks();
@@ -396,7 +401,13 @@ void setBearing(int16_t deg)
 
     dif *= -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed);
 
-    rcCommand[YAW] = - (dif * gpsRescueConfig()->yawP / 20);
+    rescueYaw = - (dif * gpsRescueConfig()->yawP / 20);
+}
+
+void gpsRescueInjectRcCommands(void)
+{
+    rcCommand[THROTTLE] = rescueThrottle;
+    rcCommand[YAW] = rescueYaw;
 }
 
 #endif

src/main/flight/gps_rescue.h

@@ -42,8 +42,6 @@ typedef struct gpsRescue_s {
 
 PG_DECLARE(gpsRescueConfig_t, gpsRescueConfig);
 
-uint16_t      rescueThrottle;
-
 typedef enum {
     RESCUE_IDLE,
     RESCUE_INITIALIZE,
@@ -110,3 +108,5 @@ void performSanityChecks(void);
 void sensorUpdate(void);
 
 void rescueAttainPosition(void);
+
+void gpsRescueInjectRcCommands(void);

src/main/io/gps.c

@@ -531,6 +531,9 @@ void gpsUpdate(timeUs_t currentTimeUs)
     if (sensors(SENSOR_GPS)) {
         updateGpsIndicator(currentTimeUs);
     }
+#if defined(USE_GPS_RESCUE)
+    updateGPSRescueState();
+#endif
 }
 
 static void gpsNewData(uint16_t c)