Remove unused filterWithBuffer code. Not useful anymore as inertial filter alone calculate velocity with sufficient accuracy

src/main/common/maths.c

@@ -304,96 +304,6 @@ void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count)
     }
 }
 
-// Base functions for FIR filters
-void filterWithBufferReset(filterWithBufferState_t * state)
-{
-    int i;
-    state->sample_index = 0;
-    for (i = 0; i < state->filter_size; i++) {
-        state->samples[i].value = 0.0f;
-        state->samples[i].timestamp = 0;
-    }
-}
-
-void filterWithBufferInit(filterWithBufferState_t * state, filterWithBufferSample_t * buffer, uint16_t size)
-{
-    state->samples = buffer;
-    state->filter_size = size;
-    filterWithBufferReset(state);
-}
-
-void filterWithBufferUpdate(filterWithBufferState_t * state, float sample, uint32_t timestamp)
-{
-    uint16_t old_sample_index;
-
-    if (state->sample_index == 0)
-        old_sample_index = state->filter_size - 1;
-    else
-        old_sample_index = state->sample_index - 1;
-
-    if (state->samples[old_sample_index].timestamp  == timestamp) {
-        return;
-    }
-
-    state->samples[state->sample_index].timestamp = timestamp;
-    state->samples[state->sample_index].value = sample;
-    state->sample_index++;
-
-    if (state->sample_index >= state->filter_size)
-        state->sample_index = 0;
-}
-
-// An implementation of a derivative (slope) filter
-// Code mostly taken from ArduPilot's DerivativeFilter.cpp
-// http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/
-float filterWithBufferApply_Derivative(filterWithBufferState_t * state)
-{
-    float result = 0;
-
-    // use f() to make the code match the maths a bit better. Note
-    // that unlike an average filter, we care about the order of the elements
-    #define f(i) (state->samples[(((state->sample_index - 1) + i + 1) + 3 * state->filter_size / 2) % state->filter_size].value)
-    #define x(i) (state->samples[(((state->sample_index - 1) + i + 1) + 3 * state->filter_size / 2) % state->filter_size].timestamp)
-
-    if (state->samples[state->filter_size - 1].timestamp == state->samples[state->filter_size - 2].timestamp) {
-        return 0;
-    }
-
-    switch (state->filter_size) {
-    case 5:
-        result = 2*2*(f(1) - f(-1)) / (x(1) - x(-1))
-                 + 4*1*(f(2) - f(-2)) / (x(2) - x(-2));
-        result /= 8;
-        break;
-    case 7:
-        result = 2*5*(f(1) - f(-1)) / (x(1) - x(-1))
-                 + 4*4*(f(2) - f(-2)) / (x(2) - x(-2))
-                 + 6*1*(f(3) - f(-3)) / (x(3) - x(-3));
-        result /= 32;
-        break;
-    case 9:
-        result = 2*14*(f(1) - f(-1)) / (x(1) - x(-1))
-                 + 4*14*(f(2) - f(-2)) / (x(2) - x(-2))
-                 + 6* 6*(f(3) - f(-3)) / (x(3) - x(-3))
-                 + 8* 1*(f(4) - f(-4)) / (x(4) - x(-4));
-        result /= 128;
-        break;
-    case 11:
-        result =  2*42*(f(1) - f(-1)) / (x(1) - x(-1))
-                 +  4*48*(f(2) - f(-2)) / (x(2) - x(-2))
-                 +  6*27*(f(3) - f(-3)) / (x(3) - x(-3))
-                 +  8* 8*(f(4) - f(-4)) / (x(4) - x(-4))
-                 + 10* 1*(f(5) - f(-5)) / (x(5) - x(-5));
-        result /= 512;
-        break;
-    default:
-        result = 0;
-        break;
-    }
-
-    return result;
-}
-
 /**
  * Sensor offset calculation code based on Freescale's AN4246
  * Initial implementation by @HaukeRa

src/main/common/maths.h

@@ -131,11 +131,6 @@ int32_t quickMedianFilter5(int32_t * v);
 int32_t quickMedianFilter7(int32_t * v);
 int32_t quickMedianFilter9(int32_t * v);
 
-void filterWithBufferReset(filterWithBufferState_t * state);
-void filterWithBufferInit(filterWithBufferState_t * state, filterWithBufferSample_t * buffer, uint16_t size);
-void filterWithBufferUpdate(filterWithBufferState_t * state, float sample, uint32_t timestamp);
-float filterWithBufferApply_Derivative(filterWithBufferState_t * state);
-
 #if defined(FAST_MATH) || defined(VERY_FAST_MATH)
 float sin_approx(float x);
 float cos_approx(float x);

src/main/flight/navigation_rewrite_pos_estimator.c

@@ -62,13 +62,11 @@
 
 #define INAV_POSITION_PUBLISH_RATE_HZ       50      // Publish position updates at this rate
 #define INAV_BARO_UPDATE_RATE               20
-#define INAV_SONAR_UPDATE_RATE              10      // Sonar is limited to 1/60ms update rate, go lower that that
+#define INAV_SONAR_UPDATE_RATE              15      // Sonar is limited to 1/60ms update rate, go lower that that
 
 #define INAV_GPS_TIMEOUT_MS                 1500    // GPS timeout
 #define INAV_BARO_TIMEOUT_MS                200     // Baro timeout
-#define INAV_SONAR_TIMEOUT_MS               250     // Sonar timeout    (missed two readings in a row
+#define INAV_SONAR_TIMEOUT_MS               200     // Sonar timeout    (missed 3 readings in a row)
-
-#define INAV_BARO_CLIMB_RATE_FILTER_SIZE    7
 
 #define INAV_SONAR_W1                       0.8461f // Sonar predictive filter gain for altitude
 #define INAV_SONAR_W2                       6.2034f // Sonar predictive filter gain for velocity
@@ -97,7 +95,6 @@ typedef struct {
 typedef struct {
     uint32_t    lastUpdateTime; // Last update time (us)
     float       alt;            // Raw barometric altitude (cm)
-    float       vel;            // Altitude derivative - velocity (cms)
     float       epv;
 } navPositionEstimatorBARO_t;
 
@@ -345,39 +342,16 @@ void onNewGPSData(int32_t newLat, int32_t newLon, int32_t newAlt, int16_t velN,
 static void updateBaroTopic(uint32_t currentTime)
 {
     static navigationTimer_t baroUpdateTimer;
-    /*
-    static filterWithBufferSample_t baroClimbRateFilterBuffer[INAV_BARO_CLIMB_RATE_FILTER_SIZE];
-    static filterWithBufferState_t baroClimbRateFilter;
-    static bool climbRateFiltersInitialized = false;
-    */
 
     if (updateTimer(&baroUpdateTimer, HZ2US(INAV_BARO_UPDATE_RATE), currentTime)) {
-        /*
-        if (!climbRateFiltersInitialized) {
-            filterWithBufferInit(&baroClimbRateFilter, &baroClimbRateFilterBuffer[0], INAV_BARO_CLIMB_RATE_FILTER_SIZE);
-            climbRateFiltersInitialized = true;
-        }
-        */
-
         float newBaroAlt = baroCalculateAltitude();
         if (sensors(SENSOR_BARO) && isBaroCalibrationComplete()) {
-            /*
-            filterWithBufferUpdate(&baroClimbRateFilter, newBaroAlt, currentTime);
-            float baroClimbRate = filterWithBufferApply_Derivative(&baroClimbRateFilter) * 1e6f;
-
-            // FIXME: do we need this?
-            baroClimbRate = constrainf(baroClimbRate, -1500, 1500);  // constrain baro velocity +/- 1500cm/s
-            baroClimbRate = applyDeadband(baroClimbRate, 10);       // to reduce noise near zero
-            */
-
             posEstimator.baro.alt = newBaroAlt;
-            posEstimator.baro.vel = 0; //baroClimbRate;
             posEstimator.baro.epv = posControl.navConfig->inav.baro_epv;
             posEstimator.baro.lastUpdateTime = currentTime;
         }
         else {
             posEstimator.baro.alt = 0;
-            posEstimator.baro.vel = 0;
             posEstimator.baro.lastUpdateTime = 0;
         }
     }
@@ -507,7 +481,7 @@ static void updateEstimatedTopic(uint32_t currentTime)
     /* Apply GPS altitude corrections only on fixed wing aircrafts */
     bool useGpsZ = STATE(FIXED_WING) && isGPSValid;
 
-    /* Pre-calculate history index for GPS & baro delay compensation */
+    /* Pre-calculate history index for GPS delay compensation */
     int gpsHistoryIndex = (posEstimator.history.index - 1) - constrain(((int)posControl.navConfig->inav.gps_delay_ms / (1000 / INAV_POSITION_PUBLISH_RATE_HZ)), 0, INAV_HISTORY_BUF_SIZE - 1);
     if (gpsHistoryIndex < 0) {
         gpsHistoryIndex += INAV_HISTORY_BUF_SIZE;
@@ -535,7 +509,6 @@ static void updateEstimatedTopic(uint32_t currentTime)
         /* accelerometer bias correction for baro */
         if (isBaroValid) {
             accelBiasCorr.V.Z -= (posEstimator.baro.alt - posEstimator.est.pos.V.Z) * sq(posControl.navConfig->inav.w_z_baro_p);
-            //accelBiasCorr.V.Z -= (posEstimator.baro.vel - posEstimator.est.vel.V.Z) * posControl.navConfig->inav.w_z_baro_v;
         }
 
         /* transform error vector from NEU frame to body frame */
@@ -548,7 +521,6 @@ static void updateEstimatedTopic(uint32_t currentTime)
     }
 
     /* Estimate Z-axis */
-    /* FIXME: baro & sonar delay compensation (~100ms for sonar, about 500ms for baro) */
     if ((posEstimator.est.epv < posControl.navConfig->inav.max_eph_epv) || useGpsZ || isBaroValid) {
         /* Predict position/velocity based on acceleration */
         inavFilterPredict(Z, dt, posEstimator.imu.accelNEU.V.Z);
@@ -557,7 +529,6 @@ static void updateEstimatedTopic(uint32_t currentTime)
         if (isBaroValid) {
             /* Apply only baro correction, no sonar */
             inavFilterCorrectPos(Z, dt, posEstimator.baro.alt - posEstimator.est.pos.V.Z, posControl.navConfig->inav.w_z_baro_p);
-            //inavFilterCorrectVel(Z, dt, posEstimator.baro.vel - posEstimator.est.vel.V.Z, posControl.navConfig->inav.w_z_baro_v);
 
             /* Adjust EPV */
             posEstimator.est.epv = MIN(posEstimator.est.epv, posEstimator.baro.epv);