aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_InertialSensor: MPU9250: Add I2C support

Browse Source
This commit is contained in:
José Roberto de Souza 2015-09-28 13:48:18 -03:00 committed by Andrew Tridgell
parent c3dae6fcec
commit 607ab5b005
2 changed files with 92 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

71
libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
View File

@ -288,6 +288,67 @@ bool AP_MPU9250_BusDriver_SPI::has_auxiliary_bus()
return true;
}
/* I2C bus driver implementation */
AP_MPU9250_BusDriver_I2C::AP_MPU9250_BusDriver_I2C(AP_HAL::I2CDriver *i2c, uint8_t addr)
: _addr(addr)
, _i2c(i2c)
{
}
void AP_MPU9250_BusDriver_I2C::read8(uint8_t reg, uint8_t *val)
{
_i2c->readRegister(_addr, reg, val);
}
void AP_MPU9250_BusDriver_I2C::read_block(uint8_t reg, uint8_t *val, uint8_t count)
{
_i2c->readRegisters(_addr, reg, count, val);
}
void AP_MPU9250_BusDriver_I2C::write8(uint8_t reg, uint8_t val)
{
_i2c->writeRegister(_addr, reg, val);
}
bool AP_MPU9250_BusDriver_I2C::read_data_transaction(uint8_t *samples,
uint8_t &n_samples)
{
uint8_t ret = 0;
struct PACKED {
uint8_t int_status;
uint8_t v[MPU9250_SAMPLE_SIZE];
} buffer;
ret = _i2c->readRegisters(_addr, MPUREG_INT_STATUS, sizeof(buffer), (uint8_t *)&buffer);
if (ret != 0) {
hal.console->printf("MPU9250: error in I2C read\n");
n_samples = 0;
return false;
}
if (!(buffer.int_status & BIT_RAW_RDY_INT)) {
#if MPU9250_DEBUG
hal.console->printf("MPU9250: No sample available.\n");
#endif
n_samples = 0;
return false;
}
memcpy(samples, buffer.v, MPU9250_SAMPLE_SIZE);
n_samples = 1;
return true;
}
AP_HAL::Semaphore* AP_MPU9250_BusDriver_I2C::get_semaphore()
{
return _i2c->get_semaphore();
}
bool AP_MPU9250_BusDriver_I2C::has_auxiliary_bus()
{
return false;
}
AP_InertialSensor_MPU9250::AP_InertialSensor_MPU9250(AP_InertialSensor &imu, AP_MPU9250_BusDriver *bus) :
AP_InertialSensor_Backend(imu),
_last_accel_filter_hz(-1),
@ -323,6 +384,16 @@ AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::detect(AP_InertialSensor &
return _detect(_imu, bus, HAL_INS_MPU9250);
}
AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::detect_i2c(AP_InertialSensor &_imu,
AP_HAL::I2CDriver *i2c,
uint8_t addr)
{
AP_MPU9250_BusDriver *bus = new AP_MPU9250_BusDriver_I2C(i2c, addr);
if (!bus)
return nullptr;
return _detect(_imu, bus, HAL_INS_MPU9250);
}
/* Common detection method - it takes ownership of the bus, freeing it if it's
* not possible to return an AP_InertialSensor_Backend */
AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::_detect(AP_InertialSensor &_imu,

21
libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h
View File

@ -55,6 +55,9 @@ public:
// detect the sensor
static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu, AP_HAL::SPIDeviceDriver *spi);
static AP_InertialSensor_Backend *detect_i2c(AP_InertialSensor &imu,
AP_HAL::I2CDriver *i2c,
uint8_t addr);
private:
static AP_InertialSensor_Backend *_detect(AP_InertialSensor &_imu,
@ -129,6 +132,24 @@ private:
AP_HAL::SPIDeviceDriver *_spi;
};
class AP_MPU9250_BusDriver_I2C : public AP_MPU9250_BusDriver
{
public:
AP_MPU9250_BusDriver_I2C(AP_HAL::I2CDriver *i2c, uint8_t addr);
void init() {};
void read8(uint8_t reg, uint8_t *val);
void write8(uint8_t reg, uint8_t val);
void read_block(uint8_t reg, uint8_t *val, uint8_t count);
void set_bus_speed(AP_HAL::SPIDeviceDriver::bus_speed speed) {};
bool read_data_transaction(uint8_t* samples, uint8_t &n_samples);
AP_HAL::Semaphore* get_semaphore();
bool has_auxiliary_bus();
private:
uint8_t _addr;
AP_HAL::I2CDriver *_i2c;
};
class AP_MPU9250_AuxiliaryBus : public AuxiliaryBus
{
friend class AP_InertialSensor_MPU9250;