Elizabeth Sonder / Mbed 2 deprecated IMU_serial_test

Speed testing IMUs

Dependencies:   MadgwickAHRS mbed

Fork of IMU_serial by Patrick Ciccone

main.cpp@1:8715b7dd52ef, 2016-10-05 (annotated)

Committer:
rctaduio
Date:
Wed Oct 05 23:37:42 2016 +0000
Revision:
1:8715b7dd52ef
Parent:
0:80a695ae3cc3
Child:
3:c1902ecb30a7
initial commit of IMU_serial;

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