From eb8843bc9c190650c21b3ec10964c1ed96ea8296 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Mon, 15 Apr 2024 18:09:06 +0100 Subject: [PATCH 1/2] changes --- src/main/fc/fc_core.c | 4 + src/main/navigation/navigation.c | 159 +++++++++++------------ src/main/navigation/navigation_private.h | 19 +-- 3 files changed, 87 insertions(+), 95 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 0d0863760de..0c8daea2ca8 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -350,6 +350,10 @@ static void updateArmingStatus(void) DISABLE_ARMING_FLAG(ARMING_DISABLED_NO_PREARM); } + if (ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED) && !IS_RC_MODE_ACTIVE(BOXARM)) { + DISABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED); + } + /* CHECK: Arming switch */ // If arming is disabled and the ARM switch is on // Note that this should be last check so all other blockers could be cleared correctly diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 9e81024c3af..d2a5732de79 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -328,8 +328,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_TRACKBACK(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigationFSMState_t previousState); -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING(navigationFSMState_t previousState); -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(navigationFSMState_t previousState); +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING(navigationFSMState_t previousState); +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LOITER_ABOVE_HOME(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHING(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigationFSMState_t previousState); @@ -423,7 +423,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_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT, .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_POSHOLD_MODE, .mwState = MW_NAV_STATE_HOLD_INFINIT, .mwError = MW_NAV_ERROR_NONE, @@ -438,7 +438,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_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_CTL_HOLD | 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, @@ -585,7 +585,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_CLIMB_TO_SAFE_ALT, [NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_TRACKBACK] = NAV_STATE_RTH_TRACKBACK, [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING] = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING, + [NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING] = NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING, [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, } }, @@ -594,7 +594,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_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_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 .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE, .mwState = MW_NAV_STATE_RTH_CLIMB, .mwError = MW_NAV_ERROR_WAIT_FOR_RTH_ALT, @@ -634,13 +634,13 @@ 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_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_HOLD | 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, .onEvent = { [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HEAD_HOME, // re-process the state - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING, + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING, [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, @@ -651,37 +651,37 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { } }, - [NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING] = { - .persistentId = NAV_PERSISTENT_ID_RTH_HOVER_PRIOR_TO_LANDING, - .onEntry = navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING, + [NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING] = { + .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_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_HOLD | 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, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING, - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_LANDING, - [NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME] = NAV_STATE_RTH_HOVER_ABOVE_HOME, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING, + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_LANDING, + [NAV_FSM_EVENT_SWITCH_TO_RTH_LOITER_ABOVE_HOME] = NAV_STATE_RTH_LOITER_ABOVE_HOME, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, } }, - [NAV_STATE_RTH_HOVER_ABOVE_HOME] = { - .persistentId = NAV_PERSISTENT_ID_RTH_HOVER_ABOVE_HOME, - .onEntry = navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME, + [NAV_STATE_RTH_LOITER_ABOVE_HOME] = { + .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_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_CTL_HOLD | 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, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HOVER_ABOVE_HOME, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_LOITER_ABOVE_HOME, [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, @@ -695,20 +695,20 @@ 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_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_HOLD | 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, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_LANDING, // re-process state - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_FINISHING, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING] = NAV_STATE_FW_LANDING_CLIMB_TO_LOITER, - [NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME] = NAV_STATE_RTH_HOVER_ABOVE_HOME, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_LANDING, // re-process state + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_FINISHING, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING] = NAV_STATE_FW_LANDING_CLIMB_TO_LOITER, + [NAV_FSM_EVENT_SWITCH_TO_RTH_LOITER_ABOVE_HOME] = NAV_STATE_RTH_LOITER_ABOVE_HOME, } }, @@ -716,7 +716,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_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_HOLD | 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, @@ -827,7 +827,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_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP, + .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, .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE, .mwState = MW_NAV_STATE_HOLD_TIMED, .mwError = MW_NAV_ERROR_NONE, @@ -848,7 +848,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_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_HOLD | 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, @@ -886,7 +886,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_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_CTL_HOLD | 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, @@ -907,7 +907,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_REQUIRE_ANGLE, + .stateFlags = NAV_CTL_EMERG | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE, .mapToFlightModes = 0, .mwState = MW_NAV_STATE_EMERGENCY_LANDING, .mwError = MW_NAV_ERROR_LANDING, @@ -925,7 +925,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_REQUIRE_ANGLE, + .stateFlags = NAV_CTL_EMERG | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE, .mapToFlightModes = 0, .mwState = MW_NAV_STATE_EMERGENCY_LANDING, .mwError = MW_NAV_ERROR_LANDING, @@ -943,7 +943,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_REQUIRE_ANGLE, + .stateFlags = NAV_CTL_EMERG | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE, .mapToFlightModes = 0, .mwState = MW_NAV_STATE_LANDED, .mwError = MW_NAV_ERROR_LANDING, @@ -1052,7 +1052,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_REQUIRE_ANGLE | NAV_AUTO_RTH, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH, .mapToFlightModes = NAV_FW_AUTOLAND, .mwState = MW_NAV_STATE_LAND_IN_PROGRESS, .mwError = MW_NAV_ERROR_NONE, @@ -1073,7 +1073,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_REQUIRE_ANGLE | NAV_AUTO_RTH, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH, .mapToFlightModes = NAV_FW_AUTOLAND, .mwState = MW_NAV_STATE_LAND_IN_PROGRESS, .mwError = MW_NAV_ERROR_NONE, @@ -1246,7 +1246,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE(n } // Prepare position controller if idle or current Mode NOT active in position hold state - if (previousState != NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING && previousState != NAV_STATE_RTH_HOVER_ABOVE_HOME && + if (previousState != NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING && previousState != NAV_STATE_RTH_LOITER_ABOVE_HOME && previousState != NAV_STATE_RTH_LANDING && previousState != NAV_STATE_WAYPOINT_RTH_LAND && previousState != NAV_STATE_WAYPOINT_FINISHED && previousState != NAV_STATE_WAYPOINT_HOLD_TIME) { @@ -1427,7 +1427,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati setHomePosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING, NAV_HOME_VALID_ALL); setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); - return NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING; // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING + return NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING; // NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING } else { // Switch to RTH trackback @@ -1501,8 +1501,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n initializeRTHSanityChecker(); } - // Save initial home distance for future use + // Save initial home distance and direction for future use posControl.rthState.rthInitialDistance = posControl.homeDistance; + posControl.activeWaypoint.bearing = posControl.homeDirection; fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL); if (navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING_LEGACY)) { @@ -1616,7 +1617,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio if ((posControl.flags.estPosStatus >= EST_USABLE)) { fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL); - if (isWaypointReached(tmpHomePos, 0)) { + if (isWaypointReached(tmpHomePos, &posControl.activeWaypoint.bearing)) { // 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); @@ -1625,7 +1626,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio 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 + return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING } else { setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_XY); return NAV_FSM_EVENT_NONE; @@ -1639,7 +1640,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio return NAV_FSM_EVENT_NONE; } -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING(navigationFSMState_t previousState) +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING(navigationFSMState_t previousState) { UNUSED(previousState); @@ -1673,7 +1674,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND if (!pauseLanding && ((ABS(wrap_18000(posControl.rthState.homePosition.heading - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) || STATE(FIXED_WING_LEGACY))) { resetLandingDetector(); // force reset landing detector just in case updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET); - return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; // success = land + return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_LOITER_ABOVE_HOME; // success = land } else { fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_FINAL); setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); @@ -1681,7 +1682,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND } } -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(navigationFSMState_t previousState) +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LOITER_ABOVE_HOME(navigationFSMState_t previousState) { UNUSED(previousState); @@ -1690,7 +1691,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(na return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } - fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_HOVER); + fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_LOITER); setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z); return NAV_FSM_EVENT_NONE; @@ -2046,9 +2047,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navig #ifdef USE_FW_AUTOLAND if (landEvent == NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING) { return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING; - } else + } else #endif - if (landEvent == NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME) { + if (landEvent == NAV_FSM_EVENT_SWITCH_TO_RTH_LOITER_ABOVE_HOME) { return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED; } else if (landEvent == NAV_FSM_EVENT_SUCCESS) { // Landing controller returned success - invoke RTH finishing state and finish the waypoint @@ -2567,7 +2568,7 @@ static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode) posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthFinalAltitude; break; - case RTH_HOME_FINAL_HOVER: + case RTH_HOME_FINAL_LOITER: if (navConfig()->general.rth_home_altitude) { posControl.rthState.homeTmpWaypoint.z = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_home_altitude; } @@ -2852,30 +2853,28 @@ static bool getLocalPosNextWaypoint(fpVector3_t * nextWpPos) /*----------------------------------------------------------- * Check if waypoint is/was reached. - * waypointBearing stores initial bearing to waypoint + * 'waypointBearing' stores initial bearing to waypoint. *-----------------------------------------------------------*/ static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * waypointBearing) { posControl.wpDistance = calculateDistanceToDestination(waypointPos); - // Airplane will do a circular loiter at hold waypoints and might never approach them closer than waypoint_radius - // Check within 10% margin of circular loiter radius - if (STATE(AIRPLANE) && isNavHoldPositionActive() && posControl.wpDistance <= (navConfig()->fw.loiter_radius * 1.10f)) { - return true; - } + // Check if waypoint was missed based on bearing to WP exceeding given angular limit relative to initial waypoint bearing. + // Default angular limit = 100 degs with a reduced limit of 60 degs used if fixed wing waypoint turn smoothing option active + uint16_t relativeBearingErrorLimit = 10000; - if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP || posControl.flags.rthTrackbackActive) { + if (STATE(AIRPLANE) && posControl.flags.wpTurnSmoothingActive) { // If WP turn smoothing CUT option used WP is reached when start of turn is initiated - if (navConfig()->fw.wp_turn_smoothing == WP_TURN_SMOOTHING_CUT && posControl.flags.wpTurnSmoothingActive) { + if (navConfig()->fw.wp_turn_smoothing == WP_TURN_SMOOTHING_CUT) { posControl.flags.wpTurnSmoothingActive = false; return true; } - // Check if waypoint was missed based on bearing to WP exceeding 100 degrees relative to waypoint Yaw - // Same method for turn smoothing option but relative bearing set at 60 degrees - uint16_t relativeBearing = posControl.flags.wpTurnSmoothingActive ? 6000 : 10000; - if (ABS(wrap_18000(calculateBearingToDestination(waypointPos) - *waypointBearing)) > relativeBearing) { - return true; - } + relativeBearingErrorLimit = 6000; + } + + + if (ABS(wrap_18000(calculateBearingToDestination(waypointPos) - *waypointBearing)) > relativeBearingErrorLimit) { + return true; } return posControl.wpDistance <= (navConfig()->general.waypoint_radius); @@ -3453,9 +3452,6 @@ void updateLandingStatus(timeMs_t currentTimeMs) } resetLandingDetector(); - if (!IS_RC_MODE_ACTIVE(BOXARM)) { - DISABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED); - } return; } @@ -4075,16 +4071,7 @@ bool isLastMissionWaypoint(void) /* Checks if Nav hold position is active */ bool isNavHoldPositionActive(void) { - // WP mode last WP hold and Timed hold positions - if (FLIGHT_MODE(NAV_WP_MODE)) { - return isLastMissionWaypoint() || NAV_Status.state == MW_NAV_STATE_HOLD_TIMED; - } - // 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(); + return navGetCurrentStateFlags() & NAV_CTL_HOLD; } float getActiveSpeed(void) @@ -4123,7 +4110,7 @@ bool isWaypointNavTrackingActive(void) // is set from current position not previous WP. Works for WP Restart intermediate WP as well as first mission WP. // (NAV_WP_MODE flag isn't set until WP initialisation is finished, i.e. after calculateAndSetActiveWaypoint called) - return FLIGHT_MODE(NAV_WP_MODE) + return FLIGHT_MODE(NAV_WP_MODE) || posControl.navState == NAV_STATE_FW_LANDING_APPROACH || (posControl.flags.rthTrackbackActive && posControl.activeRthTBPointIndex != posControl.rthTBLastSavedIndex); } @@ -5011,7 +4998,7 @@ bool navigationRTHAllowsLanding(void) return false; } #endif - + // WP mission RTH landing setting if (isWaypointMissionRTHActive() && isWaypointMissionValid()) { return posControl.waypointList[posControl.startWpIndex + posControl.waypointCount - 1].p1 > 0; diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index d716c58c496..121597c93bc 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -171,7 +171,7 @@ typedef enum { NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND = NAV_FSM_EVENT_STATE_SPECIFIC_1, NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED = NAV_FSM_EVENT_STATE_SPECIFIC_2, NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME = NAV_FSM_EVENT_STATE_SPECIFIC_3, - NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME = NAV_FSM_EVENT_STATE_SPECIFIC_4, + NAV_FSM_EVENT_SWITCH_TO_RTH_LOITER_ABOVE_HOME = NAV_FSM_EVENT_STATE_SPECIFIC_4, NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_TRACKBACK = NAV_FSM_EVENT_STATE_SPECIFIC_5, NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE = NAV_FSM_EVENT_STATE_SPECIFIC_6, NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT = NAV_FSM_EVENT_STATE_SPECIFIC_6, @@ -197,7 +197,7 @@ typedef enum { NAV_PERSISTENT_ID_RTH_INITIALIZE = 8, NAV_PERSISTENT_ID_RTH_CLIMB_TO_SAFE_ALT = 9, NAV_PERSISTENT_ID_RTH_HEAD_HOME = 10, - NAV_PERSISTENT_ID_RTH_HOVER_PRIOR_TO_LANDING = 11, + NAV_PERSISTENT_ID_RTH_LOITER_PRIOR_TO_LANDING = 11, NAV_PERSISTENT_ID_RTH_LANDING = 12, NAV_PERSISTENT_ID_RTH_FINISHING = 13, NAV_PERSISTENT_ID_RTH_FINISHED = 14, @@ -228,7 +228,7 @@ typedef enum { NAV_PERSISTENT_ID_CRUISE_ADJUSTING = 34, NAV_PERSISTENT_ID_WAYPOINT_HOLD_TIME = 35, - NAV_PERSISTENT_ID_RTH_HOVER_ABOVE_HOME = 36, + NAV_PERSISTENT_ID_RTH_LOITER_ABOVE_HOME = 36, NAV_PERSISTENT_ID_UNUSED_4 = 37, // was NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME NAV_PERSISTENT_ID_RTH_TRACKBACK = 38, @@ -258,8 +258,8 @@ typedef enum { NAV_STATE_RTH_CLIMB_TO_SAFE_ALT, NAV_STATE_RTH_TRACKBACK, NAV_STATE_RTH_HEAD_HOME, - NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING, - NAV_STATE_RTH_HOVER_ABOVE_HOME, + NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING, + NAV_STATE_RTH_LOITER_ABOVE_HOME, NAV_STATE_RTH_LANDING, NAV_STATE_RTH_FINISHING, NAV_STATE_RTH_FINISHED, @@ -287,7 +287,7 @@ typedef enum { NAV_STATE_CRUISE_INITIALIZE, NAV_STATE_CRUISE_IN_PROGRESS, NAV_STATE_CRUISE_ADJUSTING, - + NAV_STATE_FW_LANDING_CLIMB_TO_LOITER, NAV_STATE_FW_LANDING_LOITER, NAV_STATE_FW_LANDING_APPROACH, @@ -327,9 +327,10 @@ typedef enum { /* Additional flags */ NAV_CTL_LAND = (1 << 14), - NAV_AUTO_WP_DONE = (1 << 15), //Waypoint mission reached the last waypoint and is idling + NAV_AUTO_WP_DONE = (1 << 15), // Waypoint mission reached the last waypoint and is idling - NAV_MIXERAT = (1 << 16), //MIXERAT in progress + 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 { @@ -396,7 +397,7 @@ typedef enum { RTH_HOME_ENROUTE_INITIAL, // Initial position for RTH approach RTH_HOME_ENROUTE_PROPORTIONAL, // Prorpotional position for RTH approach RTH_HOME_ENROUTE_FINAL, // Final position for RTH approach - RTH_HOME_FINAL_HOVER, // Final hover altitude (if rth_home_altitude is set) + RTH_HOME_FINAL_LOITER, // Final loiter altitude (if rth_home_altitude is set) RTH_HOME_FINAL_LAND, // Home position and altitude } rthTargetMode_e; From 79e974d73555d8c91037e48acd7da35c7f547fe9 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 16 Apr 2024 09:48:31 +0100 Subject: [PATCH 2/2] Update navigation.c --- src/main/navigation/navigation.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index d2a5732de79..c85359ee50d 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2859,21 +2859,21 @@ static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * w { posControl.wpDistance = calculateDistanceToDestination(waypointPos); - // Check if waypoint was missed based on bearing to WP exceeding given angular limit relative to initial waypoint bearing. + // Check if waypoint was missed based on bearing to waypoint exceeding given angular limit relative to initial waypoint bearing. // Default angular limit = 100 degs with a reduced limit of 60 degs used if fixed wing waypoint turn smoothing option active - uint16_t relativeBearingErrorLimit = 10000; + uint16_t relativeBearingTargetAngle = 10000; if (STATE(AIRPLANE) && posControl.flags.wpTurnSmoothingActive) { - // If WP turn smoothing CUT option used WP is reached when start of turn is initiated + // If WP mode turn smoothing CUT option used waypoint is reached when start of turn is initiated if (navConfig()->fw.wp_turn_smoothing == WP_TURN_SMOOTHING_CUT) { posControl.flags.wpTurnSmoothingActive = false; return true; } - relativeBearingErrorLimit = 6000; + relativeBearingTargetAngle = 6000; } - if (ABS(wrap_18000(calculateBearingToDestination(waypointPos) - *waypointBearing)) > relativeBearingErrorLimit) { + if (ABS(wrap_18000(calculateBearingToDestination(waypointPos) - *waypointBearing)) > relativeBearingTargetAngle) { return true; }