Skip to content

Commit 09d404f

Browse files
breadovenbreadoven
committed
nav hold fix
1 parent 6f8e32f commit 09d404f

File tree

2 files changed

+26
-18
lines changed

2 files changed

+26
-18
lines changed

‎src/main/navigation/navigation.c

Lines changed: 26 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -424,7 +424,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
424424
.persistentId = NAV_PERSISTENT_ID_POSHOLD_3D_INITIALIZE,
425425
.onEntry = navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE,
426426
.timeoutMs = 0,
427-
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT,
427+
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT,
428428
.mapToFlightModes = NAV_ALTHOLD_MODE | NAV_POSHOLD_MODE,
429429
.mwState = MW_NAV_STATE_HOLD_INFINIT,
430430
.mwError = MW_NAV_ERROR_NONE,
@@ -439,7 +439,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
439439
.persistentId = NAV_PERSISTENT_ID_POSHOLD_3D_IN_PROGRESS,
440440
.onEntry = navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS,
441441
.timeoutMs = 10,
442-
.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,
442+
.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,
443443
.mapToFlightModes = NAV_ALTHOLD_MODE | NAV_POSHOLD_MODE,
444444
.mwState = MW_NAV_STATE_HOLD_INFINIT,
445445
.mwError = MW_NAV_ERROR_NONE,
@@ -595,7 +595,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
595595
.persistentId = NAV_PERSISTENT_ID_RTH_CLIMB_TO_SAFE_ALT,
596596
.onEntry = navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT,
597597
.timeoutMs = 10,
598-
.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
598+
.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
599599
.mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
600600
.mwState = MW_NAV_STATE_RTH_CLIMB,
601601
.mwError = MW_NAV_ERROR_WAIT_FOR_RTH_ALT,
@@ -635,7 +635,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
635635
.persistentId = NAV_PERSISTENT_ID_RTH_HEAD_HOME,
636636
.onEntry = navOnEnteringState_NAV_STATE_RTH_HEAD_HOME,
637637
.timeoutMs = 10,
638-
.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,
638+
.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,
639639
.mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
640640
.mwState = MW_NAV_STATE_RTH_ENROUTE,
641641
.mwError = MW_NAV_ERROR_NONE,
@@ -656,7 +656,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
656656
.persistentId = NAV_PERSISTENT_ID_RTH_LOITER_PRIOR_TO_LANDING,
657657
.onEntry = navOnEnteringState_NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING,
658658
.timeoutMs = 500,
659-
.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,
659+
.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,
660660
.mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
661661
.mwState = MW_NAV_STATE_LAND_SETTLE,
662662
.mwError = MW_NAV_ERROR_NONE,
@@ -677,7 +677,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
677677
.persistentId = NAV_PERSISTENT_ID_RTH_LOITER_ABOVE_HOME,
678678
.onEntry = navOnEnteringState_NAV_STATE_RTH_LOITER_ABOVE_HOME,
679679
.timeoutMs = 10,
680-
.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,
680+
.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,
681681
.mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
682682
.mwState = MW_NAV_STATE_HOVER_ABOVE_HOME,
683683
.mwError = MW_NAV_ERROR_NONE,
@@ -696,7 +696,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
696696
.persistentId = NAV_PERSISTENT_ID_RTH_LANDING,
697697
.onEntry = navOnEnteringState_NAV_STATE_RTH_LANDING,
698698
.timeoutMs = 10,
699-
.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,
699+
.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,
700700
.mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
701701
.mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
702702
.mwError = MW_NAV_ERROR_LANDING,
@@ -717,7 +717,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
717717
.persistentId = NAV_PERSISTENT_ID_RTH_FINISHING,
718718
.onEntry = navOnEnteringState_NAV_STATE_RTH_FINISHING,
719719
.timeoutMs = 0,
720-
.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,
720+
.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,
721721
.mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
722722
.mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
723723
.mwError = MW_NAV_ERROR_LANDING,
@@ -828,7 +828,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
828828
.persistentId = NAV_PERSISTENT_ID_WAYPOINT_HOLD_TIME, // There is no state for timed hold?
829829
.onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME,
830830
.timeoutMs = 10,
831-
.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,
831+
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
832832
.mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
833833
.mwState = MW_NAV_STATE_HOLD_TIMED,
834834
.mwError = MW_NAV_ERROR_NONE,
@@ -849,7 +849,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
849849
.persistentId = NAV_PERSISTENT_ID_WAYPOINT_RTH_LAND,
850850
.onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND,
851851
.timeoutMs = 10,
852-
.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,
852+
.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,
853853
.mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
854854
.mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
855855
.mwError = MW_NAV_ERROR_LANDING,
@@ -887,7 +887,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
887887
.persistentId = NAV_PERSISTENT_ID_WAYPOINT_FINISHED,
888888
.onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED,
889889
.timeoutMs = 10,
890-
.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,
890+
.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,
891891
.mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
892892
.mwState = MW_NAV_STATE_WP_ENROUTE,
893893
.mwError = MW_NAV_ERROR_FINISH,
@@ -908,7 +908,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
908908
.persistentId = NAV_PERSISTENT_ID_EMERGENCY_LANDING_INITIALIZE,
909909
.onEntry = navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
910910
.timeoutMs = 0,
911-
.stateFlags = NAV_CTL_EMERG | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE,
911+
.stateFlags = NAV_CTL_EMERG | NAV_REQUIRE_ANGLE,
912912
.mapToFlightModes = 0,
913913
.mwState = MW_NAV_STATE_EMERGENCY_LANDING,
914914
.mwError = MW_NAV_ERROR_LANDING,
@@ -926,7 +926,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
926926
.persistentId = NAV_PERSISTENT_ID_EMERGENCY_LANDING_IN_PROGRESS,
927927
.onEntry = navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS,
928928
.timeoutMs = 10,
929-
.stateFlags = NAV_CTL_EMERG | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE,
929+
.stateFlags = NAV_CTL_EMERG | NAV_REQUIRE_ANGLE,
930930
.mapToFlightModes = 0,
931931
.mwState = MW_NAV_STATE_EMERGENCY_LANDING,
932932
.mwError = MW_NAV_ERROR_LANDING,
@@ -944,7 +944,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
944944
.persistentId = NAV_PERSISTENT_ID_EMERGENCY_LANDING_FINISHED,
945945
.onEntry = navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINISHED,
946946
.timeoutMs = 10,
947-
.stateFlags = NAV_CTL_EMERG | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE,
947+
.stateFlags = NAV_CTL_EMERG | NAV_REQUIRE_ANGLE,
948948
.mapToFlightModes = 0,
949949
.mwState = MW_NAV_STATE_LANDED,
950950
.mwError = MW_NAV_ERROR_LANDING,
@@ -1053,7 +1053,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
10531053
.persistentId = NAV_PERSISTENT_ID_FW_LANDING_CLIMB_TO_LOITER,
10541054
.onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_CLIMB_TO_LOITER,
10551055
.timeoutMs = 10,
1056-
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH,
1056+
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH,
10571057
.mapToFlightModes = NAV_FW_AUTOLAND,
10581058
.mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
10591059
.mwError = MW_NAV_ERROR_NONE,
@@ -1074,7 +1074,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
10741074
.persistentId = NAV_PERSISTENT_ID_FW_LANDING_LOITER,
10751075
.onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_LOITER,
10761076
.timeoutMs = 10,
1077-
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH,
1077+
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH,
10781078
.mapToFlightModes = NAV_FW_AUTOLAND,
10791079
.mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
10801080
.mwError = MW_NAV_ERROR_NONE,
@@ -4081,7 +4081,16 @@ bool isLastMissionWaypoint(void)
40814081
/* Checks if Nav hold position is active */
40824082
bool isNavHoldPositionActive(void)
40834083
{
4084-
return navGetCurrentStateFlags() & NAV_CTL_HOLD;
4084+
// WP mode last WP hold and Timed hold positions
4085+
if (FLIGHT_MODE(NAV_WP_MODE)) {
4086+
return isLastMissionWaypoint() || posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_HOLD_TIME;
4087+
}
4088+
// RTH mode (spiral climb and Home positions but excluding RTH Trackback point positions) and POSHOLD mode
4089+
// Also hold position during emergency landing if position valid
4090+
return (FLIGHT_MODE(NAV_RTH_MODE) && !posControl.flags.rthTrackbackActive) ||
4091+
FLIGHT_MODE(NAV_POSHOLD_MODE) ||
4092+
(posControl.navState == NAV_STATE_FW_LANDING_CLIMB_TO_LOITER || posControl.navState == NAV_STATE_FW_LANDING_LOITER) ||
4093+
navigationIsExecutingAnEmergencyLanding();
40854094
}
40864095

40874096
float getActiveSpeed(void)

‎src/main/navigation/navigation_private.h

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -334,7 +334,6 @@ typedef enum {
334334
NAV_AUTO_WP_DONE = (1 << 15), // Waypoint mission reached the last waypoint and is idling
335335

336336
NAV_MIXERAT = (1 << 16), // MIXERAT in progress
337-
NAV_CTL_HOLD = (1 << 17), // Nav loiter active at position or will be when position reached
338337
} navigationFSMStateFlags_t;
339338

340339
typedef struct {

0 commit comments

Comments
 (0)