AP_InertialSensor: add fifo support for MPU6000

And remove the use of data rdy in this case

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -407,10 +407,9 @@ AP_InertialSensor::_detect_backends(void)
 #if HAL_INS_DEFAULT == HAL_INS_HIL
     _add_backend(AP_InertialSensor_HIL::detect);
 #elif HAL_INS_DEFAULT == HAL_INS_MPU60XX_SPI
-    _backends[_backend_count++] = new AP_InertialSensor_MPU6000(*this, new AP_MPU6000_BusDriver_SPI());
+    _add_backend(AP_InertialSensor_MPU6000::detect_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));
+    _add_backend(AP_InertialSensor_MPU6000::detect_i2c2);
 #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

@@ -61,6 +61,14 @@ extern const AP_HAL::HAL& hal;
 #define MPUREG_ZRMOT_THR                                0x21    // detection threshold for Zero Motion interrupt generation.
 #define MPUREG_ZRMOT_DUR                                0x22    // duration counter threshold for Zero Motion interrupt generation. The duration counter ticks at 16 Hz, therefore ZRMOT_DUR has a unit of 1 LSB = 64 ms.
 #define MPUREG_FIFO_EN                                  0x23
+#       define BIT_TEMP_FIFO_EN                                 0x80
+#       define BIT_XG_FIFO_EN                                   0x40
+#       define BIT_YG_FIFO_EN                                   0x20
+#       define BIT_ZG_FIFO_EN                                   0x10
+#       define BIT_ACCEL_FIFO_EN                                0x08
+#       define BIT_SLV2_FIFO_EN                                 0x04
+#       define BIT_SLV1_FIFO_EN                                 0x02
+#       define BIT_SLV0_FIFI_EN0                                0x01
 #define MPUREG_INT_PIN_CFG                              0x37
 #       define BIT_INT_RD_CLEAR                                 0x10    // clear the interrupt when any read occurs
 #       define BIT_LATCH_INT_EN                                 0x20    // latch data ready pin 
@@ -158,20 +166,41 @@ extern const AP_HAL::HAL& hal;
 #define MPU6000_REV_D8                          0x58    // 0101			1000
 #define MPU6000_REV_D9                          0x59    // 0101			1001
 
+#define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1]))
+#define uint16_val(v, idx)(((uint16_t)v[2*idx] << 8) | v[2*idx+1])
+
+#if !defined(HAL_INS_MPU60XX_I2C_ADDR)
+#define HAL_INS_MPU60XX_I2C_ADDR 0x68
+#endif
+
 /* SPI bus driver implementation */
-void AP_MPU6000_BusDriver_SPI::init()
+void AP_MPU6000_BusDriver_SPI::init(bool &fifo_mode, uint8_t &max_samples)
 {
+    fifo_mode = false;
+    _error_count = 0;
     _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);
+    /* maximum number of samples read by a burst
+     * a sample is an array containing :
+     * gyro_x
+     * gyro_y
+     * gyro_z
+     * accel_x
+     * accel_y
+     * accel_z
+     */
+    max_samples = 1;
 };
 
 void AP_MPU6000_BusDriver_SPI::read8(uint8_t reg, uint8_t *val)
 {
+    uint8_t addr = reg | 0x80; // Set most significant bit
+
     uint8_t tx[2];
     uint8_t rx[2];
 
-    tx[0] = reg;
+    tx[0] = addr;
     tx[1] = 0;
     _spi->transaction(tx, rx, 2);
 
@@ -190,10 +219,12 @@ void AP_MPU6000_BusDriver_SPI::write8(uint8_t reg, uint8_t val)
 
 void AP_MPU6000_BusDriver_SPI::set_bus_speed(AP_HAL::SPIDeviceDriver::bus_speed speed)
 {
-    set_bus_speed(speed);
+    _spi->set_bus_speed(speed);
 }
 
-uint8_t AP_MPU6000_BusDriver_SPI::read_burst(uint8_t v[14])
+void AP_MPU6000_BusDriver_SPI::read_burst(uint8_t *samples,
+                                         AP_HAL::DigitalSource *_drdy_pin,
+                                         uint8_t &n_samples)
 {
     /* one resister address followed by seven 2-byte registers */
     struct PACKED {
@@ -204,9 +235,28 @@ uint8_t AP_MPU6000_BusDriver_SPI::read_burst(uint8_t v[14])
 
     _spi->transaction((const uint8_t *)&tx, (uint8_t *)&rx, sizeof(rx));
 
-    memcpy(v, rx.d, 14);
+    /*
+      detect a bad SPI bus transaction by looking for all 14 bytes
+      zero, or the wrong INT_STATUS register value. This is used to
+      detect a too high SPI bus speed.
+    */
+    uint8_t i;
+    for (i=0; i<14; i++) {
+        if (rx.d[i] != 0) break;
+    }
+    if ((rx.int_status&~0x6) != (_drdy_pin==NULL?0:BIT_RAW_RDY_INT) || i == 14) {
+        // likely a bad bus transaction
+        if (++_error_count > 4) {
+            set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW);
+        }
+    }
+
+    n_samples = 1;
+    /* remove temperature and cmd from data sample */
+    memcpy(&samples[0], &rx.d[0], 6);
+    memcpy(&samples[6], &rx.d[8], 6);
 
-    return rx.int_status;
+    return;
 }
 
 AP_HAL::Semaphore* AP_MPU6000_BusDriver_SPI::get_semaphore()
@@ -220,8 +270,25 @@ AP_MPU6000_BusDriver_I2C::AP_MPU6000_BusDriver_I2C(AP_HAL::I2CDriver *i2c, uint8
     _addr(addr)
 {}
 
-void AP_MPU6000_BusDriver_I2C::init()
-{}
+void AP_MPU6000_BusDriver_I2C::init(bool &fifo_mode, uint8_t &max_samples)
+{
+    // enable fifo mode
+    fifo_mode = true;
+    write8(MPUREG_FIFO_EN, BIT_XG_FIFO_EN | BIT_YG_FIFO_EN |
+                           BIT_ZG_FIFO_EN | BIT_ACCEL_FIFO_EN);
+    write8(MPUREG_USER_CTRL, BIT_USER_CTRL_FIFO_RESET | BIT_USER_CTRL_SIG_COND_RESET);
+    write8(MPUREG_USER_CTRL, BIT_USER_CTRL_FIFO_EN);
+    /* maximum number of samples read by a burst
+     * a sample is an array containing :
+     * gyro_x
+     * gyro_y
+     * gyro_z
+     * accel_x
+     * accel_y
+     * accel_z
+     */
+    max_samples = MPU6000_MAX_FIFO_SAMPLES;
+}
 
 void AP_MPU6000_BusDriver_I2C::read8(uint8_t reg, uint8_t *val)
 {
@@ -236,17 +303,53 @@ void AP_MPU6000_BusDriver_I2C::write8(uint8_t reg, uint8_t 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])
+void AP_MPU6000_BusDriver_I2C::read_burst(uint8_t *samples,
+                                          AP_HAL::DigitalSource *_drdy_pin,
+                                          uint8_t &n_samples)
 {
-    struct PACKED {
-        uint8_t int_status;
-        uint8_t d[14];
-    } rx;
+	uint16_t bytes_read;
+    uint8_t ret = 0;
+
+    ret = _i2c->readRegisters(_addr, MPUREG_FIFO_COUNTH, 2, _rx);
+    if(ret != 0) {
+        hal.console->printf_P(PSTR("MPU6000: error in i2c read\n"));
+        n_samples = 0;
+        return;
+    }
 
-    _i2c->readRegisters(_addr, MPUREG_INT_STATUS, 15, (uint8_t *) &rx);
-    memcpy(v, rx.d, 14);
+    bytes_read = uint16_val(_rx, 0);
 
-    return rx.int_status;
+    n_samples = bytes_read / MPU6000_SAMPLE_SIZE;
+
+    if(n_samples > 3) {
+        hal.console->printf_P(PSTR("bytes_read = %d, n_samples %d > 3, dropping samples\n"),
+                                   bytes_read, n_samples);
+
+        /* Too many samples, do a FIFO RESET */
+        write8(MPUREG_USER_CTRL, 0);
+        write8(MPUREG_USER_CTRL, BIT_USER_CTRL_FIFO_RESET | BIT_USER_CTRL_SIG_COND_RESET);
+        write8(MPUREG_USER_CTRL, BIT_USER_CTRL_FIFO_EN);
+        n_samples = 0;
+        return;
+    }
+    else if (n_samples == 0) {
+        /* Not enough data in FIFO */
+        return;
+    }
+    else {
+        ret = _i2c->readRegisters(_addr, MPUREG_FIFO_R_W, n_samples * MPU6000_SAMPLE_SIZE, _rx);
+    }
+
+    if(ret != 0) {
+        hal.console->printf_P(PSTR("MPU6000: error in i2c read %d bytes\n"),
+                                   n_samples * MPU6000_SAMPLE_SIZE);
+        n_samples = 0;
+        return;
+    }
+
+    memcpy(samples, _rx, MPU6000_SAMPLE_SIZE * MPU6000_MAX_FIFO_SAMPLES);
+
+    return;
 }
 
 AP_HAL::Semaphore* AP_MPU6000_BusDriver_I2C::get_semaphore()
@@ -275,7 +378,6 @@ AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000(AP_InertialSensor &imu, AP_
     _bus_sem(NULL),
     _last_accel_filter_hz(-1),
     _last_gyro_filter_hz(-1),
-    _error_count(0),
 #if MPU6000_FAST_SAMPLING
     _accel_filter(1000, 15),
     _gyro_filter(1000, 15),
@@ -285,6 +387,43 @@ AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000(AP_InertialSensor &imu, AP_
     _gyro_sum(),
 #endif
     _sum_count(0)
+{
+
+}
+
+/*
+  detect the sensor
+ */
+AP_InertialSensor_Backend *AP_InertialSensor_MPU6000::detect_spi(AP_InertialSensor &_imu)
+{
+    AP_InertialSensor_MPU6000 *sensor = new AP_InertialSensor_MPU6000(_imu, new AP_MPU6000_BusDriver_SPI());
+    if (sensor == NULL) {
+        return NULL;
+    }
+    if (!sensor->_init_sensor()) {
+        delete sensor;
+        return NULL;
+    }
+
+    return sensor;
+}
+
+AP_InertialSensor_Backend *AP_InertialSensor_MPU6000::detect_i2c2(AP_InertialSensor &_imu)
+{
+    AP_InertialSensor_MPU6000 *sensor = new AP_InertialSensor_MPU6000(_imu,
+                                        new AP_MPU6000_BusDriver_I2C(hal.i2c2, HAL_INS_MPU60XX_I2C_ADDR));
+    if (sensor == NULL) {
+        return NULL;
+    }
+    if (!sensor->_init_sensor()) {
+        delete sensor;
+        return NULL;
+    }
+
+    return sensor;
+}
+
+bool AP_InertialSensor_MPU6000::_init_sensor(void)
 {
     _bus_sem = _bus->get_semaphore();
 
@@ -301,19 +440,17 @@ AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000(AP_InertialSensor &imu, AP_
         if (success) {
             hal.scheduler->delay(5+2);
             if (!_bus_sem->take(100)) {
-                return;
+                return false;
             }
             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;
+            return false;
         }
     } while (1);
 
@@ -330,10 +467,8 @@ AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000(AP_InertialSensor &imu, AP_
     _dump_registers();
 #endif
 
-    return;
+    return true;
 }
-
-
 /*
   process any 
  */
@@ -429,59 +564,50 @@ void AP_InertialSensor_MPU6000::_poll_data(void)
     if (!_bus_sem->take_nonblocking()) {
         return;
     }   
-    if (_data_ready()) {
+    if (_fifo_mode || _data_ready()) {
         _read_data_transaction(); 
     }
     _bus_sem->give();
 }
 
-
-void AP_InertialSensor_MPU6000::_read_data_transaction() {
-    /* one resister address followed by seven 2-byte registers */
-    uint8_t v[14];
-    uint8_t int_status = _bus->read_burst(v);
-    /*
-      detect a bad SPI bus transaction by looking for all 14 bytes
-      zero, or the wrong INT_STATUS register value. This is used to
-      detect a too high SPI bus speed.
-     */
-    uint8_t i;
-    for (i=0; i<14; i++) {
-        if (v[i] != 0) break;
-    }
-    if ((int_status&~0x6) != (_drdy_pin==NULL?0:BIT_RAW_RDY_INT) || i == 14) {
-        // likely a bad bus transaction
-        if (++_error_count > 4) {
-            _bus->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW);
-        }
-    }
-
-#define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1]))
+void AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples)
+{
+    for(uint8_t i=0; i < n_samples; i++) {
+        uint8_t *data = samples + MPU6000_SAMPLE_SIZE * i;
 #if MPU6000_FAST_SAMPLING
-    _accel_filtered = _accel_filter.apply(Vector3f(int16_val(v, 1),
-                                                   int16_val(v, 0),
-                                                   -int16_val(v, 2)));
+        _accel_filtered = _accel_filter.apply(Vector3f(int16_val(data, 1),
+                                                       int16_val(data, 0),
+                                                      -int16_val(data, 2)));
 
-    _gyro_filtered = _gyro_filter.apply(Vector3f(int16_val(v, 5),
-                                                 int16_val(v, 4),
-                                                 -int16_val(v, 6)));
+        _gyro_filtered = _gyro_filter.apply(Vector3f(int16_val(data, 4),
+                                                     int16_val(data, 3),
+                                                    -int16_val(data, 5)));
 #else
-    _accel_sum.x += int16_val(v, 1);
-    _accel_sum.y += int16_val(v, 0);
-    _accel_sum.z -= int16_val(v, 2);
-    _gyro_sum.x  += int16_val(v, 5);
-    _gyro_sum.y  += int16_val(v, 4);
-    _gyro_sum.z  -= int16_val(v, 6);
+        _accel_sum.x += int16_val(data, 1);
+        _accel_sum.y += int16_val(data, 0);
+        _accel_sum.z -= int16_val(data, 2);
+        _gyro_sum.x  += int16_val(data, 4);
+        _gyro_sum.y  += int16_val(data, 3);
+        _gyro_sum.z  -= int16_val(data, 5);
 #endif
-    _sum_count++;
+        _sum_count++;
 
 #if !MPU6000_FAST_SAMPLING
-    if (_sum_count == 0) {
+        if (_sum_count == 0) {
         // rollover - v unlikely
-        _accel_sum.zero();
-        _gyro_sum.zero();
-    }
+            _accel_sum.zero();
+            _gyro_sum.zero();
+        }
 #endif
+    }
+}
+
+void AP_InertialSensor_MPU6000::_read_data_transaction()
+{
+    uint8_t n_samples;
+
+    _bus->read_burst(_samples, _drdy_pin, n_samples);
+    _accumulate(_samples, n_samples);
 }
 
 uint8_t AP_InertialSensor_MPU6000::_register_read( uint8_t reg )
@@ -541,6 +667,8 @@ void AP_InertialSensor_MPU6000::_set_filter_register(uint16_t filter_hz)
 
 bool AP_InertialSensor_MPU6000::_hardware_init(void)
 {
+    uint8_t max_samples;
+
     if (!_bus_sem->take(100)) {
         hal.scheduler->panic(PSTR("MPU6000: Unable to get semaphore"));
     }
@@ -561,9 +689,9 @@ bool AP_InertialSensor_MPU6000::_hardware_init(void)
         hal.scheduler->delay(5);
 
         // check it has woken up
-        if (_register_read(MPUREG_PWR_MGMT_1) == BIT_PWR_MGMT_1_CLK_ZGYRO) {
+        if (_register_read(MPUREG_PWR_MGMT_1) == BIT_PWR_MGMT_1_CLK_ZGYRO)
             break;
-        }
+
 #if MPU6000_DEBUG
         _dump_registers();
 #endif
@@ -577,7 +705,9 @@ 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);
 
-    _bus->init();
+    _bus->init(_fifo_mode, max_samples);
+    /* each sample is on 16 bits */
+    _samples = new uint8_t[max_samples * MPU6000_SAMPLE_SIZE];
     hal.scheduler->delay(1);
 
 #if MPU6000_FAST_SAMPLING
@@ -609,7 +739,11 @@ bool AP_InertialSensor_MPU6000::_hardware_init(void)
     _set_filter_register(256);
 
     // set sample rate to 1000Hz and apply a software filter
-    _register_write(MPUREG_SMPLRT_DIV, MPUREG_SMPLRT_1000HZ);
+    // In this configuration, the gyro sample rate is 8kHz
+    // Therefore the sample rate value is 8kHz/(SMPLRT_DIV + 1)
+    // So we have to set it to 7 to have a 1kHz sampling
+    // rate on the gyro
+    _register_write(MPUREG_SMPLRT_DIV, 7);
 #else
     _set_filter_register(_accel_filter_cutoff());
 

libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h

@@ -24,17 +24,23 @@
 #include <LowPassFilter2p.h>
 #endif
 
+#define MPU6000_SAMPLE_SIZE 12
+#define MPU6000_MAX_FIFO_SAMPLES 3
+#define MAX_DATA_READ (MPU6000_MAX_FIFO_SAMPLES * MPU6000_SAMPLE_SIZE)
+
 class AP_MPU6000_BusDriver
 {
     public:
-        virtual void init() = 0;
+        virtual void init(bool &fifo_mode, uint8_t &max_samples) = 0;
         virtual void read8(uint8_t reg, uint8_t *val) = 0;
         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) = 0;
-        virtual uint8_t read_burst(uint8_t v[14]) = 0;
+        virtual void read_burst(uint8_t* samples,
+                                AP_HAL::DigitalSource *_drdy_pin,
+                                uint8_t &n_samples) = 0;
         virtual AP_HAL::Semaphore* get_semaphore() = 0;
 };
 
@@ -42,6 +48,8 @@ class AP_InertialSensor_MPU6000 : public AP_InertialSensor_Backend
 {
 public:
     AP_InertialSensor_MPU6000(AP_InertialSensor &imu, AP_MPU6000_BusDriver *bus);
+    static AP_InertialSensor_Backend *detect_i2c2(AP_InertialSensor &_imu);
+    static AP_InertialSensor_Backend *detect_spi(AP_InertialSensor &_imu);
 
     /* update accel and gyro state */
     bool update();
@@ -59,19 +67,19 @@ private:
     uint8_t _accel_instance;
 
     AP_HAL::DigitalSource *_drdy_pin;
-
     bool                 _init_sensor(void);
     bool                 _sample_available();
     void                 _read_data_transaction();
     bool                 _data_ready();
     void                 _poll_data(void);
-    uint8_t              _register_read( uint8_t reg );
+    uint8_t              _register_read( uint8_t reg);
     void                 _register_write( uint8_t reg, uint8_t val );
     void                 _register_write_check(uint8_t reg, uint8_t val);
     bool                 _hardware_init(void);
+    void                 _accumulate(uint8_t *samples, uint8_t n_samples);
 
     AP_MPU6000_BusDriver *_bus;
-    AP_HAL::Semaphore   *_bus_sem;
+    AP_HAL::Semaphore    *_bus_sem;
 
     static const float          _gyro_scale;
 
@@ -81,9 +89,6 @@ private:
 
     void _set_filter_register(uint16_t filter_hz);
 
-    // count of bus errors
-    uint16_t _error_count;
-
     // how many hardware samples before we report a sample to the caller
     uint8_t _sample_count;
 
@@ -101,37 +106,47 @@ private:
     Vector3l _gyro_sum;
 #endif
     volatile uint16_t _sum_count;
+    bool _fifo_mode;
+    uint8_t *_samples;
 };
 
 class AP_MPU6000_BusDriver_SPI : public AP_MPU6000_BusDriver
 {
     public:
-        void init();
+        void init(bool &fifo_mode, uint8_t &max_samples);
         void read8(uint8_t reg, 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]);
+        void read_burst(uint8_t* samples,
+                        AP_HAL::DigitalSource *_drdy_pin,
+                        uint8_t &n_samples);
         AP_HAL::Semaphore* get_semaphore();
 
     private:
         AP_HAL::SPIDeviceDriver *_spi;
         AP_HAL::Semaphore *_spi_sem;
+        // count of bus errors
+        uint16_t _error_count;
 };
 
 class AP_MPU6000_BusDriver_I2C : public AP_MPU6000_BusDriver
 {
     public:
         AP_MPU6000_BusDriver_I2C(AP_HAL::I2CDriver *i2c, uint8_t addr);
-        void init();
+        void init(bool &fifo_mode, uint8_t &max_samples);
         void read8(uint8_t reg, 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]);
+        void read_burst(uint8_t* samples,
+                        AP_HAL::DigitalSource *_drdy_pin,
+                        uint8_t &n_samples);
         AP_HAL::Semaphore* get_semaphore();
 
     private:
         uint8_t _addr;
+        AP_HAL::I2CDriver *_i2c;
         AP_HAL::Semaphore *_i2c_sem;
+        uint8_t _rx[MAX_DATA_READ];
 };
 
 #endif // __AP_INERTIAL_SENSOR_MPU6000_H__