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

Allow to ignore GPS loss on initial RTH climb

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2017-02-05 20:01:14 +10:00
parent 8fad3e3290
commit 8b213bdffe
2 changed files with 36 additions and 27 deletions
Download patch file Download diff file Expand all files Collapse all files

62
src/main/navigation/navigation.c
View file

@ -70,7 +70,8 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.extra_arming_safety = 1, .extra_arming_safety = 1,
.user_control_mode = NAV_GPS_ATTI, .user_control_mode = NAV_GPS_ATTI,
.rth_alt_control_mode = NAV_RTH_AT_LEAST_ALT, .rth_alt_control_mode = NAV_RTH_AT_LEAST_ALT,
.rth_climb_first = 1, // Climb first, turn after reaching safe altitude .rth_climb_first = 1, // Climb first, turn after reaching safe altitude
.rth_ignore_energency_on_climb = 0, // Ignore GPS loss on initial climb
.rth_tail_first = 0, .rth_tail_first = 0,
.disarm_on_landing = 0, .disarm_on_landing = 0,
.rth_allow_landing = 1, .rth_allow_landing = 1,
@ -993,7 +994,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_INITIALIZE(navig
return NAV_FSM_EVENT_SWITCH_TO_IDLE; return NAV_FSM_EVENT_SWITCH_TO_IDLE;
} }
else { else {
if (posControl.flags.hasValidPositionSensor) { if (posControl.flags.hasValidPositionSensor || navConfig()->general.flags.rth_ignore_energency_on_climb) {
// If close to home - reset home position and land // If close to home - reset home position and land
if (posControl.homeDistance < navConfig()->general.min_rth_distance) { if (posControl.homeDistance < navConfig()->general.min_rth_distance) {
setHomePosition(&posControl.actualState.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING); setHomePosition(&posControl.actualState.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING);
@ -1036,36 +1037,43 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_CLIMB_TO_SAFE_AL
{ {
UNUSED(previousState); UNUSED(previousState);
// If no position sensor available - land immediately // If we have valid pos sensor OR we are configured to ignore GPS loss
if (!(posControl.flags.hasValidPositionSensor && posControl.flags.hasValidHeadingSensor) && checkForPositionSensorTimeout()) { if (posControl.flags.hasValidPositionSensor || !checkForPositionSensorTimeout() || navConfig()->general.flags.rth_ignore_energency_on_climb) {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; if (((posControl.actualState.pos.V.Z - posControl.homeWaypointAbove.pos.V.Z) > -50.0f) || (!navConfig()->general.flags.rth_climb_first)) {
} // Delayed initialization for RTH sanity check on airplanes - allow to finish climb first as it can take some distance
if (STATE(FIXED_WING)) {
if (((posControl.actualState.pos.V.Z - posControl.homeWaypointAbove.pos.V.Z) > -50.0f) || (!navConfig()->general.flags.rth_climb_first)) { initializeRTHSanityChecker(&posControl.actualState.pos);
// Delayed initialization for RTH sanity check on airplanes - allow to finish climb first as it can take some distance
if (STATE(FIXED_WING)) {
initializeRTHSanityChecker(&posControl.actualState.pos);
}
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_3D_HEAD_HOME
}
else {
/* For multi-rotors execute sanity check during initial ascent as well */
if (!STATE(FIXED_WING)) {
if (!validateRTHSanityChecker()) {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
} }
}
// Climb to safe altitude and turn to correct direction return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_3D_HEAD_HOME
if (navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING)) {
setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
} }
else { else {
setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); /* For multi-rotors execute sanity check during initial ascent as well */
} if (!STATE(FIXED_WING)) {
if (!validateRTHSanityChecker()) {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
}
return NAV_FSM_EVENT_NONE; // Climb to safe altitude and turn to correct direction
if (STATE(FIXED_WING)) {
setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_Z);
}
else {
if (navConfig()->general.flags.rth_tail_first) {
setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
}
else {
setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
}
}
return NAV_FSM_EVENT_NONE;
}
}
/* Position sensor failure timeout - land */
else {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
} }
} }

1
src/main/navigation/navigation.h
View file

@ -100,6 +100,7 @@ typedef struct navConfig_s {
uint8_t rth_tail_first; // Return to home tail first uint8_t rth_tail_first; // Return to home tail first
uint8_t disarm_on_landing; // uint8_t disarm_on_landing; //
uint8_t rth_allow_landing; // Enable landing as last stage of RTH uint8_t rth_allow_landing; // Enable landing as last stage of RTH
uint8_t rth_ignore_energency_on_climb; // Option to ignore GPS loss on initial climb stage of RTH
} flags; } flags;
uint8_t pos_failure_timeout; // Time to wait before switching to emergency landing (0 - disable) uint8_t pos_failure_timeout; // Time to wait before switching to emergency landing (0 - disable)