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 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 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;
}
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;
}
