From c1062124af4e0a950b92b9b14495a5f3f75b4667 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Tue, 7 Oct 2014 11:32:20 +0200 Subject: [PATCH] OP-1531 - Add temperature filtering for baro compensation --- flight/modules/Altitude/revolution/altitude.c | 27 ++++++++++++++----- 1 file changed, 21 insertions(+), 6 deletions(-) diff --git a/flight/modules/Altitude/revolution/altitude.c b/flight/modules/Altitude/revolution/altitude.c index 4322c90d3..1c959f94c 100644 --- a/flight/modules/Altitude/revolution/altitude.c +++ b/flight/modules/Altitude/revolution/altitude.c @@ -41,14 +41,19 @@ #include "altitude.h" #include "barosensor.h" // object that will be updated by the module #include "revosettings.h" +#include #if defined(PIOS_INCLUDE_HCSR04) #include "sonaraltitude.h" // object that will be updated by the module #endif #include "taskinfo.h" // Private constants -#define STACK_SIZE_BYTES 550 -#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) +#define STACK_SIZE_BYTES 550 +#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) + +// Interval in number of sample to recalculate temp bias +#define TEMP_CALIB_INTERVAL 10 +#define TEMP_ALPHA 0.9f // Private types @@ -57,6 +62,11 @@ static xTaskHandle taskHandle; static RevoSettingsBaroTempCorrectionPolynomialData baroCorrection; static RevoSettingsBaroTempCorrectionExtentData baroCorrectionExtent; static volatile bool tempCorrectionEnabled; + +static float baro_temp_bias = 0; +static float baro_temperature = 0; +static uint8_t temp_calibration_count = 0; + // Private functions static void altitudeTask(void *parameters); static void SettingsUpdatedCb(UAVObjEvent *ev); @@ -166,13 +176,18 @@ static void altitudeTask(__attribute__((unused)) void *parameters) temp = PIOS_MS5611_GetTemperature(); press = PIOS_MS5611_GetPressure(); - if (tempCorrectionEnabled) { + baro_temperature = TEMP_ALPHA * baro_temperature + (1 - TEMP_ALPHA) * temp; + + if (tempCorrectionEnabled && !temp_calibration_count) { + temp_calibration_count = TEMP_CALIB_INTERVAL; // pressure bias = A + B*t + C*t^2 + D * t^3 // in case the temperature is outside of the calibrated range, uses the nearest extremes - float ctemp = temp > baroCorrectionExtent.max ? baroCorrectionExtent.max : - (temp < baroCorrectionExtent.min ? baroCorrectionExtent.min : temp); - press -= baroCorrection.a + ((baroCorrection.d * ctemp + baroCorrection.c) * ctemp + baroCorrection.b) * ctemp; + float ctemp = boundf(baro_temperature, baroCorrectionExtent.max, baroCorrectionExtent.min); + baro_temp_bias = baroCorrection.a + ((baroCorrection.d * ctemp + baroCorrection.c) * ctemp + baroCorrection.b) * ctemp; } + + press -= baro_temp_bias; + float altitude = 44330.0f * (1.0f - powf((press) / MS5611_P0, (1.0f / 5.255f))); if (!isnan(altitude)) {