g1smo
/
c2-utopia
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
c2-utopia/lib/RTIMULib/RTIMUMPU9150.cpp

546 lines
14 KiB
C++
Raw Blame History

////////////////////////////////////////////////////////////////////////////
//
// This file is part of RTIMULib-Arduino
//
// Copyright (c) 2014-2015, richards-tech
//
// Permission is hereby granted, free of charge, to any person obtaining a copy of
// this software and associated documentation files (the "Software"), to deal in
// the Software without restriction, including without limitation the rights to use,
// copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the
// Software, and to permit persons to whom the Software is furnished to do so,
// subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
// INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
// PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
// SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
#include "RTIMUMPU9150.h"
#include "RTIMUSettings.h"
#if defined(MPU9150_68) || defined(MPU9150_69)
RTIMUMPU9150::RTIMUMPU9150(RTIMUSettings *settings) : RTIMU(settings)
{
}
RTIMUMPU9150::~RTIMUMPU9150()
{
}
bool RTIMUMPU9150::setLpf(unsigned char lpf)
{
switch (lpf) {
case MPU9150_LPF_256:
case MPU9150_LPF_188:
case MPU9150_LPF_98:
case MPU9150_LPF_42:
case MPU9150_LPF_20:
case MPU9150_LPF_10:
case MPU9150_LPF_5:
m_lpf = lpf;
return true;
default:
return false;
}
}
bool RTIMUMPU9150::setSampleRate(int rate)
{
if ((rate < MPU9150_SAMPLERATE_MIN) || (rate > MPU9150_SAMPLERATE_MAX)) {
return false;
}
m_sampleRate = rate;
m_sampleInterval = (unsigned long)1000 / m_sampleRate;
if (m_sampleInterval == 0)
m_sampleInterval = 1;
return true;
}
bool RTIMUMPU9150::setCompassRate(int rate)
{
if ((rate < MPU9150_COMPASSRATE_MIN) || (rate > MPU9150_COMPASSRATE_MAX)) {
return false;
}
m_compassRate = rate;
return true;
}
bool RTIMUMPU9150::setGyroFsr(unsigned char fsr)
{
switch (fsr) {
case MPU9150_GYROFSR_250:
m_gyroFsr = fsr;
m_gyroScale = RTMATH_PI / (131.0 * 180.0);
return true;
case MPU9150_GYROFSR_500:
m_gyroFsr = fsr;
m_gyroScale = RTMATH_PI / (62.5 * 180.0);
return true;
case MPU9150_GYROFSR_1000:
m_gyroFsr = fsr;
m_gyroScale = RTMATH_PI / (32.8 * 180.0);
return true;
case MPU9150_GYROFSR_2000:
m_gyroFsr = fsr;
m_gyroScale = RTMATH_PI / (16.4 * 180.0);
return true;
default:
return false;
}
}
bool RTIMUMPU9150::setAccelFsr(unsigned char fsr)
{
switch (fsr) {
case MPU9150_ACCELFSR_2:
m_accelFsr = fsr;
m_accelScale = 1.0/16384.0;
return true;
case MPU9150_ACCELFSR_4:
m_accelFsr = fsr;
m_accelScale = 1.0/8192.0;
return true;
case MPU9150_ACCELFSR_8:
m_accelFsr = fsr;
m_accelScale = 1.0/4096.0;
return true;
case MPU9150_ACCELFSR_16:
m_accelFsr = fsr;
m_accelScale = 1.0/2048.0;
return true;
default:
return false;
}
}
int RTIMUMPU9150::IMUInit()
{
unsigned char result;
m_firstTime = true;
m_compassPresent = true;
#ifdef MPU9150_CACHE_MODE
m_cacheIn = m_cacheOut = m_cacheCount = 0;
#endif
// configure IMU
m_slaveAddr = m_settings->m_I2CSlaveAddress;
setSampleRate(m_settings->m_MPU9150GyroAccelSampleRate);
setCompassRate(m_settings->m_MPU9150CompassSampleRate);
setLpf(m_settings->m_MPU9150GyroAccelLpf);
setGyroFsr(m_settings->m_MPU9150GyroFsr);
setAccelFsr(m_settings->m_MPU9150AccelFsr);
setCalibrationData();
// reset the MPU9150
if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_PWR_MGMT_1, 0x80))
return -1;
delay(100);
if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_PWR_MGMT_1, 0x00))
return -4;
if (!I2Cdev::readByte(m_slaveAddr, MPU9150_WHO_AM_I, &result))
return -5;
if (result != 0x68) {
return -6;
}
// now configure the various components
if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_LPF_CONFIG, m_lpf))
return -7;
if (!setSampleRate())
return -8;
if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_GYRO_CONFIG, m_gyroFsr))
return -9;
if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_ACCEL_CONFIG, m_accelFsr))
return -10;
// now configure compass
result = configureCompass();
if (result < 0)
return result;
// enable the sensors
if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_PWR_MGMT_1, 1))
return -28;
if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_PWR_MGMT_2, 0))
return -29;
// select the data to go into the FIFO and enable
if (!resetFifo())
return -30;
gyroBiasInit();
return 1;
}
bool RTIMUMPU9150::configureCompass()
{
unsigned char asa[3];
unsigned char id;
m_compassIs5883 = false;
m_compassDataLength = 8;
if (!bypassOn())
return -11;
// get fuse ROM data
if (!I2Cdev::writeByte(AK8975_ADDRESS, AK8975_CNTL, 0)) {
// check to see if an HMC5883L is fitted
if (!I2Cdev::readBytes(HMC5883_ADDRESS, HMC5883_ID, 1, &id)) {
bypassOff();
// this is returning true so that MPU-6050 by itself will work
m_compassPresent = false;
return 1;
}
if (id != 0x48) { // incorrect id for HMC5883L
bypassOff();
// this is returning true so that MPU-6050 by itself will work
m_compassPresent = false;
return 1;
}
// HMC5883 is present - use that
if (!I2Cdev::writeByte(HMC5883_ADDRESS, HMC5883_CONFIG_A, 0x38)) {
bypassOff();
return -12;
}
if (!I2Cdev::writeByte(HMC5883_ADDRESS, HMC5883_CONFIG_B, 0x20)) {
bypassOff();
return -12;
}
if (!I2Cdev::writeByte(HMC5883_ADDRESS, HMC5883_MODE, 0x00)) {
bypassOff();
return -12;
}
Serial.println("Detected MPU-6050 with HMC5883");
m_compassDataLength = 6;
m_compassIs5883 = true;
} else {
if (!I2Cdev::writeByte(AK8975_ADDRESS, AK8975_CNTL, 0x0f)) {
bypassOff();
return -13;
}
if (!I2Cdev::readBytes(AK8975_ADDRESS, AK8975_ASAX, 3, asa)) {
bypassOff();
return -14;
}
// convert asa to usable scale factor
m_compassAdjust[0] = ((float)asa[0] - 128.0) / 256.0 + 1.0f;
m_compassAdjust[1] = ((float)asa[1] - 128.0) / 256.0 + 1.0f;
m_compassAdjust[2] = ((float)asa[2] - 128.0) / 256.0 + 1.0f;
if (!I2Cdev::writeByte(AK8975_ADDRESS, AK8975_CNTL, 0)) {
bypassOff();
return -15;
}
}
if (!bypassOff())
return -16;
// now set up MPU9150 to talk to the compass chip
if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_I2C_MST_CTRL, 0x40))
return -17;
if (m_compassIs5883) {
if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_I2C_SLV0_ADDR, 0x80 | HMC5883_ADDRESS))
return -18;
if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_I2C_SLV0_REG, HMC5883_DATA_X_HI))
return -19;
if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_I2C_SLV0_CTRL, 0x86))
return -20;
} else {
if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_I2C_SLV0_ADDR, 0x80 | AK8975_ADDRESS))
return -18;
if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_I2C_SLV0_REG, AK8975_ST1))
return -19;
if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_I2C_SLV0_CTRL, 0x88))
return -20;
if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_I2C_SLV1_ADDR, AK8975_ADDRESS))
return -21;
if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_I2C_SLV1_REG, AK8975_CNTL))
return -22;
if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_I2C_SLV1_CTRL, 0x81))
return -23;
if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_I2C_SLV1_DO, 0x1))
return -24;
}
if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_I2C_MST_DELAY_CTRL, 0x3))
return -25;
if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_YG_OFFS_TC, 0x80))
return -26;
if (!setCompassRate())
return -27;
return 1;
}
bool RTIMUMPU9150::resetFifo()
{
if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_INT_ENABLE, 0))
return false;
if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_FIFO_EN, 0))
return false;
if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_USER_CTRL, 0))
return false;
if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_USER_CTRL, 0x04))
return false;
if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_USER_CTRL, 0x60))
return false;
delay(50);
if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_INT_ENABLE, 1))
return false;
if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_FIFO_EN, 0x78))
return false;
return true;
}
bool RTIMUMPU9150::bypassOn()
{
unsigned char userControl;
if (!I2Cdev::readByte(m_slaveAddr, MPU9150_USER_CTRL, &userControl))
return false;
userControl &= ~0x20;
userControl |= 2;
if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_USER_CTRL, userControl))
return false;
delay(50);
if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_INT_PIN_CFG, 0x82))
return false;
delay(50);
return true;
}
bool RTIMUMPU9150::bypassOff()
{
unsigned char userControl;
if (!I2Cdev::readByte(m_slaveAddr, MPU9150_USER_CTRL, &userControl))
return false;
userControl |= 0x20;
if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_USER_CTRL, userControl))
return false;
delay(50);
if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_INT_PIN_CFG, 0x80))
return false;
delay(50);
return true;
}
bool RTIMUMPU9150::setSampleRate()
{
int clockRate = 1000;
if (m_lpf == MPU9150_LPF_256)
clockRate = 8000;
if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_SMPRT_DIV, (unsigned char)(clockRate / m_sampleRate - 1)))
return false;
return true;
}
bool RTIMUMPU9150::setCompassRate()
{
int rate;
rate = m_sampleRate / m_compassRate - 1;
if (rate > 31)
rate = 31;
if (!I2Cdev::writeByte(m_slaveAddr, MPU9150_I2C_SLV4_CTRL, rate))
return false;
return true;
}
int RTIMUMPU9150::IMUGetPollInterval()
{
return (400 / m_sampleRate);
}
bool RTIMUMPU9150::IMURead()
{
unsigned char fifoCount[2];
unsigned int count;
unsigned char fifoData[12];
unsigned char compassData[8];
if (!I2Cdev::readBytes(m_slaveAddr, MPU9150_FIFO_COUNT_H, 2, fifoCount))
return false;
count = ((unsigned int)fifoCount[0] << 8) + fifoCount[1];
if (count == 1024) {
resetFifo();
m_timestamp += m_sampleInterval * (1024 / MPU9150_FIFO_CHUNK_SIZE + 1); // try to fix timestamp
return false;
}
if (count > MPU9150_FIFO_CHUNK_SIZE * 40) {
// more than 40 samples behind - going too slowly so discard some samples but maintain timestamp correctly
while (count >= MPU9150_FIFO_CHUNK_SIZE * 10) {
if (!I2Cdev::readBytes(m_slaveAddr, MPU9150_FIFO_R_W, MPU9150_FIFO_CHUNK_SIZE, fifoData))
return false;
count -= MPU9150_FIFO_CHUNK_SIZE;
m_timestamp += m_sampleInterval;
}
}
if (count < MPU9150_FIFO_CHUNK_SIZE)
return false;
if (!I2Cdev::readBytes(m_slaveAddr, MPU9150_FIFO_R_W, MPU9150_FIFO_CHUNK_SIZE, fifoData))
return false;
if (!I2Cdev::readBytes(m_slaveAddr, MPU9150_EXT_SENS_DATA_00, m_compassDataLength, compassData))
return false;
RTMath::convertToVector(fifoData, m_accel, m_accelScale, true);
RTMath::convertToVector(fifoData + 6, m_gyro, m_gyroScale, true);
if (m_compassIs5883)
RTMath::convertToVector(compassData, m_compass, 0.092f, true);
else
RTMath::convertToVector(compassData + 1, m_compass, 0.3f, false);
// sort out gyro axes
m_gyro.setY(-m_gyro.y());
m_gyro.setZ(-m_gyro.z());
// sort out accel data;
m_accel.setX(-m_accel.x());
if (m_compassPresent) {
if (m_compassIs5883) {
// sort out compass axes
float temp;
temp = m_compass.y();
m_compass.setY(-m_compass.z());
m_compass.setZ(-temp);
} else {
// use the compass fuse data adjustments
m_compass.setX(m_compass.x() * m_compassAdjust[0]);
m_compass.setY(m_compass.y() * m_compassAdjust[1]);
m_compass.setZ(m_compass.z() * m_compassAdjust[2]);
// sort out compass axes
float temp;
temp = m_compass.x();
m_compass.setX(m_compass.y());
m_compass.setY(-temp);
}
} else {
m_compass.setX(0);
m_compass.setY(0);
m_compass.setZ(0);
}
// now do standard processing
handleGyroBias();
if (m_compassPresent)
calibrateAverageCompass();
if (m_firstTime)
m_timestamp = millis();
else
m_timestamp += m_sampleInterval;
m_firstTime = false;
return true;
}
#endif
Reference in New Issue View Git Blame Copy Permalink