From 0124be5efb5893102f19610609f3f20338eea23d Mon Sep 17 00:00:00 2001 From: "Konstantin Sharlaimov (DigitalEntity)" Date: Wed, 5 Sep 2018 19:21:20 +0200 Subject: [PATCH] [AGL] Fix non-working AGL estimation --- src/main/common/filter.c | 1 + .../navigation/navigation_pos_estimator.c | 9 +++++++ .../navigation/navigation_pos_estimator_agl.c | 25 +++++++++++++------ .../navigation_pos_estimator_private.h | 9 ++++++- 4 files changed, 35 insertions(+), 9 deletions(-) diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 1872d81dfd..a2a7551cdb 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -40,6 +40,7 @@ float nullFilterApply(void *filter, float input) // f_cut = cutoff frequency void pt1FilterInitRC(pt1Filter_t *filter, float tau, float dT) { + filter->state = 0.0f; filter->RC = tau; filter->dT = dT; } diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index c4fbeb4669..9993832b9c 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -309,9 +309,15 @@ void updatePositionEstimator_BaroTopic(timeUs_t currentTimeUs) } if (sensors(SENSOR_BARO) && baroIsCalibrationComplete()) { + const timeUs_t baroDtUs = currentTimeUs - posEstimator.baro.lastUpdateTime; + posEstimator.baro.alt = newBaroAlt - initialBaroAltitudeOffset; posEstimator.baro.epv = positionEstimationConfig()->baro_epv; posEstimator.baro.lastUpdateTime = currentTimeUs; + + if (baroDtUs <= MS2US(INAV_BARO_TIMEOUT_MS)) { + pt1FilterApply3(&posEstimator.baro.avgFilter, posEstimator.baro.alt, US2S(baroDtUs)); + } } else { posEstimator.baro.alt = 0; @@ -769,6 +775,9 @@ void initializePositionEstimator(void) posEstimator.est.pos.v[axis] = 0; posEstimator.est.vel.v[axis] = 0; } + + pt1FilterInit(&posEstimator.baro.avgFilter, INAV_BARO_AVERAGE_HZ, 0.0f); + pt1FilterInit(&posEstimator.surface.avgFilter, INAV_SURFACE_AVERAGE_HZ, 0.0f); } /** diff --git a/src/main/navigation/navigation_pos_estimator_agl.c b/src/main/navigation/navigation_pos_estimator_agl.c index cba35d4069..21c09909de 100644 --- a/src/main/navigation/navigation_pos_estimator_agl.c +++ b/src/main/navigation/navigation_pos_estimator_agl.c @@ -50,14 +50,16 @@ extern navigationPosEstimator_t posEstimator; */ void updatePositionEstimator_SurfaceTopic(timeUs_t currentTimeUs, float newSurfaceAlt) { - const float dt = US2S(currentTimeUs - posEstimator.surface.lastUpdateTime); + const float surfaceDtUs = currentTimeUs - posEstimator.surface.lastUpdateTime; float newReliabilityMeasurement = 0; + bool surfaceMeasurementWithinRange = false; posEstimator.surface.lastUpdateTime = currentTimeUs; if (newSurfaceAlt >= 0) { if (newSurfaceAlt <= positionEstimationConfig()->max_surface_altitude) { newReliabilityMeasurement = 1.0f; + surfaceMeasurementWithinRange = true; posEstimator.surface.alt = newSurfaceAlt; } else { @@ -70,12 +72,18 @@ void updatePositionEstimator_SurfaceTopic(timeUs_t currentTimeUs, float newSurfa } /* 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) { + if (surfaceDtUs > MS2US(INAV_SURFACE_TIMEOUT_MS)) { posEstimator.surface.reliability = 0.0f; } else { - const float relAlpha = dt / (dt + RANGEFINDER_RELIABILITY_RC_CONSTANT); + const float surfaceDt = US2S(surfaceDtUs); + const float relAlpha = surfaceDt / (surfaceDt + RANGEFINDER_RELIABILITY_RC_CONSTANT); posEstimator.surface.reliability = posEstimator.surface.reliability * (1.0f - relAlpha) + newReliabilityMeasurement * relAlpha; + + // Update average sonar altitude if range is good + if (surfaceMeasurementWithinRange) { + pt1FilterApply3(&posEstimator.surface.avgFilter, newSurfaceAlt, surfaceDt); + } } } #endif @@ -128,7 +136,8 @@ void estimationCalculateAGL(estimationContext_t * ctx) posEstimator.est.aglQual = newAglQuality; if (resetSurfaceEstimate) { - posEstimator.est.aglAlt = posEstimator.surface.alt; + posEstimator.est.aglAlt = pt1FilterGetLastOutput(&posEstimator.surface.avgFilter); + // If we have acceptable average estimate if (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv) { posEstimator.est.aglVel = posEstimator.est.vel.z; posEstimator.est.aglOffset = posEstimator.est.pos.z - posEstimator.surface.alt; @@ -140,9 +149,9 @@ void estimationCalculateAGL(estimationContext_t * ctx) } // Update estimate - posEstimator.est.aglAlt = posEstimator.est.aglAlt + posEstimator.est.aglVel * ctx->dt; - posEstimator.est.aglAlt = posEstimator.imu.accelNEU.z * sq(ctx->dt) * 0.5f; - posEstimator.est.aglVel = posEstimator.est.aglVel + posEstimator.imu.accelNEU.z * ctx->dt; + posEstimator.est.aglAlt += posEstimator.est.aglVel * ctx->dt; + posEstimator.est.aglAlt += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f; + posEstimator.est.aglVel += posEstimator.imu.accelNEU.z * ctx->dt; // Apply correction if (posEstimator.est.aglQual == SURFACE_QUAL_HIGH) { @@ -155,7 +164,7 @@ void estimationCalculateAGL(estimationContext_t * ctx) // Update estimate offset if ((posEstimator.est.aglQual == SURFACE_QUAL_HIGH) && (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv)) { - posEstimator.est.aglOffset = posEstimator.est.pos.z - posEstimator.surface.alt; + posEstimator.est.aglOffset = posEstimator.est.pos.z - pt1FilterGetLastOutput(&posEstimator.surface.avgFilter); } } else if (posEstimator.est.aglQual == SURFACE_QUAL_MID) { diff --git a/src/main/navigation/navigation_pos_estimator_private.h b/src/main/navigation/navigation_pos_estimator_private.h index 6b2b6c5d68..8fe83e70d1 100644 --- a/src/main/navigation/navigation_pos_estimator_private.h +++ b/src/main/navigation/navigation_pos_estimator_private.h @@ -22,6 +22,7 @@ #include "common/axis.h" #include "common/maths.h" +#include "common/filter.h" #include "sensors/sensors.h" @@ -40,9 +41,13 @@ #define INAV_GPS_TIMEOUT_MS 1500 // GPS timeout #define INAV_BARO_TIMEOUT_MS 200 // Baro timeout -#define INAV_SURFACE_TIMEOUT_MS 300 // Surface timeout (missed 3 readings in a row) +#define INAV_SURFACE_TIMEOUT_MS 400 // Surface timeout (missed 3 readings in a row) #define INAV_FLOW_TIMEOUT_MS 200 +// Time constants for calculating Baro/Sonar averages. Should be the same value to impose same amount of group delay +#define INAV_BARO_AVERAGE_HZ 1.0f +#define INAV_SURFACE_AVERAGE_HZ 1.0f + #define RANGEFINDER_RELIABILITY_RC_CONSTANT (0.47802f) #define RANGEFINDER_RELIABILITY_LIGHT_THRESHOLD (0.15f) #define RANGEFINDER_RELIABILITY_LOW_THRESHOLD (0.33f) @@ -67,6 +72,7 @@ typedef struct { typedef struct { timeUs_t lastUpdateTime; // Last update time (us) + pt1Filter_t avgFilter; float alt; // Raw barometric altitude (cm) float epv; } navPositionEstimatorBARO_t; @@ -84,6 +90,7 @@ typedef enum { typedef struct { timeUs_t lastUpdateTime; // Last update time (us) + pt1Filter_t avgFilter; float alt; // Raw altitude measurement (cm) float reliability; } navPositionEstimatorSURFACE_t;