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