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?
User | Revision | Line number | New 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 | } |