Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Baro: Add support for water pressure and Sub

This commit is contained in:
Jacob Walser 2017-02-03 15:18:25 +11:00 committed by Andrew Tridgell
parent 3c6df7cc0e
commit 613bc46592
4 changed files with 139 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

75
libraries/AP_Baro/AP_Baro.cpp
View File

@ -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"
@ -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) {

19
libraries/AP_Baro/AP_Baro.h
View File

@ -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?

44
libraries/AP_Baro/AP_Baro_MS5611.cpp
View File

@ -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);
}

8
libraries/AP_Baro/AP_Baro_MS5611.h
View File

@ -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]);