Revision 5:7c2e7d657716, committed 2014-05-14

Comitter:

ellingjp

Date:

Wed May 14 03:14:57 2014 +0000

Parent:

4:b962f5a783a1

Child:

6:3b9b4e2c29bf

Commit message:

rtos integration

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/capture.cpp	Wed May 14 03:14:57 2014 +0000
@@ -0,0 +1,152 @@
+#include "mbed.h"
+#include "capture.h"
+#include "MPU6050_6Axis_MotionApps20.h"
+#include "helper_3dmath.h"
+#include "SDFileSystem.h"
+#include "debug.h"
+
+// MPU
+MPU6050 mpu;
+InterruptIn dataReady(P0_15);
+
+// SD Card
+SDFileSystem sd(P0_21, P0_22, P1_15, P1_19, "sd"); // MOSI, MISO, SCLK, SSEL SPI1
+
+// LED for debug
+DigitalOut led(LED1);
+
+// Logging vars
+FILE *logFile;
+
+// Timer
+Timer captureTime;
+
+// MPU control/status vars
+bool dmpReady = false;  // set true if DMP init was successful
+uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
+uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
+uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
+uint16_t fifoCount;     // count of all bytes currently in FIFO
+uint8_t fifoBuffer[64]; // FIFO storage buffer
+
+// orientation/motion vars
+Quaternion q;           // [w, x, y, z]         quaternion container
+VectorInt16 aa;         // [x, y, z]            accel sensor measurements
+VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
+VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
+VectorFloat gravity;    // [x, y, z]            gravity vector
+float euler[3];         // [psi, theta, phi]    Euler angle container
+float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
+
+volatile bool mpuInterrupt = false;
+void dataReadyISR() {
+    mpuInterrupt = true;
+}
+
+/* Halts program and flashes LED at specified rate */
+void die(int flash_rate_s) {
+    while (1) {
+        led = 1;
+        wait(flash_rate_s/2);
+        led = 0;
+        wait(flash_rate_s/2);
+    }
+}
+
+/* Returns false on failure, true otherwise */
+bool log_open() {
+    PC_PRINTLN("WTF");
+    logFile = fopen(LOG_FILE, "w");
+    PC_PRINTLN("WTF2");
+    if (logFile == NULL) {
+        PC_PRINTLNF("SD card initialization error: Failed to open %s", LOG_FILE);
+        return false;
+    }
+    fprintf(logFile, "---- BEGIN NEW DATASET ----\n");
+    return true;
+}
+
+void log_close() {
+    if (logFile != NULL)
+        fclose(logFile);
+}
+
+void mpu_stop() {
+    // stop mpu?
+}
+
+void mpu_init() {
+    PC_PRINTLN("Initializing MPU");
+    mpu.initialize();
+    devStatus = mpu.dmpInitialize();
+    
+    if (devStatus == 0) {
+        mpu.setDMPEnabled(true);
+        packetSize = mpu.dmpGetFIFOPacketSize();
+        
+        PC_PRINTLN("DMP Initialized successfully!");
+        dmpReady = true;
+        dataReady.rise(dataReadyISR);
+    } else { // ERROR
+        PC_PRINTLNF("Error initializing MPU (code %d)", devStatus);
+        die(DMP_ERROR_RATE);
+    }
+}
+
+void captureThrInit() {
+    PC_PRINTLN("Opening log...");
+    log_open();
+    PC_PRINTLN("Initializing MPU...");
+    mpu_init();
+}
+
+void captureThrClose() {
+    log_close();
+    mpu_stop();
+}
+
+/* Requires the log to be open and mpu to be initialized*/
+void captureThr() {
+    static uint32_t n_overflows = 0;
+    while (true) {
+        if (!dmpReady) continue;   // do nothing if dmp not ready
+        
+        while (!mpuInterrupt && fifoCount < packetSize);
+        
+        // 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());
+            n_overflows++;
+            // 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);
+            
+            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);
+            
+            fprintf(logFile, "%u,%d,%d,%d,%f,%f,%f,%f,%u\n", captureTime.read_ms(), aa.x, aa.y, aa.z, q.x, q.y, q.z, q.w, n_overflows);
+        }
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/capture.h	Wed May 14 03:14:57 2014 +0000
@@ -0,0 +1,10 @@
+#include "mbed.h"
+
+#define LOG_FILE "/sd/data.log"
+
+void captureThrInit();
+void captureThrClose();
+void die(int flash_rate_s);
+void mpu_init();
+void mpu_stop();
+void captureThr();
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/debug.cpp	Wed May 14 03:14:57 2014 +0000
@@ -0,0 +1,7 @@
+#include "mbed.h"
+#include "debug.h"
+#include "USBSerial.h"
+
+#ifdef PC_DEBUG
+USBSerial pc;
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/debug.h	Wed May 14 03:14:57 2014 +0000
@@ -0,0 +1,32 @@
+#ifndef _DEBUG_H
+#define _DEBUG_H
+
+#include "mbed.h"
+#include "USBSerial.h"
+
+#define DMP_ERROR_RATE 1
+#define SD_ERROR_RATE 2
+
+#define PC_DEBUG
+#ifdef PC_DEBUG
+    #define PC_PRINT(x) pc.printf("%s", x);
+    #define PC_PRINTF(x,y) pc.printf(x, y);
+    #define PC_PRINTLN(x) pc.printf("%s\r\n", x);
+    #define PC_PRINTLNF(x,y) pc.printf(x,y); pc.printf("\r\n");
+    #define PC_PRINTR(x) pc.printf("%s\r",x);
+    #define PC_PRINTFR(x,y) pc.printf(x,y); pc.printf("\r");
+#else
+    #define PC_PRINT(x)
+    #define PC_PRINTF(x,y)
+    #define PC_PRINTLN(x)
+    #define PC_PRINTLNF(x,y)
+    #define PC_PRINTR(x)
+    #define PC_PRINTFR(x,y)
+#endif
+
+//Virtual serial port over USB
+#ifdef PC_DEBUG
+extern USBSerial pc;
+#endif
+
+#endif // _DEBUG_H
\ No newline at end of file

--- a/main.cpp	Mon May 12 18:52:45 2014 +0000
+++ b/main.cpp	Wed May 14 03:14:57 2014 +0000
@@ -1,14 +1,12 @@
-#include "main.h"
 #include "mbed.h"
+#include "rtos.h"
 #include "USBSerial.h"
 #include "Adafruit_SSD1306.h"
-#include "MPU6050_6Axis_MotionApps20.h"
-#include "SDFileSystem.h"
+#include "main.h"
+#include "capture.h"
+#include "debug.h"
 
-//Virtual serial port over USB
-#ifdef PC_DEBUG
-USBSerial pc;
-#endif
+#define SIG_IDLE 0x1
 
 // Display
 #ifdef OLED_DEBUG
@@ -16,220 +14,284 @@
 Adafruit_SSD1306 oled(spi0, P0_11, P0_12, P0_13); // DC, RST, CS
 #endif
 
-// MPU
-MPU6050 mpu;
-InterruptIn dataReady(P0_15);
+// Switch
+InterruptIn modeSwitch(P0_16);
 
-// SD Card
-SDFileSystem sd(P0_21, P0_22, P1_15, P1_19, "sd"); // MOSI, MISO, SCLK, SSEL SPI1
-
-// LED for debug
-DigitalOut led(LED1);
-
-// Logging vars
-FILE *logFile;
+// Mode
+enum mode {IDLE, CAPTURE};
+enum mode Mode;
 
 // Timer
 Timer totalTime;
-Timer captureTime;
-
-// Switch
-InterruptIn captureSwitch(P0_16);
-
-
-// State
-enum state {IDLE, CAPTURE};
-enum state State;
-
-// MPU control/status vars
-bool dmpReady = false;  // set true if DMP init was successful
-uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
-uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
-uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
-uint16_t fifoCount;     // count of all bytes currently in FIFO
-uint8_t fifoBuffer[64]; // FIFO storage buffer
 
-// orientation/motion vars
-Quaternion q;           // [w, x, y, z]         quaternion container
-VectorInt16 aa;         // [x, y, z]            accel sensor measurements
-VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
-VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
-VectorFloat gravity;    // [x, y, z]            gravity vector
-float euler[3];         // [psi, theta, phi]    Euler angle container
-float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
-
-// Forward declarations
-void die(int flash_rate_s);
-bool log_open();
-void log_data(VectorInt16 data, Quaternion q);
-void log_close();
-void mpu_init();
-void get_data();
+// Main Thread
+osThreadId mainThreadId;
 
-volatile bool mpuInterrupt = false;
-void dataReadyISR() {
-    mpuInterrupt = true;
-}
-
-//void captureSwitchISR() {
-//    // used for debouncing
-//    static int prev_time = 0;
-//
-//    if (totalTime.read_ms() - prev_time < 200)
-//        return;
-//        
-//    State = (State == IDLE) ? CAPTURE : IDLE;
-//    prev_time = totalTime.read_ms();
-//}
-
-void captureSwitchISR() {
+void modeSwInterrupt() {
     // used for debouncing
     static int prev_time = 0;
     int curr_time = totalTime.read_ms();
     
     // Only change state after an amount of time
-    // Note: abs value is necessary in case of 
-    //   overflows
-    if (abs(curr_time - prev_time) > 200)
-        State = (State == IDLE) ? CAPTURE : IDLE;
+    // Note: abs value is necessary in case of overflows
+    if (abs(curr_time - prev_time) > 200) {
+        if (Mode != IDLE) {
+            osSignalSet(mainThreadId, SIG_IDLE);
+            Mode = IDLE; 
+        } else
+            Mode = CAPTURE;
+    }
         
     prev_time = curr_time;
 }
 
-int main(void)
-{
-    totalTime.start();
+int main() {
     
-    State = IDLE;
-    captureSwitch.mode(PullUp);
-    captureSwitch.rise(captureSwitchISR);   
+    Mode = IDLE;
+    modeSwitch.mode(PullUp);
+    modeSwitch.rise(modeSwInterrupt);   
     
-    OLED_PRINTP("Waiting to capture.     ",0,0);
+    mainThreadId = osThreadGetId();
+    
+    totalTime.start();
     while (true) {
-        if (State == CAPTURE) {
-            PC_PRINTLN("Start capture button pressed!");
-            OLED_CLEAR();
-            OLED_PRINTPR("Starting capture.       ",0,0);
-            
-            // Start receiving data
-            PC_PRINTLN("Opening log file...");
-            if (!log_open()) {
-                OLED_CLEAR();
-                OLED_PRINTP("ERROR: SD (retry)", 0, 50);
-                State = IDLE;
-                continue;
-            }
-            
-            PC_PRINTLN("Initializing MPU...");
-            mpu_init();
+        if (Mode == CAPTURE) {
+            PC_PRINTLN("Initializing capture mode...");
+            captureThrInit();
             
             PC_PRINTLN("Starting capture...");
-            OLED_PRINTPR("Capturing data...       ",0,0);
-            captureTime.start();
-            while (State == CAPTURE)
-                get_data();
-            OLED_PRINTPR("Finished capture.",0,0);
+            Thread captureThr(captureThr);
+            
+            PC_PRINTLN("Capturing...");
+            Thread::signal_wait(SIG_IDLE);
             
-            log_close();
-            captureTime.stop();
-            captureTime.reset();
-        }
-    
+            PC_PRINTLN("Closing capture thread...");
+            captureThrClose();
+            captureThr.terminate();
+            
+            PC_PRINTLN("Capture complete.");
+        } 
         PC_PRINTLN("Idling...");
     }
 }
 
-/* Halts program and flashes LED at specified rate */
-void die(int flash_rate_s) {
-    while (1) {
-        led = 1;
-        wait(flash_rate_s/2);
-        led = 0;
-        wait(flash_rate_s/2);
-    }
-}
-
-/* Returns false on failure, true otherwise */
-bool log_open() {
-    logFile = fopen(LOG_FILE, "a");
-    if (logFile == NULL) {
-        PC_PRINTLNF("SD card initialization error: Failed to open %s", LOG_FILE);
-        return false;
-    }
-    fprintf(logFile, "---- BEGIN NEW DATASET ----\n");
-    return true;
-}
-
-void log_close() {
-    if (logFile != NULL)
-        fclose(logFile);
-}
-
-void mpu_init() {
-    PC_PRINTLN("Initializing MPU");
-    mpu.initialize();
-    devStatus = mpu.dmpInitialize();
-    
-    if (devStatus == 0) {
-        mpu.setDMPEnabled(true);
-        packetSize = mpu.dmpGetFIFOPacketSize();
-        
-        PC_PRINTLN("DMP Initialized successfully!");
-        dmpReady = true;
-        dataReady.rise(dataReadyISR);
-    } else { // ERROR
-        PC_PRINTLNF("Error initializing MPU (code %d)", devStatus);
-        die(DMP_ERROR_RATE);
-    }
-}
-
-/* Requires the log to be open and mpu to be initialized*/
-void get_data() {
-    static uint32_t n_overflows = 0;
-    //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();
-    
-        // 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());
-            n_overflows++;
-            // 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);
-            
-            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);
-            
-            fprintf(logFile, "%u,%d,%d,%d,%f,%f,%f,%f,%u\n", captureTime.read_ms(), aa.x, aa.y, aa.z, q.x, q.y, q.z, q.w, n_overflows);
-        //}
-    }
-}
-
-void log_data(VectorInt16 data, Quaternion q) {
-    fprintf(logFile, "%d,%d,%d,%d,%f,%f,%f,%f\n", captureTime.read_ms(), data.x, data.y, data.z, q.x, q.y, q.z, q.w);
-}
\ No newline at end of file
+////Virtual serial port over USB
+//#ifdef PC_DEBUG
+//USBSerial pc;
+//#endif
+//
+//
+//// MPU
+//MPU6050 mpu;
+//InterruptIn dataReady(P0_15);
+//
+//// SD Card
+//SDFileSystem sd(P0_21, P0_22, P1_15, P1_19, "sd"); // MOSI, MISO, SCLK, SSEL SPI1
+//
+//// LED for debug
+//DigitalOut led(LED1);
+//
+//// Logging vars
+//FILE *logFile;
+//
+//// Timer
+//Timer totalTime;
+//Timer captureTime;
+//
+//// Switch
+//InterruptIn captureSwitch(P0_16);
+//
+//
+//// State
+//enum state {IDLE, CAPTURE};
+//enum state State;
+//
+//// MPU control/status vars
+//bool dmpReady = false;  // set true if DMP init was successful
+//uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
+//uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
+//uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
+//uint16_t fifoCount;     // count of all bytes currently in FIFO
+//uint8_t fifoBuffer[64]; // FIFO storage buffer
+//
+//// orientation/motion vars
+//Quaternion q;           // [w, x, y, z]         quaternion container
+//VectorInt16 aa;         // [x, y, z]            accel sensor measurements
+//VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
+//VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
+//VectorFloat gravity;    // [x, y, z]            gravity vector
+//float euler[3];         // [psi, theta, phi]    Euler angle container
+//float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
+//
+//// Forward declarations
+//void die(int flash_rate_s);
+//bool log_open();
+//void log_data(VectorInt16 data, Quaternion q);
+//void log_close();
+//void mpu_init();
+//void get_data();
+//
+//volatile bool mpuInterrupt = false;
+//void dataReadyISR() {
+//    mpuInterrupt = true;
+//}
+//
+//void captureSwitchISR() {
+//    // used for debouncing
+//    static int prev_time = 0;
+//    int curr_time = totalTime.read_ms();
+//    
+//    // Only change state after an amount of time
+//    // Note: abs value is necessary in case of 
+//    //   overflows
+//    if (abs(curr_time - prev_time) > 200)
+//        State = (State == IDLE) ? CAPTURE : IDLE;
+//        
+//    prev_time = curr_time;
+//}
+//
+//int main(void)
+//{
+//    totalTime.start();
+//    
+//    State = IDLE;
+//    captureSwitch.mode(PullUp);
+//    captureSwitch.rise(captureSwitchISR);   
+//    
+//    OLED_PRINTP("Waiting to capture.     ",0,0);
+//    while (true) {
+//        if (State == CAPTURE) {
+//            PC_PRINTLN("Start capture button pressed!");
+//            OLED_CLEAR();
+//            OLED_PRINTPR("Starting capture.       ",0,0);
+//            
+//            // Start receiving data
+//            PC_PRINTLN("Opening log file...");
+//            if (!log_open()) {
+//                OLED_CLEAR();
+//                OLED_PRINTP("ERROR: SD (retry)", 0, 50);
+//                State = IDLE;
+//                continue;
+//            }
+//            
+//            PC_PRINTLN("Initializing MPU...");
+//            mpu_init();
+//            
+//            PC_PRINTLN("Starting capture...");
+//            OLED_PRINTPR("Capturing data...       ",0,0);
+//            captureTime.start();
+//            
+//            // Start receiving data
+//            Thread get_data_thr(get_data);
+//            
+//            // Wait for capture button
+//            Thread::wait(SIG_CAPTURE);
+//            
+//            // Stop receiving data
+//            get_data_thr.terminate();
+//            
+//            OLED_PRINTPR("Finished capture.",0,0);
+//            
+//            log_close();
+//            captureTime.stop();
+//            captureTime.reset();
+//        }
+//    
+//        PC_PRINTLN("Idling...");
+//    }
+//}
+//
+///* Halts program and flashes LED at specified rate */
+//void die(int flash_rate_s) {
+//    while (1) {
+//        led = 1;
+//        wait(flash_rate_s/2);
+//        led = 0;
+//        wait(flash_rate_s/2);
+//    }
+//}
+//
+///* Returns false on failure, true otherwise */
+//bool log_open() {
+//    logFile = fopen(LOG_FILE, "a");
+//    if (logFile == NULL) {
+//        PC_PRINTLNF("SD card initialization error: Failed to open %s", LOG_FILE);
+//        return false;
+//    }
+//    fprintf(logFile, "---- BEGIN NEW DATASET ----\n");
+//    return true;
+//}
+//
+//void log_close() {
+//    if (logFile != NULL)
+//        fclose(logFile);
+//}
+//
+//void mpu_init() {
+//    PC_PRINTLN("Initializing MPU");
+//    mpu.initialize();
+//    devStatus = mpu.dmpInitialize();
+//    
+//    if (devStatus == 0) {
+//        mpu.setDMPEnabled(true);
+//        packetSize = mpu.dmpGetFIFOPacketSize();
+//        
+//        PC_PRINTLN("DMP Initialized successfully!");
+//        dmpReady = true;
+//        dataReady.rise(dataReadyISR);
+//    } else { // ERROR
+//        PC_PRINTLNF("Error initializing MPU (code %d)", devStatus);
+//        die(DMP_ERROR_RATE);
+//    }
+//}
+//
+///* Requires the log to be open and mpu to be initialized*/
+//void get_data() {
+//    static uint32_t n_overflows = 0;
+//    //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();
+//    
+//        // 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());
+//            n_overflows++;
+//            // 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);
+//            
+//            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);
+//            
+//            fprintf(logFile, "%u,%d,%d,%d,%f,%f,%f,%f,%u\n", captureTime.read_ms(), aa.x, aa.y, aa.z, q.x, q.y, q.z, q.w, n_overflows);
+//        //}
+//    }
+//}
+//
+//void log_data(VectorInt16 data, Quaternion q) {
+//    fprintf(logFile, "%d,%d,%d,%d,%f,%f,%f,%f\n", captureTime.read_ms(), data.x, data.y, data.z, q.x, q.y, q.z, q.w);
+//}
\ No newline at end of file

--- a/main.h	Mon May 12 18:52:45 2014 +0000
+++ b/main.h	Wed May 14 03:14:57 2014 +0000
@@ -1,25 +1,3 @@
-#define DMP_ERROR_RATE 1
-#define SD_ERROR_RATE 2
-
-#define LOG_FILE "/sd/data.log"
-
-//#define PC_DEBUG
-#ifdef PC_DEBUG
-    #define PC_PRINT(x) pc.printf("%s", x);
-    #define PC_PRINTF(x,y) pc.printf(x, y);
-    #define PC_PRINTLN(x) pc.printf("%s\r\n", x);
-    #define PC_PRINTLNF(x,y) pc.printf(x,y); pc.printf("\r\n");
-    #define PC_PRINTR(x) pc.printf("%s\r",x);
-    #define PC_PRINTFR(x,y) pc.printf(x,y); pc.printf("\r");
-#else
-    #define PC_PRINT(x)
-    #define PC_PRINTF(x,y)
-    #define PC_PRINTLN(x)
-    #define PC_PRINTLNF(x,y)
-    #define PC_PRINTR(x)
-    #define PC_PRINTFR(x,y)
-#endif
-
 #define OLED_DEBUG
 #ifdef OLED_DEBUG
     #define OLED_SETCURS(xpos,ypos) oled.setCursor(xpos,ypos);

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-rtos.lib	Wed May 14 03:14:57 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed-rtos/#5dfe422a963d