diff --git a/flight/modules/Altitude/revolution/altitude.c b/flight/modules/Altitude/revolution/altitude.c index 0f395f7c8..ffbf2749f 100644 --- a/flight/modules/Altitude/revolution/altitude.c +++ b/flight/modules/Altitude/revolution/altitude.c @@ -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)); } /** * @} diff --git a/shared/uavobjectdefinition/revosettings.xml b/shared/uavobjectdefinition/revosettings.xml index 8d634d1cd..d94c89f0c 100644 --- a/shared/uavobjectdefinition/revosettings.xml +++ b/shared/uavobjectdefinition/revosettings.xml @@ -9,7 +9,8 @@ - + +