Merge pull request #9453 from iNavFlight/MrD_Add-message-when-transit…

…ioning-to-linear-descent
iNavFlight · Nov 8, 2023 · a54849b · a54849b
2 parents 168906c + 62d78ae
commit a54849b
Show file tree

Hide file tree

Showing 4 changed files with 24 additions and 3 deletions.
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
@@ -154,8 +154,9 @@
 
 #define OSD_MIN_FONT_VERSION 3
 
-static timeMs_t notify_settings_saved = 0;
-static bool     savingSettings = false;
+static timeMs_t linearDescentMessageMs  = 0;
+static timeMs_t notify_settings_saved   = 0;
+static bool     savingSettings          = false;
 
 static unsigned currentLayout = 0;
 static int layoutOverride = -1;
@@ -1000,6 +1001,9 @@ static const char * divertingToSafehomeMessage(void)
 
 static const char * navigationStateMessage(void)
 {
+    if (!posControl.rthState.rthLinearDescentActive && linearDescentMessageMs != 0)
+        linearDescentMessageMs = 0;
+
     switch (NAV_Status.state) {
         case MW_NAV_STATE_NONE:
             break;
@@ -1011,7 +1015,13 @@ static const char * navigationStateMessage(void)
             if (posControl.flags.rthTrackbackActive) {
                 return OSD_MESSAGE_STR(OSD_MSG_RTH_TRACKBACK);
             } else {
-                return OSD_MESSAGE_STR(OSD_MSG_HEADING_HOME);
+                if (posControl.rthState.rthLinearDescentActive && (linearDescentMessageMs == 0 || linearDescentMessageMs > millis())) {
+                    if (linearDescentMessageMs == 0)
+                        linearDescentMessageMs = millis() + 5000; // Show message for 5 seconds.
+
+                    return OSD_MESSAGE_STR(OSD_MSG_RTH_LINEAR_DESCENT);
+                } else 
+                    return OSD_MESSAGE_STR(OSD_MSG_HEADING_HOME);
             }
         case MW_NAV_STATE_HOLD_INFINIT:
             // Used by HOLD flight modes. No information to add.
@@ -1052,6 +1062,7 @@ static const char * navigationStateMessage(void)
             // Not used
             break;
     }
+
     return NULL;
 }
 

diff --git a/src/main/io/osd.h b/src/main/io/osd.h
@@ -93,6 +93,7 @@
 #define OSD_MSG_RTH_CLIMB           "ADJUSTING RTH ALTITUDE"
 #define OSD_MSG_RTH_TRACKBACK       "RTH BACK TRACKING"
 #define OSD_MSG_HEADING_HOME        "EN ROUTE TO HOME"
+#define OSD_MSG_RTH_LINEAR_DESCENT  "BEGIN LINEAR DESCENT"
 #define OSD_MSG_WP_FINISHED         "WP END>HOLDING POSITION"
 #define OSD_MSG_WP_LANDED           "WP END>LANDED"
 #define OSD_MSG_PREPARE_NEXT_WP     "PREPARING FOR NEXT WAYPOINT"

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
@@ -1227,6 +1227,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
 {
     UNUSED(previousState);
 
+    if (navConfig()->general.flags.rth_use_linear_descent && posControl.rthState.rthLinearDescentActive)
+                posControl.rthState.rthLinearDescentActive = false;
+
     if ((posControl.flags.estHeadingStatus == EST_NONE) || (posControl.flags.estAltStatus == EST_NONE) || !STATE(GPS_FIX_HOME)) {
         // Heading sensor, altitude sensor and HOME fix are mandatory for RTH. If not satisfied - switch to emergency landing
         // Relevant to failsafe forced RTH only. Switched RTH blocked in selectNavEventFromBoxModeInput if sensors unavailable.
@@ -1432,6 +1435,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
 
         if (homeDistance <= METERS_TO_CENTIMETERS(navConfig()->general.rth_linear_descent_start_distance)) {
             posControl.rthState.rthFinalAltitude = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_home_altitude;
+            posControl.rthState.rthLinearDescentActive = true;
         }
     }
 
@@ -1442,6 +1446,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
         if (isWaypointReached(tmpHomePos, 0)) {
             // Successfully reached position target - update XYZ-position
             setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
+
+            if (navConfig()->general.flags.rth_use_linear_descent && posControl.rthState.rthLinearDescentActive)
+                posControl.rthState.rthLinearDescentActive = false;
+
             return NAV_FSM_EVENT_SUCCESS;       // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING
         } else {
             setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_XY);

diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h
@@ -350,6 +350,7 @@ typedef struct {
     float                   rthInitialDistance;     // Distance when starting flight home
     fpVector3_t             homeTmpWaypoint;        // Temporary storage for home target
     fpVector3_t             originalHomePosition;   // the original rth home - save it, since it could be replaced by safehome or HOME_RESET
+    bool                    rthLinearDescentActive; // Activation status of Linear Descent
 } rthState_t;
 
 typedef enum {