rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

OP-1531 - Add temperature filtering for baro compensation

Browse Source
This commit is contained in:
Alessio Morale 2014-10-07 11:32:20 +02:00
parent 46fee43487
commit c1062124af
1 changed files with 21 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
flight/modules/Altitude/revolution/altitude.c
View File

@ -41,14 +41,19 @@
#include "altitude.h" #include "altitude.h"
#include "barosensor.h" // object that will be updated by the module #include "barosensor.h" // object that will be updated by the module
#include "revosettings.h" #include "revosettings.h"
#include <mathmisc.h>
#if defined(PIOS_INCLUDE_HCSR04) #if defined(PIOS_INCLUDE_HCSR04)
#include "sonaraltitude.h" // object that will be updated by the module #include "sonaraltitude.h" // object that will be updated by the module
#endif #endif
#include "taskinfo.h" #include "taskinfo.h"
// Private constants // Private constants
#define STACK_SIZE_BYTES 550 #define STACK_SIZE_BYTES 550
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) #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 // Private types
@ -57,6 +62,11 @@ static xTaskHandle taskHandle;
static RevoSettingsBaroTempCorrectionPolynomialData baroCorrection; static RevoSettingsBaroTempCorrectionPolynomialData baroCorrection;
static RevoSettingsBaroTempCorrectionExtentData baroCorrectionExtent; static RevoSettingsBaroTempCorrectionExtentData baroCorrectionExtent;
static volatile bool tempCorrectionEnabled; static volatile bool tempCorrectionEnabled;
static float baro_temp_bias = 0;
static float baro_temperature = 0;
static uint8_t temp_calibration_count = 0;
// Private functions // Private functions
static void altitudeTask(void *parameters); static void altitudeTask(void *parameters);
static void SettingsUpdatedCb(UAVObjEvent *ev); static void SettingsUpdatedCb(UAVObjEvent *ev);
@ -166,13 +176,18 @@ static void altitudeTask(__attribute__((unused)) void *parameters)
temp = PIOS_MS5611_GetTemperature(); temp = PIOS_MS5611_GetTemperature();
press = PIOS_MS5611_GetPressure(); 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 // 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 // in case the temperature is outside of the calibrated range, uses the nearest extremes
float ctemp = temp > baroCorrectionExtent.max ? baroCorrectionExtent.max : float ctemp = boundf(baro_temperature, baroCorrectionExtent.max, baroCorrectionExtent.min);
(temp < baroCorrectionExtent.min ? baroCorrectionExtent.min : temp); baro_temp_bias = baroCorrection.a + ((baroCorrection.d * ctemp + baroCorrection.c) * ctemp + baroCorrection.b) * ctemp;
press -= 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))); float altitude = 44330.0f * (1.0f - powf((press) / MS5611_P0, (1.0f / 5.255f)));
if (!isnan(altitude)) { if (!isnan(altitude)) {