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

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

@ -71,6 +71,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.user_control_mode = NAV_GPS_ATTI,
.rth_alt_control_mode = NAV_RTH_AT_LEAST_ALT,
.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,
.disarm_on_landing = 0,
.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;
}
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 (posControl.homeDistance < navConfig()->general.min_rth_distance) {
setHomePosition(&posControl.actualState.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING);
@ -1036,11 +1037,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_CLIMB_TO_SAFE_AL
{
UNUSED(previousState);
// If no position sensor available - land immediately
if (!(posControl.flags.hasValidPositionSensor && posControl.flags.hasValidHeadingSensor) && checkForPositionSensorTimeout()) {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
// If we have valid pos sensor OR we are configured to ignore GPS loss
if (posControl.flags.hasValidPositionSensor || !checkForPositionSensorTimeout() || navConfig()->general.flags.rth_ignore_energency_on_climb) {
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)) {
@ -1058,15 +1056,25 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_CLIMB_TO_SAFE_AL
}
// Climb to safe altitude and turn to correct direction
if (navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING)) {
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;
}
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_HEAD_HOME(navigationFSMState_t previousState)

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 disarm_on_landing; //
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;
uint8_t pos_failure_timeout; // Time to wait before switching to emergency landing (0 - disable)