//------------------------------------------------------------------------------
//
// Function: I2C_Close
//
// This function closes the device context.
//
BOOL
I2C_Close(
DWORD context
)
{
RETAILMSG(ZONE_FUNCTION, (L"+I2C_Close(0x%08x)\r\n",
context
));
BOOL rc = FALSE;
Instance_t *pInstance = (Instance_t*)context;
if ((pInstance == NULL) || (pInstance->cookie != I2C_INSTANCE_COOKIE))
{
RETAILMSG (ZONE_ERROR, (L"TLD: !ERROR(I2C_Close) - "
L"Incorrect context parameter\r\n"
));
goto cleanUp;
}
I2CClose(pInstance->hI2C);
LocalFree(pInstance);
rc = TRUE;
cleanUp:
RETAILMSG(ZONE_FUNCTION, (L"-I2C_Close(0x%08x)=%d\r\n",
context, rc
));
return rc;
}
//-----------------------------------------------------------------------------
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;
}
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;
}
VOID
TWLClose(
HANDLE hContext
)
{
Device_t * pDevice = (Device_t *) hContext;
pDevice->refCount--;
if (pDevice->refCount == 0)
{
if (pDevice->hI2C)
{
I2CClose(pDevice->hI2C);
pDevice->hI2C = 0;
}
}
}
void RTIMUHal::HALClose()
{
I2CClose();
SPIClose();
}
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;
}
