AP_Baro: Add SimpleUnderWaterAtmosphere function

Signed-off-by: Patrick José Pereira <patrickelectric@gmail.com>

libraries/AP_Baro/AP_Baro.h

@@ -169,6 +169,9 @@ public:
     // simple atmospheric model
     static void SimpleAtmosphere(const float alt, float &sigma, float &delta, float &theta);
 
+    // simple underwater atmospheric model
+    static void SimpleUnderWaterAtmosphere(float alt, float &rho, float &delta, float &theta);
+
     // set a pressure correction from AP_TempCalibration
     void set_pressure_correction(uint8_t instance, float p_correction);
 

libraries/AP_Baro/AP_Baro_HIL.cpp

@@ -40,6 +40,43 @@ void AP_Baro::SimpleAtmosphere(
     sigma = delta/theta;
 }
 
+void AP_Baro::SimpleUnderWaterAtmosphere(
+	float alt,            // depth, km.
+	float& rho,           // density/sea-level
+	float& delta,         // pressure/sea-level standard pressure
+	float& theta)         // temperature/sea-level standard temperature
+{
+    // Values and equations based on:
+    // https://en.wikipedia.org/wiki/Standard_sea_level
+    const float seaDensity = 1.024f;      // g/cm3
+    const float maxSeaDensity = 1.028f;   // g/cm3
+    const float pAC = maxSeaDensity - seaDensity; // pycnocline angular coefficient
+
+    // From: https://www.windows2universe.org/earth/Water/density.html
+    rho = seaDensity;
+    if (alt < 1.0f) {
+        // inside pycnocline
+        rho += pAC*alt;
+    } else {
+        rho += pAC;
+    }
+    rho = rho/seaDensity;
+
+    // From: https://www.grc.nasa.gov/www/k-12/WindTunnel/Activities/fluid_pressure.html
+    // \f$P = \rho (kg) \cdot gravity (m/s2) \cdot depth (m)\f$
+    // \f$P_{atmosphere} = 101.325 kPa\f$
+    // \f$P_{total} = P_{atmosphere} + P_{fluid}\f$
+    const float pAtm = 101325; // Pa
+    delta = (pAtm + (seaDensity * 1e3) * GRAVITY_MSS * (alt * 1e3)) / pAtm;
+
+    // From: http://residualanalysis.blogspot.com.br/2010/02/temperature-of-ocean-water-at-given.html
+    // \f$T(D)\f$ Temperature underwater at given temperature
+    // \f$S\f$ Surface temperature at the surface
+    // \f$T(D)\approx\frac{S}{1.8 \cdot 10^{-4} \cdot S \cdot T + 1}\f$
+    const float seaTempSurface = 15.0f; // Celsius
+    const float S = seaTempSurface * 0.338f;
+    theta = 1.0f / ((1.8e-4) * S * (alt * 1e3) + 1.0f);
+}
 
 /*
   convert an altitude in meters above sea level to a presssure and temperature