CSE477 / swimate_v2

Swimate V2 without RTOS code

Dependencies:   Adafruit_GFX_128x64 DS3231 PinDetect SDFileSystem USBDevice mbed RealtimeMath MODSERIAL

main.cpp@2:a28cc676a454, 2014-05-08 (annotated)

Committer:
ellingjp
Date:
Thu May 08 22:27:54 2014 +0000
Revision:
2:a28cc676a454
Parent:
0:cd1fe4f0ed39
Child:
3:9aa7c37212b5
Initial commit using real time split analysis

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 2:a28cc676a454 32 FILE *linlog;
ellingjp 2:a28cc676a454 33 FILE *worldlog;
ellingjp 2:a28cc676a454 34 FILE *quatlog;
ellingjp 2:a28cc676a454 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 2:a28cc676a454 72 bool log_open(FILE *lf, 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 0:cd1fe4f0ed39 110 PC_PRINTLN("Opening log file...");
ellingjp 2:a28cc676a454 111 if (!log_open(rawlog, RAW_ACCEL_FILE) ||
ellingjp 2:a28cc676a454 112 !log_open(linlog, LIN_ACCEL_FILE) ||
ellingjp 2:a28cc676a454 113 !log_open(worldlog, WORLD_ACCEL_FILE) ||
ellingjp 2:a28cc676a454 114 !log_open(quatlog, QUAT_FILE) ||
ellingjp 2:a28cc676a454 115 !log_open(gravlog, GRAV_FILE) ||
ellingjp 2:a28cc676a454 116 !log_open(splitlog, SPLITS_FILE)) {
ellingjp 0:cd1fe4f0ed39 117 OLED_CLEAR();
ellingjp 0:cd1fe4f0ed39 118 OLED_PRINTP("ERROR: SD (retry)", 0, 50);
ellingjp 0:cd1fe4f0ed39 119 State = IDLE;
ellingjp 0:cd1fe4f0ed39 120 continue;
ellingjp 0:cd1fe4f0ed39 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 2:a28cc676a454 133 log_open(RAW_ACCEL_FILE) ||
ellingjp 2:a28cc676a454 134 log_open(LIN_ACCEL_FILE) ||
ellingjp 2:a28cc676a454 135 log_open(worldlog, WORLD_ACCEL_FILE) ||
ellingjp 2:a28cc676a454 136 log_open(quatlog, QUAT_FILE) ||
ellingjp 2:a28cc676a454 137 log_open(gravlog, GRAV_FILE) ||
ellingjp 2:a28cc676a454 138 log_open(splitlog, SPLITS_FILE)) {
ellingjp 2:a28cc676a454 139
ellingjp 0:cd1fe4f0ed39 140 captureTime.stop();
ellingjp 0:cd1fe4f0ed39 141 }
ellingjp 0:cd1fe4f0ed39 142
ellingjp 0:cd1fe4f0ed39 143 PC_PRINTLN("Idling...");
ellingjp 0:cd1fe4f0ed39 144 }
ellingjp 0:cd1fe4f0ed39 145 }
ellingjp 0:cd1fe4f0ed39 146
ellingjp 0:cd1fe4f0ed39 147 /* Halts program and flashes LED at specified rate */
ellingjp 0:cd1fe4f0ed39 148 void die(int flash_rate_s) {
ellingjp 0:cd1fe4f0ed39 149 while (1) {
ellingjp 0:cd1fe4f0ed39 150 led = 1;
ellingjp 0:cd1fe4f0ed39 151 wait(flash_rate_s/2);
ellingjp 0:cd1fe4f0ed39 152 led = 0;
ellingjp 0:cd1fe4f0ed39 153 wait(flash_rate_s/2);
ellingjp 0:cd1fe4f0ed39 154 }
ellingjp 0:cd1fe4f0ed39 155 }
ellingjp 0:cd1fe4f0ed39 156
ellingjp 0:cd1fe4f0ed39 157 /* Returns false on failure, true otherwise */
ellingjp 2:a28cc676a454 158 FILE *log_open(FILE *lf, char *path) {
ellingjp 2:a28cc676a454 159 lf = fopen(path, "a");
ellingjp 2:a28cc676a454 160 if (lf == NULL) {
ellingjp 0:cd1fe4f0ed39 161 PC_PRINTLNF("SD card initialization error: Failed to open %s", LOG_FILE);
ellingjp 2:a28cc676a454 162 return lf;
ellingjp 0:cd1fe4f0ed39 163 }
ellingjp 2:a28cc676a454 164 fprintf(lf, "---- BEGIN NEW DATASET ----\n");
ellingjp 2:a28cc676a454 165 return lf;
ellingjp 0:cd1fe4f0ed39 166 }
ellingjp 0:cd1fe4f0ed39 167
ellingjp 2:a28cc676a454 168 void log_close(FILE *lf) {
ellingjp 2:a28cc676a454 169 if (lf != NULL)
ellingjp 2:a28cc676a454 170 fclose(lf);
ellingjp 0:cd1fe4f0ed39 171 }
ellingjp 0:cd1fe4f0ed39 172
ellingjp 2:a28cc676a454 173
ellingjp 2:a28cc676a454 174 //void log_acceleration(VectorInt16 data) {
ellingjp 2:a28cc676a454 175 // fprintf(logFile, "%d, %d,%d,%d\n", captureTime.read_ms(), data.x, data.y, data.z);
ellingjp 2:a28cc676a454 176 //}
ellingjp 2:a28cc676a454 177
ellingjp 0:cd1fe4f0ed39 178 void mpu_init() {
ellingjp 0:cd1fe4f0ed39 179 PC_PRINTLN("Initializing MPU");
ellingjp 0:cd1fe4f0ed39 180 mpu.initialize();
ellingjp 0:cd1fe4f0ed39 181 devStatus = mpu.dmpInitialize();
ellingjp 0:cd1fe4f0ed39 182
ellingjp 0:cd1fe4f0ed39 183 if (devStatus == 0) {
ellingjp 0:cd1fe4f0ed39 184 mpu.setDMPEnabled(true);
ellingjp 0:cd1fe4f0ed39 185 packetSize = mpu.dmpGetFIFOPacketSize();
ellingjp 0:cd1fe4f0ed39 186
ellingjp 0:cd1fe4f0ed39 187 PC_PRINTLN("DMP Initialized successfully!");
ellingjp 0:cd1fe4f0ed39 188 dmpReady = true;
ellingjp 0:cd1fe4f0ed39 189 dataReady.rise(dataReadyISR);
ellingjp 0:cd1fe4f0ed39 190 } else { // ERROR
ellingjp 0:cd1fe4f0ed39 191 PC_PRINTLNF("Error initializing MPU (code %d)", devStatus);
ellingjp 0:cd1fe4f0ed39 192 die(DMP_ERROR_RATE);
ellingjp 0:cd1fe4f0ed39 193 }
ellingjp 0:cd1fe4f0ed39 194 }
ellingjp 0:cd1fe4f0ed39 195
ellingjp 0:cd1fe4f0ed39 196 /* Requires the log to be open and mpu to be initialized*/
ellingjp 0:cd1fe4f0ed39 197 void get_data() {
ellingjp 2:a28cc676a454 198 if (!dmpReady) return;
ellingjp 0:cd1fe4f0ed39 199
ellingjp 2:a28cc676a454 200 while (!mpuInterrupt && fifoCount < packetSize);
ellingjp 0:cd1fe4f0ed39 201
ellingjp 2:a28cc676a454 202 // Reset interrupt
ellingjp 2:a28cc676a454 203 mpuInterrupt = false;
ellingjp 2:a28cc676a454 204 mpuIntStatus = mpu.getIntStatus();
ellingjp 2:a28cc676a454 205
ellingjp 2:a28cc676a454 206 // get current FIFO count
ellingjp 2:a28cc676a454 207 fifoCount = mpu.getFIFOCount();
ellingjp 2:a28cc676a454 208
ellingjp 2:a28cc676a454 209 // check for overflow (this should never happen unless our code is too inefficient)
ellingjp 2:a28cc676a454 210 if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
ellingjp 2:a28cc676a454 211 PC_PRINTLNF("**** FIFO OVERFLOW @ %d ms ****", captureTime.read_ms());
ellingjp 2:a28cc676a454 212 // reset so we can continue cleanly
ellingjp 2:a28cc676a454 213 mpu.resetFIFO();
ellingjp 2:a28cc676a454 214 // otherwise, check for DMP data ready interrupt (this should happen frequently)
ellingjp 2:a28cc676a454 215 } else if (mpuIntStatus & 0x02) {
ellingjp 2:a28cc676a454 216 // Wait for a full packet - should be very short wait
ellingjp 2:a28cc676a454 217 while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
ellingjp 2:a28cc676a454 218
ellingjp 2:a28cc676a454 219 // read a packet from FIFO
ellingjp 2:a28cc676a454 220 mpu.getFIFOBytes(fifoBuffer, packetSize);
ellingjp 2:a28cc676a454 221 fifoCount -= packetSize;
ellingjp 2:a28cc676a454 222
ellingjp 2:a28cc676a454 223 // Get acceleration data
ellingjp 2:a28cc676a454 224 mpu.dmpGetAccel(&aa, fifoBuffer);
ellingjp 2:a28cc676a454 225 mpu.dmpGetQuaternion(&q, fifoBuffer);
ellingjp 2:a28cc676a454 226 mpu.dmpGetGravity(&gravity, &q);
ellingjp 2:a28cc676a454 227 mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
ellingjp 2:a28cc676a454 228 mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
ellingjp 2:a28cc676a454 229
ellingjp 2:a28cc676a454 230 accel_buf.put(aa.x);
ellingjp 2:a28cc676a454 231
ellingjp 2:a28cc676a454 232 int currtime = captureTime.read_ms();
ellingjp 2:a28cc676a454 233
ellingjp 2:a28cc676a454 234 if (mov_avg(accel_buf))
ellingjp 2:a28cc676a454 235 fprintf(splitlog, "%d", currtime);
ellingjp 2:a28cc676a454 236 fprintf(rawlog, "%d,%d,%d,%d", currtime, aa.x, aa.y, aa.z);
ellingjp 2:a28cc676a454 237 fprintf(linlog, "%d,%d,%d,%d", currtime, aaReal.x, aaReal.y, aaReal.z);
ellingjp 2:a28cc676a454 238 fprintf(worldlog, "%d,%d,%d,%d", currtime, aaWorld.x, aaWorld.y, aaWorld.z);
ellingjp 2:a28cc676a454 239 fprintf(quatlog, "%d,%f,%f,%f,%f", currtime, q.x, q.y, q.z, q.w);
ellingjp 2:a28cc676a454 240 fprintf(gravlog, "%d,%f,%f,%f", currtime, gravity.x, gravity.y, gravity.z);
ellingjp 2:a28cc676a454 241
ellingjp 2:a28cc676a454 242 PC_PRINTF("%d, ", aaWorld.x); PC_PRINTF("%d, ", aaWorld.y); PC_PRINTLNF("%d", aaWorld.z);
ellingjp 2:a28cc676a454 243
ellingjp 2:a28cc676a454 244 // OLED_SETCURS(0, 10); OLED_PRINTF("%d, ", aaWorld.x); OLED_PRINTF("%d, ", aaWorld.y); OLED_PRINTLNF("%d", aaWorld.z);
ellingjp 0:cd1fe4f0ed39 245 }
ellingjp 0:cd1fe4f0ed39 246 }