From c15512b4da1720b7028209ebdfd67fab12327755 Mon Sep 17 00:00:00 2001 From: "Konstantin Sharlaimov (DigitalEntity)" Date: Fri, 11 Dec 2015 20:29:02 +1000 Subject: [PATCH] Added a 2 sec timeout before switching into EMERG mode when on a WP mission --- src/main/flight/navigation_rewrite.c | 55 ++++++++++++-------- src/main/flight/navigation_rewrite_private.h | 2 +- 2 files changed, 34 insertions(+), 23 deletions(-) diff --git a/src/main/flight/navigation_rewrite.c b/src/main/flight/navigation_rewrite.c index 16f0bfdd65..9e19d49dc4 100755 --- a/src/main/flight/navigation_rewrite.c +++ b/src/main/flight/navigation_rewrite.c @@ -701,8 +701,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati return NAV_FSM_EVENT_SWITCH_TO_RTH_2D; } } - /* No pos sensor available for RTH_WAIT_FOR_GPS_TIMEOUT_MS - land */ - else if (navGetCurrentStateTime() > RTH_WAIT_FOR_GPS_TIMEOUT_MS) { + /* No pos sensor available for NAV_WAIT_FOR_GPS_TIMEOUT_MS - land */ + else if (navGetCurrentStateTime() > NAV_WAIT_FOR_GPS_TIMEOUT_MS) { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } /* No valid POS sensor but still within valid timeout - wait */ @@ -761,8 +761,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_GPS_FAILING(navi if (posControl.flags.hasValidPositionSensor && STATE(GPS_FIX_HOME)) { return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_2D_HEAD_HOME } - /* No pos sensor available for RTH_WAIT_FOR_GPS_TIMEOUT_MS - land */ - else if (navGetCurrentStateTime() > RTH_WAIT_FOR_GPS_TIMEOUT_MS) { + /* No pos sensor available for NAV_WAIT_FOR_GPS_TIMEOUT_MS - land */ + else if (navGetCurrentStateTime() > NAV_WAIT_FOR_GPS_TIMEOUT_MS) { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } /* No valid POS sensor but still within valid timeout - wait */ @@ -964,23 +964,29 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na UNUSED(previousState); // If no position sensor available - land immediately - if (!posControl.flags.hasValidPositionSensor) { + if (posControl.flags.hasValidPositionSensor) { + switch (posControl.waypointList[posControl.activeWaypointIndex].action) { + case NAV_WP_ACTION_WAYPOINT: + case NAV_WP_ACTION_RTH: + default: + if (isWaypointReached(&posControl.activeWaypoint) || isWaypointMissed(&posControl.activeWaypoint)) { + // Waypoint reached + return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED + } + else { + // Update XY-position target to active waypoint + setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING); + return NAV_FSM_EVENT_NONE; // will re-process state in >10ms + } + break; + } + } + /* No pos sensor available for NAV_WAIT_FOR_GPS_TIMEOUT_MS - land */ + else if (navGetCurrentStateTime() > NAV_WAIT_FOR_GPS_TIMEOUT_MS) { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } - - switch (posControl.waypointList[posControl.activeWaypointIndex].action) { - case NAV_WP_ACTION_WAYPOINT: - case NAV_WP_ACTION_RTH: - default: - if (isWaypointReached(&posControl.activeWaypoint) || isWaypointMissed(&posControl.activeWaypoint)) { - // Waypoint reached - return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED - } - else { - // Update XY-position target to active waypoint - setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING); - return NAV_FSM_EVENT_NONE; // will re-process state in >10ms - } + else { + return NAV_FSM_EVENT_NONE; // will re-process state in >10ms } } @@ -1007,11 +1013,16 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED(navig UNUSED(previousState); // If no position sensor available - land immediately - if (!posControl.flags.hasValidPositionSensor) { + if (posControl.flags.hasValidPositionSensor) { + return NAV_FSM_EVENT_NONE; + } + /* No pos sensor available for NAV_WAIT_FOR_GPS_TIMEOUT_MS - land */ + else if (navGetCurrentStateTime() > NAV_WAIT_FOR_GPS_TIMEOUT_MS) { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } - - return NAV_FSM_EVENT_NONE; + else { + return NAV_FSM_EVENT_NONE; // will re-process state in >10ms + } } static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITIALIZE(navigationFSMState_t previousState) diff --git a/src/main/flight/navigation_rewrite_private.h b/src/main/flight/navigation_rewrite_private.h index 341cf2d8cb..a676205794 100755 --- a/src/main/flight/navigation_rewrite_private.h +++ b/src/main/flight/navigation_rewrite_private.h @@ -22,7 +22,7 @@ #define DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR 1.113195f // MagicEarthNumber from APM #define LAND_DETECTOR_TRIGGER_TIME_MS 2000 // 2 seconds -#define RTH_WAIT_FOR_GPS_TIMEOUT_MS 2000 // GPS wait timeout for RTH +#define NAV_WAIT_FOR_GPS_TIMEOUT_MS 2000 // GPS wait time-out #define MIN_POSITION_UPDATE_RATE_HZ 5 // Minimum position update rate at which XYZ controllers would be applied