Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-19 14:25:16 +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)
{
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));
}
}

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

@ -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;

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

@ -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