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 RTIMULSM9DS0::IMUInit() { unsigned char result; #ifdef LSM9DS0_CACHE_MODE m_firstTime = true; 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_gyroSlaveAddr = m_settings->m_I2CSlaveAddress; if (m_gyroSlaveAddr == LSM9DS0_GYRO_ADDRESS0) m_accelCompassSlaveAddr = LSM9DS0_ACCELMAG_ADDRESS0; else m_accelCompassSlaveAddr = LSM9DS0_ACCELMAG_ADDRESS1; m_bus = m_settings->m_I2CBus; setCalibrationData(m_settings->m_compassCalValid, m_settings->m_compassCalMin, m_settings->m_compassCalMax); // enable the I2C bus setI2CBus(m_bus); if (!I2COpen()) return false; // Set up the gyro if (!I2CWrite(m_gyroSlaveAddr, LSM9DS0_GYRO_CTRL5, 0x80, "Failed to boot LSM9DS0")) return false; if (!I2CRead(m_gyroSlaveAddr, LSM9DS0_GYRO_WHO_AM_I, 1, &result, "Failed to read LSM9DS0 gyro id")) return false; if (result != LSM9DS0_GYRO_ID) { HAL_ERROR1("Incorrect LSM9DS0 gyro id %d\n", result); return false; } if (!setGyroSampleRate()) return false; if (!setGyroCTRL2()) return false; if (!setGyroCTRL4()) return false; // Set up the accel if (!I2CRead(m_accelCompassSlaveAddr, LSM9DS0_WHO_AM_I, 1, &result, "Failed to read LSM9DS0 accel/mag id")) return false; if (result != LSM9DS0_ACCELMAG_ID) { HAL_ERROR1("Incorrect LSM9DS0 accel/mag id %d\n", result); return false; } if (!setAccelCTRL1()) return false; if (!setAccelCTRL2()) return false; if (!setCompassCTRL5()) return false; if (!setCompassCTRL6()) return false; if (!setCompassCTRL7()) return false; #ifdef LSM9DS0_CACHE_MODE // turn on gyro fifo if (!I2CWrite(m_gyroSlaveAddr, LSM9DS0_GYRO_FIFO_CTRL, 0x3f, "Failed to set LSM9DS0 FIFO mode")) return false; #endif if (!setGyroCTRL5()) return false; gyroBiasInit(); HAL_INFO("LSM9DS0 init complete\n"); 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 RTIMUGD20HM303DLHC::IMUInit() { unsigned char result; #ifdef GD20HM303DLHC_CACHE_MODE m_firstTime = true; 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_gyroSlaveAddr = m_settings->m_I2CSlaveAddress; m_accelSlaveAddr = LSM303DLHC_ACCEL_ADDRESS; m_compassSlaveAddr = LSM303DLHC_COMPASS_ADDRESS; setCalibrationData(); // enable the I2C bus if (!m_settings->HALOpen()) return false; // Set up the gyro if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_LOW_ODR, 0x04, "Failed to reset L3GD20H")) return false; if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_CTRL5, 0x80, "Failed to boot L3GD20H")) return false; if (!m_settings->HALRead(m_gyroSlaveAddr, L3GD20H_WHO_AM_I, 1, &result, "Failed to read L3GD20H id")) return false; if (result != L3GD20H_ID) { HAL_ERROR1("Incorrect L3GD20H id %d\n", result); return false; } if (!setGyroSampleRate()) return false; if (!setGyroCTRL2()) return false; if (!setGyroCTRL4()) return false; // Set up the accel if (!setAccelCTRL1()) return false; if (!setAccelCTRL4()) return false; // Set up the compass if (!setCompassCRA()) return false; if (!setCompassCRB()) return false; if (!setCompassCRM()) return false; #ifdef GD20HM303DLHC_CACHE_MODE // turn on gyro fifo if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20_FIFO_CTRL, 0x3f, "Failed to set L3GD20 FIFO mode")) return false; #endif if (!setGyroCTRL5()) return false; gyroBiasInit(); HAL_INFO("GD20HM303DLHC init complete\n"); return true; }
int RTIMUGD20HM303DLHC::IMUInit() { unsigned char result; // configure IMU m_gyroSlaveAddr = m_settings->m_I2CSlaveAddress; m_accelSlaveAddr = LSM303DLHC_ACCEL_ADDRESS; m_compassSlaveAddr = LSM303DLHC_COMPASS_ADDRESS; setCalibrationData(); // Set up the gyro if (!I2CWrite(m_gyroSlaveAddr, L3GD20H_LOW_ODR, 0x04)) return -1; if (!I2CWrite(m_gyroSlaveAddr, L3GD20H_CTRL5, 0x80)) return -2; if (!I2CRead(m_gyroSlaveAddr, L3GD20H_WHO_AM_I, 1, &result)) return -3; if (result != L3GD20H_ID) { return -4; } if (!setGyroSampleRate()) return -5; if (!setGyroCTRL2()) return -6; if (!setGyroCTRL4()) return -7; // Set up the accel if (!setAccelCTRL1()) return -8; if (!setAccelCTRL4()) return -9; // Set up the compass if (!setCompassCRA()) return -10; if (!setCompassCRB()) return -11; if (!setCompassCRM()) return -12; if (!setGyroCTRL5()) return -13; gyroBiasInit(); 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; }
bool RTIMUBMX055::IMUInit() { unsigned char result; m_firstTime = true; // 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_gyroSlaveAddr = m_settings->m_I2CSlaveAddress; if (!m_settings->HALRead(m_gyroSlaveAddr, BMX055_GYRO_WHO_AM_I, 1, &result, "Failed to read BMX055 gyro id")) return false; if (result != BMX055_GYRO_ID) { HAL_ERROR1("Incorrect BMX055 id %d\n", result); return false; } // work out accel address if (m_settings->HALRead(BMX055_ACCEL_ADDRESS0, BMX055_ACCEL_WHO_AM_I, 1, &result, "")) { if (result == BMX055_ACCEL_ID) { m_accelSlaveAddr = BMX055_ACCEL_ADDRESS0; } else { m_accelSlaveAddr = BMX055_ACCEL_ADDRESS1; } } // work out mag address int magAddr; for (magAddr = BMX055_MAG_ADDRESS0; magAddr <= BMX055_MAG_ADDRESS3; magAddr++) { // have to enable chip to get id... m_settings->HALWrite(magAddr, BMX055_MAG_POWER, 1, ""); m_settings->delayMs(50); if (m_settings->HALRead(magAddr, BMX055_MAG_WHO_AM_I, 1, &result, "")) { if (result == BMX055_MAG_ID) { m_magSlaveAddr = magAddr; break; } } } if (magAddr > BMX055_MAG_ADDRESS3) { HAL_ERROR("Failed to find BMX055 mag\n"); return false; } setCalibrationData(); // enable the I2C bus if (!m_settings->HALOpen()) return false; // Set up the gyro if (!m_settings->HALWrite(m_gyroSlaveAddr, BMX055_GYRO_FIFO_CONFIG_1, 0x40, "Failed to set BMX055 FIFO config")) return false; if (!setGyroSampleRate()) return false; if (!setGyroFSR()) return false; gyroBiasInit(); // set up the accel if (!setAccelSampleRate()) return false; if (!setAccelFSR()) return false; // set up the mag magInitTrimRegisters(); setMagPreset(); HAL_INFO("BMX055 init complete\n"); return true; }
bool RTIMULSM9DS1::IMUInit() { unsigned char result; // 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_accelGyroSlaveAddr = m_settings->m_I2CSlaveAddress; // work outmag address if (m_settings->HALRead(LSM9DS1_MAG_ADDRESS0, LSM9DS1_MAG_WHO_AM_I, 1, &result, "")) { if (result == LSM9DS1_MAG_ID) { m_magSlaveAddr = LSM9DS1_MAG_ADDRESS0; } } else if (m_settings->HALRead(LSM9DS1_MAG_ADDRESS1, LSM9DS1_MAG_WHO_AM_I, 1, &result, "")) { if (result == LSM9DS1_MAG_ID) { m_magSlaveAddr = LSM9DS1_MAG_ADDRESS1; } } else if (m_settings->HALRead(LSM9DS1_MAG_ADDRESS2, LSM9DS1_MAG_WHO_AM_I, 1, &result, "")) { if (result == LSM9DS1_MAG_ID) { m_magSlaveAddr = LSM9DS1_MAG_ADDRESS2; } } else if (m_settings->HALRead(LSM9DS1_MAG_ADDRESS3, LSM9DS1_MAG_WHO_AM_I, 1, &result, "")) { if (result == LSM9DS1_MAG_ID) { m_magSlaveAddr = LSM9DS1_MAG_ADDRESS3; } } setCalibrationData(); // enable the I2C bus if (!m_settings->HALOpen()) return false; // Set up the gyro/accel if (!m_settings->HALWrite(m_accelGyroSlaveAddr, LSM9DS1_CTRL8, 0x80, "Failed to boot LSM9DS1")) return false; m_settings->delayMs(100); if (!m_settings->HALRead(m_accelGyroSlaveAddr, LSM9DS1_WHO_AM_I, 1, &result, "Failed to read LSM9DS1 accel/gyro id")) return false; if (result != LSM9DS1_ID) { HAL_ERROR1("Incorrect LSM9DS1 gyro id %d\n", result); return false; } if (!setGyroSampleRate()) return false; if (!setGyroCTRL3()) return false; // Set up the mag if (!m_settings->HALRead(m_magSlaveAddr, LSM9DS1_MAG_WHO_AM_I, 1, &result, "Failed to read LSM9DS1 accel/mag id")) return false; if (result != LSM9DS1_MAG_ID) { HAL_ERROR1("Incorrect LSM9DS1 accel/mag id %d\n", result); return false; } if (!setAccelCTRL6()) return false; if (!setAccelCTRL7()) return false; if (!setCompassCTRL1()) return false; if (!setCompassCTRL2()) return false; if (!setCompassCTRL3()) return false; gyroBiasInit(); HAL_INFO("LSM9DS1 init complete\n"); return true; }
int RTIMUGD20HM303D::IMUInit() { unsigned char result; // configure IMU m_gyroSlaveAddr = m_settings->m_I2CSlaveAddress; if (m_gyroSlaveAddr == L3GD20H_ADDRESS0) m_accelCompassSlaveAddr = LSM303D_ADDRESS0; else m_accelCompassSlaveAddr = LSM303D_ADDRESS1; setCalibrationData(); // Set up the gyro if (!I2CWrite(m_gyroSlaveAddr, L3GD20H_LOW_ODR, 0x04)) return -1; if (!I2CWrite(m_gyroSlaveAddr, L3GD20H_CTRL5, 0x80)) return -2; if (!I2CRead(m_gyroSlaveAddr, L3GD20H_WHO_AM_I, 1, &result)) return -3; if (result != L3GD20H_ID) { return -4; } if (!setGyroSampleRate()) return -5; if (!setGyroCTRL2()) return -6; if (!setGyroCTRL4()) return -7; // Set up the accel/compass if (!I2CRead(m_accelCompassSlaveAddr, LSM303D_WHO_AM_I, 1, &result)) return -8; if (result != LSM303D_ID) { return -9; } if (!setAccelCTRL1()) return -10; if (!setAccelCTRL2()) return -11; if (!setCompassCTRL5()) return -12; if (!setCompassCTRL6()) return -13; if (!setCompassCTRL7()) return -14; if (!setGyroCTRL5()) return -16; gyroBiasInit(); return true; }
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; }