diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index ef8aedf7e9..c06b9ff47f 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/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 diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp index 72a037788f..d2fae8eb2e 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp +++ b/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); + } + } - return rx.int_status; + 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; } 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; - _i2c->readRegisters(_addr, MPUREG_INT_STATUS, 15, (uint8_t *) &rx); - memcpy(v, rx.d, 14); + 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; + } - return rx.int_status; + bytes_read = uint16_val(_rx, 0); + + 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()); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h index 2ab808b4ad..3fc02cbbd9 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h @@ -24,17 +24,23 @@ #include #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__