Skip to content

Commit

Permalink
Fix RTH hover above home
Browse files Browse the repository at this point in the history 
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.
  • Loading branch information
@shellixyz
shellixyz committed Jun 6, 2018
1 parent 50847b9 commit 6c57215
Show file tree
Hide file tree
Showing 2 changed files with 87 additions and 57 deletions.
67 changes: 47 additions & 20 deletions src/main/navigation/navigation.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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_LAND_SETTLE,
.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,
Expand Down Expand Up @@ -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
Expand All @@ -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);
Expand All @@ -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;
}
}
Expand Down
77 changes: 40 additions & 37 deletions src/main/navigation/navigation_private.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -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 {
Expand All @@ -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,
Expand Down

0 comments on commit 6c57215

Please sign in to comment.