Merge remote-tracking branch 'origin/amorale/OP-1139_baro_2nd_order_compensation' into corvuscorax/OP-1022_AH_improvements_amorale

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;
+        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;
@@ -171,6 +178,10 @@ static void altitudeTask(__attribute__((unused)) void *parameters)
     }
 }
 
+static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
+{
+    RevoSettingsBaroTempCorrectionPolynomialGet(&baroCorrection);
+}
 /**
  * @}
  * @}

flight/pios/common/pios_ms5611.c

@@ -52,6 +52,9 @@ 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);
 
+// 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,6 +77,9 @@ void PIOS_MS5611_Init(const struct pios_ms5611_cfg *cfg, int32_t i2c_device)
 
     uint8_t data[2];
 
+    // reset temperature compensation values
+    compensation_t2 = 0;
+
     /* Calibration parameters */
     for (int i = 0; i < 6; i++) {
         PIOS_MS5611_Read(MS5611_CALIB_ADDR + i * 2, data, 2);
@@ -190,17 +196,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;
+            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);
+                Offset2 += 7 * tcorr;
+                Sens2   += (11 * tcorr) >> 1;
+            }
+        } else {
+            compensation_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 +234,8 @@ int32_t PIOS_MS5611_ReadADC(void)
  */
 float PIOS_MS5611_GetTemperature(void)
 {
-    return ((float)Temperature) / 100.0f;
+    // Apply the second order low and very low temperature compensation offset
+    return ((float)(Temperature - compensation_t2)) / 100.0f;
 }
 
 /**

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. -->
         <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"/>
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="true" updatemode="onchange" period="0"/>
         <telemetryflight acked="true" updatemode="onchange" period="0"/>