AP_InertialSensor: add i2c bus driver for MPU6050

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -406,8 +406,11 @@ AP_InertialSensor::_detect_backends(void)
     }
 #if HAL_INS_DEFAULT == HAL_INS_HIL
     _add_backend(AP_InertialSensor_HIL::detect);
-#elif HAL_INS_DEFAULT == HAL_INS_MPU6000
+#elif HAL_INS_DEFAULT == HAL_INS_MPU60XX_SPI
-    _add_backend(AP_InertialSensor_MPU6000::detect);
+    _backends[_backend_count++] = new AP_InertialSensor_MPU6000(*this, new AP_MPU6000_BusDriver_SPI());
+#elif HAL_INS_DEFAULT == HAL_INS_MPU60XX_I2C && HAL_INS_MPU60XX_I2C_BUS == 2
+    _backends[_backend_count++] = new AP_InertialSensor_MPU6000(*this,
+                                    new AP_MPU6000_BusDriver_I2C(hal.i2c2, HAL_INS_MPU60XX_I2C_ADDR));
 #elif HAL_INS_DEFAULT == HAL_INS_PX4 || HAL_INS_DEFAULT == HAL_INS_VRBRAIN
     _add_backend(AP_InertialSensor_PX4::detect);
 #elif HAL_INS_DEFAULT == HAL_INS_OILPAN

libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp

@@ -158,30 +158,32 @@ extern const AP_HAL::HAL& hal;
 #define MPU6000_REV_D8                          0x58    // 0101			1000
 #define MPU6000_REV_D9                          0x59    // 0101			1001
 
+/* SPI bus driver implementation */
 void AP_MPU6000_BusDriver_SPI::init()
 {
     _spi = hal.spi->device(AP_HAL::SPIDevice_MPU6000);
+    // Disable I2C bus if SPI selected (Recommended in Datasheet
     write8(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS);
 };
 
-void AP_MPU6000_BusDriver_SPI::read8(uint8_t addr, uint8_t *val)
+void AP_MPU6000_BusDriver_SPI::read8(uint8_t reg, uint8_t *val)
 {
     uint8_t tx[2];
     uint8_t rx[2];
 
-    tx[0] = addr;
+    tx[0] = reg;
     tx[1] = 0;
     _spi->transaction(tx, rx, 2);
 
     *val = rx[1];
 }
 
-void AP_MPU6000_BusDriver_SPI::write8(uint8_t addr, uint8_t val)
+void AP_MPU6000_BusDriver_SPI::write8(uint8_t reg, uint8_t val)
 {
     uint8_t tx[2];
     uint8_t rx[2];
 
-    tx[0] = addr;
+    tx[0] = reg;
     tx[1] = val;
     _spi->transaction(tx, rx, 2);
 }
@@ -212,6 +214,46 @@ AP_HAL::Semaphore* AP_MPU6000_BusDriver_SPI::get_semaphore()
     return _spi->get_semaphore();
 }
 
+/* I2C bus driver implementation */
+AP_MPU6000_BusDriver_I2C::AP_MPU6000_BusDriver_I2C(AP_HAL::I2CDriver *i2c, uint8_t addr) :
+    _i2c(i2c),
+    _addr(addr)
+{}
+
+void AP_MPU6000_BusDriver_I2C::init()
+{}
+
+void AP_MPU6000_BusDriver_I2C::read8(uint8_t reg, uint8_t *val)
+{
+    _i2c->readRegister(_addr, reg, val);
+}
+
+void AP_MPU6000_BusDriver_I2C::write8(uint8_t reg, uint8_t val)
+{
+    _i2c->writeRegister(_addr, reg, val);
+}
+
+void AP_MPU6000_BusDriver_I2C::set_bus_speed(AP_HAL::SPIDeviceDriver::bus_speed speed)
+{}
+
+uint8_t AP_MPU6000_BusDriver_I2C::read_burst(uint8_t v[14])
+{
+    struct PACKED {
+        uint8_t int_status;
+        uint8_t d[14];
+    } rx;
+
+    _i2c->readRegisters(_addr, MPUREG_INT_STATUS, 15, (uint8_t *) &rx);
+    memcpy(v, rx.d, 14);
+
+    return rx.int_status;
+}
+
+AP_HAL::Semaphore* AP_MPU6000_BusDriver_I2C::get_semaphore()
+{
+    return _i2c->get_semaphore();
+}
+
 /*
  *  RM-MPU-6000A-00.pdf, page 33, section 4.25 lists LSB sensitivity of
  *  gyro as 16.4 LSB/DPS at scale factor of +/- 2000dps (FS_SEL==3)
@@ -226,10 +268,10 @@ const float AP_InertialSensor_MPU6000::_gyro_scale = (0.0174532f / 16.4f);
  *  variants however
  */
 
-AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000(AP_InertialSensor &imu) :
+AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000(AP_InertialSensor &imu, AP_MPU6000_BusDriver *bus) :
     AP_InertialSensor_Backend(imu),
     _drdy_pin(NULL),
-    _bus(NULL),
+    _bus(bus),
     _bus_sem(NULL),
     _last_accel_filter_hz(-1),
     _last_gyro_filter_hz(-1),
@@ -244,31 +286,6 @@ AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000(AP_InertialSensor &imu) :
 #endif
     _sum_count(0)
 {
-}
-
-/*
-  detect the sensor
- */
-AP_InertialSensor_Backend *AP_InertialSensor_MPU6000::detect(AP_InertialSensor &_imu)
-{
-    AP_InertialSensor_MPU6000 *sensor = new AP_InertialSensor_MPU6000(_imu);
-    if (sensor == NULL) {
-        return NULL;
-    }
-    if (!sensor->_init_sensor()) {
-        delete sensor;
-        return NULL;
-    }
-
-    return sensor;
-}
-
-/*
-  initialise the sensor
- */
-bool AP_InertialSensor_MPU6000::_init_sensor(void)
-{
-    _bus = new AP_MPU6000_BusDriver_SPI();
     _bus_sem = _bus->get_semaphore();
 
 #ifdef MPU6000_DRDY_PIN
@@ -284,17 +301,19 @@ bool AP_InertialSensor_MPU6000::_init_sensor(void)
         if (success) {
             hal.scheduler->delay(5+2);
             if (!_bus_sem->take(100)) {
-                return false;
+                return;
             }
             if (_data_ready()) {
                 _bus_sem->give();
                 break;
+            } else {
+                return;
             }
             _bus_sem->give();
         }
         if (tries++ > 5) {
             hal.console->print_P(PSTR("failed to boot MPU6000 5 times")); 
-            return false;
+            return;
         }
     } while (1);
 
@@ -311,7 +330,7 @@ bool AP_InertialSensor_MPU6000::_init_sensor(void)
     _dump_registers();
 #endif
 
-    return true;
+    return;
 }
 
 
@@ -352,6 +371,9 @@ bool AP_InertialSensor_MPU6000::update( void )
 #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF
     accel.rotate(ROTATION_PITCH_180_YAW_90);
     gyro.rotate(ROTATION_PITCH_180_YAW_90);
+#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
+    accel.rotate(ROTATION_YAW_270);
+    gyro.rotate(ROTATION_YAW_270);
 #endif
 
     _publish_accel(_accel_instance, accel);
@@ -555,9 +577,7 @@ bool AP_InertialSensor_MPU6000::_hardware_init(void)
     _register_write(MPUREG_PWR_MGMT_2, 0x00);            // only used for wake-up in accelerometer only low power mode
     hal.scheduler->delay(1);
 
-    // Disable I2C bus if SPI selected (Recommended in Datasheet
     _bus->init();
-    _register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS);
     hal.scheduler->delay(1);
 
 #if MPU6000_FAST_SAMPLING

libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h

@@ -27,21 +27,21 @@
 class AP_MPU6000_BusDriver
 {
     public:
-        virtual void init();
+        virtual void init() = 0;
-        virtual void read8(uint8_t addr, uint8_t *val);
+        virtual void read8(uint8_t reg, uint8_t *val) = 0;
-        virtual void write8(uint8_t addr, uint8_t val);
+        virtual void write8(uint8_t reg, uint8_t val) = 0;
         enum bus_speed {
                 SPEED_LOW, SPEED_HIGH
         };
-        virtual void set_bus_speed(AP_HAL::SPIDeviceDriver::bus_speed speed);
+        virtual void set_bus_speed(AP_HAL::SPIDeviceDriver::bus_speed speed) = 0;
-        virtual uint8_t read_burst(uint8_t v[14]);
+        virtual uint8_t read_burst(uint8_t v[14]) = 0;
-        virtual AP_HAL::Semaphore* get_semaphore();
+        virtual AP_HAL::Semaphore* get_semaphore() = 0;
 };
 
 class AP_InertialSensor_MPU6000 : public AP_InertialSensor_Backend
 {
 public:
-    AP_InertialSensor_MPU6000(AP_InertialSensor &imu);
+    AP_InertialSensor_MPU6000(AP_InertialSensor &imu, AP_MPU6000_BusDriver *bus);
 
     /* update accel and gyro state */
     bool update();
@@ -49,9 +49,6 @@ public:
     bool gyro_sample_available(void) { return _sum_count >= _sample_count; }
     bool accel_sample_available(void) { return _sum_count >= _sample_count; }
 
-    // detect the sensor
-    static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu);
-
 private:
 #if MPU6000_DEBUG
     void _dump_registers(void);
@@ -110,8 +107,8 @@ class AP_MPU6000_BusDriver_SPI : public AP_MPU6000_BusDriver
 {
     public:
         void init();
-        void read8(uint8_t addr, uint8_t *val);
+        void read8(uint8_t reg, uint8_t *val);
-        void write8(uint8_t addr, uint8_t val);
+        void write8(uint8_t reg, uint8_t val);
         void set_bus_speed(AP_HAL::SPIDeviceDriver::bus_speed speed);
         uint8_t read_burst(uint8_t v[14]);
         AP_HAL::Semaphore* get_semaphore();
@@ -124,15 +121,16 @@ class AP_MPU6000_BusDriver_SPI : public AP_MPU6000_BusDriver
 class AP_MPU6000_BusDriver_I2C : public AP_MPU6000_BusDriver
 {
     public:
+        AP_MPU6000_BusDriver_I2C(AP_HAL::I2CDriver *i2c, uint8_t addr);
         void init();
-        void read8(uint8_t addr, uint8_t *val);
+        void read8(uint8_t reg, uint8_t *val);
-        void write8(uint8_t addr, uint8_t val);
+        void write8(uint8_t reg, uint8_t val);
         void set_bus_speed(AP_HAL::SPIDeviceDriver::bus_speed speed);
         uint8_t read_burst(uint8_t v[14]);
         AP_HAL::Semaphore* get_semaphore();
 
     private:
-        AP_HAL::I2CDriver *_i2c;
+        uint8_t _addr;
         AP_HAL::Semaphore *_i2c_sem;
 };