bool RTIMUMPU9150::configureCompass()
{
unsigned char asa[3];
unsigned char id;
m_compassIs5883 = false;
m_compassDataLength = 8;
bypassOn();
// get fuse ROM data
if (!m_settings->HALWrite(AK8975_ADDRESS, AK8975_CNTL, 0, "Failed to set compass in power down mode 1")) {
bypassOff();
return false;
}
if (!m_settings->HALWrite(AK8975_ADDRESS, AK8975_CNTL, 0x0f, "Failed to set compass in fuse ROM mode")) {
bypassOff();
return false;
}
if (!m_settings->HALRead(AK8975_ADDRESS, AK8975_ASAX, 3, asa, "")) {
// check to see if an HMC5883L is fitted
if (!m_settings->HALRead(HMC5883_ADDRESS, HMC5883_ID, 1, &id, "Failed to find 5883")) {
bypassOff();
// this is returning true so that MPU-6050 by itself will work
HAL_INFO("Detected MPU-6050 without compass\n");
m_imuData.compassValid = false;
return true;
}
if (id != 0x48) { // incorrect id for HMC5883L
bypassOff();
// this is returning true so that MPU-6050 by itself will work
HAL_INFO("Detected MPU-6050 without compass\n");
m_imuData.compassValid = false;
return true;
}
// HMC5883 is present - use that
if (!m_settings->HALWrite(HMC5883_ADDRESS, HMC5883_CONFIG_A, 0x38, "Failed to set HMC5883 config A")) {
bypassOff();
return false;
}
if (!m_settings->HALWrite(HMC5883_ADDRESS, HMC5883_CONFIG_B, 0x20, "Failed to set HMC5883 config B")) {
bypassOff();
return false;
}
if (!m_settings->HALWrite(HMC5883_ADDRESS, HMC5883_MODE, 0x00, "Failed to set HMC5883 mode")) {
bypassOff();
return false;
}
HAL_INFO("Detected MPU-6050 with HMC5883\n");
m_compassDataLength = 6;
m_compassIs5883 = true;
} else {
// 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 (!m_settings->HALWrite(AK8975_ADDRESS, AK8975_CNTL, 0, "Failed to set compass in power down mode 2")) {
bypassOff();
return false;
}
}
bypassOff();
// now set up MPU9150 to talk to the compass chip
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_MST_CTRL, 0x40, "Failed to set I2C master mode"))
return false;
if (m_compassIs5883) {
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV0_ADDR, 0x80 | HMC5883_ADDRESS, "Failed to set slave 0 address"))
return false;
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV0_REG, HMC5883_DATA_X_HI, "Failed to set slave 0 reg"))
return false;
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV0_CTRL, 0x86, "Failed to set slave 0 ctrl"))
return false;
} else {
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV0_ADDR, 0x80 | AK8975_ADDRESS, "Failed to set slave 0 address"))
return false;
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV0_REG, AK8975_ST1, "Failed to set slave 0 reg"))
return false;
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV0_CTRL, 0x88, "Failed to set slave 0 ctrl"))
return false;
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV1_ADDR, AK8975_ADDRESS, "Failed to set slave 1 address"))
return false;
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV1_REG, AK8975_CNTL, "Failed to set slave 1 reg"))
return false;
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV1_CTRL, 0x81, "Failed to set slave 1 ctrl"))
return false;
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV1_DO, 0x1, "Failed to set slave 1 DO"))
return false;
}
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_MST_DELAY_CTRL, 0x3, "Failed to set mst delay"))
return false;
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_YG_OFFS_TC, 0x80, "Failed to set yg offs tc"))
return false;
if (!setCompassRate())
return false;
return true;
}
bool RTIMUMPU9150::IMUInit()
{
unsigned char result;
m_firstTime = true;
#ifdef MPU9150_CACHE_MODE
m_cacheIn = m_cacheOut = m_cacheCount = 0;
#endif
// set validity flags
m_imuData.fusionPoseValid = false;
m_imuData.fusionQPoseValid = false;
m_imuData.gyroValid = true;
m_imuData.accelValid = true;
m_imuData.compassValid = true;
m_imuData.pressureValid = false;
m_imuData.temperatureValid = false;
m_imuData.humidityValid = false;
// 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();
// enable the I2C bus
if (!m_settings->HALOpen())
return false;
// reset the MPU9150
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_PWR_MGMT_1, 0x80, "Failed to initiate MPU9150 reset"))
return false;
m_settings->delayMs(100);
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_PWR_MGMT_1, 0x00, "Failed to stop MPU9150 reset"))
return false;
if (!m_settings->HALRead(m_slaveAddr, MPU9150_WHO_AM_I, 1, &result, "Failed to read MPU9150 id"))
return false;
if (result != MPU9150_ID) {
HAL_ERROR1("Incorrect MPU9150 id %d\n", result);
return false;
}
// now configure the various components
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_LPF_CONFIG, m_lpf, "Failed to set lpf"))
return false;
if (!setSampleRate())
return false;
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_GYRO_CONFIG, m_gyroFsr, "Failed to set gyro fsr"))
return false;
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_ACCEL_CONFIG, m_accelFsr, "Failed to set accel fsr"))
return false;
// now configure compass
if (!configureCompass())
return false;
// enable the sensors
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_PWR_MGMT_1, 1, "Failed to set pwr_mgmt_1"))
return false;
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_PWR_MGMT_2, 0, "Failed to set pwr_mgmt_2"))
return false;
// select the data to go into the FIFO and enable
if (!resetFifo())
return false;
gyroBiasInit();
HAL_INFO("MPU9150 init complete\n");
return true;
}
bool RTIMUMPU9255::IMUInit()
{
unsigned char result;
m_firstTime = true;
#ifdef MPU9255_CACHE_MODE
m_cacheIn = m_cacheOut = m_cacheCount = 0;
#endif
// set validity flags
m_imuData.fusionPoseValid = false;
m_imuData.fusionQPoseValid = false;
m_imuData.gyroValid = true;
m_imuData.accelValid = true;
m_imuData.compassValid = true;
m_imuData.motion = true;
m_imuData.IMUtemperatureValid = false;
m_imuData.IMUtemperature = 0.0;
m_imuData.humidityValid = false;
m_imuData.humidity = -1.0;
m_imuData.humidityTemperatureValid = false;
m_imuData.humidityTemperature = 0.0;
m_imuData.pressureValid = false;
m_imuData.pressure = 0.0;
m_imuData.pressureTemperatureValid = false;
m_imuData.pressureTemperature = 0.0;
// configure IMU
m_slaveAddr = m_settings->m_I2CSlaveAddress;
setSampleRate(m_settings->m_MPU9255GyroAccelSampleRate);
setCompassRate(m_settings->m_MPU9255CompassSampleRate);
setGyroLpf(m_settings->m_MPU9255GyroLpf);
setAccelLpf(m_settings->m_MPU9255AccelLpf);
setGyroFsr(m_settings->m_MPU9255GyroFsr);
setAccelFsr(m_settings->m_MPU9255AccelFsr);
setCalibrationData();
// enable the bus
if (!m_settings->HALOpen())
return false;
// reset the MPU9255
if (!m_settings->HALWrite(m_slaveAddr, MPU9255_PWR_MGMT_1, 0x80, "Failed to initiate MPU9255 reset"))
return false;
m_settings->delayMs(100);
if (!m_settings->HALWrite(m_slaveAddr, MPU9255_PWR_MGMT_1, 0x00, "Failed to stop MPU9255 reset"))
return false;
if (!m_settings->HALRead(m_slaveAddr, MPU9255_WHO_AM_I, 1, &result, "Failed to read MPU9255 id"))
return false;
if (result != MPU9255_ID) {
HAL_ERROR2("Incorrect %s id %d\n", IMUName(), result);
return false;
}
// now configure the various components
if (!setGyroConfig())
return false;
if (!setAccelConfig())
return false;
if (!setSampleRate())
return false;
if(!compassSetup()) {
return false;
}
if (!setCompassRate())
return false;
// enable the sensors
if (!m_settings->HALWrite(m_slaveAddr, MPU9255_PWR_MGMT_1, 1, "Failed to set pwr_mgmt_1"))
return false;
if (!m_settings->HALWrite(m_slaveAddr, MPU9255_PWR_MGMT_2, 0, "Failed to set pwr_mgmt_2"))
return false;
// select the data to go into the FIFO and enable
if (!resetFifo())
return false;
gyroBiasInit();
HAL_INFO1("%s init complete\n", IMUName());
return true;
}
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 RTIMUMPU9150::IMUInit()
{
unsigned char result;
unsigned char asa[3];
m_firstTime = true;
#ifdef MPU9150_CACHE_MODE
m_cacheIn = m_cacheOut = m_cacheCount = 0;
#endif
// set validity flags
m_imuData.fusionPoseValid = false;
m_imuData.fusionQPoseValid = false;
m_imuData.gyroValid = true;
m_imuData.accelValid = true;
m_imuData.compassValid = true;
m_imuData.pressureValid = false;
m_imuData.temperatureValid = false;
m_imuData.humidityValid = false;
// 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();
// enable the I2C bus
if (!m_settings->HALOpen())
return false;
// reset the MPU9150
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_PWR_MGMT_1, 0x80, "Failed to initiate MPU9150 reset"))
return false;
m_settings->delayMs(100);
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_PWR_MGMT_1, 0x00, "Failed to stop MPU9150 reset"))
return false;
if (!m_settings->HALRead(m_slaveAddr, MPU9150_WHO_AM_I, 1, &result, "Failed to read MPU9150 id"))
return false;
if (result != MPU9150_ID) {
HAL_ERROR1("Incorrect MPU9150 id %d\n", result);
return false;
}
// now configure the various components
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_LPF_CONFIG, m_lpf, "Failed to set lpf"))
return false;
if (!setSampleRate())
return false;
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_GYRO_CONFIG, m_gyroFsr, "Failed to set gyro fsr"))
return false;
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_ACCEL_CONFIG, m_accelFsr, "Failed to set accel fsr"))
return false;
// now configure compass
bypassOn();
// get fuse ROM data
if (!m_settings->HALWrite(AK8975_ADDRESS, AK8975_CNTL, 0, "Failed to set compass in power down mode 1")) {
bypassOff();
return false;
}
if (!m_settings->HALWrite(AK8975_ADDRESS, AK8975_CNTL, 0x0f, "Failed to set compass in fuse ROM mode")) {
bypassOff();
return false;
}
if (!m_settings->HALRead(AK8975_ADDRESS, AK8975_ASAX, 3, asa, "Failed to read compass fuse ROM")) {
bypassOff();
return false;
}
// 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 (!m_settings->HALWrite(AK8975_ADDRESS, AK8975_CNTL, 0, "Failed to set compass in power down mode 2")) {
bypassOff();
return false;
}
bypassOff();
// now set up MPU9150 to talk to the compass chip
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_MST_CTRL, 0x40, "Failed to set I2C master mode"))
return false;
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV0_ADDR, 0x80 | AK8975_ADDRESS, "Failed to set slave 0 address"))
return false;
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV0_REG, AK8975_ST1, "Failed to set slave 0 reg"))
return false;
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV0_CTRL, 0x88, "Failed to set slave 0 ctrl"))
return false;
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV1_ADDR, AK8975_ADDRESS, "Failed to set slave 1 address"))
return false;
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV1_REG, AK8975_CNTL, "Failed to set slave 1 reg"))
return false;
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV1_CTRL, 0x81, "Failed to set slave 1 ctrl"))
return false;
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV1_DO, 0x1, "Failed to set slave 1 DO"))
return false;
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_MST_DELAY_CTRL, 0x3, "Failed to set mst delay"))
return false;
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_YG_OFFS_TC, 0x80, "Failed to set yg offs tc"))
return false;
if (!setCompassRate())
return false;
// enable the sensors
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_PWR_MGMT_1, 1, "Failed to set pwr_mgmt_1"))
return false;
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_PWR_MGMT_2, 0, "Failed to set pwr_mgmt_2"))
return false;
// select the data to go into the FIFO and enable
if (!resetFifo())
return false;
gyroBiasInit();
HAL_INFO("MPU9150 init complete\n");
return true;
}
