Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-20 23:05:17 +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

35
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,7 +732,13 @@ 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 (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 (posControl.flags.hasValidPositionSensor) {
// If close to home - reset home position // If close to home - reset home position
if (posControl.homeDistance < posControl.navConfig->min_rth_distance) { if (posControl.homeDistance < posControl.navConfig->min_rth_distance) {
@ -749,6 +756,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_INITIALIZE(navig
return NAV_FSM_EVENT_NONE; return NAV_FSM_EVENT_NONE;
} }
} }
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_HEAD_HOME(navigationFSMState_t previousState) static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_HEAD_HOME(navigationFSMState_t previousState)
{ {
@ -809,16 +817,24 @@ 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 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)) { if (!STATE(FIXED_WING)) {
// Multicopter, hover and climb // Multicopter, hover and climb
calculateInitialHoldPosition(&targetHoldPos); calculateInitialHoldPosition(&targetHoldPos);
@ -826,12 +842,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_INITIALIZE(navig
// Airplane - climbout before turning around // Airplane - climbout before turning around
calculateFarAwayTarget(&targetHoldPos, posControl.actualState.yaw, 100000.0f); // 1km away calculateFarAwayTarget(&targetHoldPos, posControl.actualState.yaw, 100000.0f); // 1km away
} }
}
setDesiredPosition(&targetHoldPos, 0, NAV_POS_UPDATE_XY); setDesiredPosition(&targetHoldPos, 0, NAV_POS_UPDATE_XY);
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_3D_CLIMB_TO_SAFE_ALT return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_3D_CLIMB_TO_SAFE_ALT
} }
}
/* Position sensor failure timeout - land */ /* Position sensor failure timeout - land */
else if (checkForPositionSensorTimeout()) { else if (checkForPositionSensorTimeout()) {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
@ -841,6 +857,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_INITIALIZE(navig
return NAV_FSM_EVENT_NONE; return NAV_FSM_EVENT_NONE;
} }
} }
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_CLIMB_TO_SAFE_ALT(navigationFSMState_t previousState) static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_CLIMB_TO_SAFE_ALT(navigationFSMState_t previousState)
{ {

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,