Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-20 06:45:14 +03:00
Code Issues Wiki Activity

Merge pull request #1401 from iNavFlight/nav-cleanups-fixes

Browse source 
Speed control and RTH/FAILSAFE improvements
This commit is contained in:
Konstantin Sharlaimov 2017-03-30 18:38:44 +10:00 • committed by GitHub
commit 44cfad464b
8 changed files with 169 additions and 345 deletions
Download patch file Download diff file Expand all files Collapse all files

4
docs/Cli.md
View file

@ -160,8 +160,8 @@ Re-apply any new defaults as desired.
| nav_position_timeout | 5 | If GPS fails wait for this much seconds before switching to emergency landing mode (0 - disable) |
| nav_wp_radius | 100 | Waypoint radius [cm]. Waypoint would be considered reached if machine is within this radius |
| nav_wp_safe_distance | 10000 | First waypoint in the mission should be closer than this value (cm) |
| nav_max_speed | 300 | Maximum velocity firmware is allowed in full auto modes (POSHOLD, RTH, WP) [cm/s] [Multirotor only] |
| nav_max_climb_rate | 500 | Maximum climb/descent rate that UAV is allowed to reach during navigation modes. In cm/s |
| nav_auto_speed | 300 | Maximum velocity firmware is allowed in full auto modes (POSHOLD, RTH, WP) [cm/s] [Multirotor only] |
| nav_auto_climb_rate | 500 | Maximum climb/descent rate that UAV is allowed to reach during navigation modes. In cm/s |
| nav_manual_speed | 500 | Maximum velocity firmware is allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only] |
| nav_manual_climb_rate | 200 | Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s] |
| nav_landing_speed | 200 | Vertical descent velocity during the RTH landing phase. [cm/s] |

4
src/main/fc/cli.c
View file

@ -815,8 +815,8 @@ static const clivalue_t valueTable[] = {
{ "nav_position_timeout", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 10 }, PG_NAV_CONFIG, offsetof(navConfig_t, general.pos_failure_timeout) },
{ "nav_wp_radius", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 10, 10000 }, PG_NAV_CONFIG, offsetof(navConfig_t, general.waypoint_radius) },
{ "nav_wp_safe_distance", VAR_UINT16 | MASTER_VALUE | MODE_MAX, .config.max = { 65000 }, PG_NAV_CONFIG, offsetof(navConfig_t, general.waypoint_safe_distance) },
{ "nav_max_speed", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 10, 2000 }, PG_NAV_CONFIG, offsetof(navConfig_t, general.max_speed) },
{ "nav_max_climb_rate", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 10, 2000 }, PG_NAV_CONFIG, offsetof(navConfig_t, general.max_climb_rate) },
{ "nav_auto_speed", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 10, 2000 }, PG_NAV_CONFIG, offsetof(navConfig_t, general.max_auto_speed) },
{ "nav_auto_climb_rate", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 10, 2000 }, PG_NAV_CONFIG, offsetof(navConfig_t, general.max_auto_climb_rate) },
{ "nav_manual_speed", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 10, 2000 }, PG_NAV_CONFIG, offsetof(navConfig_t, general.max_manual_speed) },
{ "nav_manual_climb_rate", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 10, 2000 }, PG_NAV_CONFIG, offsetof(navConfig_t, general.max_manual_climb_rate ) },
{ "nav_landing_speed", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 100, 2000 }, PG_NAV_CONFIG, offsetof(navConfig_t, general.land_descent_rate) },

8
src/main/fc/fc_msp.c
View file

@ -1159,8 +1159,8 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
#ifdef NAV
case MSP_NAV_POSHOLD:
sbufWriteU8(dst, navConfig()->general.flags.user_control_mode);
sbufWriteU16(dst, navConfig()->general.max_speed);
sbufWriteU16(dst, navConfig()->general.max_climb_rate);
sbufWriteU16(dst, navConfig()->general.max_auto_speed);
sbufWriteU16(dst, navConfig()->general.max_auto_climb_rate);
sbufWriteU16(dst, navConfig()->general.max_manual_speed);
sbufWriteU16(dst, navConfig()->general.max_manual_climb_rate);
sbufWriteU8(dst, navConfig()->mc.max_bank_angle);
@ -1631,8 +1631,8 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
#ifdef NAV
case MSP_SET_NAV_POSHOLD:
navConfigMutable()->general.flags.user_control_mode = sbufReadU8(src);
navConfigMutable()->general.max_speed = sbufReadU16(src);
navConfigMutable()->general.max_climb_rate = sbufReadU16(src);
navConfigMutable()->general.max_auto_speed = sbufReadU16(src);
navConfigMutable()->general.max_auto_climb_rate = sbufReadU16(src);
navConfigMutable()->general.max_manual_speed = sbufReadU16(src);
navConfigMutable()->general.max_manual_climb_rate = sbufReadU16(src);
navConfigMutable()->mc.max_bank_angle = sbufReadU8(src);

17
src/main/flight/failsafe.c
View file

@ -418,19 +418,24 @@ void failsafeUpdateState(void)
if (armed) {
beeperMode = BEEPER_RX_LOST_LANDING;
}
bool rthIdleOrLanded;
bool rthLanded = false;
switch (getStateOfForcedRTH()) {
case RTH_IN_PROGRESS:
rthIdleOrLanded = false;
break;
default:
case RTH_IDLE:
case RTH_HAS_LANDED:
rthIdleOrLanded = true;
rthLanded = true;
break;
case RTH_IDLE:
default:
// This shouldn't happen. If RTH was somehow aborted during failsafe - fallback to FAILSAFE_LANDING procedure
abortForcedRTH();
failsafeActivate(FAILSAFE_LANDING);
reprocessState = true;
break;
}
if (rthIdleOrLanded || !armed) {
if (rthLanded || !armed) {
failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_30_SECONDS; // require 30 seconds of valid rxData
failsafeState.phase = FAILSAFE_LANDED;
reprocessState = true;

349
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,10 +785,21 @@ 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);
if (!posControl.flags.hasValidHeadingSensor || !posControl.flags.hasValidAltitudeSensor || !STATE(GPS_FIX_HOME)) {
// Heading sensor, altitude sensor and HOME fix are mandatory for RTH. If not satisfied - switch to emergency landing
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
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;
}
// 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) {
// Reset altitude and position controllers if necessary
if ((prevFlags & NAV_CTL_POS) == 0) {
resetPositionController();
}
@ -883,130 +809,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
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
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
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;
}
}
}
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;
}
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;
}
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);
}
return NAV_FSM_EVENT_NONE;
}
}
/* 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_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)
{
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;
}
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
return NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING; // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING
}
else {
t_fp_vector targetHoldPos;
@ -1025,7 +833,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_INITIALIZE(navig
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
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_CLIMB_TO_SAFE_ALT
}
}
/* Position sensor failure timeout - land */
@ -1036,13 +844,16 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_INITIALIZE(navig
else {
return NAV_FSM_EVENT_NONE;
}
}
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_CLIMB_TO_SAFE_ALT(navigationFSMState_t previousState)
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(navigationFSMState_t previousState)
{
UNUSED(previousState);
if (!posControl.flags.hasValidHeadingSensor) {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
// 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,15 +934,16 @@ 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;
}
// 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();
@ -1149,9 +963,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_HOVER_PRIOR_TO_L
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 {
@ -2365,19 +2183,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;
const float wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p1;
if (wpSpecificSpeed >= 50.0f && wpSpecificSpeed <= navConfig()->general.max_auto_speed) {
waypointSpeed = wpSpecificSpeed;
}
}
}
return waypointSpeed;
}
}
/*-----------------------------------------------------------
@ -2532,7 +2355,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
}
// RTH/Failsafe_RTH can override PASSTHRU
if (posControl.flags.forcedRTHActivated || (IS_RC_MODE_ACTIVE(BOXNAVRTH) && canActivatePosHold && STATE(GPS_FIX_HOME))) {
if (posControl.flags.forcedRTHActivated || (IS_RC_MODE_ACTIVE(BOXNAVRTH) && canActivatePosHold && canActivateAltHold && STATE(GPS_FIX_HOME))) {
// If we request forced RTH - attempt to activate it no matter what
// This might switch to emergency landing controller if GPS is unavailable
canActivateWaypoint = false; // Block WP mode if we switched to RTH for whatever reason
@ -2819,7 +2642,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 {

4
src/main/navigation/navigation.h
View file

@ -114,8 +114,8 @@ typedef struct navConfig_s {
uint8_t pos_failure_timeout; // Time to wait before switching to emergency landing (0 - disable)
uint16_t waypoint_radius; // if we are within this distance to a waypoint then we consider it reached (distance is in cm)
uint16_t waypoint_safe_distance; // Waypoint mission sanity check distance
uint16_t max_speed; // autonomous navigation speed cm/sec
uint16_t max_climb_rate; // max vertical speed limitation cm/sec
uint16_t max_auto_speed; // autonomous navigation speed cm/sec
uint16_t max_auto_climb_rate; // max vertical speed limitation cm/sec
uint16_t max_manual_speed; // manual velocity control max horizontal speed
uint16_t max_manual_climb_rate; // manual velocity control max vertical speed
uint16_t land_descent_rate; // normal RTH landing descent rate

7
src/main/navigation/navigation_multicopter.c
View file

@ -86,7 +86,12 @@ static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros)
float targetVel = altitudeError * posControl.pids.pos[Z].param.kP;
// hard limit desired target velocity to max_climb_rate
targetVel = constrainf(targetVel, -navConfig()->general.max_climb_rate, navConfig()->general.max_climb_rate);
if (posControl.flags.isAdjustingAltitude) {
targetVel = constrainf(targetVel, -navConfig()->general.max_manual_climb_rate, navConfig()->general.max_manual_climb_rate);
}
else {
targetVel = constrainf(targetVel, -navConfig()->general.max_auto_climb_rate, navConfig()->general.max_auto_climb_rate);
}
// limit max vertical acceleration to 1/5G (~200 cm/s/s) if we are increasing velocity.
// if we are decelerating - don't limit (allow better recovery from falling)

51
src/main/navigation/navigation_private.h
View file

@ -154,15 +154,13 @@ typedef enum {
NAV_FSM_EVENT_SWITCH_TO_POSHOLD_2D,
NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D,
NAV_FSM_EVENT_SWITCH_TO_RTH,
NAV_FSM_EVENT_SWITCH_TO_RTH_2D,
NAV_FSM_EVENT_SWITCH_TO_RTH_3D,
NAV_FSM_EVENT_SWITCH_TO_WAYPOINT,
NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING,
NAV_FSM_EVENT_SWITCH_TO_LAUNCH,
NAV_FSM_EVENT_STATE_SPECIFIC_1, // State-specific event
NAV_FSM_EVENT_STATE_SPECIFIC_2, // State-specific event
NAV_FSM_EVENT_SWITCH_TO_RTH_3D_LANDING = NAV_FSM_EVENT_STATE_SPECIFIC_1,
NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING = NAV_FSM_EVENT_STATE_SPECIFIC_1,
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,
@ -184,36 +182,29 @@ typedef enum {
NAV_STATE_POSHOLD_3D_IN_PROGRESS, // 7
NAV_STATE_RTH_INITIALIZE, // 8
NAV_STATE_RTH_CLIMB_TO_SAFE_ALT, // 9
NAV_STATE_RTH_HEAD_HOME, // 10
NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING, // 11
NAV_STATE_RTH_LANDING, // 12
NAV_STATE_RTH_FINISHING, // 13
NAV_STATE_RTH_FINISHED, // 14
NAV_STATE_RTH_2D_INITIALIZE, // 9
NAV_STATE_RTH_2D_HEAD_HOME, // 10
NAV_STATE_RTH_2D_FINISHING, // 11
NAV_STATE_RTH_2D_FINISHED, // 12
NAV_STATE_WAYPOINT_INITIALIZE, // 15
NAV_STATE_WAYPOINT_PRE_ACTION, // 16
NAV_STATE_WAYPOINT_IN_PROGRESS, // 17
NAV_STATE_WAYPOINT_REACHED, // 18
NAV_STATE_WAYPOINT_NEXT, // 19
NAV_STATE_WAYPOINT_FINISHED, // 20
NAV_STATE_WAYPOINT_RTH_LAND, // 21
NAV_STATE_RTH_3D_INITIALIZE, // 13
NAV_STATE_RTH_3D_CLIMB_TO_SAFE_ALT, // 14
NAV_STATE_RTH_3D_HEAD_HOME, // 15
NAV_STATE_RTH_3D_HOVER_PRIOR_TO_LANDING, // 16
NAV_STATE_RTH_3D_LANDING, // 17
NAV_STATE_RTH_3D_FINISHING, // 18
NAV_STATE_RTH_3D_FINISHED, // 19
NAV_STATE_EMERGENCY_LANDING_INITIALIZE, // 22
NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS, // 23
NAV_STATE_EMERGENCY_LANDING_FINISHED, // 24
NAV_STATE_WAYPOINT_INITIALIZE, // 20
NAV_STATE_WAYPOINT_PRE_ACTION, // 21
NAV_STATE_WAYPOINT_IN_PROGRESS, // 22
NAV_STATE_WAYPOINT_REACHED, // 23
NAV_STATE_WAYPOINT_NEXT, // 24
NAV_STATE_WAYPOINT_FINISHED, // 25
NAV_STATE_WAYPOINT_RTH_LAND, // 26
NAV_STATE_EMERGENCY_LANDING_INITIALIZE, // 27
NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS, // 28
NAV_STATE_EMERGENCY_LANDING_FINISHED, // 29
NAV_STATE_LAUNCH_INITIALIZE, // 30
NAV_STATE_LAUNCH_WAIT, // 31
NAV_STATE_LAUNCH_MOTOR_DELAY, // 32
NAV_STATE_LAUNCH_IN_PROGRESS, // 33
NAV_STATE_LAUNCH_INITIALIZE, // 25
NAV_STATE_LAUNCH_WAIT, // 26
NAV_STATE_LAUNCH_MOTOR_DELAY, // 27
NAV_STATE_LAUNCH_IN_PROGRESS, // 28
NAV_STATE_COUNT,
} navigationFSMState_t;