l3gd20: Add support for I2C connections

The existing driver only supported IIO.  This change adds support for
controlling the device via an I2C connection. In addition, there is a
new C++ example for it (l3gd20-i2c.cxx).

Note: Only basic functionality is supported, though a full register
map and access functions are available to fill in any desired
functionality.

Note, that some methods are only usable with specific connection
types.  See the documentation.

Signed-off-by: Jon Trulson <jtrulson@ics.com>
This commit is contained in:
Jon Trulson
2016-08-09 15:54:14 -06:00
parent 4faa71d239
commit 06ecae7212
4 changed files with 882 additions and 13 deletions

View File

@ -1,5 +1,6 @@
/*
* Author: Lay, Kuan Loon <kuan.loon.lay@intel.com>
* Jon Trulson <jtrulson@ics.com>
* Copyright (c) 2016 Intel Corporation.
*
* Permission is hereby granted, free of charge, to any person obtaining
@ -38,8 +39,15 @@
#define GYRO_DENOISE_NUM_FIELDS 3
using namespace upm;
using namespace std;
L3GD20::L3GD20(int device)
static float c2f(float c)
{
return (c * (9.0 / 5.0) + 32.0);
}
L3GD20::L3GD20(int device) :
m_i2c(0)
{
float gyro_scale;
char trigger[64];
@ -49,6 +57,7 @@ L3GD20::L3GD20(int device)
": mraa_iio_init() failed, invalid device?");
return;
}
m_scale = 1;
m_iio_device_num = device;
sprintf(trigger, "hrtimer-l3gd20-hr-dev%d", device);
@ -82,6 +91,72 @@ L3GD20::L3GD20(int device)
m_filter.idx = 0;
}
L3GD20::L3GD20(int bus, int addr)
{
m_i2c = new mraa::I2c(bus);
if (m_i2c->address(addr) != mraa::SUCCESS)
{
throw std::runtime_error(std::string(__FUNCTION__) +
": I2c.address() failed");
}
m_scale = 1.0;
m_iio_device_num = 0;
m_gyrScale = 1.0;
m_gyrX = 0.0;
m_gyrY = 0.0;
m_gyrZ = 0.0;
m_temperature = 0.0;
m_mount_matrix_exist = false;
m_event_count = 0;
// initial calibrate data
initCalibrate();
// initial denoise data
m_filter.buff =
(float*) calloc(GYRO_DENOISE_MAX_SAMPLES,
sizeof(float) * GYRO_DENOISE_NUM_FIELDS);
if (m_filter.buff == NULL)
{
throw std::bad_alloc();
return;
}
m_filter.sample_size = GYRO_DENOISE_MAX_SAMPLES;
m_filter.count = 0;
m_filter.idx = 0;
// check ChipID
uint8_t cid = getChipID();
if (cid != L3GD20_DEFAULT_CHIP_ID)
{
throw std::runtime_error(std::string(__FUNCTION__) +
": invalid Chip ID: expected "
+ std::to_string(L3GD20_DEFAULT_CHIP_ID)
+ ", got "
+ std::to_string(int(cid)));
return;
}
// set a normal power mode (with all axes enabled)
setPowerMode(POWER_NORMAL);
// enable block update mode
enableBDU(true);
// Set range to 250 degrees/sec/
setRange(FS_250);
// Set ODR to 95Hz, 25Hz cut-off
setODR(ODR_CUTOFF_95_25);
}
L3GD20::~L3GD20()
{
if (m_filter.buff) {
@ -92,6 +167,180 @@ L3GD20::~L3GD20()
mraa_iio_close(m_iio);
}
uint8_t L3GD20::readReg(uint8_t reg)
{
return m_i2c->readReg(reg);
}
int L3GD20::readRegs(uint8_t reg, uint8_t *buffer, int len)
{
// For multi-byte reads, the reg must have the MSb set
return m_i2c->readBytesReg(reg | 0x80, buffer, len);
}
void L3GD20::writeReg(uint8_t reg, uint8_t val)
{
if (m_i2c->writeReg(reg, val) != mraa::SUCCESS)
{
throw std::runtime_error(std::string(__FUNCTION__)
+ ": I2c.writeReg() failed");
}
}
uint8_t L3GD20::getChipID()
{
return readReg(REG_WHO_AM_I);
}
void L3GD20::setPowerMode(POWER_MODES_T mode)
{
uint8_t reg = readReg(REG_CTRL_REG1);
// setting the power modes involves setting certain combinations of
// the PD, and the X, Y, and Zen bitfields.
switch(mode)
{
case POWER_DOWN:
// clear PD
reg &= ~(CTRL_REG1_PD);
break;
case POWER_SLEEP:
// set PD, clear X, Y, and Zen.
reg |= CTRL_REG1_PD;
reg &= ~(CTRL_REG1_YEN | CTRL_REG1_XEN | CTRL_REG1_ZEN);
break;
case POWER_NORMAL:
// set PD, X, Y, and Zen.
reg |= (CTRL_REG1_PD | CTRL_REG1_YEN | CTRL_REG1_XEN | CTRL_REG1_ZEN);
break;
}
writeReg(REG_CTRL_REG1, reg);
}
void L3GD20::setRange(FS_T range)
{
switch(range)
{
case FS_250:
m_gyrScale = 8.75; // milli-degrees
break;
case FS_500:
m_gyrScale = 17.50;
break;
case FS_2000:
m_gyrScale = 70.0;
break;
}
uint8_t reg = readReg(REG_CTRL_REG4) & ~(_CTRL_REG4_RESERVED_BITS);
// mask off current FS
reg &= ~(_CTRL_REG4_FS_MASK << _CTRL_REG4_FS_SHIFT);
// add our new FS
reg |= (range << _CTRL_REG4_FS_SHIFT);
writeReg(REG_CTRL_REG4, reg);
}
void L3GD20::enableBDU(bool enable)
{
uint8_t reg = readReg(REG_CTRL_REG4) & ~(_CTRL_REG4_RESERVED_BITS);
if (enable)
reg |= CTRL_REG4_BDU;
else
reg &= ~CTRL_REG4_BDU;
writeReg(REG_CTRL_REG4, reg);
}
void L3GD20::getGyroscope(float *x, float *y, float *z)
{
if (x)
*x = m_gyrX;
if (y)
*y = m_gyrY;
if (z)
*z = m_gyrZ;
}
void L3GD20::update()
{
int bufLen = 6;
uint8_t buf[bufLen];
if (readRegs(REG_OUT_X_L, buf, bufLen) != bufLen)
{
throw std::runtime_error(std::string(__FUNCTION__)
+ ": readRegs() failed to read "
+ std::to_string(bufLen)
+ " bytes");
}
int16_t val;
// The calibration and denoise algorithms depend on the use of
// radians rather than degrees, so we convert to radians here.
val = int16_t(buf[1] << 8 | buf[0]);
m_gyrX = ((float(val) * m_gyrScale) / 1000.0) * (M_PI/180.0);
m_gyrX = m_gyrX - m_cal_data.bias_x;
// y
val = int16_t(buf[3] << 8 | buf[2]);
m_gyrY = ((float(val) * m_gyrScale) / 1000.0) * (M_PI/180.0);
m_gyrY = m_gyrY - m_cal_data.bias_y;
// z
val = int16_t(buf[5] << 8 | buf[4]);
m_gyrZ = ((float(val) * m_gyrScale) / 1000.0) * (M_PI/180.0);
m_gyrZ = m_gyrZ - m_cal_data.bias_z;
if (m_calibrated == false)
m_calibrated = gyroCollect(m_gyrX, m_gyrY, m_gyrZ);
if (m_event_count++ >= GYRO_MIN_SAMPLES)
{
gyroDenoiseMedian(&m_gyrX, &m_gyrY, &m_gyrZ);
clampGyroReadingsToZero(&m_gyrX, &m_gyrY, &m_gyrZ);
}
// get the temperature...
uint8_t temp = readReg(REG_OUT_TEMPERATURE);
m_temperature = (float)temp;
}
float L3GD20::getTemperature(bool fahrenheit)
{
if (fahrenheit)
return c2f(m_temperature);
else
return m_temperature;
}
void L3GD20::setODR(ODR_CUTOFF_T odr)
{
uint8_t reg = readReg(REG_CTRL_REG1);
reg &= ~(_CTRL_REG1_ODR_CUTOFF_MASK << _CTRL_REG1_ODR_CUTOFF_SHIFT);
reg |= (odr << _CTRL_REG1_ODR_CUTOFF_SHIFT);
writeReg(REG_CTRL_REG1, reg);
}
uint8_t L3GD20::getStatusBits()
{
return readReg(REG_STATUS_REG);
}
void
L3GD20::installISR(void (*isr)(char*), void* arg)
{