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