From 613bc465920c5213432f680cca388c47ea7af0d3 Mon Sep 17 00:00:00 2001 From: Jacob Walser Date: Fri, 3 Feb 2017 15:18:25 +1100 Subject: [PATCH] AP_Baro: Add support for water pressure and Sub --- libraries/AP_Baro/AP_Baro.cpp | 75 ++++++++++++++++++++++++++-- libraries/AP_Baro/AP_Baro.h | 19 ++++++- libraries/AP_Baro/AP_Baro_MS5611.cpp | 44 ++++++++++++++++ libraries/AP_Baro/AP_Baro_MS5611.h | 8 ++- 4 files changed, 139 insertions(+), 7 deletions(-) diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index 95fc329e04..9a002a1ddc 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -25,6 +25,7 @@ #include #include #include +#include #include "AP_Baro_BMP085.h" #include "AP_Baro_BMP280.h" @@ -85,7 +86,57 @@ const AP_Param::GroupInfo AP_Baro::var_info[] = { // @Values: -1:Disabled,0:Bus0:1:Bus1 // @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,8 +207,10 @@ void AP_Baro::calibrate() if (count[i] == 0) { sensors[i].calibrated = false; } else { - sensors[i].ground_pressure.set_and_save(sum_pressure[i] / count[i]); - sensors[i].ground_temperature.set_and_save(sum_temperature[i] / count[i]); + 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]); + } } } @@ -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) { diff --git a/libraries/AP_Baro/AP_Baro.h b/libraries/AP_Baro/AP_Baro.h index e3dedc0116..1e3e5bd3c4 100644 --- a/libraries/AP_Baro/AP_Baro.h +++ b/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? diff --git a/libraries/AP_Baro/AP_Baro_MS5611.cpp b/libraries/AP_Baro/AP_Baro_MS5611.cpp index 25674ceac1..66ab261776 100644 --- a/libraries/AP_Baro/AP_Baro_MS5611.cpp +++ b/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); +} diff --git a/libraries/AP_Baro/AP_Baro_MS5611.h b/libraries/AP_Baro/AP_Baro_MS5611.h index d74b136003..b81900fd61 100644 --- a/libraries/AP_Baro/AP_Baro_MS5611.h +++ b/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 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]);