Elizabeth Sonder / Mbed 2 deprecated IMU_serial_test

Speed testing IMUs

Dependencies:   MadgwickAHRS mbed

Fork of IMU_serial by Patrick Ciccone

main.cpp@0:80a695ae3cc3, 2016-09-28 (annotated)

Committer:
rctaduio
Date:
Wed Sep 28 23:59:57 2016 +0000
Revision:
0:80a695ae3cc3
Child:
1:8715b7dd52ef
stuff

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 0:80a695ae3cc3 107
rctaduio 0:80a695ae3cc3 108 /* //offsets from calibration
rctaduio 0:80a695ae3cc3 109 mpu9250[0].magbias[0] = -136.016697;
rctaduio 0:80a695ae3cc3 110 mpu9250[0].magbias[1] = 180.7041165;
rctaduio 0:80a695ae3cc3 111 mpu9250[0].magbias[2] = 531.2385865;
rctaduio 0:80a695ae3cc3 112 mpu9250[1].magbias[0] = -100.170666;
rctaduio 0:80a695ae3cc3 113 mpu9250[1].magbias[1] = 38.8694595;
rctaduio 0:80a695ae3cc3 114 mpu9250[1].magbias[2] = 162.8754425;
rctaduio 0:80a695ae3cc3 115 mpu9250[2].magbias[0] = -3.1089525;
rctaduio 0:80a695ae3cc3 116 mpu9250[2].magbias[1] = -143.011841;
rctaduio 0:80a695ae3cc3 117 mpu9250[2].magbias[2] = 612.3983765;
rctaduio 0:80a695ae3cc3 118 mpu9250[3].magbias[0] = 100.5001715;
rctaduio 0:80a695ae3cc3 119 mpu9250[3].magbias[1] = 244.6917115;
rctaduio 0:80a695ae3cc3 120 mpu9250[3].magbias[2] = 302.4588625;
rctaduio 0:80a695ae3cc3 121 mpu9250[4].magbias[0] = -175.6558455;
rctaduio 0:80a695ae3cc3 122 mpu9250[4].magbias[1] = 200.3966675;
rctaduio 0:80a695ae3cc3 123 mpu9250[4].magbias[2] = 573.4207155;
rctaduio 0:80a695ae3cc3 124 mpu9250[5].magbias[0] = -38.8619135;
rctaduio 0:80a695ae3cc3 125 mpu9250[5].magbias[1] = -484.9966735;
rctaduio 0:80a695ae3cc3 126 mpu9250[5].magbias[2] = 690.353607;
rctaduio 0:80a695ae3cc3 127 mpu9250[6].magbias[0] = -83.4814225;
rctaduio 0:80a695ae3cc3 128 mpu9250[6].magbias[1] = 131.300442;
rctaduio 0:80a695ae3cc3 129 mpu9250[6].magbias[2] = 200.63311;
rctaduio 0:80a695ae3cc3 130 */
rctaduio 0:80a695ae3cc3 131
rctaduio 0:80a695ae3cc3 132 mpu9250[0].magbias[0] = -259.875;
rctaduio 0:80a695ae3cc3 133 mpu9250[0].magbias[1] = 306.65;
rctaduio 0:80a695ae3cc3 134 mpu9250[0].magbias[2] = 334.755;
rctaduio 0:80a695ae3cc3 135 mpu9250[1].magbias[0] = -111.64;
rctaduio 0:80a695ae3cc3 136 mpu9250[1].magbias[1] = 162.335;
rctaduio 0:80a695ae3cc3 137 mpu9250[1].magbias[2] = -152.555;
rctaduio 0:80a695ae3cc3 138 mpu9250[2].magbias[0] = -51.295;
rctaduio 0:80a695ae3cc3 139 mpu9250[2].magbias[1] = 2.33;
rctaduio 0:80a695ae3cc3 140 mpu9250[2].magbias[2] = 300.575;
rctaduio 0:80a695ae3cc3 141 mpu9250[3].magbias[0] = 92.83;
rctaduio 0:80a695ae3cc3 142 mpu9250[3].magbias[1] = 397.625;
rctaduio 0:80a695ae3cc3 143 mpu9250[3].magbias[2] = 8.135;
rctaduio 0:80a695ae3cc3 144 mpu9250[4].magbias[0] = -202.085;
rctaduio 0:80a695ae3cc3 145 mpu9250[4].magbias[1] = 353.225;
rctaduio 0:80a695ae3cc3 146 mpu9250[4].magbias[2] = 252.605;
rctaduio 0:80a695ae3cc3 147 mpu9250[5].magbias[0] = -49.745;
rctaduio 0:80a695ae3cc3 148 mpu9250[5].magbias[1] = -314.78;
rctaduio 0:80a695ae3cc3 149 mpu9250[5].magbias[2] = 382.28;
rctaduio 0:80a695ae3cc3 150 mpu9250[6].magbias[0] = -97.02;
rctaduio 0:80a695ae3cc3 151 mpu9250[6].magbias[1] = 278.445;
rctaduio 0:80a695ae3cc3 152 mpu9250[6].magbias[2] = 23.985;
rctaduio 0:80a695ae3cc3 153
rctaduio 0:80a695ae3cc3 154
rctaduio 0:80a695ae3cc3 155 menu(pc);
rctaduio 0:80a695ae3cc3 156 } // end of main
rctaduio 0:80a695ae3cc3 157
rctaduio 0:80a695ae3cc3 158
rctaduio 0:80a695ae3cc3 159 ////////////////////////////////////////
rctaduio 0:80a695ae3cc3 160 //
rctaduio 0:80a695ae3cc3 161 // termCLS
rctaduio 0:80a695ae3cc3 162 // Clears the terminal screen
rctaduio 0:80a695ae3cc3 163 //
rctaduio 0:80a695ae3cc3 164 ////////////////////////////////////////
rctaduio 0:80a695ae3cc3 165 void termCLS(Serial &pc)
rctaduio 0:80a695ae3cc3 166 {
rctaduio 0:80a695ae3cc3 167 pc.printf("\033[2J");
rctaduio 0:80a695ae3cc3 168 }
rctaduio 0:80a695ae3cc3 169
rctaduio 0:80a695ae3cc3 170
rctaduio 0:80a695ae3cc3 171 //////////////////////////////////////////
rctaduio 0:80a695ae3cc3 172 //
rctaduio 0:80a695ae3cc3 173 // menu
rctaduio 0:80a695ae3cc3 174 // main program menu
rctaduio 0:80a695ae3cc3 175 //
rctaduio 0:80a695ae3cc3 176 ///////////////////////////////////////////
rctaduio 0:80a695ae3cc3 177 void menu(Serial &pc)
rctaduio 0:80a695ae3cc3 178 {
rctaduio 0:80a695ae3cc3 179 pc.printf("\033[2J");
rctaduio 0:80a695ae3cc3 180 // turn on led now that you are ready to go
rctaduio 0:80a695ae3cc3 181 myled= 1;
rctaduio 0:80a695ae3cc3 182 char inval =0 ;
rctaduio 0:80a695ae3cc3 183 uint32_t stime = t.read_ms();
rctaduio 0:80a695ae3cc3 184 pc.printf("\r\nWelcome to data logging.\r\nPlease press 'm' for a list of commands\r\n");
rctaduio 0:80a695ae3cc3 185
rctaduio 0:80a695ae3cc3 186 while(1) {
rctaduio 0:80a695ae3cc3 187 if ((t.read_ms() - stime) > timeout)
rctaduio 0:80a695ae3cc3 188 menu(pc);
rctaduio 0:80a695ae3cc3 189 inval = 0;
rctaduio 0:80a695ae3cc3 190 if (pc.readable()) inval = pc.getc();
rctaduio 0:80a695ae3cc3 191 //flushSerialBuffer(pc);
rctaduio 0:80a695ae3cc3 192 switch (inval) {
rctaduio 0:80a695ae3cc3 193 case 'm' :
rctaduio 0:80a695ae3cc3 194 //display menu
rctaduio 0:80a695ae3cc3 195 menuOptions(pc);
rctaduio 0:80a695ae3cc3 196 break;
rctaduio 0:80a695ae3cc3 197
rctaduio 0:80a695ae3cc3 198 case '0':
rctaduio 0:80a695ae3cc3 199 menu(pc);
rctaduio 0:80a695ae3cc3 200 break;
rctaduio 0:80a695ae3cc3 201
rctaduio 0:80a695ae3cc3 202 case '1' :
rctaduio 0:80a695ae3cc3 203 // start logging data
rctaduio 0:80a695ae3cc3 204 getdata(pc);
rctaduio 0:80a695ae3cc3 205 break;
rctaduio 0:80a695ae3cc3 206
rctaduio 0:80a695ae3cc3 207 case '2':
rctaduio 0:80a695ae3cc3 208 //calibrate
rctaduio 0:80a695ae3cc3 209 calibrate(pc);
rctaduio 0:80a695ae3cc3 210 break;
rctaduio 0:80a695ae3cc3 211
rctaduio 0:80a695ae3cc3 212 case '3':
rctaduio 0:80a695ae3cc3 213 configIMU(pc);
rctaduio 0:80a695ae3cc3 214 break;
rctaduio 0:80a695ae3cc3 215
rctaduio 0:80a695ae3cc3 216 case '4':
rctaduio 0:80a695ae3cc3 217 getHeader(pc);
rctaduio 0:80a695ae3cc3 218 break;
rctaduio 0:80a695ae3cc3 219
rctaduio 0:80a695ae3cc3 220 default:
rctaduio 0:80a695ae3cc3 221 //break;
rctaduio 0:80a695ae3cc3 222 }
rctaduio 0:80a695ae3cc3 223 wait(.1);
rctaduio 0:80a695ae3cc3 224 } // end of wait to start
rctaduio 0:80a695ae3cc3 225 }// end of menu
rctaduio 0:80a695ae3cc3 226
rctaduio 0:80a695ae3cc3 227
rctaduio 0:80a695ae3cc3 228 ///////////////////////////////////////////////
rctaduio 0:80a695ae3cc3 229 //
rctaduio 0:80a695ae3cc3 230 // Menu options
rctaduio 0:80a695ae3cc3 231 // Come back and dynamically alocate this
rctaduio 0:80a695ae3cc3 232 //
rctaduio 0:80a695ae3cc3 233 ///////////////////////////////////////////////
rctaduio 0:80a695ae3cc3 234 void menuOptions(Serial &pc)
rctaduio 0:80a695ae3cc3 235 {
rctaduio 0:80a695ae3cc3 236 termCLS(pc);
rctaduio 0:80a695ae3cc3 237 pc.printf("'m':\tShow the menu\r\n");
rctaduio 0:80a695ae3cc3 238 pc.printf("'0':\tReturn to this menu at anytime\r\n");
rctaduio 0:80a695ae3cc3 239 pc.printf("'1':\tStart logging\r\n");
rctaduio 0:80a695ae3cc3 240 pc.printf("'2':\tEnter Calibration mode\r\n");
rctaduio 0:80a695ae3cc3 241 pc.printf("'3':\tEnter Configuration mode\r\n");
rctaduio 0:80a695ae3cc3 242 pc.printf("'4':\tGet current data header configuration\r\n");
rctaduio 0:80a695ae3cc3 243
rctaduio 0:80a695ae3cc3 244
rctaduio 0:80a695ae3cc3 245 pc.printf("\n");
rctaduio 0:80a695ae3cc3 246 } // end of menuOptions
rctaduio 0:80a695ae3cc3 247
rctaduio 0:80a695ae3cc3 248
rctaduio 0:80a695ae3cc3 249 /////////////////////////////////////////////
rctaduio 0:80a695ae3cc3 250 //
rctaduio 0:80a695ae3cc3 251 // getHeader
rctaduio 0:80a695ae3cc3 252 // Transmits header configuration over serial
rctaduio 0:80a695ae3cc3 253 // to host data logger
rctaduio 0:80a695ae3cc3 254 //
rctaduio 0:80a695ae3cc3 255 ////////////////////////////////////////////
rctaduio 0:80a695ae3cc3 256 void getHeader(Serial &pc)
rctaduio 0:80a695ae3cc3 257 {
rctaduio 0:80a695ae3cc3 258 //termCLS(pc);
rctaduio 0:80a695ae3cc3 259 uint8_t dcount = 0;
rctaduio 0:80a695ae3cc3 260 for (uint8_t i = 0; i < hSize; i++)
rctaduio 0:80a695ae3cc3 261 if (tData[i]) {
rctaduio 0:80a695ae3cc3 262 if (dcount > 0)
rctaduio 0:80a695ae3cc3 263 pc.printf(",");
rctaduio 0:80a695ae3cc3 264 pc.printf("%s", header[i]);
rctaduio 0:80a695ae3cc3 265 dcount++;
rctaduio 0:80a695ae3cc3 266 }
rctaduio 0:80a695ae3cc3 267 pc.printf("\r\n");
rctaduio 0:80a695ae3cc3 268 while(1) {
rctaduio 0:80a695ae3cc3 269 if (pc.readable())
rctaduio 0:80a695ae3cc3 270 if (pc.getc() == '0') menu(pc);
rctaduio 0:80a695ae3cc3 271 }
rctaduio 0:80a695ae3cc3 272 } // end of getHeader
rctaduio 0:80a695ae3cc3 273
rctaduio 0:80a695ae3cc3 274
rctaduio 0:80a695ae3cc3 275 ////////////////////////////////////////////
rctaduio 0:80a695ae3cc3 276 //
rctaduio 0:80a695ae3cc3 277 // Calibration feature, may not need
rctaduio 0:80a695ae3cc3 278 //
rctaduio 0:80a695ae3cc3 279 ////////////////////////////////////////////
rctaduio 0:80a695ae3cc3 280 void calibrate(Serial &pc)
rctaduio 0:80a695ae3cc3 281 {
rctaduio 0:80a695ae3cc3 282 // have the host computer do the following
rctaduio 0:80a695ae3cc3 283 // save current desired data members
rctaduio 0:80a695ae3cc3 284 // change members to only mag and imu
rctaduio 0:80a695ae3cc3 285 // get all the datas
rctaduio 0:80a695ae3cc3 286 // calculate calibration value
rctaduio 0:80a695ae3cc3 287 // then write values to mbed
rctaduio 0:80a695ae3cc3 288
rctaduio 0:80a695ae3cc3 289 //mbed then stores values as current calibration values?
rctaduio 0:80a695ae3cc3 290 termCLS(pc);
rctaduio 0:80a695ae3cc3 291 //pc.printf("Welcome to the calibration option\r\n");
rctaduio 0:80a695ae3cc3 292 //pc.printf("Please make sure to run the 'Calibrate' notebook to follow calibration options\r\n");
rctaduio 0:80a695ae3cc3 293 //pc.printf("Remember, you can press '0' at anytime to return to the main menu\r\n");
rctaduio 0:80a695ae3cc3 294
rctaduio 0:80a695ae3cc3 295 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 296 memcpy(tData, newtData, sizeof(tData));
rctaduio 0:80a695ae3cc3 297 menu(pc);
rctaduio 0:80a695ae3cc3 298 }// end of calibrate
rctaduio 0:80a695ae3cc3 299
rctaduio 0:80a695ae3cc3 300
rctaduio 0:80a695ae3cc3 301 //////////////////////////////////////
rctaduio 0:80a695ae3cc3 302 //
rctaduio 0:80a695ae3cc3 303 // Flush serial input buffer
rctaduio 0:80a695ae3cc3 304 // may have issues, currently not using
rctaduio 0:80a695ae3cc3 305 //
rctaduio 0:80a695ae3cc3 306 ////////////////////////////////////////////////
rctaduio 0:80a695ae3cc3 307 void flushSerialBuffer(Serial &pc)
rctaduio 0:80a695ae3cc3 308 {
rctaduio 0:80a695ae3cc3 309 char char1 = 0;
rctaduio 0:80a695ae3cc3 310 while (pc.readable()) {
rctaduio 0:80a695ae3cc3 311 char1 = pc.getc();
rctaduio 0:80a695ae3cc3 312 }
rctaduio 0:80a695ae3cc3 313 return;
rctaduio 0:80a695ae3cc3 314 }
rctaduio 0:80a695ae3cc3 315
rctaduio 0:80a695ae3cc3 316
rctaduio 0:80a695ae3cc3 317 ////////////////////////////////////////////////
rctaduio 0:80a695ae3cc3 318 //
rctaduio 0:80a695ae3cc3 319 // Configureation menu
rctaduio 0:80a695ae3cc3 320 // Allows for adjustable options without recompiling
rctaduio 0:80a695ae3cc3 321 //
rctaduio 0:80a695ae3cc3 322 ////////////////////////////////////////////////
rctaduio 0:80a695ae3cc3 323 void configIMU(Serial &pc)
rctaduio 0:80a695ae3cc3 324 {
rctaduio 0:80a695ae3cc3 325 char inval =0 ;
rctaduio 0:80a695ae3cc3 326 dispHeader(pc);
rctaduio 0:80a695ae3cc3 327
rctaduio 0:80a695ae3cc3 328 while(1) {
rctaduio 0:80a695ae3cc3 329 inval = 0;
rctaduio 0:80a695ae3cc3 330 if (pc.readable()) inval = pc.getc();
rctaduio 0:80a695ae3cc3 331 //flushSerialBuffer(pc);
rctaduio 0:80a695ae3cc3 332
rctaduio 0:80a695ae3cc3 333 switch (inval) {
rctaduio 0:80a695ae3cc3 334 case '0' :
rctaduio 0:80a695ae3cc3 335 //go back to main menu
rctaduio 0:80a695ae3cc3 336 menu(pc);
rctaduio 0:80a695ae3cc3 337 break;
rctaduio 0:80a695ae3cc3 338
rctaduio 0:80a695ae3cc3 339 default:
rctaduio 0:80a695ae3cc3 340 if ((inval >= 'a') && (inval <= 'a' + hSize)) {
rctaduio 0:80a695ae3cc3 341 tData[inval - 'a'] =! tData[inval - 'a'];
rctaduio 0:80a695ae3cc3 342 dispHeader(pc);
rctaduio 0:80a695ae3cc3 343 } else if (inval != 0) {
rctaduio 0:80a695ae3cc3 344 pc.printf("Invalid input, please try again\r\n");
rctaduio 0:80a695ae3cc3 345 configIMU(pc);
rctaduio 0:80a695ae3cc3 346 }// end of valid selection
rctaduio 0:80a695ae3cc3 347 }// END OF SWITCH
rctaduio 0:80a695ae3cc3 348 }// end of while
rctaduio 0:80a695ae3cc3 349 }// end of config IMU
rctaduio 0:80a695ae3cc3 350
rctaduio 0:80a695ae3cc3 351
rctaduio 0:80a695ae3cc3 352 /////////////////////////////////////////////////
rctaduio 0:80a695ae3cc3 353 //
rctaduio 0:80a695ae3cc3 354 // Display data packet header
rctaduio 0:80a695ae3cc3 355 //
rctaduio 0:80a695ae3cc3 356 ////////////////////////////////////////////////
rctaduio 0:80a695ae3cc3 357 void dispHeader(Serial &pc)
rctaduio 0:80a695ae3cc3 358 {
rctaduio 0:80a695ae3cc3 359 termCLS(pc);
rctaduio 0:80a695ae3cc3 360 pc.printf("Welcome to the configuration option\r\n");
rctaduio 0:80a695ae3cc3 361 pc.printf("Below are the options you can choose for sending data\r\n");
rctaduio 0:80a695ae3cc3 362 pc.printf("Please press the value you wish to enable / disable\r\n\n");
rctaduio 0:80a695ae3cc3 363 pc.printf("\t0:\tReturn to main menu\r\n\n");
rctaduio 0:80a695ae3cc3 364 for(uint8_t i = 0; i < hSize; i++) {
rctaduio 0:80a695ae3cc3 365 pc.printf("\t%c)\t[%c]\t%s\r\n", 'a' + i, (tData[i] == 0) ? ' ': 'X', header[i]);
rctaduio 0:80a695ae3cc3 366 }
rctaduio 0:80a695ae3cc3 367 } // end of dispHeader
rctaduio 0:80a695ae3cc3 368
rctaduio 0:80a695ae3cc3 369
rctaduio 0:80a695ae3cc3 370 ///////////////////////////////////////////////////
rctaduio 0:80a695ae3cc3 371 //
rctaduio 0:80a695ae3cc3 372 // getdata from imu and transfer
rctaduio 0:80a695ae3cc3 373 //
rctaduio 0:80a695ae3cc3 374 ///////////////////////////////////////////////////
rctaduio 0:80a695ae3cc3 375 void getdata(Serial &pc)
rctaduio 0:80a695ae3cc3 376 {
rctaduio 0:80a695ae3cc3 377 while(1) {
rctaduio 0:80a695ae3cc3 378 if (pc.readable())
rctaduio 0:80a695ae3cc3 379 if (pc.getc() == '0') menu(pc);
rctaduio 0:80a695ae3cc3 380 for (uint8_t i = 0; i < 8; i++) {
rctaduio 0:80a695ae3cc3 381 mux.addrSelect(i);
rctaduio 0:80a695ae3cc3 382 if (!validIMU[i]) continue;
rctaduio 0:80a695ae3cc3 383 // If intPin goes high, all data registers have new data
rctaduio 0:80a695ae3cc3 384 if(mpu9250[i].readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {
rctaduio 0:80a695ae3cc3 385 if (i==0) myled= !myled;
rctaduio 0:80a695ae3cc3 386 // On interrupt, check if data ready interrupt
rctaduio 0:80a695ae3cc3 387 uint32_t checksum = 0;
rctaduio 0:80a695ae3cc3 388 mpu9250[i].readimu();
rctaduio 0:80a695ae3cc3 389
rctaduio 0:80a695ae3cc3 390
rctaduio 0:80a695ae3cc3 391 mpu9250[i].Now = t.read_us();
rctaduio 0:80a695ae3cc3 392 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 393 mpu9250[i].lastUpdate = mpu9250[i].Now;
rctaduio 0:80a695ae3cc3 394 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 395 mpu9250[i].q[0] = q0;
rctaduio 0:80a695ae3cc3 396 mpu9250[i].q[1] = q1;
rctaduio 0:80a695ae3cc3 397 mpu9250[i].q[2] = q2;
rctaduio 0:80a695ae3cc3 398 mpu9250[i].q[3] = q3;
rctaduio 0:80a695ae3cc3 399
rctaduio 0:80a695ae3cc3 400 delt_t = t.read_ms() - count;
rctaduio 0:80a695ae3cc3 401
rctaduio 0:80a695ae3cc3 402 mpu9250[i].tempCount = mpu9250[i].readTempData(); // Read the adc values
rctaduio 0:80a695ae3cc3 403 mpu9250[i].temperature = ((float) mpu9250[i].tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
rctaduio 0:80a695ae3cc3 404
rctaduio 0:80a695ae3cc3 405 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 406 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 407 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 408
rctaduio 0:80a695ae3cc3 409 mpu9250[i].pitch *= 180.0f / PI;
rctaduio 0:80a695ae3cc3 410 mpu9250[i].yaw *= 180.0f / PI;
rctaduio 0:80a695ae3cc3 411 mpu9250[i].roll *= 180.0f / PI;
rctaduio 0:80a695ae3cc3 412
rctaduio 0:80a695ae3cc3 413
rctaduio 0:80a695ae3cc3 414
rctaduio 0:80a695ae3cc3 415 transmit(pc);
rctaduio 0:80a695ae3cc3 416 Data[1] = 1000*mpu9250[i].ax;
rctaduio 0:80a695ae3cc3 417 Data[2] = 1000*mpu9250[i].ay;
rctaduio 0:80a695ae3cc3 418 Data[3] = 1000*mpu9250[i].az;
rctaduio 0:80a695ae3cc3 419 Data[4] = mpu9250[i].gx;
rctaduio 0:80a695ae3cc3 420 Data[5] = mpu9250[i].gy;
rctaduio 0:80a695ae3cc3 421 Data[6] = mpu9250[i].gz;
rctaduio 0:80a695ae3cc3 422 Data[7] = mpu9250[i].mx;
rctaduio 0:80a695ae3cc3 423 Data[8] = mpu9250[i].my;
rctaduio 0:80a695ae3cc3 424 Data[9] = mpu9250[i].mz;
rctaduio 0:80a695ae3cc3 425
rctaduio 0:80a695ae3cc3 426 Data[10] = mpu9250[i].temperature;
rctaduio 0:80a695ae3cc3 427 Data[11] = mpu9250[i].q[0];
rctaduio 0:80a695ae3cc3 428 Data[12] = mpu9250[i].q[1];
rctaduio 0:80a695ae3cc3 429 Data[13] = mpu9250[i].q[2];
rctaduio 0:80a695ae3cc3 430 Data[14] = mpu9250[i].q[3];
rctaduio 0:80a695ae3cc3 431 Data[15] = mpu9250[i].yaw;
rctaduio 0:80a695ae3cc3 432 Data[16] = mpu9250[i].pitch;
rctaduio 0:80a695ae3cc3 433 Data[17] = mpu9250[i].roll;
rctaduio 0:80a695ae3cc3 434 Data[18] = mpu9250[i].checksum;
rctaduio 0:80a695ae3cc3 435
rctaduio 0:80a695ae3cc3 436 Data[19] = i;
rctaduio 0:80a695ae3cc3 437
rctaduio 0:80a695ae3cc3 438 count = t.read_ms();
rctaduio 0:80a695ae3cc3 439
rctaduio 0:80a695ae3cc3 440 if(count > 1<<21) {
rctaduio 0:80a695ae3cc3 441 t.start(); // start the timer over again if ~30 minutes has passed
rctaduio 0:80a695ae3cc3 442 count = 0;
rctaduio 0:80a695ae3cc3 443 mpu9250[i].deltat= 0;
rctaduio 0:80a695ae3cc3 444 mpu9250[i].lastUpdate = t.read_us();
rctaduio 0:80a695ae3cc3 445 } // end of if
rctaduio 0:80a695ae3cc3 446 } // end of if
rctaduio 0:80a695ae3cc3 447 }// end of for
rctaduio 0:80a695ae3cc3 448 } // end of while
rctaduio 0:80a695ae3cc3 449 }// end of get data
rctaduio 0:80a695ae3cc3 450
rctaduio 0:80a695ae3cc3 451
rctaduio 0:80a695ae3cc3 452 /////////////////////////////////////////
rctaduio 0:80a695ae3cc3 453 //
rctaduio 0:80a695ae3cc3 454 // Transmit data over link
rctaduio 0:80a695ae3cc3 455 //
rctaduio 0:80a695ae3cc3 456 ////////////////////////////////////////
rctaduio 0:80a695ae3cc3 457 void transmit(Serial &pc)
rctaduio 0:80a695ae3cc3 458 {
rctaduio 0:80a695ae3cc3 459 uint8_t stuff = 0;
rctaduio 0:80a695ae3cc3 460 if(tData[0]) {
rctaduio 0:80a695ae3cc3 461 pc.printf("%u,", t.read_ms());
rctaduio 0:80a695ae3cc3 462 stuff ++;
rctaduio 0:80a695ae3cc3 463 }
rctaduio 0:80a695ae3cc3 464 for(uint8_t i = 1; i < 18; i++)
rctaduio 0:80a695ae3cc3 465 if (tData[i]) {
rctaduio 0:80a695ae3cc3 466 pc.printf("%.2f,", Data[i]);
rctaduio 0:80a695ae3cc3 467 stuff++;
rctaduio 0:80a695ae3cc3 468 }
rctaduio 0:80a695ae3cc3 469
rctaduio 0:80a695ae3cc3 470 if (tData[18]) {
rctaduio 0:80a695ae3cc3 471 pc.printf("%u,", (int32_t)Data[18]);
rctaduio 0:80a695ae3cc3 472 stuff++;
rctaduio 0:80a695ae3cc3 473 }
rctaduio 0:80a695ae3cc3 474 if (tData[19]) {
rctaduio 0:80a695ae3cc3 475 pc.printf("%u", (int32_t)Data[19]);
rctaduio 0:80a695ae3cc3 476 stuff++;
rctaduio 0:80a695ae3cc3 477 }
rctaduio 0:80a695ae3cc3 478 if (stuff >0) pc.printf("\r\n");
rctaduio 0:80a695ae3cc3 479 }