Merge pull request #4196 from iNavFlight/de_agl_fix

Fix non-working AGL estimation

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;
 }

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);
 }
 
 /**

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.est.aglVel * ctx->dt;
-        posEstimator.est.aglAlt = posEstimator.imu.accelNEU.z * sq(ctx->dt) * 0.5f;
+        posEstimator.est.aglAlt += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f;
-        posEstimator.est.aglVel = posEstimator.est.aglVel + posEstimator.imu.accelNEU.z * ctx->dt;
+        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) {

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;