Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 09:16:01 +03:00
Code Issues Wiki Activity

Split auto/manual speed and climb rate limitation; Remove 2D RTH; RTH safety refactoring

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2017-03-13 21:14:42 +10:00
parent 9afbec557c
commit 8b7a14aea7
8 changed files with 159 additions and 347 deletions
Download patch file Download diff file Expand all files Collapse all files

419
src/main/navigation/navigation.c
View file

@ -86,8 +86,8 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.pos_failure_timeout = 5, // 5 sec
.waypoint_radius = 100, // 2m diameter
.waypoint_safe_distance = 10000, // 100m - first waypoint should be closer than this
.max_speed = 300, // 3 m/s = 10.8 km/h
.max_climb_rate = 500, // 5 m/s
.max_auto_speed = 300, // 3 m/s = 10.8 km/h
.max_auto_climb_rate = 500, // 5 m/s
.max_manual_speed = 500,
.max_manual_climb_rate = 200,
.land_descent_rate = 200, // 2 m/s
@ -175,17 +175,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_2D_IN_PROGRESS(
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_INITIALIZE(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_HEAD_HOME(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_FINISHING(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_FINISHED(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_INITIALIZE(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_CLIMB_TO_SAFE_ALT(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_HEAD_HOME(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_HOVER_PRIOR_TO_LANDING(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_LANDING(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_FINISHING(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_FINISHED(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(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_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);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(navigationFSMState_t previousState);
@ -319,113 +314,33 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
}
},
/** RTH mode entry point ************************************************/
/** RTH_3D mode ************************************************/
[NAV_STATE_RTH_INITIALIZE] = {
.onEntry = navOnEnteringState_NAV_STATE_RTH_INITIALIZE,
.timeoutMs = 10,
.stateFlags = NAV_REQUIRE_ANGLE | NAV_AUTO_RTH,
.mapToFlightModes = NAV_RTH_MODE,
.mwState = MW_NAV_STATE_RTH_START,
.mwError = MW_NAV_ERROR_SPOILED_GPS, // we are stuck in this state only if GPS is compromised
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_INITIALIZE, // re-process the state
[NAV_FSM_EVENT_SWITCH_TO_RTH_2D] = NAV_STATE_RTH_2D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_RTH_3D] = NAV_STATE_RTH_3D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
}
},
/** RTH_2D mode ************************************************/
[NAV_STATE_RTH_2D_INITIALIZE] = {
.onEntry = navOnEnteringState_NAV_STATE_RTH_2D_INITIALIZE,
.timeoutMs = 10,
.stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_AUTO_RTH,
.mapToFlightModes = NAV_RTH_MODE,
.mwState = MW_NAV_STATE_RTH_START,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_2D_INITIALIZE, // re-process the state
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_2D_HEAD_HOME,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
}
},
[NAV_STATE_RTH_2D_HEAD_HOME] = {
.onEntry = navOnEnteringState_NAV_STATE_RTH_2D_HEAD_HOME,
.timeoutMs = 10,
.stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,
.mapToFlightModes = NAV_RTH_MODE,
.mwState = MW_NAV_STATE_RTH_ENROUTE,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_2D_HEAD_HOME, // re-process the state
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_2D_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_2D] = NAV_STATE_POSHOLD_2D_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_STATE_RTH_2D_FINISHING] = {
.onEntry = navOnEnteringState_NAV_STATE_RTH_2D_FINISHING,
.timeoutMs = 0,
.stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,
.mapToFlightModes = NAV_RTH_MODE,
.mwState = MW_NAV_STATE_RTH_ENROUTE,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_2D_FINISHED,
}
},
[NAV_STATE_RTH_2D_FINISHED] = {
.onEntry = navOnEnteringState_NAV_STATE_RTH_2D_FINISHED,
.timeoutMs = 10,
.stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,
.mapToFlightModes = NAV_RTH_MODE,
.mwState = MW_NAV_STATE_RTH_ENROUTE,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_2D_FINISHED, // re-process the state
[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_2D] = NAV_STATE_POSHOLD_2D_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,
}
},
/** RTH_3D mode ************************************************/
[NAV_STATE_RTH_3D_INITIALIZE] = {
.onEntry = navOnEnteringState_NAV_STATE_RTH_3D_INITIALIZE,
.timeoutMs = 10,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH,
.mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_RTH_START,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_3D_INITIALIZE, // re-process the state
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_3D_CLIMB_TO_SAFE_ALT,
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_INITIALIZE, // re-process the state
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_CLIMB_TO_SAFE_ALT,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_RTH_3D_LANDING] = NAV_STATE_RTH_3D_HOVER_PRIOR_TO_LANDING,
[NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING] = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
}
},
[NAV_STATE_RTH_3D_CLIMB_TO_SAFE_ALT] = {
.onEntry = navOnEnteringState_NAV_STATE_RTH_3D_CLIMB_TO_SAFE_ALT,
[NAV_STATE_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
.mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_RTH_ENROUTE,
.mwError = MW_NAV_ERROR_WAIT_FOR_RTH_ALT,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_3D_CLIMB_TO_SAFE_ALT, // re-process the state
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_3D_HEAD_HOME,
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_CLIMB_TO_SAFE_ALT, // re-process the state
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_HEAD_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_2D] = NAV_STATE_POSHOLD_2D_INITIALIZE,
@ -433,16 +348,16 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
}
},
[NAV_STATE_RTH_3D_HEAD_HOME] = {
.onEntry = navOnEnteringState_NAV_STATE_RTH_3D_HEAD_HOME,
[NAV_STATE_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,
.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_3D_HEAD_HOME, // re-process the state
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_3D_HOVER_PRIOR_TO_LANDING,
[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_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_2D] = NAV_STATE_POSHOLD_2D_INITIALIZE,
@ -451,16 +366,16 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
}
},
[NAV_STATE_RTH_3D_HOVER_PRIOR_TO_LANDING] = {
.onEntry = navOnEnteringState_NAV_STATE_RTH_3D_HOVER_PRIOR_TO_LANDING,
[NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING] = {
.onEntry = navOnEnteringState_NAV_STATE_RTH_HOVER_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,
.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_3D_HOVER_PRIOR_TO_LANDING,
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_3D_LANDING,
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING,
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_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_2D] = NAV_STATE_POSHOLD_2D_INITIALIZE,
@ -469,16 +384,16 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
}
},
[NAV_STATE_RTH_3D_LANDING] = {
.onEntry = navOnEnteringState_NAV_STATE_RTH_3D_LANDING,
[NAV_STATE_RTH_LANDING] = {
.onEntry = navOnEnteringState_NAV_STATE_RTH_LANDING,
.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,
.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_3D_LANDING, // re-process state
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_3D_FINISHING,
[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_2D] = NAV_STATE_POSHOLD_2D_INITIALIZE,
@ -487,28 +402,28 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
}
},
[NAV_STATE_RTH_3D_FINISHING] = {
.onEntry = navOnEnteringState_NAV_STATE_RTH_3D_FINISHING,
[NAV_STATE_RTH_FINISHING] = {
.onEntry = navOnEnteringState_NAV_STATE_RTH_FINISHING,
.timeoutMs = 0,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | 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,
.onEvent = {
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_3D_FINISHED,
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_FINISHED,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
}
},
[NAV_STATE_RTH_3D_FINISHED] = {
.onEntry = navOnEnteringState_NAV_STATE_RTH_3D_FINISHED,
[NAV_STATE_RTH_FINISHED] = {
.onEntry = navOnEnteringState_NAV_STATE_RTH_FINISHED,
.timeoutMs = 10,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH,
.mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_LANDED,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_3D_FINISHED, // re-process state
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_FINISHED, // re-process state
[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_2D] = NAV_STATE_POSHOLD_2D_INITIALIZE,
@ -870,102 +785,55 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS(
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigationFSMState_t previousState)
{
if (STATE(GPS_FIX_HOME)) {
if (posControl.flags.hasValidHeadingSensor) {
navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
if ((prevFlags & NAV_CTL_POS) == 0) {
resetPositionController();
}
if ((prevFlags & NAV_CTL_ALT) == 0) {
resetAltitudeController();
setupAltitudeController();
}
// Switch between 2D and 3D RTH depending on altitude sensor availability
if (posControl.flags.hasValidAltitudeSensor) {
// We might have GPS unavailable - in case of 3D RTH set current altitude target
setDesiredPosition(&posControl.actualState.pos, 0, NAV_POS_UPDATE_Z);
return NAV_FSM_EVENT_SWITCH_TO_RTH_3D;
}
else {
return NAV_FSM_EVENT_SWITCH_TO_RTH_2D;
}
}
/* Position sensor failure timeout - land */
else if (checkForPositionSensorTimeout()) {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
/* No valid POS sensor but still within valid timeout - wait */
else {
return NAV_FSM_EVENT_NONE;
}
}
else {
// No home position set or no heading sensor
if (!posControl.flags.hasValidHeadingSensor || !STATE(GPS_FIX_HOME)) {
// Heading sensor and HOME fix are mandatory for RTH. If not satisfied - switch to emergency landing
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_INITIALIZE(navigationFSMState_t previousState)
{
UNUSED(previousState);
if (STATE(FIXED_WING) && (posControl.homeDistance < navConfig()->general.min_rth_distance)) {
// Prevent RTH from activating on airplanes if too close to home
if (STATE(FIXED_WING) && (posControl.homeDistance < navConfig()->general.min_rth_distance) && !posControl.flags.forcedRTHActivated) {
// Prevent RTH from activating on airplanes if too close to home unless it's a failsafe RTH
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
}
else {
if (posControl.flags.hasValidPositionSensor) {
// If close to home - reset home position
if (posControl.homeDistance < navConfig()->general.min_rth_distance) {
setHomePosition(&posControl.actualState.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING);
}
// Initialize RTH sanity check to prevent fly-aways on RTH
initializeRTHSanityChecker(&posControl.actualState.pos);
return NAV_FSM_EVENT_SUCCESS;
}
/* Position sensor failure timeout - land */
else if (checkForPositionSensorTimeout()) {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
/* No valid POS sensor but still within valid timeout - wait */
else {
return NAV_FSM_EVENT_NONE;
}
// Reset altitude and position controllers if necessary
if ((prevFlags & NAV_CTL_POS) == 0) {
resetPositionController();
}
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_HEAD_HOME(navigationFSMState_t previousState)
{
UNUSED(previousState);
if (!posControl.flags.hasValidHeadingSensor) {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
if ((prevFlags & NAV_CTL_ALT) == 0) {
resetAltitudeController();
setupAltitudeController();
}
else if (posControl.flags.hasValidPositionSensor) {
if (isWaypointReached(&posControl.homeWaypointAbove, true)) {
// Successfully reached position target
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_2D_FINISHING
}
else if (!validateRTHSanityChecker()) {
// Sanity check of RTH
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
// If we have valid position sensor or configured to ignore it's loss at initial stage - continue
if (posControl.flags.hasValidPositionSensor || navConfig()->general.flags.rth_climb_ignore_emerg) {
// If close to home - reset home position and land
if (posControl.homeDistance < navConfig()->general.min_rth_distance) {
setHomePosition(&posControl.actualState.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING);
setDesiredPosition(&posControl.actualState.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
}
else {
// Update XY-position target
if (navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING)) {
setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
}
else {
setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING);
t_fp_vector targetHoldPos;
if (STATE(FIXED_WING)) {
// Airplane - climbout before turning around
calculateFarAwayTarget(&targetHoldPos, posControl.actualState.yaw, 100000.0f); // 1km away
} else {
// Multicopter, hover and climb
calculateInitialHoldPosition(&targetHoldPos);
// Initialize RTH sanity check to prevent fly-aways on RTH
// For airplanes this is delayed until climb-out is finished
initializeRTHSanityChecker(&targetHoldPos);
}
return NAV_FSM_EVENT_NONE;
setDesiredPosition(&targetHoldPos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING);
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_CLIMB_TO_SAFE_ALT
}
}
/* Position sensor failure timeout - land */
@ -978,71 +846,14 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_HEAD_HOME(naviga
}
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_FINISHING(navigationFSMState_t previousState)
{
UNUSED(previousState);
setDesiredPosition(&posControl.homeWaypointAbove.pos, posControl.homeWaypointAbove.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING);
return NAV_FSM_EVENT_SUCCESS;
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_FINISHED(navigationFSMState_t previousState)
{
// Same logic as PH_2D
return navOnEnteringState_NAV_STATE_POSHOLD_2D_IN_PROGRESS(previousState);
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_INITIALIZE(navigationFSMState_t previousState)
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(navigationFSMState_t previousState)
{
UNUSED(previousState);
if (STATE(FIXED_WING) && (posControl.homeDistance < navConfig()->general.min_rth_distance)) {
// Prevent RTH from activating on airplanes if too close to home
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
if (!posControl.flags.hasValidHeadingSensor) {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
else {
if (posControl.flags.hasValidPositionSensor || navConfig()->general.flags.rth_climb_ignore_emerg) {
// If close to home - reset home position and land
if (posControl.homeDistance < navConfig()->general.min_rth_distance) {
setHomePosition(&posControl.actualState.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING);
setDesiredPosition(&posControl.actualState.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
return NAV_FSM_EVENT_SWITCH_TO_RTH_3D_LANDING; // NAV_STATE_RTH_3D_HOVER_PRIOR_TO_LANDING
}
else {
t_fp_vector targetHoldPos;
if (STATE(FIXED_WING)) {
// Airplane - climbout before turning around
calculateFarAwayTarget(&targetHoldPos, posControl.actualState.yaw, 100000.0f); // 1km away
} else {
// Multicopter, hover and climb
calculateInitialHoldPosition(&targetHoldPos);
// Initialize RTH sanity check to prevent fly-aways on RTH
// For airplanes this is delayed until climb-out is finished
initializeRTHSanityChecker(&targetHoldPos);
}
setDesiredPosition(&targetHoldPos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING);
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_3D_CLIMB_TO_SAFE_ALT
}
}
/* Position sensor failure timeout - land */
else if (checkForPositionSensorTimeout()) {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
/* No valid POS sensor but still within valid timeout - wait */
else {
return NAV_FSM_EVENT_NONE;
}
}
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_CLIMB_TO_SAFE_ALT(navigationFSMState_t previousState)
{
UNUSED(previousState);
// If we have valid pos sensor OR we are configured to ignore GPS loss
if (posControl.flags.hasValidPositionSensor || !checkForPositionSensorTimeout() || navConfig()->general.flags.rth_climb_ignore_emerg) {
if (((posControl.actualState.pos.V.Z - posControl.homeWaypointAbove.pos.V.Z) > -50.0f) || (!navConfig()->general.flags.rth_climb_first)) {
@ -1051,7 +862,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_CLIMB_TO_SAFE_AL
initializeRTHSanityChecker(&posControl.actualState.pos);
}
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_3D_HEAD_HOME
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_HEAD_HOME
}
else {
/* For multi-rotors execute sanity check during initial ascent as well */
@ -1083,18 +894,20 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_CLIMB_TO_SAFE_AL
}
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_HEAD_HOME(navigationFSMState_t previousState)
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigationFSMState_t previousState)
{
UNUSED(previousState);
if (!posControl.flags.hasValidHeadingSensor) {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
else if (posControl.flags.hasValidPositionSensor) {
// If we have position sensor - continue home
if (posControl.flags.hasValidPositionSensor) {
if (isWaypointReached(&posControl.homeWaypointAbove, true)) {
// Successfully reached position target - update XYZ-position
setDesiredPosition(&posControl.homeWaypointAbove.pos, posControl.homeWaypointAbove.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_3D_HOVER_PRIOR_TO_LANDING
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING
}
else if (!validateRTHSanityChecker()) {
// Sanity check of RTH
@ -1121,37 +934,42 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_HEAD_HOME(naviga
}
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_HOVER_PRIOR_TO_LANDING(navigationFSMState_t previousState)
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING(navigationFSMState_t previousState)
{
UNUSED(previousState);
// If no position sensor available - land immediately
if (!(posControl.flags.hasValidPositionSensor && posControl.flags.hasValidHeadingSensor) && checkForPositionSensorTimeout()) {
if (!posControl.flags.hasValidHeadingSensor) {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
// Wait until target heading is reached (with 15 deg margin for error)
if (STATE(FIXED_WING)) {
resetLandingDetector();
return NAV_FSM_EVENT_SUCCESS;
}
else {
if (ABS(wrap_18000(posControl.homeWaypointAbove.yaw - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) {
// If position ok OR within valid timeout - continue
if (posControl.flags.hasValidPositionSensor || !checkForPositionSensorTimeout()) {
// Wait until target heading is reached (with 15 deg margin for error)
if (STATE(FIXED_WING)) {
resetLandingDetector();
return NAV_FSM_EVENT_SUCCESS;
}
else if (!validateRTHSanityChecker()) {
// Continue to check for RTH sanity during pre-landing hover
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
else {
setDesiredPosition(&posControl.homeWaypointAbove.pos, posControl.homeWaypointAbove.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
return NAV_FSM_EVENT_NONE;
if (ABS(wrap_18000(posControl.homeWaypointAbove.yaw - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) {
resetLandingDetector();
return NAV_FSM_EVENT_SUCCESS;
}
else if (!validateRTHSanityChecker()) {
// Continue to check for RTH sanity during pre-landing hover
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
else {
setDesiredPosition(&posControl.homeWaypointAbove.pos, posControl.homeWaypointAbove.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
return NAV_FSM_EVENT_NONE;
}
}
}
else {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_LANDING(navigationFSMState_t previousState)
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationFSMState_t previousState)
{
UNUSED(previousState);
@ -1163,7 +981,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_LANDING(navigati
}
else {
if (!validateRTHSanityChecker()) {
// Continue to check for RTH sanity during pre-landing hover
// Continue to check for RTH sanity during landing
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
@ -1192,7 +1010,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_LANDING(navigati
}
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_FINISHING(navigationFSMState_t previousState)
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHING(navigationFSMState_t previousState)
{
UNUSED(previousState);
@ -1203,7 +1021,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_FINISHING(naviga
return NAV_FSM_EVENT_SUCCESS;
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_FINISHED(navigationFSMState_t previousState)
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigationFSMState_t previousState)
{
// Stay in this state
UNUSED(previousState);
@ -1302,10 +1120,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navig
{
UNUSED(previousState);
const navigationFSMEvent_t landEvent = navOnEnteringState_NAV_STATE_RTH_3D_LANDING(previousState);
const navigationFSMEvent_t landEvent = navOnEnteringState_NAV_STATE_RTH_LANDING(previousState);
if (landEvent == NAV_FSM_EVENT_SUCCESS) {
// Landing controller returned success - invoke RTH finishing state and finish the waypoint
navOnEnteringState_NAV_STATE_RTH_3D_FINISHING(previousState);
navOnEnteringState_NAV_STATE_RTH_FINISHING(previousState);
return NAV_FSM_EVENT_SUCCESS;
}
else {
@ -2355,19 +2173,24 @@ bool isApproachingLastWaypoint(void)
float getActiveWaypointSpeed(void)
{
uint16_t waypointSpeed = navConfig()->general.max_speed;
if (posControl.flags.isAdjustingPosition) {
// In manual control mode use different cap for speed
return navConfig()->general.max_manual_speed;
}
else {
uint16_t waypointSpeed = navConfig()->general.max_auto_speed;
if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP) {
if (posControl.waypointCount > 0 && posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_WAYPOINT) {
waypointSpeed = posControl.waypointList[posControl.activeWaypointIndex].p1;
if (waypointSpeed < 50 || waypointSpeed > navConfig()->general.max_speed) {
waypointSpeed = navConfig()->general.max_speed;
if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP) {
if (posControl.waypointCount > 0 && posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_WAYPOINT) {
const float wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p1;
if (wpSpecificSpeed >= 50.0f && wpSpecificSpeed <= navConfig()->general.max_auto_speed) {
waypointSpeed = wpSpecificSpeed;
}
}
}
}
return waypointSpeed;
return waypointSpeed;
}
}
/*-----------------------------------------------------------
@ -2809,7 +2632,7 @@ rthState_e getStateOfForcedRTH(void)
{
/* If forced RTH activated and in AUTO_RTH or EMERG state */
if (posControl.flags.forcedRTHActivated && (navGetStateFlags(posControl.navState) & (NAV_AUTO_RTH | NAV_CTL_EMERG))) {
if (posControl.navState == NAV_STATE_RTH_2D_FINISHED || posControl.navState == NAV_STATE_RTH_3D_FINISHED || posControl.navState == NAV_STATE_EMERGENCY_LANDING_FINISHED) {
if (posControl.navState == NAV_STATE_RTH_FINISHED || posControl.navState == NAV_STATE_EMERGENCY_LANDING_FINISHED) {
return RTH_HAS_LANDED;
}
else {