diff --git a/src/main/build/debug.c b/src/main/build/debug.c
index bc8556077f..9ad6d60404 100644
--- a/src/main/build/debug.c
+++ b/src/main/build/debug.c
@@ -68,4 +68,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
     "CURRENT_SENSOR",
     "USB",
     "SMARTAUDIO",
+    "RTH",
 };
diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c
index 2b18e88954..e7f7fa8e83 100644
--- a/src/main/flight/gps_rescue.c
+++ b/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);
-
-    rcCommand[YAW] -= dif * gpsRescue()->yawP / 4;
+    dif = constrain(dif, -5, 5); // TODO: very basic smoothing, we need something less twitchy
+    rcCommand[YAW] -= dif * gpsRescue()->yawP / 20;
 }
 
 #endif
diff --git a/src/main/interface/msp_box.c b/src/main/interface/msp_box.c
index b3639ee95f..0060f87db7 100644
--- a/src/main/interface/msp_box.c
+++ b/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
diff --git a/src/main/io/gps.c b/src/main/io/gps.c
index d7a3c38a0c..f5732a661a 100644
--- a/src/main/io/gps.c
+++ b/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,
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index b3a85f1b0f..b8b4fd5b23 100644
--- a/src/main/io/osd.c
+++ b/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 {