diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index 12ada6c99b..967d87dc62 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -522,6 +522,10 @@ void AP_Baro::init(void) #elif HAL_BARO_DEFAULT == HAL_BARO_LPS25H ADD_BACKEND(AP_Baro_LPS2XH::probe(*this, std::move(hal.i2c_mgr->get_device(HAL_BARO_LPS25H_I2C_BUS, HAL_BARO_LPS25H_I2C_ADDR)))); +#elif HAL_BARO_DEFAULT == HAL_BARO_LPS25H_IMU_I2C + ADD_BACKEND(AP_Baro_LPS2XH::probe_InvensenseIMU(*this, + std::move(hal.i2c_mgr->get_device(HAL_BARO_LPS25H_I2C_BUS, HAL_BARO_LPS25H_I2C_ADDR)), + HAL_BARO_LPS25H_I2C_IMU_ADDR)); #elif HAL_BARO_DEFAULT == HAL_BARO_20789_I2C_I2C ADD_BACKEND(AP_Baro_ICM20789::probe(*this, std::move(hal.i2c_mgr->get_device(HAL_BARO_20789_I2C_BUS, HAL_BARO_20789_I2C_ADDR_PRESS)), diff --git a/libraries/AP_Baro/AP_Baro_LPS2XH.cpp b/libraries/AP_Baro/AP_Baro_LPS2XH.cpp index 5dcd8a9038..b03966f5a7 100644 --- a/libraries/AP_Baro/AP_Baro_LPS2XH.cpp +++ b/libraries/AP_Baro/AP_Baro_LPS2XH.cpp @@ -16,8 +16,15 @@ #include #include "AP_Baro_LPS2XH.h" + +#include + extern const AP_HAL::HAL &hal; +// WHOAMI values +#define LPS22HB_WHOAMI 0xB1 +#define LPS25HB_WHOAMI 0xBD + #define REG_ID 0x0F #define LPS22H_ID 0xB1 @@ -66,6 +73,62 @@ AP_Baro_Backend *AP_Baro_LPS2XH::probe(AP_Baro &baro, return sensor; } +AP_Baro_Backend *AP_Baro_LPS2XH::probe_InvensenseIMU(AP_Baro &baro, + AP_HAL::OwnPtr dev, + uint8_t imu_address) +{ + if (!dev) { + return nullptr; + } + AP_Baro_LPS2XH *sensor = new AP_Baro_LPS2XH(baro, std::move(dev)); + if (sensor) { + if (!sensor->_imu_i2c_init(imu_address)) { + delete sensor; + return nullptr; + } + } + if (!sensor || !sensor->_init()) { + delete sensor; + return nullptr; + } + return sensor; +} + +/* + setup invensense IMU to enable barometer, assuming both IMU and baro + on the same i2c bus +*/ +bool AP_Baro_LPS2XH::_imu_i2c_init(uint8_t imu_address) +{ + if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + return false; + } + // as the baro device is already locked we need to re-use it, + // changing its address to match the IMU address + uint8_t old_address = _dev->get_bus_address(); + _dev->set_address(imu_address); + + _dev->set_retries(4); + + uint8_t whoami=0; + _dev->read_registers(MPUREG_WHOAMI, &whoami, 1); + hal.console->printf("IMU: whoami 0x%02x old_address=%02x\n", whoami, old_address); + + _dev->write_register(MPUREG_FIFO_EN, 0x00); + _dev->write_register(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_CLK_XGYRO); + + // wait for sensor to settle + hal.scheduler->delay(10); + + _dev->write_register(MPUREG_INT_PIN_CFG, BIT_BYPASS_EN); + + _dev->set_address(old_address); + + _dev->get_semaphore()->give(); + + return true; +} + bool AP_Baro_LPS2XH::_init() { if (!_dev || !_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { diff --git a/libraries/AP_Baro/AP_Baro_LPS2XH.h b/libraries/AP_Baro/AP_Baro_LPS2XH.h index e4936afe1a..627ceff6c7 100644 --- a/libraries/AP_Baro/AP_Baro_LPS2XH.h +++ b/libraries/AP_Baro/AP_Baro_LPS2XH.h @@ -24,7 +24,7 @@ public: void update(); static AP_Baro_Backend *probe(AP_Baro &baro, AP_HAL::OwnPtr dev); - + static AP_Baro_Backend *probe_InvensenseIMU(AP_Baro &baro, AP_HAL::OwnPtr dev, uint8_t imu_address); private: virtual ~AP_Baro_LPS2XH(void) {}; @@ -33,8 +33,9 @@ private: void _timer(void); void _update_temperature(void); void _update_pressure(void); + bool _imu_i2c_init(uint8_t imu_address); - bool _check_whoami(void); + bool _check_whoami(void); AP_HAL::OwnPtr _dev; @@ -45,9 +46,5 @@ private: uint32_t CallTime = 0; - enum LPS2XH_TYPE _lps2xh_type; - - // WHOAMI values -#define LPS22HB_WHOAMI 0xB1 -#define LPS25HB_WHOAMI 0xBD + enum LPS2XH_TYPE _lps2xh_type; };