Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-19 22:35:19 +03:00
Code Issues Wiki Activity

[PITOT] Migrate pitot to new calibration

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2019-01-13 21:18:22 +01:00
parent 75ea456836
commit 2a514b880b
3 changed files with 9 additions and 14 deletions
Download patch file Download diff file Expand all files Collapse all files

18
src/main/sensors/pitotmeter.c
View file

@ -153,27 +153,21 @@ bool pitotInit(void)
bool pitotIsCalibrationComplete(void) bool pitotIsCalibrationComplete(void)
{ {
return pitot.calibrationFinished; return zeroCalibrationIsCompleteS(&pitot.zeroCalibration) && zeroCalibrationIsSuccessfulS(&pitot.zeroCalibration);
} }
void pitotStartCalibration(void) void pitotStartCalibration(void)
{ {
pitot.calibrationTimeoutMs = millis(); zeroCalibrationStartS(&pitot.zeroCalibration, CALIBRATING_PITOT_TIME_MS, P0 * 0.00001f, false);
pitot.calibrationFinished = false;
} }
static void performPitotCalibrationCycle(void) static void performPitotCalibrationCycle(void)
{ {
const float pitotPressureZeroError = pitot.pressure - pitot.pressureZero; zeroCalibrationAddValueS(&pitot.zeroCalibration, pitot.pressure);
pitot.pressureZero += pitotPressureZeroError * 0.25f;
if (fabsf(pitotPressureZeroError) < (P0 * 0.000005f)) { if (zeroCalibrationIsCompleteS(&pitot.zeroCalibration)) {
if ((millis() - pitot.calibrationTimeoutMs) > 500) { zeroCalibrationGetZeroS(&pitot.zeroCalibration, &pitot.pressureZero);
pitot.calibrationFinished = true; DEBUG_TRACE_SYNC("Pitot calibration complete (%d)", lrintf(pitot.pressureZero));
}
}
else {
pitot.calibrationTimeoutMs = millis();
} }
} }

4
src/main/sensors/pitotmeter.h
View file

@ -19,6 +19,7 @@
#include "config/parameter_group.h" #include "config/parameter_group.h"
#include "common/filter.h" #include "common/filter.h"
#include "common/calibration.h"
#include "drivers/pitotmeter.h" #include "drivers/pitotmeter.h"
@ -46,11 +47,10 @@ typedef struct pito_s {
pitotDev_t dev; pitotDev_t dev;
float airSpeed; float airSpeed;
zeroCalibrationScalar_t zeroCalibration;
pt1Filter_t lpfState; pt1Filter_t lpfState;
timeUs_t lastMeasurementUs; timeUs_t lastMeasurementUs;
timeMs_t lastSeenHealthyMs; timeMs_t lastSeenHealthyMs;
timeMs_t calibrationTimeoutMs;
bool calibrationFinished;
float pressureZero; float pressureZero;
float pressure; float pressure;

1
src/main/sensors/sensors.h
View file

@ -40,6 +40,7 @@ typedef union flightDynamicsTrims_u {
} flightDynamicsTrims_t; } flightDynamicsTrims_t;
#define CALIBRATING_BARO_TIME_MS 2000 #define CALIBRATING_BARO_TIME_MS 2000
#define CALIBRATING_PITOT_TIME_MS 2000
#define CALIBRATING_GYRO_TIME_MS 2000 #define CALIBRATING_GYRO_TIME_MS 2000
#define CALIBRATING_ACC_TIME_MS 500 #define CALIBRATING_ACC_TIME_MS 500