From 4e8f9cda2dc9a96ebf1c2f969ebbafd29e8a2449 Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Wed, 6 Jun 2018 17:27:25 +0200 Subject: [PATCH] Fix RTH hover above home The state machine was switching to NAV_STATE_RTH_LANDING even when landing was not allowed which made the throttle during hover unable to go over cruise throttle impeding the ability of the craft to stay at the target altitude. --- src/main/io/osd.c | 11 ++-- src/main/navigation/navigation.c | 67 +++++++++++++++------ src/main/navigation/navigation.h | 3 +- src/main/navigation/navigation_private.h | 77 ++++++++++++------------ 4 files changed, 94 insertions(+), 64 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index cfd216871a8..6a4126b40e3 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -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"); + else + return OSD_MESSAGE_STR("HOVERING"); case MW_NAV_STATE_LANDED: return OSD_MESSAGE_STR("LANDED"); case MW_NAV_STATE_LAND_SETTLE: diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index dabbe1845ec..d358d9dbd41 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -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,29 +992,27 @@ 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); + // 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 - // Do not allow descent velocity slower than -50cm/s so the landing detector works. - descentVelLimited = MIN(-descentVelScaling * navConfig()->general.land_descent_rate, -50.0f); - } + descentVelScaling = constrainf(descentVelScaling, 0.25f, 1.0f); - updateClimbRateToAltitudeController(descentVelLimited, ROC_TO_ALT_NORMAL); + // 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; } } diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index fb30fa33ed1..44e825a5c00 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -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 { diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index e837ad76170..2edd74d4978 100755 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -163,6 +163,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, @@ -179,43 +180,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_IDLE, // 1 - - 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_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_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_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_UNDEFINED = 0, + + NAV_PERSISTENT_ID_IDLE = 1, + + 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_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_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_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, } navigationPersistentId_e; typedef enum { @@ -233,6 +235,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,