Swimate V2 without RTOS code
Dependencies: Adafruit_GFX_128x64 DS3231 PinDetect SDFileSystem USBDevice mbed RealtimeMath MODSERIAL
Diff: main.cpp
- Revision:
- 2:a28cc676a454
- Parent:
- 0:cd1fe4f0ed39
- Child:
- 3:9aa7c37212b5
diff -r cd1fe4f0ed39 -r a28cc676a454 main.cpp --- a/main.cpp Thu May 08 20:01:10 2014 +0000 +++ b/main.cpp Thu May 08 22:27:54 2014 +0000 @@ -4,6 +4,7 @@ #include "Adafruit_SSD1306.h" #include "MPU6050_6Axis_MotionApps20.h" #include "SDFileSystem.h" +#include "misc.h" //Virtual serial port over USB #ifdef PC_DEBUG @@ -27,7 +28,12 @@ DigitalOut led(LED1); // Logging vars -FILE *logFile; +FILE *rawlog; +FILE *linlog; +FILE *worldlog; +FILE *quatlog; +FILE *gravlog; +FILE *splitlog; // Timer Timer totalTime; @@ -58,11 +64,14 @@ float euler[3]; // [psi, theta, phi] Euler angle container float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector +// Real time split detection +RingBuffer accel_buf(32); + // Forward declarations void die(int flash_rate_s); -bool log_open(); +bool log_open(FILE *lf, char *path); void log_acceleration(VectorInt16 data); -void log_close(); +void log_close(FILE *lf); void mpu_init(); void get_data(); @@ -99,7 +108,12 @@ // Start receiving data PC_PRINTLN("Opening log file..."); - if (!log_open()) { + if (!log_open(rawlog, RAW_ACCEL_FILE) || + !log_open(linlog, LIN_ACCEL_FILE) || + !log_open(worldlog, WORLD_ACCEL_FILE) || + !log_open(quatlog, QUAT_FILE) || + !log_open(gravlog, GRAV_FILE) || + !log_open(splitlog, SPLITS_FILE)) { OLED_CLEAR(); OLED_PRINTP("ERROR: SD (retry)", 0, 50); State = IDLE; @@ -116,7 +130,13 @@ get_data(); OLED_PRINTPR("Finished capture.",0,0); - log_close(); + log_open(RAW_ACCEL_FILE) || + log_open(LIN_ACCEL_FILE) || + log_open(worldlog, WORLD_ACCEL_FILE) || + log_open(quatlog, QUAT_FILE) || + log_open(gravlog, GRAV_FILE) || + log_open(splitlog, SPLITS_FILE)) { + captureTime.stop(); } @@ -135,21 +155,26 @@ } /* Returns false on failure, true otherwise */ -bool log_open() { - logFile = fopen(LOG_FILE, "a"); - if (logFile == NULL) { +FILE *log_open(FILE *lf, char *path) { + lf = fopen(path, "a"); + if (lf == NULL) { PC_PRINTLNF("SD card initialization error: Failed to open %s", LOG_FILE); - return false; + return lf; } - fprintf(logFile, "---- BEGIN NEW DATASET ----\n"); - return true; + fprintf(lf, "---- BEGIN NEW DATASET ----\n"); + return lf; } -void log_close() { - if (logFile != NULL) - fclose(logFile); +void log_close(FILE *lf) { + if (lf != NULL) + fclose(lf); } + +//void log_acceleration(VectorInt16 data) { +// fprintf(logFile, "%d, %d,%d,%d\n", captureTime.read_ms(), data.x, data.y, data.z); +//} + void mpu_init() { PC_PRINTLN("Initializing MPU"); mpu.initialize(); @@ -170,49 +195,52 @@ /* Requires the log to be open and mpu to be initialized*/ void get_data() { - //while (true) { - //if (!dmpReady) break; // do nothing if dmp not ready - if (!dmpReady) return; - - while (!mpuInterrupt && fifoCount < packetSize); - - // Reset interrupt - mpuInterrupt = false; - mpuIntStatus = mpu.getIntStatus(); - - // get current FIFO count - fifoCount = mpu.getFIFOCount(); + if (!dmpReady) return; - // check for overflow (this should never happen unless our code is too inefficient) - if ((mpuIntStatus & 0x10) || fifoCount == 1024) { - PC_PRINTLNF("**** FIFO OVERFLOW @ %d ms ****", captureTime.read_ms()); - // reset so we can continue cleanly - mpu.resetFIFO(); - // otherwise, check for DMP data ready interrupt (this should happen frequently) - } else if (mpuIntStatus & 0x02) { - // Wait for a full packet - should be very short wait - while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); + while (!mpuInterrupt && fifoCount < packetSize); - // read a packet from FIFO - mpu.getFIFOBytes(fifoBuffer, packetSize); - fifoCount -= packetSize; - - // Get acceleration data - mpu.dmpGetAccel(&aa, fifoBuffer); - mpu.dmpGetQuaternion(&q, fifoBuffer); - mpu.dmpGetGravity(&gravity, &q); - mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); - mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q); - - PC_PRINTF("%d, ", aaWorld.x); PC_PRINTF("%d, ", aaWorld.y); PC_PRINTLNF("%d", aaWorld.z); - -// OLED_SETCURS(0, 10); OLED_PRINTF("%d, ", aaWorld.x); OLED_PRINTF("%d, ", aaWorld.y); OLED_PRINTLNF("%d", aaWorld.z); - - log_acceleration(aaWorld); - //} + // Reset interrupt + mpuInterrupt = false; + mpuIntStatus = mpu.getIntStatus(); + + // get current FIFO count + fifoCount = mpu.getFIFOCount(); + + // check for overflow (this should never happen unless our code is too inefficient) + if ((mpuIntStatus & 0x10) || fifoCount == 1024) { + PC_PRINTLNF("**** FIFO OVERFLOW @ %d ms ****", captureTime.read_ms()); + // reset so we can continue cleanly + mpu.resetFIFO(); + // otherwise, check for DMP data ready interrupt (this should happen frequently) + } else if (mpuIntStatus & 0x02) { + // Wait for a full packet - should be very short wait + while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); + + // read a packet from FIFO + mpu.getFIFOBytes(fifoBuffer, packetSize); + fifoCount -= packetSize; + + // Get acceleration data + mpu.dmpGetAccel(&aa, fifoBuffer); + mpu.dmpGetQuaternion(&q, fifoBuffer); + mpu.dmpGetGravity(&gravity, &q); + mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); + mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q); + + accel_buf.put(aa.x); + + int currtime = captureTime.read_ms(); + + if (mov_avg(accel_buf)) + fprintf(splitlog, "%d", currtime); + fprintf(rawlog, "%d,%d,%d,%d", currtime, aa.x, aa.y, aa.z); + fprintf(linlog, "%d,%d,%d,%d", currtime, aaReal.x, aaReal.y, aaReal.z); + fprintf(worldlog, "%d,%d,%d,%d", currtime, aaWorld.x, aaWorld.y, aaWorld.z); + fprintf(quatlog, "%d,%f,%f,%f,%f", currtime, q.x, q.y, q.z, q.w); + fprintf(gravlog, "%d,%f,%f,%f", currtime, gravity.x, gravity.y, gravity.z); + + PC_PRINTF("%d, ", aaWorld.x); PC_PRINTF("%d, ", aaWorld.y); PC_PRINTLNF("%d", aaWorld.z); + +// OLED_SETCURS(0, 10); OLED_PRINTF("%d, ", aaWorld.x); OLED_PRINTF("%d, ", aaWorld.y); OLED_PRINTLNF("%d", aaWorld.z); } -} - -void log_acceleration(VectorInt16 data) { - fprintf(logFile, "%d, %d,%d,%d\n", captureTime.read_ms(), data.x, data.y, data.z); } \ No newline at end of file