From f7954ee8850ab3c643d5b36220782319945da48c Mon Sep 17 00:00:00 2001 From: Lucas De Marchi Date: Sun, 16 Aug 2015 16:06:41 -0300 Subject: [PATCH] AP_InertialSensor: MPU6000: allow to read generic block We were able to read only the block of registers that are part of the data output from accelerometer/gyroscope. In order to support reading the external sensors we need support for reading a generic block of registers. --- .../AP_InertialSensor_MPU6000.cpp | 30 +++++++++++++++++++ .../AP_InertialSensor_MPU6000.h | 7 +++++ 2 files changed, 37 insertions(+) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp index 784416b613..4817631240 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp @@ -1,5 +1,7 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include + #include #include "AP_InertialSensor_MPU6000.h" @@ -139,6 +141,8 @@ extern const AP_HAL::HAL& hal; #define MPUREG_FIFO_R_W 0x74 #define MPUREG_WHOAMI 0x75 +#define BIT_READ_FLAG 0x80 + // Configuration bits MPU 3000 and MPU 6000 (not revised)? #define BITS_DLPF_CFG_256HZ_NOLPF2 0x00 #define BITS_DLPF_CFG_188HZ 0x01 @@ -194,6 +198,21 @@ void AP_MPU6000_BusDriver_SPI::init(bool &fifo_mode, uint8_t &max_samples) max_samples = 1; }; +/* + * This implementation is limited to a block of at most 32 bytes + */ +void AP_MPU6000_BusDriver_SPI::read_block(uint8_t reg, uint8_t *buf, uint32_t size) +{ + assert(size < 32); + + reg |= BIT_READ_FLAG; + uint8_t tx[32] = { reg, }; + uint8_t rx[32] = { }; + + _spi->transaction(tx, rx, size + 1); + memcpy(buf, rx + 1, size); +} + void AP_MPU6000_BusDriver_SPI::read8(uint8_t reg, uint8_t *val) { uint8_t addr = reg | 0x80; // Set most significant bit @@ -297,6 +316,11 @@ void AP_MPU6000_BusDriver_I2C::read8(uint8_t reg, uint8_t *val) _i2c->readRegister(_addr, reg, val); } +void AP_MPU6000_BusDriver_I2C::read_block(uint8_t reg, uint8_t *buf, uint32_t size) +{ + _i2c->readRegisters(_addr, reg, size, buf); +} + void AP_MPU6000_BusDriver_I2C::write8(uint8_t reg, uint8_t val) { _i2c->writeRegister(_addr, reg, val); @@ -626,6 +650,12 @@ void AP_InertialSensor_MPU6000::_read_data_transaction() _accumulate(_samples, n_samples); } +void AP_InertialSensor_MPU6000::_read_block(uint8_t reg, uint8_t *buf, + uint32_t size) +{ + _bus->read_block(reg, buf, size); +} + uint8_t AP_InertialSensor_MPU6000::_register_read( uint8_t reg ) { uint8_t val; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h index ae9fb1fc35..e10b0c6846 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h @@ -34,6 +34,10 @@ public: virtual ~AP_MPU6000_BusDriver() { }; virtual void init(bool &fifo_mode, uint8_t &max_samples) = 0; virtual void read8(uint8_t reg, uint8_t *val) = 0; + + /// Copy data from the device to @p buf starting at @p reg with @size + /// length. + virtual void read_block(uint8_t reg, uint8_t *buf, uint32_t size) = 0; virtual void write8(uint8_t reg, uint8_t val) = 0; virtual void set_bus_speed(AP_HAL::SPIDeviceDriver::bus_speed speed) = 0; virtual void read_data_transaction(uint8_t* samples, @@ -76,6 +80,7 @@ private: void _read_data_transaction(); bool _data_ready(); void _poll_data(void); + void _read_block(uint8_t reg, uint8_t *buf, uint32_t size); 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); @@ -120,6 +125,7 @@ public: AP_MPU6000_BusDriver_SPI(void); void init(bool &fifo_mode, uint8_t &max_samples); void read8(uint8_t reg, uint8_t *val); + void read_block(uint8_t reg, uint8_t *buf, uint32_t size) override; void write8(uint8_t reg, uint8_t val); void set_bus_speed(AP_HAL::SPIDeviceDriver::bus_speed speed); void read_data_transaction(uint8_t* samples, @@ -140,6 +146,7 @@ public: AP_MPU6000_BusDriver_I2C(AP_HAL::I2CDriver *i2c, uint8_t addr); void init(bool &fifo_mode, uint8_t &max_samples); void read8(uint8_t reg, uint8_t *val); + void read_block(uint8_t reg, uint8_t *buf, uint32_t size) override; void write8(uint8_t reg, uint8_t val); void set_bus_speed(AP_HAL::SPIDeviceDriver::bus_speed speed); void read_data_transaction(uint8_t* samples,