//-----------------------------------------------------------------------------
BOOL bq27410_init()
{
BOOL rc = FALSE;
USHORT volt = 0;
if((hI2C = I2COpen(OMAP_DEVICE_I2C3)) == NULL)
{
RETAILMSG(1,(L"ERROR: bq27410_init Failed open I2C device driver\r\n"));
goto cleanUp;
}
if( I2CSetSlaveAddress(hI2C, BQ27410_SLAVE_ADDRESS) == FALSE )
RETAILMSG(1,(L"ERROR: bq27410_init - I2CSetSlaveAddress Failed\r\n"));
I2CSetSubAddressMode(hI2C, I2C_SUBADDRESS_MODE_8);
I2CSetBaudIndex(hI2C, FULLSPEED_MODE);
if( BQ27410_WriteReg(bq27410CMD_CNTL_LSB, 0x0C) == FALSE ) // send battery insert
RETAILMSG(1,(L"ERROR: BatteryPDDInitialize: Battery insert Failed\r\n"));
if( BQ27410_ReadReg(bq27410CMD_VOLT_LSB, &volt) )
RETAILMSG(1,(L"bq27410_init: volt = %d\r\n", volt));
I2CClose(hI2C);
rc = TRUE;
cleanUp:
return rc;
}
//------------------------------------------------------------------------------
//
// Function: I2C_Open
//
// Called by device manager to open a device for reading and/or writing.
//
DWORD
I2C_Open(
DWORD context,
DWORD accessCode,
DWORD shareMode
)
{
DWORD dw = 0;
Instance_t *pInstance = NULL;
Device_t *pDevice = (Device_t*)context;
UNREFERENCED_PARAMETER(accessCode);
UNREFERENCED_PARAMETER(shareMode);
RETAILMSG(ZONE_FUNCTION, (L"+I2C_Open(0x%08x, 0x%08x, 0x%08x)\r\n",
context, accessCode, shareMode
));
if ((pDevice == NULL) || (pDevice->cookie != I2C_DEVICE_COOKIE))
{
RETAILMSG (ZONE_ERROR, (L"TLD: !ERROR(I2C_Open) - "
L"Incorrect context parameter\r\n"
));
goto cleanUp;
}
// Create device structure
pInstance = (Instance_t*)LocalAlloc(LPTR, sizeof(Instance_t));
if (pInstance == NULL)
{
DEBUGMSG(ZONE_ERROR, (L"ERROR: I2C_Open: "
L"Failed allocate I2C instance structure\r\n"
));
goto cleanUp;
}
// initialize instance
memset(pInstance, 0, sizeof(Instance_t));
pInstance->cookie = I2C_INSTANCE_COOKIE;
pInstance->pDevice = pDevice;
pInstance->hI2C = I2COpen(pDevice->devId);
dw = (DWORD)pInstance;
cleanUp:
RETAILMSG(ZONE_FUNCTION, (L"-I2C_Open(0x%08x, 0x%08x, 0x%08x) == %d\r\n",
context, accessCode, shareMode, dw
));
return dw;
}
bool RTIMUSettings::discoverIMU(int& imuType, unsigned char& slaveAddress)
{
unsigned char result;
unsigned char altResult;
setI2CBus(m_I2CBus);
if (!I2COpen()) {
HAL_ERROR1("Failed to open I2C bus %d\n", m_I2CBus);
return false;
}
if (I2CRead(MPU6050_ADDRESS0, MPU6050_WHO_AM_I, 1, &result, "")) {
if (result == MPU6050_ID) {
imuType = RTIMU_TYPE_MPU6050;
slaveAddress = MPU6050_ADDRESS0;
I2CClose();
HAL_INFO("Detected MPU6050 at standard address\n");
return true;
}
}
if (I2CRead(MPU6050_ADDRESS1, MPU6050_WHO_AM_I, 1, &result, "")) {
if (result == MPU6050_ID) {
imuType = RTIMU_TYPE_MPU6050;
slaveAddress = MPU6050_ADDRESS1;
I2CClose();
HAL_INFO("Detected MPU6050 at option address\n");
return true;
}
}
I2CClose();
HAL_ERROR("No IMU detected\n");
return false;
}
//------------------------------------------------------------------------------
//
// Functions: TWLxxx
//
HANDLE
TWLOpen(
)
{
static Device_t Device = {NULL,0,FALSE,0};
// Open i2c bus
if (Device.refCount == 0)
{
Device.hI2C = I2COpen(BSPGetTritonBusID());
if (Device.hI2C == NULL)
{
DEBUGMSG(ZONE_ERROR, (L"ERROR: TWLOpen: "
L"Failed open I2C bus driver\r\n"
));
return NULL;
}
Device.slaveAddress = BSPGetTritonSlaveAddress();
I2CSetSlaveAddress(Device.hI2C, Device.slaveAddress);
}
Device.refCount++;
return &Device;
}
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;
}
bool RTIMUSettings::discoverIMU(int& imuType, unsigned char& slaveAddress)
{
unsigned char result;
unsigned char altResult;
setI2CBus(m_I2CBus);
if (!I2COpen()) {
HAL_ERROR1("Failed to open I2C bus %d\n", m_I2CBus);
return false;
}
if (I2CRead(MPU9150_ADDRESS0, MPU9150_WHO_AM_I, 1, &result, "")) {
if (result == MPU9150_ID) {
imuType = RTIMU_TYPE_MPU9150;
slaveAddress = MPU9150_ADDRESS0;
I2CClose();
HAL_INFO("Detected MPU9150 at standard address\n");
return true;
}
}
if (I2CRead(MPU9150_ADDRESS1, MPU9150_WHO_AM_I, 1, &result, "")) {
if (result == MPU9150_ID) {
imuType = RTIMU_TYPE_MPU9150;
slaveAddress = MPU9150_ADDRESS1;
I2CClose();
HAL_INFO("Detected MPU9150 at option address\n");
return true;
}
}
if (I2CRead(L3GD20H_ADDRESS0, L3GD20H_WHO_AM_I, 1, &result, "")) {
if (result == L3GD20H_ID) {
imuType = RTIMU_TYPE_GD20HM303D;
slaveAddress = L3GD20H_ADDRESS0;
I2CClose();
HAL_INFO("Detected L3GD20H at standard address\n");
return true;
} else if (result == LSM9DS0_GYRO_ID) {
if (I2CRead(LSM9DS0_ACCELMAG_ADDRESS0, LSM9DS0_WHO_AM_I, 1, &altResult, "")) {
if (altResult == LSM9DS0_ACCELMAG_ID) {
imuType = RTIMU_TYPE_LSM9DS0;
slaveAddress = LSM9DS0_GYRO_ADDRESS0;
I2CClose();
HAL_INFO("Detected LSM9DS0 at standard address\n");
return true;
}
}
}
}
if (I2CRead(L3GD20H_ADDRESS1, L3GD20H_WHO_AM_I, 1, &result, "")) {
if (result == L3GD20H_ID) {
imuType = RTIMU_TYPE_GD20HM303D;
slaveAddress = L3GD20H_ADDRESS1;
I2CClose();
HAL_INFO("Detected L3GD20H at option address\n");
return true;
} else if (result == LSM9DS0_GYRO_ID) {
if (I2CRead(LSM9DS0_ACCELMAG_ADDRESS1, LSM9DS0_WHO_AM_I, 1, &altResult, "")) {
if (altResult == LSM9DS0_ACCELMAG_ID) {
imuType = RTIMU_TYPE_LSM9DS0;
slaveAddress = LSM9DS0_GYRO_ADDRESS1;
I2CClose();
HAL_INFO("Detected LSM9DS0 at standard address\n");
return true;
}
}
}
}
if (I2CRead(L3GD20_ADDRESS0, L3GD20_WHO_AM_I, 1, &result, "")) {
if (result == L3GD20_ID) {
imuType = RTIMU_TYPE_GD20M303DLHC;
slaveAddress = L3GD20_ADDRESS0;
I2CClose();
HAL_INFO("Detected L3GD20 at standard address\n");
return true;
}
}
if (I2CRead(L3GD20_ADDRESS1, L3GD20_WHO_AM_I, 1, &result, "")) {
if (result == L3GD20_ID) {
imuType = RTIMU_TYPE_GD20M303DLHC;
slaveAddress = L3GD20_ADDRESS1;
I2CClose();
HAL_INFO("Detected L3GD20 at option address\n");
return true;
}
}
I2CClose();
HAL_ERROR("No IMU detected\n");
return false;
}
