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

Merge branch 'rth-safety'

Browse source 
Fixes #129
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2016-04-02 17:00:01 +10:00
commit 1c20cdad75
2 changed files with 58 additions and 40 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -336,6 +336,7 @@ static navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_3D_INITIALIZE, // re-process the state [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_SUCCESS] = NAV_STATE_RTH_3D_CLIMB_TO_SAFE_ALT,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, [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_IDLE] = NAV_STATE_IDLE, [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
} }
}, },
@ -731,22 +732,29 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_INITIALIZE(navigationFSMState_t previousState) static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_INITIALIZE(navigationFSMState_t previousState)
{ {
/* All good for RTH */ UNUSED(previousState);
if (posControl.flags.hasValidPositionSensor) {
// If close to home - reset home position
if (posControl.homeDistance < posControl.navConfig->min_rth_distance) {
setHomePosition(&posControl.actualState.pos, posControl.actualState.yaw);
}
return NAV_FSM_EVENT_SUCCESS; if (STATE(FIXED_WING) && (posControl.homeDistance < posControl.navConfig->min_rth_distance)) {
// Prevent RTH from activating on airplanes if too close to home
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
} }
/* 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 { else {
return NAV_FSM_EVENT_NONE; if (posControl.flags.hasValidPositionSensor) {
// If close to home - reset home position
if (posControl.homeDistance < posControl.navConfig->min_rth_distance) {
setHomePosition(&posControl.actualState.pos, posControl.actualState.yaw);
}
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;
}
} }
} }
@ -809,36 +817,45 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_FINISHED(navigat
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_INITIALIZE(navigationFSMState_t previousState) static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_INITIALIZE(navigationFSMState_t previousState)
{ {
/* All good for RTH */ UNUSED(previousState);
if (posControl.flags.hasValidPositionSensor) {
t_fp_vector targetHoldPos;
// If close to home - reset home position if (STATE(FIXED_WING) && (posControl.homeDistance < posControl.navConfig->min_rth_distance)) {
if (posControl.homeDistance < posControl.navConfig->min_rth_distance) { // Prevent RTH from activating on airplanes if too close to home
setHomePosition(&posControl.actualState.pos, posControl.actualState.yaw); return NAV_FSM_EVENT_SWITCH_TO_IDLE;
targetHoldPos = posControl.actualState.pos; }
} else {
else { if (posControl.flags.hasValidPositionSensor) {
if (!STATE(FIXED_WING)) { // If close to home - reset home position and land
// Multicopter, hover and climb if (posControl.homeDistance < posControl.navConfig->min_rth_distance) {
calculateInitialHoldPosition(&targetHoldPos); setHomePosition(&posControl.actualState.pos, posControl.actualState.yaw);
} else { setDesiredPosition(&posControl.actualState.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z);
// Airplane - climbout before turning around
calculateFarAwayTarget(&targetHoldPos, posControl.actualState.yaw, 100000.0f); // 1km away return NAV_FSM_EVENT_SWITCH_TO_RTH_3D_LANDING; // NAV_STATE_RTH_3D_HOVER_PRIOR_TO_LANDING
}
else {
t_fp_vector targetHoldPos;
if (!STATE(FIXED_WING)) {
// Multicopter, hover and climb
calculateInitialHoldPosition(&targetHoldPos);
} else {
// Airplane - climbout before turning around
calculateFarAwayTarget(&targetHoldPos, posControl.actualState.yaw, 100000.0f); // 1km away
}
setDesiredPosition(&targetHoldPos, 0, NAV_POS_UPDATE_XY);
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_3D_CLIMB_TO_SAFE_ALT
} }
} }
/* Position sensor failure timeout - land */
setDesiredPosition(&targetHoldPos, 0, NAV_POS_UPDATE_XY); else if (checkForPositionSensorTimeout()) {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_3D_CLIMB_TO_SAFE_ALT }
} /* No valid POS sensor but still within valid timeout - wait */
/* Position sensor failure timeout - land */ else {
else if (checkForPositionSensorTimeout()) { return NAV_FSM_EVENT_NONE;
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; }
}
/* No valid POS sensor but still within valid timeout - wait */
else {
return NAV_FSM_EVENT_NONE;
} }
} }

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

@ -126,6 +126,7 @@ typedef enum {
NAV_FSM_EVENT_STATE_SPECIFIC, // State-specific event NAV_FSM_EVENT_STATE_SPECIFIC, // State-specific event
NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED = NAV_FSM_EVENT_STATE_SPECIFIC, NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED = NAV_FSM_EVENT_STATE_SPECIFIC,
NAV_FSM_EVENT_SWITCH_TO_RTH_3D_LANDING = NAV_FSM_EVENT_STATE_SPECIFIC,
NAV_FSM_EVENT_SWITCH_TO_IDLE, NAV_FSM_EVENT_SWITCH_TO_IDLE,
NAV_FSM_EVENT_SWITCH_TO_ALTHOLD, NAV_FSM_EVENT_SWITCH_TO_ALTHOLD,