SITL: add subclass for MS5525

libraries/SITL/SIM_MS5525.cpp

@@ -0,0 +1,61 @@
+#include "SIM_MS5525.h"
+
+#include <SITL/SITL.h>
+
+using namespace SITL;
+
+void MS5525::convert_forward(int32_t D1, int32_t D2, float &P_Pa, float &Temp_C)
+{
+    const uint8_t Q1 = Qx_coeff[0];
+    const uint8_t Q2 = Qx_coeff[1];
+    const uint8_t Q3 = Qx_coeff[2];
+    const uint8_t Q4 = Qx_coeff[3];
+    const uint8_t Q5 = Qx_coeff[4];
+    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);
+    const float PSI_to_Pa = 6894.757f;
+    P_Pa = PSI_to_Pa * 1.0e-4 * P;
+    Temp_C = TEMP * 0.01;
+}
+
+void MS5525::convert(float P_Pa, float Temp_C, uint32_t &D1, uint32_t &D2)
+{
+    const uint8_t Q1 = Qx_coeff[0];
+    const uint8_t Q2 = Qx_coeff[1];
+    const uint8_t Q3 = Qx_coeff[2];
+    const uint8_t Q4 = Qx_coeff[3];
+    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 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);
+
+
+    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");
+    }
+    if (fabs(f_Temp_C - Temp_C) > 0.1) {
+        AP_HAL::panic("Invalid conversion");
+    }
+}
+
+void MS5525::get_pressure_temperature_readings(float &P_Pa, float &Temp_C)
+{
+    Temp_C = 25.0f;
+    P_Pa = AP::sitl()->state.airspeed_raw_pressure[0];
+}

libraries/SITL/SIM_MS5525.h

@@ -0,0 +1,44 @@
+#include "SIM_MS5XXX.h"
+
+#include <AP_Common/Bitmask.h>
+
+namespace SITL {
+
+class MS5525 : public MS5XXX
+{
+public:
+
+protected:
+
+    void get_pressure_temperature_readings(float &P_Pa, float &Temp_C) override;
+
+    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 load_prom(uint16_t *_loaded_prom, uint8_t len) const override {
+        memcpy(_loaded_prom, prom, len);
+    }
+
+private:
+
+    // this data comes from the datasheet page 7
+    const uint16_t prom[8] {
+        0xFFFF,  // reserved
+        36402, // C1, pressure sensitivity
+        39473, // C2, pressure offset
+        40393, // C3, temperature coeff of press sensit
+        29523, // C4, temperature cofff of press offs
+        29854, // C5, ref temperature
+        21917,  // C6, temperature coeff of temperature
+        0x000c  // checksum
+    };
+
+    // for 5525DSO-pp001DS
+    const uint8_t Qx_coeff[6] {
+        15, 17, 7, 5, 7, 21
+    };
+
+
+};
+
+} // namespace SITL