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

RTH: Better RTH safety. Prevent RTH from activating on airplanes if close to home. Initiate a land on multirotors when close to home.

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2016-03-24 11:15:00 +10:00
parent 517c958d17
commit 6d12c38b6c
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_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_RTH_3D_LANDING] = NAV_STATE_RTH_3D_HOVER_PRIOR_TO_LANDING,
[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)
{
/* All good for RTH */
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);
}
UNUSED(previousState);
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 {
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)
{
/* All good for RTH */
if (posControl.flags.hasValidPositionSensor) {
t_fp_vector targetHoldPos;
UNUSED(previousState);
// If close to home - reset home position
if (posControl.homeDistance < posControl.navConfig->min_rth_distance) {
setHomePosition(&posControl.actualState.pos, posControl.actualState.yaw);
targetHoldPos = posControl.actualState.pos;
}
else {
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
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;
}
else {
if (posControl.flags.hasValidPositionSensor) {
// If close to home - reset home position and land
if (posControl.homeDistance < posControl.navConfig->min_rth_distance) {
setHomePosition(&posControl.actualState.pos, posControl.actualState.yaw);
setDesiredPosition(&posControl.actualState.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z);
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
}
}
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 */
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;
/* 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;
}
}
}

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_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_ALTHOLD,