Elizabeth Sonder
Speed testing IMUs
Dependencies: MadgwickAHRS mbed
Fork of
IMU_serial
by 
Patrick Ciccone
Diff: main.cpp
- Revision:
- 0:80a695ae3cc3
- Child:
- 1:8715b7dd52ef
diff -r 000000000000 -r 80a695ae3cc3 main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Sep 28 23:59:57 2016 +0000
@@ -0,0 +1,479 @@
+/*
+
+MPU9250 calibration program
+This program will send data that can be used to create a file that can be used to calibrate the imus
+
+
+*/
+
+#include "mbed.h"
+#include "MPU9250.h"
+#include "TCA9548.h"
+#include "MadgwickAHRS.h"
+
+MPU9250 mpu9250[8];
+bool validIMU[8] = {0};
+Timer t;
+//Serial pc(USBTX, USBRX); // tx, rx
+Serial pc(USBTX, USBRX); // tx, rx
+
+TCA9548 mux;
+uint16_t timeout= 60000; // set menu timeout to 60 seconds
+const uint8_t hSize = 20; // max number of datas to transmit
+const uint8_t hLen = 10; // max size of each header data name
+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
+float Data[hSize];
+char header [hSize][hLen] = {
+ {"Time(ms)"},
+ {"Accel X"},
+ {"Accel Y"},
+ {"Accel Z"},
+ {"Gyro X"},
+ {"Gyro Y"},
+ {"Gyro Z"},
+ {"Mag X"},
+ {"Mag Y"},
+ {"Mag Z"},
+ {"Temp"},
+ {"Quatern 0"},
+ {"Quatern 1"},
+ {"Quatern 2"},
+ {"Quatern 3"},
+ {"Yaw"},
+ {"Pitch"},
+ {"Roll"},
+ {"Checksum"},
+ {"Imu"}
+};
+
+/////////////////////////////////////////////////////
+//
+// protoyptes
+//
+////////////////////////////////////////////////////
+void menu(Serial &pc);
+void getdata(Serial &pc);
+void menuOptions(Serial &pc);
+void calibrate(Serial &pc);
+void flushSerialBuffer(Serial &pc);
+void configIMU(Serial &pc);
+void dispHeader(Serial &pc);
+void termCLS(Serial &pc);
+void transmit(Serial &pc);
+void getHeader(Serial &pc);
+
+
+////////////////////////////////////////////////////
+//
+// main
+//
+///////////////////////////////////////////////////
+int main()
+{
+ // supported baud rates
+ // 110, 300, 600, 1200, 2400, 4800, 9600,
+ // 14400, 19200, 38400, 57600, 115200
+ // 230400, 460800, 921600
+ pc.baud(230400);
+ pc.format(8, Serial::None, 1);
+ //pc.set_flow_control(Serial::RTSCTS, Serial::RTS, Serial::CTS);
+ //Set up I2C
+ i2c.frequency(400000); // use fast (400 kHz) I2C
+
+ t.start();
+
+ //turn off led while everything is being configured
+ myled =0;
+
+ // Configure the IMUs
+ mux.addrSelect(0);
+ uint8_t imuCount = 0;
+ for (uint8_t i = 0; i < 8; i++) {
+ mux.addrSelect(i);
+ if (mpu9250[i].readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250) == 0x71) {
+ pc.printf("MPU9250 at addr %u is online...\n\r", i);
+ validIMU[i] = true;
+ imuCount++;
+ mpu9250[i].selfTest(pc);
+ mpu9250[i].config(pc);
+ mpu9250[i].sensitivity(pc);
+ }// end of if valid
+ } // end of initalize
+ if (imuCount <= 0) {
+ pc.printf("No imus detected, please check wiring and reset.");
+ while(1);
+ }// end of no IMU's
+
+
+ /* //offsets from calibration
+ mpu9250[0].magbias[0] = -136.016697;
+ mpu9250[0].magbias[1] = 180.7041165;
+ mpu9250[0].magbias[2] = 531.2385865;
+ mpu9250[1].magbias[0] = -100.170666;
+ mpu9250[1].magbias[1] = 38.8694595;
+ mpu9250[1].magbias[2] = 162.8754425;
+ mpu9250[2].magbias[0] = -3.1089525;
+ mpu9250[2].magbias[1] = -143.011841;
+ mpu9250[2].magbias[2] = 612.3983765;
+ mpu9250[3].magbias[0] = 100.5001715;
+ mpu9250[3].magbias[1] = 244.6917115;
+ mpu9250[3].magbias[2] = 302.4588625;
+ mpu9250[4].magbias[0] = -175.6558455;
+ mpu9250[4].magbias[1] = 200.3966675;
+ mpu9250[4].magbias[2] = 573.4207155;
+ mpu9250[5].magbias[0] = -38.8619135;
+ mpu9250[5].magbias[1] = -484.9966735;
+ mpu9250[5].magbias[2] = 690.353607;
+ mpu9250[6].magbias[0] = -83.4814225;
+ mpu9250[6].magbias[1] = 131.300442;
+ mpu9250[6].magbias[2] = 200.63311;
+ */
+
+ mpu9250[0].magbias[0] = -259.875;
+ mpu9250[0].magbias[1] = 306.65;
+ mpu9250[0].magbias[2] = 334.755;
+ mpu9250[1].magbias[0] = -111.64;
+ mpu9250[1].magbias[1] = 162.335;
+ mpu9250[1].magbias[2] = -152.555;
+ mpu9250[2].magbias[0] = -51.295;
+ mpu9250[2].magbias[1] = 2.33;
+ mpu9250[2].magbias[2] = 300.575;
+ mpu9250[3].magbias[0] = 92.83;
+ mpu9250[3].magbias[1] = 397.625;
+ mpu9250[3].magbias[2] = 8.135;
+ mpu9250[4].magbias[0] = -202.085;
+ mpu9250[4].magbias[1] = 353.225;
+ mpu9250[4].magbias[2] = 252.605;
+ mpu9250[5].magbias[0] = -49.745;
+ mpu9250[5].magbias[1] = -314.78;
+ mpu9250[5].magbias[2] = 382.28;
+ mpu9250[6].magbias[0] = -97.02;
+ mpu9250[6].magbias[1] = 278.445;
+ mpu9250[6].magbias[2] = 23.985;
+
+
+ menu(pc);
+} // end of main
+
+
+////////////////////////////////////////
+//
+// termCLS
+// Clears the terminal screen
+//
+////////////////////////////////////////
+void termCLS(Serial &pc)
+{
+ pc.printf("\033[2J");
+}
+
+
+//////////////////////////////////////////
+//
+// menu
+// main program menu
+//
+///////////////////////////////////////////
+void menu(Serial &pc)
+{
+ pc.printf("\033[2J");
+ // turn on led now that you are ready to go
+ myled= 1;
+ char inval =0 ;
+ uint32_t stime = t.read_ms();
+ pc.printf("\r\nWelcome to data logging.\r\nPlease press 'm' for a list of commands\r\n");
+
+ while(1) {
+ if ((t.read_ms() - stime) > timeout)
+ menu(pc);
+ inval = 0;
+ if (pc.readable()) inval = pc.getc();
+ //flushSerialBuffer(pc);
+ switch (inval) {
+ case 'm' :
+ //display menu
+ menuOptions(pc);
+ break;
+
+ case '0':
+ menu(pc);
+ break;
+
+ case '1' :
+ // start logging data
+ getdata(pc);
+ break;
+
+ case '2':
+ //calibrate
+ calibrate(pc);
+ break;
+
+ case '3':
+ configIMU(pc);
+ break;
+
+ case '4':
+ getHeader(pc);
+ break;
+
+ default:
+ //break;
+ }
+ wait(.1);
+ } // end of wait to start
+}// end of menu
+
+
+///////////////////////////////////////////////
+//
+// Menu options
+// Come back and dynamically alocate this
+//
+///////////////////////////////////////////////
+void menuOptions(Serial &pc)
+{
+ termCLS(pc);
+ pc.printf("'m':\tShow the menu\r\n");
+ pc.printf("'0':\tReturn to this menu at anytime\r\n");
+ pc.printf("'1':\tStart logging\r\n");
+ pc.printf("'2':\tEnter Calibration mode\r\n");
+ pc.printf("'3':\tEnter Configuration mode\r\n");
+ pc.printf("'4':\tGet current data header configuration\r\n");
+
+
+ pc.printf("\n");
+} // end of menuOptions
+
+
+/////////////////////////////////////////////
+//
+// getHeader
+// Transmits header configuration over serial
+// to host data logger
+//
+////////////////////////////////////////////
+void getHeader(Serial &pc)
+{
+ //termCLS(pc);
+ uint8_t dcount = 0;
+ for (uint8_t i = 0; i < hSize; i++)
+ if (tData[i]) {
+ if (dcount > 0)
+ pc.printf(",");
+ pc.printf("%s", header[i]);
+ dcount++;
+ }
+ pc.printf("\r\n");
+ while(1) {
+ if (pc.readable())
+ if (pc.getc() == '0') menu(pc);
+ }
+} // end of getHeader
+
+
+////////////////////////////////////////////
+//
+// Calibration feature, may not need
+//
+////////////////////////////////////////////
+void calibrate(Serial &pc)
+{
+ // have the host computer do the following
+ // save current desired data members
+ // change members to only mag and imu
+ // get all the datas
+ // calculate calibration value
+ // then write values to mbed
+
+ //mbed then stores values as current calibration values?
+ termCLS(pc);
+ //pc.printf("Welcome to the calibration option\r\n");
+ //pc.printf("Please make sure to run the 'Calibrate' notebook to follow calibration options\r\n");
+ //pc.printf("Remember, you can press '0' at anytime to return to the main menu\r\n");
+
+ bool newtData[hSize] = {0,0,0,0,0,0,0,1,1,1,0,0,0,0,0,0,0,0,0,1};
+ memcpy(tData, newtData, sizeof(tData));
+ menu(pc);
+}// end of calibrate
+
+
+//////////////////////////////////////
+//
+// Flush serial input buffer
+// may have issues, currently not using
+//
+////////////////////////////////////////////////
+void flushSerialBuffer(Serial &pc)
+{
+ char char1 = 0;
+ while (pc.readable()) {
+ char1 = pc.getc();
+ }
+ return;
+}
+
+
+////////////////////////////////////////////////
+//
+// Configureation menu
+// Allows for adjustable options without recompiling
+//
+////////////////////////////////////////////////
+void configIMU(Serial &pc)
+{
+ char inval =0 ;
+ dispHeader(pc);
+
+ while(1) {
+ inval = 0;
+ if (pc.readable()) inval = pc.getc();
+ //flushSerialBuffer(pc);
+
+ switch (inval) {
+ case '0' :
+ //go back to main menu
+ menu(pc);
+ break;
+
+ default:
+ if ((inval >= 'a') && (inval <= 'a' + hSize)) {
+ tData[inval - 'a'] =! tData[inval - 'a'];
+ dispHeader(pc);
+ } else if (inval != 0) {
+ pc.printf("Invalid input, please try again\r\n");
+ configIMU(pc);
+ }// end of valid selection
+ }// END OF SWITCH
+ }// end of while
+}// end of config IMU
+
+
+/////////////////////////////////////////////////
+//
+// Display data packet header
+//
+////////////////////////////////////////////////
+void dispHeader(Serial &pc)
+{
+ termCLS(pc);
+ pc.printf("Welcome to the configuration option\r\n");
+ pc.printf("Below are the options you can choose for sending data\r\n");
+ pc.printf("Please press the value you wish to enable / disable\r\n\n");
+ pc.printf("\t0:\tReturn to main menu\r\n\n");
+ for(uint8_t i = 0; i < hSize; i++) {
+ pc.printf("\t%c)\t[%c]\t%s\r\n", 'a' + i, (tData[i] == 0) ? ' ': 'X', header[i]);
+ }
+} // end of dispHeader
+
+
+///////////////////////////////////////////////////
+//
+// getdata from imu and transfer
+//
+///////////////////////////////////////////////////
+void getdata(Serial &pc)
+{
+ while(1) {
+ if (pc.readable())
+ if (pc.getc() == '0') menu(pc);
+ for (uint8_t i = 0; i < 8; i++) {
+ mux.addrSelect(i);
+ if (!validIMU[i]) continue;
+ // If intPin goes high, all data registers have new data
+ if(mpu9250[i].readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {
+ if (i==0) myled= !myled;
+ // On interrupt, check if data ready interrupt
+ uint32_t checksum = 0;
+ mpu9250[i].readimu();
+
+
+ mpu9250[i].Now = t.read_us();
+ mpu9250[i].deltat = (float)((mpu9250[i].Now - mpu9250[i].lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
+ mpu9250[i].lastUpdate = mpu9250[i].Now;
+ 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] = q0;
+ mpu9250[i].q[1] = q1;
+ mpu9250[i].q[2] = q2;
+ mpu9250[i].q[3] = q3;
+
+ delt_t = t.read_ms() - count;
+
+ mpu9250[i].tempCount = mpu9250[i].readTempData(); // Read the adc values
+ mpu9250[i].temperature = ((float) mpu9250[i].tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
+
+ 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]));
+ mpu9250[i].pitch = asin(2.0f * (mpu9250[i].q[0] * mpu9250[i].q[2] - mpu9250[i].q[3] * mpu9250[i].q[1]));
+ 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]));
+
+ mpu9250[i].pitch *= 180.0f / PI;
+ mpu9250[i].yaw *= 180.0f / PI;
+ mpu9250[i].roll *= 180.0f / PI;
+
+
+
+ transmit(pc);
+ Data[1] = 1000*mpu9250[i].ax;
+ Data[2] = 1000*mpu9250[i].ay;
+ Data[3] = 1000*mpu9250[i].az;
+ Data[4] = mpu9250[i].gx;
+ Data[5] = mpu9250[i].gy;
+ Data[6] = mpu9250[i].gz;
+ Data[7] = mpu9250[i].mx;
+ Data[8] = mpu9250[i].my;
+ Data[9] = mpu9250[i].mz;
+
+ Data[10] = mpu9250[i].temperature;
+ Data[11] = mpu9250[i].q[0];
+ Data[12] = mpu9250[i].q[1];
+ Data[13] = mpu9250[i].q[2];
+ Data[14] = mpu9250[i].q[3];
+ Data[15] = mpu9250[i].yaw;
+ Data[16] = mpu9250[i].pitch;
+ Data[17] = mpu9250[i].roll;
+ Data[18] = mpu9250[i].checksum;
+
+ Data[19] = i;
+
+ count = t.read_ms();
+
+ if(count > 1<<21) {
+ t.start(); // start the timer over again if ~30 minutes has passed
+ count = 0;
+ mpu9250[i].deltat= 0;
+ mpu9250[i].lastUpdate = t.read_us();
+ } // end of if
+ } // end of if
+ }// end of for
+ } // end of while
+}// end of get data
+
+
+/////////////////////////////////////////
+//
+// Transmit data over link
+//
+////////////////////////////////////////
+void transmit(Serial &pc)
+{
+ uint8_t stuff = 0;
+ if(tData[0]) {
+ pc.printf("%u,", t.read_ms());
+ stuff ++;
+ }
+ for(uint8_t i = 1; i < 18; i++)
+ if (tData[i]) {
+ pc.printf("%.2f,", Data[i]);
+ stuff++;
+ }
+
+ if (tData[18]) {
+ pc.printf("%u,", (int32_t)Data[18]);
+ stuff++;
+ }
+ if (tData[19]) {
+ pc.printf("%u", (int32_t)Data[19]);
+ stuff++;
+ }
+ if (stuff >0) pc.printf("\r\n");
+}