imu_serial initial commit

Dependencies:   MadgwickAHRS mpu9250 mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 /*
00002 
00003 MPU9250 calibration program
00004 This program will send data that can be used to create a file that can be used to calibrate the imus
00005 
00006 TODO:
00007 
00008 (1) hardcoded output - allow for an output object to change what interface data comes from
00009 (2) hardcoded input -allow for an input object to change what interface data goes to
00010 (3) robustness, sometimes needs to be rebooted.  maybe the serial interface times out?
00011 (4) optimization - lots of ineffeciencies in code,
00012 (5) need to rerun some speed tests - find out how long it takes to get data from IMU, current sampling rate, if rate can be increased
00013 (6) have ?4? i2c ports, can maybe use all 4 to log faster?
00014 (7) need to run some speed tests on mux, find out how long it takes to switch a channel
00015 (8) automate calibration - python logger gets data, finds calibration offsets, then saves file, then sends file to mbed to use
00016 (9) move all constants to a seperate header file to make code more readable
00017 */
00018 
00019 #include "mbed.h"
00020 #include "MPU9250.h"
00021 #include "TCA9548.h"
00022 #include "MadgwickAHRS.h"
00023 
00024 MPU9250 mpu9250[8];
00025 bool validIMU[8] = {0};
00026 Timer t;
00027 //Serial pc(USBTX, USBRX); // tx, rx
00028 Serial pc(USBTX, USBRX); // tx, rx
00029 
00030 TCA9548 mux;
00031 uint16_t timeout= 60000;    // set menu timeout to 60 seconds
00032 const uint8_t hSize = 20;   // max number of datas to transmit
00033 const uint8_t hLen = 10;    // max size of each header data name
00034 bool tData[hSize] = {1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1};    // bool array to track what to transmit
00035 float Data[hSize];
00036 char header [hSize][hLen] = {
00037     {"Time(ms)"},
00038     {"Accel X"},
00039     {"Accel Y"},
00040     {"Accel Z"},
00041     {"Gyro X"},
00042     {"Gyro Y"},
00043     {"Gyro Z"},
00044     {"Mag X"},
00045     {"Mag Y"},
00046     {"Mag Z"},
00047     {"Temp"},
00048     {"Quatern 0"},
00049     {"Quatern 1"},
00050     {"Quatern 2"},
00051     {"Quatern 3"},
00052     {"Yaw"},
00053     {"Pitch"},
00054     {"Roll"},
00055     {"Checksum"},
00056     {"Imu"}
00057 };
00058 
00059 /////////////////////////////////////////////////////
00060 //
00061 //                  protoyptes
00062 //
00063 ////////////////////////////////////////////////////
00064 void menu(Serial &pc);
00065 void getdata(Serial &pc);
00066 void menuOptions(Serial &pc);
00067 void calibrate(Serial &pc);
00068 void flushSerialBuffer(Serial &pc);
00069 void configIMU(Serial &pc);
00070 void dispHeader(Serial &pc);
00071 void termCLS(Serial &pc);
00072 void transmit(Serial &pc);
00073 void getHeader(Serial &pc);
00074 
00075 
00076 ////////////////////////////////////////////////////
00077 //
00078 //                      main
00079 //
00080 ///////////////////////////////////////////////////
00081 int main()
00082 {
00083     // supported baud rates
00084     // 110, 300, 600, 1200, 2400, 4800, 9600,
00085     // 14400, 19200, 38400, 57600, 115200
00086     // 230400, 460800, 921600
00087     pc.baud(230400);
00088     pc.format(8, Serial::None, 1);
00089     //pc.set_flow_control(Serial::RTSCTS, Serial::RTS, Serial::CTS);
00090     //Set up I2C
00091     i2c.frequency(400000);  // use fast (400 kHz) I2C
00092 
00093     t.start();
00094 
00095     //turn off led while everything is being configured
00096     myled =0;
00097 
00098     // Configure the IMUs
00099     mux.addrSelect(0);
00100     uint8_t imuCount = 0;
00101     for (uint8_t i = 0; i < 8; i++) {
00102         mux.addrSelect(i);
00103         if (mpu9250[i].readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250) == 0x71) {
00104             pc.printf("MPU9250 at addr %u is online...\n\r", i);
00105             validIMU[i] = true;
00106             imuCount++;
00107             mpu9250[i].selfTest(pc);
00108             mpu9250[i].config(pc);
00109             mpu9250[i].sensitivity(pc);
00110         }// end of if valid
00111     } // end of initalize
00112     if (imuCount <= 0) {
00113         pc.printf("No imus detected, please check wiring and reset.");
00114         while(1);
00115     }// end of no IMU's
00116 
00117     /*
00118     mpu9250[0].magbias[0] = -259.875;
00119     mpu9250[0].magbias[1] = 306.65;
00120     mpu9250[0].magbias[2] = 334.755;
00121     mpu9250[1].magbias[0] = -111.64;
00122     mpu9250[1].magbias[1] = 162.335;
00123     mpu9250[1].magbias[2] = -152.555;
00124     mpu9250[2].magbias[0] = -51.295;
00125     mpu9250[2].magbias[1] = 2.33;
00126     mpu9250[2].magbias[2] = 300.575;
00127     mpu9250[3].magbias[0] = 92.83;
00128     mpu9250[3].magbias[1] = 397.625;
00129     mpu9250[3].magbias[2] = 8.135;
00130     mpu9250[4].magbias[0] = -202.085;
00131     mpu9250[4].magbias[1] = 353.225;
00132     mpu9250[4].magbias[2] = 252.605;
00133     mpu9250[5].magbias[0] = -49.745;
00134     mpu9250[5].magbias[1] = -314.78;
00135     mpu9250[5].magbias[2] = 382.28;
00136     mpu9250[6].magbias[0] = -97.02;
00137     mpu9250[6].magbias[1] = 278.445;
00138     mpu9250[6].magbias[2] = 23.985;
00139     */
00140     /*
00141     mpu9250[0].magbias[0] = 528.025;
00142     mpu9250[0].magbias[1] = -77.24;
00143     mpu9250[0].magbias[2] = -364.87;
00144     mpu9250[1].magbias[0] = 210.35;
00145     mpu9250[1].magbias[1] = -60.52;
00146     mpu9250[1].magbias[2] = 232.01;
00147     mpu9250[2].magbias[0] = 226.175;
00148     mpu9250[2].magbias[1] = 5.445;
00149     mpu9250[2].magbias[2] = -451.24;
00150     mpu9250[3].magbias[0] = 97.445;
00151     mpu9250[3].magbias[1] = -185.585;
00152     mpu9250[3].magbias[2] = -376.69;
00153     mpu9250[4].magbias[0] = 428.44;
00154     mpu9250[4].magbias[1] = -364.695;
00155     mpu9250[4].magbias[2] = -550.065;
00156     mpu9250[5].magbias[0] = 308.285;
00157     mpu9250[5].magbias[1] = 668.055;
00158     mpu9250[5].magbias[2] = -491.725;
00159     mpu9250[6].magbias[0] = 83.03;
00160     mpu9250[6].magbias[1] = 20.205;
00161     mpu9250[6].magbias[2] = -151.41;
00162     mpu9250[7].magbias[0] = -103.81;
00163     mpu9250[7].magbias[1] = 35.75;
00164     mpu9250[7].magbias[2] = -94.875;
00165     */
00166     /*
00167     mpu9250[0].magbias[0] = 278.785;
00168     mpu9250[0].magbias[1] = 205.86;
00169     mpu9250[0].magbias[2] = 60.225;
00170     mpu9250[1].magbias[0] = 102.595;
00171     mpu9250[1].magbias[1] = 87.055;
00172     mpu9250[1].magbias[2] = 163.405;
00173     mpu9250[2].magbias[0] = 183.43;
00174     mpu9250[2].magbias[1] = -4.665;
00175     mpu9250[2].magbias[2] = -60.715;
00176     mpu9250[3].magbias[0] = 190.275;
00177     mpu9250[3].magbias[1] = 199.215;
00178     mpu9250[3].magbias[2] = -275.505;
00179     mpu9250[4].magbias[0] = 240.075;
00180     mpu9250[4].magbias[1] = -26.0;
00181     mpu9250[4].magbias[2] = -209.325;
00182     mpu9250[5].magbias[0] = 270.815;
00183     mpu9250[5].magbias[1] = 361.685;
00184     mpu9250[5].magbias[2] = -20.705;
00185     mpu9250[6].magbias[0] = -10.88;
00186     mpu9250[6].magbias[1] = 288.51;
00187     mpu9250[6].magbias[2] = -38.98;
00188     mpu9250[7].magbias[0] = -92.965;
00189     mpu9250[7].magbias[1] = 34.195;
00190     mpu9250[7].magbias[2] = -18.675;
00191     */
00192     mpu9250[0].magbias[0] = 153.895;
00193     mpu9250[0].magbias[1] = 347.325;
00194     mpu9250[0].magbias[2] = -227.89;
00195     mpu9250[1].magbias[0] = 158.285;
00196     mpu9250[1].magbias[1] = 314.005;
00197     mpu9250[1].magbias[2] = -302.905;
00198 
00199 
00200     menu(pc);
00201 } // end of main
00202 
00203 
00204 ////////////////////////////////////////
00205 //
00206 //      termCLS
00207 //      Clears the terminal screen
00208 //
00209 ////////////////////////////////////////
00210 void termCLS(Serial &pc)
00211 {
00212     pc.printf("\033[2J");
00213 }
00214 
00215 
00216 //////////////////////////////////////////
00217 //
00218 //      menu
00219 //      main program menu
00220 //
00221 ///////////////////////////////////////////
00222 void menu(Serial &pc)
00223 {
00224     pc.printf("\033[2J");
00225     // turn on led now that you are ready to go
00226     myled= 1;
00227     char inval =0 ;
00228     uint32_t stime = t.read_ms();
00229     pc.printf("\r\nWelcome to data logging.\r\nPlease press 'm' for a list of commands\r\n");
00230 
00231     while(1) {
00232         if ((t.read_ms() - stime) > timeout)
00233             menu(pc);
00234         inval = 0;
00235         if (pc.readable()) inval = pc.getc();
00236         //flushSerialBuffer(pc);
00237         switch (inval) {
00238             case 'm' :
00239                 //display menu
00240                 menuOptions(pc);
00241                 break;
00242 
00243             case '0':
00244                 menu(pc);
00245                 break;
00246 
00247             case '1' :
00248                 // start logging data
00249                 getdata(pc);
00250                 break;
00251 
00252             case '2':
00253                 //calibrate
00254                 calibrate(pc);
00255                 break;
00256 
00257             case '3':
00258                 configIMU(pc);
00259                 break;
00260 
00261             case '4':
00262                 getHeader(pc);
00263                 break;
00264 
00265             default:
00266                 //break;
00267         }
00268         wait(.1);
00269     } // end of wait to start
00270 }// end of menu
00271 
00272 
00273 ///////////////////////////////////////////////
00274 //
00275 //      Menu options
00276 //      Come back and dynamically alocate this
00277 //
00278 ///////////////////////////////////////////////
00279 void menuOptions(Serial &pc)
00280 {
00281     termCLS(pc);
00282     pc.printf("'m':\tShow the menu\r\n");
00283     pc.printf("'0':\tReturn to this menu at anytime\r\n");
00284     pc.printf("'1':\tStart logging\r\n");
00285     pc.printf("'2':\tEnter Calibration mode\r\n");
00286     pc.printf("'3':\tEnter Configuration mode\r\n");
00287     pc.printf("'4':\tGet current data header configuration\r\n");
00288 
00289 
00290     pc.printf("\n");
00291 } // end of menuOptions
00292 
00293 
00294 /////////////////////////////////////////////
00295 //
00296 //      getHeader
00297 //      Transmits header configuration over serial
00298 //      to host data logger
00299 //
00300 ////////////////////////////////////////////
00301 void getHeader(Serial &pc)
00302 {
00303     //termCLS(pc);
00304     uint8_t dcount = 0;
00305     for (uint8_t i = 0; i < hSize; i++)
00306         if (tData[i]) {
00307             if (dcount > 0)
00308                 pc.printf(",");
00309             pc.printf("%s", header[i]);
00310             dcount++;
00311         }
00312     pc.printf("\r\n");
00313     while(1) {
00314         if (pc.readable())
00315             if (pc.getc() == '0') menu(pc);
00316     }
00317 } // end of getHeader
00318 
00319 
00320 ////////////////////////////////////////////
00321 //
00322 //      Calibration feature, may not need
00323 //
00324 ////////////////////////////////////////////
00325 void calibrate(Serial &pc)
00326 {
00327     // have the host computer do the following
00328     // save current desired data members
00329     // change members to only mag and imu
00330     // get all the datas
00331     // calculate calibration value
00332     // then write values to mbed
00333 
00334     //mbed then stores values as current calibration values?
00335     termCLS(pc);
00336     //pc.printf("Welcome to the calibration option\r\n");
00337     //pc.printf("Please make sure to run the 'Calibrate' notebook to follow calibration options\r\n");
00338     //pc.printf("Remember, you can press '0' at anytime to return to the main menu\r\n");
00339 
00340     bool newtData[hSize] = {0,0,0,0,0,0,0,1,1,1,0,0,0,0,0,0,0,0,0,1};
00341     memcpy(tData, newtData, sizeof(tData));
00342     menu(pc);
00343 }// end of calibrate
00344 
00345 
00346 //////////////////////////////////////
00347 //
00348 //      Flush serial input buffer
00349 //      may have issues, currently not using
00350 //
00351 ////////////////////////////////////////////////
00352 void flushSerialBuffer(Serial &pc)
00353 {
00354     char char1 = 0;
00355     while (pc.readable()) {
00356         char1 = pc.getc();
00357     }
00358     return;
00359 }
00360 
00361 
00362 ////////////////////////////////////////////////
00363 //
00364 //      Configureation menu
00365 //      Allows for adjustable options without recompiling
00366 //
00367 ////////////////////////////////////////////////
00368 void configIMU(Serial &pc)
00369 {
00370     char inval =0 ;
00371     dispHeader(pc);
00372 
00373     while(1) {
00374         inval = 0;
00375         if (pc.readable()) inval = pc.getc();
00376         //flushSerialBuffer(pc);
00377 
00378         switch (inval) {
00379             case '0' :
00380                 //go back to main menu
00381                 menu(pc);
00382                 break;
00383 
00384             default:
00385                 if ((inval >= 'a') && (inval <= 'a' + hSize)) {
00386                     tData[inval - 'a'] =! tData[inval - 'a'];
00387                     dispHeader(pc);
00388                 } else if (inval != 0) {
00389                     pc.printf("Invalid input, please try again\r\n");
00390                     configIMU(pc);
00391                 }// end of valid selection
00392         }// END OF SWITCH
00393     }// end of while
00394 }// end of config IMU
00395 
00396 
00397 /////////////////////////////////////////////////
00398 //
00399 //          Display data packet header
00400 //
00401 ////////////////////////////////////////////////
00402 void dispHeader(Serial &pc)
00403 {
00404     termCLS(pc);
00405     pc.printf("Welcome to the configuration option\r\n");
00406     pc.printf("Below are the options you can choose for sending data\r\n");
00407     pc.printf("Please press the value you wish to enable / disable\r\n\n");
00408     pc.printf("\t0:\tReturn to main menu\r\n\n");
00409     for(uint8_t i = 0; i < hSize; i++) {
00410         pc.printf("\t%c)\t[%c]\t%s\r\n", 'a' + i, (tData[i] == 0) ? ' ': 'X', header[i]);
00411     }
00412 } // end of dispHeader
00413 
00414 
00415 ///////////////////////////////////////////////////
00416 //
00417 //          getdata from imu and transfer
00418 //
00419 ///////////////////////////////////////////////////
00420 void getdata(Serial &pc)
00421 {
00422     while(1) {
00423         if (pc.readable())
00424             if (pc.getc() == '0') menu(pc);
00425         for (uint8_t i = 0; i < 8; i++) {
00426             mux.addrSelect(i);
00427             if (!validIMU[i]) continue;
00428             // If intPin goes high, all data registers have new data
00429             if(mpu9250[i].readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {
00430                 if (i==0) myled= !myled;
00431                 // On interrupt, check if data ready interrupt
00432                 uint32_t checksum = 0;
00433                 mpu9250[i].readimu();
00434 
00435 
00436                 mpu9250[i].Now = t.read_us();
00437                 mpu9250[i].deltat = (float)((mpu9250[i].Now - mpu9250[i].lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
00438                 mpu9250[i].lastUpdate = mpu9250[i].Now;
00439                 //mpu9250[i].MadgwickAHRSupdate(mpu9250[i].gx*PI/180.0f, mpu9250[i].gy*PI/180.0f, mpu9250[i].gz*PI/180.0f, mpu9250[i].ax, mpu9250[i].ay, mpu9250[i].az, mpu9250[i].my, mpu9250[i].mx, -1.0f*mpu9250[i].mz, mpu9250[i].q[0], mpu9250[i].q[1], mpu9250[i].q[2], mpu9250[i].q[3]);
00440                 mpu9250[i].Madgwickupdate();
00441                 //mpu9250[i].q[0] = q0;
00442                 //mpu9250[i].q[1] = q1;
00443                 //mpu9250[i].q[2] = q2;
00444                 //mpu9250[i].q[3] = q3;
00445 
00446                 delt_t = t.read_ms() - count;
00447 
00448                 mpu9250[i].tempCount = mpu9250[i].readTempData();  // Read the adc values
00449                 mpu9250[i].temperature = ((float) mpu9250[i].tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
00450 
00451                 mpu9250[i].roll  = atan2(2.0f * (mpu9250[i].q[0] * mpu9250[i].q[1] + mpu9250[i].q[2] * mpu9250[i].q[3]), (1.0f - 2.0f*mpu9250[i].q[1]*mpu9250[i].q[1] - 2.0f*mpu9250[i].q[2]*mpu9250[i].q[2]));
00452                 mpu9250[i].pitch = asin(2.0f * (mpu9250[i].q[0] * mpu9250[i].q[2] - mpu9250[i].q[3] * mpu9250[i].q[1]));
00453                 mpu9250[i].yaw   = atan2(2.0f * (mpu9250[i].q[0] * mpu9250[i].q[3] + mpu9250[i].q[1] * mpu9250[i].q[2]), (1.0f - 2.0f*mpu9250[i].q[2]*mpu9250[i].q[2] - 2.0f*mpu9250[i].q[3]*mpu9250[i].q[3]));
00454 
00455                 mpu9250[i].pitch *= 180.0f / PI;
00456                 mpu9250[i].yaw   *= 180.0f / PI;
00457                 mpu9250[i].roll  *= 180.0f / PI;
00458 
00459 
00460 
00461                 transmit(pc);
00462                 Data[1] = 1000*mpu9250[i].ax;
00463                 Data[2] = 1000*mpu9250[i].ay;
00464                 Data[3] = 1000*mpu9250[i].az;
00465                 Data[4] = mpu9250[i].gx;
00466                 Data[5] = mpu9250[i].gy;
00467                 Data[6] = mpu9250[i].gz;
00468                 Data[7] = mpu9250[i].mx;
00469                 Data[8] = mpu9250[i].my;
00470                 Data[9] = mpu9250[i].mz;
00471 
00472                 Data[10] = mpu9250[i].temperature;
00473                 Data[11] = mpu9250[i].q[0];
00474                 Data[12] = mpu9250[i].q[1];
00475                 Data[13] = mpu9250[i].q[2];
00476                 Data[14] = mpu9250[i].q[3];
00477                 Data[15] = mpu9250[i].yaw;
00478                 Data[16] = mpu9250[i].pitch;
00479                 Data[17] = mpu9250[i].roll;
00480                 Data[18] = mpu9250[i].checksum;
00481 
00482                 Data[19] = i;
00483 
00484                 count = t.read_ms();
00485 
00486                 if(count > 1<<21) {
00487                     t.start(); // start the timer over again if ~30 minutes has passed
00488                     count = 0;
00489                     mpu9250[i].deltat= 0;
00490                     mpu9250[i].lastUpdate = t.read_us();
00491                 } // end of if
00492             } // end of if
00493         }// end of for
00494     } // end of while
00495 }// end of get data
00496 
00497 
00498 /////////////////////////////////////////
00499 //
00500 //          Transmit data over link
00501 //
00502 ////////////////////////////////////////
00503 void transmit(Serial &pc)
00504 {
00505     uint8_t stuff = 0;
00506     if(tData[0]) {
00507         pc.printf("%u,", t.read_ms());
00508         stuff ++;
00509     }
00510     for(uint8_t i = 1; i < 18; i++)
00511         if (tData[i]) {
00512             pc.printf("%.2f,", Data[i]);
00513             stuff++;
00514         }
00515 
00516     if (tData[18]) {
00517         pc.printf("%u,", (int32_t)Data[18]);
00518         stuff++;
00519     }
00520     if (tData[19]) {
00521         pc.printf("%u", (int32_t)Data[19]);
00522         stuff++;
00523     }
00524     if (stuff >0) pc.printf("\r\n");
00525 }