diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c index 7e3a9ec077..385cac4b08 100644 --- a/src/main/sensors/pitotmeter.c +++ b/src/main/sensors/pitotmeter.c @@ -153,27 +153,21 @@ bool pitotInit(void) bool pitotIsCalibrationComplete(void) { - return pitot.calibrationFinished; + return zeroCalibrationIsCompleteS(&pitot.zeroCalibration) && zeroCalibrationIsSuccessfulS(&pitot.zeroCalibration); } void pitotStartCalibration(void) { - pitot.calibrationTimeoutMs = millis(); - pitot.calibrationFinished = false; + zeroCalibrationStartS(&pitot.zeroCalibration, CALIBRATING_PITOT_TIME_MS, P0 * 0.00001f, false); } static void performPitotCalibrationCycle(void) { - const float pitotPressureZeroError = pitot.pressure - pitot.pressureZero; - pitot.pressureZero += pitotPressureZeroError * 0.25f; + zeroCalibrationAddValueS(&pitot.zeroCalibration, pitot.pressure); - if (fabsf(pitotPressureZeroError) < (P0 * 0.000005f)) { - if ((millis() - pitot.calibrationTimeoutMs) > 500) { - pitot.calibrationFinished = true; - } - } - else { - pitot.calibrationTimeoutMs = millis(); + if (zeroCalibrationIsCompleteS(&pitot.zeroCalibration)) { + zeroCalibrationGetZeroS(&pitot.zeroCalibration, &pitot.pressureZero); + DEBUG_TRACE_SYNC("Pitot calibration complete (%d)", lrintf(pitot.pressureZero)); } } diff --git a/src/main/sensors/pitotmeter.h b/src/main/sensors/pitotmeter.h index 73425f3f56..548b23be02 100644 --- a/src/main/sensors/pitotmeter.h +++ b/src/main/sensors/pitotmeter.h @@ -19,6 +19,7 @@ #include "config/parameter_group.h" #include "common/filter.h" +#include "common/calibration.h" #include "drivers/pitotmeter.h" @@ -46,11 +47,10 @@ typedef struct pito_s { pitotDev_t dev; float airSpeed; + zeroCalibrationScalar_t zeroCalibration; pt1Filter_t lpfState; timeUs_t lastMeasurementUs; timeMs_t lastSeenHealthyMs; - timeMs_t calibrationTimeoutMs; - bool calibrationFinished; float pressureZero; float pressure; diff --git a/src/main/sensors/sensors.h b/src/main/sensors/sensors.h index 48847ec50c..dbcceb562d 100644 --- a/src/main/sensors/sensors.h +++ b/src/main/sensors/sensors.h @@ -40,6 +40,7 @@ typedef union flightDynamicsTrims_u { } flightDynamicsTrims_t; #define CALIBRATING_BARO_TIME_MS 2000 +#define CALIBRATING_PITOT_TIME_MS 2000 #define CALIBRATING_GYRO_TIME_MS 2000 #define CALIBRATING_ACC_TIME_MS 500