CR131

breadoven · Sep 22, 2024 · f356f7b · f356f7b
1 parent ecf0222
commit f356f7b
Show file tree

Hide file tree

Showing 4 changed files with 73 additions and 16 deletions.
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
@@ -636,10 +636,10 @@ groups:
         max: 1000
         # CR131
       - name: inav_baro_temp_correction
-        description: "Baro temperature correction factor"
+        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: -100
-        max: 1000
+        min: -50
+        max: 51
         default_value: 0
 
   - name: PG_PITOTMETER_CONFIG

diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c
@@ -282,6 +282,7 @@ void onNewGPSData(void)
 void updatePositionEstimator_BaroTopic(timeUs_t currentTimeUs)
 {
     float newBaroAlt = baroCalculateAltitude();
+    DEBUG_SET(DEBUG_ALWAYS, 4, newBaroAlt);
 
     /* If we are required - keep altitude at zero */
     if (shouldResetReferenceAltitude()) {
@@ -539,9 +540,9 @@ static void estimationPredict(estimationContext_t * ctx)
         posEstimator.est.pos.z += posEstimator.est.vel.z * ctx->dt;
         posEstimator.est.pos.z += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f;
         // CR131
-        // if (ARMING_FLAG(WAS_EVER_ARMED)) {   // Hold at zero until first armed
+        if (ARMING_FLAG(WAS_EVER_ARMED)) {   // Hold at zero until first armed
             posEstimator.est.vel.z += posEstimator.imu.accelNEU.z * ctx->dt;
-        // }
+        }
         // CR131
         // DEBUG_SET(DEBUG_ALWAYS, 0, posEstimator.est.vel.z * 1000);
         // DEBUG_SET(DEBUG_ALWAYS, 1, posEstimator.imu.accelNEU.z);
@@ -600,9 +601,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
         }
     }
     // CR131
-        DEBUG_SET(DEBUG_ALWAYS, 2, 10);
     if (ctx->newFlags & EST_BARO_VALID && wBaro) { // CR131
-        DEBUG_SET(DEBUG_ALWAYS, 2, 100);
         timeUs_t currentTimeUs = micros();
 
         if (!ARMING_FLAG(ARMED)) {
@@ -644,9 +643,8 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
         correctOK = ARMING_FLAG(WAS_EVER_ARMED);
         // CR131
     }
-DEBUG_SET(DEBUG_ALWAYS, 3, 10);
+// DEBUG_SET(DEBUG_ALWAYS, 4, posEstimator.baro.baroAltRate);
     if (ctx->newFlags & EST_GPS_Z_VALID && wGps) {  // CR131
-        DEBUG_SET(DEBUG_ALWAYS, 3, 100);
         // Reset current estimate to GPS altitude if estimate not valid
         if (!(ctx->newFlags & EST_Z_VALID)) {
             ctx->estPosCorr.z += posEstimator.gps.pos.z - posEstimator.est.pos.z;
@@ -673,8 +671,8 @@ DEBUG_SET(DEBUG_ALWAYS, 3, 10);
         correctOK = ARMING_FLAG(WAS_EVER_ARMED);
         // CR131
     }
-DEBUG_SET(DEBUG_ALWAYS, 0, wBaro);
-DEBUG_SET(DEBUG_ALWAYS, 1, wGps);
+// DEBUG_SET(DEBUG_ALWAYS, 0, wBaro * 100);
+// DEBUG_SET(DEBUG_ALWAYS, 1, wGps * 100);
     return correctOK;
 }
 

diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c
@@ -62,7 +62,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_INAV_BARO_TEMP_CORRECTION_DEFAULT,   // CR131
 );
 
 static zeroCalibrationScalar_t zeroCalibration;
@@ -309,6 +310,57 @@ void baroStartCalibration(void)
     zeroCalibrationStartS(&zeroCalibration, CALIBRATING_BARO_TIME_MS, acceptedPressureVariance, false);
 }
 
+// CR131
+float processBaroTempCorrection(void)
+{
+    static float correctionFactor = 0.0f;
+    static baroTempCalState_e calibrationState = BARO_TEMP_CAL_INITIALISE;
+    static int16_t baroTemp1 = 0.0f;
+    static timeMs_t startTimeMs = 0;
+    float setting = barometerConfig()->baro_temp_correction;
+
+    DEBUG_SET(DEBUG_ALWAYS, 0, correctionFactor * 100);
+    DEBUG_SET(DEBUG_ALWAYS, 3, 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) {
+            baroTemp1 = baroTemp2 = baro.baroTemperature;
+            baroAlt1 = newBaroAlt;
+            calibrationState = BARO_TEMP_CAL_IN_PROGRESS;
+            startTimeMs = millis();
+        }
+
+        if (setting == 51.0f) {
+            float referenceDeltaTemp = ABS(baro.baroTemperature - baroTemp1);
+            if (referenceDeltaTemp > 100 && referenceDeltaTemp > ABS(baroTemp2 - baroTemp1)) {  // centidegrees
+                baroTemp2 = baro.baroTemperature;
+                correctionFactor = 0.8f * correctionFactor + 0.2f * (newBaroAlt - baroAlt1) / CENTIDEGREES_TO_DEGREES(baroTemp2 - baroTemp1));
+            }
+        } else {
+            correctionFactor = setting;
+            calibrationState = BARO_TEMP_CAL_COMPLETE;
+        }
+    }
+
+    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 (calibrationState == BARO_TEMP_CAL_COMPLETE) {
+        float tempCal = constrainf(correctionFactor, -50.0f, 50.0f) * CENTIDEGREES_TO_DEGREES(baroTemp1 - baro.baroTemperature);
+        DEBUG_SET(DEBUG_ALWAYS, 2, tempCal);
+        return tempCal;
+        // return constrainf(correctionFactor, -50.0f, 50.0f) * CENTIDEGREES_TO_DEGREES(baroTemp1 - baro.baroTemperature);
+    }
+
+    return 0.0f;
+}
+// CR131
+
 int32_t baroCalculateAltitude(void)
 {
     if (!baroIsCalibrationComplete()) {
@@ -324,8 +376,8 @@ 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();  // CR131
+    }
 
     return baro.BaroAlt;
 }
@@ -336,7 +388,7 @@ int32_t baroGetLatestAltitude(void)
 }
 
 int16_t baroGetTemperature(void)
-{   
+{
     return CENTIDEGREES_TO_DECIDEGREES(baro.baroTemperature);
 }
 

diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h
@@ -37,7 +37,13 @@ typedef enum {
     BARO_FAKE   = 12,
     BARO_MAX    = BARO_FAKE
 } baroSensor_e;
-
+// CR131
+typedef enum {
+    BARO_TEMP_CAL_INITIALISE,
+    BARO_TEMP_CAL_IN_PROGRESS,
+    BARO_TEMP_CAL_COMPLETE,
+} baroTempCalState_e;
+// CR131
 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;   // CR131
 } barometerConfig_t;
 
 PG_DECLARE(barometerConfig_t, barometerConfig);