Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-19 14:25:16 +03:00
Code Issues Wiki Activity

Initial cut of AGL

Browse source 
Optimize and refactor

Refactoring
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2017-07-30 23:47:43 +10:00
parent 9e3180143f
commit 2c65d2273e
5 changed files with 218 additions and 124 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -696,11 +696,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS(nav
resetGCSFlags(); resetGCSFlags();
} }
// If we enable terrain mode and surface offset is not set yet - do it
if (posControl.flags.hasValidSurfaceSensor && posControl.flags.isTerrainFollowEnabled && posControl.desiredState.surface < 0) {
setDesiredSurfaceOffset(posControl.actualState.surface);
}
return NAV_FSM_EVENT_NONE; return NAV_FSM_EVENT_NONE;
} }
@ -779,11 +774,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS(
resetGCSFlags(); resetGCSFlags();
} }
// If we enable terrain mode and surface offset is not set yet - do it
if (posControl.flags.hasValidSurfaceSensor && posControl.flags.isTerrainFollowEnabled && posControl.desiredState.surface < 0) {
setDesiredSurfaceOffset(posControl.actualState.surface);
}
return NAV_FSM_EVENT_NONE; return NAV_FSM_EVENT_NONE;
} }
@ -791,7 +781,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
{ {
navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState); navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
if (!posControl.flags.hasValidHeadingSensor || !posControl.flags.hasValidAltitudeSensor || !STATE(GPS_FIX_HOME)) { if ((posControl.flags.estHeadingStatus == EST_NONE) || (posControl.flags.estAltStatus == EST_NONE) || !STATE(GPS_FIX_HOME)) {
// Heading sensor, altitude sensor and HOME fix are mandatory for RTH. If not satisfied - switch to emergency landing // Heading sensor, altitude sensor and HOME fix are mandatory for RTH. If not satisfied - switch to emergency landing
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
} }
@ -802,7 +792,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
} }
// If we have valid position sensor or configured to ignore it's loss at initial stage - continue // If we have valid position sensor or configured to ignore it's loss at initial stage - continue
if (posControl.flags.hasValidPositionSensor || navConfig()->general.flags.rth_climb_ignore_emerg) { if ((posControl.flags.estPosStatue >= EST_USABLE) || navConfig()->general.flags.rth_climb_ignore_emerg) {
// Reset altitude and position controllers if necessary // Reset altitude and position controllers if necessary
if ((prevFlags & NAV_CTL_POS) == 0) { if ((prevFlags & NAV_CTL_POS) == 0) {
resetPositionController(); resetPositionController();
@ -854,12 +844,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
{ {
UNUSED(previousState); UNUSED(previousState);
if (!posControl.flags.hasValidHeadingSensor) { if ((posControl.flags.estHeadingStatus == EST_NONE)) {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
} }
// If we have valid pos sensor OR we are configured to ignore GPS loss // If we have valid pos sensor OR we are configured to ignore GPS loss
if (posControl.flags.hasValidPositionSensor || !checkForPositionSensorTimeout() || navConfig()->general.flags.rth_climb_ignore_emerg) { if ((posControl.flags.estPosStatue >= EST_USABLE) || !checkForPositionSensorTimeout() || navConfig()->general.flags.rth_climb_ignore_emerg) {
const float rthAltitudeMargin = STATE(FIXED_WING) ? const float rthAltitudeMargin = STATE(FIXED_WING) ?
MAX(100.0f, 0.10f * ABS(posControl.homeWaypointAbove.pos.V.Z - posControl.homePosition.pos.V.Z)) : // Airplane: 10% of target altitude but no less than 1m MAX(100.0f, 0.10f * ABS(posControl.homeWaypointAbove.pos.V.Z - posControl.homePosition.pos.V.Z)) : // Airplane: 10% of target altitude but no less than 1m
MAX( 50.0f, 0.05f * ABS(posControl.homeWaypointAbove.pos.V.Z - posControl.homePosition.pos.V.Z)); // Copters: 5% of target altitude but no less than 50cm MAX( 50.0f, 0.05f * ABS(posControl.homeWaypointAbove.pos.V.Z - posControl.homePosition.pos.V.Z)); // Copters: 5% of target altitude but no less than 50cm
@ -906,12 +896,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
{ {
UNUSED(previousState); UNUSED(previousState);
if (!posControl.flags.hasValidHeadingSensor) { if ((posControl.flags.estHeadingStatus == EST_NONE)) {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
} }
// If we have position sensor - continue home // If we have position sensor - continue home
if (posControl.flags.hasValidPositionSensor) { if ((posControl.flags.estPosStatue >= EST_USABLE)) {
if (isWaypointReached(&posControl.homeWaypointAbove, true)) { if (isWaypointReached(&posControl.homeWaypointAbove, true)) {
// Successfully reached position target - update XYZ-position // Successfully reached position target - update XYZ-position
setDesiredPosition(&posControl.homeWaypointAbove.pos, posControl.homeWaypointAbove.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); setDesiredPosition(&posControl.homeWaypointAbove.pos, posControl.homeWaypointAbove.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
@ -946,12 +936,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND
{ {
UNUSED(previousState); UNUSED(previousState);
if (!posControl.flags.hasValidHeadingSensor) { if ((posControl.flags.estHeadingStatus == EST_NONE)) {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
} }
// If position ok OR within valid timeout - continue // If position ok OR within valid timeout - continue
if (posControl.flags.hasValidPositionSensor || !checkForPositionSensorTimeout()) { if ((posControl.flags.estPosStatue >= EST_USABLE) || !checkForPositionSensorTimeout()) {
// 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();
@ -997,7 +987,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
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.hasValidSurfaceSensor && posControl.actualState.surface < 50.0f) { if ((posControl.flags.estSurfaceStatus == EST_TRUSTED) && posControl.actualState.surface < 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);
@ -1080,7 +1070,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
UNUSED(previousState); UNUSED(previousState);
// If no position sensor available - land immediately // If no position sensor available - land immediately
if (posControl.flags.hasValidPositionSensor && posControl.flags.hasValidHeadingSensor) { if ((posControl.flags.estPosStatue >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE)) {
const bool isDoingRTH = (posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_RTH); const bool isDoingRTH = (posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_RTH);
switch (posControl.waypointList[posControl.activeWaypointIndex].action) { switch (posControl.waypointList[posControl.activeWaypointIndex].action) {
@ -1164,7 +1154,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED(navig
UNUSED(previousState); UNUSED(previousState);
// If no position sensor available - land immediately // If no position sensor available - land immediately
if (posControl.flags.hasValidPositionSensor && posControl.flags.hasValidHeadingSensor) { if ((posControl.flags.estPosStatue >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE)) {
return NAV_FSM_EVENT_NONE; return NAV_FSM_EVENT_NONE;
} }
/* No pos sensor available for NAV_WAIT_FOR_GPS_TIMEOUT_MS - land */ /* No pos sensor available for NAV_WAIT_FOR_GPS_TIMEOUT_MS - land */
@ -1435,7 +1425,7 @@ bool isThrustFacingDownwards(void)
bool checkForPositionSensorTimeout(void) bool checkForPositionSensorTimeout(void)
{ {
if (navConfig()->general.pos_failure_timeout) { if (navConfig()->general.pos_failure_timeout) {
if (!posControl.flags.hasValidPositionSensor && ((millis() - posControl.lastValidPositionTimeMs) > (1000 * navConfig()->general.pos_failure_timeout))) { if ((posControl.flags.estPosStatue == EST_NONE) && ((millis() - posControl.lastValidPositionTimeMs) > (1000 * navConfig()->general.pos_failure_timeout))) {
return true; return true;
} }
else { else {
@ -1451,7 +1441,7 @@ bool checkForPositionSensorTimeout(void)
/*----------------------------------------------------------- /*-----------------------------------------------------------
* Processes an update to XY-position and velocity * Processes an update to XY-position and velocity
*-----------------------------------------------------------*/ *-----------------------------------------------------------*/
void updateActualHorizontalPositionAndVelocity(bool hasValidSensor, float newX, float newY, float newVelX, float newVelY) void updateActualHorizontalPositionAndVelocity(bool estimateValid, float newX, float newY, float newVelX, float newVelY)
{ {
posControl.actualState.pos.V.X = newX; posControl.actualState.pos.V.X = newX;
posControl.actualState.pos.V.Y = newY; posControl.actualState.pos.V.Y = newY;
@ -1460,17 +1450,18 @@ void updateActualHorizontalPositionAndVelocity(bool hasValidSensor, float newX,
posControl.actualState.vel.V.Y = newVelY; posControl.actualState.vel.V.Y = newVelY;
posControl.actualState.velXY = sqrtf(sq(newVelX) + sq(newVelY)); posControl.actualState.velXY = sqrtf(sq(newVelX) + sq(newVelY));
posControl.flags.hasValidPositionSensor = hasValidSensor; if (estimateValid) {
posControl.flags.hasValidHeadingSensor = isImuHeadingValid(); posControl.flags.estPosStatue = EST_TRUSTED;
if (hasValidSensor) {
posControl.flags.horizontalPositionDataNew = 1; posControl.flags.horizontalPositionDataNew = 1;
posControl.lastValidPositionTimeMs = millis(); posControl.lastValidPositionTimeMs = millis();
} }
else { else {
posControl.flags.estPosStatue = EST_NONE;
posControl.flags.horizontalPositionDataNew = 0; posControl.flags.horizontalPositionDataNew = 0;
} }
posControl.flags.estHeadingStatus = isImuHeadingValid() ? EST_TRUSTED : EST_NONE;
#if defined(NAV_BLACKBOX) #if defined(NAV_BLACKBOX)
navLatestActualPosition[X] = newX; navLatestActualPosition[X] = newX;
navLatestActualPosition[Y] = newY; navLatestActualPosition[Y] = newY;
@ -1482,20 +1473,20 @@ void updateActualHorizontalPositionAndVelocity(bool hasValidSensor, float newX,
/*----------------------------------------------------------- /*-----------------------------------------------------------
* Processes an update to Z-position and velocity * Processes an update to Z-position and velocity
*-----------------------------------------------------------*/ *-----------------------------------------------------------*/
void updateActualAltitudeAndClimbRate(bool hasValidSensor, float newAltitude, float newVelocity) void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity)
{ {
posControl.actualState.pos.V.Z = newAltitude; posControl.actualState.pos.V.Z = newAltitude;
posControl.actualState.vel.V.Z = newVelocity; posControl.actualState.vel.V.Z = newVelocity;
posControl.flags.hasValidAltitudeSensor = hasValidSensor;
// Update altitude that would be used when executing RTH // Update altitude that would be used when executing RTH
if (hasValidSensor) { if (estimateValid) {
updateDesiredRTHAltitude(); updateDesiredRTHAltitude();
posControl.flags.estAltStatus = EST_TRUSTED;
posControl.flags.verticalPositionDataNew = 1; posControl.flags.verticalPositionDataNew = 1;
posControl.lastValidAltitudeTimeMs = millis(); posControl.lastValidAltitudeTimeMs = millis();
} }
else { else {
posControl.flags.estAltStatus = EST_NONE;
posControl.flags.verticalPositionDataNew = 0; posControl.flags.verticalPositionDataNew = 0;
} }
@ -1508,25 +1499,24 @@ void updateActualAltitudeAndClimbRate(bool hasValidSensor, float newAltitude, fl
/*----------------------------------------------------------- /*-----------------------------------------------------------
* Processes an update to surface distance * Processes an update to surface distance
*-----------------------------------------------------------*/ *-----------------------------------------------------------*/
void updateActualSurfaceDistance(bool hasValidSensor, float surfaceDistance, float surfaceVelocity) void updateActualAGLAndClimgRate(bool estimateValid, bool estimateReliable, float surfaceDistance, float surfaceVelocity)
{ {
posControl.actualState.surface = surfaceDistance; posControl.actualState.surface = surfaceDistance;
posControl.actualState.surfaceVel = surfaceVelocity; posControl.actualState.surfaceVel = surfaceVelocity;
// Update validity // Update validity
// Update validity if (estimateValid) {
if (hasValidSensor) { posControl.flags.estSurfaceStatus = estimateReliable ? EST_TRUSTED : EST_USABLE;
posControl.flags.hasValidSurfaceSensor = true;
posControl.flags.surfaceDistanceDataNew = 1; posControl.flags.surfaceDistanceDataNew = 1;
} }
else { else {
posControl.flags.hasValidSurfaceSensor = false; posControl.flags.estSurfaceStatus = EST_NONE;
posControl.flags.surfaceDistanceDataNew = 0; posControl.flags.surfaceDistanceDataNew = 0;
} }
// Update minimum surface distance (landing detection threshold) // Update minimum surface distance (landing detection threshold)
if (ARMING_FLAG(ARMED)) { if (ARMING_FLAG(ARMED)) {
if (hasValidSensor && posControl.actualState.surface > 0) { if ((posControl.flags.estSurfaceStatus == EST_TRUSTED) && posControl.actualState.surface > 0) {
if (posControl.actualState.surfaceMin > 0) { if (posControl.actualState.surfaceMin > 0) {
posControl.actualState.surfaceMin = MIN(posControl.actualState.surfaceMin, posControl.actualState.surface); posControl.actualState.surfaceMin = MIN(posControl.actualState.surfaceMin, posControl.actualState.surface);
} }
@ -1723,7 +1713,7 @@ void updateHomePosition(void)
{ {
// Disarmed and have a valid position, constantly update home // Disarmed and have a valid position, constantly update home
if (!ARMING_FLAG(ARMED)) { if (!ARMING_FLAG(ARMED)) {
if (posControl.flags.hasValidPositionSensor) { if ((posControl.flags.estPosStatue >= EST_USABLE)) {
setHomePosition(&posControl.actualState.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING ); setHomePosition(&posControl.actualState.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING );
} }
} }
@ -1732,7 +1722,7 @@ void updateHomePosition(void)
// If pilot so desires he may reset home position to current position // If pilot so desires he may reset home position to current position
if (IS_RC_MODE_ACTIVE(BOXHOMERESET)) { if (IS_RC_MODE_ACTIVE(BOXHOMERESET)) {
if (isHomeResetAllowed && !FLIGHT_MODE(NAV_RTH_MODE) && !FLIGHT_MODE(NAV_WP_MODE) && posControl.flags.hasValidPositionSensor) { if (isHomeResetAllowed && !FLIGHT_MODE(NAV_RTH_MODE) && !FLIGHT_MODE(NAV_WP_MODE) && (posControl.flags.estPosStatue >= EST_USABLE)) {
const navSetWaypointFlags_t homeUpdateFlags = STATE(GPS_FIX_HOME) ? (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING) : (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); const navSetWaypointFlags_t homeUpdateFlags = STATE(GPS_FIX_HOME) ? (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING) : (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
setHomePosition(&posControl.actualState.pos, posControl.actualState.yaw, homeUpdateFlags); setHomePosition(&posControl.actualState.pos, posControl.actualState.yaw, homeUpdateFlags);
isHomeResetAllowed = false; isHomeResetAllowed = false;
@ -1771,23 +1761,6 @@ int32_t getTotalTravelDistance(void)
return lrintf(posControl.totalTripDistance); return lrintf(posControl.totalTripDistance);
} }
/*-----------------------------------------------------------
* Set surface tracking target
*-----------------------------------------------------------*/
void setDesiredSurfaceOffset(float surfaceOffset)
{
if (surfaceOffset > 0) {
posControl.desiredState.surface = constrainf(surfaceOffset, 1.0f, INAV_SURFACE_MAX_DISTANCE);
}
else {
posControl.desiredState.surface = -1;
}
#if defined(NAV_BLACKBOX)
navTargetSurface = constrain(lrintf(posControl.desiredState.surface), -32678, 32767);
#endif
}
/*----------------------------------------------------------- /*-----------------------------------------------------------
* Calculate platform-specific hold position (account for deceleration) * Calculate platform-specific hold position (account for deceleration)
*-----------------------------------------------------------*/ *-----------------------------------------------------------*/
@ -2040,7 +2013,7 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
wpLLH.alt = wpData->alt; wpLLH.alt = wpData->alt;
// WP #0 - special waypoint - HOME // WP #0 - special waypoint - HOME
if ((wpNumber == 0) && ARMING_FLAG(ARMED) && posControl.flags.hasValidPositionSensor && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled) { if ((wpNumber == 0) && ARMING_FLAG(ARMED) && (posControl.flags.estPosStatue >= EST_USABLE) && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled) {
// Forcibly set home position. Note that this is only valid if already armed, otherwise home will be reset instantly // Forcibly set home position. Note that this is only valid if already armed, otherwise home will be reset instantly
geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, &wpPos.pos, GEO_ALT_RELATIVE); geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, &wpPos.pos, GEO_ALT_RELATIVE);
setHomePosition(&wpPos.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); setHomePosition(&wpPos.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
@ -2048,7 +2021,7 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
// WP #255 - special waypoint - directly set desiredPosition // WP #255 - special waypoint - directly set desiredPosition
// Only valid when armed and in poshold mode // Only valid when armed and in poshold mode
else if ((wpNumber == 255) && (wpData->action == NAV_WP_ACTION_WAYPOINT) && else if ((wpNumber == 255) && (wpData->action == NAV_WP_ACTION_WAYPOINT) &&
ARMING_FLAG(ARMED) && posControl.flags.hasValidPositionSensor && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled && ARMING_FLAG(ARMED) && (posControl.flags.estPosStatue >= EST_USABLE) && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled &&
(posControl.navState == NAV_STATE_POSHOLD_2D_IN_PROGRESS || posControl.navState == NAV_STATE_POSHOLD_3D_IN_PROGRESS)) { (posControl.navState == NAV_STATE_POSHOLD_2D_IN_PROGRESS || posControl.navState == NAV_STATE_POSHOLD_3D_IN_PROGRESS)) {
// Convert to local coordinates // Convert to local coordinates
geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, &wpPos.pos, GEO_ALT_RELATIVE); geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, &wpPos.pos, GEO_ALT_RELATIVE);
@ -2265,14 +2238,13 @@ void applyWaypointNavigationAndAltitudeHold(void)
#if defined(NAV_BLACKBOX) #if defined(NAV_BLACKBOX)
navFlags = 0; navFlags = 0;
if (posControl.flags.hasValidAltitudeSensor) navFlags |= (1 << 0); if (posControl.flags.estAltStatus == EST_TRUSTED) navFlags |= (1 << 0);
if (posControl.flags.hasValidSurfaceSensor) navFlags |= (1 << 1); if (posControl.flags.estSurfaceStatus == EST_TRUSTED) navFlags |= (1 << 1);
if (posControl.flags.hasValidPositionSensor) navFlags |= (1 << 2); if (posControl.flags.estPosStatue == EST_TRUSTED) navFlags |= (1 << 2);
//if (STATE(GPS_FIX)) navFlags |= (1 << 3);
#if defined(NAV_GPS_GLITCH_DETECTION) #if defined(NAV_GPS_GLITCH_DETECTION)
if (isGPSGlitchDetected()) navFlags |= (1 << 4); if (isGPSGlitchDetected()) navFlags |= (1 << 4);
#endif #endif
if (posControl.flags.hasValidHeadingSensor) navFlags |= (1 << 5); if (posControl.flags.estHeadingStatus == EST_TRUSTED) navFlags |= (1 << 5);
#endif #endif
// Reset all navigation requests - NAV controllers will set them if necessary // Reset all navigation requests - NAV controllers will set them if necessary
@ -2334,12 +2306,12 @@ void swithNavigationFlightModes(void)
*-----------------------------------------------------------*/ *-----------------------------------------------------------*/
static bool canActivateAltHoldMode(void) static bool canActivateAltHoldMode(void)
{ {
return posControl.flags.hasValidAltitudeSensor; return (posControl.flags.estAltStatus >= EST_USABLE);
} }
static bool canActivatePosHoldMode(void) static bool canActivatePosHoldMode(void)
{ {
return posControl.flags.hasValidPositionSensor && posControl.flags.hasValidHeadingSensor; return (posControl.flags.estPosStatue >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE);
} }
static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
@ -2487,7 +2459,7 @@ bool navigationBlockArming(void)
return false; return false;
// Apply extra arming safety only if pilot has any of GPS modes configured // Apply extra arming safety only if pilot has any of GPS modes configured
if ((isUsingNavigationModes() || failsafeMayRequireNavigationMode()) && !(posControl.flags.hasValidPositionSensor && STATE(GPS_FIX_HOME))) { if ((isUsingNavigationModes() || failsafeMayRequireNavigationMode()) && !((posControl.flags.estPosStatue >= EST_USABLE) && STATE(GPS_FIX_HOME))) {
shouldBlockArming = true; shouldBlockArming = true;
} }
@ -2514,7 +2486,7 @@ bool navigationBlockArming(void)
bool navigationPositionEstimateIsHealthy(void) bool navigationPositionEstimateIsHealthy(void)
{ {
return posControl.flags.hasValidPositionSensor && STATE(GPS_FIX_HOME); return (posControl.flags.estPosStatue >= EST_USABLE) && STATE(GPS_FIX_HOME);
} }
/** /**
@ -2627,10 +2599,10 @@ void navigationInit(void)
posControl.flags.surfaceDistanceDataNew = 0; posControl.flags.surfaceDistanceDataNew = 0;
posControl.flags.headingDataNew = 0; posControl.flags.headingDataNew = 0;
posControl.flags.hasValidAltitudeSensor = 0; posControl.flags.estAltStatus = EST_NONE;
posControl.flags.hasValidPositionSensor = 0; posControl.flags.estPosStatue = EST_NONE;
posControl.flags.hasValidSurfaceSensor = 0; posControl.flags.estHeadingStatus = EST_NONE;
posControl.flags.hasValidHeadingSensor = 0; posControl.flags.estSurfaceStatus = EST_NONE;
posControl.flags.forcedRTHActivated = 0; posControl.flags.forcedRTHActivated = 0;
posControl.waypointCount = 0; posControl.waypointCount = 0;

11
src/main/navigation/navigation_fixedwing.c
View file

@ -151,7 +151,7 @@ void applyFixedWingAltitudeAndThrottleController(timeUs_t currentTimeUs)
return; return;
} }
if (posControl.flags.hasValidAltitudeSensor) { if ((posControl.flags.estAltStatus >= EST_USABLE)) {
if (posControl.flags.verticalPositionDataNew) { if (posControl.flags.verticalPositionDataNew) {
const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
previousTimePositionUpdate = currentTimeUs; previousTimePositionUpdate = currentTimeUs;
@ -330,7 +330,7 @@ void applyFixedWingPositionController(timeUs_t currentTimeUs)
} }
// Apply controller only if position source is valid. In absence of valid pos sensor (GPS loss), we'd stick in forced ANGLE mode // Apply controller only if position source is valid. In absence of valid pos sensor (GPS loss), we'd stick in forced ANGLE mode
if (posControl.flags.hasValidPositionSensor) { if ((posControl.flags.estPosStatue >= EST_USABLE)) {
// If we have new position - update velocity and acceleration controllers // If we have new position - update velocity and acceleration controllers
if (posControl.flags.horizontalPositionDataNew) { if (posControl.flags.horizontalPositionDataNew) {
const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
@ -378,7 +378,7 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs)
} }
// Apply controller only if position source is valid // Apply controller only if position source is valid
if (posControl.flags.hasValidPositionSensor) { if ((posControl.flags.estPosStatue >= EST_USABLE)) {
// If we have new position - update velocity and acceleration controllers // If we have new position - update velocity and acceleration controllers
if (posControl.flags.horizontalPositionDataNew) { if (posControl.flags.horizontalPositionDataNew) {
const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
@ -473,7 +473,8 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
* TODO refactor conditions in this metod if logic is proven to be correct * TODO refactor conditions in this metod if logic is proven to be correct
*/ */
if (navStateFlags & NAV_CTL_LAND) { if (navStateFlags & NAV_CTL_LAND) {
if (posControl.flags.hasValidAltitudeSensor && posControl.actualState.pos.V.Z < navConfig()->general.land_slowdown_minalt) { if ( ((posControl.flags.estAltStatus >= EST_USABLE) && (posControl.actualState.pos.V.Z <= navConfig()->general.land_slowdown_minalt)) ||
((posControl.flags.estSurfaceStatus == EST_TRUSTED) && (posControl.actualState.surface <= navConfig()->general.land_slowdown_minalt)) ) {
/* /*
* Set motor to min. throttle and stop it when MOTOR_STOP feature is enabled * Set motor to min. throttle and stop it when MOTOR_STOP feature is enabled
*/ */
@ -481,7 +482,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);
/* /*
* Stabilize ROLL axis on 0 degress banking regardless of loiter radius and position * Stabilize ROLL axis on 0 degrees banking regardless of loiter radius and position
*/ */
rcCommand[ROLL] = 0; rcCommand[ROLL] = 0;

8
src/main/navigation/navigation_multicopter.c
View file

@ -422,7 +422,7 @@ static void applyMulticopterPositionController(timeUs_t currentTimeUs)
// Apply controller only if position source is valid. In absence of valid pos sensor (GPS loss), we'd stick in forced ANGLE mode // Apply controller only if position source is valid. In absence of valid pos sensor (GPS loss), we'd stick in forced ANGLE mode
// and pilots input would be passed thru to PID controller // and pilots input would be passed thru to PID controller
if (posControl.flags.hasValidPositionSensor) { if ((posControl.flags.estPosStatue >= EST_USABLE)) {
// If we have new position - update velocity and acceleration controllers // If we have new position - update velocity and acceleration controllers
if (posControl.flags.horizontalPositionDataNew) { if (posControl.flags.horizontalPositionDataNew) {
const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
@ -509,7 +509,7 @@ bool isMulticopterLandingDetected(void)
} }
// If we have surface sensor (for example sonar) - use it to detect touchdown // If we have surface sensor (for example sonar) - use it to detect touchdown
if (posControl.flags.hasValidSurfaceSensor && posControl.actualState.surfaceMin >= 0) { if ((posControl.flags.estSurfaceStatus == EST_TRUSTED) && (posControl.actualState.surfaceMin >= 0)) {
// TODO: Come up with a clever way to let sonar increase detection performance, not just add extra safety. // TODO: Come up with a clever way to let sonar increase detection performance, not just add extra safety.
// TODO: Out of range sonar may give reading that looks like we landed, find a way to check if sonar is healthy. // TODO: Out of range sonar may give reading that looks like we landed, find a way to check if sonar is healthy.
// surfaceMin is our ground reference. If we are less than 5cm above the ground - we are likely landed // surfaceMin is our ground reference. If we are less than 5cm above the ground - we are likely landed
@ -540,9 +540,7 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
rcCommand[PITCH] = 0; rcCommand[PITCH] = 0;
rcCommand[YAW] = 0; rcCommand[YAW] = 0;
if (posControl.flags.hasValidAltitudeSensor) { if ((posControl.flags.estAltStatus >= EST_USABLE)) {
/* We have an altitude reference, apply AH-based landing controller */
// If last position update was too long in the past - ignore it (likely restarting altitude controller) // If last position update was too long in the past - ignore it (likely restarting altitude controller)
if (deltaMicros > HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) { if (deltaMicros > HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
previousTimeUpdate = currentTimeUs; previousTimeUpdate = currentTimeUs;

185
src/main/navigation/navigation_pos_estimator.c
View file

@ -109,9 +109,16 @@ typedef struct {
float airspeed; // airspeed (cm/s) float airspeed; // airspeed (cm/s)
} navPositionEstimatorPITOT_t; } navPositionEstimatorPITOT_t;
typedef enum {
SURFACE_QUAL_LOW, // Surface sensor signal lost long ago - most likely surface distance is incorrect
SURFACE_QUAL_MID, // Surface sensor is not available but we can somewhat trust the estimate
SURFACE_QUAL_HIGH // All good
} navAGLEstimateQuality_e;
typedef struct { typedef struct {
timeUs_t lastUpdateTime; // Last update time (us) timeUs_t lastUpdateTime; // Last update time (us)
float alt; // Raw altitude measurement (cm) float alt; // Raw altitude measurement (cm)
float reliability;
} navPositionEstimatorSURFACE_t; } navPositionEstimatorSURFACE_t;
typedef struct { typedef struct {
@ -124,15 +131,18 @@ typedef struct {
typedef struct { typedef struct {
timeUs_t lastUpdateTime; // Last update time (us) timeUs_t lastUpdateTime; // Last update time (us)
// 3D position, velocity and confidence // 3D position, velocity and confidence
t_fp_vector pos; t_fp_vector pos;
t_fp_vector vel; t_fp_vector vel;
float eph; float eph;
float epv; float epv;
// Surface offset
float surface; // AGL
float surfaceVel; navAGLEstimateQuality_e aglQual;
bool surfaceValid; float aglOffset; // Offset between surface and pos.Z
float aglAlt;
float aglVel;
} navPositionEstimatorESTIMATE_t; } navPositionEstimatorESTIMATE_t;
typedef struct { typedef struct {
@ -484,11 +494,39 @@ static void updatePitotTopic(timeUs_t currentTimeUs)
* Read surface and update alt/vel topic * Read surface and update alt/vel topic
* Function is called from TASK_RANGEFINDER at arbitrary rate - as soon as new measurements are available * Function is called from TASK_RANGEFINDER at arbitrary rate - as soon as new measurements are available
*/ */
#define RANGEFINDER_RELIABILITY_RC_CONSTANT (0.47802f)
#define RANGEFINDER_RELIABILITY_LIGHT_THRESHOLD (0.15f)
#define RANGEFINDER_RELIABILITY_LOW_THRESHOLD (0.33f)
#define RANGEFINDER_RELIABILITY_HIGH_THRESHOLD (0.75f)
void updatePositionEstimator_SurfaceTopic(timeUs_t currentTimeUs, float newSurfaceAlt) void updatePositionEstimator_SurfaceTopic(timeUs_t currentTimeUs, float newSurfaceAlt)
{ {
if (newSurfaceAlt > 0 && newSurfaceAlt <= positionEstimationConfig()->max_surface_altitude) { const float dt = US2S(currentTimeUs - posEstimator.surface.lastUpdateTime);
posEstimator.surface.alt = newSurfaceAlt; float newReliabilityMeasurement = 0;
posEstimator.surface.lastUpdateTime = currentTimeUs; posEstimator.surface.lastUpdateTime = currentTimeUs;
if (newSurfaceAlt >= 0) {
if (newSurfaceAlt <= positionEstimationConfig()->max_surface_altitude) {
newReliabilityMeasurement = 1.0f;
posEstimator.surface.alt = newSurfaceAlt;
}
else {
newReliabilityMeasurement = 0.0f;
}
}
else {
// Negative values - out of range or failed hardware
newReliabilityMeasurement = 0.0f;
}
/* Reliability is a measure of confidence of rangefinder measurement. It's increased with each valid sample and decreased with each invalid sample */
if (dt > 0.5f) {
posEstimator.surface.reliability = 0.0f;
}
else {
const float relAlpha = dt / (dt + RANGEFINDER_RELIABILITY_RC_CONSTANT);
posEstimator.surface.reliability = posEstimator.surface.reliability * (1.0f - relAlpha) + newReliabilityMeasurement * relAlpha;
} }
} }
#endif #endif
@ -831,32 +869,111 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
} }
} }
/* Surface offset */
#ifdef USE_RANGEFINDER
posEstimator.est.surface = posEstimator.est.surface + posEstimator.est.surfaceVel * dt;
if (isSurfaceValid) {
const float surfaceResidual = posEstimator.surface.alt - posEstimator.est.surface;
const float bellCurveScaler = scaleRangef(bellCurve(surfaceResidual, 50.0f), 0.0f, 1.0f, 0.1f, 1.0f);
posEstimator.est.surface += surfaceResidual * positionEstimationConfig()->w_z_surface_p * bellCurveScaler * dt;
posEstimator.est.surfaceVel += surfaceResidual * positionEstimationConfig()->w_z_surface_v * sq(bellCurveScaler) * dt;
posEstimator.est.surfaceValid = true;
}
else {
posEstimator.est.surfaceVel = 0; // Zero out velocity to prevent estimate to drift away
posEstimator.est.surfaceValid = false;
}
#else
posEstimator.est.surface = -1;
posEstimator.est.surfaceVel = 0;
posEstimator.est.surfaceValid = false;
#endif
/* Update uncertainty */ /* Update uncertainty */
posEstimator.est.eph = newEPH; posEstimator.est.eph = newEPH;
posEstimator.est.epv = newEPV; posEstimator.est.epv = newEPV;
#ifdef USE_RANGEFINDER
/* Surface offset */
if (isSurfaceValid) { // If surface topic is updated in timely manner - do something smart
navAGLEstimateQuality_e newAglQuality = posEstimator.est.aglQual;
bool resetSurfaceEstimate = false;
switch (posEstimator.est.aglQual) {
case SURFACE_QUAL_LOW:
if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_HIGH_THRESHOLD) {
newAglQuality = SURFACE_QUAL_HIGH;
resetSurfaceEstimate = true;
}
else if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_LOW_THRESHOLD) {
newAglQuality = SURFACE_QUAL_LOW;
}
else {
newAglQuality = SURFACE_QUAL_LOW;
}
break;
case SURFACE_QUAL_MID:
if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_HIGH_THRESHOLD) {
newAglQuality = SURFACE_QUAL_HIGH;
}
else if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_LOW_THRESHOLD) {
newAglQuality = SURFACE_QUAL_MID;
}
else {
newAglQuality = SURFACE_QUAL_LOW;
}
break;
case SURFACE_QUAL_HIGH:
if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_HIGH_THRESHOLD) {
newAglQuality = SURFACE_QUAL_HIGH;
}
else if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_LOW_THRESHOLD) {
newAglQuality = SURFACE_QUAL_MID;
}
else {
newAglQuality = SURFACE_QUAL_LOW;
}
break;
}
posEstimator.est.aglQual = newAglQuality;
if (resetSurfaceEstimate) {
posEstimator.est.aglAlt = posEstimator.surface.alt;
if (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv) {
posEstimator.est.aglVel = posEstimator.est.vel.V.Z;
posEstimator.est.aglOffset = posEstimator.est.pos.V.Z - posEstimator.surface.alt;
}
else {
posEstimator.est.aglVel = 0;
posEstimator.est.aglOffset = 0;
}
}
// Update estimate
posEstimator.est.aglAlt = posEstimator.est.aglAlt + posEstimator.est.aglVel * dt + posEstimator.imu.accelNEU.V.Z * dt * dt * 0.5f;
posEstimator.est.aglVel = posEstimator.est.aglVel + posEstimator.imu.accelNEU.V.Z * dt;
// Apply correction
if (posEstimator.est.aglQual == SURFACE_QUAL_HIGH) {
// Correct estimate from rangefinder
const float surfaceResidual = posEstimator.surface.alt - posEstimator.est.aglAlt;
const float bellCurveScaler = scaleRangef(bellCurve(surfaceResidual, 50.0f), 0.0f, 1.0f, 0.1f, 1.0f);
posEstimator.est.aglAlt += surfaceResidual * positionEstimationConfig()->w_z_surface_p * bellCurveScaler * posEstimator.surface.reliability * dt;
posEstimator.est.aglVel += surfaceResidual * positionEstimationConfig()->w_z_surface_v * sq(bellCurveScaler) * sq(posEstimator.surface.reliability) * dt;
// Update estimate offset
if ((posEstimator.est.aglQual == SURFACE_QUAL_HIGH) && (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv)) {
posEstimator.est.aglOffset = posEstimator.est.pos.V.Z - posEstimator.surface.alt;
}
}
else if (posEstimator.est.aglQual == SURFACE_QUAL_MID) {
// Correct estimate from altitude fused from rangefinder and global altitude
const float estAltResidual = (posEstimator.est.pos.V.Z - posEstimator.est.aglOffset) - posEstimator.est.aglAlt;
const float surfaceResidual = posEstimator.surface.alt - posEstimator.est.aglAlt;
const float surfaceWeightScaler = scaleRangef(bellCurve(surfaceResidual, 50.0f), 0.0f, 1.0f, 0.1f, 1.0f) * posEstimator.surface.reliability;
const float mixedResidual = surfaceResidual * surfaceWeightScaler + estAltResidual * (1.0f - surfaceWeightScaler);
posEstimator.est.aglAlt += mixedResidual * positionEstimationConfig()->w_z_surface_p * dt;
posEstimator.est.aglVel += mixedResidual * positionEstimationConfig()->w_z_surface_v * dt;
}
else { // SURFACE_QUAL_LOW
// In this case rangefinder can't be trusted - simply use global altitude
posEstimator.est.aglAlt = posEstimator.est.pos.V.Z - posEstimator.est.aglOffset;
posEstimator.est.aglVel = posEstimator.est.vel.V.Z;
}
}
else {
posEstimator.est.aglVel = 0;
posEstimator.est.aglQual = SURFACE_QUAL_LOW;
}
#else
posEstimator.est.aglAlt = posEstimator.est.pos.V.Z;
posEstimator.est.aglVel = posEstimator.est.vel.V.Z;
posEstimator.est.aglQual = SURFACE_QUAL_LOW;
#endif
} }
/** /**
@ -882,15 +999,15 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs)
/* Publish altitude update and set altitude validity */ /* Publish altitude update and set altitude validity */
if (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv) { if (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv) {
const bool aglReliable = (posEstimator.est.aglQual == SURFACE_QUAL_HIGH);
updateActualAltitudeAndClimbRate(true, posEstimator.est.pos.V.Z, posEstimator.est.vel.V.Z); updateActualAltitudeAndClimbRate(true, posEstimator.est.pos.V.Z, posEstimator.est.vel.V.Z);
updateActualAGLAndClimgRate(true, aglReliable, posEstimator.est.aglAlt, posEstimator.est.aglVel);
} }
else { else {
updateActualAltitudeAndClimbRate(false, posEstimator.est.pos.V.Z, 0); updateActualAltitudeAndClimbRate(false, posEstimator.est.pos.V.Z, 0);
updateActualAGLAndClimgRate(false, false, posEstimator.est.aglAlt, 0);
} }
/* Publish surface distance */
updateActualSurfaceDistance(posEstimator.est.surfaceValid, posEstimator.est.surface, posEstimator.est.surfaceVel);
/* Store history data */ /* Store history data */
posEstimator.history.pos[posEstimator.history.index] = posEstimator.est.pos; posEstimator.history.pos[posEstimator.history.index] = posEstimator.est.pos;
posEstimator.history.vel[posEstimator.history.index] = posEstimator.est.vel; posEstimator.history.vel[posEstimator.history.index] = posEstimator.est.vel;
@ -928,8 +1045,8 @@ void initializePositionEstimator(void)
posEstimator.baro.lastUpdateTime = 0; posEstimator.baro.lastUpdateTime = 0;
posEstimator.surface.lastUpdateTime = 0; posEstimator.surface.lastUpdateTime = 0;
posEstimator.est.surface = 0; posEstimator.est.aglAlt = 0;
posEstimator.est.surfaceVel = 0; posEstimator.est.aglVel = 0;
posEstimator.history.index = 0; posEstimator.history.index = 0;

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

@ -56,6 +56,12 @@ typedef enum {
ROC_TO_ALT_NORMAL ROC_TO_ALT_NORMAL
} climbRateToAltitudeControllerMode_e; } climbRateToAltitudeControllerMode_e;
typedef enum {
EST_NONE = 0, // No valid sensor present
EST_USABLE = 1, // Estimate is usable but may be inaccurate
EST_TRUSTED = 2 // Estimate is usable and based on actual sensor data
} navigationEstimateStatus_e;
typedef struct navigationFlags_s { typedef struct navigationFlags_s {
bool horizontalPositionDataNew; bool horizontalPositionDataNew;
bool verticalPositionDataNew; bool verticalPositionDataNew;
@ -65,10 +71,10 @@ typedef struct navigationFlags_s {
bool horizontalPositionDataConsumed; bool horizontalPositionDataConsumed;
bool verticalPositionDataConsumed; bool verticalPositionDataConsumed;
bool hasValidAltitudeSensor; // Indicates that we have a working altitude sensor (got at least one valid reading from it) navigationEstimateStatus_e estAltStatus; // Indicates that we have a working altitude sensor (got at least one valid reading from it)
bool hasValidPositionSensor; // Indicates that GPS is working (or not) navigationEstimateStatus_e estPosStatue; // Indicates that GPS is working (or not)
bool hasValidSurfaceSensor; navigationEstimateStatus_e estHeadingStatus; // Indicate valid heading - wither mag or GPS at certain speed on airplane
bool hasValidHeadingSensor; // Indicate valid heading - wither mag or GPS at certain speed on airplane navigationEstimateStatus_e estSurfaceStatus;
bool isAdjustingPosition; bool isAdjustingPosition;
bool isAdjustingAltitude; bool isAdjustingAltitude;
@ -321,9 +327,9 @@ bool isApproachingLastWaypoint(void);
float getActiveWaypointSpeed(void); float getActiveWaypointSpeed(void);
void updateActualHeading(int32_t newHeading); void updateActualHeading(int32_t newHeading);
void updateActualHorizontalPositionAndVelocity(bool hasValidSensor, float newX, float newY, float newVelX, float newVelY); void updateActualHorizontalPositionAndVelocity(bool estimateValid, float newX, float newY, float newVelX, float newVelY);
void updateActualAltitudeAndClimbRate(bool hasValidSensor, float newAltitude, float newVelocity); void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity);
void updateActualSurfaceDistance(bool hasValidSensor, float surfaceDistance, float surfaceVelocity); void updateActualAGLAndClimgRate(bool estimateValid, bool estimateReliable, float surfaceDistance, float surfaceVelocity);
bool checkForPositionSensorTimeout(void); bool checkForPositionSensorTimeout(void);