AP_Baro: support LPS25H on MPU9250 AUX

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)),

libraries/AP_Baro/AP_Baro_LPS2XH.cpp

@@ -16,8 +16,15 @@
 #include <stdio.h>
 
 #include "AP_Baro_LPS2XH.h"
+
+#include <AP_InertialSensor/AP_InertialSensor_Invensense_registers.h>
+
 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<AP_HAL::Device> 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)) {

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<AP_HAL::Device> dev);
-
+    static AP_Baro_Backend *probe_InvensenseIMU(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> dev, uint8_t imu_address);
 
 private:
     virtual ~AP_Baro_LPS2XH(void) {};
@@ -33,6 +33,7 @@ private:
     void _timer(void);
     void _update_temperature(void);
     void _update_pressure(void);
+    bool _imu_i2c_init(uint8_t imu_address);
 
     bool _check_whoami(void);
 
@@ -46,8 +47,4 @@ private:
     uint32_t CallTime = 0;
 
     enum LPS2XH_TYPE _lps2xh_type;
-
-   // WHOAMI values
-#define   LPS22HB_WHOAMI     0xB1
-#define 	 LPS25HB_WHOAMI		0xBD
 };