From 12b8413b973b6c284ec7be90af908c2ee3845b8f Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Mon, 23 Sep 2024 15:10:47 +0100 Subject: [PATCH] baro temp correction --- docs/Settings.md | 10 ++++++ src/main/fc/settings.yaml | 6 ++++ src/main/sensors/barometer.c | 67 ++++++++++++++++++++++++++++++++++-- src/main/sensors/barometer.h | 7 ++++ 4 files changed, 87 insertions(+), 3 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 362f65b92df..a5d4bfaeaa2 100644 --- a/docs/Settings.md +++ b/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. diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 0524a1e935d..c171c5a3681 100644 --- a/src/main/fc/settings.yaml +++ b/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 diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index 6604ccc1c82..8370878bb00 100644 --- a/src/main/sensors/barometer.c +++ b/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); } diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h index 2bbb1a976b9..4a0e35a5210 100644 --- a/src/main/sensors/barometer.h +++ b/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);