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;
@@ -182,6 +185,8 @@ void updateGPSRescueState(void)
         disarm();
         rescueStop();
         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)