void MPU9250::beginAccel() {
delay(40);
I2CwriteByte(address, 27, GYRO_FULL_SCALE_2000_DPS);
I2CwriteByte(address, 28, ACC_FULL_SCALE_16_G);
delay(10);
}
void LSM330D::xmWriteByte(uint8_t subAddress, uint8_t data)
{
// Whether we're using I2C or SPI, write a byte using the
// accelerometer-specific I2C address or SPI CS pin.
if (interfaceMode == MODE_I2C)
return I2CwriteByte(xmAddress, subAddress, data);
}
void LSM330D::gWriteByte(uint8_t subAddress, uint8_t data)
{
// Whether we're using I2C or SPI, write a byte using the
// gyro-specific I2C address or SPI CS pin.
if (interfaceMode == MODE_I2C)
I2CwriteByte(gAddress, subAddress, data);
}
void ITG3701::begin(Gscale m_gscale, Godr m_godr, Gbw m_gbw)
{
I2CwriteByte(ITG3701_ADDRESS, ITG3701_PWR_MGMT_1, 0x00); // Clear sleep mode bit (6), enable all sensors
I2CwriteByte(ITG3701_ADDRESS, ITG3701_PWR_MGMT_1, 0x01); // Auto select clock source to be PLL gyroscope reference if ready else
I2CwriteByte(ITG3701_ADDRESS, ITG3701_CONFIG, 0x03);
I2CwriteByte(ITG3701_ADDRESS, ITG3701_SMPLRT_DIV, 0x04);
uint8_t c = I2CreadByte(ITG3701_ADDRESS, ITG3701_GYRO_CONFIG);
I2CwriteByte(ITG3701_ADDRESS, ITG3701_GYRO_CONFIG, c & ~0x02); // Clear Fchoice bits [1:0]
I2CwriteByte(ITG3701_ADDRESS, ITG3701_GYRO_CONFIG, c & ~0x18); // Clear AFS bits [4:3]
I2CwriteByte(ITG3701_ADDRESS, ITG3701_GYRO_CONFIG, c | m_gscale << 3); // Set full scale range for the gyro
I2CwriteByte(ITG3701_ADDRESS, ITG3701_INT_PIN_CFG, 0x20);
I2CwriteByte(ITG3701_ADDRESS, ITG3701_INT_ENABLE, 0x01); // Enable data ready (bit 0) interrupt
Wire.begin();
}
void xmWriteByte(LSM9DS0_t* lsm_t, uint8_t subAddress, uint8_t data)
{
// Whether we're using I2C or SPI, write a byte using the
// accelerometer-specific I2C address or SPI CS pin.
if (lsm_t->interfaceMode == MODE_I2C)
I2CwriteByte(lsm_t->xmAddress, subAddress, data);
//else if (lsm_t->interfaceMode == MODE_SPI)
//SPIwriteByte(lsm_t->xmAddress, subAddress, data,'a');
}
void mWriteByte(uint8_t subAddress, uint8_t data)
{
// Whether we're using I2C or SPI, write a byte using the
// accelerometer-specific I2C address or SPI CS pin.
if (settings.device.commInterface == IMU_MODE_I2C)
{
return I2CwriteByte(_mAddress, subAddress, data);
}
}
void xgWriteByte(uint8_t subAddress, uint8_t data)
{
// Whether we're using I2C or SPI, write a byte using the
// gyro-specific I2C address or SPI CS pin.
if (settings.device.commInterface == IMU_MODE_I2C)
{
I2CwriteByte(_xgAddress, subAddress, data);
}
}
/* ************************************************************************** */
static void xmWriteByte(stLSM9DS0_t * stThis, uint8_t subAddress, uint8_t data)
{
// Whether we're using I2C or SPI, write a byte using the
// accelerometer-specific I2C address or SPI CS pin.
if (stThis->interfaceMode == MODE_I2C)
return I2CwriteByte(stThis, stThis->xmAddress, subAddress, data);
else if (stThis->interfaceMode == MODE_SPI)
return SPIwriteByte(stThis, stThis->xmAddress, subAddress, data);
}
/* ************************************************************************** */
static void gWriteByte(stLSM9DS0_t * stThis, uint8_t subAddress, uint8_t data)
{
// Whether we're using I2C or SPI, write a byte using the
// gyro-specific I2C address or SPI CS pin.
if (stThis->interfaceMode == MODE_I2C)
I2CwriteByte(stThis, stThis->gAddress, subAddress, data);
else if (stThis->interfaceMode == MODE_SPI)
SPIwriteByte(stThis, stThis->gAddress, subAddress, data);
}
void gWriteByte(LSM9DS0_t* lsm_t, uint8_t subAddress, uint8_t data)
{
// Whether we're using I2C or SPI, write a byte using the
// gyro-specific I2C address or SPI CS pin.
#if(LSM_I2C_SUPPORT==1)
if (lsm_t->interfaceMode == MODE_I2C)
I2CwriteByte(lsm_t->lsm_t->gAddress, subAddress, data);
else if (lsm_t->interfaceMode == MODE_SPI)
#endif
SPIwriteByte(lsm_t->gAddress, subAddress, data,'g');
}
void MPU9250::beginMag(uint8_t mode) {
delay(10);
// trun on magnetometor
I2CwriteByte(address, 0x37, 0x02);
delay(10);
magReadAdjustValues();
magSetMode(AK8963_MODE_POWERDOWN);
magSetMode(mode);
}
/***
* read all 9 sensors from the IMU and convert values into metric units
* note: these values will be reported in the coordinate system of the sensor,
* which may be different for gyro, accelerometer, and magnetometer
*/
void Imu::read() {
// all measurements are converted to 16 bits by the IMU-internal ADC
double max16BitValue = 32767.0;
uint8_t Buf[14];
this->I2Cread(MPU9250_ADDRESS, 0x3B, 14, Buf);
/* Read accelerometer */
/* 16 bit accelerometer data */
int16_t ax = Buf[0] << 8 | Buf[1];
int16_t ay = Buf[2] << 8 | Buf[3];
int16_t az = Buf[4] << 8 | Buf[5];
/* scale to get metric data in m/s^2 */
// float maxAccRange = 2.0; // in g
double maxAccRange = 16.0; // max range (in g)
// as set in setup()
// function
double g2ms2 = 9.80665;
double accScale = g2ms2 * maxAccRange / max16BitValue; // convert 16 bit to
// float
/* convert 16 bit raw measurement to metric float */
accX = - double(ax) * accScale;
accY = double(ay) * accScale;
accZ = - double(az) * accScale;
/* Read gyroscope */
/* 16 bit gyroscope raw data */
int16_t gx = Buf[8] << 8 | Buf[9];
int16_t gy = Buf[10] << 8 | Buf[11];
int16_t gz = Buf[12] << 8 | Buf[13];
double maxGyrRange = 2000.0; // max range (in deg per sec)
// as set in setup() function
// float maxGyrRange = 500.0;
double gyrScale = maxGyrRange / max16BitValue; // convert 16 bit to float
/* convert 16 bit raw measurement to metric float */
gyrX = - double(gx) * gyrScale;
gyrY = double(gy) * gyrScale;
gyrZ = - double(gz) * gyrScale;
/* Read magnetometer */
uint8_t ST1;
I2Cread(MAG_ADDRESS, 0x02, 1, &ST1);
/* new measurement available (otherwise just move on) */
if (ST1 & 0x01) {
/* Read magnetometer data */
uint8_t m[6];
I2Cread(MAG_ADDRESS, 0x03, 6, m);
/***
* see datatsheet:
* - byte order is reverse from other sensors
* - x and y are flipped
* - z axis is reverse
*/
int16_t mmy = m[1] << 8 | m[0];
int16_t mmx = m[3] << 8 | m[2];
int16_t mmz = -m[5] << 8 | m[4];
/* convert 16 bit raw measurement to metric float */
double magScale = 4912.0 / max16BitValue;
this->magX = double(mmx) * magScale * this->_magnetometerAdjustmentScaleX;
this->magY = double(mmy) * magScale * this->_magnetometerAdjustmentScaleY;
this->magZ = double(mmz) * magScale * this->_magnetometerAdjustmentScaleZ;
/* request next reading on magnetometer */
I2CwriteByte(MAG_ADDRESS, 0x0A, B00010001);
}
}
void MPU9250::magSetMode(uint8_t mode) {
I2CwriteByte(MAG_ADDRESS, AK8963_RA_CNTL1, mode);
delay(10);
}
