Skip to content

Commit dd91a87

Browse files
DzikuVxDzikuVx
authored
Merge pull request #10002 from breadoven/abo_wp_alt_enforce_hold_fix
WP mode altitude enforce hold fix
2 parents f88f630 + 32ba122 commit dd91a87

File tree

3 files changed

+34
-26
lines changed

3 files changed

+34
-26
lines changed

‎src/main/navigation/navigation.c

Lines changed: 27 additions & 23 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_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT,
427+
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_HOLD | 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_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_CTL_HOLD | 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_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_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW, // allow pos adjustment while climbing 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,
@@ -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_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_CTL_HOLD | 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_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_CTL_HOLD | 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_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_HOLD | 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_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_HOLD | 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_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
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,
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_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_HOLD | 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_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_CTL_HOLD | 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,
@@ -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_REQUIRE_ANGLE,
929+
.stateFlags = NAV_CTL_HOLD | 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_REQUIRE_ANGLE,
947+
.stateFlags = NAV_CTL_HOLD | 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_REQUIRE_ANGLE | NAV_AUTO_RTH,
1056+
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | 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_REQUIRE_ANGLE | NAV_AUTO_RTH,
1077+
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | 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,16 +4081,20 @@ bool isLastMissionWaypoint(void)
40814081
/* Checks if Nav hold position is active */
40824082
bool isNavHoldPositionActive(void)
40834083
{
4084-
// WP mode last WP hold and Timed hold positions
4084+
/* If the current Nav state isn't flagged as a hold point (NAV_CTL_HOLD) then
4085+
* waypoints are assumed to be hold points by default unless excluded as defined here */
4086+
4087+
if (navGetCurrentStateFlags() & NAV_CTL_HOLD) {
4088+
return true;
4089+
}
4090+
4091+
// No hold required for basic WP type unless it's the last mission waypoint
40854092
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();
4093+
return posControl.waypointList[posControl.activeWaypointIndex].action != NAV_WP_ACTION_WAYPOINT || isLastMissionWaypoint();
4094+
}
4095+
4096+
// No hold required for Trackback WPs or for fixed wing autoland WPs not flagged as hold points (returned above if they are)
4097+
return !FLIGHT_MODE(NAV_FW_AUTOLAND) && !posControl.flags.rthTrackbackActive;
40944098
}
40954099

40964100
float getActiveSpeed(void)

‎src/main/navigation/navigation_fixedwing.c

Lines changed: 6 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -290,9 +290,12 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
290290
// Detemine if a circular loiter is required.
291291
// For waypoints only use circular loiter when angular visibility is > 30 degs, otherwise head straight toward target
292292
#define TAN_15DEG 0.26795f
293-
needToCalculateCircularLoiter = isNavHoldPositionActive() &&
294-
(distanceToActualTarget <= (navLoiterRadius / TAN_15DEG)) &&
295-
(distanceToActualTarget > 50.0f);
293+
294+
bool loiterApproachActive = isNavHoldPositionActive() &&
295+
distanceToActualTarget <= (navLoiterRadius / TAN_15DEG) &&
296+
distanceToActualTarget > 50.0f;
297+
needToCalculateCircularLoiter = loiterApproachActive || (navGetCurrentStateFlags() & NAV_CTL_HOLD);
298+
296299
//if vtol landing is required, fly straight to homepoint
297300
if ((posControl.navState == NAV_STATE_RTH_HEAD_HOME) && navigationRTHAllowsLanding() && checkMixerATRequired(MIXERAT_REQUEST_LAND)){
298301
needToCalculateCircularLoiter = false;

‎src/main/navigation/navigation_private.h

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -334,6 +334,7 @@ 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
337338
} navigationFSMStateFlags_t;
338339

339340
typedef struct {

0 commit comments

Comments
 (0)