Example #1
0
//=============================================================
// 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;
}
Example #2
0
//=============================================================
// 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);
}
Example #3
0
//=============================================================
// 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;
	}