IMU Ethernet initial commit

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