MS5525 driver fix conversion when temperature <20C or pressure negative

libraries/AP_Airspeed/AP_Airspeed_MS5525.cpp

@@ -182,11 +182,11 @@ void AP_Airspeed_MS5525::calculate(void)
     const uint8_t Q5 = 7;
     const uint8_t Q6 = 21;
 
-    int64_t dT = D2 - int64_t(prom[5]) * (1UL<<Q5);
-    int64_t TEMP = 2000 + (dT*int64_t(prom[6]))/(1UL<<Q6);
-    int64_t OFF =  int64_t(prom[2])*(1UL<<Q2) + (int64_t(prom[4])*dT)/(1UL<<Q4);
-    int64_t SENS = int64_t(prom[1])*(1UL<<Q1) + (int64_t(prom[3])*dT)/(1UL<<Q3);
-    int64_t P = (D1*SENS/(1UL<<21)-OFF)/(1UL<<15);
+    float dT = float(D2) - int64_t(prom[5]) * (1L<<Q5);
+    float TEMP = 2000 + (dT*int64_t(prom[6]))/(1L<<Q6);
+    float OFF  = int64_t(prom[2])*(1L<<Q2) + (int64_t(prom[4])*dT)/(1L<<Q4);
+    float SENS = int64_t(prom[1])*(1L<<Q1) + (int64_t(prom[3])*dT)/(1L<<Q3);
+    float P = (float(D1)*SENS/(1L<<21)-OFF)/(1L<<15);
     const float PSI_to_Pa = 6894.757f;
     float P_Pa = PSI_to_Pa * 1.0e-4 * P;
     float Temp_C = TEMP * 0.01;

libraries/SITL/SIM_Airspeed_DLVR.cpp

@@ -61,5 +61,13 @@ void SITL::Airspeed_DLVR::update(const class Aircraft &aircraft)
     last_update_ms = now_ms;
 
     pressure = AP::sitl()->state.airspeed_raw_pressure[0];
-    temperature = 25.0f;
+
+    float sim_alt = AP::sitl()->state.altitude;
+    sim_alt += 2 * rand_float();
+
+    float sigma, delta, theta;
+    AP_Baro::SimpleAtmosphere(sim_alt * 0.001f, sigma, delta, theta);
+
+    // To Do: Add a sensor board temperature offset parameter
+    temperature = (SSL_AIR_TEMPERATURE * theta - C_TO_KELVIN) + 25.0;
 }

libraries/SITL/SIM_MS5525.cpp

@@ -14,11 +14,11 @@ void MS5525::convert_forward(int32_t D1, int32_t D2, float &P_Pa, float &Temp_C)
     const uint8_t Q6 = Qx_coeff[5];
 
     // this is the forward conversion copied from the driver:
-    int64_t dT = D2 - int64_t(prom[5]) * (1UL<<Q5);
-    int64_t TEMP = 2000 + (dT*int64_t(prom[6]))/(1UL<<Q6);
-    int64_t OFF =  int64_t(prom[2])*(1UL<<Q2) + (int64_t(prom[4])*dT)/(1UL<<Q4);
-    int64_t SENS = int64_t(prom[1])*(1UL<<Q1) + (int64_t(prom[3])*dT)/(1UL<<Q3);
-    int64_t P = (D1*SENS/(1UL<<21)-OFF)/(1UL<<15);
+    float dT = float(D2) - int64_t(prom[5]) * (1L<<Q5);
+    float TEMP = 2000 + (dT*int64_t(prom[6]))/(1L<<Q6);
+    float OFF  = int64_t(prom[2])*(1L<<Q2) + (int64_t(prom[4])*dT)/(1L<<Q4);
+    float SENS = int64_t(prom[1])*(1L<<Q1) + (int64_t(prom[3])*dT)/(1L<<Q3);
+    float P = (float(D1)*SENS/(1L<<21)-OFF)/(1L<<15);
     const float PSI_to_Pa = 6894.757f;
     P_Pa = PSI_to_Pa * 1.0e-4 * P;
     Temp_C = TEMP * 0.01;
@@ -33,29 +33,45 @@ void MS5525::convert(float P_Pa, float Temp_C, uint32_t &D1, uint32_t &D2)
     const uint8_t Q5 = Qx_coeff[4];
     const uint8_t Q6 = Qx_coeff[5];
 
-    const int64_t TEMP = Temp_C * 100.0f;
-    const float dT = ((TEMP-2000)*(1UL<<Q6))/prom[6];
+    const float TEMP = Temp_C * 100.0f;
+    const float dT = ((TEMP - 2000.0) * (1L<<Q6)) / int64_t(prom[6]);
     const float PSI_to_Pa = 6894.757f;
     const float P = P_Pa / (PSI_to_Pa * 1.0e-4);
-    const int64_t SENS = int64_t(prom[1])*(1UL<<Q1) + (int64_t(prom[3])*dT)/(1UL<<Q3);
-    const int64_t OFF =  int64_t(prom[2])*(1UL<<Q2) + (int64_t(prom[4])*dT)/(1UL<<Q4);
-    D1 = (((uint64_t(P*(1U<<15)))+OFF)<<21)/SENS;
-    D2 = dT + int64_t(prom[5]) * (1UL<<Q5);
+    const float OFF  = int64_t(prom[2]) * (1L<<Q2) + (int64_t(prom[4]) * dT) / (1L<<Q4);
+    const float SENS = int64_t(prom[1]) * (1L<<Q1) + (int64_t(prom[3]) * dT) / (1L<<Q3);
 
+    D1 = ((P * (1L<<15) + OFF) * (1L<<21)) / SENS;
+    D2 = dT + int64_t(prom[5]) * (1L<<Q5);
+}
 
+void MS5525::check_conversion_accuracy(float P_Pa, float Temp_C, uint32_t D1, uint32_t D2)
+{
     float f_P_Pa;
     float f_Temp_C;
     convert_forward(D1, D2, f_P_Pa, f_Temp_C);
-    if (fabs(f_P_Pa - P_Pa) > 1) {
-        AP_HAL::panic("Invalid conversion");
+
+    const float p_error = fabs(f_P_Pa - P_Pa);
+    const float p_error_max = 0.1;
+    if (p_error > p_error_max) {
+        AP_HAL::panic("Invalid pressure conversion: error %f Pa > %f Pa error max", p_error, p_error_max);
     }
-    if (fabs(f_Temp_C - Temp_C) > 0.1) {
-        AP_HAL::panic("Invalid conversion");
+
+    const float t_error = fabs(f_Temp_C - Temp_C);
+    const float t_error_max = 0.01;
+    if (t_error > t_error_max) {
+        AP_HAL::panic("Invalid temperature conversion: error %f degC > %f degC error max", t_error, t_error_max);
     }
 }
 
 void MS5525::get_pressure_temperature_readings(float &P_Pa, float &Temp_C)
 {
-    Temp_C = 25.0f;
+    float sim_alt = AP::sitl()->state.altitude;
+    sim_alt += 2 * rand_float();
+
+    float sigma, delta, theta;
+    AP_Baro::SimpleAtmosphere(sim_alt * 0.001f, sigma, delta, theta);
+
+    // To Do: Add a sensor board temperature offset parameter
+    Temp_C = (SSL_AIR_TEMPERATURE * theta - C_TO_KELVIN) + 25.0;
     P_Pa = AP::sitl()->state.airspeed_raw_pressure[0];
 }

libraries/SITL/SIM_MS5525.h

@@ -14,6 +14,7 @@ class MS5525 : public MS5XXX
 
     void convert(float P_Pa, float Temp_C, uint32_t &D1, uint32_t &D2) override;
     void convert_forward(int32_t D1, int32_t D2, float &P_Pa, float &Temp_C) override;
+    void check_conversion_accuracy(float P_Pa, float Temp_C, uint32_t D1, uint32_t D2) override;
 
     void load_prom(uint16_t *_loaded_prom, uint8_t len) const override {
         memcpy(_loaded_prom, prom, len);

libraries/SITL/SIM_MS5611.cpp

@@ -88,7 +88,10 @@ void MS5611::convert(float P_Pa, float Temp_C, uint32_t &D1, uint32_t &D2)
         D1 = ((P_Pa * float(1L << 15) + OFF) * float(1L << 21)) / SENS;
         D2 = dT + (prom[5] << Q5);
     }
+}
 
+void MS5611::check_conversion_accuracy(float P_Pa, float Temp_C, uint32_t D1, uint32_t D2)
+{
     float f_P_Pa;
     float f_Temp_C;
     convert_forward(D1, D2, f_P_Pa, f_Temp_C);
@@ -101,7 +104,6 @@ void MS5611::convert(float P_Pa, float Temp_C, uint32_t &D1, uint32_t &D2)
     }
 }
 
-
 void MS5611::get_pressure_temperature_readings(float &P_Pa, float &Temp_C)
 {
     float sigma, delta, theta;

libraries/SITL/SIM_MS5611.h

@@ -16,6 +16,7 @@ class MS5611 : public MS5XXX
 
     void convert(float P_Pa, float Temp_C, uint32_t &D1, uint32_t &D2) override;
     void convert_forward(int32_t D1, int32_t D2, float &P_Pa, float &Temp_C) override;
+    void check_conversion_accuracy(float P_Pa, float Temp_C, uint32_t D1, uint32_t D2) override;
 
     void load_prom(uint16_t *_loaded_prom, uint8_t len) const override {
         memcpy(_loaded_prom, prom, len);

libraries/SITL/SIM_MS5XXX.cpp

@@ -34,6 +34,10 @@ void MS5XXX::convert_D1()
     uint32_t D1;
     uint32_t D2;
     convert(P_Pa, temperature, D1, D2);
+
+    // Check the accuracy of the returned conversion by utilizing a copy of the drivers
+    check_conversion_accuracy(P_Pa, temperature, D1, D2);
+
     convert_out[2] = D1 & 0xff;
     D1 >>= 8;
     convert_out[1] = D1 & 0xff;

libraries/SITL/SIM_MS5XXX.h

@@ -82,7 +82,7 @@ class MS5XXX : public I2CDevice
     virtual void convert(float P_Pa, float Temp_C, uint32_t &D1, uint32_t &D2) =0;
     virtual void convert_forward(int32_t D1, int32_t D2, float &P_Pa, float &Temp_C) = 0;
     virtual void get_pressure_temperature_readings(float &P_Pa, float &Temp_C) = 0;
-
+    virtual void check_conversion_accuracy(float P_Pa, float Temp_C, uint32_t D1, uint32_t D2) = 0;
 
     void convert_D1();
     void convert_D2();

libraries/SITL/tests/test_sim_ms5525.cpp

@@ -0,0 +1,78 @@
+#include <AP_gtest.h>
+
+#include <SITL/SIM_MS5525.h>
+const AP_HAL::HAL& hal = AP_HAL::get_HAL();
+
+using namespace SITL;
+
+// Dummy class to access protected functions through a public interface for unit tests
+class dummy : public SITL::MS5525
+{
+public:
+    void convert_forward_pub(int32_t D1, int32_t D2, float &P_Pa, float &Temp_C);
+    void convert_pub(float P_Pa, float Temp_C, uint32_t &D1, uint32_t &D2);
+
+};
+
+void dummy::convert_forward_pub(int32_t D1, int32_t D2, float &P_Pa, float &Temp_C)
+{
+    dummy::convert_forward(D1, D2, P_Pa, Temp_C);
+}
+
+void dummy::convert_pub(float P_Pa, float Temp_C, uint32_t &D1, uint32_t &D2)
+{
+    dummy::convert(P_Pa, Temp_C, D1, D2);
+}
+
+const struct forward_check {
+        uint32_t D1; // pressure bytes
+        uint32_t D2; // temperature bytes
+        float P_Pa;
+        float Temp_C;
+    } conversion_table [] = {
+        {9453147, 3438567, 245, -20},           // Negative Temperatures, Positive Pressure
+
+        {9407591, 3438567, -245, -20},          // Negative Temperatures, Negative Pressure
+
+        {9153883, 3774904, 245, 15.15},         // < 20 deg C Temperatures, Positive Pressure
+
+        {9112431, 3774904, -245, 15.15},        // < 20 deg C Temperatures, Negative Pressure
+
+        {8979245, 3976802, 1.2250, 36.25},      // > 20 deg C Temperature, Positive Pressure
+        {8998809, 3976802, 245, 36.25},         // > 20 deg C Temperature, Positive Pressure
+        {9500811, 3976802, 6500, 36.25},        // > 20 deg C Temperature, Positive Pressure
+
+        {8959483, 3976802, -245, 36.25},        // > 20 deg C Temperature, Negative Pressure
+    };
+
+TEST(MS5525, convert_forward)
+{
+    dummy ms5525;
+    float accuracy_Pa = 0.05;
+    float accuracy_Temp_C = 0.01;
+
+    for(auto elem: conversion_table) {
+        float P_Pa;
+        float Temp_C;
+        ms5525.convert_forward_pub(elem.D1, elem.D2, P_Pa, Temp_C);
+        EXPECT_NEAR(P_Pa, elem.P_Pa, accuracy_Pa);
+        EXPECT_NEAR(Temp_C, elem.Temp_C, accuracy_Temp_C);
+    }
+}
+
+TEST(MS5525, convert)
+{
+    dummy ms5525;
+
+    for(auto elem: conversion_table) {
+        uint32_t D1;
+        uint32_t D2;
+        ms5525.convert_pub(elem.P_Pa, elem.Temp_C, D1, D2);
+
+        // Expect NEAR here instead of EQ because in 32bit they are off by 1
+        EXPECT_NEAR(D1, elem.D1, 1);
+        EXPECT_NEAR(D2, elem.D2, 1);
+    }
+}
+
+AP_GTEST_MAIN()