Merge pull request #6132 from mikeller/fix_pid_loop_slowness

Remove GPS rescue update from PID loop.

src/main/fc/fc_core.c

@@ -548,7 +548,7 @@ uint8_t calculateThrottlePercent(void)
 
 static bool airmodeIsActivated;
 
-bool isAirmodeActivated() 
+bool isAirmodeActivated()
 {
     return airmodeIsActivated;
 }
@@ -808,7 +808,7 @@ bool processRx(timeUs_t currentTimeUs)
         }
     }
 #endif
-    
+
     if (IS_RC_MODE_ACTIVE(BOXPASSTHRU)) {
         ENABLE_FLIGHT_MODE(PASSTHRU_MODE);
     } else {
@@ -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