Elizabeth Sonder / Mbed 2 deprecated IMU_serial_test

Speed testing IMUs

Dependencies:   MadgwickAHRS mbed

Fork of IMU_serial by Patrick Ciccone

main.cpp@3:c1902ecb30a7, 2016-10-06 (annotated)

Committer:
rctaduio
Date:
Thu Oct 06 17:35:14 2016 +0000
Revision:
3:c1902ecb30a7
Parent:
1:8715b7dd52ef 
added todo comments

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rctaduio 0:80a695ae3cc3 1 /*
rctaduio 0:80a695ae3cc3 2
rctaduio 0:80a695ae3cc3 3 MPU9250 calibration program
rctaduio 0:80a695ae3cc3 4 This program will send data that can be used to create a file that can be used to calibrate the imus
rctaduio 0:80a695ae3cc3 5
rctaduio 3:c1902ecb30a7 6 TODO:
rctaduio 0:80a695ae3cc3 7
rctaduio 3:c1902ecb30a7 8 (1) hardcoded output - allow for an output object to change what interface data comes from
rctaduio 3:c1902ecb30a7 9 (2) hardcoded input -allow for an input object to change what interface data goes to
rctaduio 3:c1902ecb30a7 10 (3) robustness, sometimes needs to be rebooted. maybe the serial interface times out?
rctaduio 3:c1902ecb30a7 11 (4) optimization - lots of ineffeciencies in code,
rctaduio 3:c1902ecb30a7 12 (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
rctaduio 3:c1902ecb30a7 13 (6) have ?4? i2c ports, can maybe use all 4 to log faster?
rctaduio 3:c1902ecb30a7 14 (7) need to run some speed tests on mux, find out how long it takes to switch a channel
rctaduio 3:c1902ecb30a7 15 (8) automate calibration - python logger gets data, finds calibration offsets, then saves file, then sends file to mbed to use
rctaduio 3:c1902ecb30a7 16 (9) move all constants to a seperate header file to make code more readable
rctaduio 0:80a695ae3cc3 17 */
rctaduio 0:80a695ae3cc3 18
rctaduio 0:80a695ae3cc3 19 #include "mbed.h"
rctaduio 0:80a695ae3cc3 20 #include "MPU9250.h"
rctaduio 0:80a695ae3cc3 21 #include "TCA9548.h"
rctaduio 0:80a695ae3cc3 22 #include "MadgwickAHRS.h"
rctaduio 0:80a695ae3cc3 23
rctaduio 0:80a695ae3cc3 24 MPU9250 mpu9250[8];
rctaduio 0:80a695ae3cc3 25 bool validIMU[8] = {0};
rctaduio 0:80a695ae3cc3 26 Timer t;
rctaduio 0:80a695ae3cc3 27 //Serial pc(USBTX, USBRX); // tx, rx
rctaduio 0:80a695ae3cc3 28 Serial pc(USBTX, USBRX); // tx, rx
rctaduio 0:80a695ae3cc3 29
rctaduio 0:80a695ae3cc3 30 TCA9548 mux;
rctaduio 0:80a695ae3cc3 31 uint16_t timeout= 60000; // set menu timeout to 60 seconds
rctaduio 0:80a695ae3cc3 32 const uint8_t hSize = 20; // max number of datas to transmit
rctaduio 0:80a695ae3cc3 33 const uint8_t hLen = 10; // max size of each header data name
rctaduio 0:80a695ae3cc3 34 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
rctaduio 0:80a695ae3cc3 35 float Data[hSize];
rctaduio 0:80a695ae3cc3 36 char header [hSize][hLen] = {
rctaduio 0:80a695ae3cc3 37 {"Time(ms)"},
rctaduio 0:80a695ae3cc3 38 {"Accel X"},
rctaduio 0:80a695ae3cc3 39 {"Accel Y"},
rctaduio 0:80a695ae3cc3 40 {"Accel Z"},
rctaduio 0:80a695ae3cc3 41 {"Gyro X"},
rctaduio 0:80a695ae3cc3 42 {"Gyro Y"},
rctaduio 0:80a695ae3cc3 43 {"Gyro Z"},
rctaduio 0:80a695ae3cc3 44 {"Mag X"},
rctaduio 0:80a695ae3cc3 45 {"Mag Y"},
rctaduio 0:80a695ae3cc3 46 {"Mag Z"},
rctaduio 0:80a695ae3cc3 47 {"Temp"},
rctaduio 0:80a695ae3cc3 48 {"Quatern 0"},
rctaduio 0:80a695ae3cc3 49 {"Quatern 1"},
rctaduio 0:80a695ae3cc3 50 {"Quatern 2"},
rctaduio 0:80a695ae3cc3 51 {"Quatern 3"},
rctaduio 0:80a695ae3cc3 52 {"Yaw"},
rctaduio 0:80a695ae3cc3 53 {"Pitch"},
rctaduio 0:80a695ae3cc3 54 {"Roll"},
rctaduio 0:80a695ae3cc3 55 {"Checksum"},
rctaduio 0:80a695ae3cc3 56 {"Imu"}
rctaduio 0:80a695ae3cc3 57 };
rctaduio 0:80a695ae3cc3 58
rctaduio 0:80a695ae3cc3 59 /////////////////////////////////////////////////////
rctaduio 0:80a695ae3cc3 60 //
rctaduio 0:80a695ae3cc3 61 // protoyptes
rctaduio 0:80a695ae3cc3 62 //
rctaduio 0:80a695ae3cc3 63 ////////////////////////////////////////////////////
rctaduio 0:80a695ae3cc3 64 void menu(Serial &pc);
rctaduio 0:80a695ae3cc3 65 void getdata(Serial &pc);
rctaduio 0:80a695ae3cc3 66 void menuOptions(Serial &pc);
rctaduio 0:80a695ae3cc3 67 void calibrate(Serial &pc);
rctaduio 0:80a695ae3cc3 68 void flushSerialBuffer(Serial &pc);
rctaduio 0:80a695ae3cc3 69 void configIMU(Serial &pc);
rctaduio 0:80a695ae3cc3 70 void dispHeader(Serial &pc);
rctaduio 0:80a695ae3cc3 71 void termCLS(Serial &pc);
rctaduio 0:80a695ae3cc3 72 void transmit(Serial &pc);
rctaduio 0:80a695ae3cc3 73 void getHeader(Serial &pc);
rctaduio 0:80a695ae3cc3 74
rctaduio 0:80a695ae3cc3 75
rctaduio 0:80a695ae3cc3 76 ////////////////////////////////////////////////////
rctaduio 0:80a695ae3cc3 77 //
rctaduio 0:80a695ae3cc3 78 // main
rctaduio 0:80a695ae3cc3 79 //
rctaduio 0:80a695ae3cc3 80 ///////////////////////////////////////////////////
rctaduio 0:80a695ae3cc3 81 int main()
rctaduio 0:80a695ae3cc3 82 {
rctaduio 0:80a695ae3cc3 83 // supported baud rates
rctaduio 0:80a695ae3cc3 84 // 110, 300, 600, 1200, 2400, 4800, 9600,
rctaduio 0:80a695ae3cc3 85 // 14400, 19200, 38400, 57600, 115200
rctaduio 0:80a695ae3cc3 86 // 230400, 460800, 921600
rctaduio 0:80a695ae3cc3 87 pc.baud(230400);
rctaduio 0:80a695ae3cc3 88 pc.format(8, Serial::None, 1);
rctaduio 0:80a695ae3cc3 89 //pc.set_flow_control(Serial::RTSCTS, Serial::RTS, Serial::CTS);
rctaduio 0:80a695ae3cc3 90 //Set up I2C
rctaduio 0:80a695ae3cc3 91 i2c.frequency(400000); // use fast (400 kHz) I2C
rctaduio 0:80a695ae3cc3 92
rctaduio 0:80a695ae3cc3 93 t.start();
rctaduio 0:80a695ae3cc3 94
rctaduio 0:80a695ae3cc3 95 //turn off led while everything is being configured
rctaduio 0:80a695ae3cc3 96 myled =0;
rctaduio 0:80a695ae3cc3 97
rctaduio 0:80a695ae3cc3 98 // Configure the IMUs
rctaduio 0:80a695ae3cc3 99 mux.addrSelect(0);
rctaduio 0:80a695ae3cc3 100 uint8_t imuCount = 0;
rctaduio 0:80a695ae3cc3 101 for (uint8_t i = 0; i < 8; i++) {
rctaduio 0:80a695ae3cc3 102 mux.addrSelect(i);
rctaduio 0:80a695ae3cc3 103 if (mpu9250[i].readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250) == 0x71) {
rctaduio 0:80a695ae3cc3 104 pc.printf("MPU9250 at addr %u is online...\n\r", i);
rctaduio 0:80a695ae3cc3 105 validIMU[i] = true;
rctaduio 0:80a695ae3cc3 106 imuCount++;
rctaduio 0:80a695ae3cc3 107 mpu9250[i].selfTest(pc);
rctaduio 0:80a695ae3cc3 108 mpu9250[i].config(pc);
rctaduio 0:80a695ae3cc3 109 mpu9250[i].sensitivity(pc);
rctaduio 0:80a695ae3cc3 110 }// end of if valid
rctaduio 0:80a695ae3cc3 111 } // end of initalize
rctaduio 0:80a695ae3cc3 112 if (imuCount <= 0) {
rctaduio 0:80a695ae3cc3 113 pc.printf("No imus detected, please check wiring and reset.");
rctaduio 0:80a695ae3cc3 114 while(1);
rctaduio 0:80a695ae3cc3 115 }// end of no IMU's
rctaduio 0:80a695ae3cc3 116
rctaduio 1:8715b7dd52ef 117 /*
rctaduio 0:80a695ae3cc3 118 mpu9250[0].magbias[0] = -259.875;
rctaduio 0:80a695ae3cc3 119 mpu9250[0].magbias[1] = 306.65;
rctaduio 0:80a695ae3cc3 120 mpu9250[0].magbias[2] = 334.755;
rctaduio 0:80a695ae3cc3 121 mpu9250[1].magbias[0] = -111.64;
rctaduio 0:80a695ae3cc3 122 mpu9250[1].magbias[1] = 162.335;
rctaduio 0:80a695ae3cc3 123 mpu9250[1].magbias[2] = -152.555;
rctaduio 0:80a695ae3cc3 124 mpu9250[2].magbias[0] = -51.295;
rctaduio 0:80a695ae3cc3 125 mpu9250[2].magbias[1] = 2.33;
rctaduio 0:80a695ae3cc3 126 mpu9250[2].magbias[2] = 300.575;
rctaduio 0:80a695ae3cc3 127 mpu9250[3].magbias[0] = 92.83;
rctaduio 0:80a695ae3cc3 128 mpu9250[3].magbias[1] = 397.625;
rctaduio 0:80a695ae3cc3 129 mpu9250[3].magbias[2] = 8.135;
rctaduio 0:80a695ae3cc3 130 mpu9250[4].magbias[0] = -202.085;
rctaduio 0:80a695ae3cc3 131 mpu9250[4].magbias[1] = 353.225;
rctaduio 0:80a695ae3cc3 132 mpu9250[4].magbias[2] = 252.605;
rctaduio 0:80a695ae3cc3 133 mpu9250[5].magbias[0] = -49.745;
rctaduio 0:80a695ae3cc3 134 mpu9250[5].magbias[1] = -314.78;
rctaduio 0:80a695ae3cc3 135 mpu9250[5].magbias[2] = 382.28;
rctaduio 0:80a695ae3cc3 136 mpu9250[6].magbias[0] = -97.02;
rctaduio 0:80a695ae3cc3 137 mpu9250[6].magbias[1] = 278.445;
rctaduio 0:80a695ae3cc3 138 mpu9250[6].magbias[2] = 23.985;
rctaduio 1:8715b7dd52ef 139 */
rctaduio 1:8715b7dd52ef 140 /*
rctaduio 1:8715b7dd52ef 141 mpu9250[0].magbias[0] = 528.025;
rctaduio 1:8715b7dd52ef 142 mpu9250[0].magbias[1] = -77.24;
rctaduio 1:8715b7dd52ef 143 mpu9250[0].magbias[2] = -364.87;
rctaduio 1:8715b7dd52ef 144 mpu9250[1].magbias[0] = 210.35;
rctaduio 1:8715b7dd52ef 145 mpu9250[1].magbias[1] = -60.52;
rctaduio 1:8715b7dd52ef 146 mpu9250[1].magbias[2] = 232.01;
rctaduio 1:8715b7dd52ef 147 mpu9250[2].magbias[0] = 226.175;
rctaduio 1:8715b7dd52ef 148 mpu9250[2].magbias[1] = 5.445;
rctaduio 1:8715b7dd52ef 149 mpu9250[2].magbias[2] = -451.24;
rctaduio 1:8715b7dd52ef 150 mpu9250[3].magbias[0] = 97.445;
rctaduio 1:8715b7dd52ef 151 mpu9250[3].magbias[1] = -185.585;
rctaduio 1:8715b7dd52ef 152 mpu9250[3].magbias[2] = -376.69;
rctaduio 1:8715b7dd52ef 153 mpu9250[4].magbias[0] = 428.44;
rctaduio 1:8715b7dd52ef 154 mpu9250[4].magbias[1] = -364.695;
rctaduio 1:8715b7dd52ef 155 mpu9250[4].magbias[2] = -550.065;
rctaduio 1:8715b7dd52ef 156 mpu9250[5].magbias[0] = 308.285;
rctaduio 1:8715b7dd52ef 157 mpu9250[5].magbias[1] = 668.055;
rctaduio 1:8715b7dd52ef 158 mpu9250[5].magbias[2] = -491.725;
rctaduio 1:8715b7dd52ef 159 mpu9250[6].magbias[0] = 83.03;
rctaduio 1:8715b7dd52ef 160 mpu9250[6].magbias[1] = 20.205;
rctaduio 1:8715b7dd52ef 161 mpu9250[6].magbias[2] = -151.41;
rctaduio 1:8715b7dd52ef 162 mpu9250[7].magbias[0] = -103.81;
rctaduio 1:8715b7dd52ef 163 mpu9250[7].magbias[1] = 35.75;
rctaduio 1:8715b7dd52ef 164 mpu9250[7].magbias[2] = -94.875;
rctaduio 1:8715b7dd52ef 165 */
rctaduio 0:80a695ae3cc3 166
rctaduio 1:8715b7dd52ef 167 mpu9250[0].magbias[0] = 278.785;
rctaduio 1:8715b7dd52ef 168 mpu9250[0].magbias[1] = 205.86;
rctaduio 1:8715b7dd52ef 169 mpu9250[0].magbias[2] = 60.225;
rctaduio 1:8715b7dd52ef 170 mpu9250[1].magbias[0] = 102.595;
rctaduio 1:8715b7dd52ef 171 mpu9250[1].magbias[1] = 87.055;
rctaduio 1:8715b7dd52ef 172 mpu9250[1].magbias[2] = 163.405;
rctaduio 1:8715b7dd52ef 173 mpu9250[2].magbias[0] = 183.43;
rctaduio 1:8715b7dd52ef 174 mpu9250[2].magbias[1] = -4.665;
rctaduio 1:8715b7dd52ef 175 mpu9250[2].magbias[2] = -60.715;
rctaduio 1:8715b7dd52ef 176 mpu9250[3].magbias[0] = 190.275;
rctaduio 1:8715b7dd52ef 177 mpu9250[3].magbias[1] = 199.215;
rctaduio 1:8715b7dd52ef 178 mpu9250[3].magbias[2] = -275.505;
rctaduio 1:8715b7dd52ef 179 mpu9250[4].magbias[0] = 240.075;
rctaduio 1:8715b7dd52ef 180 mpu9250[4].magbias[1] = -26.0;
rctaduio 1:8715b7dd52ef 181 mpu9250[4].magbias[2] = -209.325;
rctaduio 1:8715b7dd52ef 182 mpu9250[5].magbias[0] = 270.815;
rctaduio 1:8715b7dd52ef 183 mpu9250[5].magbias[1] = 361.685;
rctaduio 1:8715b7dd52ef 184 mpu9250[5].magbias[2] = -20.705;
rctaduio 1:8715b7dd52ef 185 mpu9250[6].magbias[0] = -10.88;
rctaduio 1:8715b7dd52ef 186 mpu9250[6].magbias[1] = 288.51;
rctaduio 1:8715b7dd52ef 187 mpu9250[6].magbias[2] = -38.98;
rctaduio 1:8715b7dd52ef 188 mpu9250[7].magbias[0] = -92.965;
rctaduio 1:8715b7dd52ef 189 mpu9250[7].magbias[1] = 34.195;
rctaduio 1:8715b7dd52ef 190 mpu9250[7].magbias[2] = -18.675;
rctaduio 0:80a695ae3cc3 191
rctaduio 0:80a695ae3cc3 192 menu(pc);
rctaduio 0:80a695ae3cc3 193 } // end of main
rctaduio 0:80a695ae3cc3 194
rctaduio 0:80a695ae3cc3 195
rctaduio 0:80a695ae3cc3 196 ////////////////////////////////////////
rctaduio 0:80a695ae3cc3 197 //
rctaduio 0:80a695ae3cc3 198 // termCLS
rctaduio 0:80a695ae3cc3 199 // Clears the terminal screen
rctaduio 0:80a695ae3cc3 200 //
rctaduio 0:80a695ae3cc3 201 ////////////////////////////////////////
rctaduio 0:80a695ae3cc3 202 void termCLS(Serial &pc)
rctaduio 0:80a695ae3cc3 203 {
rctaduio 0:80a695ae3cc3 204 pc.printf("\033[2J");
rctaduio 0:80a695ae3cc3 205 }
rctaduio 0:80a695ae3cc3 206
rctaduio 0:80a695ae3cc3 207
rctaduio 0:80a695ae3cc3 208 //////////////////////////////////////////
rctaduio 0:80a695ae3cc3 209 //
rctaduio 0:80a695ae3cc3 210 // menu
rctaduio 0:80a695ae3cc3 211 // main program menu
rctaduio 0:80a695ae3cc3 212 //
rctaduio 0:80a695ae3cc3 213 ///////////////////////////////////////////
rctaduio 0:80a695ae3cc3 214 void menu(Serial &pc)
rctaduio 0:80a695ae3cc3 215 {
rctaduio 0:80a695ae3cc3 216 pc.printf("\033[2J");
rctaduio 0:80a695ae3cc3 217 // turn on led now that you are ready to go
rctaduio 0:80a695ae3cc3 218 myled= 1;
rctaduio 0:80a695ae3cc3 219 char inval =0 ;
rctaduio 0:80a695ae3cc3 220 uint32_t stime = t.read_ms();
rctaduio 0:80a695ae3cc3 221 pc.printf("\r\nWelcome to data logging.\r\nPlease press 'm' for a list of commands\r\n");
rctaduio 0:80a695ae3cc3 222
rctaduio 0:80a695ae3cc3 223 while(1) {
rctaduio 0:80a695ae3cc3 224 if ((t.read_ms() - stime) > timeout)
rctaduio 0:80a695ae3cc3 225 menu(pc);
rctaduio 0:80a695ae3cc3 226 inval = 0;
rctaduio 0:80a695ae3cc3 227 if (pc.readable()) inval = pc.getc();
rctaduio 0:80a695ae3cc3 228 //flushSerialBuffer(pc);
rctaduio 0:80a695ae3cc3 229 switch (inval) {
rctaduio 0:80a695ae3cc3 230 case 'm' :
rctaduio 0:80a695ae3cc3 231 //display menu
rctaduio 0:80a695ae3cc3 232 menuOptions(pc);
rctaduio 0:80a695ae3cc3 233 break;
rctaduio 0:80a695ae3cc3 234
rctaduio 0:80a695ae3cc3 235 case '0':
rctaduio 0:80a695ae3cc3 236 menu(pc);
rctaduio 0:80a695ae3cc3 237 break;
rctaduio 0:80a695ae3cc3 238
rctaduio 0:80a695ae3cc3 239 case '1' :
rctaduio 0:80a695ae3cc3 240 // start logging data
rctaduio 0:80a695ae3cc3 241 getdata(pc);
rctaduio 0:80a695ae3cc3 242 break;
rctaduio 0:80a695ae3cc3 243
rctaduio 0:80a695ae3cc3 244 case '2':
rctaduio 0:80a695ae3cc3 245 //calibrate
rctaduio 0:80a695ae3cc3 246 calibrate(pc);
rctaduio 0:80a695ae3cc3 247 break;
rctaduio 0:80a695ae3cc3 248
rctaduio 0:80a695ae3cc3 249 case '3':
rctaduio 0:80a695ae3cc3 250 configIMU(pc);
rctaduio 0:80a695ae3cc3 251 break;
rctaduio 0:80a695ae3cc3 252
rctaduio 0:80a695ae3cc3 253 case '4':
rctaduio 0:80a695ae3cc3 254 getHeader(pc);
rctaduio 0:80a695ae3cc3 255 break;
rctaduio 0:80a695ae3cc3 256
rctaduio 0:80a695ae3cc3 257 default:
rctaduio 0:80a695ae3cc3 258 //break;
rctaduio 0:80a695ae3cc3 259 }
rctaduio 0:80a695ae3cc3 260 wait(.1);
rctaduio 0:80a695ae3cc3 261 } // end of wait to start
rctaduio 0:80a695ae3cc3 262 }// end of menu
rctaduio 0:80a695ae3cc3 263
rctaduio 0:80a695ae3cc3 264
rctaduio 0:80a695ae3cc3 265 ///////////////////////////////////////////////
rctaduio 0:80a695ae3cc3 266 //
rctaduio 0:80a695ae3cc3 267 // Menu options
rctaduio 0:80a695ae3cc3 268 // Come back and dynamically alocate this
rctaduio 0:80a695ae3cc3 269 //
rctaduio 0:80a695ae3cc3 270 ///////////////////////////////////////////////
rctaduio 0:80a695ae3cc3 271 void menuOptions(Serial &pc)
rctaduio 0:80a695ae3cc3 272 {
rctaduio 0:80a695ae3cc3 273 termCLS(pc);
rctaduio 0:80a695ae3cc3 274 pc.printf("'m':\tShow the menu\r\n");
rctaduio 0:80a695ae3cc3 275 pc.printf("'0':\tReturn to this menu at anytime\r\n");
rctaduio 0:80a695ae3cc3 276 pc.printf("'1':\tStart logging\r\n");
rctaduio 0:80a695ae3cc3 277 pc.printf("'2':\tEnter Calibration mode\r\n");
rctaduio 0:80a695ae3cc3 278 pc.printf("'3':\tEnter Configuration mode\r\n");
rctaduio 0:80a695ae3cc3 279 pc.printf("'4':\tGet current data header configuration\r\n");
rctaduio 0:80a695ae3cc3 280
rctaduio 0:80a695ae3cc3 281
rctaduio 0:80a695ae3cc3 282 pc.printf("\n");
rctaduio 0:80a695ae3cc3 283 } // end of menuOptions
rctaduio 0:80a695ae3cc3 284
rctaduio 0:80a695ae3cc3 285
rctaduio 0:80a695ae3cc3 286 /////////////////////////////////////////////
rctaduio 0:80a695ae3cc3 287 //
rctaduio 0:80a695ae3cc3 288 // getHeader
rctaduio 0:80a695ae3cc3 289 // Transmits header configuration over serial
rctaduio 0:80a695ae3cc3 290 // to host data logger
rctaduio 0:80a695ae3cc3 291 //
rctaduio 0:80a695ae3cc3 292 ////////////////////////////////////////////
rctaduio 0:80a695ae3cc3 293 void getHeader(Serial &pc)
rctaduio 0:80a695ae3cc3 294 {
rctaduio 0:80a695ae3cc3 295 //termCLS(pc);
rctaduio 0:80a695ae3cc3 296 uint8_t dcount = 0;
rctaduio 0:80a695ae3cc3 297 for (uint8_t i = 0; i < hSize; i++)
rctaduio 0:80a695ae3cc3 298 if (tData[i]) {
rctaduio 0:80a695ae3cc3 299 if (dcount > 0)
rctaduio 0:80a695ae3cc3 300 pc.printf(",");
rctaduio 0:80a695ae3cc3 301 pc.printf("%s", header[i]);
rctaduio 0:80a695ae3cc3 302 dcount++;
rctaduio 0:80a695ae3cc3 303 }
rctaduio 0:80a695ae3cc3 304 pc.printf("\r\n");
rctaduio 0:80a695ae3cc3 305 while(1) {
rctaduio 0:80a695ae3cc3 306 if (pc.readable())
rctaduio 0:80a695ae3cc3 307 if (pc.getc() == '0') menu(pc);
rctaduio 0:80a695ae3cc3 308 }
rctaduio 0:80a695ae3cc3 309 } // end of getHeader
rctaduio 0:80a695ae3cc3 310
rctaduio 0:80a695ae3cc3 311
rctaduio 0:80a695ae3cc3 312 ////////////////////////////////////////////
rctaduio 0:80a695ae3cc3 313 //
rctaduio 0:80a695ae3cc3 314 // Calibration feature, may not need
rctaduio 0:80a695ae3cc3 315 //
rctaduio 0:80a695ae3cc3 316 ////////////////////////////////////////////
rctaduio 0:80a695ae3cc3 317 void calibrate(Serial &pc)
rctaduio 0:80a695ae3cc3 318 {
rctaduio 0:80a695ae3cc3 319 // have the host computer do the following
rctaduio 0:80a695ae3cc3 320 // save current desired data members
rctaduio 0:80a695ae3cc3 321 // change members to only mag and imu
rctaduio 0:80a695ae3cc3 322 // get all the datas
rctaduio 0:80a695ae3cc3 323 // calculate calibration value
rctaduio 0:80a695ae3cc3 324 // then write values to mbed
rctaduio 0:80a695ae3cc3 325
rctaduio 0:80a695ae3cc3 326 //mbed then stores values as current calibration values?
rctaduio 0:80a695ae3cc3 327 termCLS(pc);
rctaduio 0:80a695ae3cc3 328 //pc.printf("Welcome to the calibration option\r\n");
rctaduio 0:80a695ae3cc3 329 //pc.printf("Please make sure to run the 'Calibrate' notebook to follow calibration options\r\n");
rctaduio 0:80a695ae3cc3 330 //pc.printf("Remember, you can press '0' at anytime to return to the main menu\r\n");
rctaduio 0:80a695ae3cc3 331
rctaduio 0:80a695ae3cc3 332 bool newtData[hSize] = {0,0,0,0,0,0,0,1,1,1,0,0,0,0,0,0,0,0,0,1};
rctaduio 0:80a695ae3cc3 333 memcpy(tData, newtData, sizeof(tData));
rctaduio 0:80a695ae3cc3 334 menu(pc);
rctaduio 0:80a695ae3cc3 335 }// end of calibrate
rctaduio 0:80a695ae3cc3 336
rctaduio 0:80a695ae3cc3 337
rctaduio 0:80a695ae3cc3 338 //////////////////////////////////////
rctaduio 0:80a695ae3cc3 339 //
rctaduio 0:80a695ae3cc3 340 // Flush serial input buffer
rctaduio 0:80a695ae3cc3 341 // may have issues, currently not using
rctaduio 0:80a695ae3cc3 342 //
rctaduio 0:80a695ae3cc3 343 ////////////////////////////////////////////////
rctaduio 0:80a695ae3cc3 344 void flushSerialBuffer(Serial &pc)
rctaduio 0:80a695ae3cc3 345 {
rctaduio 0:80a695ae3cc3 346 char char1 = 0;
rctaduio 0:80a695ae3cc3 347 while (pc.readable()) {
rctaduio 0:80a695ae3cc3 348 char1 = pc.getc();
rctaduio 0:80a695ae3cc3 349 }
rctaduio 0:80a695ae3cc3 350 return;
rctaduio 0:80a695ae3cc3 351 }
rctaduio 0:80a695ae3cc3 352
rctaduio 0:80a695ae3cc3 353
rctaduio 0:80a695ae3cc3 354 ////////////////////////////////////////////////
rctaduio 0:80a695ae3cc3 355 //
rctaduio 0:80a695ae3cc3 356 // Configureation menu
rctaduio 0:80a695ae3cc3 357 // Allows for adjustable options without recompiling
rctaduio 0:80a695ae3cc3 358 //
rctaduio 0:80a695ae3cc3 359 ////////////////////////////////////////////////
rctaduio 0:80a695ae3cc3 360 void configIMU(Serial &pc)
rctaduio 0:80a695ae3cc3 361 {
rctaduio 0:80a695ae3cc3 362 char inval =0 ;
rctaduio 0:80a695ae3cc3 363 dispHeader(pc);
rctaduio 0:80a695ae3cc3 364
rctaduio 0:80a695ae3cc3 365 while(1) {
rctaduio 0:80a695ae3cc3 366 inval = 0;
rctaduio 0:80a695ae3cc3 367 if (pc.readable()) inval = pc.getc();
rctaduio 0:80a695ae3cc3 368 //flushSerialBuffer(pc);
rctaduio 0:80a695ae3cc3 369
rctaduio 0:80a695ae3cc3 370 switch (inval) {
rctaduio 0:80a695ae3cc3 371 case '0' :
rctaduio 0:80a695ae3cc3 372 //go back to main menu
rctaduio 0:80a695ae3cc3 373 menu(pc);
rctaduio 0:80a695ae3cc3 374 break;
rctaduio 0:80a695ae3cc3 375
rctaduio 0:80a695ae3cc3 376 default:
rctaduio 0:80a695ae3cc3 377 if ((inval >= 'a') && (inval <= 'a' + hSize)) {
rctaduio 0:80a695ae3cc3 378 tData[inval - 'a'] =! tData[inval - 'a'];
rctaduio 0:80a695ae3cc3 379 dispHeader(pc);
rctaduio 0:80a695ae3cc3 380 } else if (inval != 0) {
rctaduio 0:80a695ae3cc3 381 pc.printf("Invalid input, please try again\r\n");
rctaduio 0:80a695ae3cc3 382 configIMU(pc);
rctaduio 0:80a695ae3cc3 383 }// end of valid selection
rctaduio 0:80a695ae3cc3 384 }// END OF SWITCH
rctaduio 0:80a695ae3cc3 385 }// end of while
rctaduio 0:80a695ae3cc3 386 }// end of config IMU
rctaduio 0:80a695ae3cc3 387
rctaduio 0:80a695ae3cc3 388
rctaduio 0:80a695ae3cc3 389 /////////////////////////////////////////////////
rctaduio 0:80a695ae3cc3 390 //
rctaduio 0:80a695ae3cc3 391 // Display data packet header
rctaduio 0:80a695ae3cc3 392 //
rctaduio 0:80a695ae3cc3 393 ////////////////////////////////////////////////
rctaduio 0:80a695ae3cc3 394 void dispHeader(Serial &pc)
rctaduio 0:80a695ae3cc3 395 {
rctaduio 0:80a695ae3cc3 396 termCLS(pc);
rctaduio 0:80a695ae3cc3 397 pc.printf("Welcome to the configuration option\r\n");
rctaduio 0:80a695ae3cc3 398 pc.printf("Below are the options you can choose for sending data\r\n");
rctaduio 0:80a695ae3cc3 399 pc.printf("Please press the value you wish to enable / disable\r\n\n");
rctaduio 0:80a695ae3cc3 400 pc.printf("\t0:\tReturn to main menu\r\n\n");
rctaduio 0:80a695ae3cc3 401 for(uint8_t i = 0; i < hSize; i++) {
rctaduio 0:80a695ae3cc3 402 pc.printf("\t%c)\t[%c]\t%s\r\n", 'a' + i, (tData[i] == 0) ? ' ': 'X', header[i]);
rctaduio 0:80a695ae3cc3 403 }
rctaduio 0:80a695ae3cc3 404 } // end of dispHeader
rctaduio 0:80a695ae3cc3 405
rctaduio 0:80a695ae3cc3 406
rctaduio 0:80a695ae3cc3 407 ///////////////////////////////////////////////////
rctaduio 0:80a695ae3cc3 408 //
rctaduio 0:80a695ae3cc3 409 // getdata from imu and transfer
rctaduio 0:80a695ae3cc3 410 //
rctaduio 0:80a695ae3cc3 411 ///////////////////////////////////////////////////
rctaduio 0:80a695ae3cc3 412 void getdata(Serial &pc)
rctaduio 0:80a695ae3cc3 413 {
rctaduio 0:80a695ae3cc3 414 while(1) {
rctaduio 0:80a695ae3cc3 415 if (pc.readable())
rctaduio 0:80a695ae3cc3 416 if (pc.getc() == '0') menu(pc);
rctaduio 0:80a695ae3cc3 417 for (uint8_t i = 0; i < 8; i++) {
rctaduio 0:80a695ae3cc3 418 mux.addrSelect(i);
rctaduio 0:80a695ae3cc3 419 if (!validIMU[i]) continue;
rctaduio 0:80a695ae3cc3 420 // If intPin goes high, all data registers have new data
rctaduio 0:80a695ae3cc3 421 if(mpu9250[i].readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {
rctaduio 0:80a695ae3cc3 422 if (i==0) myled= !myled;
rctaduio 0:80a695ae3cc3 423 // On interrupt, check if data ready interrupt
rctaduio 0:80a695ae3cc3 424 uint32_t checksum = 0;
rctaduio 0:80a695ae3cc3 425 mpu9250[i].readimu();
rctaduio 1:8715b7dd52ef 426
rctaduio 1:8715b7dd52ef 427
rctaduio 0:80a695ae3cc3 428 mpu9250[i].Now = t.read_us();
rctaduio 0:80a695ae3cc3 429 mpu9250[i].deltat = (float)((mpu9250[i].Now - mpu9250[i].lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
rctaduio 0:80a695ae3cc3 430 mpu9250[i].lastUpdate = mpu9250[i].Now;
rctaduio 0:80a695ae3cc3 431 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);
rctaduio 0:80a695ae3cc3 432 mpu9250[i].q[0] = q0;
rctaduio 0:80a695ae3cc3 433 mpu9250[i].q[1] = q1;
rctaduio 0:80a695ae3cc3 434 mpu9250[i].q[2] = q2;
rctaduio 0:80a695ae3cc3 435 mpu9250[i].q[3] = q3;
rctaduio 0:80a695ae3cc3 436
rctaduio 0:80a695ae3cc3 437 delt_t = t.read_ms() - count;
rctaduio 0:80a695ae3cc3 438
rctaduio 0:80a695ae3cc3 439 mpu9250[i].tempCount = mpu9250[i].readTempData(); // Read the adc values
rctaduio 0:80a695ae3cc3 440 mpu9250[i].temperature = ((float) mpu9250[i].tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
rctaduio 0:80a695ae3cc3 441
rctaduio 0:80a695ae3cc3 442 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]));
rctaduio 0:80a695ae3cc3 443 mpu9250[i].pitch = asin(2.0f * (mpu9250[i].q[0] * mpu9250[i].q[2] - mpu9250[i].q[3] * mpu9250[i].q[1]));
rctaduio 0:80a695ae3cc3 444 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]));
rctaduio 0:80a695ae3cc3 445
rctaduio 0:80a695ae3cc3 446 mpu9250[i].pitch *= 180.0f / PI;
rctaduio 0:80a695ae3cc3 447 mpu9250[i].yaw *= 180.0f / PI;
rctaduio 0:80a695ae3cc3 448 mpu9250[i].roll *= 180.0f / PI;
rctaduio 1:8715b7dd52ef 449
rctaduio 1:8715b7dd52ef 450
rctaduio 1:8715b7dd52ef 451
rctaduio 0:80a695ae3cc3 452 transmit(pc);
rctaduio 0:80a695ae3cc3 453 Data[1] = 1000*mpu9250[i].ax;
rctaduio 0:80a695ae3cc3 454 Data[2] = 1000*mpu9250[i].ay;
rctaduio 0:80a695ae3cc3 455 Data[3] = 1000*mpu9250[i].az;
rctaduio 0:80a695ae3cc3 456 Data[4] = mpu9250[i].gx;
rctaduio 0:80a695ae3cc3 457 Data[5] = mpu9250[i].gy;
rctaduio 0:80a695ae3cc3 458 Data[6] = mpu9250[i].gz;
rctaduio 0:80a695ae3cc3 459 Data[7] = mpu9250[i].mx;
rctaduio 0:80a695ae3cc3 460 Data[8] = mpu9250[i].my;
rctaduio 0:80a695ae3cc3 461 Data[9] = mpu9250[i].mz;
rctaduio 1:8715b7dd52ef 462
rctaduio 0:80a695ae3cc3 463 Data[10] = mpu9250[i].temperature;
rctaduio 0:80a695ae3cc3 464 Data[11] = mpu9250[i].q[0];
rctaduio 0:80a695ae3cc3 465 Data[12] = mpu9250[i].q[1];
rctaduio 0:80a695ae3cc3 466 Data[13] = mpu9250[i].q[2];
rctaduio 0:80a695ae3cc3 467 Data[14] = mpu9250[i].q[3];
rctaduio 0:80a695ae3cc3 468 Data[15] = mpu9250[i].yaw;
rctaduio 0:80a695ae3cc3 469 Data[16] = mpu9250[i].pitch;
rctaduio 0:80a695ae3cc3 470 Data[17] = mpu9250[i].roll;
rctaduio 0:80a695ae3cc3 471 Data[18] = mpu9250[i].checksum;
rctaduio 1:8715b7dd52ef 472
rctaduio 0:80a695ae3cc3 473 Data[19] = i;
rctaduio 0:80a695ae3cc3 474
rctaduio 0:80a695ae3cc3 475 count = t.read_ms();
rctaduio 0:80a695ae3cc3 476
rctaduio 0:80a695ae3cc3 477 if(count > 1<<21) {
rctaduio 0:80a695ae3cc3 478 t.start(); // start the timer over again if ~30 minutes has passed
rctaduio 0:80a695ae3cc3 479 count = 0;
rctaduio 0:80a695ae3cc3 480 mpu9250[i].deltat= 0;
rctaduio 0:80a695ae3cc3 481 mpu9250[i].lastUpdate = t.read_us();
rctaduio 0:80a695ae3cc3 482 } // end of if
rctaduio 0:80a695ae3cc3 483 } // end of if
rctaduio 0:80a695ae3cc3 484 }// end of for
rctaduio 0:80a695ae3cc3 485 } // end of while
rctaduio 0:80a695ae3cc3 486 }// end of get data
rctaduio 0:80a695ae3cc3 487
rctaduio 0:80a695ae3cc3 488
rctaduio 0:80a695ae3cc3 489 /////////////////////////////////////////
rctaduio 0:80a695ae3cc3 490 //
rctaduio 0:80a695ae3cc3 491 // Transmit data over link
rctaduio 0:80a695ae3cc3 492 //
rctaduio 0:80a695ae3cc3 493 ////////////////////////////////////////
rctaduio 0:80a695ae3cc3 494 void transmit(Serial &pc)
rctaduio 0:80a695ae3cc3 495 {
rctaduio 0:80a695ae3cc3 496 uint8_t stuff = 0;
rctaduio 0:80a695ae3cc3 497 if(tData[0]) {
rctaduio 0:80a695ae3cc3 498 pc.printf("%u,", t.read_ms());
rctaduio 0:80a695ae3cc3 499 stuff ++;
rctaduio 0:80a695ae3cc3 500 }
rctaduio 0:80a695ae3cc3 501 for(uint8_t i = 1; i < 18; i++)
rctaduio 0:80a695ae3cc3 502 if (tData[i]) {
rctaduio 0:80a695ae3cc3 503 pc.printf("%.2f,", Data[i]);
rctaduio 0:80a695ae3cc3 504 stuff++;
rctaduio 0:80a695ae3cc3 505 }
rctaduio 0:80a695ae3cc3 506
rctaduio 0:80a695ae3cc3 507 if (tData[18]) {
rctaduio 0:80a695ae3cc3 508 pc.printf("%u,", (int32_t)Data[18]);
rctaduio 0:80a695ae3cc3 509 stuff++;
rctaduio 0:80a695ae3cc3 510 }
rctaduio 0:80a695ae3cc3 511 if (tData[19]) {
rctaduio 0:80a695ae3cc3 512 pc.printf("%u", (int32_t)Data[19]);
rctaduio 0:80a695ae3cc3 513 stuff++;
rctaduio 0:80a695ae3cc3 514 }
rctaduio 0:80a695ae3cc3 515 if (stuff >0) pc.printf("\r\n");
rctaduio 0:80a695ae3cc3 516 }