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.

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 <assert.h>
+
 #include <AP_HAL/AP_HAL.h>
 #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;

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,