AP_InertialSensor: Add MPU9250 multiple instance support

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -512,7 +512,7 @@ AP_InertialSensor::_detect_backends(void)
 #elif HAL_INS_DEFAULT == HAL_INS_OILPAN
     _add_backend(AP_InertialSensor_Oilpan::detect(*this));
 #elif HAL_INS_DEFAULT == HAL_INS_MPU9250
-    _add_backend(AP_InertialSensor_MPU9250::detect(*this));
+    _add_backend(AP_InertialSensor_MPU9250::detect(*this, hal.spi->device(AP_HAL::SPIDevice_MPU9250)));
 #elif HAL_INS_DEFAULT == HAL_INS_FLYMAPLE
     _add_backend(AP_InertialSensor_Flymaple::detect(*this));
 #elif HAL_INS_DEFAULT == HAL_INS_LSM9DS0

libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp

@@ -201,13 +201,13 @@ AP_InertialSensor_MPU9250::AP_InertialSensor_MPU9250(AP_InertialSensor &imu) :
 /*
   detect the sensor
  */
-AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::detect(AP_InertialSensor &_imu)
+AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::detect(AP_InertialSensor &_imu, AP_HAL::SPIDeviceDriver *spi)
 {
     AP_InertialSensor_MPU9250 *sensor = new AP_InertialSensor_MPU9250(_imu);
     if (sensor == NULL) {
         return NULL;
     }
-    if (!sensor->_init_sensor()) {
+    if (!sensor->_init_sensor(spi)) {
         delete sensor;
         return NULL;
     }
@@ -215,8 +215,8 @@ AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::detect(AP_InertialSensor &
     return sensor;
 }
 
-bool AP_InertialSensor_MPU9250::initialize_driver_state() {
+bool AP_InertialSensor_MPU9250::initialize_driver_state(AP_HAL::SPIDeviceDriver *spi) {
-    AP_HAL::SPIDeviceDriver *spi = hal.spi->device(AP_HAL::SPIDevice_MPU9250);
+
     if (!spi)
         return false;
 
@@ -276,7 +276,7 @@ bool AP_InertialSensor_MPU9250::initialize_driver_state() {
             break;
         }
 #if MPU9250_DEBUG
-        _dump_registers(_spi);
+        _dump_registers(spi);
 #endif
     }
 
@@ -299,9 +299,9 @@ fail_whoami:
 /*
   initialise the sensor
  */
-bool AP_InertialSensor_MPU9250::_init_sensor(void)
+bool AP_InertialSensor_MPU9250::_init_sensor(AP_HAL::SPIDeviceDriver *spi)
 {
-    _spi = hal.spi->device(AP_HAL::SPIDevice_MPU9250);
+    _spi = spi;
     _spi_sem = _spi->get_semaphore();
 
     if (!_hardware_init())
@@ -487,7 +487,7 @@ bool AP_InertialSensor_MPU9250::_hardware_init(void)
         return false;
     }
 
-    if (!initialize_driver_state())
+    if (!initialize_driver_state(_spi))
         return false;
 
     _register_write(MPUREG_PWR_MGMT_2, 0x00);            // only used for wake-up in accelerometer only low power mode

libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h

@@ -35,10 +35,10 @@ public:
      * successful the bus is left on low speed so the caller can finish the
      * initialization of its driver.
      */
-    static bool initialize_driver_state();
+    static bool initialize_driver_state(AP_HAL::SPIDeviceDriver *spi);
 
     // detect the sensor
-    static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu);
+    static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu, AP_HAL::SPIDeviceDriver *spi);
 
 private:
 
@@ -46,7 +46,7 @@ private:
     static void _register_write(AP_HAL::SPIDeviceDriver *spi, uint8_t reg,
                                 uint8_t val);
 
-    bool                 _init_sensor(void);
+    bool                 _init_sensor(AP_HAL::SPIDeviceDriver *spi);
     void                 _read_data_transaction();
     bool                 _data_ready();
     void                 _poll_data(void);