int main(void)
{
//*******************************************************************
DBGInit(); // Initialize Debug interface
DBG_1_ON();
Init();
DBG_1_Flip();
TMRInit(2); // Initialize Timer interface with Priority=2
BLIInit(); // Initialize Signal interface
//*******************************************************************
ulong i;
DBG_1_ON();
BLIAsyncMorse("SSS", 3);
while(1)
{
TMRDelay(50); // Delay 50 msec
DBG_1_Flip();
DBG_2_Flip();
}
/*
BLIAsyncStart(30, 30);
while(1)
{TMRDelay(30);}
*/
BLISignalON();
while (1)
{
// TMRDelay(50);
for (i=0; i < 1000001; i++);
BLISignalFlip();
}
while(1);
{
for (i=0; i < 1000001; i++);
BLISignalFlip();
}
while(1)
{
BLIAsyncStart(500, 1000);
TMRDelay(5000); // Delay 5 sec
//--------------------------------
BLIAsyncMorse("SOS", 3);
TMRDelay(5000); // Delay 5 sec
}
//*******************************************************************
return 1;
}
int main(void)
{
//*******************************************************************
Init();
TMRInit(2); // Initialize Timer interface with Priority=2
BLIInit(); // Initialize Signal interface
ADCInit(3); // Initialize ADC
UARTInitTX(6, 350); // Initialize UART1 for TX
// 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
//*******************************************************************
struct
{
float V;
uint Raw;
uint Stat;
} UData;
int i = 0;
int j = 0;
BLISignalON();
while(j < 10)
{
TMRDelay(10);
//--------------------------
UData.V = ADCGetBatteryVoltage();
UData.Raw = ADCGetRawSample();
UData.Stat = ADCGetBatteryStatus();
//--------------------------
UARTPostWhenReady((uchar*)&UData, sizeof(UData));
i++;
//--------------------------
if (i >= 100)
{
UARTPostWhenReady(NULL, 0);
BLISignalFlip();
//------------------------
i = 0;
j++;
}
TMRDelay(10);
}
return 1;
}
int main(void)
{
//*******************************************************************
Init();
TMRInit(2); // Initialize Timer interface with Priority=2
BLIInit(); // Initialize Signal interface
//-------------------------------------------------------------------
BLIAsyncStart(100, 100);
TMRDelay(5000); // To avoid false-start at Reset
BLIAsyncStop();
BLISignalOFF();
//*******************************************************************
// 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);
//*******************************************************************
// HMC5983 Magnetometer initialization
//-------------------------------------------------------------------
byte IL = 5; // Interrupt level
//------------------------------------------------------
// <editor-fold defaultstate="collapsed" desc="Output Data Rate (ODR">
//------------------------------------------------------
// Typical Data Output Rate (Hz) for various ODR values
//------------------------------------------------------
// ODR = 0: 0.75
// ODR = 2: 1.5
// ODR = 2: 3
// ODR = 3: 7.5
// ODR = 4: 15 (Default)
// ODR = 5: 30
// ODR = 6: 75
// ODR = 7: 220 (fastest)
// </editor-fold>
byte ODR = 7; // Rate (fastest): 220 Hz
// <editor-fold defaultstate="collapsed" desc="Low-pass filtering (DLPF)">
//------------------------------------------------------
// Low-pass filtering achieved through sample averaging.
// Averaging does not affect effective ODR, so I assume
// that wit averaging enabled each successive reported
// sample is an average of the new measurement with "n"
// previous measurements - something like a FIR filter.
//------------------------------------------------------
// DLPF = 0 => 1-average
// DLPF = 1 => 2-average
// DLPF = 2 => 4-average
// DLPF = 3 => 8-average
//------------------------------------------------------
// </editor-fold>
byte DLPF = 0;
// <editor-fold defaultstate="collapsed" desc="Sensor Field Range (Gain)">
//------------------------------------------------------
// Recommended sensor field range (Ga) for various Gains
//------------------------------------------------------
// Gain = 0: 0.9
// Gain = 1: 1.3
// Gain = 2: 1.9
// Gain = 3: 2.5
// Gain = 4: 4.0
// Gain = 5: 4.7
// Gain = 6: 5.6
// Gain = 7: 8.1
//------------------------------------------------------
// The magnitude of Earth magnetic field varies over the
// surface of the Earth in the range 0.3 to 0.6 Gauss.
//------------------------------------------------------
// </editor-fold>
byte Gain = 1; // +/- 1.3 Ga
//------------------------------------------------------
HMC_Init(IL, ODR, Gain, DLPF);
//*******************************************************************
HMC_RC RC;
//----------------------
ulong i = 0;
//-----------------------------------------
byte RegA;
byte RegB;
//-------------------------------
RC = HMC_ReadA(&RegA);
i++;
//-------------------------------
RC = HMC_ReadB(&RegB);
i++;
//-------------------------------
//----------------------
struct
{
HMCData Sample;
ulong Count;
} SDLData;
SDLData.Count = 0;
//----------------------
while (1)
{
//-------------------------------
if ( HMC_OK != (RC = HMC_ReadSample(&SDLData.Sample)) )
BLIDeadStop("SOS", 3);
SDLData.Count++;
//-------------------------------
BLISignalFlip();
//-------------------------------
SDLPostIfReady((byte*)&SDLData, sizeof(SDLData));
//-------------------------------
}
//*******************************************************************
return 0;
}
int main(void)
{
//*******************************************************************
Init();
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);
//*******************************************************************
I2CInit(5, 1); // 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;
//==================================================================
BLIAsyncStart(100,100);
TMRDelay(2000);
BLIAsyncStop();
//==================================================================
BLIAsyncStart(50,50);
if (_SW2)
// Switch 2 is ON - Configuring MPU fo Alt. sensitivity
RC = MPUInit(0, 3, MPU_GYRO_1000ds, MPU_ACC_4g);
// Initialize motion Sensor
// 1 kHz/(0+1) = 1000 Hz (1ms)
// DLPF=3 => Bandwidth 44 Hz (delay: 4.9 msec)
else
// Switch 2 is OFF - Configuring MPU fo normal sensitivity
RC = MPUInit(0, 3, MPU_GYRO_2000ds, MPU_ACC_2g);
// Initialize motion Sensor
// 1 kHz/(0+1) = 1000 Hz (1ms)
if (RC) BLIDeadStop("EG", 2);
BLIAsyncStop();
//*******************************************************************
BLISignalOFF();
//====================================================
// byte mpuID;
// byte mpuDLPF;
// byte mpuINT;
// byte mpuPWRM1;
// //---------------------------
// RC = MPUReadID(2, &mpuID);
// RC = MPUGetPWRM1(2, &mpuPWRM1);
// RC = MPUGetDLPF(2, &mpuDLPF);
// RC = MPUGetINT(2, &mpuINT);
//-----------------------------------------------------
//====================================================
// Synchronous interface
//-----------------------------------------------------
struct
{
MPUData Sample1;
MPUData Sample2;
} MPU;
//-----------------------------------------------------
while (TRUE)
{
Alarm = TMRSetAlarm(500);
//------------------------------------
if ( (RC = MPUReadSample(1, &MPU.Sample1)) )
BLIDeadStop("SOS", 3);
//------------------------
if ( (RC = MPUReadSample(2, &MPU.Sample2)) )
BLIDeadStop("SOS", 3);
//------------------------------------
BLISignalFlip();
//-------------------------
SDLPostIfReady((byte*)&MPU, sizeof(MPU));
//-------------------------
TMRWaitAlarm(Alarm);
}
//*******************************************************************
return 0;
}
int main(void)
{
//*******************************************************************
Init();
TMRInit(2); // Initialize Timer interface with Priority=2
BLIInit(); // Initialize Signal interface
I2CInit(5, 0); // Initialize I2C1 module with IPL=5 and Fscl=400 KHz
//--------------------------
TMRDelay(1000); // Wait for 1 sec so that the shake from turning on
// power switch dissipates...
//--------------------------
if (MPUInit(3, 1)) // Initialize motion Sensor - 1 kHz/4 (250 Hz)
BLIDeadStop("EA", 2);
//--------------------------
#ifdef __MAG_Use__
if (HMCInit(6, 1, 0)) // Initialize magnetic Sensor
// ODR = 6 (max, 75 Hz),
// Gain = 2 (1.3 Gs)
// DLPF = 0 (no averaging)
BLIDeadStop("EM", 2);
#endif
//--------------------------
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;
//--------------------------
MPUSample AGSample;
#ifdef __MAG_Use__
HMCSample MSample;
#endif
//--------------------------
if (MPUAsyncStart())
BLIDeadStop("A", 1);
//--------------------------
#ifdef __MAG_Use__
if (HMCAsyncStart())
BLIDeadStop("M", 1);
#endif
//--------------------------
struct
{
ulong TS; // Timestamp of the cycle
//-----------------------------------------------
ulong MPUCount; // Sequential number of MPU sample
#ifdef __MAG_Use__
ulong MAGCount; // Sequential number of MAG sample
#endif
//-----------------------------------------------
// Accelerometer (in units of G)
//-----------------------------------------------
Vector A;
//-----------------------------------------------
// Gyroscopes (in Rad/sec)
//-----------------------------------------------
Vector G;
#ifdef __MAG_Use__
//-----------------------------------------------
// Magnetometer (in mGs)
//-----------------------------------------------
Vector M;
#endif
} UData;
//*******************************************************************
BLISignalON();
while(1)
{
TMRDelay(100);
//------------------------
#ifdef __MAG_Use__
RC = HMCAsyncReadWhenReady(&MSample);
if (RC) BLIDeadStop("M", 1);
#endif
//------------------------
RC = MPUAsyncReadWhenReady(&AGSample);
if (RC) BLIDeadStop("A", 1);
//---------------------------------------------
UData.MPUCount = AGSample.Count;
#ifdef __MAG_Use__
UData.MAGCount = MSample.Count;
#endif
//------------------------
VectorCopy(&AGSample.A, &UData.A);
VectorCopy(&AGSample.G, &UData.G);
#ifdef __MAG_Use__
VectorCopy(&MSample.M, &UData.M);
#endif
//------------------------
UData.TS = AGSample.TS;
//---------------------------------------------
UARTPostWhenReady((uchar*)&UData, sizeof(UData));
//---------------------------------------------
BLISignalFlip();
}
return 1;
}
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();
TMRInit(2); // Initialize Timer interface with Priority=2
BLIInit(); // Initialize Signal interface
//--------------------------
BLIAsyncMorse("S", 1); // dot-dot-dot
MCMInitF(50, 2500); // Initialize Motor Control at 50 Hz with setting
// Throttle to HIGH for delay interval to let ESC
// capture Throttle range
BLIAsyncStop();
//--------------------------
RCInit(4); // Initialize Receiver interface with Priority=4
//--------------------------
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
//*******************************************************************
BLISignalON();
TMRDelay(2000); // Wait for extra 2 sec - to let ESC arm...
// (finish the song :) )
BLISignalOFF();
//==================================================================
MCMData MC;
RCData RC;
//-------------------------------------------------
BLIAsyncMorse("R", 1); // dot-doh-dot
RCArm();
BLIAsyncStop();
//-------------------------------------------------
BLISignalON();
while(1)
{
RCReadWhenReady(&RC);
//---------------------------------------------
if (0 == RC.Control)
MC.F = MC.B = MC.L = MC.R = 0.0;
else
{
MC.F = RC.Throttle;
MC.B = RC.Throttle;
//--------------
MC.L = RC.Throttle;
MC.R = RC.Throttle;
//--------------
}
//---------------------------------------------
MCMSet(&MC);
//---------------------------------------------
UARTPostWhenReady((uchar*)&RC, sizeof(RC));
//---------------------------------------------
BLISignalFlip();
}
return 1;
}
int main(void)
{
//*******************************************************************
Init();
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);
//*******************************************************************
I2CInit(5, 2); // First param: IL = 5 (interrupt request priority
// Second param: I2C speed
// 0 - lowest (123 kHz at Fcy = 64MHz)
// 1 - 200 kHz - MPU-6050 stable
// 2 - 400 kHz
// 3 - 1 MHz
//-------------------------------------------------------------------
uint RC = 0;
ulong Alarm = 0;
//==================================================================
BLIAsyncStart(50,50);
TMRDelay(5000);
BLIAsyncStop();
//==================================================================
if (_SW2)
// Switch 2 is ON - Configuring MPU fo Alt. sensitivity
RC = MPUInit(0, 1, MPU_GYRO_1000ds, MPU_ACC_4g);
// Initialize motion Sensor
// 1 kHz/(0+1) = 1000 Hz (1ms)
// DLPF = 3 => Bandwidth 44 Hz (delay: 4.9 msec)
else
// Switch 2 is OFF - Configuring MPU fo normal sensitivity
RC = MPUInit(0, 1, MPU_GYRO_2000ds, MPU_ACC_2g);
// Initialize motion Sensor
// 1 kHz/(0+1) = 1000 Hz (1ms)
// DLPF = 1 => Bandwidth 184 Hz (delay: 2.0 msec)
if (RC) BLIDeadStop("EG", 2);
//*******************************************************************
struct
{
MPUData Sample1;
MPUData Sample2;
} MPU;
//=====================================================
// Initialize Asynchronous mode
//-----------------------------------------------------
if ( (RC = MPUAsyncStart(2)) )
BLIDeadStop("S2", 2);
//------------------------------
if ( (RC = MPUAsyncStart(1)) )
BLIDeadStop("S1", 2);
//=====================================================
// Calibrate Gyros
//-----------------------------------------------------
// BLIAsyncStart(100,100);
// //------------------------------
// if ( (RC = MPUCalibrate(1)) )
// BLIDeadStop("C1", 2);
// //------------------------------
// if ( (RC = MPUCalibrate(2)) )
// BLIDeadStop("C2", 2);
// //------------------------------
// BLIAsyncStop();
//=====================================================
// Main Loop
//-----------------------------------------------------
BLISignalOFF();
while (TRUE)
{
Alarm = TMRSetAlarm(1000);
//------------------------------------
if ( (RC = MPUAsyncReadWhenReady(1, &MPU.Sample1)) )
BLIDeadStop("SOS", 3);
//--------------------------
if ( (RC = MPUAsyncReadWhenReady(2, &MPU.Sample2)) )
BLIDeadStop("SOS", 3);
//------------------------
BLISignalFlip();
//-------------------------
SDLPostIfReady((byte*)&MPU, sizeof(MPU));
//-------------------------
TMRWaitAlarm(Alarm);
}
//*******************************************************************
return 0;
}
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;
}
int main(void)
{
//*******************************************************************
Init();
TMRInit(2); // Initialize Timer interface with Priority=2
BLIInit(); // Initialize Signal interface
//==================================================================
// Provide a 5 second delay prior to initialization of I2C interface
// to avoid false-start during programming by PIC Kit 3
//------------------------------------------------------------------
BLIAsyncStart(50,200);
TMRDelay(5000);
BLIAsyncStop();
//*******************************************************************
// 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 Asynchronous mode
//-----------------------------------------------------
if ( (RC = MPUAsyncStart(2)) )
BLIDeadStop("S2", 2);
//------------------------------
if ( (RC = MPUAsyncStart(1)) )
BLIDeadStop("S1", 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(1000);
BLIAsyncStop();
//*******************************************************************
// Calibrate Gyros
//-----------------------------------------------------
// MPUSetOptions(Rotate, TempComp, CrossAxis)
MPUSetOptions(TRUE, TRUE, TRUE);
//-----------------------------------------------------
BLIAsyncStart(100,100);
//------------------------------
if ( (RC = MPUCalibrateGyro(1)) )
BLIDeadStop("C1", 2);
//------------------------------
if ( (RC = MPUCalibrateGyro(2)) )
BLIDeadStop("C2", 2);
//------------------------------
BLIAsyncStop();
//=====================================================
struct
{
MPUData Sample1;
MPUData Sample2;
} MPU;
//=====================================================
// Main Loop
//-----------------------------------------------------
BLISignalOFF();
while (TRUE)
{
Alarm = TMRSetAlarm(20);
//------------------------------------
if ( (RC = MPUAsyncReadWhenReady(1, &MPU.Sample1)) )
BLIDeadStop("SM1", 3);
//--------------------------
if ( (RC = MPUAsyncReadWhenReady(2, &MPU.Sample2)) )
BLIDeadStop("SM2", 3);
//------------------------
BLISignalFlip();
//-------------------------
SDLPostIfReady((byte*)&MPU, sizeof(MPU));
//-------------------------
TMRWaitAlarm(Alarm);
}
//*******************************************************************
return 0;
}
