From 5e90649a6e0bd9408ab98b1ccf2d3ad3abca0ac7 Mon Sep 17 00:00:00 2001
From: Josh Henderson <hendjoshsr71@gmail.com>
Date: Mon, 16 Aug 2021 03:14:21 -0400
Subject: [PATCH] SITL: fix MS5525 for temp <20C and negative pressures

---
 libraries/SITL/SIM_MS5525.cpp | 41 +++++++++++++++++++++--------------
 1 file changed, 25 insertions(+), 16 deletions(-)

diff --git a/libraries/SITL/SIM_MS5525.cpp b/libraries/SITL/SIM_MS5525.cpp
index b56ceebccb..ab7b7675c3 100644
--- a/libraries/SITL/SIM_MS5525.cpp
+++ b/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,38 @@ 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);
+}
 
 
     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_P_Pa - P_Pa) > 0.1) {
+        AP_HAL::panic("Invalid pressure conversion");
     }
-    if (fabs(f_Temp_C - Temp_C) > 0.1) {
-        AP_HAL::panic("Invalid conversion");
+    if (fabs(f_Temp_C - Temp_C) > 0.01) {
+        AP_HAL::panic("Invalid temperature conversion");
     }
 }
 
 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];
 }