imu_serial initial commit
Dependencies: MadgwickAHRS mpu9250 mbed
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 }
Generated on Sat Jul 23 2022 10:23:43 by
1.7.2