Skip to content

Commit

Permalink
baro temp correction
Browse files Browse the repository at this point in the history
breadoven committed Sep 23, 2024

Verified

This commit was created on GitHub.com and signed with GitHub’s verified signature.
1 parent 9923c30 commit 12b8413
Showing 4 changed files with 87 additions and 3 deletions.
10 changes: 10 additions & 0 deletions docs/Settings.md
Original file line number Diff line number Diff line change
@@ -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.
6 changes: 6 additions & 0 deletions src/main/fc/settings.yaml
Original file line number Diff line number Diff line change
@@ -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
67 changes: 64 additions & 3 deletions src/main/sensors/barometer.c
Original file line number Diff line number Diff line change
@@ -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);
}

7 changes: 7 additions & 0 deletions src/main/sensors/barometer.h
Original file line number Diff line number Diff line change
@@ -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);

0 comments on commit 12b8413

Please sign in to comment.