Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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.
This commit is contained in:
Lucas De Marchi 2015-08-16 16:06:41 -03:00 committed by Andrew Tridgell
parent b5da8ad61f
commit f7954ee885
2 changed files with 37 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
View File

@ -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;

7
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
View File

@ -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,