Fix GPS Rescue (needs yaw smoothing but works).

src/main/build/debug.c

@@ -68,4 +68,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
     "CURRENT_SENSOR",
     "USB",
     "SMARTAUDIO",
+    "RTH",
 };

src/main/flight/gps_rescue.c

@@ -164,7 +164,6 @@ void updateGPSRescueState(void)
     }
     
     newGPSData = false;
-    
 }
 
 void sensorUpdate()
@@ -304,6 +303,7 @@ void rescueAttainPosition()
         return;
     }
 
+    
     /**
         Speed controller
     */
@@ -363,8 +363,8 @@ void setBearing(int16_t deg)
         dif -= 360;
 
     dif *= -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed);
-
+    dif = constrain(dif, -5, 5); // TODO: very basic smoothing, we need something less twitchy
-    rcCommand[YAW] -= dif * gpsRescue()->yawP / 4;
+    rcCommand[YAW] -= dif * gpsRescue()->yawP / 20;
 }
 
 #endif

src/main/interface/msp_box.c

@@ -196,6 +196,7 @@ void initActiveBoxIds(void)
     if (feature(FEATURE_GPS)) {
         BME(BOXGPSHOME);
         BME(BOXGPSHOLD);
+        BME(BOXGPSRESCUE);
         BME(BOXBEEPGPSCOUNT);
     }
 #endif

src/main/io/gps.c

@@ -243,7 +243,7 @@ PG_RESET_TEMPLATE(gpsRescue_t, gpsRescue,
     .velP = 80,
     .velI = 20,
     .velD = 15,
-    .yawP = 15,
+    .yawP = 40,
     .throttleMin = 1200,
     .throttleMax = 1600,
     .throttleHover = 1280,

src/main/io/osd.c

@@ -548,6 +548,8 @@ static bool osdDrawSingleElement(uint8_t item)
                 strcpy(buff, "STAB");
             } else if (FLIGHT_MODE(HORIZON_MODE)) {
                 strcpy(buff, "HOR ");
+            } else if (FLIGHT_MODE(GPS_RESCUE_MODE)) {
+                strcpy(buff, "RESC");
             } else if (isAirmodeActive()) {
                 strcpy(buff, "AIR ");
             } else {