From a47afe2187f7120ef2462b049ab21f7817595c3e Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Mon, 6 Nov 2023 21:49:20 +0000 Subject: [PATCH 1/2] Show system message when linear descent begins --- src/main/io/osd.c | 17 ++++++++++++++--- src/main/io/osd.h | 1 + src/main/navigation/navigation.c | 5 +++++ src/main/navigation/navigation_private.h | 1 + 4 files changed, 21 insertions(+), 3 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 1662094cabb..89ee0642a48 100644 --- 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; @@ -1011,7 +1012,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 +1059,10 @@ static const char * navigationStateMessage(void) // Not used break; } + + if (!posControl.rthState.rthLinearDescentActive && linearDescentMessageMs != 0) + linearDescentMessageMs = 0; + return NULL; } diff --git a/src/main/io/osd.h b/src/main/io/osd.h index f09c9d049b3..18851c5d804 100644 --- 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 index c991531f32b..db04d38bd70 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -1432,6 +1432,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 +1443,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 = 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 index c408f109c9b..776eca78ea3 100644 --- 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 { From 62d78ae7c2e6b8ed72e9651ca5aa4b24d62ea76b Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Tue, 7 Nov 2023 08:13:35 +0000 Subject: [PATCH 2/2] Bug fix Multiple RTH and early exit from RTH fixed. Message would only be displayed for first RTH. --- src/main/io/osd.c | 6 +++--- src/main/navigation/navigation.c | 5 ++++- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 89ee0642a48..a178d5b55f1 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1001,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; @@ -1060,9 +1063,6 @@ static const char * navigationStateMessage(void) break; } - if (!posControl.rthState.rthLinearDescentActive && linearDescentMessageMs != 0) - linearDescentMessageMs = 0; - return NULL; } diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index db04d38bd70..94014e86c48 100644 --- 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. @@ -1444,7 +1447,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio // 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) + 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