//=============================================================
// Synchronous READ of the raw sensor data
//-------------------------------------------------------------
uint _MPUReadRawData(MPU_CB* pCB, _MPURawData* pData)
{
int RC = 0;
byte Status = 0;
ulong Alarm;
//-----------------------------------
if ( (RC = _MPURead(pCB, MPU6050_INT_STATUS, &Status, 1)) )
return RC; // Error...
//-----------------------------------
Alarm = TMRSetAlarm(50); // Expecting to get sample within 50 msec
//-----------------------------------
while ( (Status & 0x01) != 0x01 )
{
TMRDelayTicks(1); // Short delay...
if ( (RC = _MPURead(pCB, MPU6050_INT_STATUS, &Status, 1)) )
return RC; // Error...
if (TMRCheckAlarm(Alarm))
return MPU_TMOUT;
}
//-----------------------------------------
// MPU has a measurement!
//-----------------------------------------
byte Data[14];
// Read measurement
if ( (RC = _MPURead(pCB, MPU6050_DATA_START, Data, 14) ) )
return RC; // Error...
//===============================================
// Process sensor data
_MPU_ConvertReadBuffer(Data, pData);
//===============================================
return MPU_OK;
}
//=============================================================
// Byte-wise Synchronous Get/Put functions
//=============================================================
uint MPUGetByte(uint MPUx, byte Address, byte* Value)
{
//---------------------------------------------------------
MPU_CB* pCB = MPUpCB(MPUx);
if (NULL == pCB) return MPU_NOTA; // Should never happened!
//------------------------------------------------------------------
return _MPURead(pCB, Address, Value, 1);
}
//=============================================================
// Synchronous READ of the raw sensor data
//-------------------------------------------------------------
uint _MPUReadRawData(_pMPURawData pData)
{
int RC = 0;
byte Status = 0;
int i;
//-----------------------------------------
// "Unpack" data
//-----------------------------------------
union
{
int VInt;
byte VByte[2];
} U;
//-----------------------------------
Retry: // Wait for RDY signal
if ( (RC = MPUGetINT(&Status)) )
return RC; // Error...
if ( (Status & 0x01) != 0x01 )
{
for (i = 0; i < 1000; i++); // Short delay...
goto Retry;
}
//-----------------------------------------
// MPU has a measurement!
//-----------------------------------------
byte Data[14];
// Read measurement
if ( (RC = _MPURead(0x3B, Data, 14) ) )
return RC; // Error...
//===============================================
// Due to orientation of the sensor on the board
// in reference to orientation of the board in
// the vehicle, direction of the axis need to change
// to align right-handed coordinate system with
// the vehicle
//===============================================
// Accelerometer:
//-----------------------------------------------
// Yveh = -Xa
//-----------------------------------------------
U.VByte[1] = Data[0];
U.VByte[0] = Data[1];
pData->AY = -U.VInt;
//-----------------------------------------------
// Xveh = -Ya
//-----------------------------------------------
U.VByte[1] = Data[2];
U.VByte[0] = Data[3];
pData->AX = -U.VInt;
//-----------------------------------------------
// Zveh = -Za
//-----------------------------------------------
U.VByte[1] = Data[4];
U.VByte[0] = Data[5];
pData->AZ = -U.VInt;
//-----------------------------------------------
// Temperature
//-----------------------------------------------
U.VByte[1] = Data[6];
U.VByte[0] = Data[7];
pData->Temp = U.VInt;
//-----------------------------------------------
// Gyroscopes
//-----------------------------------------------
// Yveh = -Xa
//-----------------------------------------------
U.VByte[1] = Data[8];
U.VByte[0] = Data[9];
pData->GY = -U.VInt;
//-----------------------------------------------
// Xveh = -Ya
//-----------------------------------------------
U.VByte[1] = Data[10];
U.VByte[0] = Data[11];
pData->GX = -U.VInt;
//-----------------------------------------------
// Zveh = -Za
//-----------------------------------------------
U.VByte[1] = Data[12];
U.VByte[0] = Data[13];
pData->GZ = -U.VInt;
//-----------------------------------------------
return MPU_OK;
}