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

Fix RTH landing on airplanes

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2017-02-14 21:52:54 +10:00
parent e0a5278f3d
commit cfca306eca
2 changed files with 16 additions and 6 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -948,7 +948,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_HEAD_HOME(naviga
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
} }
else if (posControl.flags.hasValidPositionSensor) { else if (posControl.flags.hasValidPositionSensor) {
if (isWaypointReached(&posControl.homeWaypointAbove)) { if (isWaypointReached(&posControl.homeWaypointAbove, true)) {
// Successfully reached position target // Successfully reached position target
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_2D_FINISHING return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_2D_FINISHING
} }
@ -1091,7 +1091,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_HEAD_HOME(naviga
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
} }
else if (posControl.flags.hasValidPositionSensor) { else if (posControl.flags.hasValidPositionSensor) {
if (isWaypointReached(&posControl.homeWaypointAbove)) { if (isWaypointReached(&posControl.homeWaypointAbove, true)) {
// Successfully reached position target - update XYZ-position // Successfully reached position target - update XYZ-position
setDesiredPosition(&posControl.homeWaypointAbove.pos, posControl.homeWaypointAbove.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); setDesiredPosition(&posControl.homeWaypointAbove.pos, posControl.homeWaypointAbove.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_3D_HOVER_PRIOR_TO_LANDING return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_3D_HOVER_PRIOR_TO_LANDING
@ -1253,11 +1253,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
// If no position sensor available - land immediately // If no position sensor available - land immediately
if (posControl.flags.hasValidPositionSensor && posControl.flags.hasValidHeadingSensor) { if (posControl.flags.hasValidPositionSensor && posControl.flags.hasValidHeadingSensor) {
const bool isDoingRTH = (posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_RTH);
switch (posControl.waypointList[posControl.activeWaypointIndex].action) { switch (posControl.waypointList[posControl.activeWaypointIndex].action) {
case NAV_WP_ACTION_WAYPOINT: case NAV_WP_ACTION_WAYPOINT:
case NAV_WP_ACTION_RTH: case NAV_WP_ACTION_RTH:
default: default:
if (isWaypointReached(&posControl.activeWaypoint) || isWaypointMissed(&posControl.activeWaypoint)) { if (isWaypointReached(&posControl.activeWaypoint, isDoingRTH) || isWaypointMissed(&posControl.activeWaypoint)) {
// Waypoint reached // Waypoint reached
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED
} }
@ -1750,11 +1752,19 @@ bool isWaypointMissed(const navWaypointPosition_t * waypoint)
return ABS(bearingError) > 10000; // TRUE if we passed the waypoint by 100 degrees return ABS(bearingError) > 10000; // TRUE if we passed the waypoint by 100 degrees
} }
bool isWaypointReached(const navWaypointPosition_t * waypoint) bool isWaypointReached(const navWaypointPosition_t * waypoint, const bool isWaypointHome)
{ {
// We consider waypoint reached if within specified radius // We consider waypoint reached if within specified radius
const uint32_t wpDistance = calculateDistanceToDestination(&waypoint->pos); const uint32_t wpDistance = calculateDistanceToDestination(&waypoint->pos);
if (STATE(FIXED_WING) && isWaypointHome) {
// Airplane will do a circular loiter over home and might never approach it closer than waypoint_radius - need extra check
return (wpDistance <= navConfig()->general.waypoint_radius)
|| (wpDistance <= (navConfig()->fw.loiter_radius * 1.10f)); // 10% margin of desired circular loiter radius
}
else {
return (wpDistance <= navConfig()->general.waypoint_radius); return (wpDistance <= navConfig()->general.waypoint_radius);
}
} }
static void updateHomePositionCompatibility(void) static void updateHomePositionCompatibility(void)

2
src/main/navigation/navigation_private.h
View file

@ -322,7 +322,7 @@ void setDesiredPosition(const t_fp_vector * pos, int32_t yaw, navSetWaypointFlag
void setDesiredSurfaceOffset(float surfaceOffset); void setDesiredSurfaceOffset(float surfaceOffset);
void setDesiredPositionToFarAwayTarget(int32_t yaw, int32_t distance, navSetWaypointFlags_t useMask); void setDesiredPositionToFarAwayTarget(int32_t yaw, int32_t distance, navSetWaypointFlags_t useMask);
bool isWaypointReached(const navWaypointPosition_t * waypoint); bool isWaypointReached(const navWaypointPosition_t * waypoint, const bool isWaypointHome);
bool isWaypointMissed(const navWaypointPosition_t * waypoint); bool isWaypointMissed(const navWaypointPosition_t * waypoint);
bool isApproachingLastWaypoint(void); bool isApproachingLastWaypoint(void);
float getActiveWaypointSpeed(void); float getActiveWaypointSpeed(void);