mirror of https://github.com/ArduPilot/ardupilot
AP_Baro: Add support for water pressure and Sub
This commit is contained in:
parent
3c6df7cc0e
commit
613bc46592
|
@ -25,6 +25,7 @@
|
||||||
#include <AP_HAL/AP_HAL.h>
|
#include <AP_HAL/AP_HAL.h>
|
||||||
#include <AP_Math/AP_Math.h>
|
#include <AP_Math/AP_Math.h>
|
||||||
#include <AP_BoardConfig/AP_BoardConfig.h>
|
#include <AP_BoardConfig/AP_BoardConfig.h>
|
||||||
|
#include <AP_Vehicle/AP_Vehicle_Type.h>
|
||||||
|
|
||||||
#include "AP_Baro_BMP085.h"
|
#include "AP_Baro_BMP085.h"
|
||||||
#include "AP_Baro_BMP280.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
|
// @Values: -1:Disabled,0:Bus0:1:Bus1
|
||||||
// @User: Advanced
|
// @User: Advanced
|
||||||
AP_GROUPINFO("EXT_BUS", 7, AP_Baro, _ext_bus, -1),
|
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
|
AP_GROUPEND
|
||||||
};
|
};
|
||||||
|
|
||||||
|
@ -99,7 +150,7 @@ AP_Baro::AP_Baro()
|
||||||
|
|
||||||
// calibrate the barometer. This must be called at least once before
|
// calibrate the barometer. This must be called at least once before
|
||||||
// the altitude() or climb_rate() interfaces can be used
|
// 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
|
// reset the altitude offset when we calibrate. The altitude
|
||||||
// offset is supposed to be for within a flight
|
// offset is supposed to be for within a flight
|
||||||
|
@ -156,8 +207,10 @@ void AP_Baro::calibrate()
|
||||||
if (count[i] == 0) {
|
if (count[i] == 0) {
|
||||||
sensors[i].calibrated = false;
|
sensors[i].calibrated = false;
|
||||||
} else {
|
} else {
|
||||||
sensors[i].ground_pressure.set_and_save(sum_pressure[i] / count[i]);
|
if (save) {
|
||||||
sensors[i].ground_temperature.set_and_save(sum_temperature[i] / count[i]);
|
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
|
// can optionally have baro on I2C too
|
||||||
if (_ext_bus >= 0) {
|
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,
|
ADD_BACKEND(AP_Baro_MS56XX::probe(*this,
|
||||||
std::move(hal.i2c_mgr->get_device(_ext_bus, HAL_BARO_MS5611_I2C_ADDR))));
|
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) {
|
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)) {
|
if (is_zero(ground_pressure) || isnan(ground_pressure) || isinf(ground_pressure)) {
|
||||||
sensors[i].ground_pressure = sensors[i].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
|
// sanity check altitude
|
||||||
sensors[i].alt_ok = !(isnan(altitude) || isinf(altitude));
|
sensors[i].alt_ok = !(isnan(altitude) || isinf(altitude));
|
||||||
if (sensors[i].alt_ok) {
|
if (sensors[i].alt_ok) {
|
||||||
|
|
|
@ -22,6 +22,12 @@ public:
|
||||||
// constructor
|
// constructor
|
||||||
AP_Baro();
|
AP_Baro();
|
||||||
|
|
||||||
|
// barometer types
|
||||||
|
typedef enum {
|
||||||
|
BARO_TYPE_AIR,
|
||||||
|
BARO_TYPE_WATER
|
||||||
|
} baro_type_t;
|
||||||
|
|
||||||
// initialise the barometer object, loading backend drivers
|
// initialise the barometer object, loading backend drivers
|
||||||
void init(void);
|
void init(void);
|
||||||
|
|
||||||
|
@ -50,7 +56,7 @@ public:
|
||||||
|
|
||||||
// calibrate the barometer. This must be called on startup if the
|
// calibrate the barometer. This must be called on startup if the
|
||||||
// altitude/climb_rate/acceleration interfaces are ever used
|
// 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
|
// update the barometer calibration to the current pressure. Can
|
||||||
// be used for incremental preflight update of baro
|
// be used for incremental preflight update of baro
|
||||||
|
@ -107,6 +113,15 @@ public:
|
||||||
// used by Replay
|
// used by Replay
|
||||||
void setHIL(uint8_t instance, float pressure, float temperature, float altitude, float climb_rate, uint32_t last_update_ms);
|
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
|
// HIL variables
|
||||||
struct {
|
struct {
|
||||||
float pressure;
|
float pressure;
|
||||||
|
@ -147,6 +162,7 @@ private:
|
||||||
uint8_t _primary;
|
uint8_t _primary;
|
||||||
|
|
||||||
struct sensor {
|
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
|
uint32_t last_update_ms; // last update time in ms
|
||||||
bool healthy:1; // true if sensor is healthy
|
bool healthy:1; // true if sensor is healthy
|
||||||
bool alt_ok:1; // true if calculated altitude is ok
|
bool alt_ok:1; // true if calculated altitude is ok
|
||||||
|
@ -167,6 +183,7 @@ private:
|
||||||
float _external_temperature;
|
float _external_temperature;
|
||||||
uint32_t _last_external_temperature_ms;
|
uint32_t _last_external_temperature_ms;
|
||||||
DerivativeFilterFloat_Size7 _climb_rate_filter;
|
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;
|
bool _hil_mode:1;
|
||||||
|
|
||||||
// when did we last notify the GCS of new pressure reference?
|
// when did we last notify the GCS of new pressure reference?
|
||||||
|
|
|
@ -101,6 +101,10 @@ bool AP_Baro_MS56XX::_init()
|
||||||
case BARO_MS5611:
|
case BARO_MS5611:
|
||||||
prom_read_ok = _read_prom_5611(prom);
|
prom_read_ok = _read_prom_5611(prom);
|
||||||
break;
|
break;
|
||||||
|
case BARO_MS5837:
|
||||||
|
name = "MS5837";
|
||||||
|
prom_read_ok = _read_prom_5637(prom);
|
||||||
|
break;
|
||||||
case BARO_MS5637:
|
case BARO_MS5637:
|
||||||
name = "MS5637";
|
name = "MS5637";
|
||||||
prom_read_ok = _read_prom_5637(prom);
|
prom_read_ok = _read_prom_5637(prom);
|
||||||
|
@ -130,6 +134,10 @@ bool AP_Baro_MS56XX::_init()
|
||||||
|
|
||||||
_instance = _frontend.register_sensor();
|
_instance = _frontend.register_sensor();
|
||||||
|
|
||||||
|
if (_ms56xx_type == BARO_MS5837) {
|
||||||
|
_frontend.set_type(_instance, AP_Baro::BARO_TYPE_WATER);
|
||||||
|
}
|
||||||
|
|
||||||
// lower retries for run
|
// lower retries for run
|
||||||
_dev->set_retries(3);
|
_dev->set_retries(3);
|
||||||
|
|
||||||
|
@ -358,6 +366,8 @@ void AP_Baro_MS56XX::update()
|
||||||
case BARO_MS5637:
|
case BARO_MS5637:
|
||||||
_calculate_5637();
|
_calculate_5637();
|
||||||
break;
|
break;
|
||||||
|
case BARO_MS5837:
|
||||||
|
_calculate_5837();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -463,3 +473,37 @@ void AP_Baro_MS56XX::_calculate_5637()
|
||||||
float temperature = TEMP * 0.01f;
|
float temperature = TEMP * 0.01f;
|
||||||
_copy_to_frontend(_instance, (float)pressure, temperature);
|
_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);
|
||||||
|
}
|
||||||
|
|
|
@ -10,6 +10,10 @@
|
||||||
#define HAL_BARO_MS5611_I2C_ADDR 0x77
|
#define HAL_BARO_MS5611_I2C_ADDR 0x77
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#ifndef HAL_BARO_MS5837_I2C_ADDR
|
||||||
|
#define HAL_BARO_MS5837_I2C_ADDR 0x76
|
||||||
|
#endif
|
||||||
|
|
||||||
class AP_Baro_MS56XX : public AP_Baro_Backend
|
class AP_Baro_MS56XX : public AP_Baro_Backend
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
|
@ -18,7 +22,8 @@ public:
|
||||||
enum MS56XX_TYPE {
|
enum MS56XX_TYPE {
|
||||||
BARO_MS5611 = 0,
|
BARO_MS5611 = 0,
|
||||||
BARO_MS5607 = 1,
|
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);
|
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_5611();
|
||||||
void _calculate_5607();
|
void _calculate_5607();
|
||||||
void _calculate_5637();
|
void _calculate_5637();
|
||||||
|
void _calculate_5837();
|
||||||
bool _read_prom_5611(uint16_t prom[8]);
|
bool _read_prom_5611(uint16_t prom[8]);
|
||||||
bool _read_prom_5637(uint16_t prom[8]);
|
bool _read_prom_5637(uint16_t prom[8]);
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue