Revision 1:93723df61425, committed 2014-05-08

Comitter:

ellingjp

Date:

Thu May 08 20:59:29 2014 +0000

Parent:

0:cd1fe4f0ed39

Commit message:

Initial RTOS integration commit

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/get_data_thread.cpp	Thu May 08 20:59:29 2014 +0000
@@ -0,0 +1,96 @@
+#include "mbed.h"
+#include "rtos.h"
+#include "threads.h"
+#include "signals.h"
+#include "MPU6050_6Axis_MotionApps20.h"
+#include "output.h"
+
+// MPU
+MPU6050 mpu;
+InterruptIn dataReady(P0_15);
+
+// 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
+
+// This thread reference
+osThreadId getDataID;
+
+void dataReadyISR() {
+    osSignalSet(getDataID, SIG_DATAREADY);
+}
+
+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);
+        
+    }
+}
+
+/* Requires the log to be open and mpu to be initialized */
+void get_data(FILE *log) {
+    getDataID = osThreadGetId();
+    
+    mpu_init();
+    while (true) {
+        if (!dmpReady) break;   // do nothing if dmp not ready
+        
+        if (fifoCount < packetSize)
+            Thread::wait(SIG_DATAREADY);
+
+        mpuIntStatus = mpu.getIntStatus();
+        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);
+            
+            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);
+        }
+    }
+}
\ No newline at end of file

--- a/main.cpp	Thu May 08 20:01:10 2014 +0000
+++ b/main.cpp	Thu May 08 20:59:29 2014 +0000
@@ -1,24 +1,13 @@
 #include "main.h"
 #include "mbed.h"
+#include "rtos.h"
 #include "USBSerial.h"
 #include "Adafruit_SSD1306.h"
 #include "MPU6050_6Axis_MotionApps20.h"
 #include "SDFileSystem.h"
-
-//Virtual serial port over USB
-#ifdef PC_DEBUG
-USBSerial pc;
-#endif
-
-// Display
-#ifdef OLED_DEBUG
-SPI spi0(P0_9, NC, P0_10); // mosi, miso, sclk
-Adafruit_SSD1306 oled(spi0, P0_11, P0_12, P0_13); // MOSI, MISO, SCLK SPI0
-#endif
-
-// MPU
-MPU6050 mpu;
-InterruptIn dataReady(P0_15);
+#include "output.h"
+#include "threads.h"
+#include "signals.h"
 
 // SD Card
 SDFileSystem sd(P0_21, P0_22, P1_15, P1_19, "sd"); // MOSI, MISO, SCLK, SSEL SPI1
@@ -26,9 +15,6 @@
 // LED for debug
 DigitalOut led(LED1);
 
-// Logging vars
-FILE *logFile;
-
 // Timer
 Timer totalTime;
 Timer captureTime;
@@ -36,40 +22,17 @@
 // Switch
 InterruptIn captureSwitch(P0_16);
 
+// Main thread reference
+osThreadId mainThreadID;
 
 // 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_acceleration(VectorInt16 data);
 void log_close();
-void mpu_init();
-void get_data();
-
-volatile bool mpuInterrupt = false;
-void dataReadyISR() {
-    mpuInterrupt = true;
-}
 
 void captureSwitchISR() {
     // used for debouncing
@@ -78,7 +41,8 @@
     if (totalTime.read_ms() - prev_time < 200)
         return;
         
-    State = (State == IDLE) ? CAPTURE : IDLE;
+//    State = (State == IDLE) ? CAPTURE : IDLE;
+    osSignalSet(mainThreadID, SIG_CAPTURE);
     prev_time = totalTime.read_ms();
 }
 
@@ -86,41 +50,20 @@
 {
     totalTime.start();
     
-    State = IDLE;
     captureSwitch.mode(PullUp);
     captureSwitch.rise(captureSwitchISR);   
     
-    OLED_PRINTP("Waiting to capture.     ",0,0);
+    mainThreadID = osThreadGetId();
+
     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();
-            while (State == CAPTURE)
-                get_data();
-            OLED_PRINTPR("Finished capture.",0,0);
-            
-            log_close();
-            captureTime.stop();
-        }
-    
-        PC_PRINTLN("Idling...");
+        Thread::wait(SIG_CAPTURE);
+        
+        FILE *logfile = log_open();
+        Thread get_data_thread(get_data, logfile, osPriority priority=osPriorityHigh, DEFAULT_STACK_SIZE, NULL);
+        
+        Thread::wait(SIG_CAPTURE);
+
+        get_data.terminate();
     }
 }
 
@@ -134,15 +77,15 @@
     }
 }
 
-/* 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;
+/* Blocks until logfile is successfully opened
+ * There are cases where this will hang forever and need to reset (unavoidable) */
+FILE *log_open() {
+    FILE *logFile = fopen(LOG_FILE, "a");
+    while (logFile == NULL) {
+        PC_PRINTLN("Error opening SD card!");
+        OLED_PRINTP("ERROR: SD (retry?)", 0, 50);
     }
-    fprintf(logFile, "---- BEGIN NEW DATASET ----\n");
-    return true;
+    return logFile;
 }
 
 void log_close() {
@@ -150,69 +93,6 @@
         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() {
-    //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());
-            // 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);
-            
-            log_acceleration(aaWorld);
-        //}
-    }
-}
-
 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

--- a/main.h	Thu May 08 20:01:10 2014 +0000
+++ b/main.h	Thu May 08 20:59:29 2014 +0000
@@ -1,59 +1,4 @@
 #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);
-    #define OLED_CLEAR() oled.clearDisplay();
-    
-    #define OLED_PRINT(x) oled.printf("%s", x); oled.display();
-    #define OLED_PRINTF(x,y) oled.printf(x, y); oled.display();
-    #define OLED_PRINTLN(x) oled.printf("%s\r\n", x); oled.display();
-    #define OLED_PRINTLNF(x,y) oled.printf(x,y); oled.printf("\r\n"); oled.display();
-    #define OLED_PRINTR(x) oled.printf("%s\r",x); oled.display();
-    #define OLED_PRINTFR(x,y) oled.printf(x,y); oled.printf("\r"); oled.display();
-
-    // Specify position
-    #define OLED_PRINTP(x,xpos,ypos) oled.setCursor(xpos,ypos); oled.printf("%s", x); oled.display();
-    #define OLED_PRINTPF(x,y,xpos,ypos) oled.setCursor(xpos,ypos); oled.printf(x, y); oled.display();
-    #define OLED_PRINTPLN(x,xpos,ypos) oled.setCursor(xpos,ypos); oled.printf("%s\r\n", x); oled.display();
-    #define OLED_PRINTPLNF(x,y,xpos,ypos) oled.setCursor(xpos,ypos); oled.printf(x,y); oled.printf("\r\n"); oled.display();
-    #define OLED_PRINTPR(x,xpos,ypos) oled.setCursor(xpos,ypos); oled.printf("%s\r",x); oled.display();
-    #define OLED_PRINTPFR(x,y,xpos,ypos) oled.setCursor(xpos,ypos); oled.printf(x,y); oled.printf("\r"); oled.display();
-#else
-    #define OLED_SETCURS(xpos,ypos)
-
-    #define OLED_PRINT(x)
-    #define OLED_PRINTF(x,y)
-    #define OLED_PRINTLN(x)
-    #define OLED_PRINTLNF(x,y)
-    #define OLED_PRINTR(x)
-    #define OLED_PRINTFR(x,y)
-    
-    // Specify position
-    #define OLED_PRINTP(x,xpos,ypos)
-    #define OLED_PRINTPF(x,y,xpos,ypos)
-    #define OLED_PRINTPLN(x,xpos,ypos)
-    #define OLED_PRINTPLNF(x,y,xpos,ypos)
-    #define OLED_PRINTPR(x,xpos,ypos)
-    #define OLED_PRINTPFR(x,y,xpos,ypos)
-#endif
\ No newline at end of file
+#define LOG_FILE "/sd/data.log"
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-rtos.lib	Thu May 08 20:59:29 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed-rtos/#5dfe422a963d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/output.cpp	Thu May 08 20:59:29 2014 +0000
@@ -0,0 +1,15 @@
+#include "mbed.h"
+#include "output.h"
+#include "USBSerial.h"
+#include "Adafruit_SSD1306.h"
+
+//Virtual serial port over USB
+#ifdef PC_DEBUG
+USBSerial pc;
+#endif
+
+// Display
+#ifdef OLED_DEBUG
+SPI spi0(P0_9, NC, P0_10); // mosi, miso, sclk
+Adafruit_SSD1306 oled(spi0, P0_11, P0_12, P0_13); // MOSI, MISO, SCLK SPI0
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/output.h	Thu May 08 20:59:29 2014 +0000
@@ -0,0 +1,69 @@
+#include "mbed.h"
+#include "USBSerial.h"
+#include "Adafruit_SSD1306.h"
+
+#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);
+    #define OLED_CLEAR() oled.clearDisplay();
+    
+    #define OLED_PRINT(x) oled.printf("%s", x); oled.display();
+    #define OLED_PRINTF(x,y) oled.printf(x, y); oled.display();
+    #define OLED_PRINTLN(x) oled.printf("%s\r\n", x); oled.display();
+    #define OLED_PRINTLNF(x,y) oled.printf(x,y); oled.printf("\r\n"); oled.display();
+    #define OLED_PRINTR(x) oled.printf("%s\r",x); oled.display();
+    #define OLED_PRINTFR(x,y) oled.printf(x,y); oled.printf("\r"); oled.display();
+
+    // Specify position
+    #define OLED_PRINTP(x,xpos,ypos) oled.setCursor(xpos,ypos); oled.printf("%s", x); oled.display();
+    #define OLED_PRINTPF(x,y,xpos,ypos) oled.setCursor(xpos,ypos); oled.printf(x, y); oled.display();
+    #define OLED_PRINTPLN(x,xpos,ypos) oled.setCursor(xpos,ypos); oled.printf("%s\r\n", x); oled.display();
+    #define OLED_PRINTPLNF(x,y,xpos,ypos) oled.setCursor(xpos,ypos); oled.printf(x,y); oled.printf("\r\n"); oled.display();
+    #define OLED_PRINTPR(x,xpos,ypos) oled.setCursor(xpos,ypos); oled.printf("%s\r",x); oled.display();
+    #define OLED_PRINTPFR(x,y,xpos,ypos) oled.setCursor(xpos,ypos); oled.printf(x,y); oled.printf("\r"); oled.display();
+#else
+    #define OLED_SETCURS(xpos,ypos)
+
+    #define OLED_PRINT(x)
+    #define OLED_PRINTF(x,y)
+    #define OLED_PRINTLN(x)
+    #define OLED_PRINTLNF(x,y)
+    #define OLED_PRINTR(x)
+    #define OLED_PRINTFR(x,y)
+    
+    // Specify position
+    #define OLED_PRINTP(x,xpos,ypos)
+    #define OLED_PRINTPF(x,y,xpos,ypos)
+    #define OLED_PRINTPLN(x,xpos,ypos)
+    #define OLED_PRINTPLNF(x,y,xpos,ypos)
+    #define OLED_PRINTPR(x,xpos,ypos)
+    #define OLED_PRINTPFR(x,y,xpos,ypos)
+#endif
+
+//Virtual serial port over USB
+#ifdef PC_DEBUG
+extern USBSerial pc;
+#endif
+
+// Display
+#ifdef OLED_DEBUG
+extern SPI spi0; // mosi, miso, sclk
+extern Adafruit_SSD1306 oled; // MOSI, MISO, SCLK SPI0
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/signals.h	Thu May 08 20:59:29 2014 +0000
@@ -0,0 +1,2 @@
+#define SIG_CAPTURE      0x0
+#define SIG_DATAREADY    0x1
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/threads.h	Thu May 08 20:59:29 2014 +0000
@@ -0,0 +1,2 @@
+/* get_data_thread */
+void get_data(FILE *log);
\ No newline at end of file