Diff: main.cpp

Revision:

2:a28cc676a454

Parent:

0:cd1fe4f0ed39

Child:

3:9aa7c37212b5

--- 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