//=============================================================
// 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;
}
//=============================================================
// 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;
}
//=============================================================
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;
}
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;
}