Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 12:14:10 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Baro: added MS5611 probing, and support 3 baros on Pixhawk2

Browse Source 
this adds sensor probing for barometers, simplifies the MS5611 driver,
and adds support for the I2C external barometers on Pixhawk2
This commit is contained in:
Andrew Tridgell 2016-11-25 11:59:14 +11:00
parent 954728c9e9
commit f2246326bb
4 changed files with 143 additions and 94 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

75
libraries/AP_Baro/AP_Baro.cpp
View File

@ -280,6 +280,30 @@ float AP_Baro::get_calibration_temperature(uint8_t instance) const
}
bool AP_Baro::_add_backend(AP_Baro_Backend *backend)
{
if (!backend) {
return false;
}
if (_num_drivers >= BARO_MAX_DRIVERS) {
AP_HAL::panic("Too many barometer drivers");
}
drivers[_num_drivers++] = backend;
return true;
}
/*
macro to add a backend with check for too many sensors
We don't try to start more than the maximum allowed
*/
#define ADD_BACKEND(backend) \
do { _add_backend(backend); \
if (_num_drivers == BARO_MAX_DRIVERS || \
_num_sensors == BARO_MAX_INSTANCES) { \
return; \
} \
} while (0)
/*
initialise the barometer object, loading backend drivers
*/
@ -294,26 +318,27 @@ void AP_Baro::init(void)
#if HAL_BARO_DEFAULT == HAL_BARO_PX4 || HAL_BARO_DEFAULT == HAL_BARO_VRBRAIN
if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PX4V1) {
#ifdef HAL_BARO_MS5611_I2C_BUS
drivers[0] = new AP_Baro_MS5611(*this,
std::move(hal.i2c_mgr->get_device(HAL_BARO_MS5611_I2C_BUS, HAL_BARO_MS5611_I2C_ADDR)));
_num_drivers = 1;
ADD_BACKEND(AP_Baro_MS56XX::probe(*this,
std::move(hal.i2c_mgr->get_device(HAL_BARO_MS5611_I2C_BUS, HAL_BARO_MS5611_I2C_ADDR))));
#endif
} else if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK ||
AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PHMINI ||
AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PH2SLIM) {
drivers[0] = new AP_Baro_MS5611(*this,
std::move(hal.spi->get_device(HAL_BARO_MS5611_NAME)));
_num_drivers = 1;
ADD_BACKEND(AP_Baro_MS56XX::probe(*this,
std::move(hal.spi->get_device(HAL_BARO_MS5611_NAME))));
} else if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK2) {
drivers[0] = new AP_Baro_MS5611(*this,
std::move(hal.spi->get_device(HAL_BARO_MS5611_SPI_EXT_NAME)));
drivers[1] = new AP_Baro_MS5611(*this,
std::move(hal.spi->get_device(HAL_BARO_MS5611_NAME)));
_num_drivers = 2;
ADD_BACKEND(AP_Baro_MS56XX::probe(*this,
std::move(hal.spi->get_device(HAL_BARO_MS5611_SPI_EXT_NAME))));
ADD_BACKEND(AP_Baro_MS56XX::probe(*this,
std::move(hal.spi->get_device(HAL_BARO_MS5611_NAME))));
// can have baro on I2C too
ADD_BACKEND(AP_Baro_MS56XX::probe(*this,
std::move(hal.i2c_mgr->get_device(0, HAL_BARO_MS5611_I2C_ADDR))));
ADD_BACKEND(AP_Baro_MS56XX::probe(*this,
std::move(hal.i2c_mgr->get_device(1, HAL_BARO_MS5611_I2C_ADDR))));
} else if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXRACER) {
drivers[0] = new AP_Baro_MS5611(*this,
std::move(hal.spi->get_device(HAL_BARO_MS5611_SPI_INT_NAME)));
_num_drivers = 1;
ADD_BACKEND(AP_Baro_MS56XX::probe(*this,
std::move(hal.spi->get_device(HAL_BARO_MS5611_SPI_INT_NAME))));
} else {
drivers[0] = new AP_Baro_PX4(*this);
_num_drivers = 1;
@ -326,21 +351,19 @@ void AP_Baro::init(void)
std::move(hal.i2c_mgr->get_device(HAL_BARO_BMP085_BUS, HAL_BARO_BMP085_I2C_ADDR)));
_num_drivers = 1;
#elif HAL_BARO_DEFAULT == HAL_BARO_MS5611_I2C
drivers[0] = new AP_Baro_MS5611(*this,
std::move(hal.i2c_mgr->get_device(HAL_BARO_MS5611_I2C_BUS, HAL_BARO_MS5611_I2C_ADDR)));
_num_drivers = 1;
ADD_BACKEND(AP_Baro_MS56XX::probe(*this,
std::move(hal.i2c_mgr->get_device(HAL_BARO_MS5611_I2C_BUS, HAL_BARO_MS5611_I2C_ADDR))));
#elif HAL_BARO_DEFAULT == HAL_BARO_MS5611_SPI
drivers[0] = new AP_Baro_MS5611(*this,
std::move(hal.spi->get_device(HAL_BARO_MS5611_NAME)));
_num_drivers = 1;
ADD_BACKEND(AP_Baro_MS56XX::probe(*this,
std::move(hal.spi->get_device(HAL_BARO_MS5611_NAME))));
#elif HAL_BARO_DEFAULT == HAL_BARO_MS5607_I2C
drivers[0] = new AP_Baro_MS5607(*this,
std::move(hal.i2c_mgr->get_device(HAL_BARO_MS5607_I2C_BUS, HAL_BARO_MS5607_I2C_ADDR)));
_num_drivers = 1;
ADD_BACKEND(AP_Baro_MS56XX::probe(*this,
std::move(hal.i2c_mgr->get_device(HAL_BARO_MS5607_I2C_BUS, HAL_BARO_MS5607_I2C_ADDR)),
AP_Baro_MS56XX::BARO_MS5607));
#elif HAL_BARO_DEFAULT == HAL_BARO_MS5637_I2C
drivers[0] = new AP_Baro_MS5637(*this,
std::move(hal.i2c_mgr->get_device(HAL_BARO_MS5637_I2C_BUS, HAL_BARO_MS5637_I2C_ADDR)));
_num_drivers = 1;
ADD_BACKEND(AP_Baro_MS56XX::probe(*this,
std::move(hal.i2c_mgr->get_device(HAL_BARO_MS5637_I2C_BUS, HAL_BARO_MS5637_I2C_ADDR)),
AP_Baro_MS56XX::BARO_MS5637));
#elif HAL_BARO_DEFAULT == HAL_BARO_QFLIGHT
drivers[0] = new AP_Baro_QFLIGHT(*this);
_num_drivers = 1;

3
libraries/AP_Baro/AP_Baro.h
View File

@ -10,7 +10,7 @@
// maximum number of drivers. Note that a single driver can provide
// multiple sensor instances
#define BARO_MAX_DRIVERS 2
#define BARO_MAX_DRIVERS 3
class AP_Baro_Backend;
@ -172,4 +172,5 @@ private:
uint32_t _last_notify_ms;
void SimpleAtmosphere(const float alt, float &sigma, float &delta, float &theta);
bool _add_backend(AP_Baro_Backend *backend);
};

107
libraries/AP_Baro/AP_Baro_MS5611.cpp
View File

@ -53,17 +53,32 @@ static const uint8_t ADDR_CMD_CONVERT_TEMPERATURE = ADDR_CMD_CONVERT_D2_OSR1024;
/*
constructor
*/
AP_Baro_MS56XX::AP_Baro_MS56XX(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> dev)
AP_Baro_MS56XX::AP_Baro_MS56XX(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> dev, enum MS56XX_TYPE ms56xx_type)
: AP_Baro_Backend(baro)
, _dev(std::move(dev))
, _ms56xx_type(ms56xx_type)
{
}
void AP_Baro_MS56XX::_init()
AP_Baro_Backend *AP_Baro_MS56XX::probe(AP_Baro &baro,
AP_HAL::OwnPtr<AP_HAL::Device> dev,
enum MS56XX_TYPE ms56xx_type)
{
if (!dev) {
return nullptr;
}
AP_Baro_MS56XX *sensor = new AP_Baro_MS56XX(baro, std::move(dev), ms56xx_type);
if (!sensor || !sensor->_init()) {
delete sensor;
return nullptr;
}
return sensor;
}
bool AP_Baro_MS56XX::_init()
{
if (!_dev) {
printf("MS5611: no device available");
return;
return false;
}
if (!_dev->get_semaphore()->take(0)) {
@ -71,10 +86,21 @@ void AP_Baro_MS56XX::_init()
}
uint16_t prom[8];
if (!_read_prom(prom)) {
printf("MS5611: Can't read PROM on bus %d", _dev->get_bus_id());
bool prom_read_ok = false;
switch (_ms56xx_type) {
case BARO_MS5607:
case BARO_MS5611:
prom_read_ok = _read_prom_5611(prom);
break;
case BARO_MS5637:
prom_read_ok = _read_prom_5637(prom);
break;
}
if (!prom_read_ok) {
_dev->get_semaphore()->give();
return;
return false;
}
_dev->transfer(&CMD_MS56XX_RESET, 1, nullptr, 0);
@ -101,6 +127,7 @@ void AP_Baro_MS56XX::_init()
/* Request 100Hz update */
_dev->register_periodic_callback(10 * USEC_PER_MSEC,
FUNCTOR_BIND_MEMBER(&AP_Baro_MS56XX::_timer, bool));
return true;
}
/**
@ -135,7 +162,6 @@ uint16_t AP_Baro_MS56XX::_read_prom_word(uint8_t word)
{
const uint8_t reg = CMD_MS56XX_PROM + (word << 1);
uint8_t val[2];
if (!_dev->transfer(&reg, 1, val, 2)) {
return 0;
}
@ -145,14 +171,13 @@ uint16_t AP_Baro_MS56XX::_read_prom_word(uint8_t word)
uint32_t AP_Baro_MS56XX::_read_adc()
{
uint8_t val[3];
if (!_dev->transfer(&CMD_MS56XX_READ_ADC, 1, val, 3)) {
return 0;
}
return (val[0] << 16) | (val[1] << 8) | val[2];
}
bool AP_Baro_MS56XX::_read_prom(uint16_t prom[8])
bool AP_Baro_MS56XX::_read_prom_5611(uint16_t prom[8])
{
/*
* MS5611-01BA datasheet, CYCLIC REDUNDANCY CHECK (CRC): "MS5611-01BA
@ -161,8 +186,16 @@ bool AP_Baro_MS56XX::_read_prom(uint16_t prom[8])
*
* CRC field must me removed for CRC-4 calculation.
*/
bool all_zero = true;
for (uint8_t i = 0; i < 8; i++) {
prom[i] = _read_prom_word(i);
if (prom[i] != 0) {
all_zero = false;
}
}
if (all_zero) {
return false;
}
/* save the read crc */
@ -174,7 +207,7 @@ bool AP_Baro_MS56XX::_read_prom(uint16_t prom[8])
return crc_read == crc4(prom);
}
bool AP_Baro_MS5637::_read_prom(uint16_t prom[8])
bool AP_Baro_MS56XX::_read_prom_5637(uint16_t prom[8])
{
/*
* MS5637-02BA03 datasheet, CYCLIC REDUNDANCY CHECK (CRC): "MS5637
@ -184,8 +217,16 @@ bool AP_Baro_MS5637::_read_prom(uint16_t prom[8])
* 8th PROM word must be zeroed and CRC field removed for CRC-4
* calculation.
*/
bool all_zero = true;
for (uint8_t i = 0; i < 7; i++) {
prom[i] = _read_prom_word(i);
if (prom[i] != 0) {
all_zero = false;
}
}
if (all_zero) {
return false;
}
prom[7] = 0;
@ -231,8 +272,8 @@ bool AP_Baro_MS56XX::_timer(void)
/* if we had a failed read we are all done */
if (adc_val == 0) {
// a failed read can mean the returned value is corrupt, we
// must discard it
// a failed read can mean the next returned value will be
// corrupt, we must discard it
_discard_next = true;
return true;
}
@ -274,7 +315,7 @@ void AP_Baro_MS56XX::update()
uint32_t sD1, sD2;
uint8_t d1count, d2count;
if (!_sem->take_nonblocking()) {
if (!_sem->take(0)) {
return;
}
@ -297,18 +338,22 @@ void AP_Baro_MS56XX::update()
if (d2count != 0) {
_D2 = ((float)sD2) / d2count;
}
_calculate();
}
/* MS5611 class */
AP_Baro_MS5611::AP_Baro_MS5611(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> dev)
: AP_Baro_MS56XX(baro, std::move(dev))
{
_init();
switch (_ms56xx_type) {
case BARO_MS5607:
_calculate_5607();
break;
case BARO_MS5611:
_calculate_5611();
break;
case BARO_MS5637:
_calculate_5637();
break;
}
}
// Calculate Temperature and compensated Pressure in real units (Celsius degrees*100, mbar*100).
void AP_Baro_MS5611::_calculate()
void AP_Baro_MS56XX::_calculate_5611()
{
float dT;
float TEMP;
@ -342,15 +387,8 @@ void AP_Baro_MS5611::_calculate()
_copy_to_frontend(_instance, pressure, temperature);
}
/* MS5607 Class */
AP_Baro_MS5607::AP_Baro_MS5607(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> dev)
: AP_Baro_MS56XX(baro, std::move(dev))
{
_init();
}
// Calculate Temperature and compensated Pressure in real units (Celsius degrees*100, mbar*100).
void AP_Baro_MS5607::_calculate()
void AP_Baro_MS56XX::_calculate_5607()
{
float dT;
float TEMP;
@ -384,15 +422,8 @@ void AP_Baro_MS5607::_calculate()
_copy_to_frontend(_instance, pressure, temperature);
}
/* MS5637 Class */
AP_Baro_MS5637::AP_Baro_MS5637(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> dev)
: AP_Baro_MS56XX(baro, std::move(dev))
{
_init();
}
// Calculate Temperature and compensated Pressure in real units (Celsius degrees*100, mbar*100).
void AP_Baro_MS5637::_calculate()
void AP_Baro_MS56XX::_calculate_5637()
{
int32_t dT, TEMP;
int64_t OFF, SENS;

52
libraries/AP_Baro/AP_Baro_MS5611.h
View File

@ -6,12 +6,24 @@
#include <AP_HAL/Semaphores.h>
#include <AP_HAL/Device.h>
#ifndef HAL_BARO_MS5611_I2C_ADDR
#define HAL_BARO_MS5611_I2C_ADDR 0x77
#endif
class AP_Baro_MS56XX : public AP_Baro_Backend
{
public:
void update();
protected:
enum MS56XX_TYPE {
BARO_MS5611 = 0,
BARO_MS5607 = 1,
BARO_MS5637 = 2
};
static AP_Baro_Backend *probe(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> dev, enum MS56XX_TYPE ms56xx_type = BARO_MS5611);
private:
/*
* Update @accum and @count with the new sample in @val, taking into
* account a maximum number of samples given by @max_count; in case
@ -20,11 +32,16 @@ protected:
static void _update_and_wrap_accumulator(uint32_t *accum, uint32_t val,
uint8_t *count, uint8_t max_count);
AP_Baro_MS56XX(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> dev);
void _init();
AP_Baro_MS56XX(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> dev, enum MS56XX_TYPE ms56xx_type);
virtual ~AP_Baro_MS56XX(void) {};
virtual void _calculate() = 0;
virtual bool _read_prom(uint16_t prom[8]);
bool _init();
void _calculate_5611();
void _calculate_5607();
void _calculate_5637();
bool _read_prom_5611(uint16_t prom[8]);
bool _read_prom_5637(uint16_t prom[8]);
uint16_t _read_prom_word(uint8_t word);
uint32_t _read_adc();
@ -53,29 +70,6 @@ protected:
} _cal_reg;
bool _discard_next;
};
class AP_Baro_MS5611 : public AP_Baro_MS56XX
{
public:
AP_Baro_MS5611(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> dev);
protected:
void _calculate() override;
};
class AP_Baro_MS5607 : public AP_Baro_MS56XX
{
public:
AP_Baro_MS5607(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> dev);
protected:
void _calculate() override;
};
class AP_Baro_MS5637 : public AP_Baro_MS56XX
{
public:
AP_Baro_MS5637(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> dev);
protected:
void _calculate() override;
bool _read_prom(uint16_t prom[8]) override;
enum MS56XX_TYPE _ms56xx_type;
};