SITL: MS5XXX drivers use check_conversion_accuracy

libraries/SITL/SIM_MS5525.cpp

@@ -44,15 +44,22 @@ void MS5525::convert(float P_Pa, float Temp_C, uint32_t &D1, uint32_t &D2)
     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) > 0.1) {
-        AP_HAL::panic("Invalid pressure 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.01) {
-        AP_HAL::panic("Invalid temperature 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);
     }
 }
 

libraries/SITL/SIM_MS5525.h

@@ -14,6 +14,7 @@ protected:
 
     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 @@ protected:
 
     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 @@ private:
     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();