Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-23 00:05:28 +03:00
Code Issues Wiki Activity

Merge pull request #3332 from shellixyz/fix_rth_hover

Browse source 
Fix RTH hover above home
This commit is contained in:
Alberto García Hierro 2018-06-11 12:54:35 +01:00 • committed by GitHub
commit f7f34ea1ab
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
4 changed files with 88 additions and 58 deletions
Download patch file Download diff file Expand all files Collapse all files

11
src/main/io/osd.c
View file

@ -678,13 +678,12 @@ static const char * navigationStateMessage(void)
case MW_NAV_STATE_LAND_START: case MW_NAV_STATE_LAND_START:
return OSD_MESSAGE_STR("STARTING EMERGENCY LANDING"); return OSD_MESSAGE_STR("STARTING EMERGENCY LANDING");
case MW_NAV_STATE_LAND_IN_PROGRESS: case MW_NAV_STATE_LAND_IN_PROGRESS:
if (!navigationRTHAllowsLanding()) {
if (STATE(FIXED_WING)) {
return OSD_MESSAGE_STR("LOITERING AROUND HOME");
}
return OSD_MESSAGE_STR("HOVERING");
}
return OSD_MESSAGE_STR("LANDING"); return OSD_MESSAGE_STR("LANDING");
case MW_NAV_STATE_HOVER_ABOVE_HOME:
if (STATE(FIXED_WING)) {
return OSD_MESSAGE_STR("LOITERING AROUND HOME");
}
return OSD_MESSAGE_STR("HOVERING");
case MW_NAV_STATE_LANDED: case MW_NAV_STATE_LANDED:
return OSD_MESSAGE_STR("LANDED"); return OSD_MESSAGE_STR("LANDED");
case MW_NAV_STATE_LAND_SETTLE: case MW_NAV_STATE_LAND_SETTLE:

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

@ -192,6 +192,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHING(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHING(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigationFSMState_t previousState);
@ -361,6 +362,24 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
.onEvent = { .onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING, [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING,
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_LANDING, [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_LANDING,
[NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME] = NAV_STATE_RTH_HOVER_ABOVE_HOME,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
}
},
[NAV_STATE_RTH_HOVER_ABOVE_HOME] = {
.persistentId = NAV_PERSISTENT_ID_RTH_HOVER_ABOVE_HOME,
.onEntry = navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME,
.timeoutMs = 500,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT,
.mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_HOVER_ABOVE_HOME,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HOVER_ABOVE_HOME,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
@ -925,12 +944,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND
// Wait until target heading is reached (with 15 deg margin for error) // Wait until target heading is reached (with 15 deg margin for error)
if (STATE(FIXED_WING)) { if (STATE(FIXED_WING)) {
resetLandingDetector(); resetLandingDetector();
return NAV_FSM_EVENT_SUCCESS; return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME;
} }
else { else {
if (ABS(wrap_18000(posControl.homeWaypointAbove.yaw - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) { if (ABS(wrap_18000(posControl.homeWaypointAbove.yaw - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) {
resetLandingDetector(); resetLandingDetector();
return NAV_FSM_EVENT_SUCCESS; return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME;
} }
else if (!validateRTHSanityChecker()) { else if (!validateRTHSanityChecker()) {
// Continue to check for RTH sanity during pre-landing hover // Continue to check for RTH sanity during pre-landing hover
@ -947,6 +966,16 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND
} }
} }
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(navigationFSMState_t previousState)
{
UNUSED(previousState);
if (!(validateRTHSanityChecker() || (posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout()))
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
return NAV_FSM_EVENT_NONE;
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationFSMState_t previousState) static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationFSMState_t previousState)
{ {
UNUSED(previousState); UNUSED(previousState);
@ -963,28 +992,26 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
} }
if (navigationRTHAllowsLanding()) { float descentVelLimited = 0;
float descentVelLimited = 0;
// A safeguard - if surface altitude sensors is available and it is reading < 50cm altitude - drop to low descend speed // A safeguard - if surface altitude sensors is available and it is reading < 50cm altitude - drop to low descend speed
if ((posControl.flags.estAglStatus == EST_TRUSTED) && posControl.actualState.agl.pos.z < 50.0f) { if ((posControl.flags.estAglStatus == EST_TRUSTED) && posControl.actualState.agl.pos.z < 50.0f) {
// land_descent_rate == 200 : descend speed = 30 cm/s, gentle touchdown // land_descent_rate == 200 : descend speed = 30 cm/s, gentle touchdown
// Do not allow descent velocity slower than -30cm/s so the landing detector works. // Do not allow descent velocity slower than -30cm/s so the landing detector works.
descentVelLimited = MIN(-0.15f * navConfig()->general.land_descent_rate, -30.0f); descentVelLimited = MIN(-0.15f * navConfig()->general.land_descent_rate, -30.0f);
}
else {
// Ramp down descent velocity from 100% at maxAlt altitude to 25% from minAlt to 0cm.
float descentVelScaling = (navGetCurrentActualPositionAndVelocity()->pos.z - posControl.homePosition.pos.z - navConfig()->general.land_slowdown_minalt)
/ (navConfig()->general.land_slowdown_maxalt - navConfig()->general.land_slowdown_minalt) * 0.75f + 0.25f; // Yield 1.0 at 2000 alt and 0.25 at 500 alt
descentVelScaling = constrainf(descentVelScaling, 0.25f, 1.0f);
// Do not allow descent velocity slower than -50cm/s so the landing detector works.
descentVelLimited = MIN(-descentVelScaling * navConfig()->general.land_descent_rate, -50.0f);
}
updateClimbRateToAltitudeController(descentVelLimited, ROC_TO_ALT_NORMAL);
} }
else {
// Ramp down descent velocity from 100% at maxAlt altitude to 25% from minAlt to 0cm.
float descentVelScaling = (navGetCurrentActualPositionAndVelocity()->pos.z - posControl.homePosition.pos.z - navConfig()->general.land_slowdown_minalt)
/ (navConfig()->general.land_slowdown_maxalt - navConfig()->general.land_slowdown_minalt) * 0.75f + 0.25f; // Yield 1.0 at 2000 alt and 0.25 at 500 alt
descentVelScaling = constrainf(descentVelScaling, 0.25f, 1.0f);
// Do not allow descent velocity slower than -50cm/s so the landing detector works.
descentVelLimited = MIN(-descentVelScaling * navConfig()->general.land_descent_rate, -50.0f);
}
updateClimbRateToAltitudeController(descentVelLimited, ROC_TO_ALT_NORMAL);
return NAV_FSM_EVENT_NONE; return NAV_FSM_EVENT_NONE;
} }

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

@ -223,7 +223,8 @@ typedef enum {
MW_NAV_STATE_LAND_IN_PROGRESS, // Land in Progress MW_NAV_STATE_LAND_IN_PROGRESS, // Land in Progress
MW_NAV_STATE_LANDED, // Landed MW_NAV_STATE_LANDED, // Landed
MW_NAV_STATE_LAND_SETTLE, // Settling before land MW_NAV_STATE_LAND_SETTLE, // Settling before land
MW_NAV_STATE_LAND_START_DESCENT // Start descent MW_NAV_STATE_LAND_START_DESCENT, // Start descent
MW_NAV_STATE_HOVER_ABOVE_HOME // Hover/Loitering above home
} navSystemStatus_State_e; } navSystemStatus_State_e;
typedef enum { typedef enum {

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

@ -170,6 +170,7 @@ typedef enum {
NAV_FSM_EVENT_SWITCH_TO_ALTHOLD, NAV_FSM_EVENT_SWITCH_TO_ALTHOLD,
NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D, NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D,
NAV_FSM_EVENT_SWITCH_TO_RTH, NAV_FSM_EVENT_SWITCH_TO_RTH,
NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME,
NAV_FSM_EVENT_SWITCH_TO_WAYPOINT, NAV_FSM_EVENT_SWITCH_TO_WAYPOINT,
NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING, NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING,
NAV_FSM_EVENT_SWITCH_TO_LAUNCH, NAV_FSM_EVENT_SWITCH_TO_LAUNCH,
@ -186,43 +187,44 @@ typedef enum {
// This enum is used to keep values in blackbox logs stable, so we can // This enum is used to keep values in blackbox logs stable, so we can
// freely change navigationFSMState_t. // freely change navigationFSMState_t.
typedef enum { typedef enum {
NAV_PERSISTENT_ID_UNDEFINED = 0, // 0 NAV_PERSISTENT_ID_UNDEFINED = 0,
NAV_PERSISTENT_ID_IDLE, // 1 NAV_PERSISTENT_ID_IDLE = 1,
NAV_PERSISTENT_ID_ALTHOLD_INITIALIZE, // 2 NAV_PERSISTENT_ID_ALTHOLD_INITIALIZE = 2,
NAV_PERSISTENT_ID_ALTHOLD_IN_PROGRESS, // 3 NAV_PERSISTENT_ID_ALTHOLD_IN_PROGRESS = 3,
NAV_PERSISTENT_ID_UNUSED_1, // 4, was NAV_STATE_POSHOLD_2D_INITIALIZE NAV_PERSISTENT_ID_UNUSED_1 = 4, // was NAV_STATE_POSHOLD_2D_INITIALIZE
NAV_PERSISTENT_ID_UNUSED_2, // 5, was NAV_STATE_POSHOLD_2D_IN_PROGRESS NAV_PERSISTENT_ID_UNUSED_2 = 5, // was NAV_STATE_POSHOLD_2D_IN_PROGRESS
NAV_PERSISTENT_ID_POSHOLD_3D_INITIALIZE, // 6 NAV_PERSISTENT_ID_POSHOLD_3D_INITIALIZE = 6,
NAV_PERSISTENT_ID_POSHOLD_3D_IN_PROGRESS, // 7 NAV_PERSISTENT_ID_POSHOLD_3D_IN_PROGRESS = 7,
NAV_PERSISTENT_ID_RTH_INITIALIZE, // 8 NAV_PERSISTENT_ID_RTH_INITIALIZE = 8,
NAV_PERSISTENT_ID_RTH_CLIMB_TO_SAFE_ALT, // 9 NAV_PERSISTENT_ID_RTH_CLIMB_TO_SAFE_ALT = 9,
NAV_PERSISTENT_ID_RTH_HEAD_HOME, // 10 NAV_PERSISTENT_ID_RTH_HEAD_HOME = 10,
NAV_PERSISTENT_ID_RTH_HOVER_PRIOR_TO_LANDING, // 11 NAV_PERSISTENT_ID_RTH_HOVER_PRIOR_TO_LANDING = 11,
NAV_PERSISTENT_ID_RTH_LANDING, // 12 NAV_PERSISTENT_ID_RTH_HOVER_ABOVE_HOME = 29,
NAV_PERSISTENT_ID_RTH_FINISHING, // 13 NAV_PERSISTENT_ID_RTH_LANDING = 12,
NAV_PERSISTENT_ID_RTH_FINISHED, // 14 NAV_PERSISTENT_ID_RTH_FINISHING = 13,
NAV_PERSISTENT_ID_RTH_FINISHED = 14,
NAV_PERSISTENT_ID_WAYPOINT_INITIALIZE, // 15 NAV_PERSISTENT_ID_WAYPOINT_INITIALIZE = 15,
NAV_PERSISTENT_ID_WAYPOINT_PRE_ACTION, // 16 NAV_PERSISTENT_ID_WAYPOINT_PRE_ACTION = 16,
NAV_PERSISTENT_ID_WAYPOINT_IN_PROGRESS, // 17 NAV_PERSISTENT_ID_WAYPOINT_IN_PROGRESS = 17,
NAV_PERSISTENT_ID_WAYPOINT_REACHED, // 18 NAV_PERSISTENT_ID_WAYPOINT_REACHED = 18,
NAV_PERSISTENT_ID_WAYPOINT_NEXT, // 19 NAV_PERSISTENT_ID_WAYPOINT_NEXT = 19,
NAV_PERSISTENT_ID_WAYPOINT_FINISHED, // 20 NAV_PERSISTENT_ID_WAYPOINT_FINISHED = 20,
NAV_PERSISTENT_ID_WAYPOINT_RTH_LAND, // 21 NAV_PERSISTENT_ID_WAYPOINT_RTH_LAND = 21,
NAV_PERSISTENT_ID_EMERGENCY_LANDING_INITIALIZE, // 22 NAV_PERSISTENT_ID_EMERGENCY_LANDING_INITIALIZE = 22,
NAV_PERSISTENT_ID_EMERGENCY_LANDING_IN_PROGRESS, // 23 NAV_PERSISTENT_ID_EMERGENCY_LANDING_IN_PROGRESS = 23,
NAV_PERSISTENT_ID_EMERGENCY_LANDING_FINISHED, // 24 NAV_PERSISTENT_ID_EMERGENCY_LANDING_FINISHED = 24,
NAV_PERSISTENT_ID_LAUNCH_INITIALIZE, // 25 NAV_PERSISTENT_ID_LAUNCH_INITIALIZE = 25,
NAV_PERSISTENT_ID_LAUNCH_WAIT, // 26 NAV_PERSISTENT_ID_LAUNCH_WAIT = 26,
NAV_PERSISTENT_ID_UNUSED_3, // 27, was NAV_STATE_LAUNCH_MOTOR_DELAY NAV_PERSISTENT_ID_UNUSED_3 = 27, // was NAV_STATE_LAUNCH_MOTOR_DELAY
NAV_PERSISTENT_ID_LAUNCH_IN_PROGRESS, // 28 NAV_PERSISTENT_ID_LAUNCH_IN_PROGRESS = 28,
} navigationPersistentId_e; } navigationPersistentId_e;
typedef enum { typedef enum {
@ -240,6 +242,7 @@ typedef enum {
NAV_STATE_RTH_CLIMB_TO_SAFE_ALT, NAV_STATE_RTH_CLIMB_TO_SAFE_ALT,
NAV_STATE_RTH_HEAD_HOME, NAV_STATE_RTH_HEAD_HOME,
NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING, NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING,
NAV_STATE_RTH_HOVER_ABOVE_HOME,
NAV_STATE_RTH_LANDING, NAV_STATE_RTH_LANDING,
NAV_STATE_RTH_FINISHING, NAV_STATE_RTH_FINISHING,
NAV_STATE_RTH_FINISHED, NAV_STATE_RTH_FINISHED,