Happy Turkey Day
Dependencies: mbed GPSINT SDFileSystem1 FATDirHandle1 MPU9250
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
Generated on Thu Aug 4 2022 22:45:52 by 1.7.2