Patrick Ciccone / Mbed 2 deprecated IMU_ethernet

IMU Ethernet initial commit

Dependencies:   F7_Ethernet MadgwickAHRS mbed

main.cpp@1:7d5d767744cd, 2016-10-06 (annotated)

Committer:
rctaduio
Date:
Thu Oct 06 16:57:03 2016 +0000
Revision:
1:7d5d767744cd
Parent:
0:80a695ae3cc3 
IMU Ethernet initial commit;

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