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