494 lines
12 KiB
C++
494 lines
12 KiB
C++
////////////////////////////////////////////////////////////////////////////
|
|
//
|
|
// 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 "RTIMUMPU9250.h"
|
|
#include "RTIMUSettings.h"
|
|
|
|
#if defined(MPU9250_68) || defined(MPU9250_69)
|
|
|
|
RTIMUMPU9250::RTIMUMPU9250(RTIMUSettings *settings) : RTIMU(settings)
|
|
{
|
|
|
|
}
|
|
|
|
RTIMUMPU9250::~RTIMUMPU9250()
|
|
{
|
|
}
|
|
|
|
|
|
bool RTIMUMPU9250::setSampleRate(int rate)
|
|
{
|
|
if ((rate < MPU9250_SAMPLERATE_MIN) || (rate > MPU9250_SAMPLERATE_MAX)) {
|
|
return false;
|
|
}
|
|
m_sampleRate = rate;
|
|
m_sampleInterval = (unsigned long)1000 / m_sampleRate;
|
|
if (m_sampleInterval == 0)
|
|
m_sampleInterval = 1;
|
|
return true;
|
|
}
|
|
|
|
bool RTIMUMPU9250::setGyroLpf(unsigned char lpf)
|
|
{
|
|
switch (lpf) {
|
|
case MPU9250_GYRO_LPF_8800:
|
|
case MPU9250_GYRO_LPF_3600:
|
|
case MPU9250_GYRO_LPF_250:
|
|
case MPU9250_GYRO_LPF_184:
|
|
case MPU9250_GYRO_LPF_92:
|
|
case MPU9250_GYRO_LPF_41:
|
|
case MPU9250_GYRO_LPF_20:
|
|
case MPU9250_GYRO_LPF_10:
|
|
case MPU9250_GYRO_LPF_5:
|
|
m_gyroLpf = lpf;
|
|
return true;
|
|
|
|
default:
|
|
return false;
|
|
}
|
|
}
|
|
|
|
bool RTIMUMPU9250::setAccelLpf(unsigned char lpf)
|
|
{
|
|
switch (lpf) {
|
|
case MPU9250_ACCEL_LPF_1130:
|
|
case MPU9250_ACCEL_LPF_460:
|
|
case MPU9250_ACCEL_LPF_184:
|
|
case MPU9250_ACCEL_LPF_92:
|
|
case MPU9250_ACCEL_LPF_41:
|
|
case MPU9250_ACCEL_LPF_20:
|
|
case MPU9250_ACCEL_LPF_10:
|
|
case MPU9250_ACCEL_LPF_5:
|
|
m_accelLpf = lpf;
|
|
return true;
|
|
|
|
default:
|
|
return false;
|
|
}
|
|
}
|
|
|
|
bool RTIMUMPU9250::setCompassRate(int rate)
|
|
{
|
|
if ((rate < MPU9250_COMPASSRATE_MIN) || (rate > MPU9250_COMPASSRATE_MAX)) {
|
|
return false;
|
|
}
|
|
m_compassRate = rate;
|
|
return true;
|
|
}
|
|
|
|
bool RTIMUMPU9250::setGyroFsr(unsigned char fsr)
|
|
{
|
|
switch (fsr) {
|
|
case MPU9250_GYROFSR_250:
|
|
m_gyroFsr = fsr;
|
|
m_gyroScale = RTMATH_PI / (131.0 * 180.0);
|
|
return true;
|
|
|
|
case MPU9250_GYROFSR_500:
|
|
m_gyroFsr = fsr;
|
|
m_gyroScale = RTMATH_PI / (62.5 * 180.0);
|
|
return true;
|
|
|
|
case MPU9250_GYROFSR_1000:
|
|
m_gyroFsr = fsr;
|
|
m_gyroScale = RTMATH_PI / (32.8 * 180.0);
|
|
return true;
|
|
|
|
case MPU9250_GYROFSR_2000:
|
|
m_gyroFsr = fsr;
|
|
m_gyroScale = RTMATH_PI / (16.4 * 180.0);
|
|
return true;
|
|
|
|
default:
|
|
return false;
|
|
}
|
|
}
|
|
|
|
bool RTIMUMPU9250::setAccelFsr(unsigned char fsr)
|
|
{
|
|
switch (fsr) {
|
|
case MPU9250_ACCELFSR_2:
|
|
m_accelFsr = fsr;
|
|
m_accelScale = 1.0/16384.0;
|
|
return true;
|
|
|
|
case MPU9250_ACCELFSR_4:
|
|
m_accelFsr = fsr;
|
|
m_accelScale = 1.0/8192.0;
|
|
return true;
|
|
|
|
case MPU9250_ACCELFSR_8:
|
|
m_accelFsr = fsr;
|
|
m_accelScale = 1.0/4096.0;
|
|
return true;
|
|
|
|
case MPU9250_ACCELFSR_16:
|
|
m_accelFsr = fsr;
|
|
m_accelScale = 1.0/2048.0;
|
|
return true;
|
|
|
|
default:
|
|
return false;
|
|
}
|
|
}
|
|
|
|
|
|
int RTIMUMPU9250::IMUInit()
|
|
{
|
|
unsigned char result;
|
|
unsigned char asa[3];
|
|
|
|
m_firstTime = true;
|
|
|
|
#ifdef MPU9250_CACHE_MODE
|
|
m_cacheIn = m_cacheOut = m_cacheCount = 0;
|
|
#endif
|
|
// configure IMU
|
|
|
|
m_slaveAddr = m_settings->m_I2CSlaveAddress;
|
|
|
|
setSampleRate(m_settings->m_MPU9250GyroAccelSampleRate);
|
|
setCompassRate(m_settings->m_MPU9250CompassSampleRate);
|
|
setGyroLpf(m_settings->m_MPU9250GyroLpf);
|
|
setAccelLpf(m_settings->m_MPU9250AccelLpf);
|
|
setGyroFsr(m_settings->m_MPU9250GyroFsr);
|
|
setAccelFsr(m_settings->m_MPU9250AccelFsr);
|
|
|
|
setCalibrationData();
|
|
|
|
// reset the MPU9250
|
|
|
|
if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_PWR_MGMT_1, 0x80))
|
|
return -1;
|
|
|
|
delay(100);
|
|
|
|
if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_PWR_MGMT_1, 0x00))
|
|
return -4;
|
|
|
|
if (!I2Cdev::readByte(m_slaveAddr, MPU9250_WHO_AM_I, &result))
|
|
return -5;
|
|
|
|
if (result != MPU9250_ID) {
|
|
return -6;
|
|
}
|
|
|
|
// now configure the various components
|
|
|
|
if (!setGyroConfig())
|
|
return -7;
|
|
|
|
if (!setAccelConfig())
|
|
return -8;
|
|
|
|
if (!setSampleRate())
|
|
return -9;
|
|
|
|
// now configure compass
|
|
|
|
if (!bypassOn())
|
|
return -11;
|
|
|
|
// get fuse ROM data
|
|
|
|
if (!I2Cdev::writeByte(AK8963_ADDRESS, AK8963_CNTL, 0)) {
|
|
bypassOff();
|
|
return -12;
|
|
}
|
|
|
|
if (!I2Cdev::writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x0f)) {
|
|
bypassOff();
|
|
return -13;
|
|
}
|
|
|
|
if (!I2Cdev::readBytes(AK8963_ADDRESS, AK8963_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(AK8963_ADDRESS, AK8963_CNTL, 0)) {
|
|
bypassOff();
|
|
return -15;
|
|
}
|
|
|
|
if (!bypassOff())
|
|
return -16;
|
|
|
|
// now set up MPU9250 to talk to the compass chip
|
|
|
|
if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_I2C_MST_CTRL, 0x40))
|
|
return -17;
|
|
|
|
if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_I2C_SLV0_ADDR, 0x80 | AK8963_ADDRESS))
|
|
return -18;
|
|
|
|
if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_I2C_SLV0_REG, AK8963_ST1))
|
|
return -19;
|
|
|
|
if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_I2C_SLV0_CTRL, 0x88))
|
|
return -20;
|
|
|
|
if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_I2C_SLV1_ADDR, AK8963_ADDRESS))
|
|
return -21;
|
|
|
|
if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_I2C_SLV1_REG, AK8963_CNTL))
|
|
return -22;
|
|
|
|
if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_I2C_SLV1_CTRL, 0x81))
|
|
return -23;
|
|
|
|
if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_I2C_SLV1_DO, 0x1))
|
|
return -24;
|
|
|
|
if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_I2C_MST_DELAY_CTRL, 0x3))
|
|
return -25;
|
|
|
|
if (!setCompassRate())
|
|
return -27;
|
|
|
|
// enable the sensors
|
|
|
|
if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_PWR_MGMT_1, 1))
|
|
return -28;
|
|
|
|
if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_PWR_MGMT_2, 0))
|
|
return -29;
|
|
|
|
// select the data to go into the FIFO and enable
|
|
|
|
if (!resetFifo())
|
|
return -30;
|
|
|
|
gyroBiasInit();
|
|
return 1;
|
|
}
|
|
|
|
bool RTIMUMPU9250::resetFifo()
|
|
{
|
|
if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_INT_ENABLE, 0))
|
|
return false;
|
|
if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_FIFO_EN, 0))
|
|
return false;
|
|
if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_USER_CTRL, 0))
|
|
return false;
|
|
|
|
if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_USER_CTRL, 0x04))
|
|
return false;
|
|
|
|
if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_USER_CTRL, 0x60))
|
|
return false;
|
|
|
|
delay(50);
|
|
|
|
if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_INT_ENABLE, 1))
|
|
return false;
|
|
|
|
if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_FIFO_EN, 0x78))
|
|
return false;
|
|
|
|
return true;
|
|
}
|
|
|
|
bool RTIMUMPU9250::bypassOn()
|
|
{
|
|
unsigned char userControl;
|
|
|
|
if (!I2Cdev::readByte(m_slaveAddr, MPU9250_USER_CTRL, &userControl))
|
|
return false;
|
|
|
|
userControl &= ~0x20;
|
|
userControl |= 2;
|
|
|
|
if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_USER_CTRL, userControl))
|
|
return false;
|
|
|
|
delay(50);
|
|
|
|
if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_INT_PIN_CFG, 0x82))
|
|
return false;
|
|
|
|
delay(50);
|
|
return true;
|
|
}
|
|
|
|
|
|
bool RTIMUMPU9250::bypassOff()
|
|
{
|
|
unsigned char userControl;
|
|
|
|
if (!I2Cdev::readByte(m_slaveAddr, MPU9250_USER_CTRL, &userControl))
|
|
return false;
|
|
|
|
userControl |= 0x20;
|
|
|
|
if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_USER_CTRL, userControl))
|
|
return false;
|
|
|
|
delay(50);
|
|
|
|
if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_INT_PIN_CFG, 0x80))
|
|
return false;
|
|
|
|
delay(50);
|
|
return true;
|
|
}
|
|
|
|
bool RTIMUMPU9250::setGyroConfig()
|
|
{
|
|
unsigned char gyroConfig = m_gyroFsr + ((m_gyroLpf >> 3) & 3);
|
|
unsigned char gyroLpf = m_gyroLpf & 7;
|
|
|
|
if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_GYRO_CONFIG, gyroConfig))
|
|
return false;
|
|
|
|
if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_GYRO_LPF, gyroLpf))
|
|
return false;
|
|
return true;
|
|
}
|
|
|
|
bool RTIMUMPU9250::setAccelConfig()
|
|
{
|
|
if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_ACCEL_CONFIG, m_accelFsr))
|
|
return false;
|
|
|
|
if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_ACCEL_LPF, m_accelLpf))
|
|
return false;
|
|
return true;
|
|
}
|
|
|
|
bool RTIMUMPU9250::setSampleRate()
|
|
{
|
|
if (m_sampleRate > 1000)
|
|
return true; // SMPRT not used above 1000Hz
|
|
|
|
if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_SMPRT_DIV, (unsigned char) (1000 / m_sampleRate - 1)))
|
|
return false;
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
bool RTIMUMPU9250::setCompassRate()
|
|
{
|
|
int rate;
|
|
|
|
rate = m_sampleRate / m_compassRate - 1;
|
|
|
|
if (rate > 31)
|
|
rate = 31;
|
|
if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_I2C_SLV4_CTRL, rate))
|
|
return false;
|
|
return true;
|
|
}
|
|
|
|
int RTIMUMPU9250::IMUGetPollInterval()
|
|
{
|
|
return (400 / m_sampleRate);
|
|
}
|
|
|
|
bool RTIMUMPU9250::IMURead()
|
|
{
|
|
unsigned char fifoCount[2];
|
|
unsigned int count;
|
|
unsigned char fifoData[12];
|
|
unsigned char compassData[8];
|
|
|
|
if (!I2Cdev::readBytes(m_slaveAddr, MPU9250_FIFO_COUNT_H, 2, fifoCount))
|
|
return false;
|
|
|
|
count = ((unsigned int)fifoCount[0] << 8) + fifoCount[1];
|
|
|
|
if (count == 1024) {
|
|
resetFifo();
|
|
m_timestamp += m_sampleInterval * (1024 / MPU9250_FIFO_CHUNK_SIZE + 1); // try to fix timestamp
|
|
return false;
|
|
}
|
|
|
|
if (count > MPU9250_FIFO_CHUNK_SIZE * 40) {
|
|
// more than 40 samples behind - going too slowly so discard some samples but maintain timestamp correctly
|
|
while (count >= MPU9250_FIFO_CHUNK_SIZE * 10) {
|
|
if (!I2Cdev::readBytes(m_slaveAddr, MPU9250_FIFO_R_W, MPU9250_FIFO_CHUNK_SIZE, fifoData))
|
|
return false;
|
|
count -= MPU9250_FIFO_CHUNK_SIZE;
|
|
m_timestamp += m_sampleInterval;
|
|
}
|
|
}
|
|
|
|
if (count < MPU9250_FIFO_CHUNK_SIZE)
|
|
return false;
|
|
|
|
if (!I2Cdev::readBytes(m_slaveAddr, MPU9250_FIFO_R_W, MPU9250_FIFO_CHUNK_SIZE, fifoData))
|
|
return false;
|
|
|
|
if (!I2Cdev::readBytes(m_slaveAddr, MPU9250_EXT_SENS_DATA_00, 8, compassData))
|
|
return false;
|
|
|
|
RTMath::convertToVector(fifoData, m_accel, m_accelScale, true);
|
|
RTMath::convertToVector(fifoData + 6, m_gyro, m_gyroScale, true);
|
|
RTMath::convertToVector(compassData + 1, m_compass, 0.6f, 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());
|
|
|
|
// use the fuse data adjustments for compass
|
|
|
|
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);
|
|
|
|
// now do standard processing
|
|
|
|
handleGyroBias();
|
|
calibrateAverageCompass();
|
|
|
|
if (m_firstTime)
|
|
m_timestamp = millis();
|
|
else
|
|
m_timestamp += m_sampleInterval;
|
|
|
|
m_firstTime = false;
|
|
|
|
return true;
|
|
}
|
|
#endif
|