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