Пример #1
0
//=============================================================
// Synchronous READ SAMPLE API (visible externally)
//-------------------------------------------------------------
uint        MPUReadSample(uint MPUx, MPUData* pSample)
{
    if (!_MPU_Init)
        return MPU_NOTINIT;        // Not initialized...
    //---------------------------------------------------------
    MPU_CB*        pCB = MPUpCB(MPUx);
    if (NULL == pCB)    return MPU_NOTA;    // Should never happened!
    //------------------------------------------------------------------
    if (pCB->_MPU_Async)
        return MPU_ABSY;        // Asynchronous operation in progress...
    //*********************************************************
    uint            RC        = 0;
    _MPURawData     RawData;
    //-----------------------------------------
    // Read MPU measurement!
    //-----------------------------------------
    if ( (RC = _MPUReadRawData(pCB, &RawData)) )
        return RC;                            // Error...
    //-----------------------------------------------
    // Sample Timestamp
    //-----------------------------------------------
    pSample->TS     = TMRGetTS();
    //-----------------------------------------------
    // Temperature (C)
    //-----------------------------------------------
    pSample->Temp = (RawData.Temp - pCB->_MPU_Temp_OffsetTo0)
                    * pCB->_MPU_Temp_Sensitivity;
    //-----------------------------------------------
    // Acceleration (G)
    //-----------------------------------------------
    VectorSet    (
        RawData.AX * pCB->_MPU_Accl_Sensitivity,
        RawData.AY * pCB->_MPU_Accl_Sensitivity,
        RawData.AZ * pCB->_MPU_Accl_Sensitivity,
        //---------
        &pSample->A
    );
    //-----------------------------------------------
    // Gyroscopes (Rad/sec)
    //-----------------------------------------------
    VectorSet   (
        RawData.GX * pCB->_MPU_Gyro_Sensitivity,
        RawData.GY * pCB->_MPU_Gyro_Sensitivity,
        RawData.GZ * pCB->_MPU_Gyro_Sensitivity,
        &pSample->G
    );
    //-----------------------------------------------
    // Applying calibration and tuning parameters
    //-----------------------------------------------
    _MPU_ApplyCalibration(pCB, pSample);
    //-----------------------------------------------
    return    MPU_OK;
}
Пример #2
0
//=============================================================
// Synchronous READ SAMPLE API (visible externally)
//-------------------------------------------------------------
uint		MPUReadSample(MPUData* pSample)
	{
	if (!_MPU_Init)
		return MPU_NOTINIT;		// Not initialized...
	//-----------------------
	if (_MPU_Async)
		return MPU_ABSY;		// Asynchronous operation in progress...
	//*********************************************************
	uint			RC		= 0;
	_MPURawData		RawData;
	float			TDev;
	//-----------------------------------------
	// Read MPU measurement!
	//-----------------------------------------
	if ( (RC = _MPUReadRawData(&RawData)) )	
		return RC;							// Error...
	//-----------------------------------------------
	// Timestamp and Count
	//-----------------------------------------------
	pSample->TS 	= TMRGetTS();
	pSample->Count	= ++_MPU_Count;
	//-----------------------------------------------
	// Temperature (C) (will be used in subsequent
	// temperature compensation calculation)
	//-----------------------------------------------
	pSample->Temp = (RawData.Temp - _MPU_Temp_OffsetTo0) * _MPU_Temp_Sensitivity;
	//-----------------------------------------------
	// Acceleration (G)
	//-----------------------------------------------
	TDev	= pSample->Temp - _MPU_Accl_BaseTemp;
	//-----------------------------------------------
	VectorSet	(
				((float)RawData.AX - (_MPU_Accl_XOffset + _MPU_Accl_XSlope*TDev)) * _MPU_Accl_Sensitivity,
				((float)RawData.AY - (_MPU_Accl_YOffset + _MPU_Accl_YSlope*TDev)) * _MPU_Accl_Sensitivity,
				((float)RawData.AZ - (_MPU_Accl_ZOffset + _MPU_Accl_ZSlope*TDev)) * _MPU_Accl_Sensitivity,
				&pSample->A
				);
	//-----------------------------------------------
	// Gyroscopes (Rad/sec)
	//-----------------------------------------------
	TDev	= pSample->Temp - _MPU_Gyro_BaseTemp;
	//-----------------------------------------------
	VectorSet	(
				((float)RawData.GX - (_MPU_Gyro_XOffset + _MPU_Gyro_XSlope*TDev)) * _MPU_Gyro_Sensitivity,
				((float)RawData.GY - (_MPU_Gyro_YOffset + _MPU_Gyro_YSlope*TDev)) * _MPU_Gyro_Sensitivity,
				((float)RawData.GZ - (_MPU_Gyro_ZOffset + _MPU_Gyro_ZSlope*TDev)) * _MPU_Gyro_Sensitivity,
				&pSample->G
				);
	//-----------------------------------------------
	return	MPU_OK;
	}
Пример #3
0
//=============================================================
uint	_MPUCalibrateSync()
	{
	if (!_MPU_Init)
		return MPU_NOTINIT;		// Not initialized...
	//-----------------------
	if (_MPU_Async)
		return MPU_ABSY;		// Asynchronous operation in progress...

	//=========================================================
	// Local Variables
	//---------------------------------------------------------
	ulong Alarm	= TMRSetAlarm(2000);	// Set Alarm time 2 sec
										// into the future.
	//---------------------------------------------------------
	_MPURawData		RawData;
	//------------------------
	long			GX			= 0;
	long			GY			= 0;
	long			GZ			= 0;
	long			T			= 0;
	//------------------------
	long			SampleCount	= 0;
	//------------------------
	uint			RC			= MPU_OK;	// Pre-set to Success
	//=========================================================

	//=========================================================
	// Reset  Gyro offsets...
	//---------------------------------------------------------
	_MPU_Gyro_XOffset		= 0;
	_MPU_Gyro_YOffset		= 0;
	_MPU_Gyro_ZOffset		= 0;
	//---------------------------------------------------------
	// To collect averages we would like to "watch" MPU for at
	// least 2 seconds; number of samples will be variable
	// depending on the value of RateDiv
	//---------------------------------------------------------
	do
		{
		if ( (RC = _MPUReadRawData(&RawData)) )
			return RC;							// Error...
		//------------------
		GX += RawData.GX;
		GY += RawData.GY;
		GZ += RawData.GZ;
		T  += RawData.Temp;
		//------------------
		SampleCount++;
		}
	while ( FALSE == TMRTestAlarm(Alarm) );
	//---------------------------------------
	// Gyro offset is calculated in LSB units
	//---------------------------------------
	_MPU_Gyro_XOffset	= (float)GX/(float)SampleCount;
	_MPU_Gyro_YOffset	= (float)GY/(float)SampleCount;
	_MPU_Gyro_ZOffset	= (float)GZ/(float)SampleCount;
	//---------------------------------------
	// Base temperature converted to degrees C
	//---------------------------------------
	_MPU_Gyro_BaseTemp	= ((float)T/(float)SampleCount - _MPU_Temp_OffsetTo0)
						  * _MPU_Temp_Sensitivity;
	//*********************************************************
	return MPU_OK;
	}
Пример #4
0
int main(void)
	{
	//*******************************************************************
	Init();
	TMRInit(2);			// Initialize Timer interface with Priority=2
	BLIInit();			// Initialize Signal interface
	ADCInit(3);			// Initialize ADC to control battery
	I2CInit(5, 0);		// Initialize I2C1 module with IPL=5 and Fscl=400 KHz
	//--------------------------
	BLISignalON();
	//--------------------------
	if (MPUInit(3, 1))	// Initialize motion Sensor - 1 kHz/4 (250 Hz)
		DeadStop("A", 1);
	//--------------------------
	BLISignalOFF();
	//--------------------------
	UARTInitTX(6, 48);	// Initialize UART1 for TX on IPL=6 at 115200 bps
	// This initialization routine accepts BaudRate in multiples
	// of 2400 bps; Thus:
	// BaudRate =   1	=>   2400 bps
	// BaudRate =   2	=>   4800 bps
	// ...
	// BaudRate =  48	=> 115200 bps
	//------------------------------------------------------------
	// High speed
	//------------------------------------------------------------
	// BaudRate =  100	=>   250,000 bps
	// BaudRate =  200	=>   500,000 bps
	// BaudRate =  250	=>   625,000 bps
	// BaudRate =  350	=>   833,333 bps
	// BaudRate =  500	=> 1,250,000 bps
	// BaudRate = 1000	=> 2,500,000 bps
	//*******************************************************************
	uint			RC			= 0;
	//--------------------------
	_MPURawData	RawData;
	//--------------------------
	struct 
		{
		ulong	TS;		// Timestamp of the cycle
		//-----------------------------------------------
		// Temperature
		//-----------------------------------------------
		float			Temp;
		//-----------------------------------------------
		// Accelerometer
		//-----------------------------------------------
		float			AX;
		float			AY;
		float			AZ;
		//-----------------------------------------------
		// Gyroscopes
		//-----------------------------------------------
		float			GX;
		float			GY;
		float			GZ;
		//-----------------------------------------------
		} UData;
	//*******************************************************************
	long	AX, AY, AZ, GX, GY, GZ, Temp;
	//*******************************************************************
	while(1)
		{
		//------------------------	
		if (ADCGetBatteryStatus() < 30)
			DeadStop("SOS", 3);
		//------------------------
		AX= AY= AZ= GX= GY= GZ= Temp = 0;
		//------------------------
		int i;
		for (i=0; i<128; i++)	
			{
			RC 	= _MPUReadRawData(&RawData);
			if (RC) DeadStop("A", 1);
			//-----------------------------
			AX	+=	RawData.AX;
			AY	+=	RawData.AY;
			AZ	+=	RawData.AZ;
			//-----------------
			GX	+=	RawData.GX;
			GY	+=	RawData.GY;
			GZ	+=	RawData.GZ;
			//-----------------
			Temp+=	RawData.Temp;
			//-----------------------------
			}
		//---------------------------------------------	
		UData.TS	= 	TMRGetTS();
		//------------------------
		UData.Temp	=	((float)Temp/128.0 - _MPU_Temp_OffsetTo0) * _MPU_Temp_Sensitivity - 25.0;
		//------------------------
		UData.AX	= 	(float)AX/128.0;
		UData.AY	=	(float)AY/128.0;
		UData.AZ	=	(float)AZ/128.0;
		//------------------------
		UData.GX	= 	(float)GX/128.0;
		UData.GY	=	(float)GY/128.0;
		UData.GZ	=	(float)GZ/128.0;
		//---------------------------------------------	
		UARTPostWhenReady((uchar*)&UData, sizeof(UData));
		//---------------------------------------------	
		}
	return 1;
	}