BARO: Remove averaging and extra LPF filtering. Changed defaults for sensor fusion for INAV

src/main/config/config.c

@@ -134,7 +134,7 @@ static uint32_t activeFeaturesLatch = 0;
 static uint8_t currentControlRateProfileIndex = 0;
 controlRateConfig_t *currentControlRateProfile;
 
-static const uint8_t EEPROM_CONF_VERSION = 114;
+static const uint8_t EEPROM_CONF_VERSION = 115;
 
 static void resetAccelerometerTrims(flightDynamicsTrims_t * accZero, flightDynamicsTrims_t * accGain)
 {
@@ -231,10 +231,10 @@ void resetNavConfig(navConfig_t * navConfig)
     navConfig->inav.accz_unarmed_cal = 1;
     navConfig->inav.use_gps_velned = 0;         // "Disabled" is mandatory with gps_nav_model = LOW_G
 
-    navConfig->inav.w_z_baro_p = 1.0f;
+    navConfig->inav.w_z_baro_p = 0.35f;
 
-    navConfig->inav.w_z_gps_p = 0.3f;
+    navConfig->inav.w_z_gps_p = 0.2f;
-    navConfig->inav.w_z_gps_v = 0.3f;
+    navConfig->inav.w_z_gps_v = 0.2f;
 
     navConfig->inav.w_xy_gps_p = 1.0f;
     navConfig->inav.w_xy_gps_v = 2.0f;
@@ -277,8 +277,7 @@ void resetNavConfig(navConfig_t * navConfig)
 
 void resetBarometerConfig(barometerConfig_t *barometerConfig)
 {
-    barometerConfig->baro_sample_count = 7;
+    barometerConfig->use_median_filtering = 1;
-    barometerConfig->baro_noise_lpf = 0.35f;
 }
 
 void resetSensorAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)
@@ -546,7 +545,7 @@ static void resetConf(void)
 
     currentProfile->mag_declination = 0;
 
-    resetBarometerConfig(&currentProfile->barometerConfig);
+    resetBarometerConfig(&masterConfig.barometerConfig);
 
     // Radio
     parseRcChannels("AETR1234", &masterConfig.rxConfig);
@@ -808,7 +807,7 @@ void activateConfig(void)
 #endif
 
 #ifdef BARO
-    useBarometerConfig(&currentProfile->barometerConfig);
+    useBarometerConfig(&masterConfig.barometerConfig);
 #endif
 }
 

src/main/config/config_master.h

@@ -59,6 +59,8 @@ typedef struct master_t {
 
     gyroConfig_t gyroConfig;
 
+    barometerConfig_t barometerConfig;
+
     uint8_t mag_hardware;                   // Which mag hardware to use on boards with more than one device
     uint8_t baro_hardware;                  // Barometer hardware to use
 

src/main/config/config_profile.h

@@ -25,8 +25,6 @@ typedef struct profile_s {
     int16_t mag_declination;                // Get your magnetic decliniation from here : http://magnetic-declination.com/
                                             // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero.
 
-    barometerConfig_t barometerConfig;
-
     modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT];
 
     adjustmentRange_t adjustmentRanges[MAX_ADJUSTMENT_RANGE_COUNT];

src/main/io/serial_cli.c

@@ -701,8 +701,7 @@ const clivalue_t valueTable[] = {
 
     { "acc_hardware",               VAR_UINT8  | MASTER_VALUE,  &masterConfig.acc_hardware, .config.minmax = { 0,  ACC_MAX }, 0 },
 
-    { "baro_tab_size",              VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].barometerConfig.baro_sample_count, .config.minmax = { 0,  BARO_SAMPLE_COUNT_MAX }, 0 },
+    { "baro_use_median_filter",     VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, &masterConfig.barometerConfig.use_median_filtering, .config.lookup = { TABLE_OFF_ON }, 0 },
-    { "baro_noise_lpf",             VAR_FLOAT  | PROFILE_VALUE, &masterConfig.profile[0].barometerConfig.baro_noise_lpf, .config.minmax = { 0,  1 }, 0 },
     { "baro_hardware",              VAR_UINT8  | MASTER_VALUE,  &masterConfig.baro_hardware, .config.minmax = { 0,  BARO_MAX }, 0 },
 
     { "mag_hardware",               VAR_UINT8  | MASTER_VALUE,  &masterConfig.mag_hardware, .config.minmax = { 0,  MAG_MAX }, 0 },

src/main/sensors/barometer.c

@@ -43,7 +43,6 @@ int32_t BaroAlt = 0;
 
 static int32_t baroGroundAltitude = 0;
 static int32_t baroGroundPressure = 0;
-static uint32_t baroPressureSum = 0;
 
 static barometerConfig_t *barometerConfig;
 
@@ -72,7 +71,7 @@ static int32_t applyBarometerMedianFilter(int32_t newPressureReading)
     static int currentFilterSampleIndex = 0;
     static bool medianFilterReady = false;
     int nextSampleIndex;
-    
+
     nextSampleIndex = (currentFilterSampleIndex + 1);
     if (nextSampleIndex == PRESSURE_SAMPLES_MEDIAN) {
         nextSampleIndex = 0;
@@ -81,45 +80,18 @@ static int32_t applyBarometerMedianFilter(int32_t newPressureReading)
 
     barometerFilterSamples[currentFilterSampleIndex] = newPressureReading;
     currentFilterSampleIndex = nextSampleIndex;
-    
+
     if (medianFilterReady)
         return quickMedianFilter3(barometerFilterSamples);
     else
         return newPressureReading;
 }
 
-#define PRESSURE_SAMPLE_COUNT (barometerConfig->baro_sample_count - 1)
-
-static uint32_t recalculateBarometerTotal(uint8_t baroSampleCount, uint32_t pressureTotal, int32_t newPressureReading)
-{
-    static int32_t barometerSamples[BARO_SAMPLE_COUNT_MAX];
-    static int currentSampleIndex = 0;
-    int nextSampleIndex;
-
-    // store current pressure in barometerSamples
-    nextSampleIndex = (currentSampleIndex + 1);
-    if (nextSampleIndex == baroSampleCount) {
-        nextSampleIndex = 0;
-        baroReady = true;
-    }
-    barometerSamples[currentSampleIndex] = applyBarometerMedianFilter(newPressureReading);
-
-    // recalculate pressure total
-    // Note, the pressure total is made up of baroSampleCount - 1 samples - See PRESSURE_SAMPLE_COUNT
-    pressureTotal += barometerSamples[currentSampleIndex];
-    pressureTotal -= barometerSamples[nextSampleIndex];
-
-    currentSampleIndex = nextSampleIndex;
-
-    return pressureTotal;
-}
-
 typedef enum {
     BAROMETER_NEEDS_SAMPLES = 0,
     BAROMETER_NEEDS_CALCULATION
 } barometerState_e;
 
-
 bool isBaroReady(void) {
 	return baroReady;
 }
@@ -141,7 +113,9 @@ uint32_t baroUpdate(void)
             baro.get_up();
             baro.start_ut();
             baro.calculate(&baroPressure, &baroTemperature);
-            baroPressureSum = recalculateBarometerTotal(barometerConfig->baro_sample_count, baroPressureSum, baroPressure);
+            if (barometerConfig->use_median_filtering) {
+                baroPressure = applyBarometerMedianFilter(baroPressure);
+            }
             state = BAROMETER_NEEDS_SAMPLES;
             return baro.ut_delay;
         break;
@@ -151,7 +125,7 @@ uint32_t baroUpdate(void)
 static void performBaroCalibrationCycle(void)
 {
     baroGroundPressure -= baroGroundPressure / 8;
-    baroGroundPressure += baroPressureSum / PRESSURE_SAMPLE_COUNT;
+    baroGroundPressure += baroPressure;
     baroGroundAltitude = (1.0f - powf((baroGroundPressure / 8) / 101325.0f, 0.190295f)) * 4433000.0f;
 
     calibratingB--;
@@ -167,13 +141,10 @@ int32_t baroCalculateAltitude(void)
 #ifdef HIL
         if (!hilActive) {
 #endif
-            int32_t BaroAlt_tmp;
-
             // calculates height from ground via baro readings
             // see: https://github.com/diydrones/ardupilot/blob/master/libraries/AP_Baro/AP_Baro.cpp#L140
-            BaroAlt_tmp = lrintf((1.0f - powf((float)(baroPressureSum / PRESSURE_SAMPLE_COUNT) / 101325.0f, 0.190295f)) * 4433000.0f); // in cm
+            BaroAlt = lrintf((1.0f - powf((float)(baroPressure) / 101325.0f, 0.190295f)) * 4433000.0f); // in cm
-            BaroAlt_tmp -= baroGroundAltitude;
+            BaroAlt -= baroGroundAltitude;
-            BaroAlt = lrintf((float)BaroAlt * barometerConfig->baro_noise_lpf + (float)BaroAlt_tmp * (1.0f - barometerConfig->baro_noise_lpf)); // additional LPF to reduce baro noise
 #ifdef HIL
         } else {
             BaroAlt = hilToFC.baroAlt;

src/main/sensors/barometer.h

@@ -30,8 +30,7 @@ typedef enum {
 #define BARO_MAX BARO_FAKE
 
 typedef struct barometerConfig_s {
-    uint8_t baro_sample_count;              // size of baro filter array
+    uint8_t use_median_filtering;       // Use 3-point median filtering
-    float baro_noise_lpf;                   // additional LPF to reduce baro noise
 } barometerConfig_t;
 
 extern int32_t BaroAlt;