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 RTIMUMPU9255::compassSetup() { unsigned char asa[3]; if (m_settings->m_busIsI2C) { // I2C mode bypassOn(); // get fuse ROM data if (!m_settings->HALWrite(AK8963_ADDRESS, AK8963_CNTL, 0, "Failed to set compass in power down mode 1")) { bypassOff(); return false; } if (!m_settings->HALWrite(AK8963_ADDRESS, AK8963_CNTL, 0x0f, "Failed to set compass in fuse ROM mode")) { bypassOff(); return false; } if (!m_settings->HALRead(AK8963_ADDRESS, AK8963_ASAX, 3, asa, "Failed to read compass fuse ROM")) { bypassOff(); return false; } if (!m_settings->HALWrite(AK8963_ADDRESS, AK8963_CNTL, 0, "Failed to set compass in power down mode 2")) { bypassOff(); return false; } bypassOff(); } else { // SPI mode bypassOff(); if (!m_settings->HALWrite(m_slaveAddr, MPU9255_I2C_MST_CTRL, 0x40, "Failed to set I2C master mode")) return false; if (!m_settings->HALWrite(m_slaveAddr, MPU9255_I2C_SLV0_ADDR, 0x80 | AK8963_ADDRESS, "Failed to set slave 0 address")) return false; if (!m_settings->HALWrite(m_slaveAddr, MPU9255_I2C_SLV0_REG, AK8963_ASAX, "Failed to set slave 0 reg")) return false; if (!m_settings->HALWrite(m_slaveAddr, MPU9255_I2C_SLV0_CTRL, 0x83, "Failed to set slave 0 ctrl")) return false; if (!m_settings->HALWrite(m_slaveAddr, MPU9255_I2C_SLV1_ADDR, AK8963_ADDRESS, "Failed to set slave 1 address")) return false; if (!m_settings->HALWrite(m_slaveAddr, MPU9255_I2C_SLV1_REG, AK8963_CNTL, "Failed to set slave 1 reg")) return false; if (!m_settings->HALWrite(m_slaveAddr, MPU9255_I2C_SLV1_CTRL, 0x81, "Failed to set slave 1 ctrl")) return false; if (!m_settings->HALWrite(m_slaveAddr, MPU9255_I2C_SLV1_DO, 0x00, "Failed to set compass in power down mode 2")) return false; m_settings->delayMs(10); if (!m_settings->HALWrite(m_slaveAddr, MPU9255_I2C_SLV1_DO, 0x0f, "Failed to set compass in fuse mode")) return false; if (!m_settings->HALRead(m_slaveAddr, MPU9255_EXT_SENS_DATA_00, 3, asa, "Failed to read compass rom")) return false; if (!m_settings->HALWrite(m_slaveAddr, MPU9255_I2C_SLV1_DO, 0x0, "Failed to set compass in power down mode 2")) return false; } // both interfaces // 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(m_slaveAddr, MPU9255_I2C_MST_CTRL, 0x40, "Failed to set I2C master mode")) return false; if (!m_settings->HALWrite(m_slaveAddr, MPU9255_I2C_SLV0_ADDR, 0x80 | AK8963_ADDRESS, "Failed to set slave 0 address")) return false; if (!m_settings->HALWrite(m_slaveAddr, MPU9255_I2C_SLV0_REG, AK8963_ST1, "Failed to set slave 0 reg")) return false; if (!m_settings->HALWrite(m_slaveAddr, MPU9255_I2C_SLV0_CTRL, 0x88, "Failed to set slave 0 ctrl")) return false; if (!m_settings->HALWrite(m_slaveAddr, MPU9255_I2C_SLV1_ADDR, AK8963_ADDRESS, "Failed to set slave 1 address")) return false; if (!m_settings->HALWrite(m_slaveAddr, MPU9255_I2C_SLV1_REG, AK8963_CNTL, "Failed to set slave 1 reg")) return false; if (!m_settings->HALWrite(m_slaveAddr, MPU9255_I2C_SLV1_CTRL, 0x81, "Failed to set slave 1 ctrl")) return false; if (!m_settings->HALWrite(m_slaveAddr, MPU9255_I2C_SLV1_DO, 0x1, "Failed to set slave 1 DO")) return false; if (!m_settings->HALWrite(m_slaveAddr, MPU9255_I2C_MST_DELAY_CTRL, 0x3, "Failed to set mst delay")) return false; 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; }