int main(void)
{
//*******************************************************************
Init();
TMRInit(2); // Initialize Timer interface with Priority=2
//--------------------------
BLIInit(); // Initialize Signal interface
I2CInit(5, 1); // Initialize I2C1 module with IPL=5 and Fscl=400 KHz
//--------------------------
UARTInitTX(6, 350); // Initialize UART1 for TX on IPL=6 at
// BaudRate = 48 => 115,200 bps - ZigBEE
//--------------------------------------
// BaudRate = 100 => 250,000 bps
// BaudRate = 200 => 500,000 bps
// BaudRate = 250 => 625,000 bps
// BaudRate = 350 => 833,333 bps - SD Logger, FTDI cable
// BaudRate = 500 => 1,250,000 bps
// BaudRate = 1000 => 2,500,000 bps
//*******************************************************************
if ( MPUInit(0, 3) ) // Initialize motion Sensor
// 1 kHz/(0+1) = 1000 Hz (1ms)
// DLPF=3 => Bandwidth 44 Hz (delay: 4.9 msec)
BLIDeadStop("EG", 2);
//--------------------------
if (HMCInit(6, 1, 0)) // Initialize magnetic Sensor
// ODR = 6 (max, 75 Hz),
// Gain = 1 (1.3 Gs)
// DLPF = 0 (no averaging)
BLIDeadStop("EM", 2);
//--------------------------
if ( MPLInit(5) ) // Average over 32 samples providing
// update rate about 10 Hz
BLIDeadStop("EA", 2);
//*******************************************************************
uint RC = 0;
ulong Alarm = 0;
ulong TS = 0;
//-------------------------------
struct
{
ulong TS;
MPUSample IMUData;
HMCSample MagData;
MPLSample AltData;
} UData;
//*******************************************************************
BLIAsyncStart(100, 50);
RC = MPLSetGround();
if (RC) BLIDeadStop("SOS", 3); // Failure...
BLIAsyncStop();
//====================================================
BLISignalOFF();
//====================================================
// Testing MPU, HMC, and MPL together in a real-life
// scenario
//====================================================
if(MPUAsyncStart()) BLIDeadStop("SG", 2);
//------------------------
if(HMCAsyncStart()) BLIDeadStop("SM", 2);
//------------------------
if(MPLAsyncStart()) BLIDeadStop("SA", 2);
RC = MPLAsyncReadWhenReady(&UData.AltData);
if (RC) BLIDeadStop("SAS", 3); // Failure...
//====================================================
while (TRUE)
{
Alarm = TMRSetAlarm(500);
//------------------------------------
RC = MPUAsyncReadIfReady(&UData.IMUData);
if (MPU_OK != RC && MPU_NRDY != RC)
BLIDeadStop("G", 1);
//------------------------
RC = HMCAsyncReadIfReady(&UData.MagData);
if (HMC_OK != RC && HMC_NRDY != RC)
BLIDeadStop("M", 1);
//------------------------
RC = MPLAsyncReadIfReady(&UData.AltData);
if (MPL_OK != RC && MPL_NRDY != RC)
BLIDeadStop("A", 3); // Failure...
//-------------------------
UData.TS = TMRGetTS();
//-------------------------
if (0 == TS) TS = UData.TS;
UData.TS -= TS;
BLISignalFlip();
//-------------------------
UARTPostIfReady((byte*)&UData, sizeof(UData));
//-------------------------
TMRWaitAlarm(Alarm);
}
//====================================================
return 1;
}
int main(void)
{
//*******************************************************************
Init(); // Initialize microprocessor
TMRInit(2); // Initialize Timer interface with Priority=2
BLIInit(); // Initialize Signal interface
//*******************************************************************
// Switch 1 controls the Serial Data Logger (SDL) communication speed
//-------------------------------------------------------------------
if (_SW1)
// Switch 1 is ON - Configuring SDL for PIC-to-PIC
// high-speed communication at 1 MBaud
SDLInit(3, BAUD_1M);
else
// Switch 1 is OFF - Configuring SDL for ZigBEE
// wireless communication at 115.2 KBaud
SDLInit(3, BAUD_115200);
//*******************************************************************
// Initialize I2C Library
//-------------------------------------------------------------------
I2CInit(5, 2); // First param: IL = 5 (interrupt request priority
// Second param: I2C speed
// 0 - lowest (123 kHz at Fcy = 64MHz)
// 1 - 200 kHz
// 2 - 400 kHz
// 3 - 1 MHz
//*******************************************************************
uint RC = 0;
ulong Alarm = 0;
//==================================================================
// Initialize MPUs
//------------------------------------------------------------------
RC = MPUInit(0, 3, MPU_GYRO_2000ds, MPU_ACC_2g);
// Initialize motion Sensor
// 1 kHz/(0+1) = 1000 Hz (1msec)
// DLPF = 3 => Bandwidth 44 Hz (delay: 4.9 msec)
if (RC) BLIDeadStop("EG", 2);
//==================================================================
// Initialize MPL3115 Altimeter
//------------------------------------------------------------------
// OSR = 0 => No averaging ( 2^0= 1), update rate about 166.6 Hz
// OSR = 1 => Average 2^1= 2 samples, update rate about 111.1 Hz
// OSR = 2 => Average 2^2= 4 samples, update rate about 67.8 Hz
// OSR = 3 => Average 2^3= 8 samples, update rate about 37.7 Hz
// OSR = 4 => Average 2^4= 16 samples, update rate about 20.1 Hz
// OSR = 5 => Average 2^5= 32 samples, update rate about 10.4 Hz
// OSR = 6 => Average 2^6= 64 samples, update rate about 5.3 Hz
// OSR = 7 => Average 2^7= 128 samples, update rate about 2.7 Hz
//------------------------------------------------------------------
byte OSR = 3;
//------------------------------------------------------------------
if ( MPL_OK != MPLInit (OSR) )
BLIDeadStop ("EB", 2);
//==================================================================
// Initialize Asynchronous mode
//-----------------------------------------------------
if ( (RC = MPUAsyncStart(1)) )
BLIDeadStop("S1", 2);
//------------------------------
if ( (RC = MPUAsyncStart(2)) )
BLIDeadStop("S2", 2);
//-----------------------------------------------------
if ( (RC = MPLAsyncStart()) )
BLIDeadStop("S3", 2);
//==================================================================
// Provide a few second delay prior to calibrating Gyros to make
// sure that the board is stable after the "turn-on" shake
//------------------------------------------------------------------
BLIAsyncStart(50,50);
TMRDelay(5000);
BLIAsyncStop();
//==================================================================
// Calibrate Gyros
//------------------------------------------------------------------
BLIAsyncStart(100,100);
//------------------------------
if ( (RC = MPUCalibrateGyro(1)) )
BLIDeadStop("C1", 2);
//------------------------------
if ( (RC = MPUCalibrateGyro(2)) )
BLIDeadStop("C2", 2);
//------------------------------
BLIAsyncStop();
//==================================================================
//==================================================================
struct
{
MPUData MPUSample1;
MPUData MPUSample2;
MPLData MPLSample;
} SensorData;
//==================================================================
// Main Loop
//------------------------------------------------------------------
BLISignalOFF();
while (TRUE)
{
Alarm = TMRSetAlarm(1000);
//-----------------------------------------------------
if ( (RC = MPUAsyncReadWhenReady(1, &SensorData.MPUSample1)) )
BLIDeadStop("SOS", 3);
//--------------------------
if ( (RC = MPUAsyncReadWhenReady(2, &SensorData.MPUSample2)) )
BLIDeadStop("SOS", 3);
if (MPL_OK != MPLAsyncReadWhenReady(&SensorData.MPLSample))
BLIDeadStop("SOS", 3);
//-----------------------------------------------------
BLISignalFlip();
//-------------------------
SDLPostIfReady((byte*)&SensorData, sizeof(SensorData));
//-------------------------
TMRWaitAlarm(Alarm);
}
//*******************************************************************
return 0;
}
