From 09d404f06493e71cf3a96b153c7b0cd909129918 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Wed, 1 May 2024 19:58:53 +0100 Subject: [PATCH] nav hold fix --- src/main/navigation/navigation.c | 43 ++++++++++++++---------- src/main/navigation/navigation_private.h | 1 - 2 files changed, 26 insertions(+), 18 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index b443dc49941..eca17c2d656 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -424,7 +424,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_POSHOLD_3D_INITIALIZE, .onEntry = navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE, .timeoutMs = 0, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT, .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_POSHOLD_MODE, .mwState = MW_NAV_STATE_HOLD_INFINIT, .mwError = MW_NAV_ERROR_NONE, @@ -439,7 +439,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_POSHOLD_3D_IN_PROGRESS, .onEntry = navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS, .timeoutMs = 10, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT | NAV_RC_ALT | NAV_RC_POS | NAV_RC_YAW, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT | NAV_RC_ALT | NAV_RC_POS | NAV_RC_YAW, .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_POSHOLD_MODE, .mwState = MW_NAV_STATE_HOLD_INFINIT, .mwError = MW_NAV_ERROR_NONE, @@ -595,7 +595,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_RTH_CLIMB_TO_SAFE_ALT, .onEntry = navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT, .timeoutMs = 10, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW, // allow pos adjustment while climbind to safe alt + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW, // allow pos adjustment while climbind to safe alt .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE, .mwState = MW_NAV_STATE_RTH_CLIMB, .mwError = MW_NAV_ERROR_WAIT_FOR_RTH_ALT, @@ -635,7 +635,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_RTH_HEAD_HOME, .onEntry = navOnEnteringState_NAV_STATE_RTH_HEAD_HOME, .timeoutMs = 10, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW, .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE, .mwState = MW_NAV_STATE_RTH_ENROUTE, .mwError = MW_NAV_ERROR_NONE, @@ -656,7 +656,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_RTH_LOITER_PRIOR_TO_LANDING, .onEntry = navOnEnteringState_NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING, .timeoutMs = 500, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW, .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE, .mwState = MW_NAV_STATE_LAND_SETTLE, .mwError = MW_NAV_ERROR_NONE, @@ -677,7 +677,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_RTH_LOITER_ABOVE_HOME, .onEntry = navOnEnteringState_NAV_STATE_RTH_LOITER_ABOVE_HOME, .timeoutMs = 10, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT, .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE, .mwState = MW_NAV_STATE_HOVER_ABOVE_HOME, .mwError = MW_NAV_ERROR_NONE, @@ -696,7 +696,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_RTH_LANDING, .onEntry = navOnEnteringState_NAV_STATE_RTH_LANDING, .timeoutMs = 10, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW, .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE, .mwState = MW_NAV_STATE_LAND_IN_PROGRESS, .mwError = MW_NAV_ERROR_LANDING, @@ -717,7 +717,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_RTH_FINISHING, .onEntry = navOnEnteringState_NAV_STATE_RTH_FINISHING, .timeoutMs = 0, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH, .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE, .mwState = MW_NAV_STATE_LAND_IN_PROGRESS, .mwError = MW_NAV_ERROR_LANDING, @@ -828,7 +828,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_WAYPOINT_HOLD_TIME, // There is no state for timed hold? .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME, .timeoutMs = 10, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP, .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE, .mwState = MW_NAV_STATE_HOLD_TIMED, .mwError = MW_NAV_ERROR_NONE, @@ -849,7 +849,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_WAYPOINT_RTH_LAND, .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND, .timeoutMs = 10, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP, .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE, .mwState = MW_NAV_STATE_LAND_IN_PROGRESS, .mwError = MW_NAV_ERROR_LANDING, @@ -887,7 +887,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_WAYPOINT_FINISHED, .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED, .timeoutMs = 10, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP | NAV_AUTO_WP_DONE, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP | NAV_AUTO_WP_DONE, .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE, .mwState = MW_NAV_STATE_WP_ENROUTE, .mwError = MW_NAV_ERROR_FINISH, @@ -908,7 +908,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_EMERGENCY_LANDING_INITIALIZE, .onEntry = navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITIALIZE, .timeoutMs = 0, - .stateFlags = NAV_CTL_EMERG | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE, + .stateFlags = NAV_CTL_EMERG | NAV_REQUIRE_ANGLE, .mapToFlightModes = 0, .mwState = MW_NAV_STATE_EMERGENCY_LANDING, .mwError = MW_NAV_ERROR_LANDING, @@ -926,7 +926,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_EMERGENCY_LANDING_IN_PROGRESS, .onEntry = navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS, .timeoutMs = 10, - .stateFlags = NAV_CTL_EMERG | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE, + .stateFlags = NAV_CTL_EMERG | NAV_REQUIRE_ANGLE, .mapToFlightModes = 0, .mwState = MW_NAV_STATE_EMERGENCY_LANDING, .mwError = MW_NAV_ERROR_LANDING, @@ -944,7 +944,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_EMERGENCY_LANDING_FINISHED, .onEntry = navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINISHED, .timeoutMs = 10, - .stateFlags = NAV_CTL_EMERG | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE, + .stateFlags = NAV_CTL_EMERG | NAV_REQUIRE_ANGLE, .mapToFlightModes = 0, .mwState = MW_NAV_STATE_LANDED, .mwError = MW_NAV_ERROR_LANDING, @@ -1053,7 +1053,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_FW_LANDING_CLIMB_TO_LOITER, .onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_CLIMB_TO_LOITER, .timeoutMs = 10, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH, .mapToFlightModes = NAV_FW_AUTOLAND, .mwState = MW_NAV_STATE_LAND_IN_PROGRESS, .mwError = MW_NAV_ERROR_NONE, @@ -1074,7 +1074,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_FW_LANDING_LOITER, .onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_LOITER, .timeoutMs = 10, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH, .mapToFlightModes = NAV_FW_AUTOLAND, .mwState = MW_NAV_STATE_LAND_IN_PROGRESS, .mwError = MW_NAV_ERROR_NONE, @@ -4081,7 +4081,16 @@ bool isLastMissionWaypoint(void) /* Checks if Nav hold position is active */ bool isNavHoldPositionActive(void) { - return navGetCurrentStateFlags() & NAV_CTL_HOLD; + // WP mode last WP hold and Timed hold positions + if (FLIGHT_MODE(NAV_WP_MODE)) { + return isLastMissionWaypoint() || posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_HOLD_TIME; + } + // RTH mode (spiral climb and Home positions but excluding RTH Trackback point positions) and POSHOLD mode + // Also hold position during emergency landing if position valid + return (FLIGHT_MODE(NAV_RTH_MODE) && !posControl.flags.rthTrackbackActive) || + FLIGHT_MODE(NAV_POSHOLD_MODE) || + (posControl.navState == NAV_STATE_FW_LANDING_CLIMB_TO_LOITER || posControl.navState == NAV_STATE_FW_LANDING_LOITER) || + navigationIsExecutingAnEmergencyLanding(); } float getActiveSpeed(void) diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 015c9e371c5..5d23d36fc59 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -334,7 +334,6 @@ typedef enum { NAV_AUTO_WP_DONE = (1 << 15), // Waypoint mission reached the last waypoint and is idling NAV_MIXERAT = (1 << 16), // MIXERAT in progress - NAV_CTL_HOLD = (1 << 17), // Nav loiter active at position or will be when position reached } navigationFSMStateFlags_t; typedef struct {