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
//*******************************************************************
// 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;
}
//************************************************************
uint IMUReset()
{
_IMUReady = 0;
//*******************************************************
BLIAsyncMorse("W", 1);
//*******************************************************
MPUData MPUReading;
DCMData IMUResult;
//-------------------------------------------------------
HMCSample HMCReading;
//*******************************************************
// Start HMC module so that it may accumulate multiple
// samples for averaging while DCM stabilizes
//-------------------------------------------------------
if (HMCAsyncStart())
BLIDeadStop("M", 1);
// Clear accumulated sensor readings
if (HMCAsyncReadWhenReady(&HMCReading))
BLIDeadStop("M", 1);
//*******************************************************
// Reset DCM algorithm
//-------------------------------------------------------
DCMReset();
//*******************************************************
uint IsReady = 0;
ulong StCount = 0;
ulong Alarm = 0;
//-------------------------------------------------------
// Start MPU6050 and calibrate Gyro offset
//-------------------------------------------------------
if (MPUAsyncStart())
// Async start failed...
BLIDeadStop("A", 1);
if (MPUCalibrate () != MPU_OK)
// Gyro Calibration failed
BLIDeadStop ("CA", 2);
//*******************************************************
// Read first sample orientation vector
//-------------------------------------------------------
if (MPUAsyncReadWhenReady(&MPUReading))
BLIDeadStop("A", 1);
//-------------------------------------------------------
// Update DCM until it is synchronized with current
// orientation vector
//-------------------------------------------------------
// Set Alarm for 10 msec so we read an average of
// about 10 MPU6050 samples for each DCM step
Alarm = TMRSetAlarm(10);
//-------------------------------------------------------
while (0 == IsReady)
{
TMRWaitAlarm(Alarm);
//--------------------------------------
// Read average of MPU6050 samples
//--------------------------------------
if (MPUAsyncReadWhenReady(&MPUReading))
BLIDeadStop("A", 1);
//--------------------------------------
Alarm = TMRSetAlarm(10); // Set Alarm for next measurement
//--------------------------------------
IsReady = DCMPerformStep( MPUReading.TS,
&MPUReading.G,
&MPUReading.A,
&IMUResult);
//----------------------------
StCount++;
}
//*******************************************************
// Attitude calculation stabilized; now we may adjust
// Acc Zx base...
//*******************************************************
if (MPU_OK != MPUAsyncAdjustAccZBase(IMUResult.Incl))
BLIDeadStop("A", 1);
//*******************************************************
// Now that we are done with the attitude calculation we
// may finalize calculation of true Azimuth from the
// magnetometer.
//*******************************************************
if (HMC_OK == HMCAsyncReadWhenReady(&HMCReading))
DCMSetAzimuth(&HMCReading.M);
//----------------------------------------------
// Stop magnetometer as it is not used in flight
//----------------------------------------------
HMCAsyncStop();
//*******************************************************
_IMUReady = 1;
//*******************************************************
BLIAsyncStop();
//*******************************************************
return StCount;
}