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