Refactor Baro to floats, filter at position rate

convert pressure to altitude early
remove median filter
PT2 filtering upsampled to altitude function in position.c - thanks KarateBrot
baro task synced to position task - thanks Steve
PT2 implementation - thanks KarateBrot
ground altitude from filtered altitude
baro cali by average of calibration samples over cal period
adjust vario and smoothing defaults
don't say haveBaroAlt until cal is complete
reduce PIDs since Baro is faster
add baro smoothing values to blackbox header

Co-Authored-By: Jan Post <post@stud.tu-darmstadt.de>
Co-Authored-By: Steve Evans <SteveCEvans@users.noreply.github.com>

src/main/blackbox/blackbox.c

@@ -1447,6 +1447,8 @@ static bool blackboxWriteSysinfo(void)
 #endif
 #ifdef USE_BARO
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_BARO_HARDWARE, "%d",           barometerConfig()->baro_hardware);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_BARO_NOISE_LPF, "%d",          barometerConfig()->baro_noise_lpf);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_BARO_VARIO_LPF, "%d",          barometerConfig()->baro_vario_lpf);
 #endif
 #ifdef USE_MAG
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MAG_HARDWARE, "%d",           compassConfig()->mag_hardware);

src/main/cli/settings.c

@@ -732,9 +732,8 @@ const clivalue_t valueTable[] = {
     { "baro_i2c_device",            VAR_UINT8  | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, 5 }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_i2c_device) },
     { "baro_i2c_address",           VAR_UINT8  | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, I2C_ADDR7_MAX }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_i2c_address) },
     { PARAM_NAME_BARO_HARDWARE,     VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BARO_HARDWARE }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_hardware) },
-    { "baro_tab_size",              VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 1, BARO_SAMPLE_COUNT_MAX }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_sample_count) },
+    { "baro_noise_lpf",             VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 1000 }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_noise_lpf) },
-    { "baro_noise_lpf",             VAR_UINT16  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_noise_lpf) },
+    { "baro_vario_lpf",             VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 1000 }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_vario_lpf) },
-    { "baro_cf_vel",                VAR_UINT16  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_cf_vel) },
 #endif
 
 // PG_RX_CONFIG

src/main/cms/cms_menu_firmware.c

@@ -81,7 +81,7 @@ static const void *cmsx_CalibrationOnDisplayUpdate(displayPort_t *pDisp, const O
     tfp_sprintf(accCalibrationStatus, sensors(SENSOR_ACC) ? accIsCalibrationComplete() ? accHasBeenCalibrated() ? CALIBRATION_STATUS_OK : CALIBRATION_STATUS_NOK : CALIBRATION_STATUS_WAIT: CALIBRATION_STATUS_OFF);
 #endif
 #if defined(USE_BARO)
-    tfp_sprintf(baroCalibrationStatus, sensors(SENSOR_BARO) ? baroIsCalibrationComplete() ? CALIBRATION_STATUS_OK : CALIBRATION_STATUS_WAIT: CALIBRATION_STATUS_OFF);
+    tfp_sprintf(baroCalibrationStatus, sensors(SENSOR_BARO) ? baroIsCalibrated() ? CALIBRATION_STATUS_OK : CALIBRATION_STATUS_WAIT: CALIBRATION_STATUS_OFF);
 #endif
 
     return NULL;

src/main/fc/core.c

@@ -186,7 +186,7 @@ static bool isCalibrating(void)
         || (sensors(SENSOR_ACC) && !accIsCalibrationComplete())
 #endif
 #ifdef USE_BARO
-        || (sensors(SENSOR_BARO) && !baroIsCalibrationComplete())
+        || (sensors(SENSOR_BARO) && !baroIsCalibrated())
 #endif
 #ifdef USE_MAG
         || (sensors(SENSOR_MAG) && !compassIsCalibrationComplete())

src/main/fc/parameter_names.h

@@ -34,6 +34,8 @@
 #define PARAM_NAME_ACC_LPF_HZ "acc_lpf_hz"
 #define PARAM_NAME_MAG_HARDWARE "mag_hardware"
 #define PARAM_NAME_BARO_HARDWARE "baro_hardware"
+#define PARAM_NAME_BARO_NOISE_LPF "baro_noise_lpf"
+#define PARAM_NAME_BARO_VARIO_LPF "baro_vario_lpf"
 #define PARAM_NAME_RC_SMOOTHING "rc_smoothing"
 #define PARAM_NAME_RC_SMOOTHING_AUTO_FACTOR "rc_smoothing_auto_factor"
 #define PARAM_NAME_RC_SMOOTHING_AUTO_FACTOR_THROTTLE "rc_smoothing_auto_factor_throttle"

src/main/fc/tasks.c

@@ -294,9 +294,25 @@ void taskUpdateRangefinder(timeUs_t currentTimeUs)
 #endif
 
 #if defined(USE_BARO) || defined(USE_GPS)
-static void taskCalculateAltitude(timeUs_t currentTimeUs)
+static bool taskAltitudeCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs)
 {
-    calculateEstimatedAltitude(currentTimeUs);
+    UNUSED(currentTimeUs);
+
+#if defined(USE_BARO)
+    if (isBaroSampleReady()) {
+        return true;
+    }
+#endif
+
+    if (currentDeltaTimeUs > getTask(TASK_ALTITUDE)->attribute->desiredPeriodUs) {
+        return true;
+    }
+
+    return false;
+ }
+ static void taskCalculateAltitude()
+{
+    calculateEstimatedAltitude();
 }
 #endif // USE_BARO || USE_GPS
 
@@ -374,11 +390,11 @@ task_attribute_t task_attributes[TASK_COUNT] = {
 #endif
 
 #ifdef USE_BARO
-    [TASK_BARO] = DEFINE_TASK("BARO", NULL, NULL, taskUpdateBaro, TASK_PERIOD_HZ(20), TASK_PRIORITY_LOW),
+    [TASK_BARO] = DEFINE_TASK("BARO", NULL, NULL, taskUpdateBaro, TASK_PERIOD_HZ(TASK_BARO_RATE_HZ), TASK_PRIORITY_LOW),
 #endif
 
 #if defined(USE_BARO) || defined(USE_GPS)
-    [TASK_ALTITUDE] = DEFINE_TASK("ALTITUDE", NULL, NULL, taskCalculateAltitude, TASK_PERIOD_HZ(40), TASK_PRIORITY_LOW),
+    [TASK_ALTITUDE] = DEFINE_TASK("ALTITUDE", NULL, taskAltitudeCheck, taskCalculateAltitude, TASK_PERIOD_HZ(TASK_ALTITUDE_RATE_HZ), TASK_PRIORITY_LOW),
 #endif
 
 #ifdef USE_DASHBOARD

src/main/flight/gps_rescue.c

@@ -146,9 +146,9 @@ PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
     .initialAltitudeM = 30,
     .descentDistanceM = 20,
     .rescueGroundspeed = 500,
-    .throttleP = 18,
+    .throttleP = 20,
-    .throttleI = 40,
+    .throttleI = 20,
-    .throttleD = 13,
+    .throttleD = 10,
     .velP = 6,
     .velI = 20,
     .velD = 70,

src/main/flight/position.c

@@ -29,6 +29,7 @@
 #include "build/debug.h"
 
 #include "common/maths.h"
+#include "common/filter.h"
 
 #include "fc/runtime_config.h"
 
@@ -46,6 +47,11 @@
 #include "pg/pg.h"
 #include "pg/pg_ids.h"
 
+static int32_t estimatedAltitudeCm = 0;                // in cm
+#ifdef USE_BARO
+    static pt2Filter_t baroDerivativeLpf;
+#endif
+
 typedef enum {
     DEFAULT = 0,
     BARO_ONLY,
@@ -60,30 +66,13 @@ PG_RESET_TEMPLATE(positionConfig_t, positionConfig,
     .altNumSatsBaroFallback = POSITION_DEFAULT_ALT_NUM_SATS_BARO_FALLBACK,
 );
 
-static int32_t estimatedAltitudeCm = 0;                // in cm
-
-#define BARO_UPDATE_FREQUENCY_40HZ (1000 * 25)
-
 #ifdef USE_VARIO
 static int16_t estimatedVario = 0;                   // in cm/s
 
-int16_t calculateEstimatedVario(int32_t baroAlt, const uint32_t dTime) {
+int16_t calculateEstimatedVario(float baroAltVelocity)
-    static float vel = 0;
+{
-    static int32_t lastBaroAlt = 0;
+    baroAltVelocity = applyDeadband(baroAltVelocity, 10.0f); // cm/s, so ignore climb rates less than 0.1 m/s
-
+    return constrain(lrintf(baroAltVelocity), -1500, 1500);
-    int32_t baroVel = 0;
-
-    baroVel = (baroAlt - lastBaroAlt) * 1000000.0f / dTime;
-    lastBaroAlt = baroAlt;
-
-    baroVel = constrain(baroVel, -1500.0f, 1500.0f);
-    baroVel = applyDeadband(baroVel, 10.0f);
-
-    vel = vel * CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_cf_vel) + baroVel * (1.0f - CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_cf_vel));
-    int32_t vel_tmp = lrintf(vel);
-    vel_tmp = applyDeadband(vel_tmp, 5.0f);
-
-    return constrain(vel_tmp, SHRT_MIN, SHRT_MAX);
 }
 #endif
 
@@ -91,35 +80,38 @@ int16_t calculateEstimatedVario(int32_t baroAlt, const uint32_t dTime) {
 static bool altitudeOffsetSetBaro = false;
 static bool altitudeOffsetSetGPS = false;
 
-void calculateEstimatedAltitude(timeUs_t currentTimeUs)
+void calculateEstimatedAltitude()
 {
-    static timeUs_t previousTimeUs = 0;
+    static float baroAltOffset = 0;
-    static int32_t baroAltOffset = 0;
+    static float gpsAltOffset = 0;
-    static int32_t gpsAltOffset = 0;
 
-    const uint32_t dTime = currentTimeUs - previousTimeUs;
+    float baroAlt = 0;
-    if (dTime < BARO_UPDATE_FREQUENCY_40HZ) {
+    float baroAltVelocity = 0;
-        schedulerIgnoreTaskExecTime();
+    float gpsAlt = 0;
-        return;
-    }
-    previousTimeUs = currentTimeUs;
-
-    int32_t baroAlt = 0;
-    int32_t gpsAlt = 0;
     uint8_t gpsNumSat = 0;
 
 #if defined(USE_GPS) && defined(USE_VARIO)
-    int16_t gpsVertSpeed = 0;
+    float gpsVertSpeed = 0;
 #endif
     float gpsTrust = 0.3; //conservative default
     bool haveBaroAlt = false;
     bool haveGpsAlt = false;
 #ifdef USE_BARO
     if (sensors(SENSOR_BARO)) {
-        if (!baroIsCalibrationComplete()) {
+        static float lastBaroAlt = 0;
-            performBaroCalibrationCycle();
+        static bool initBaroFilter = false;
-        } else {
+        if (!initBaroFilter) {
-            baroAlt = baroCalculateAltitude();
+            const float cutoffHz = barometerConfig()->baro_vario_lpf / 100.0f;
+            const float sampleTimeS = HZ_TO_INTERVAL(TASK_ALTITUDE_RATE_HZ);
+            const float gain = pt2FilterGain(cutoffHz, sampleTimeS);
+            pt2FilterInit(&baroDerivativeLpf, gain);
+            initBaroFilter = true;
+        }
+        baroAlt = baroUpsampleAltitude();
+        const float baroAltVelocityRaw = (baroAlt - lastBaroAlt) * TASK_ALTITUDE_RATE_HZ; // cm/s
+        baroAltVelocity = pt2FilterApply(&baroDerivativeLpf, baroAltVelocityRaw);
+        lastBaroAlt = baroAlt;
+        if (baroIsCalibrated()) {
             haveBaroAlt = true;
         }
     }
@@ -185,7 +177,7 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs)
         }
 #ifdef USE_VARIO
         // baro is a better source for vario, so ignore gpsVertSpeed
-        estimatedVario = calculateEstimatedVario(baroAlt, dTime);
+        estimatedVario = calculateEstimatedVario(baroAltVelocity);
 #endif
     } else if (haveGpsAlt && (positionConfig()->altSource == GPS_ONLY || positionConfig()->altSource == DEFAULT )) {
         estimatedAltitudeCm = gpsAlt;
@@ -195,12 +187,10 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs)
     } else if (haveBaroAlt && (positionConfig()->altSource == BARO_ONLY || positionConfig()->altSource == DEFAULT)) {
         estimatedAltitudeCm = baroAlt;
 #ifdef USE_VARIO
-        estimatedVario = calculateEstimatedVario(baroAlt, dTime);
+        estimatedVario = calculateEstimatedVario(baroAltVelocity); // cm/s
 #endif
     }
 
-
-
     DEBUG_SET(DEBUG_ALTITUDE, 0, (int32_t)(100 * gpsTrust));
     DEBUG_SET(DEBUG_ALTITUDE, 1, baroAlt);
     DEBUG_SET(DEBUG_ALTITUDE, 2, gpsAlt);

src/main/flight/position.h

@@ -22,6 +22,7 @@
 
 #include "common/time.h"
 
+#define TASK_ALTITUDE_RATE_HZ 120
 #define POSITION_DEFAULT_ALT_NUM_SATS_GPS_USE 10
 #define POSITION_DEFAULT_ALT_NUM_SATS_BARO_FALLBACK 7
 
@@ -34,6 +35,6 @@ typedef struct positionConfig_s {
 PG_DECLARE(positionConfig_t, positionConfig);
 
 bool isAltitudeOffset(void);
-void calculateEstimatedAltitude(timeUs_t currentTimeUs);
+void calculateEstimatedAltitude();
 int32_t getEstimatedAltitudeCm(void);
 int16_t getEstimatedVario(void);

src/main/sensors/barometer.c

@@ -29,6 +29,7 @@
 #include "build/debug.h"
 
 #include "common/maths.h"
+#include "common/filter.h"
 
 #include "pg/pg.h"
 #include "pg/pg_ids.h"
@@ -58,13 +59,12 @@
 
 baro_t baro;                        // barometer access functions
 
-PG_REGISTER_WITH_RESET_FN(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 1);
+PG_REGISTER_WITH_RESET_FN(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 2);
 
 void pgResetFn_barometerConfig(barometerConfig_t *barometerConfig)
 {
-    barometerConfig->baro_sample_count = 21;
+    barometerConfig->baro_noise_lpf = 50;
-    barometerConfig->baro_noise_lpf = 600;
+    barometerConfig->baro_vario_lpf = 10;
-    barometerConfig->baro_cf_vel = 985;
     barometerConfig->baro_hardware = BARO_DEFAULT;
 
     // For backward compatibility; ceate a valid default value for bus parameters
@@ -139,18 +139,20 @@ void pgResetFn_barometerConfig(barometerConfig_t *barometerConfig)
     barometerConfig->baro_xclr_tag = IO_TAG(BARO_XCLR_PIN);
 }
 
-static uint16_t calibratingB = 0;      // baro calibration = get new ground pressure value
+static uint16_t calibrationCycles = 0;      // baro calibration = get new ground pressure value
+static bool baroCalibrated = false;
 static int32_t baroPressure = 0;
 static int32_t baroTemperature = 0;
+static float baroGroundAltitude = 0.0f;
+static float baroAltitudeRaw = 0.0f;
+static pt2Filter_t baroUpsampleLpf;
 
-static int32_t baroGroundAltitude = 0;
-static int32_t baroGroundPressure = 8*101325;
-static uint32_t baroPressureSum = 0;
 
-#define CALIBRATING_BARO_CYCLES 200 // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
+#define CALIBRATING_BARO_CYCLES 100 // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
 #define SET_GROUND_LEVEL_BARO_CYCLES 10 // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking)
 
 static bool baroReady = false;
+static bool baroSampleReady = false;
 
 void baroPreInit(void)
 {
@@ -161,7 +163,21 @@ void baroPreInit(void)
 #endif
 }
 
-bool baroDetect(baroDev_t *baroDev, baroSensor_e baroHardwareToUse)
+static bool baroDetect(baroDev_t *baroDev, baroSensor_e baroHardwareToUse);
+
+void baroInit(void)
+{
+    baroDetect(&baro.dev, barometerConfig()->baro_hardware);
+
+    const float cutoffHz = barometerConfig()->baro_noise_lpf / 100.0f;
+    const float sampleTimeS = HZ_TO_INTERVAL(TASK_ALTITUDE_RATE_HZ);
+    const float gain = pt2FilterGain(cutoffHz, sampleTimeS);
+    pt2FilterInit(&baroUpsampleLpf, gain);
+
+    baroReady = true;
+}
+
+static bool baroDetect(baroDev_t *baroDev, baroSensor_e baroHardwareToUse)
 {
     extDevice_t *dev = &baroDev->dev;
 
@@ -295,72 +311,26 @@ bool baroDetect(baroDev_t *baroDev, baroSensor_e baroHardwareToUse)
     return true;
 }
 
-bool baroIsCalibrationComplete(void)
+bool baroIsCalibrated(void)
 {
-    return calibratingB == 0;
+    return baroCalibrated;
 }
 
 static void baroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
 {
-    calibratingB = calibrationCyclesRequired;
+    calibrationCycles = calibrationCyclesRequired;
 }
 
 void baroStartCalibration(void)
 {
     baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
+    baroCalibrated = false;
 }
 
 void baroSetGroundLevel(void)
 {
     baroSetCalibrationCycles(SET_GROUND_LEVEL_BARO_CYCLES);
-}
+    baroCalibrated = false;
-
-#define PRESSURE_SAMPLES_MEDIAN 3
-
-static int32_t applyBarometerMedianFilter(int32_t newPressureReading)
-{
-    static int32_t barometerFilterSamples[PRESSURE_SAMPLES_MEDIAN];
-    static int currentFilterSampleIndex = 0;
-    static bool medianFilterReady = false;
-    int nextSampleIndex;
-
-    nextSampleIndex = (currentFilterSampleIndex + 1);
-    if (nextSampleIndex == PRESSURE_SAMPLES_MEDIAN) {
-        nextSampleIndex = 0;
-        medianFilterReady = true;
-    }
-
-    barometerFilterSamples[currentFilterSampleIndex] = newPressureReading;
-    currentFilterSampleIndex = nextSampleIndex;
-
-    if (medianFilterReady)
-        return quickMedianFilter3(barometerFilterSamples);
-    else
-        return newPressureReading;
-}
-
-static uint32_t recalculateBarometerTotal(uint32_t pressureTotal, int32_t newPressureReading)
-{
-    static int32_t barometerSamples[BARO_SAMPLE_COUNT_MAX + 1];
-    static int currentSampleIndex = 0;
-    int nextSampleIndex;
-
-    // store current pressure in barometerSamples
-    if (currentSampleIndex >= barometerConfig()->baro_sample_count) {
-        nextSampleIndex = 0;
-        baroReady = true;
-    } else {
-        nextSampleIndex = (currentSampleIndex + 1);
-    }
-    barometerSamples[currentSampleIndex] = applyBarometerMedianFilter(newPressureReading);
-
-    // recalculate pressure total
-    pressureTotal += barometerSamples[currentSampleIndex];
-    pressureTotal -= barometerSamples[nextSampleIndex];
-
-    currentSampleIndex = nextSampleIndex;
-
-    return pressureTotal;
 }
 
 typedef enum {
@@ -373,11 +343,24 @@ typedef enum {
     BARO_STATE_COUNT
 } barometerState_e;
 
-
 bool isBaroReady(void) {
     return baroReady;
 }
 
+bool isBaroSampleReady(void)
+{
+     if (baroSampleReady) {
+         baroSampleReady = false;
+         return true;
+     }
+     return false;
+ }
+
+static float pressureToAltitude(const float pressure)
+{
+    return (1.0f - powf(pressure / 101325.0f, 0.190295f)) * 4433000.0f;
+}
+
 uint32_t baroUpdate(timeUs_t currentTimeUs)
 {
     static timeUs_t baroStateDurationUs[BARO_STATE_COUNT];
@@ -441,21 +424,24 @@ uint32_t baroUpdate(timeUs_t currentTimeUs)
                 schedulerIgnoreTaskExecTime();
                 break;
             }
-
             baro.dev.calculate(&baroPressure, &baroTemperature);
             baro.baroPressure = baroPressure;
             baro.baroTemperature = baroTemperature;
-            baroPressureSum = recalculateBarometerTotal(baroPressureSum, baroPressure);
+
+            baroAltitudeRaw = pressureToAltitude(baroPressure);
+
+            performBaroCalibrationCycle();
+
+            DEBUG_SET(DEBUG_BARO, 1, baro.baroTemperature);
+            DEBUG_SET(DEBUG_BARO, 2, baroAltitudeRaw - baroGroundAltitude);
+
+            baroSampleReady = true;
+
             if (baro.dev.combined_read) {
                 state = BARO_STATE_PRESSURE_START;
             } else {
                 state = BARO_STATE_TEMPERATURE_START;
             }
-
-            DEBUG_SET(DEBUG_BARO, 1, baroTemperature);
-            DEBUG_SET(DEBUG_BARO, 2, baroPressure);
-            DEBUG_SET(DEBUG_BARO, 3, baroPressureSum);
-
             break;
     }
 
@@ -475,40 +461,30 @@ uint32_t baroUpdate(timeUs_t currentTimeUs)
     return sleepTime;
 }
 
-static float pressureToAltitude(const float pressure)
+// baroAltitudeRaw will get updated in the BARO task while baroUpsampleAltitude() will run in the ALTITUDE task.
+float baroUpsampleAltitude(void)
 {
-    return (1.0f - powf(pressure / 101325.0f, 0.190295f)) * 4433000.0f;
+    const float baroAltitudeRawFiltered = pt2FilterApply(&baroUpsampleLpf, baroAltitudeRaw);
-}
+    if (baroIsCalibrated()) {
-
+        baro.BaroAlt = baroAltitudeRawFiltered - baroGroundAltitude;
-int32_t baroCalculateAltitude(void)
+    } else {
-{
+        baro.BaroAlt = 0.0f;
-    int32_t BaroAlt_tmp;
-
-    // calculates height from ground via baro readings
-    if (baroIsCalibrationComplete()) {
-        BaroAlt_tmp = lrintf(pressureToAltitude((float)(baroPressureSum / barometerConfig()->baro_sample_count)));
-        BaroAlt_tmp -= baroGroundAltitude;
-        baro.BaroAlt = lrintf((float)baro.BaroAlt * CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_noise_lpf) + (float)BaroAlt_tmp * (1.0f - CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_noise_lpf))); // additional LPF to reduce baro noise
-    }
-    else {
-        baro.BaroAlt = 0;
     }
+    DEBUG_SET(DEBUG_BARO, 3, baro.BaroAlt); // cm
     return baro.BaroAlt;
 }
 
 void performBaroCalibrationCycle(void)
 {
-    static int32_t savedGroundPressure = 0;
+    static uint16_t cycleCount = 0;
-
+    if (!baroCalibrated) {
-    baroGroundPressure -= baroGroundPressure / 8;
+        cycleCount++;
-    baroGroundPressure += baroPressureSum / barometerConfig()->baro_sample_count;
+        baroGroundAltitude += baroAltitudeRaw;
-    baroGroundAltitude = (1.0f - pow_approx((baroGroundPressure / 8) / 101325.0f, 0.190259f)) * 4433000.0f;
+        if (cycleCount == calibrationCycles) {
-
+            baroGroundAltitude = baroGroundAltitude / calibrationCycles; // simple average
-    if (baroGroundPressure == savedGroundPressure) {
+            baroCalibrated = true;
-        calibratingB = 0;
+            cycleCount = 0;
-    } else {
+        }
-        calibratingB--;
-        savedGroundPressure = baroGroundPressure;
     }
 }
 

src/main/sensors/barometer.h

@@ -22,6 +22,7 @@
 
 #include "pg/pg.h"
 #include "drivers/barometer/barometer.h"
+#include "flight/position.h"
 
 typedef enum {
     BARO_DEFAULT = 0,
@@ -35,8 +36,6 @@ typedef enum {
     BARO_DPS310 = 8,
 } baroSensor_e;
 
-#define BARO_SAMPLE_COUNT_MAX   48
-
 typedef struct barometerConfig_s {
     uint8_t baro_busType;
     uint8_t baro_spi_device;
@@ -44,18 +43,21 @@ typedef struct barometerConfig_s {
     uint8_t baro_i2c_device;
     uint8_t baro_i2c_address;
     uint8_t baro_hardware;                  // Barometer hardware to use
-    uint8_t baro_sample_count;              // size of baro filter array
+    uint16_t baro_noise_lpf;                // lowpass cutoff (value / 100) Hz for baro smoothing
-    uint16_t baro_noise_lpf;                // additional LPF to reduce baro noise
+    uint16_t baro_vario_lpf;                // lowpass for (value / 100) Hz baro derivative smoothing
-    uint16_t baro_cf_vel;                   // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity)
     ioTag_t baro_eoc_tag;
     ioTag_t baro_xclr_tag;
 } barometerConfig_t;
 
 PG_DECLARE(barometerConfig_t, barometerConfig);
 
+// #define TASK_BARO_DENOM       3
+// #define TASK_BARO_RATE_HZ     (TASK_ALTITUDE_RATE_HZ / TASK_BARO_DENOM)
+#define TASK_BARO_RATE_HZ        40
+
 typedef struct baro_s {
     baroDev_t dev;
-    int32_t BaroAlt;
+    float BaroAlt;
     int32_t baroTemperature;             // Use temperature for telemetry
     int32_t baroPressure;                // Use pressure for telemetry
 } baro_t;
@@ -63,11 +65,12 @@ typedef struct baro_s {
 extern baro_t baro;
 
 void baroPreInit(void);
-bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse);
+void baroInit(void);
-bool baroIsCalibrationComplete(void);
+bool baroIsCalibrated(void);
 void baroStartCalibration(void);
 void baroSetGroundLevel(void);
 uint32_t baroUpdate(timeUs_t currentTimeUs);
 bool isBaroReady(void);
-int32_t baroCalculateAltitude(void);
+bool isBaroSampleReady(void);
+float baroUpsampleAltitude(void);
 void performBaroCalibrationCycle(void);

src/main/sensors/initialisation.c

@@ -81,7 +81,7 @@ bool sensorsAutodetect(void)
 #endif
 
 #ifdef USE_BARO
-    baroDetect(&baro.dev, barometerConfig()->baro_hardware);
+    baroInit();
 #endif
 
 #ifdef USE_RANGEFINDER

src/test/unit/arming_prevention_unittest.cc

@@ -1042,7 +1042,7 @@ extern "C" {
     void saveConfigAndNotify(void) {}
     void blackboxFinish(void) {}
     bool accIsCalibrationComplete(void) { return true; }
-    bool baroIsCalibrationComplete(void) { return true; }
+    bool baroIsCalibrated(void) { return true; }
     bool gyroIsCalibrationComplete(void) { return gyroCalibDone; }
     void gyroStartCalibration(bool) {}
     bool isFirstArmingGyroCalibrationRunning(void) { return false; }

src/test/unit/flight_imu_unittest.cc

@@ -43,6 +43,7 @@ extern "C" {
     #include "flight/mixer.h"
     #include "flight/pid.h"
     #include "flight/imu.h"
+    #include "flight/position.h"
 
     #include "io/gps.h"
 
@@ -202,57 +203,64 @@ TEST(FlightImuTest, TestSmallAngle)
 // STUBS
 
 extern "C" {
-boxBitmask_t rcModeActivationMask;
+    boxBitmask_t rcModeActivationMask;
-float rcCommand[4];
+    float rcCommand[4];
-float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
+    float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
 
-gyro_t gyro;
+    gyro_t gyro;
-acc_t acc;
+    acc_t acc;
-mag_t mag;
+    mag_t mag;
 
-gpsSolutionData_t gpsSol;
+    gpsSolutionData_t gpsSol;
-int16_t GPS_verticalSpeedInCmS;
+    int16_t GPS_verticalSpeedInCmS;
 
-uint8_t debugMode;
+    uint8_t debugMode;
-int16_t debug[DEBUG16_VALUE_COUNT];
+    int16_t debug[DEBUG16_VALUE_COUNT];
 
-uint8_t stateFlags;
+    uint8_t stateFlags;
-uint16_t flightModeFlags;
+    uint16_t flightModeFlags;
-uint8_t armingFlags;
+    uint8_t armingFlags;
 
-pidProfile_t *currentPidProfile;
+    pidProfile_t *currentPidProfile;
 
-uint16_t enableFlightMode(flightModeFlags_e mask)
+    uint16_t enableFlightMode(flightModeFlags_e mask) {
-{
+        return flightModeFlags |= (mask);
-    return flightModeFlags |= (mask);
+    }
-}
+
-
+    uint16_t disableFlightMode(flightModeFlags_e mask) {
-uint16_t disableFlightMode(flightModeFlags_e mask)
+        return flightModeFlags &= ~(mask);
-{
+    }
-    return flightModeFlags &= ~(mask);
+
-}
+    bool sensors(uint32_t mask) {
-
+        return mask & SENSOR_ACC;
-bool sensors(uint32_t mask)
+    };
-{
+
-    return mask & SENSOR_ACC;
+    uint32_t millis(void) { return 0; }
-};
+    uint32_t micros(void) { return 0; }
-
+
-uint32_t millis(void) { return 0; }
+    bool compassIsHealthy(void) { return true; }
-uint32_t micros(void) { return 0; }
+    bool baroIsCalibrated(void) { return true; }
-
+    void performBaroCalibrationCycle(void) {}
-bool compassIsHealthy(void) { return true; }
+    float baroCalculateAltitude(void) { return 0; }
-bool baroIsCalibrationComplete(void) { return true; }
+    bool gyroGetAccumulationAverage(float *) { return false; }
-void performBaroCalibrationCycle(void) {}
+    bool accGetAccumulationAverage(float *) { return false; }
-int32_t baroCalculateAltitude(void) { return 0; }
+    void mixerSetThrottleAngleCorrection(int) {};
-bool gyroGetAccumulationAverage(float *) { return false; }
+    bool gpsRescueIsRunning(void) { return false; }
-bool accGetAccumulationAverage(float *) { return false; }
+    bool isFixedWing(void) { return false; }
-void mixerSetThrottleAngleCorrection(int) {};
+    void pinioBoxTaskControl(void) {}
-bool gpsRescueIsRunning(void) { return false; }
+    void schedulerIgnoreTaskExecTime(void) {}
-bool isFixedWing(void) { return false; }
+    void schedulerIgnoreTaskStateTime(void) {}
-void pinioBoxTaskControl(void) {}
+    void schedulerSetNextStateTime(timeDelta_t) {}
-void schedulerIgnoreTaskExecTime(void) {}
+    bool schedulerGetIgnoreTaskExecTime() { return false; }
-void schedulerIgnoreTaskStateTime(void) {}
+    float gyroGetFilteredDownsampled(int) { return 0.0f; }
-void schedulerSetNextStateTime(timeDelta_t) {}
+    float baroUpsampleAltitude()  { return 0.0f; }
-bool schedulerGetIgnoreTaskExecTime() { return false; }
+    float pt2FilterGain(float, float)  { return 0.0f; }
-float gyroGetFilteredDownsampled(int) { return 0.0f; }
+
+    void pt2FilterInit(pt2Filter_t *baroDerivativeLpf, float) {
+        UNUSED(baroDerivativeLpf);
+    }
+    float pt2FilterApply(pt2Filter_t *baroDerivativeLpf, float) {
+        UNUSED(baroDerivativeLpf);
+        return 0.0f;
+    }
 }

src/test/unit/telemetry_ibus_unittest.cc

@@ -82,7 +82,7 @@ int32_t getAmperage(void)
     return amperage;
 }
 
-int32_t getEstimatedVario(void)
+int16_t getEstimatedVario(void)
 {
     return estimatedVario;
 }

src/test/unit/vtx_unittest.cc

@@ -136,7 +136,7 @@ extern "C" {
     void blackboxFinish(void) {}
     bool accIsCalibrationComplete(void) { return true; }
     bool accHasBeenCalibrated(void) { return true; }
-    bool baroIsCalibrationComplete(void) { return true; }
+    bool baroIsCalibrated(void) { return true; }
     bool gyroIsCalibrationComplete(void) { return gyroCalibDone; }
     void gyroStartCalibration(bool) {}
     bool isFirstArmingGyroCalibrationRunning(void) { return false; }