main.cpp

00001 #include "mbed.h"
00002 #include "MPU9250.h"
00003 #include "SDFileSystem.h"
00004 #include "GPSINT.h"
00005 DigitalOut myled(LED1);
00006 MPU9250 mpu9250(p28,p27);               // IMU
00007 GPSINT gps(p13,p14);                    // GPS
00008 Serial pc(USBTX, USBRX);                // tx, rx
00009 SDFileSystem sd(p5, p6, p7, p8, "sd");  // defines sd system
00010 Timer t;
00011 float sum = 0;
00012 uint32_t sumCount = 0;
00013 
00014 Ticker log_ticker;              // creates ticker
00015 FILE *fp;                       // defines file
00016 char file_name[100];            // creates file name character
00017 
00018 void mpu9250_initialization(){
00019     pc.printf("####CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);  
00020     //initial com check
00021     uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  // Read WHO_AM_I register for MPU-9250
00022     pc.printf("####I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x71\n\r");
00023     if (whoami == 0x71) // WHO_AM_I should always be 0x68
00024     {  
00025         pc.printf("####MPU9250 WHO_AM_I is 0x%x\n\r", whoami);
00026         pc.printf("####MPU9250 is online...\n\r");
00027         wait(1);
00028         //reset MPU and conduct self test//
00029         pc.printf("####Please wait,IMU Resetting####\r\n");
00030         mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
00031         pc.printf("####Self Test####\r\n");
00032         //initial MPU9250 Parameters
00033         mpu9250.Ascale = AFS_2G;
00034         mpu9250.Gscale = GFS_250DPS;               // GFS_250DPS, GFS_500DPS, GFS_1000DPS, GFS_2000DPS
00035         mpu9250.Mscale = MFS_16BITS;               // MFS_14BITS or MFS_16BITS, 14-bit or 16-bit magnetometer resolution
00036         mpu9250.Mmode = 0x06;
00037         mpu9250.delt_t=0;
00038         mpu9250.deltat=0.0f;
00039         mpu9250.lastUpdate = 0;
00040         mpu9250.firstUpdate = 0;
00041         mpu9250.Now = 0;
00042         mpu9250.count=0;
00043         mpu9250.q[0] = 1.0f;
00044         mpu9250.q[1] = 0.0f;
00045         mpu9250.q[2] = 0.0f;
00046         mpu9250.q[3] = 0.0f;
00047         // mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values
00048         //pc.printf("x-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[0]);  
00049         // pc.printf("y-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[1]);  
00050         //pc.printf("z-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[2]);  
00051         //pc.printf("x-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[3]);  
00052         //pc.printf("y-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[4]);  
00053         //pc.printf("z-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[5]);  
00054         pc.printf("####Gyro and accelerometer Calibration will start in 5 seconds####\r\n");
00055         pc.printf("####Please keep the IMU still\r\n");
00056         wait(5);
00057         pc.printf("####Calibration starts\r\n");
00058         mpu9250.calibrateMPU9250(mpu9250.gyroBias, mpu9250.accelBias); // Calibrate gyro and accelerometers, load biases in bias registers  
00059         pc.printf("x gyro bias = %f\n\r", mpu9250.gyroBias[0]);
00060         pc.printf("y gyro bias = %f\n\r", mpu9250.gyroBias[1]);
00061         pc.printf("z gyro bias = %f\n\r", mpu9250.gyroBias[2]);
00062         pc.printf("x accel bias = %f\n\r", mpu9250.accelBias[0]);
00063         pc.printf("y accel bias = %f\n\r", mpu9250.accelBias[1]);
00064         pc.printf("z accel bias = %f\n\r", mpu9250.accelBias[2]);
00065         wait(2);
00066         ///initialization
00067         mpu9250.initMPU9250();
00068         pc.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
00069         mpu9250.initAK8963(mpu9250.magCalibration);
00070         pc.printf("Magnetometer initilized\r\n");
00071        // pc.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
00072        // pc.printf("Accelerometer full-scale range = %f  g\n\r", 2.0f*(float)(1<<Ascale));
00073        // pc.printf("Gyroscope full-scale range = %f  deg/s\n\r", 250.0f*(float)(1<<Gscale));
00074         if(mpu9250.Mscale == 0) pc.printf("Magnetometer resolution = 14  bits\n\r");
00075         if(mpu9250.Mscale == 1) pc.printf("Magnetometer resolution = 16  bits\n\r");
00076         if(mpu9250.Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r");
00077         if(mpu9250.Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r");
00078     }
00079     else
00080     {
00081         pc.printf("Could not connect to MPU9250: \n\r");
00082         pc.printf("%#x \n",  whoami);
00083         while(1) ; // Loop forever if communication doesn't happen
00084     }
00085     mpu9250.getAres(); // Get accelerometer sensitivity
00086     mpu9250.getGres(); // Get gyro sensitivity
00087     mpu9250.getMres(); // Get magnetometer sensitivity
00088     pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/mpu9250.aRes);
00089     pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/mpu9250.gRes);
00090     pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mpu9250.mRes);
00091     pc.printf("####IMU initialization done####\r\n");
00092     wait(1);
00093 }
00094 
00095 
00096 void mag_cali(){
00097     int32_t mag_bias[3] = {0, 0, 0}, mag_scale[3] = {0, 0, 0};
00098     int16_t mag_max[3] = {-32767, -32767, -32767}, mag_min[3] = {32767, 32767, 32767}, mag_temp[3] = {0, 0, 0};
00099     //float dest1[3]={0,0,0}, dest2[3]={0,0,0};
00100     pc.printf("####Compass Calibration starts in 5 seconds\r\n");
00101     wait(5);
00102     pc.printf("###Start moving your imu in figure 8\r\n");
00103     for (int i=0;i<1500;i++){   //1500 for 100 Hz          
00104             mpu9250.readMagData(mag_temp);  // Read the x/y/z adc values  
00105             for(int jj=0; jj<3; jj++){
00106             if(mag_temp[jj] > mag_max[jj]) mag_max[jj] = mag_temp[jj];
00107             if(mag_temp[jj] < mag_min[jj]) mag_min[jj] = mag_temp[jj];
00108             }
00109             wait(0.01);//delay for 10 ms.
00110     }
00111     //get hard iron correction
00112     // Get hard iron correction
00113     mag_bias[0]  = (mag_max[0] + mag_min[0])/2;  // get average x mag bias in counts
00114     mag_bias[1]  = (mag_max[1] + mag_min[1])/2;  // get average y mag bias in counts
00115     mag_bias[2]  = (mag_max[2] + mag_min[2])/2;  // get average z mag bias in counts
00116     mpu9250.magbias[0] = (float) mag_bias[0]*mpu9250.mRes*mpu9250.magCalibration[0];  // save mag biases in G for main program
00117     mpu9250.magbias[1] = (float) mag_bias[1]*mpu9250.mRes*mpu9250.magCalibration[1];  
00118     mpu9250.magbias[2] = (float) mag_bias[2]*mpu9250.mRes*mpu9250.magCalibration[2];  
00119     pc.printf("####Mag bias =%f,%f,%f\r\n",mpu9250.magbias[0],mpu9250.magbias[1],mpu9250.magbias[2]);
00120     /*//get soft iron correction
00121     // Get soft iron correction estimate
00122     mag_scale[0]  = (mag_max[0] - mag_min[0])/2;  // get average x axis max chord length in counts
00123     mag_scale[1]  = (mag_max[1] - mag_min[1])/2;  // get average y axis max chord length in counts
00124     mag_scale[2]  = (mag_max[2] - mag_min[2])/2;  // get average z axis max chord length in counts
00125    
00126     float avg_rad = mag_scale[0] + mag_scale[1] + mag_scale[2];
00127     avg_rad /= 3.0;
00128    
00129     dest2[0] = avg_rad/((float)mag_scale[0]);
00130     dest2[1] = avg_rad/((float)mag_scale[1]);
00131     dest2[2] = avg_rad/((float)mag_scale[2]);
00132     */
00133     pc.printf("####Mag Calibration done!\r\n");
00134 }
00135 
00136 void sdwrite1(){        // Writes first set to SD & to Coolterm to confirm data
00137     fprintf(fp,"$IMUPS,%f,%f,%f,%f\r\n",t.read(),mpu9250.roll,mpu9250.pitch,mpu9250.yaw);
00138     pc.printf("$IMUPS,%f,%f,%f,%f\r\n",t.read(),mpu9250.roll,mpu9250.pitch,mpu9250.yaw);
00139 }
00140 
00141 void sdwrite2(){        // Writes 2nd set to SD & Coolterm
00142     fprintf(fp,"$GPSST,%d,%f,%d\r\n",gps.lock,gps.utc_time,gps.satelites);
00143     pc.printf("$GPSST,%d,%f,%d\r\n",gps.lock,gps.utc_time,gps.satelites);
00144 }
00145 
00146 void sdwrite3(){        // Writes 3rd set to SD & Coolterm
00147     fprintf(fp,"$GPSPS,%f,%f,%f,%f,%f\r\n",t.read(),gps.nmea_latitude,gps.nmea_longitude,gps.speed_k,gps.course_d);
00148     pc.printf("$GPSPS,%f,%f,%f,%f,%f\r\n",t.read(),gps.nmea_latitude,gps.nmea_longitude,gps.speed_k,gps.course_d);
00149 }
00150 
00151 
00152 int main() {
00153     char buffer[100];                                           // creates buffer character
00154     mkdir("/sd/A5_Beason", 0777);                                   // file location
00155     pc.printf("Please set a file name\r\n");                    // asks user for file name
00156     pc.scanf("%s",buffer);                                      // looks at the name given
00157     sprintf(file_name,"/sd/A5_Beason/%s.txt",buffer);               // creates file location
00158     pc.printf("The file name and directory is: %s\r\n",file_name);  // tells user the information
00159     fp = fopen(file_name, "w");     // opens file to be written on
00160     pc.printf("file_opened \n");    // tells user the file is opened
00161     if (fp == NULL) {               // if the file is not opened
00162         error("Could not open file for writing\n"); // informs user that the file was not opened
00163     }
00164     fclose(fp);                     // closes file
00165    
00166     pc.baud(9600);
00167     t.start();  
00168     mpu9250_initialization();
00169     mag_cali();
00170     pc.printf("####IMU is all set, going to start sensing in 5 seconds\r\n");
00171     wait(5);
00172     while(1){
00173         if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {  // On interrupt, check if data ready interrupt
00174             mpu9250.readAccelData(mpu9250.accelCount);  // Read the x/y/z adc values  
00175             // Now we'll calculate the accleration value into actual g's
00176             mpu9250.ax = (float)mpu9250.accelCount[0]*mpu9250.aRes - mpu9250.accelBias[0];  // get actual g value, this depends on scale being set
00177             mpu9250.ay = (float)mpu9250.accelCount[1]*mpu9250.aRes - mpu9250.accelBias[1];  
00178             mpu9250.az = (float)mpu9250.accelCount[2]*mpu9250.aRes - mpu9250.accelBias[2];  
00179            
00180             mpu9250.readGyroData(mpu9250.gyroCount);  // Read the x/y/z adc values
00181             // Calculate the gyro value into actual degrees per second
00182             mpu9250.gx = (float)mpu9250.gyroCount[0]*mpu9250.gRes - mpu9250.gyroBias[0];  // get actual gyro value, this depends on scale being set
00183             mpu9250.gy = (float)mpu9250.gyroCount[1]*mpu9250.gRes - mpu9250.gyroBias[1];  
00184             mpu9250.gz = (float)mpu9250.gyroCount[2]*mpu9250.gRes - mpu9250.gyroBias[2];  
00185          
00186             mpu9250.readMagData(mpu9250.magCount);  // Read the x/y/z adc values  
00187             // Calculate the magnetometer values in milliGauss
00188             // Include factory calibration per data sheet and user environmental corrections
00189             mpu9250.mx = (float)mpu9250.magCount[0]*mpu9250.mRes*mpu9250.magCalibration[0] - mpu9250.magbias[0];  // get actual magnetometer value, this depends on scale being set
00190             mpu9250.my = (float)mpu9250.magCount[1]*mpu9250.mRes*mpu9250.magCalibration[1] - mpu9250.magbias[1];  
00191             mpu9250.mz = (float)mpu9250.magCount[2]*mpu9250.mRes*mpu9250.magCalibration[2] - mpu9250.magbias[2];  
00192         }
00193        
00194         mpu9250.Now = t.read_us();
00195         mpu9250.deltat = (float)((mpu9250.Now - mpu9250.lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
00196         mpu9250.lastUpdate = mpu9250.Now;
00197         sum += mpu9250.deltat;
00198         sumCount++;
00199         //mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  mx,  my, mz);
00200         //compute the quaternion.
00201 
00202          mpu9250.MahonyQuaternionUpdate(mpu9250.ax, mpu9250.ay, mpu9250.az,
00203                                        mpu9250.gx*PI/180.00, mpu9250.gy*PI/180.00, mpu9250.gz*PI/180.00,
00204                                        mpu9250.mx, mpu9250.my, mpu9250.mz);
00205 
00206            //    pc.printf("Q=%f,%f,%f,%f\r\n",mpu9250.q[0],mpu9250.q[1],mpu9250.q[2],mpu9250.q[3]);
00207         // Serial print and/or display at 0.5 s rate independent of data rates
00208         mpu9250.delt_t = t.read_ms() - mpu9250.count;
00209         if (mpu9250.delt_t > 100) {
00210 //            pc.printf("ax = %f", 1000*mpu9250.ax);
00211 //            pc.printf(" ay = %f", 1000*mpu9250.ay);
00212 //            pc.printf(" az = %f  mg\n\r", 1000*mpu9250.az);
00213 //            pc.printf("gx = %f", mpu9250.gx);
00214 //            pc.printf("gy = %f", mpu9250.gy);
00215 //            pc.printf("gz = %f  deg/s\n\r", mpu9250.gz);
00216             //pc.printf("mx = %f", mpu9250.mx);
00217             //pc.printf(" my = %f", mpu9250.my);
00218             //pc.printf(" mz = %f  mG\n\r", mpu9250.mz);
00219 //            tempCount = mpu9250.readTempData();  // Read the adc values
00220 //            temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
00221 //            pc.printf(" temperature = %f  C\n\r", temperature);
00222            
00223             mpu9250.roll  = atan2(2.0f * (mpu9250.q[0] * mpu9250.q[1] + mpu9250.q[2] * mpu9250.q[3]),
00224                                   mpu9250.q[0] * mpu9250.q[0] - mpu9250.q[1] * mpu9250.q[1] - mpu9250.q[2] * mpu9250.q[2] + mpu9250.q[3] * mpu9250.q[3]);
00225             mpu9250.pitch = -asin(2.0f * (mpu9250.q[1] * mpu9250.q[3] - mpu9250.q[0] * mpu9250.q[2]));
00226             mpu9250.yaw   = atan2(2.0f * (mpu9250.q[1] * mpu9250.q[2] + mpu9250.q[0] * mpu9250.q[3]),
00227                                   mpu9250.q[0] * mpu9250.q[0] + mpu9250.q[1] * mpu9250.q[1] - mpu9250.q[2] * mpu9250.q[2] - mpu9250.q[3] * mpu9250.q[3]);  
00228             mpu9250.pitch *= 180.0f / PI;
00229             mpu9250.yaw   *= 180.0f / PI;
00230             mpu9250.yaw   += 15.0f; // Declination at RI
00231             mpu9250.roll  *= 180.0f / PI;
00232            
00233             fp = fopen(file_name,"a");      // Open file
00234                        
00235             sdwrite1();                 // Write $IMUPS to SD card
00236             sdwrite2();                 // Write $GPSST to SD card
00237             sdwrite3();                 // Write $GPSPS to SD card
00238             fclose(fp);                                     // close file
00239            
00240             myled= !myled;
00241             mpu9250.count = t.read_ms();
00242 
00243             if(mpu9250.count > 1<<21) {
00244                 t.start(); // start the timer over again if ~30 minutes has passed
00245                 mpu9250.count = 0;
00246                 mpu9250.deltat= 0;
00247                 mpu9250.lastUpdate = t.read_us();
00248             }
00249             sum = 0;
00250             sumCount = 0;
00251            
00252             pc.printf("lock=%d %f %f %c %f %c %f %f\r\n",gps.lock,gps.utc_time,gps.nmea_longitude,gps.ns,gps.nmea_latitude,gps.ew,gps.speed_k,gps.course_d);
00253             wait(1);
00254         }
00255    
00256     }    
00257 
00258 }
00259