From b40df56f160fd5ebb4804ceec9b545663c5f11b6 Mon Sep 17 00:00:00 2001 From: "Konstantin Sharlaimov (DigitalEntity)" Date: Mon, 30 Nov 2015 14:37:47 +1000 Subject: [PATCH] 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 --- src/main/flight/navigation_rewrite.c | 112 ++++++++++++++++--- src/main/flight/navigation_rewrite_private.h | 42 ++++--- 2 files changed, 118 insertions(+), 36 deletions(-) diff --git a/src/main/flight/navigation_rewrite.c b/src/main/flight/navigation_rewrite.c index ce2e80c7f1..65f9da6954 100755 --- a/src/main/flight/navigation_rewrite.c +++ b/src/main/flight/navigation_rewrite.c @@ -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; diff --git a/src/main/flight/navigation_rewrite_private.h b/src/main/flight/navigation_rewrite_private.h index 9e30724255..9e8f5b7db8 100755 --- a/src/main/flight/navigation_rewrite_private.h +++ b/src/main/flight/navigation_rewrite_private.h @@ -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;