//*************************************************************
uint MPUAsyncReadCombined(MPUData* pSample)
{
//---------------------------------------------------------
// This function attempts to read both sensors (if both are
// enabled) and returns average of measurements from both.
// If only one sensor is enabled, this function returns
// MPU_NOTA error code; in this case please use
// MPUAsyncReadWhenReady(...) specifying available sensor.
//---------------------------------------------------------
#ifndef Use_MPU1
return MPU_NOTA;
#endif
#ifndef Use_MPU2
return MPU_NOTA;
#endif
//---------------------------------------------------------
MPUData Sample1;
MPUData Sample2;
//---------------------------------------------------------
uint RC;
//---------------------------------------------------------
if ( MPU_OK != (RC = MPUAsyncReadWhenReady(1, &Sample1)) )
return RC;
if ( MPU_OK != (RC = MPUAsyncReadWhenReady(2, &Sample2)) )
return RC;
//---------------------------------------------------------
pSample->TS = Sample1.TS;
pSample->Temp = (Sample1.Temp + Sample2.Temp)*0.5;
//---------------------------------------------------------
VectorAdd(&Sample1.A, &Sample2.A, &pSample->A);
VectorScale(&pSample->A, 0.5, &pSample->A);
//---------------------------------------------------------
VectorAdd(&Sample1.G, &Sample2.G, &pSample->G);
VectorScale(&pSample->G, 0.5, &pSample->G);
//---------------------------------------------------------
return MPU_OK;
}
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
//*******************************************************************
// 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;
}
