diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index b96fe0d5b3..c1f789a732 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -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; diff --git a/libraries/AP_Baro/AP_Baro.h b/libraries/AP_Baro/AP_Baro.h index ebfc5e7ae1..ee51fea1bd 100644 --- a/libraries/AP_Baro/AP_Baro.h +++ b/libraries/AP_Baro/AP_Baro.h @@ -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); }; diff --git a/libraries/AP_Baro/AP_Baro_MS5611.cpp b/libraries/AP_Baro/AP_Baro_MS5611.cpp index 66876153d3..45a423e15e 100644 --- a/libraries/AP_Baro/AP_Baro_MS5611.cpp +++ b/libraries/AP_Baro/AP_Baro_MS5611.cpp @@ -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 dev) +AP_Baro_MS56XX::AP_Baro_MS56XX(AP_Baro &baro, AP_HAL::OwnPtr 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 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(®, 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 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 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 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; diff --git a/libraries/AP_Baro/AP_Baro_MS5611.h b/libraries/AP_Baro/AP_Baro_MS5611.h index 6bb81f8a62..e230f4591a 100644 --- a/libraries/AP_Baro/AP_Baro_MS5611.h +++ b/libraries/AP_Baro/AP_Baro_MS5611.h @@ -6,12 +6,24 @@ #include #include +#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 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 dev); - void _init(); + AP_Baro_MS56XX(AP_Baro &baro, AP_HAL::OwnPtr dev, enum MS56XX_TYPE ms56xx_type); + virtual ~AP_Baro_MS56XX(void) {}; + + bool _init(); - virtual void _calculate() = 0; - virtual bool _read_prom(uint16_t prom[8]); + 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 dev); -protected: - void _calculate() override; -}; - -class AP_Baro_MS5607 : public AP_Baro_MS56XX -{ -public: - AP_Baro_MS5607(AP_Baro &baro, AP_HAL::OwnPtr dev); -protected: - void _calculate() override; -}; - -class AP_Baro_MS5637 : public AP_Baro_MS56XX -{ -public: - AP_Baro_MS5637(AP_Baro &baro, AP_HAL::OwnPtr dev); -protected: - void _calculate() override; - bool _read_prom(uint16_t prom[8]) override; + enum MS56XX_TYPE _ms56xx_type; };