diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index e337a99328..891b7534cb 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -696,11 +696,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS(nav 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; } @@ -779,11 +774,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS( 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; } @@ -791,7 +781,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati { 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 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 (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 if ((prevFlags & NAV_CTL_POS) == 0) { resetPositionController(); @@ -854,12 +844,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n { UNUSED(previousState); - if (!posControl.flags.hasValidHeadingSensor) { + if ((posControl.flags.estHeadingStatus == EST_NONE)) { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } // 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) ? 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 @@ -906,12 +896,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio { UNUSED(previousState); - if (!posControl.flags.hasValidHeadingSensor) { + if ((posControl.flags.estHeadingStatus == EST_NONE)) { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } // If we have position sensor - continue home - if (posControl.flags.hasValidPositionSensor) { + if ((posControl.flags.estPosStatue >= EST_USABLE)) { if (isWaypointReached(&posControl.homeWaypointAbove, true)) { // 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); @@ -946,12 +936,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND { UNUSED(previousState); - if (!posControl.flags.hasValidHeadingSensor) { + if ((posControl.flags.estHeadingStatus == EST_NONE)) { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } // 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) if (STATE(FIXED_WING)) { resetLandingDetector(); @@ -997,7 +987,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF 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.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 // 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); @@ -1080,7 +1070,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na UNUSED(previousState); // 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); switch (posControl.waypointList[posControl.activeWaypointIndex].action) { @@ -1164,7 +1154,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED(navig UNUSED(previousState); // 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; } /* No pos sensor available for NAV_WAIT_FOR_GPS_TIMEOUT_MS - land */ @@ -1435,7 +1425,7 @@ bool isThrustFacingDownwards(void) bool checkForPositionSensorTimeout(void) { 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; } else { @@ -1451,7 +1441,7 @@ bool checkForPositionSensorTimeout(void) /*----------------------------------------------------------- * 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.Y = newY; @@ -1460,17 +1450,18 @@ void updateActualHorizontalPositionAndVelocity(bool hasValidSensor, float newX, posControl.actualState.vel.V.Y = newVelY; posControl.actualState.velXY = sqrtf(sq(newVelX) + sq(newVelY)); - posControl.flags.hasValidPositionSensor = hasValidSensor; - posControl.flags.hasValidHeadingSensor = isImuHeadingValid(); - - if (hasValidSensor) { + if (estimateValid) { + posControl.flags.estPosStatue = EST_TRUSTED; posControl.flags.horizontalPositionDataNew = 1; posControl.lastValidPositionTimeMs = millis(); } else { + posControl.flags.estPosStatue = EST_NONE; posControl.flags.horizontalPositionDataNew = 0; } + posControl.flags.estHeadingStatus = isImuHeadingValid() ? EST_TRUSTED : EST_NONE; + #if defined(NAV_BLACKBOX) navLatestActualPosition[X] = newX; navLatestActualPosition[Y] = newY; @@ -1482,20 +1473,20 @@ void updateActualHorizontalPositionAndVelocity(bool hasValidSensor, float newX, /*----------------------------------------------------------- * 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.vel.V.Z = newVelocity; - posControl.flags.hasValidAltitudeSensor = hasValidSensor; - // Update altitude that would be used when executing RTH - if (hasValidSensor) { + if (estimateValid) { updateDesiredRTHAltitude(); + posControl.flags.estAltStatus = EST_TRUSTED; posControl.flags.verticalPositionDataNew = 1; posControl.lastValidAltitudeTimeMs = millis(); } else { + posControl.flags.estAltStatus = EST_NONE; posControl.flags.verticalPositionDataNew = 0; } @@ -1508,25 +1499,24 @@ void updateActualAltitudeAndClimbRate(bool hasValidSensor, float newAltitude, fl /*----------------------------------------------------------- * 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.surfaceVel = surfaceVelocity; // Update validity - // Update validity - if (hasValidSensor) { - posControl.flags.hasValidSurfaceSensor = true; + if (estimateValid) { + posControl.flags.estSurfaceStatus = estimateReliable ? EST_TRUSTED : EST_USABLE; posControl.flags.surfaceDistanceDataNew = 1; } else { - posControl.flags.hasValidSurfaceSensor = false; + posControl.flags.estSurfaceStatus = EST_NONE; posControl.flags.surfaceDistanceDataNew = 0; } // Update minimum surface distance (landing detection threshold) 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) { 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 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 ); } } @@ -1732,7 +1722,7 @@ void updateHomePosition(void) // If pilot so desires he may reset home position to current position 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); setHomePosition(&posControl.actualState.pos, posControl.actualState.yaw, homeUpdateFlags); isHomeResetAllowed = false; @@ -1771,23 +1761,6 @@ int32_t getTotalTravelDistance(void) 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) *-----------------------------------------------------------*/ @@ -2040,7 +2013,7 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData) wpLLH.alt = wpData->alt; // 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 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); @@ -2048,7 +2021,7 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData) // WP #255 - special waypoint - directly set desiredPosition // Only valid when armed and in poshold mode 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)) { // Convert to local coordinates geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, &wpPos.pos, GEO_ALT_RELATIVE); @@ -2265,14 +2238,13 @@ void applyWaypointNavigationAndAltitudeHold(void) #if defined(NAV_BLACKBOX) navFlags = 0; - if (posControl.flags.hasValidAltitudeSensor) navFlags |= (1 << 0); - if (posControl.flags.hasValidSurfaceSensor) navFlags |= (1 << 1); - if (posControl.flags.hasValidPositionSensor) navFlags |= (1 << 2); - //if (STATE(GPS_FIX)) navFlags |= (1 << 3); + if (posControl.flags.estAltStatus == EST_TRUSTED) navFlags |= (1 << 0); + if (posControl.flags.estSurfaceStatus == EST_TRUSTED) navFlags |= (1 << 1); + if (posControl.flags.estPosStatue == EST_TRUSTED) navFlags |= (1 << 2); #if defined(NAV_GPS_GLITCH_DETECTION) - if (isGPSGlitchDetected()) navFlags |= (1 << 4); + if (isGPSGlitchDetected()) navFlags |= (1 << 4); #endif - if (posControl.flags.hasValidHeadingSensor) navFlags |= (1 << 5); + if (posControl.flags.estHeadingStatus == EST_TRUSTED) navFlags |= (1 << 5); #endif // Reset all navigation requests - NAV controllers will set them if necessary @@ -2334,12 +2306,12 @@ void swithNavigationFlightModes(void) *-----------------------------------------------------------*/ static bool canActivateAltHoldMode(void) { - return posControl.flags.hasValidAltitudeSensor; + return (posControl.flags.estAltStatus >= EST_USABLE); } 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) @@ -2487,7 +2459,7 @@ bool navigationBlockArming(void) return false; // 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; } @@ -2514,7 +2486,7 @@ bool navigationBlockArming(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.headingDataNew = 0; - posControl.flags.hasValidAltitudeSensor = 0; - posControl.flags.hasValidPositionSensor = 0; - posControl.flags.hasValidSurfaceSensor = 0; - posControl.flags.hasValidHeadingSensor = 0; + posControl.flags.estAltStatus = EST_NONE; + posControl.flags.estPosStatue = EST_NONE; + posControl.flags.estHeadingStatus = EST_NONE; + posControl.flags.estSurfaceStatus = EST_NONE; posControl.flags.forcedRTHActivated = 0; posControl.waypointCount = 0; diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 961b1cc94a..4bcae7567b 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -151,7 +151,7 @@ void applyFixedWingAltitudeAndThrottleController(timeUs_t currentTimeUs) return; } - if (posControl.flags.hasValidAltitudeSensor) { + if ((posControl.flags.estAltStatus >= EST_USABLE)) { if (posControl.flags.verticalPositionDataNew) { const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; 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 - if (posControl.flags.hasValidPositionSensor) { + if ((posControl.flags.estPosStatue >= EST_USABLE)) { // If we have new position - update velocity and acceleration controllers if (posControl.flags.horizontalPositionDataNew) { const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; @@ -378,7 +378,7 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs) } // 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 (posControl.flags.horizontalPositionDataNew) { 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 */ 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 */ @@ -481,7 +482,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat 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; diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index b97b9bd6b5..c663af3faf 100755 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -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 // 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 (posControl.flags.horizontalPositionDataNew) { 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 (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: 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 @@ -540,9 +540,7 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs) rcCommand[PITCH] = 0; rcCommand[YAW] = 0; - if (posControl.flags.hasValidAltitudeSensor) { - /* We have an altitude reference, apply AH-based landing controller */ - + if ((posControl.flags.estAltStatus >= EST_USABLE)) { // If last position update was too long in the past - ignore it (likely restarting altitude controller) if (deltaMicros > HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) { previousTimeUpdate = currentTimeUs; diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 00082304e8..115a27c4fe 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -109,9 +109,16 @@ typedef struct { float airspeed; // airspeed (cm/s) } 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 { timeUs_t lastUpdateTime; // Last update time (us) float alt; // Raw altitude measurement (cm) + float reliability; } navPositionEstimatorSURFACE_t; typedef struct { @@ -124,15 +131,18 @@ typedef struct { typedef struct { timeUs_t lastUpdateTime; // Last update time (us) + // 3D position, velocity and confidence t_fp_vector pos; t_fp_vector vel; float eph; float epv; - // Surface offset - float surface; - float surfaceVel; - bool surfaceValid; + + // AGL + navAGLEstimateQuality_e aglQual; + float aglOffset; // Offset between surface and pos.Z + float aglAlt; + float aglVel; } navPositionEstimatorESTIMATE_t; typedef struct { @@ -484,11 +494,39 @@ static void updatePitotTopic(timeUs_t currentTimeUs) * Read surface and update alt/vel topic * 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) { - if (newSurfaceAlt > 0 && newSurfaceAlt <= positionEstimationConfig()->max_surface_altitude) { - posEstimator.surface.alt = newSurfaceAlt; - posEstimator.surface.lastUpdateTime = currentTimeUs; + const float dt = US2S(currentTimeUs - posEstimator.surface.lastUpdateTime); + float newReliabilityMeasurement = 0; + + 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 @@ -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 */ posEstimator.est.eph = newEPH; 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 */ 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); + updateActualAGLAndClimgRate(true, aglReliable, posEstimator.est.aglAlt, posEstimator.est.aglVel); } else { 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 */ posEstimator.history.pos[posEstimator.history.index] = posEstimator.est.pos; posEstimator.history.vel[posEstimator.history.index] = posEstimator.est.vel; @@ -928,8 +1045,8 @@ void initializePositionEstimator(void) posEstimator.baro.lastUpdateTime = 0; posEstimator.surface.lastUpdateTime = 0; - posEstimator.est.surface = 0; - posEstimator.est.surfaceVel = 0; + posEstimator.est.aglAlt = 0; + posEstimator.est.aglVel = 0; posEstimator.history.index = 0; diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 9024ccf2b7..f91505cbc8 100755 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -56,6 +56,12 @@ typedef enum { ROC_TO_ALT_NORMAL } 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 { bool horizontalPositionDataNew; bool verticalPositionDataNew; @@ -65,10 +71,10 @@ typedef struct navigationFlags_s { bool horizontalPositionDataConsumed; bool verticalPositionDataConsumed; - bool hasValidAltitudeSensor; // 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) - bool hasValidSurfaceSensor; - bool hasValidHeadingSensor; // Indicate valid heading - wither mag or GPS at certain speed on airplane + navigationEstimateStatus_e estAltStatus; // Indicates that we have a working altitude sensor (got at least one valid reading from it) + navigationEstimateStatus_e estPosStatue; // Indicates that GPS is working (or not) + navigationEstimateStatus_e estHeadingStatus; // Indicate valid heading - wither mag or GPS at certain speed on airplane + navigationEstimateStatus_e estSurfaceStatus; bool isAdjustingPosition; bool isAdjustingAltitude; @@ -321,9 +327,9 @@ bool isApproachingLastWaypoint(void); float getActiveWaypointSpeed(void); void updateActualHeading(int32_t newHeading); -void updateActualHorizontalPositionAndVelocity(bool hasValidSensor, float newX, float newY, float newVelX, float newVelY); -void updateActualAltitudeAndClimbRate(bool hasValidSensor, float newAltitude, float newVelocity); -void updateActualSurfaceDistance(bool hasValidSensor, float surfaceDistance, float surfaceVelocity); +void updateActualHorizontalPositionAndVelocity(bool estimateValid, float newX, float newY, float newVelX, float newVelY); +void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity); +void updateActualAGLAndClimgRate(bool estimateValid, bool estimateReliable, float surfaceDistance, float surfaceVelocity); bool checkForPositionSensorTimeout(void);