Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 00:35:34 +03:00
Code Issues Wiki Activity

Added GPS failure handling during RTH. Wait for 2 sec for GPS to get online again, if this does not happen, switch to emergency landing

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2015-11-30 14:37:47 +10:00
parent 43de844aca
commit b40df56f16
2 changed files with 118 additions and 36 deletions
Download patch file Download diff file Expand all files Collapse all files

112
src/main/flight/navigation_rewrite.c
View file

@ -87,11 +87,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS(
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_GPS_FAILING(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_GPS_FAILING(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);
@ -211,10 +213,11 @@ static navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
/** RTH mode entry point ************************************************/
[NAV_STATE_RTH_INITIALIZE] = {
.onEntry = navOnEnteringState_NAV_STATE_RTH_INITIALIZE,
.timeoutMs = 0,
.timeoutMs = 10,
.stateFlags = NAV_REQUIRE_ANGLE | NAV_AUTO_RTH,
.mapToFlightModes = NAV_RTH_MODE,
.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,
@ -242,11 +245,24 @@ static navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
.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_ERROR] = NAV_STATE_RTH_2D_GPS_FAILING,
[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_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
}
},
[NAV_STATE_RTH_2D_GPS_FAILING] = {
.onEntry = navOnEnteringState_NAV_STATE_RTH_2D_GPS_FAILING,
.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,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_2D_GPS_FAILING, // re-process the state
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_2D_HEAD_HOME,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
}
},
@ -297,12 +313,12 @@ static navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
.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_ERROR] = NAV_STATE_RTH_3D_GPS_FAILING,
[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_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
}
},
@ -314,11 +330,24 @@ static navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
.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_ERROR] = NAV_STATE_RTH_3D_GPS_FAILING,
[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_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
}
},
[NAV_STATE_RTH_3D_GPS_FAILING] = {
.onEntry = navOnEnteringState_NAV_STATE_RTH_3D_GPS_FAILING,
.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,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_3D_GPS_FAILING, // re-process the state
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_3D_HEAD_HOME,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
}
},
@ -493,6 +522,11 @@ static flightModeFlags_e navGetMappedFlightModes(navigationFSMState_t state)
return navFSM[state].mapToFlightModes;
}
static uint32_t navGetCurrentStateTime(void)
{
return millis() - posControl.navStateActivationTimeMs;
}
/*************************************************************************************************/
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_IDLE(navigationFSMState_t previousState)
{
@ -576,6 +610,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS(
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigationFSMState_t previousState)
{
UNUSED(previousState);
/* All good for RTH */
if (posControl.flags.hasValidPositionSensor && STATE(GPS_FIX_HOME)) {
// Switch between 2D and 3D RTH depending on altitude sensor availability
if (posControl.flags.hasValidAltitudeSensor) {
@ -585,10 +620,14 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
return NAV_FSM_EVENT_SWITCH_TO_RTH_2D;
}
}
else {
// No pos sensor available - land
/* No pos sensor available for RTH_WAIT_FOR_GPS_TIMEOUT_MS - land */
else if (navGetCurrentStateTime() > RTH_WAIT_FOR_GPS_TIMEOUT_MS) {
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_INITIALIZE(navigationFSMState_t previousState)
@ -607,9 +646,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_HEAD_HOME(naviga
{
UNUSED(previousState);
// If no position sensor available - land immediately
// If no position sensor available - switch to NAV_STATE_RTH_2D_GPS_FAILING
if (!posControl.flags.hasValidPositionSensor) {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
return NAV_FSM_EVENT_ERROR;
}
if (isWaypointReached(&posControl.homeWaypointAbove)) {
@ -623,6 +662,24 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_HEAD_HOME(naviga
}
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_GPS_FAILING(navigationFSMState_t previousState)
{
UNUSED(previousState);
/* Wait for GPS to be online again */
if (posControl.flags.hasValidPositionSensor && STATE(GPS_FIX_HOME)) {
return NAV_FSM_EVENT_SUCCESS;
}
/* No pos sensor available for RTH_WAIT_FOR_GPS_TIMEOUT_MS - land */
else if (navGetCurrentStateTime() > RTH_WAIT_FOR_GPS_TIMEOUT_MS) {
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);
@ -695,6 +752,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_HEAD_HOME(naviga
}
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_GPS_FAILING(navigationFSMState_t previousState)
{
/* Same logic as for 2D GPS RTH */
return navOnEnteringState_NAV_STATE_RTH_2D_GPS_FAILING(previousState);
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_HOVER_PRIOR_TO_LANDING(navigationFSMState_t previousState)
{
UNUSED(previousState);
@ -843,55 +906,65 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINIS
return NAV_FSM_EVENT_SUCCESS;
}
static navigationFSMState_t navSetNewFSMState(navigationFSMState_t newState, uint32_t currentMillis)
{
navigationFSMState_t previousState;
previousState = posControl.navState;
posControl.navState = newState;
if (previousState != newState) {
posControl.navStateActivationTimeMs = currentMillis;
}
return previousState;
}
static void navProcessFSMEvents(navigationFSMEvent_t injectedEvent)
{
uint32_t currentMillis = millis();
navigationFSMState_t previousState;
static uint32_t lastStateSwitchTime = 0;
static uint32_t lastStateProcessTime = 0;
/* If timeout event defined and timeout reached - switch state */
if ((navFSM[posControl.navState].timeoutMs > 0) && (navFSM[posControl.navState].onEvent[NAV_FSM_EVENT_TIMEOUT] != NAV_STATE_UNDEFINED) &&
((currentMillis - lastStateSwitchTime) >= navFSM[posControl.navState].timeoutMs)) {
((currentMillis - lastStateProcessTime) >= navFSM[posControl.navState].timeoutMs)) {
/* Update state */
previousState = posControl.navState;
posControl.navState = navFSM[posControl.navState].onEvent[NAV_FSM_EVENT_TIMEOUT];
previousState = navSetNewFSMState(navFSM[posControl.navState].onEvent[NAV_FSM_EVENT_TIMEOUT], currentMillis);
/* Call new state's entry function */
while (navFSM[posControl.navState].onEntry) {
navigationFSMEvent_t newEvent = navFSM[posControl.navState].onEntry(previousState);
if ((newEvent != NAV_FSM_EVENT_NONE) && (navFSM[posControl.navState].onEvent[newEvent] != NAV_STATE_UNDEFINED)) {
previousState = posControl.navState;
posControl.navState = navFSM[posControl.navState].onEvent[newEvent];
previousState = navSetNewFSMState(navFSM[posControl.navState].onEvent[newEvent], currentMillis);
}
else {
break;
}
}
lastStateSwitchTime = currentMillis;
lastStateProcessTime = currentMillis;
}
/* Inject new event */
if (injectedEvent != NAV_FSM_EVENT_NONE && navFSM[posControl.navState].onEvent[injectedEvent] != NAV_STATE_UNDEFINED) {
/* Update state */
previousState = posControl.navState;
posControl.navState = navFSM[posControl.navState].onEvent[injectedEvent];
previousState = navSetNewFSMState(navFSM[posControl.navState].onEvent[injectedEvent], currentMillis);
/* Call new state's entry function */
while (navFSM[posControl.navState].onEntry) {
navigationFSMEvent_t newEvent = navFSM[posControl.navState].onEntry(previousState);
if ((newEvent != NAV_FSM_EVENT_NONE) && (navFSM[posControl.navState].onEvent[newEvent] != NAV_STATE_UNDEFINED)) {
previousState = posControl.navState;
posControl.navState = navFSM[posControl.navState].onEvent[newEvent];
previousState = navSetNewFSMState(navFSM[posControl.navState].onEvent[newEvent], currentMillis);
}
else {
break;
}
}
lastStateSwitchTime = currentMillis;
lastStateProcessTime = currentMillis;
}
}
@ -1801,6 +1874,7 @@ void updateWaypointsAndNavigationMode(bool isRXDataNew)
}
debug[0] = posControl.navState;
debug[1] = navGetCurrentStateTime();
#if defined(NAV_BLACKBOX)
navCurrentState = (int16_t)posControl.navState;
#endif
@ -1869,6 +1943,8 @@ void navigationInit(navConfig_t *initialnavConfig,
{
/* Initial state */
posControl.navState = NAV_STATE_IDLE;
posControl.navStateActivationTimeMs = millis();
posControl.flags.horizontalPositionNewData = 0;
posControl.flags.verticalPositionNewData = 0;
posControl.flags.surfaceDistanceNewData = 0;

42
src/main/flight/navigation_rewrite_private.h
View file

@ -21,8 +21,10 @@
#define DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR 1.113195f // MagicEarthNumber from APM
#define LAND_DETECTOR_TRIGGER_TIME_MS 2000 // 2 seconds
#define NAV_ROLL_PITCH_MAX_DECIDEGREES (30 * 10) // Max control input from NAV (30 deg)
#define LAND_DETECTOR_TRIGGER_TIME_MS 2000 // 2 seconds
#define NAV_ROLL_PITCH_MAX_DECIDEGREES (30 * 10) // Max control input from NAV (30 deg)
#define RTH_WAIT_FOR_GPS_TIMEOUT_MS 2000 // GPS wait timeout for RTH
#define POSITION_TARGET_UPDATE_RATE_HZ 5 // Rate manual position target update (minumum possible speed in cms will be this value)
#define MIN_POSITION_UPDATE_RATE_HZ 5 // Minimum position update rate at which XYZ controllers would be applied
@ -158,25 +160,27 @@ typedef enum {
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_RTH_2D_GPS_FAILING, // 11
NAV_STATE_RTH_2D_FINISHING, // 12
NAV_STATE_RTH_2D_FINISHED, // 13
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_RTH_3D_INITIALIZE, // 14
NAV_STATE_RTH_3D_CLIMB_TO_SAFE_ALT, // 15
NAV_STATE_RTH_3D_HEAD_HOME, // 16
NAV_STATE_RTH_3D_GPS_FAILING, // 17
NAV_STATE_RTH_3D_HOVER_PRIOR_TO_LANDING, // 18
NAV_STATE_RTH_3D_LANDING, // 19
NAV_STATE_RTH_3D_FINISHING, // 20
NAV_STATE_RTH_3D_FINISHED, // 21
NAV_STATE_WAYPOINT_INITIALIZE, // 20
NAV_STATE_WAYPOINT_IN_PROGRESS, // 21
NAV_STATE_WAYPOINT_REACHED, // 22
NAV_STATE_WAYPOINT_FINISHED, // 23
NAV_STATE_WAYPOINT_INITIALIZE, // 22
NAV_STATE_WAYPOINT_IN_PROGRESS, // 23
NAV_STATE_WAYPOINT_REACHED, // 24
NAV_STATE_WAYPOINT_FINISHED, // 25
NAV_STATE_EMERGENCY_LANDING_INITIALIZE, // 24
NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS, // 25
NAV_STATE_EMERGENCY_LANDING_FINISHED, // 26
NAV_STATE_EMERGENCY_LANDING_INITIALIZE, // 26
NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS, // 27
NAV_STATE_EMERGENCY_LANDING_FINISHED, // 28
NAV_STATE_COUNT,
} navigationFSMState_t;
@ -214,6 +218,8 @@ typedef struct {
typedef struct {
/* Flags and navigation system state */
navigationFSMState_t navState;
uint32_t navStateActivationTimeMs;
bool enabled;
navigationFlags_t flags;