rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00
Code Issues Releases Activity

OP-1149: limit baro correction to calibrated temperature range.

Browse Source
This commit is contained in:
Alessio Morale 2014-01-28 23:08:28 +01:00
parent 61354eaac9
commit f1bea0556d
2 changed files with 14 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -55,6 +55,8 @@
// Private variables
static xTaskHandle taskHandle;
static RevoSettingsBaroTempCorrectionPolynomialData baroCorrection;
static RevoSettingsBaroTempCorrectionExtentData baroCorrectionExtent;
static volatile bool tempCorrectionEnabled;
// Private functions
static void altitudeTask(void *parameters);
static void SettingsUpdatedCb(UAVObjEvent *ev);
@ -164,9 +166,13 @@ static void altitudeTask(__attribute__((unused)) void *parameters)
temp = PIOS_MS5611_GetTemperature();
press = PIOS_MS5611_GetPressure();
// pressure bias = A + B*t + C*t^2 + D * t^3
press -= baroCorrection.a + ((baroCorrection.d * temp + baroCorrection.c) * temp + baroCorrection.b) * temp;
if (tempCorrectionEnabled) {
// 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 altitude = 44330.0f * (1.0f - powf((press) / MS5611_P0, (1.0f / 5.255f)));
if (!isnan(altitude)) {
@ -182,6 +188,9 @@ static void altitudeTask(__attribute__((unused)) void *parameters)
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
RevoSettingsBaroTempCorrectionPolynomialGet(&baroCorrection);
RevoSettingsBaroTempCorrectionExtentGet(&baroCorrectionExtent);
tempCorrectionEnabled = !(baroCorrectionExtent.max - baroCorrectionExtent.min < 0.1f ||
(baroCorrection.a < 1e-9f && baroCorrection.b < 1e-9f && baroCorrection.c < 1e-9f && baroCorrection.d < 1e-9f));
}
/**
* @}

3
shared/uavobjectdefinition/revosettings.xml
View File

@ -9,7 +9,8 @@
<field name="BaroGPSOffsetCorrectionAlpha" units="" type="float" elements="1" defaultvalue="0.9993335555062"/>
<!-- Cooefficients for the polynomial that models the barometer pressure bias as a function of temperature
bias = a + b * temp + c * temp^2 + d * temp^3 -->
<field name="BaroTempCorrectionPolynomial" units="" type="float" elements="3" elementnames="a,b,c,d" defaultvalue="0,0,0,0"/>
<field name="BaroTempCorrectionPolynomial" units="" type="float" elements="4" elementnames="a,b,c,d" defaultvalue="0,0,0,0"/>
<field name="BaroTempCorrectionExtent" units="" type="float" elements="2" elementnames="min,max" defaultvalue="0,0"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>