Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-22 15:55:40 +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:
return OSD_MESSAGE_STR("STARTING EMERGENCY LANDING");
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");
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:
return OSD_MESSAGE_STR("LANDED");
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_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_ABOVE_HOME(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_FINISHED(navigationFSMState_t previousState);
@ -361,6 +362,24 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HOVER_PRIOR_TO_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_ALTHOLD] = NAV_STATE_ALTHOLD_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)
if (STATE(FIXED_WING)) {
resetLandingDetector();
return NAV_FSM_EVENT_SUCCESS;
return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME;
}
else {
if (ABS(wrap_18000(posControl.homeWaypointAbove.yaw - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) {
resetLandingDetector();
return NAV_FSM_EVENT_SUCCESS;
return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME;
}
else if (!validateRTHSanityChecker()) {
// 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)
{
UNUSED(previousState);
@ -963,28 +992,26 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
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
if ((posControl.flags.estAglStatus == EST_TRUSTED) && posControl.actualState.agl.pos.z < 50.0f) {
// 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.
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);
// 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) {
// 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.
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);
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_LANDED, // Landed
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;
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_POSHOLD_3D,
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_EMERGENCY_LANDING,
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
// freely change navigationFSMState_t.
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_IN_PROGRESS, // 3
NAV_PERSISTENT_ID_ALTHOLD_INITIALIZE = 2,
NAV_PERSISTENT_ID_ALTHOLD_IN_PROGRESS = 3,
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_1 = 4, // was NAV_STATE_POSHOLD_2D_INITIALIZE
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_IN_PROGRESS, // 7
NAV_PERSISTENT_ID_POSHOLD_3D_INITIALIZE = 6,
NAV_PERSISTENT_ID_POSHOLD_3D_IN_PROGRESS = 7,
NAV_PERSISTENT_ID_RTH_INITIALIZE, // 8
NAV_PERSISTENT_ID_RTH_CLIMB_TO_SAFE_ALT, // 9
NAV_PERSISTENT_ID_RTH_HEAD_HOME, // 10
NAV_PERSISTENT_ID_RTH_HOVER_PRIOR_TO_LANDING, // 11
NAV_PERSISTENT_ID_RTH_LANDING, // 12
NAV_PERSISTENT_ID_RTH_FINISHING, // 13
NAV_PERSISTENT_ID_RTH_FINISHED, // 14
NAV_PERSISTENT_ID_RTH_INITIALIZE = 8,
NAV_PERSISTENT_ID_RTH_CLIMB_TO_SAFE_ALT = 9,
NAV_PERSISTENT_ID_RTH_HEAD_HOME = 10,
NAV_PERSISTENT_ID_RTH_HOVER_PRIOR_TO_LANDING = 11,
NAV_PERSISTENT_ID_RTH_HOVER_ABOVE_HOME = 29,
NAV_PERSISTENT_ID_RTH_LANDING = 12,
NAV_PERSISTENT_ID_RTH_FINISHING = 13,
NAV_PERSISTENT_ID_RTH_FINISHED = 14,
NAV_PERSISTENT_ID_WAYPOINT_INITIALIZE, // 15
NAV_PERSISTENT_ID_WAYPOINT_PRE_ACTION, // 16
NAV_PERSISTENT_ID_WAYPOINT_IN_PROGRESS, // 17
NAV_PERSISTENT_ID_WAYPOINT_REACHED, // 18
NAV_PERSISTENT_ID_WAYPOINT_NEXT, // 19
NAV_PERSISTENT_ID_WAYPOINT_FINISHED, // 20
NAV_PERSISTENT_ID_WAYPOINT_RTH_LAND, // 21
NAV_PERSISTENT_ID_WAYPOINT_INITIALIZE = 15,
NAV_PERSISTENT_ID_WAYPOINT_PRE_ACTION = 16,
NAV_PERSISTENT_ID_WAYPOINT_IN_PROGRESS = 17,
NAV_PERSISTENT_ID_WAYPOINT_REACHED = 18,
NAV_PERSISTENT_ID_WAYPOINT_NEXT = 19,
NAV_PERSISTENT_ID_WAYPOINT_FINISHED = 20,
NAV_PERSISTENT_ID_WAYPOINT_RTH_LAND = 21,
NAV_PERSISTENT_ID_EMERGENCY_LANDING_INITIALIZE, // 22
NAV_PERSISTENT_ID_EMERGENCY_LANDING_IN_PROGRESS, // 23
NAV_PERSISTENT_ID_EMERGENCY_LANDING_FINISHED, // 24
NAV_PERSISTENT_ID_EMERGENCY_LANDING_INITIALIZE = 22,
NAV_PERSISTENT_ID_EMERGENCY_LANDING_IN_PROGRESS = 23,
NAV_PERSISTENT_ID_EMERGENCY_LANDING_FINISHED = 24,
NAV_PERSISTENT_ID_LAUNCH_INITIALIZE, // 25
NAV_PERSISTENT_ID_LAUNCH_WAIT, // 26
NAV_PERSISTENT_ID_UNUSED_3, // 27, was NAV_STATE_LAUNCH_MOTOR_DELAY
NAV_PERSISTENT_ID_LAUNCH_IN_PROGRESS, // 28
NAV_PERSISTENT_ID_LAUNCH_INITIALIZE = 25,
NAV_PERSISTENT_ID_LAUNCH_WAIT = 26,
NAV_PERSISTENT_ID_UNUSED_3 = 27, // was NAV_STATE_LAUNCH_MOTOR_DELAY
NAV_PERSISTENT_ID_LAUNCH_IN_PROGRESS = 28,
} navigationPersistentId_e;
typedef enum {
@ -240,6 +242,7 @@ typedef enum {
NAV_STATE_RTH_CLIMB_TO_SAFE_ALT,
NAV_STATE_RTH_HEAD_HOME,
NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING,
NAV_STATE_RTH_HOVER_ABOVE_HOME,
NAV_STATE_RTH_LANDING,
NAV_STATE_RTH_FINISHING,
NAV_STATE_RTH_FINISHED,