Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Fix RTH hover above home #3332

Merged
merged 1 commit into from
Jun 11, 2018
Merged

Fix RTH hover above home #3332

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
11 changes: 5 additions & 6 deletions src/main/io/osd.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down
67 changes: 47 additions & 20 deletions src/main/navigation/navigation.c
View file
Open in desktop
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_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,
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
3 changes: 2 additions & 1 deletion src/main/navigation/navigation.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down
77 changes: 40 additions & 37 deletions src/main/navigation/navigation_private.h
View file
Open in desktop
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