AP_Baro: Add support for water pressure and Sub

libraries/AP_Baro/AP_Baro.cpp

@@ -25,6 +25,7 @@
 #include <AP_HAL/AP_HAL.h>
 #include <AP_Math/AP_Math.h>
 #include <AP_BoardConfig/AP_BoardConfig.h>
+#include <AP_Vehicle/AP_Vehicle_Type.h>
 
 #include "AP_Baro_BMP085.h"
 #include "AP_Baro_BMP280.h"
@@ -86,6 +87,56 @@ const AP_Param::GroupInfo AP_Baro::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("EXT_BUS", 7, AP_Baro, _ext_bus, -1),
 
+    // @Param: SPEC_GRAV
+    // @DisplayName: Specific Gravity (For water depth measurement)
+    // @Description: This sets the specific gravity of the fluid when flying an underwater ROV.
+    // @Values: 1.0:Freshwater,1.024:Saltwater
+    AP_GROUPINFO_FRAME("SPEC_GRAV", 8, AP_Baro, _specific_gravity, 1.0, AP_PARAM_FRAME_SUB),
+
+#if BARO_MAX_INSTANCES > 1
+    // @Param: ABS_PRESS2
+    // @DisplayName: Absolute Pressure
+    // @Description: calibrated ground pressure in Pascals
+    // @Units: pascals
+    // @Increment: 1
+    // @ReadOnly: True
+    // @Volatile: True
+    // @User: Advanced
+    AP_GROUPINFO("ABS_PRESS2", 9, AP_Baro, sensors[1].ground_pressure, 0),
+
+    // @Param: TEMP2
+    // @DisplayName: ground temperature
+    // @Description: calibrated ground temperature in degrees Celsius
+    // @Units: degrees celsius
+    // @Increment: 1
+    // @ReadOnly: True
+    // @Volatile: True
+    // @User: Advanced
+    AP_GROUPINFO("TEMP2", 10, AP_Baro, sensors[1].ground_temperature, 0),
+#endif
+
+#if BARO_MAX_INSTANCES > 2
+    // @Param: ABS_PRESS3
+    // @DisplayName: Absolute Pressure
+    // @Description: calibrated ground pressure in Pascals
+    // @Units: pascals
+    // @Increment: 1
+    // @ReadOnly: True
+    // @Volatile: True
+    // @User: Advanced
+    AP_GROUPINFO("ABS_PRESS3", 11, AP_Baro, sensors[2].ground_pressure, 0),
+
+    // @Param: TEMP3
+    // @DisplayName: ground temperature
+    // @Description: calibrated ground temperature in degrees Celsius
+    // @Units: degrees celsius
+    // @Increment: 1
+    // @ReadOnly: True
+    // @Volatile: True
+    // @User: Advanced
+    AP_GROUPINFO("TEMP3", 12, AP_Baro, sensors[2].ground_temperature, 0),
+#endif
+
     AP_GROUPEND
 };
 
@@ -99,7 +150,7 @@ AP_Baro::AP_Baro()
 
 // calibrate the barometer. This must be called at least once before
 // the altitude() or climb_rate() interfaces can be used
-void AP_Baro::calibrate()
+void AP_Baro::calibrate(bool save)
 {
     // reset the altitude offset when we calibrate. The altitude
     // offset is supposed to be for within a flight
@@ -156,10 +207,12 @@ void AP_Baro::calibrate()
         if (count[i] == 0) {
             sensors[i].calibrated = false;
         } else {
+            if (save) {
                 sensors[i].ground_pressure.set_and_save(sum_pressure[i] / count[i]);
                 sensors[i].ground_temperature.set_and_save(sum_temperature[i] / count[i]);
             }
         }
+    }
 
     // panic if all sensors are not calibrated
     for (uint8_t i=0; i<_num_sensors; i++) {
@@ -393,8 +446,13 @@ void AP_Baro::init(void)
 
     // can optionally have baro on I2C too
     if (_ext_bus >= 0) {
+#if APM_BUILD_TYPE(APM_BUILD_ArduSub)
+        ADD_BACKEND(AP_Baro_MS56XX::probe(*this,
+                                          std::move(hal.i2c_mgr->get_device(_ext_bus, HAL_BARO_MS5837_I2C_ADDR)), AP_Baro_MS56XX::BARO_MS5837));
+#else
         ADD_BACKEND(AP_Baro_MS56XX::probe(*this,
                                           std::move(hal.i2c_mgr->get_device(_ext_bus, HAL_BARO_MS5611_I2C_ADDR))));
+#endif
     }
     
     if (_num_drivers == 0 || _num_sensors == 0 || drivers[0] == nullptr) {
@@ -436,7 +494,14 @@ void AP_Baro::update(void)
             if (is_zero(ground_pressure) || isnan(ground_pressure) || isinf(ground_pressure)) {
                 sensors[i].ground_pressure = sensors[i].pressure;
             }
-            float altitude = get_altitude_difference(sensors[i].ground_pressure, sensors[i].pressure);
+            float altitude = sensors[i].altitude;
+            if (sensors[i].type == BARO_TYPE_AIR) {
+                altitude = get_altitude_difference(sensors[i].ground_pressure, sensors[i].pressure);
+            } else if (sensors[i].type == BARO_TYPE_WATER) {
+                //101325Pa is sea level air pressure, 9800 Pascal/ m depth in water.
+                //No temperature or depth compensation for density of water.
+                altitude = (sensors[i].ground_pressure - sensors[i].pressure) / 9800.0f / _specific_gravity;
+            }
             // sanity check altitude
             sensors[i].alt_ok = !(isnan(altitude) || isinf(altitude));
             if (sensors[i].alt_ok) {

libraries/AP_Baro/AP_Baro.h

@@ -22,6 +22,12 @@ public:
     // constructor
     AP_Baro();
 
+    // barometer types
+    typedef enum {
+        BARO_TYPE_AIR,
+        BARO_TYPE_WATER
+    } baro_type_t;
+
     // initialise the barometer object, loading backend drivers
     void init(void);
 
@@ -50,7 +56,7 @@ public:
 
     // calibrate the barometer. This must be called on startup if the
     // altitude/climb_rate/acceleration interfaces are ever used
-    void calibrate(void);
+    void calibrate(bool save=true);
 
     // update the barometer calibration to the current pressure. Can
     // be used for incremental preflight update of baro
@@ -107,6 +113,15 @@ public:
     // used by Replay
     void setHIL(uint8_t instance, float pressure, float temperature, float altitude, float climb_rate, uint32_t last_update_ms);
 
+    // Set the primary baro
+    void set_primary_baro(uint8_t primary) { _primary_baro.set_and_save(primary); };
+
+    // Set the type (Air or Water) of a particular instance
+    void set_type(uint8_t instance, baro_type_t type) { sensors[instance].type = type; };
+
+    // Get the type (Air or Water) of a particular instance
+    baro_type_t get_type(uint8_t instance) { return sensors[instance].type; };
+
     // HIL variables
     struct {
         float pressure;
@@ -147,6 +162,7 @@ private:
     uint8_t _primary;
 
     struct sensor {
+        baro_type_t type;                   // 0 for air pressure (default), 1 for water pressure
         uint32_t last_update_ms;        // last update time in ms
         bool healthy:1;                 // true if sensor is healthy
         bool alt_ok:1;                  // true if calculated altitude is ok
@@ -167,6 +183,7 @@ private:
     float                               _external_temperature;
     uint32_t                            _last_external_temperature_ms;
     DerivativeFilterFloat_Size7         _climb_rate_filter;
+    AP_Float                            _specific_gravity; // the specific gravity of fluid for an ROV 1.00 for freshwater, 1.024 for salt water
     bool                                _hil_mode:1;
 
     // when did we last notify the GCS of new pressure reference?

libraries/AP_Baro/AP_Baro_MS5611.cpp

@@ -101,6 +101,10 @@ bool AP_Baro_MS56XX::_init()
     case BARO_MS5611:
         prom_read_ok = _read_prom_5611(prom);
         break;
+    case BARO_MS5837:
+        name = "MS5837";
+        prom_read_ok = _read_prom_5637(prom);
+        break;
     case BARO_MS5637:
         name = "MS5637";
         prom_read_ok = _read_prom_5637(prom);
@@ -130,6 +134,10 @@ bool AP_Baro_MS56XX::_init()
 
     _instance = _frontend.register_sensor();
 
+    if (_ms56xx_type == BARO_MS5837) {
+        _frontend.set_type(_instance, AP_Baro::BARO_TYPE_WATER);
+    }
+
     // lower retries for run
     _dev->set_retries(3);
     
@@ -358,6 +366,8 @@ void AP_Baro_MS56XX::update()
     case BARO_MS5637:
         _calculate_5637();
         break;
+    case BARO_MS5837:
+        _calculate_5837();
     }
 }
 
@@ -463,3 +473,37 @@ void AP_Baro_MS56XX::_calculate_5637()
     float temperature = TEMP * 0.01f;
     _copy_to_frontend(_instance, (float)pressure, temperature);
 }
+
+// Calculate Temperature and compensated Pressure in real units (Celsius degrees*100, mbar*100).
+void AP_Baro_MS56XX::_calculate_5837()
+{
+    int32_t dT, TEMP;
+    int64_t OFF, SENS;
+    int32_t raw_pressure = _D1;
+    int32_t raw_temperature = _D2;
+
+    // Formulas from manufacturer datasheet
+    // sub -15c temperature compensation is not included
+
+    dT = raw_temperature - (((uint32_t)_cal_reg.c5) << 8);
+    TEMP = 2000 + ((int64_t)dT * (int64_t)_cal_reg.c6) / 8388608;
+    OFF = (int64_t)_cal_reg.c2 * (int64_t)65536 + ((int64_t)_cal_reg.c4 * (int64_t)dT) / (int64_t)128;
+    SENS = (int64_t)_cal_reg.c1 * (int64_t)32768 + ((int64_t)_cal_reg.c3 * (int64_t)dT) / (int64_t)256;
+
+    if (TEMP < 2000) {
+        // second order temperature compensation when under 20 degrees C
+        int32_t T2 = ((int64_t)3 * ((int64_t)dT * (int64_t)dT) / (int64_t)8589934592);
+        int64_t aux = (TEMP - 2000) * (TEMP - 2000);
+        int64_t OFF2 = 3 * aux / 2;
+        int64_t SENS2 = 5 * aux / 8;
+
+        TEMP = TEMP - T2;
+        OFF = OFF - OFF2;
+        SENS = SENS - SENS2;
+    }
+
+    int32_t pressure = ((int64_t)raw_pressure * SENS / (int64_t)2097152 - OFF) / (int64_t)8192;
+    pressure = pressure * 10; // MS5837 only reports to 0.1 mbar
+    float temperature = TEMP * 0.01f;
+    _copy_to_frontend(_instance, (float)pressure, temperature);
+}

libraries/AP_Baro/AP_Baro_MS5611.h

@@ -10,6 +10,10 @@
 #define HAL_BARO_MS5611_I2C_ADDR 0x77
 #endif
 
+#ifndef HAL_BARO_MS5837_I2C_ADDR
+#define HAL_BARO_MS5837_I2C_ADDR 0x76
+#endif
+
 class AP_Baro_MS56XX : public AP_Baro_Backend
 {
 public:
@@ -18,7 +22,8 @@ public:
     enum MS56XX_TYPE {
         BARO_MS5611 = 0,
         BARO_MS5607 = 1,
-        BARO_MS5637 = 2
+        BARO_MS5637 = 2,
+        BARO_MS5837 = 3
     };
 
     static AP_Baro_Backend *probe(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> dev, enum MS56XX_TYPE ms56xx_type = BARO_MS5611);
@@ -40,6 +45,7 @@ private:
     void _calculate_5611();
     void _calculate_5607();
     void _calculate_5637();
+    void _calculate_5837();
     bool _read_prom_5611(uint16_t prom[8]);
     bool _read_prom_5637(uint16_t prom[8]);