IMU Ethernet initial commit
Dependencies: F7_Ethernet MadgwickAHRS 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 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
Generated on Wed Jul 13 2022 19:50:58 by
1.7.2