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

Added a 2 sec timeout before switching into EMERG mode when on a WP mission

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2015-12-11 20:29:02 +10:00
parent 7b598ae39a
commit c15512b4da
2 changed files with 34 additions and 23 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -701,8 +701,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
return NAV_FSM_EVENT_SWITCH_TO_RTH_2D; return NAV_FSM_EVENT_SWITCH_TO_RTH_2D;
} }
} }
/* No pos sensor available for RTH_WAIT_FOR_GPS_TIMEOUT_MS - land */ /* No pos sensor available for NAV_WAIT_FOR_GPS_TIMEOUT_MS - land */
else if (navGetCurrentStateTime() > RTH_WAIT_FOR_GPS_TIMEOUT_MS) { else if (navGetCurrentStateTime() > NAV_WAIT_FOR_GPS_TIMEOUT_MS) {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
} }
/* No valid POS sensor but still within valid timeout - wait */ /* 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)) { if (posControl.flags.hasValidPositionSensor && STATE(GPS_FIX_HOME)) {
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_2D_HEAD_HOME return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_2D_HEAD_HOME
} }
/* No pos sensor available for RTH_WAIT_FOR_GPS_TIMEOUT_MS - land */ /* No pos sensor available for NAV_WAIT_FOR_GPS_TIMEOUT_MS - land */
else if (navGetCurrentStateTime() > RTH_WAIT_FOR_GPS_TIMEOUT_MS) { else if (navGetCurrentStateTime() > NAV_WAIT_FOR_GPS_TIMEOUT_MS) {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
} }
/* No valid POS sensor but still within valid timeout - wait */ /* 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); UNUSED(previousState);
// If no position sensor available - land immediately // 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; return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
} }
else {
switch (posControl.waypointList[posControl.activeWaypointIndex].action) { return NAV_FSM_EVENT_NONE; // will re-process state in >10ms
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
}
} }
} }
@ -1007,11 +1013,16 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED(navig
UNUSED(previousState); UNUSED(previousState);
// If no position sensor available - land immediately // 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_SWITCH_TO_EMERGENCY_LANDING;
} }
else {
return NAV_FSM_EVENT_NONE; return NAV_FSM_EVENT_NONE; // will re-process state in >10ms
}
} }
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITIALIZE(navigationFSMState_t previousState) static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITIALIZE(navigationFSMState_t previousState)

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

@ -22,7 +22,7 @@
#define DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR 1.113195f // MagicEarthNumber from APM #define DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR 1.113195f // MagicEarthNumber from APM
#define LAND_DETECTOR_TRIGGER_TIME_MS 2000 // 2 seconds #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 #define MIN_POSITION_UPDATE_RATE_HZ 5 // Minimum position update rate at which XYZ controllers would be applied