Patrick Ciccone / Mbed 2 deprecated IMU_serial

imu_serial initial commit

Dependencies:   MadgwickAHRS mpu9250 mbed

main.cpp@4:7fa9cfebb00c, 2016-10-06 (annotated)

Committer:
rctaduio
Date:
Thu Oct 06 23:42:28 2016 +0000
Revision:
4:7fa9cfebb00c
Parent:
3:c1902ecb30a7 
asdf

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 4:7fa9cfebb00c 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 4:7fa9cfebb00c 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 4:7fa9cfebb00c 191 */
rctaduio 4:7fa9cfebb00c 192 mpu9250[0].magbias[0] = 153.895;
rctaduio 4:7fa9cfebb00c 193 mpu9250[0].magbias[1] = 347.325;
rctaduio 4:7fa9cfebb00c 194 mpu9250[0].magbias[2] = -227.89;
rctaduio 4:7fa9cfebb00c 195 mpu9250[1].magbias[0] = 158.285;
rctaduio 4:7fa9cfebb00c 196 mpu9250[1].magbias[1] = 314.005;
rctaduio 4:7fa9cfebb00c 197 mpu9250[1].magbias[2] = -302.905;
rctaduio 4:7fa9cfebb00c 198
rctaduio 0:80a695ae3cc3 199
rctaduio 0:80a695ae3cc3 200 menu(pc);
rctaduio 0:80a695ae3cc3 201 } // end of main
rctaduio 0:80a695ae3cc3 202
rctaduio 0:80a695ae3cc3 203
rctaduio 0:80a695ae3cc3 204 ////////////////////////////////////////
rctaduio 0:80a695ae3cc3 205 //
rctaduio 0:80a695ae3cc3 206 // termCLS
rctaduio 0:80a695ae3cc3 207 // Clears the terminal screen
rctaduio 0:80a695ae3cc3 208 //
rctaduio 0:80a695ae3cc3 209 ////////////////////////////////////////
rctaduio 0:80a695ae3cc3 210 void termCLS(Serial &pc)
rctaduio 0:80a695ae3cc3 211 {
rctaduio 0:80a695ae3cc3 212 pc.printf("\033[2J");
rctaduio 0:80a695ae3cc3 213 }
rctaduio 0:80a695ae3cc3 214
rctaduio 0:80a695ae3cc3 215
rctaduio 0:80a695ae3cc3 216 //////////////////////////////////////////
rctaduio 0:80a695ae3cc3 217 //
rctaduio 0:80a695ae3cc3 218 // menu
rctaduio 0:80a695ae3cc3 219 // main program menu
rctaduio 0:80a695ae3cc3 220 //
rctaduio 0:80a695ae3cc3 221 ///////////////////////////////////////////
rctaduio 0:80a695ae3cc3 222 void menu(Serial &pc)
rctaduio 0:80a695ae3cc3 223 {
rctaduio 0:80a695ae3cc3 224 pc.printf("\033[2J");
rctaduio 0:80a695ae3cc3 225 // turn on led now that you are ready to go
rctaduio 0:80a695ae3cc3 226 myled= 1;
rctaduio 0:80a695ae3cc3 227 char inval =0 ;
rctaduio 0:80a695ae3cc3 228 uint32_t stime = t.read_ms();
rctaduio 0:80a695ae3cc3 229 pc.printf("\r\nWelcome to data logging.\r\nPlease press 'm' for a list of commands\r\n");
rctaduio 0:80a695ae3cc3 230
rctaduio 0:80a695ae3cc3 231 while(1) {
rctaduio 0:80a695ae3cc3 232 if ((t.read_ms() - stime) > timeout)
rctaduio 0:80a695ae3cc3 233 menu(pc);
rctaduio 0:80a695ae3cc3 234 inval = 0;
rctaduio 0:80a695ae3cc3 235 if (pc.readable()) inval = pc.getc();
rctaduio 0:80a695ae3cc3 236 //flushSerialBuffer(pc);
rctaduio 0:80a695ae3cc3 237 switch (inval) {
rctaduio 0:80a695ae3cc3 238 case 'm' :
rctaduio 0:80a695ae3cc3 239 //display menu
rctaduio 0:80a695ae3cc3 240 menuOptions(pc);
rctaduio 0:80a695ae3cc3 241 break;
rctaduio 0:80a695ae3cc3 242
rctaduio 0:80a695ae3cc3 243 case '0':
rctaduio 0:80a695ae3cc3 244 menu(pc);
rctaduio 0:80a695ae3cc3 245 break;
rctaduio 0:80a695ae3cc3 246
rctaduio 0:80a695ae3cc3 247 case '1' :
rctaduio 0:80a695ae3cc3 248 // start logging data
rctaduio 0:80a695ae3cc3 249 getdata(pc);
rctaduio 0:80a695ae3cc3 250 break;
rctaduio 0:80a695ae3cc3 251
rctaduio 0:80a695ae3cc3 252 case '2':
rctaduio 0:80a695ae3cc3 253 //calibrate
rctaduio 0:80a695ae3cc3 254 calibrate(pc);
rctaduio 0:80a695ae3cc3 255 break;
rctaduio 0:80a695ae3cc3 256
rctaduio 0:80a695ae3cc3 257 case '3':
rctaduio 0:80a695ae3cc3 258 configIMU(pc);
rctaduio 0:80a695ae3cc3 259 break;
rctaduio 0:80a695ae3cc3 260
rctaduio 0:80a695ae3cc3 261 case '4':
rctaduio 0:80a695ae3cc3 262 getHeader(pc);
rctaduio 0:80a695ae3cc3 263 break;
rctaduio 0:80a695ae3cc3 264
rctaduio 0:80a695ae3cc3 265 default:
rctaduio 0:80a695ae3cc3 266 //break;
rctaduio 0:80a695ae3cc3 267 }
rctaduio 0:80a695ae3cc3 268 wait(.1);
rctaduio 0:80a695ae3cc3 269 } // end of wait to start
rctaduio 0:80a695ae3cc3 270 }// end of menu
rctaduio 0:80a695ae3cc3 271
rctaduio 0:80a695ae3cc3 272
rctaduio 0:80a695ae3cc3 273 ///////////////////////////////////////////////
rctaduio 0:80a695ae3cc3 274 //
rctaduio 0:80a695ae3cc3 275 // Menu options
rctaduio 0:80a695ae3cc3 276 // Come back and dynamically alocate this
rctaduio 0:80a695ae3cc3 277 //
rctaduio 0:80a695ae3cc3 278 ///////////////////////////////////////////////
rctaduio 0:80a695ae3cc3 279 void menuOptions(Serial &pc)
rctaduio 0:80a695ae3cc3 280 {
rctaduio 0:80a695ae3cc3 281 termCLS(pc);
rctaduio 0:80a695ae3cc3 282 pc.printf("'m':\tShow the menu\r\n");
rctaduio 0:80a695ae3cc3 283 pc.printf("'0':\tReturn to this menu at anytime\r\n");
rctaduio 0:80a695ae3cc3 284 pc.printf("'1':\tStart logging\r\n");
rctaduio 0:80a695ae3cc3 285 pc.printf("'2':\tEnter Calibration mode\r\n");
rctaduio 0:80a695ae3cc3 286 pc.printf("'3':\tEnter Configuration mode\r\n");
rctaduio 0:80a695ae3cc3 287 pc.printf("'4':\tGet current data header configuration\r\n");
rctaduio 0:80a695ae3cc3 288
rctaduio 0:80a695ae3cc3 289
rctaduio 0:80a695ae3cc3 290 pc.printf("\n");
rctaduio 0:80a695ae3cc3 291 } // end of menuOptions
rctaduio 0:80a695ae3cc3 292
rctaduio 0:80a695ae3cc3 293
rctaduio 0:80a695ae3cc3 294 /////////////////////////////////////////////
rctaduio 0:80a695ae3cc3 295 //
rctaduio 0:80a695ae3cc3 296 // getHeader
rctaduio 0:80a695ae3cc3 297 // Transmits header configuration over serial
rctaduio 0:80a695ae3cc3 298 // to host data logger
rctaduio 0:80a695ae3cc3 299 //
rctaduio 0:80a695ae3cc3 300 ////////////////////////////////////////////
rctaduio 0:80a695ae3cc3 301 void getHeader(Serial &pc)
rctaduio 0:80a695ae3cc3 302 {
rctaduio 0:80a695ae3cc3 303 //termCLS(pc);
rctaduio 0:80a695ae3cc3 304 uint8_t dcount = 0;
rctaduio 0:80a695ae3cc3 305 for (uint8_t i = 0; i < hSize; i++)
rctaduio 0:80a695ae3cc3 306 if (tData[i]) {
rctaduio 0:80a695ae3cc3 307 if (dcount > 0)
rctaduio 0:80a695ae3cc3 308 pc.printf(",");
rctaduio 0:80a695ae3cc3 309 pc.printf("%s", header[i]);
rctaduio 0:80a695ae3cc3 310 dcount++;
rctaduio 0:80a695ae3cc3 311 }
rctaduio 0:80a695ae3cc3 312 pc.printf("\r\n");
rctaduio 0:80a695ae3cc3 313 while(1) {
rctaduio 0:80a695ae3cc3 314 if (pc.readable())
rctaduio 0:80a695ae3cc3 315 if (pc.getc() == '0') menu(pc);
rctaduio 0:80a695ae3cc3 316 }
rctaduio 0:80a695ae3cc3 317 } // end of getHeader
rctaduio 0:80a695ae3cc3 318
rctaduio 0:80a695ae3cc3 319
rctaduio 0:80a695ae3cc3 320 ////////////////////////////////////////////
rctaduio 0:80a695ae3cc3 321 //
rctaduio 0:80a695ae3cc3 322 // Calibration feature, may not need
rctaduio 0:80a695ae3cc3 323 //
rctaduio 0:80a695ae3cc3 324 ////////////////////////////////////////////
rctaduio 0:80a695ae3cc3 325 void calibrate(Serial &pc)
rctaduio 0:80a695ae3cc3 326 {
rctaduio 0:80a695ae3cc3 327 // have the host computer do the following
rctaduio 0:80a695ae3cc3 328 // save current desired data members
rctaduio 0:80a695ae3cc3 329 // change members to only mag and imu
rctaduio 0:80a695ae3cc3 330 // get all the datas
rctaduio 0:80a695ae3cc3 331 // calculate calibration value
rctaduio 0:80a695ae3cc3 332 // then write values to mbed
rctaduio 0:80a695ae3cc3 333
rctaduio 0:80a695ae3cc3 334 //mbed then stores values as current calibration values?
rctaduio 0:80a695ae3cc3 335 termCLS(pc);
rctaduio 0:80a695ae3cc3 336 //pc.printf("Welcome to the calibration option\r\n");
rctaduio 0:80a695ae3cc3 337 //pc.printf("Please make sure to run the 'Calibrate' notebook to follow calibration options\r\n");
rctaduio 0:80a695ae3cc3 338 //pc.printf("Remember, you can press '0' at anytime to return to the main menu\r\n");
rctaduio 0:80a695ae3cc3 339
rctaduio 0:80a695ae3cc3 340 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 341 memcpy(tData, newtData, sizeof(tData));
rctaduio 0:80a695ae3cc3 342 menu(pc);
rctaduio 0:80a695ae3cc3 343 }// end of calibrate
rctaduio 0:80a695ae3cc3 344
rctaduio 0:80a695ae3cc3 345
rctaduio 0:80a695ae3cc3 346 //////////////////////////////////////
rctaduio 0:80a695ae3cc3 347 //
rctaduio 0:80a695ae3cc3 348 // Flush serial input buffer
rctaduio 0:80a695ae3cc3 349 // may have issues, currently not using
rctaduio 0:80a695ae3cc3 350 //
rctaduio 0:80a695ae3cc3 351 ////////////////////////////////////////////////
rctaduio 0:80a695ae3cc3 352 void flushSerialBuffer(Serial &pc)
rctaduio 0:80a695ae3cc3 353 {
rctaduio 0:80a695ae3cc3 354 char char1 = 0;
rctaduio 0:80a695ae3cc3 355 while (pc.readable()) {
rctaduio 0:80a695ae3cc3 356 char1 = pc.getc();
rctaduio 0:80a695ae3cc3 357 }
rctaduio 0:80a695ae3cc3 358 return;
rctaduio 0:80a695ae3cc3 359 }
rctaduio 0:80a695ae3cc3 360
rctaduio 0:80a695ae3cc3 361
rctaduio 0:80a695ae3cc3 362 ////////////////////////////////////////////////
rctaduio 0:80a695ae3cc3 363 //
rctaduio 0:80a695ae3cc3 364 // Configureation menu
rctaduio 0:80a695ae3cc3 365 // Allows for adjustable options without recompiling
rctaduio 0:80a695ae3cc3 366 //
rctaduio 0:80a695ae3cc3 367 ////////////////////////////////////////////////
rctaduio 0:80a695ae3cc3 368 void configIMU(Serial &pc)
rctaduio 0:80a695ae3cc3 369 {
rctaduio 0:80a695ae3cc3 370 char inval =0 ;
rctaduio 0:80a695ae3cc3 371 dispHeader(pc);
rctaduio 0:80a695ae3cc3 372
rctaduio 0:80a695ae3cc3 373 while(1) {
rctaduio 0:80a695ae3cc3 374 inval = 0;
rctaduio 0:80a695ae3cc3 375 if (pc.readable()) inval = pc.getc();
rctaduio 0:80a695ae3cc3 376 //flushSerialBuffer(pc);
rctaduio 0:80a695ae3cc3 377
rctaduio 0:80a695ae3cc3 378 switch (inval) {
rctaduio 0:80a695ae3cc3 379 case '0' :
rctaduio 0:80a695ae3cc3 380 //go back to main menu
rctaduio 0:80a695ae3cc3 381 menu(pc);
rctaduio 0:80a695ae3cc3 382 break;
rctaduio 0:80a695ae3cc3 383
rctaduio 0:80a695ae3cc3 384 default:
rctaduio 0:80a695ae3cc3 385 if ((inval >= 'a') && (inval <= 'a' + hSize)) {
rctaduio 0:80a695ae3cc3 386 tData[inval - 'a'] =! tData[inval - 'a'];
rctaduio 0:80a695ae3cc3 387 dispHeader(pc);
rctaduio 0:80a695ae3cc3 388 } else if (inval != 0) {
rctaduio 0:80a695ae3cc3 389 pc.printf("Invalid input, please try again\r\n");
rctaduio 0:80a695ae3cc3 390 configIMU(pc);
rctaduio 0:80a695ae3cc3 391 }// end of valid selection
rctaduio 0:80a695ae3cc3 392 }// END OF SWITCH
rctaduio 0:80a695ae3cc3 393 }// end of while
rctaduio 0:80a695ae3cc3 394 }// end of config IMU
rctaduio 0:80a695ae3cc3 395
rctaduio 0:80a695ae3cc3 396
rctaduio 0:80a695ae3cc3 397 /////////////////////////////////////////////////
rctaduio 0:80a695ae3cc3 398 //
rctaduio 0:80a695ae3cc3 399 // Display data packet header
rctaduio 0:80a695ae3cc3 400 //
rctaduio 0:80a695ae3cc3 401 ////////////////////////////////////////////////
rctaduio 0:80a695ae3cc3 402 void dispHeader(Serial &pc)
rctaduio 0:80a695ae3cc3 403 {
rctaduio 0:80a695ae3cc3 404 termCLS(pc);
rctaduio 0:80a695ae3cc3 405 pc.printf("Welcome to the configuration option\r\n");
rctaduio 0:80a695ae3cc3 406 pc.printf("Below are the options you can choose for sending data\r\n");
rctaduio 0:80a695ae3cc3 407 pc.printf("Please press the value you wish to enable / disable\r\n\n");
rctaduio 0:80a695ae3cc3 408 pc.printf("\t0:\tReturn to main menu\r\n\n");
rctaduio 0:80a695ae3cc3 409 for(uint8_t i = 0; i < hSize; i++) {
rctaduio 0:80a695ae3cc3 410 pc.printf("\t%c)\t[%c]\t%s\r\n", 'a' + i, (tData[i] == 0) ? ' ': 'X', header[i]);
rctaduio 0:80a695ae3cc3 411 }
rctaduio 0:80a695ae3cc3 412 } // end of dispHeader
rctaduio 0:80a695ae3cc3 413
rctaduio 0:80a695ae3cc3 414
rctaduio 0:80a695ae3cc3 415 ///////////////////////////////////////////////////
rctaduio 0:80a695ae3cc3 416 //
rctaduio 0:80a695ae3cc3 417 // getdata from imu and transfer
rctaduio 0:80a695ae3cc3 418 //
rctaduio 0:80a695ae3cc3 419 ///////////////////////////////////////////////////
rctaduio 0:80a695ae3cc3 420 void getdata(Serial &pc)
rctaduio 0:80a695ae3cc3 421 {
rctaduio 0:80a695ae3cc3 422 while(1) {
rctaduio 0:80a695ae3cc3 423 if (pc.readable())
rctaduio 0:80a695ae3cc3 424 if (pc.getc() == '0') menu(pc);
rctaduio 0:80a695ae3cc3 425 for (uint8_t i = 0; i < 8; i++) {
rctaduio 0:80a695ae3cc3 426 mux.addrSelect(i);
rctaduio 0:80a695ae3cc3 427 if (!validIMU[i]) continue;
rctaduio 0:80a695ae3cc3 428 // If intPin goes high, all data registers have new data
rctaduio 0:80a695ae3cc3 429 if(mpu9250[i].readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {
rctaduio 0:80a695ae3cc3 430 if (i==0) myled= !myled;
rctaduio 0:80a695ae3cc3 431 // On interrupt, check if data ready interrupt
rctaduio 0:80a695ae3cc3 432 uint32_t checksum = 0;
rctaduio 0:80a695ae3cc3 433 mpu9250[i].readimu();
rctaduio 1:8715b7dd52ef 434
rctaduio 1:8715b7dd52ef 435
rctaduio 0:80a695ae3cc3 436 mpu9250[i].Now = t.read_us();
rctaduio 0:80a695ae3cc3 437 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 438 mpu9250[i].lastUpdate = mpu9250[i].Now;
rctaduio 4:7fa9cfebb00c 439 //mpu9250[i].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], mpu9250[i].q[1], mpu9250[i].q[2], mpu9250[i].q[3]);
rctaduio 4:7fa9cfebb00c 440 mpu9250[i].Madgwickupdate();
rctaduio 4:7fa9cfebb00c 441 //mpu9250[i].q[0] = q0;
rctaduio 4:7fa9cfebb00c 442 //mpu9250[i].q[1] = q1;
rctaduio 4:7fa9cfebb00c 443 //mpu9250[i].q[2] = q2;
rctaduio 4:7fa9cfebb00c 444 //mpu9250[i].q[3] = q3;
rctaduio 0:80a695ae3cc3 445
rctaduio 0:80a695ae3cc3 446 delt_t = t.read_ms() - count;
rctaduio 0:80a695ae3cc3 447
rctaduio 0:80a695ae3cc3 448 mpu9250[i].tempCount = mpu9250[i].readTempData(); // Read the adc values
rctaduio 0:80a695ae3cc3 449 mpu9250[i].temperature = ((float) mpu9250[i].tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
rctaduio 0:80a695ae3cc3 450
rctaduio 0:80a695ae3cc3 451 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 452 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 453 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 454
rctaduio 0:80a695ae3cc3 455 mpu9250[i].pitch *= 180.0f / PI;
rctaduio 0:80a695ae3cc3 456 mpu9250[i].yaw *= 180.0f / PI;
rctaduio 0:80a695ae3cc3 457 mpu9250[i].roll *= 180.0f / PI;
rctaduio 1:8715b7dd52ef 458
rctaduio 1:8715b7dd52ef 459
rctaduio 1:8715b7dd52ef 460
rctaduio 0:80a695ae3cc3 461 transmit(pc);
rctaduio 0:80a695ae3cc3 462 Data[1] = 1000*mpu9250[i].ax;
rctaduio 0:80a695ae3cc3 463 Data[2] = 1000*mpu9250[i].ay;
rctaduio 0:80a695ae3cc3 464 Data[3] = 1000*mpu9250[i].az;
rctaduio 0:80a695ae3cc3 465 Data[4] = mpu9250[i].gx;
rctaduio 0:80a695ae3cc3 466 Data[5] = mpu9250[i].gy;
rctaduio 0:80a695ae3cc3 467 Data[6] = mpu9250[i].gz;
rctaduio 0:80a695ae3cc3 468 Data[7] = mpu9250[i].mx;
rctaduio 0:80a695ae3cc3 469 Data[8] = mpu9250[i].my;
rctaduio 0:80a695ae3cc3 470 Data[9] = mpu9250[i].mz;
rctaduio 1:8715b7dd52ef 471
rctaduio 0:80a695ae3cc3 472 Data[10] = mpu9250[i].temperature;
rctaduio 0:80a695ae3cc3 473 Data[11] = mpu9250[i].q[0];
rctaduio 0:80a695ae3cc3 474 Data[12] = mpu9250[i].q[1];
rctaduio 0:80a695ae3cc3 475 Data[13] = mpu9250[i].q[2];
rctaduio 0:80a695ae3cc3 476 Data[14] = mpu9250[i].q[3];
rctaduio 0:80a695ae3cc3 477 Data[15] = mpu9250[i].yaw;
rctaduio 0:80a695ae3cc3 478 Data[16] = mpu9250[i].pitch;
rctaduio 0:80a695ae3cc3 479 Data[17] = mpu9250[i].roll;
rctaduio 0:80a695ae3cc3 480 Data[18] = mpu9250[i].checksum;
rctaduio 1:8715b7dd52ef 481
rctaduio 0:80a695ae3cc3 482 Data[19] = i;
rctaduio 0:80a695ae3cc3 483
rctaduio 0:80a695ae3cc3 484 count = t.read_ms();
rctaduio 0:80a695ae3cc3 485
rctaduio 0:80a695ae3cc3 486 if(count > 1<<21) {
rctaduio 0:80a695ae3cc3 487 t.start(); // start the timer over again if ~30 minutes has passed
rctaduio 0:80a695ae3cc3 488 count = 0;
rctaduio 0:80a695ae3cc3 489 mpu9250[i].deltat= 0;
rctaduio 0:80a695ae3cc3 490 mpu9250[i].lastUpdate = t.read_us();
rctaduio 0:80a695ae3cc3 491 } // end of if
rctaduio 0:80a695ae3cc3 492 } // end of if
rctaduio 0:80a695ae3cc3 493 }// end of for
rctaduio 0:80a695ae3cc3 494 } // end of while
rctaduio 0:80a695ae3cc3 495 }// end of get data
rctaduio 0:80a695ae3cc3 496
rctaduio 0:80a695ae3cc3 497
rctaduio 0:80a695ae3cc3 498 /////////////////////////////////////////
rctaduio 0:80a695ae3cc3 499 //
rctaduio 0:80a695ae3cc3 500 // Transmit data over link
rctaduio 0:80a695ae3cc3 501 //
rctaduio 0:80a695ae3cc3 502 ////////////////////////////////////////
rctaduio 0:80a695ae3cc3 503 void transmit(Serial &pc)
rctaduio 0:80a695ae3cc3 504 {
rctaduio 0:80a695ae3cc3 505 uint8_t stuff = 0;
rctaduio 0:80a695ae3cc3 506 if(tData[0]) {
rctaduio 0:80a695ae3cc3 507 pc.printf("%u,", t.read_ms());
rctaduio 0:80a695ae3cc3 508 stuff ++;
rctaduio 0:80a695ae3cc3 509 }
rctaduio 0:80a695ae3cc3 510 for(uint8_t i = 1; i < 18; i++)
rctaduio 0:80a695ae3cc3 511 if (tData[i]) {
rctaduio 0:80a695ae3cc3 512 pc.printf("%.2f,", Data[i]);
rctaduio 0:80a695ae3cc3 513 stuff++;
rctaduio 0:80a695ae3cc3 514 }
rctaduio 0:80a695ae3cc3 515
rctaduio 0:80a695ae3cc3 516 if (tData[18]) {
rctaduio 0:80a695ae3cc3 517 pc.printf("%u,", (int32_t)Data[18]);
rctaduio 0:80a695ae3cc3 518 stuff++;
rctaduio 0:80a695ae3cc3 519 }
rctaduio 0:80a695ae3cc3 520 if (tData[19]) {
rctaduio 0:80a695ae3cc3 521 pc.printf("%u", (int32_t)Data[19]);
rctaduio 0:80a695ae3cc3 522 stuff++;
rctaduio 0:80a695ae3cc3 523 }
rctaduio 0:80a695ae3cc3 524 if (stuff >0) pc.printf("\r\n");
rctaduio 0:80a695ae3cc3 525 }