baro temp correction

docs/Settings.md

@@ -452,6 +452,16 @@ Selection of baro hardware. See Wiki Sensor auto detect and hardware failure det
 
 ---
 
+### baro_temp_correction
+
+Baro temperature correction factor to compensate for Baro altitude drift with changes in Baro temperature [cm/Degs C]. Internally limited to between -50 and 50. Typical setting for BMP280 Baro is around 20. Setting to 51 initiates auto calibration which ends after 5 minutes or on first Arm.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0 | -50 | 51 |
+
+---
+
 ### bat_cells
 
 Number of cells of the battery (0 = auto-detect), see battery documentation. 7S, 9S and 11S batteries cannot be auto-detected.

src/main/fc/settings.yaml

@@ -631,6 +631,12 @@ groups:
         field: baro_calibration_tolerance
         min: 0
         max: 1000
+      - name: baro_temp_correction
+        description: "Baro temperature correction factor to compensate for Baro altitude drift with changes in Baro temperature [cm/Degs C]. Internally limited to between -50 and 50. Typical setting for BMP280 Baro is around 20. Setting to 51 initiates auto calibration which ends after 5 minutes or on first Arm."
+        field: baro_temp_correction
+        min: -50
+        max: 51
+        default_value: 0
 
   - name: PG_PITOTMETER_CONFIG
     type: pitotmeterConfig_t

src/main/sensors/barometer.c

@@ -50,6 +50,8 @@
 #include "sensors/barometer.h"
 #include "sensors/sensors.h"
 
+#include "io/beeper.h"
+
 #ifdef USE_HARDWARE_REVISION_DETECTION
 #include "hardware_revision.h"
 #endif
@@ -62,7 +64,8 @@ PG_REGISTER_WITH_RESET_TEMPLATE(barometerConfig_t, barometerConfig, PG_BAROMETER
 
 PG_RESET_TEMPLATE(barometerConfig_t, barometerConfig,
     .baro_hardware = SETTING_BARO_HARDWARE_DEFAULT,
-    .baro_calibration_tolerance = SETTING_BARO_CAL_TOLERANCE_DEFAULT
+    .baro_calibration_tolerance = SETTING_BARO_CAL_TOLERANCE_DEFAULT,
+    .baro_temp_correction = SETTING_BARO_TEMP_CORRECTION_DEFAULT,
 );
 
 static zeroCalibrationScalar_t zeroCalibration;
@@ -309,6 +312,64 @@ void baroStartCalibration(void)
     zeroCalibrationStartS(&zeroCalibration, CALIBRATING_BARO_TIME_MS, acceptedPressureVariance, false);
 }
 
+float processBaroTempCorrection(void)
+{
+    float setting = barometerConfig()->baro_temp_correction;
+
+    if (setting == 0.0f) {
+        return 0.0f;
+    }
+
+    static float correctionFactor = 0.0f;
+    static baroTempCalState_e calibrationState = BARO_TEMP_CAL_INITIALISE;
+    static int16_t baroTemp1 = 0.0f;
+    static timeMs_t startTimeMs = 0;
+
+    DEBUG_SET(DEBUG_ALWAYS, 0, correctionFactor * 100);
+    DEBUG_SET(DEBUG_ALWAYS, 1, baro.baroTemperature);
+    if (!ARMING_FLAG(WAS_EVER_ARMED)) {
+        static float baroAlt1 = 0.0f;
+        static int16_t baroTemp2 = 0.0f;
+        float newBaroAlt = pressureToAltitude(baro.baroPressure) - baroGroundAltitude;
+
+        if (calibrationState == BARO_TEMP_CAL_INITIALISE) {  // set initial correction reference temps/pressures
+            baroTemp1 = baroTemp2 = baro.baroTemperature;
+            baroAlt1 = newBaroAlt;
+            calibrationState = BARO_TEMP_CAL_IN_PROGRESS;
+            startTimeMs = millis();
+        }
+
+        if (setting == 51.0f) {  // Auto calibration triggered with setting = 51
+            /* Min 1 deg temp difference required.
+             * Correction adjusted only if temperature difference to reference temperature increasing */
+            float referenceDeltaTemp = ABS(baro.baroTemperature - baroTemp1);
+            if (referenceDeltaTemp > 100 && referenceDeltaTemp > ABS(baroTemp2 - baroTemp1)) {
+                baroTemp2 = baro.baroTemperature;
+                correctionFactor = 0.8f * correctionFactor + 0.2f * (newBaroAlt - baroAlt1) / CENTIDEGREES_TO_DEGREES(baroTemp2 - baroTemp1);
+                correctionFactor = constrainf(correctionFactor, -50.0f, 50.0f);
+            }
+        } else {
+            correctionFactor = setting;
+            calibrationState = BARO_TEMP_CAL_COMPLETE;
+        }
+    }
+
+    // Calibration ends on first Arm or after 5 min timeout
+    if (calibrationState == BARO_TEMP_CAL_IN_PROGRESS && (ARMING_FLAG(WAS_EVER_ARMED) || millis() > startTimeMs + 300000)) {
+        barometerConfigMutable()->baro_temp_correction = correctionFactor;
+        calibrationState = BARO_TEMP_CAL_COMPLETE;
+        if (!ARMING_FLAG(WAS_EVER_ARMED)) {
+            beeper(correctionFactor != 51.0f ? BEEPER_ACTION_SUCCESS : BEEPER_ACTION_FAIL);
+        }
+    }
+
+    if (calibrationState == BARO_TEMP_CAL_COMPLETE) {
+        return correctionFactor * CENTIDEGREES_TO_DEGREES(baroTemp1 - baro.baroTemperature);
+    }
+
+    return 0.0f;
+}
+
 int32_t baroCalculateAltitude(void)
 {
     if (!baroIsCalibrationComplete()) {
@@ -324,7 +385,7 @@ int32_t baroCalculateAltitude(void)
     }
     else {
         // calculates height from ground via baro readings
-        baro.BaroAlt = pressureToAltitude(baro.baroPressure) - baroGroundAltitude;
+        baro.BaroAlt = pressureToAltitude(baro.baroPressure) - baroGroundAltitude + processBaroTempCorrection();
    }
 
     return baro.BaroAlt;
@@ -336,7 +397,7 @@ int32_t baroGetLatestAltitude(void)
 }
 
 int16_t baroGetTemperature(void)
-{   
+{
     return CENTIDEGREES_TO_DECIDEGREES(baro.baroTemperature);
 }
 

src/main/sensors/barometer.h

@@ -38,6 +38,12 @@ typedef enum {
     BARO_MAX    = BARO_FAKE
 } baroSensor_e;
 
+typedef enum {
+    BARO_TEMP_CAL_INITIALISE,
+    BARO_TEMP_CAL_IN_PROGRESS,
+    BARO_TEMP_CAL_COMPLETE,
+} baroTempCalState_e;
+
 typedef struct baro_s {
     baroDev_t dev;
     int32_t BaroAlt;
@@ -52,6 +58,7 @@ extern baro_t baro;
 typedef struct barometerConfig_s {
     uint8_t baro_hardware;                  // Barometer hardware to use
     uint16_t baro_calibration_tolerance;    // Baro calibration tolerance (cm at sea level)
+    float baro_temp_correction;             // Baro temperature correction value (cm/K)
 } barometerConfig_t;
 
 PG_DECLARE(barometerConfig_t, barometerConfig);