CSE477 / swimate_v2

Swimate V2 without RTOS code

Dependencies:   Adafruit_GFX_128x64 DS3231 PinDetect SDFileSystem USBDevice mbed RealtimeMath MODSERIAL

main.cpp@3:9aa7c37212b5, 2014-05-09 (annotated)

Committer:
ellingjp
Date:
Fri May 09 00:04:44 2014 +0000
Revision:
3:9aa7c37212b5
Parent:
2:a28cc676a454 
Fixes for real time analysis, still doesn't seem to be working right

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ellingjp 0:cd1fe4f0ed39 1 #include "main.h"
ellingjp 0:cd1fe4f0ed39 2 #include "mbed.h"
ellingjp 0:cd1fe4f0ed39 3 #include "USBSerial.h"
ellingjp 0:cd1fe4f0ed39 4 #include "Adafruit_SSD1306.h"
ellingjp 0:cd1fe4f0ed39 5 #include "MPU6050_6Axis_MotionApps20.h"
ellingjp 0:cd1fe4f0ed39 6 #include "SDFileSystem.h"
ellingjp 2:a28cc676a454 7 #include "misc.h"
ellingjp 0:cd1fe4f0ed39 8
ellingjp 0:cd1fe4f0ed39 9 //Virtual serial port over USB
ellingjp 0:cd1fe4f0ed39 10 #ifdef PC_DEBUG
ellingjp 0:cd1fe4f0ed39 11 USBSerial pc;
ellingjp 0:cd1fe4f0ed39 12 #endif
ellingjp 0:cd1fe4f0ed39 13
ellingjp 0:cd1fe4f0ed39 14 // Display
ellingjp 0:cd1fe4f0ed39 15 #ifdef OLED_DEBUG
ellingjp 0:cd1fe4f0ed39 16 SPI spi0(P0_9, NC, P0_10); // mosi, miso, sclk
ellingjp 0:cd1fe4f0ed39 17 Adafruit_SSD1306 oled(spi0, P0_11, P0_12, P0_13); // MOSI, MISO, SCLK SPI0
ellingjp 0:cd1fe4f0ed39 18 #endif
ellingjp 0:cd1fe4f0ed39 19
ellingjp 0:cd1fe4f0ed39 20 // MPU
ellingjp 0:cd1fe4f0ed39 21 MPU6050 mpu;
ellingjp 0:cd1fe4f0ed39 22 InterruptIn dataReady(P0_15);
ellingjp 0:cd1fe4f0ed39 23
ellingjp 0:cd1fe4f0ed39 24 // SD Card
ellingjp 0:cd1fe4f0ed39 25 SDFileSystem sd(P0_21, P0_22, P1_15, P1_19, "sd"); // MOSI, MISO, SCLK, SSEL SPI1
ellingjp 0:cd1fe4f0ed39 26
ellingjp 0:cd1fe4f0ed39 27 // LED for debug
ellingjp 0:cd1fe4f0ed39 28 DigitalOut led(LED1);
ellingjp 0:cd1fe4f0ed39 29
ellingjp 0:cd1fe4f0ed39 30 // Logging vars
ellingjp 2:a28cc676a454 31 FILE *rawlog;
ellingjp 3:9aa7c37212b5 32 //FILE *linlog;
ellingjp 3:9aa7c37212b5 33 //FILE *worldlog;
ellingjp 2:a28cc676a454 34 FILE *quatlog;
ellingjp 3:9aa7c37212b5 35 //FILE *gravlog;
ellingjp 2:a28cc676a454 36 FILE *splitlog;
ellingjp 0:cd1fe4f0ed39 37
ellingjp 0:cd1fe4f0ed39 38 // Timer
ellingjp 0:cd1fe4f0ed39 39 Timer totalTime;
ellingjp 0:cd1fe4f0ed39 40 Timer captureTime;
ellingjp 0:cd1fe4f0ed39 41
ellingjp 0:cd1fe4f0ed39 42 // Switch
ellingjp 0:cd1fe4f0ed39 43 InterruptIn captureSwitch(P0_16);
ellingjp 0:cd1fe4f0ed39 44
ellingjp 0:cd1fe4f0ed39 45
ellingjp 0:cd1fe4f0ed39 46 // State
ellingjp 0:cd1fe4f0ed39 47 enum state {IDLE, CAPTURE};
ellingjp 0:cd1fe4f0ed39 48 enum state State;
ellingjp 0:cd1fe4f0ed39 49
ellingjp 0:cd1fe4f0ed39 50 // MPU control/status vars
ellingjp 0:cd1fe4f0ed39 51 bool dmpReady = false; // set true if DMP init was successful
ellingjp 0:cd1fe4f0ed39 52 uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
ellingjp 0:cd1fe4f0ed39 53 uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
ellingjp 0:cd1fe4f0ed39 54 uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
ellingjp 0:cd1fe4f0ed39 55 uint16_t fifoCount; // count of all bytes currently in FIFO
ellingjp 0:cd1fe4f0ed39 56 uint8_t fifoBuffer[64]; // FIFO storage buffer
ellingjp 0:cd1fe4f0ed39 57
ellingjp 0:cd1fe4f0ed39 58 // orientation/motion vars
ellingjp 0:cd1fe4f0ed39 59 Quaternion q; // [w, x, y, z] quaternion container
ellingjp 0:cd1fe4f0ed39 60 VectorInt16 aa; // [x, y, z] accel sensor measurements
ellingjp 0:cd1fe4f0ed39 61 VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
ellingjp 0:cd1fe4f0ed39 62 VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
ellingjp 0:cd1fe4f0ed39 63 VectorFloat gravity; // [x, y, z] gravity vector
ellingjp 0:cd1fe4f0ed39 64 float euler[3]; // [psi, theta, phi] Euler angle container
ellingjp 0:cd1fe4f0ed39 65 float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
ellingjp 0:cd1fe4f0ed39 66
ellingjp 2:a28cc676a454 67 // Real time split detection
ellingjp 2:a28cc676a454 68 RingBuffer accel_buf(32);
ellingjp 2:a28cc676a454 69
ellingjp 0:cd1fe4f0ed39 70 // Forward declarations
ellingjp 0:cd1fe4f0ed39 71 void die(int flash_rate_s);
ellingjp 3:9aa7c37212b5 72 FILE *log_open(char *path);
ellingjp 0:cd1fe4f0ed39 73 void log_acceleration(VectorInt16 data);
ellingjp 2:a28cc676a454 74 void log_close(FILE *lf);
ellingjp 0:cd1fe4f0ed39 75 void mpu_init();
ellingjp 0:cd1fe4f0ed39 76 void get_data();
ellingjp 0:cd1fe4f0ed39 77
ellingjp 0:cd1fe4f0ed39 78 volatile bool mpuInterrupt = false;
ellingjp 0:cd1fe4f0ed39 79 void dataReadyISR() {
ellingjp 0:cd1fe4f0ed39 80 mpuInterrupt = true;
ellingjp 0:cd1fe4f0ed39 81 }
ellingjp 0:cd1fe4f0ed39 82
ellingjp 0:cd1fe4f0ed39 83 void captureSwitchISR() {
ellingjp 0:cd1fe4f0ed39 84 // used for debouncing
ellingjp 0:cd1fe4f0ed39 85 static int prev_time = 0;
ellingjp 0:cd1fe4f0ed39 86
ellingjp 0:cd1fe4f0ed39 87 if (totalTime.read_ms() - prev_time < 200)
ellingjp 0:cd1fe4f0ed39 88 return;
ellingjp 0:cd1fe4f0ed39 89
ellingjp 0:cd1fe4f0ed39 90 State = (State == IDLE) ? CAPTURE : IDLE;
ellingjp 0:cd1fe4f0ed39 91 prev_time = totalTime.read_ms();
ellingjp 0:cd1fe4f0ed39 92 }
ellingjp 0:cd1fe4f0ed39 93
ellingjp 0:cd1fe4f0ed39 94 int main(void)
ellingjp 0:cd1fe4f0ed39 95 {
ellingjp 0:cd1fe4f0ed39 96 totalTime.start();
ellingjp 0:cd1fe4f0ed39 97
ellingjp 0:cd1fe4f0ed39 98 State = IDLE;
ellingjp 0:cd1fe4f0ed39 99 captureSwitch.mode(PullUp);
ellingjp 0:cd1fe4f0ed39 100 captureSwitch.rise(captureSwitchISR);
ellingjp 0:cd1fe4f0ed39 101
ellingjp 0:cd1fe4f0ed39 102 OLED_PRINTP("Waiting to capture. ",0,0);
ellingjp 0:cd1fe4f0ed39 103 while (true) {
ellingjp 0:cd1fe4f0ed39 104 if (State == CAPTURE) {
ellingjp 0:cd1fe4f0ed39 105 PC_PRINTLN("Start capture button pressed!");
ellingjp 0:cd1fe4f0ed39 106 OLED_CLEAR();
ellingjp 0:cd1fe4f0ed39 107 OLED_PRINTPR("Starting capture. ",0,0);
ellingjp 0:cd1fe4f0ed39 108
ellingjp 0:cd1fe4f0ed39 109 // Start receiving data
ellingjp 3:9aa7c37212b5 110 // PC_PRINTLN("Opening log file...");
ellingjp 3:9aa7c37212b5 111 // if (!(rawlog = log_open(RAW_ACCEL_FILE)) ||
ellingjp 3:9aa7c37212b5 112 // !(linlog = log_open(LIN_ACCEL_FILE)) ||
ellingjp 3:9aa7c37212b5 113 // !(worldlog = log_open(WORLD_ACCEL_FILE)) ||
ellingjp 3:9aa7c37212b5 114 // !(quatlog = log_open(QUAT_FILE)) ||
ellingjp 3:9aa7c37212b5 115 // !(gravlog = log_open(GRAV_FILE)) ||
ellingjp 3:9aa7c37212b5 116 // !(splitlog = log_open(SPLITS_FILE)) ) {
ellingjp 3:9aa7c37212b5 117 // OLED_CLEAR();
ellingjp 3:9aa7c37212b5 118 // OLED_PRINTP("ERROR: SD (retry)", 0, 50);
ellingjp 3:9aa7c37212b5 119 // State = IDLE;
ellingjp 3:9aa7c37212b5 120 // continue;
ellingjp 3:9aa7c37212b5 121 // }
ellingjp 0:cd1fe4f0ed39 122
ellingjp 0:cd1fe4f0ed39 123 PC_PRINTLN("Initializing MPU...");
ellingjp 0:cd1fe4f0ed39 124 mpu_init();
ellingjp 0:cd1fe4f0ed39 125
ellingjp 0:cd1fe4f0ed39 126 PC_PRINTLN("Starting capture...");
ellingjp 0:cd1fe4f0ed39 127 OLED_PRINTPR("Capturing data... ",0,0);
ellingjp 0:cd1fe4f0ed39 128 captureTime.start();
ellingjp 0:cd1fe4f0ed39 129 while (State == CAPTURE)
ellingjp 0:cd1fe4f0ed39 130 get_data();
ellingjp 0:cd1fe4f0ed39 131 OLED_PRINTPR("Finished capture.",0,0);
ellingjp 0:cd1fe4f0ed39 132
ellingjp 3:9aa7c37212b5 133 // log_close(rawlog);
ellingjp 3:9aa7c37212b5 134 // log_close(linlog);
ellingjp 3:9aa7c37212b5 135 // log_close(worldlog);
ellingjp 3:9aa7c37212b5 136 // log_close(quatlog);
ellingjp 3:9aa7c37212b5 137 // log_close(gravlog);
ellingjp 3:9aa7c37212b5 138 // log_close(splitlog);
ellingjp 2:a28cc676a454 139
ellingjp 0:cd1fe4f0ed39 140 captureTime.stop();
ellingjp 3:9aa7c37212b5 141 captureTime.reset();
ellingjp 0:cd1fe4f0ed39 142 }
ellingjp 0:cd1fe4f0ed39 143
ellingjp 0:cd1fe4f0ed39 144 PC_PRINTLN("Idling...");
ellingjp 0:cd1fe4f0ed39 145 }
ellingjp 0:cd1fe4f0ed39 146 }
ellingjp 0:cd1fe4f0ed39 147
ellingjp 0:cd1fe4f0ed39 148 /* Halts program and flashes LED at specified rate */
ellingjp 0:cd1fe4f0ed39 149 void die(int flash_rate_s) {
ellingjp 0:cd1fe4f0ed39 150 while (1) {
ellingjp 0:cd1fe4f0ed39 151 led = 1;
ellingjp 0:cd1fe4f0ed39 152 wait(flash_rate_s/2);
ellingjp 0:cd1fe4f0ed39 153 led = 0;
ellingjp 0:cd1fe4f0ed39 154 wait(flash_rate_s/2);
ellingjp 0:cd1fe4f0ed39 155 }
ellingjp 0:cd1fe4f0ed39 156 }
ellingjp 0:cd1fe4f0ed39 157
ellingjp 0:cd1fe4f0ed39 158 /* Returns false on failure, true otherwise */
ellingjp 3:9aa7c37212b5 159 FILE *log_open(char *path) {
ellingjp 3:9aa7c37212b5 160 FILE *lf = fopen(path, "a");
ellingjp 2:a28cc676a454 161 if (lf == NULL) {
ellingjp 3:9aa7c37212b5 162 PC_PRINTLNF("SD card initialization error: Failed to open %s", path);
ellingjp 2:a28cc676a454 163 return lf;
ellingjp 0:cd1fe4f0ed39 164 }
ellingjp 2:a28cc676a454 165 fprintf(lf, "---- BEGIN NEW DATASET ----\n");
ellingjp 2:a28cc676a454 166 return lf;
ellingjp 0:cd1fe4f0ed39 167 }
ellingjp 0:cd1fe4f0ed39 168
ellingjp 2:a28cc676a454 169 void log_close(FILE *lf) {
ellingjp 2:a28cc676a454 170 if (lf != NULL)
ellingjp 2:a28cc676a454 171 fclose(lf);
ellingjp 0:cd1fe4f0ed39 172 }
ellingjp 0:cd1fe4f0ed39 173
ellingjp 2:a28cc676a454 174
ellingjp 2:a28cc676a454 175 //void log_acceleration(VectorInt16 data) {
ellingjp 2:a28cc676a454 176 // fprintf(logFile, "%d, %d,%d,%d\n", captureTime.read_ms(), data.x, data.y, data.z);
ellingjp 2:a28cc676a454 177 //}
ellingjp 2:a28cc676a454 178
ellingjp 0:cd1fe4f0ed39 179 void mpu_init() {
ellingjp 0:cd1fe4f0ed39 180 PC_PRINTLN("Initializing MPU");
ellingjp 0:cd1fe4f0ed39 181 mpu.initialize();
ellingjp 0:cd1fe4f0ed39 182 devStatus = mpu.dmpInitialize();
ellingjp 0:cd1fe4f0ed39 183
ellingjp 0:cd1fe4f0ed39 184 if (devStatus == 0) {
ellingjp 0:cd1fe4f0ed39 185 mpu.setDMPEnabled(true);
ellingjp 0:cd1fe4f0ed39 186 packetSize = mpu.dmpGetFIFOPacketSize();
ellingjp 0:cd1fe4f0ed39 187
ellingjp 0:cd1fe4f0ed39 188 PC_PRINTLN("DMP Initialized successfully!");
ellingjp 0:cd1fe4f0ed39 189 dmpReady = true;
ellingjp 0:cd1fe4f0ed39 190 dataReady.rise(dataReadyISR);
ellingjp 0:cd1fe4f0ed39 191 } else { // ERROR
ellingjp 0:cd1fe4f0ed39 192 PC_PRINTLNF("Error initializing MPU (code %d)", devStatus);
ellingjp 0:cd1fe4f0ed39 193 die(DMP_ERROR_RATE);
ellingjp 0:cd1fe4f0ed39 194 }
ellingjp 0:cd1fe4f0ed39 195 }
ellingjp 0:cd1fe4f0ed39 196
ellingjp 0:cd1fe4f0ed39 197 /* Requires the log to be open and mpu to be initialized*/
ellingjp 0:cd1fe4f0ed39 198 void get_data() {
ellingjp 2:a28cc676a454 199 if (!dmpReady) return;
ellingjp 0:cd1fe4f0ed39 200
ellingjp 2:a28cc676a454 201 while (!mpuInterrupt && fifoCount < packetSize);
ellingjp 0:cd1fe4f0ed39 202
ellingjp 2:a28cc676a454 203 // Reset interrupt
ellingjp 2:a28cc676a454 204 mpuInterrupt = false;
ellingjp 2:a28cc676a454 205 mpuIntStatus = mpu.getIntStatus();
ellingjp 2:a28cc676a454 206
ellingjp 2:a28cc676a454 207 // get current FIFO count
ellingjp 2:a28cc676a454 208 fifoCount = mpu.getFIFOCount();
ellingjp 2:a28cc676a454 209
ellingjp 2:a28cc676a454 210 // check for overflow (this should never happen unless our code is too inefficient)
ellingjp 2:a28cc676a454 211 if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
ellingjp 2:a28cc676a454 212 PC_PRINTLNF("**** FIFO OVERFLOW @ %d ms ****", captureTime.read_ms());
ellingjp 2:a28cc676a454 213 // reset so we can continue cleanly
ellingjp 2:a28cc676a454 214 mpu.resetFIFO();
ellingjp 2:a28cc676a454 215 // otherwise, check for DMP data ready interrupt (this should happen frequently)
ellingjp 2:a28cc676a454 216 } else if (mpuIntStatus & 0x02) {
ellingjp 2:a28cc676a454 217 // Wait for a full packet - should be very short wait
ellingjp 2:a28cc676a454 218 while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
ellingjp 2:a28cc676a454 219
ellingjp 2:a28cc676a454 220 // read a packet from FIFO
ellingjp 2:a28cc676a454 221 mpu.getFIFOBytes(fifoBuffer, packetSize);
ellingjp 2:a28cc676a454 222 fifoCount -= packetSize;
ellingjp 2:a28cc676a454 223
ellingjp 2:a28cc676a454 224 // Get acceleration data
ellingjp 2:a28cc676a454 225 mpu.dmpGetAccel(&aa, fifoBuffer);
ellingjp 2:a28cc676a454 226 mpu.dmpGetQuaternion(&q, fifoBuffer);
ellingjp 2:a28cc676a454 227 mpu.dmpGetGravity(&gravity, &q);
ellingjp 2:a28cc676a454 228 mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
ellingjp 2:a28cc676a454 229 mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
ellingjp 2:a28cc676a454 230
ellingjp 2:a28cc676a454 231 accel_buf.put(aa.x);
ellingjp 2:a28cc676a454 232
ellingjp 2:a28cc676a454 233 int currtime = captureTime.read_ms();
ellingjp 2:a28cc676a454 234
ellingjp 3:9aa7c37212b5 235 bool print_split = false;
ellingjp 3:9aa7c37212b5 236 if (mov_avg(accel_buf)) {
ellingjp 3:9aa7c37212b5 237 // fprintf(splitlog, "%d", currtime);
ellingjp 3:9aa7c37212b5 238 print_split = true;
ellingjp 3:9aa7c37212b5 239 }
ellingjp 3:9aa7c37212b5 240 // fprintf(rawlog, "%d,%d,%d,%d", currtime, aa.x, aa.y, aa.z);
ellingjp 3:9aa7c37212b5 241 // fprintf(linlog, "%d,%d,%d,%d", currtime, aaReal.x, aaReal.y, aaReal.z);
ellingjp 3:9aa7c37212b5 242 // fprintf(worldlog, "%d,%d,%d,%d", currtime, aaWorld.x, aaWorld.y, aaWorld.z);
ellingjp 3:9aa7c37212b5 243 // fprintf(quatlog, "%d,%f,%f,%f,%f", currtime, q.x, q.y, q.z, q.w);
ellingjp 3:9aa7c37212b5 244 // fprintf(gravlog, "%d,%f,%f,%f", currtime, gravity.x, gravity.y, gravity.z);
ellingjp 2:a28cc676a454 245
ellingjp 3:9aa7c37212b5 246 // PC_PRINTF("%d, ", aa.x); PC_PRINTF("%d, ", aa.y); PC_PRINTLNF("%d, ", aa.z);
ellingjp 3:9aa7c37212b5 247 if (print_split) PC_PRINTLNF("%d", currtime);
ellingjp 2:a28cc676a454 248
ellingjp 2:a28cc676a454 249 // OLED_SETCURS(0, 10); OLED_PRINTF("%d, ", aaWorld.x); OLED_PRINTF("%d, ", aaWorld.y); OLED_PRINTLNF("%d", aaWorld.z);
ellingjp 0:cd1fe4f0ed39 250 }
ellingjp 0:cd1fe4f0ed39 251 }