Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Handle invalid baro pressure values #12815

Merged
merged 1 commit into from
May 21, 2023
Merged

Handle invalid baro pressure values #12815

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
44 changes: 24 additions & 20 deletions src/main/sensors/barometer.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -160,6 +160,7 @@ void pgResetFn_barometerConfig(barometerConfig_t *barometerConfig)
#define NUM_GROUND_LEVEL_CYCLES 10 // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking)

static uint16_t calibrationCycles = 0; // baro calibration = get new ground pressure value
static uint16_t calibrationCycleCount = 0;
static float baroGroundAltitude = 0.0f;
static bool baroCalibrated = false;
static bool baroReady = false;
Expand Down Expand Up @@ -341,23 +342,20 @@ bool baroIsCalibrated(void)
return baroCalibrated;
}

static void baroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
{
calibrationCycles = calibrationCyclesRequired;
}

void baroStartCalibration(void)
{
baroSetCalibrationCycles(NUM_CALIBRATION_CYCLES);
baroGroundAltitude = 0;
baroCalibrated = false;
calibrationCycles = NUM_CALIBRATION_CYCLES;
calibrationCycleCount = 0;
}

void baroSetGroundLevel(void)
{
baroSetCalibrationCycles(NUM_GROUND_LEVEL_CYCLES);
baroGroundAltitude = 0;
baroCalibrated = false;
calibrationCycles = NUM_GROUND_LEVEL_CYCLES;
calibrationCycleCount = 0;
}

typedef enum {
Expand Down Expand Up @@ -449,15 +447,23 @@ uint32_t baroUpdate(timeUs_t currentTimeUs)

// update baro data
baro.dev.calculate(&baro.pressure, &baro.temperature);
baro.altitude = pressureToAltitude(baro.pressure);

if (baroIsCalibrated()) {
// zero baro altitude
baro.altitude -= baroGroundAltitude;
// If baro.pressure is invalid then skip altitude counting / call of calibration cycle
haslinghuis marked this conversation as resolved.
Show resolved Hide resolved
if (baro.pressure > 0) {
const float altitude = pressureToAltitude(baro.pressure);
if (baroIsCalibrated()) {
// zero baro altitude
baro.altitude = altitude - baroGroundAltitude;
} else {
// establish stable baroGroundAltitude value to zero baro altitude with
performBaroCalibrationCycle(altitude);
baro.altitude = 0.0f;
}
} else {
// establish stable baroGroundAltitude value to zero baro altitude with
performBaroCalibrationCycle(baro.altitude);
baro.altitude = 0.0f;
// return 0 during calibration, reuse last value otherwise
if (!baroIsCalibrated()) {
baro.altitude = 0.0f;
}
}

DEBUG_SET(DEBUG_BARO, 1, lrintf(baro.pressure / 100.0f)); // hPa
Expand Down Expand Up @@ -495,15 +501,13 @@ float getBaroAltitude(void)

static void performBaroCalibrationCycle(const float altitude)
haslinghuis marked this conversation as resolved.
Show resolved Hide resolved
{
static uint16_t cycleCount = 0;

baroGroundAltitude += altitude;
cycleCount++;
calibrationCycleCount++;

if (cycleCount >= calibrationCycles) {
baroGroundAltitude /= cycleCount; // simple average
if (calibrationCycleCount >= calibrationCycles) {
baroGroundAltitude /= calibrationCycleCount; // simple average
baroCalibrated = true;
cycleCount = 0;
calibrationCycleCount = 0;
}
}

Expand Down