Skip to content

Commit

Permalink
nav hold fix
Browse files Browse the repository at this point in the history
  • Loading branch information
@breadoven
breadoven committed May 1, 2024
1 parent 6f8e32f commit 09d404f
Show file tree
Hide file tree
Showing 2 changed files with 26 additions and 18 deletions.
43 changes: 26 additions & 17 deletions src/main/navigation/navigation.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -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,
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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,
Expand All @@ -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,
Expand All @@ -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,
Expand All @@ -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,
Expand All @@ -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,
Expand Down Expand Up @@ -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,
Expand All @@ -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,
Expand Down Expand Up @@ -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,
Expand All @@ -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,
Expand All @@ -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,
Expand All @@ -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,
Expand Down Expand Up @@ -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,
Expand All @@ -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,
Expand Down Expand Up @@ -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)
Expand Down
1 change: 0 additions & 1 deletion src/main/navigation/navigation_private.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down

0 comments on commit 09d404f

Please sign in to comment.