From 75842cb64840396b42acf3b88ba0734888f83899 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Tue, 10 Dec 2013 01:21:05 +0100 Subject: [PATCH 1/6] =?UTF-8?q?OP-1139:=20perform=20second=20order=20low?= =?UTF-8?q?=20temperature=20compensation.=20It=20follows=20the=20procedure?= =?UTF-8?q?=20described=20in=20MS5611=20datasheet(http://www.meas-spec.com?= =?UTF-8?q?/downloads/MS5611-01BA03.pdf,=20page=208)=20to=20perform=20low(?= =?UTF-8?q?20=C2=B0)=20and=20very=20low(-15=C2=B0C)=20temperature=20compen?= =?UTF-8?q?sation.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- flight/pios/common/pios_ms5611.c | 32 ++++++++++++++++++++++++++------ 1 file changed, 26 insertions(+), 6 deletions(-) diff --git a/flight/pios/common/pios_ms5611.c b/flight/pios/common/pios_ms5611.c index 1d1a289e7..aeb99de45 100644 --- a/flight/pios/common/pios_ms5611.c +++ b/flight/pios/common/pios_ms5611.c @@ -51,7 +51,7 @@ static int64_t Temperature; static int32_t lastConversionStart; static int32_t PIOS_MS5611_Read(uint8_t address, uint8_t *buffer, uint8_t len); static int32_t PIOS_MS5611_WriteCommand(uint8_t command); - +static int64_t t2; // Move into proper driver structure with cfg stored static uint32_t oversampling; static const struct pios_ms5611_cfg *dev_cfg; @@ -74,6 +74,9 @@ void PIOS_MS5611_Init(const struct pios_ms5611_cfg *cfg, int32_t i2c_device) uint8_t data[2]; + // reset calibration values + t2 = 0; + /* Calibration parameters */ for (int i = 0; i < 6; i++) { PIOS_MS5611_Read(MS5611_CALIB_ADDR + i * 2, data, 2); @@ -190,17 +193,34 @@ int32_t PIOS_MS5611_ReadADC(void) } else { int64_t Offset; int64_t Sens; + // used for second order temperature compensation + int64_t Offset2 = 0; + int64_t Sens2 = 0; /* Read the pressure conversion */ if (PIOS_MS5611_Read(MS5611_ADC_READ, Data, 3) != 0) { return -1; } + // check if temperature is less than 20°C + if(Temperature < 2000){ + Offset2 = 5 * (Temperature - 2000) >> 1; + Sens2 = Offset2 >> 1; + t2 = (deltaTemp*deltaTemp) >> 31; + // Apply the "Very low temperature compensation" when temp is less than -15°C + if(Temperature < -1500){ + int64_t tcorr = (Temperature + 1500)*(Temperature + 1500); + Offset2 += 7 * tcorr; + Sens2 += (11 * tcorr) >> 1; + } + } else { + t2 = 0; + Offset2 = 0; + Sens2 = 0; + } RawPressure = ((Data[0] << 16) | (Data[1] << 8) | Data[2]); - - Offset = (((int64_t)CalibData.C[1]) << 16) + ((((int64_t)CalibData.C[3]) * deltaTemp) >> 7); + Offset = (((int64_t)CalibData.C[1]) << 16) + ((((int64_t)CalibData.C[3]) * deltaTemp) >> 7) - Offset2; Sens = ((int64_t)CalibData.C[0]) << 15; - Sens = Sens + ((((int64_t)CalibData.C[2]) * deltaTemp) >> 8); - + Sens = Sens + ((((int64_t)CalibData.C[2]) * deltaTemp) >> 8) - Sens2; Pressure = (((((int64_t)RawPressure) * Sens) >> 21) - Offset) >> 15; } return 0; @@ -211,7 +231,7 @@ int32_t PIOS_MS5611_ReadADC(void) */ float PIOS_MS5611_GetTemperature(void) { - return ((float)Temperature) / 100.0f; + return ((float)(Temperature - t2)) / 100.0f; } /** From 13b45b2b7846055ed55266c0bdec21690ff76e81 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Tue, 10 Dec 2013 01:44:13 +0100 Subject: [PATCH 2/6] OP.1139: uncrustify --- flight/pios/common/pios_ms5611.c | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/flight/pios/common/pios_ms5611.c b/flight/pios/common/pios_ms5611.c index aeb99de45..f21fff0fc 100644 --- a/flight/pios/common/pios_ms5611.c +++ b/flight/pios/common/pios_ms5611.c @@ -195,27 +195,27 @@ int32_t PIOS_MS5611_ReadADC(void) int64_t Sens; // used for second order temperature compensation int64_t Offset2 = 0; - int64_t Sens2 = 0; + int64_t Sens2 = 0; /* Read the pressure conversion */ if (PIOS_MS5611_Read(MS5611_ADC_READ, Data, 3) != 0) { return -1; } // check if temperature is less than 20°C - if(Temperature < 2000){ + if (Temperature < 2000) { Offset2 = 5 * (Temperature - 2000) >> 1; - Sens2 = Offset2 >> 1; - t2 = (deltaTemp*deltaTemp) >> 31; + Sens2 = Offset2 >> 1; + t2 = (deltaTemp * deltaTemp) >> 31; // Apply the "Very low temperature compensation" when temp is less than -15°C - if(Temperature < -1500){ - int64_t tcorr = (Temperature + 1500)*(Temperature + 1500); + if (Temperature < -1500) { + int64_t tcorr = (Temperature + 1500) * (Temperature + 1500); Offset2 += 7 * tcorr; - Sens2 += (11 * tcorr) >> 1; + Sens2 += (11 * tcorr) >> 1; } } else { - t2 = 0; + t2 = 0; Offset2 = 0; - Sens2 = 0; + Sens2 = 0; } RawPressure = ((Data[0] << 16) | (Data[1] << 8) | Data[2]); Offset = (((int64_t)CalibData.C[1]) << 16) + ((((int64_t)CalibData.C[3]) * deltaTemp) >> 7) - Offset2; From 634ba79dcb4ee858524d34c64d386a9eb48818cf Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Tue, 10 Dec 2013 01:46:28 +0100 Subject: [PATCH 3/6] OP-1141: allow to input a barometer bias model. It uses a 3rd degree polynomial to model pressure bias as a function of temperature. --- flight/modules/Altitude/revolution/altitude.c | 21 ++++++++++++++----- shared/uavobjectdefinition/revosettings.xml | 4 +++- 2 files changed, 19 insertions(+), 6 deletions(-) diff --git a/flight/modules/Altitude/revolution/altitude.c b/flight/modules/Altitude/revolution/altitude.c index c66f52ae8..27a26278d 100644 --- a/flight/modules/Altitude/revolution/altitude.c +++ b/flight/modules/Altitude/revolution/altitude.c @@ -40,22 +40,24 @@ #include "altitude.h" #include "barosensor.h" // object that will be updated by the module +#include "revosettings.h" #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 500 +#define STACK_SIZE_BYTES 550 #define TASK_PRIORITY (tskIDLE_PRIORITY + 1) // Private types // Private variables static xTaskHandle taskHandle; - +static RevoSettingsBaroTempCorrectionPolynomialData baroCorrection; // Private functions static void altitudeTask(void *parameters); +static void SettingsUpdatedCb(UAVObjEvent *ev); /** * Initialise the module, called on startup @@ -77,6 +79,9 @@ int32_t AltitudeStart() int32_t AltitudeInitialize() { BaroSensorInitialize(); + RevoSettingsInitialize(); + RevoSettingsConnectCallback(&SettingsUpdatedCb); + #if defined(PIOS_INCLUDE_HCSR04) SonarAltitudeInitialize(); #endif @@ -103,6 +108,8 @@ static void altitudeTask(__attribute__((unused)) void *parameters) // Undef for normal operation // #define PIOS_MS5611_SLOW_TEMP_RATE 20 + RevoSettingsBaroTempCorrectionPolynomialGet(&baroCorrection); + #ifdef PIOS_MS5611_SLOW_TEMP_RATE uint8_t temp_press_interleave_count = 1; #endif @@ -154,12 +161,12 @@ static void altitudeTask(__attribute__((unused)) void *parameters) vTaskDelay(PIOS_MS5611_GetDelay()); PIOS_MS5611_ReadADC(); - temp = PIOS_MS5611_GetTemperature(); press = PIOS_MS5611_GetPressure(); + float temp2 = temp * temp; + float correction = baroCorrection.a + temp * baroCorrection.b + temp2 * baroCorrection.c + temp * temp2 * baroCorrection.d; - - float altitude = 44330.0f * (1.0f - powf(press / MS5611_P0, (1.0f / 5.255f))); + float altitude = 44330.0f * (1.0f - powf((press - correction) / MS5611_P0, (1.0f / 5.255f))); if (!isnan(altitude)) { data.Altitude = altitude; @@ -171,6 +178,10 @@ static void altitudeTask(__attribute__((unused)) void *parameters) } } +static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + RevoSettingsBaroTempCorrectionPolynomialGet(&baroCorrection); +} /** * @} * @} diff --git a/shared/uavobjectdefinition/revosettings.xml b/shared/uavobjectdefinition/revosettings.xml index 0cf9a4561..8d634d1cd 100644 --- a/shared/uavobjectdefinition/revosettings.xml +++ b/shared/uavobjectdefinition/revosettings.xml @@ -7,7 +7,9 @@ Defaults: updates at 5 Hz, tau = 300s settle time, exp(-(1/f)/tau) ~= 0.9993335555062 Set BaroGPSOffsetCorrectionAlpha = 1.0 to completely disable baro offset updates. --> - + + From 44269b67623069b576294eb8c545227bd56f7e09 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Tue, 10 Dec 2013 02:02:30 +0100 Subject: [PATCH 4/6] OP-1139: export corrected pressure value to uavobject --- flight/modules/Altitude/revolution/altitude.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/flight/modules/Altitude/revolution/altitude.c b/flight/modules/Altitude/revolution/altitude.c index 27a26278d..d46c7c5bb 100644 --- a/flight/modules/Altitude/revolution/altitude.c +++ b/flight/modules/Altitude/revolution/altitude.c @@ -164,9 +164,9 @@ static void altitudeTask(__attribute__((unused)) void *parameters) temp = PIOS_MS5611_GetTemperature(); press = PIOS_MS5611_GetPressure(); float temp2 = temp * temp; - float correction = baroCorrection.a + temp * baroCorrection.b + temp2 * baroCorrection.c + temp * temp2 * baroCorrection.d; + press = press - baroCorrection.a + temp * baroCorrection.b + temp2 * baroCorrection.c + temp * temp2 * baroCorrection.d; - float altitude = 44330.0f * (1.0f - powf((press - correction) / MS5611_P0, (1.0f / 5.255f))); + float altitude = 44330.0f * (1.0f - powf((press) / MS5611_P0, (1.0f / 5.255f))); if (!isnan(altitude)) { data.Altitude = altitude; From 29df9d6dad71b0b2f7fa641ea8e6c569ec0af59f Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Fri, 13 Dec 2013 17:10:11 +0100 Subject: [PATCH 5/6] OP-1139: Add some more descriptive names and comments for compensation variables --- flight/pios/common/pios_ms5611.c | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/flight/pios/common/pios_ms5611.c b/flight/pios/common/pios_ms5611.c index f21fff0fc..db8d16f1b 100644 --- a/flight/pios/common/pios_ms5611.c +++ b/flight/pios/common/pios_ms5611.c @@ -51,7 +51,10 @@ static int64_t Temperature; static int32_t lastConversionStart; static int32_t PIOS_MS5611_Read(uint8_t address, uint8_t *buffer, uint8_t len); static int32_t PIOS_MS5611_WriteCommand(uint8_t command); -static int64_t t2; + +// Second order temperature compensation. Temperature offset +static int64_t compensation_t2; + // Move into proper driver structure with cfg stored static uint32_t oversampling; static const struct pios_ms5611_cfg *dev_cfg; @@ -74,8 +77,8 @@ void PIOS_MS5611_Init(const struct pios_ms5611_cfg *cfg, int32_t i2c_device) uint8_t data[2]; - // reset calibration values - t2 = 0; + // reset temperature compensation values + compensation_t2 = 0; /* Calibration parameters */ for (int i = 0; i < 6; i++) { @@ -205,7 +208,7 @@ int32_t PIOS_MS5611_ReadADC(void) if (Temperature < 2000) { Offset2 = 5 * (Temperature - 2000) >> 1; Sens2 = Offset2 >> 1; - t2 = (deltaTemp * deltaTemp) >> 31; + compensation_t2 = (deltaTemp * deltaTemp) >> 31; // Apply the "Very low temperature compensation" when temp is less than -15°C if (Temperature < -1500) { int64_t tcorr = (Temperature + 1500) * (Temperature + 1500); @@ -213,9 +216,9 @@ int32_t PIOS_MS5611_ReadADC(void) Sens2 += (11 * tcorr) >> 1; } } else { - t2 = 0; + compensation_t2 = 0; Offset2 = 0; - Sens2 = 0; + Sens2 = 0; } RawPressure = ((Data[0] << 16) | (Data[1] << 8) | Data[2]); Offset = (((int64_t)CalibData.C[1]) << 16) + ((((int64_t)CalibData.C[3]) * deltaTemp) >> 7) - Offset2; @@ -231,7 +234,8 @@ int32_t PIOS_MS5611_ReadADC(void) */ float PIOS_MS5611_GetTemperature(void) { - return ((float)(Temperature - t2)) / 100.0f; + // Apply the second order low and very low temperature compensation offset + return ((float)(Temperature - compensation_t2)) / 100.0f; } /** From f95a86eaed8971761bd78209c7c75166cf037255 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Fri, 13 Dec 2013 17:10:39 +0100 Subject: [PATCH 6/6] OP-1139: missing uncrustification --- flight/modules/Altitude/revolution/altitude.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/flight/modules/Altitude/revolution/altitude.c b/flight/modules/Altitude/revolution/altitude.c index d46c7c5bb..0c6dbd2ed 100644 --- a/flight/modules/Altitude/revolution/altitude.c +++ b/flight/modules/Altitude/revolution/altitude.c @@ -163,10 +163,10 @@ static void altitudeTask(__attribute__((unused)) void *parameters) temp = PIOS_MS5611_GetTemperature(); press = PIOS_MS5611_GetPressure(); - float temp2 = temp * temp; + float temp2 = temp * temp; press = press - baroCorrection.a + temp * baroCorrection.b + temp2 * baroCorrection.c + temp * temp2 * baroCorrection.d; - 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)) { data.Altitude = altitude;