Revision 6:3b9b4e2c29bf, committed 2014-05-16

Comitter:

ellingjp

Date:

Fri May 16 23:32:49 2014 +0000

Parent:

5:7c2e7d657716

Commit message:

rtos breaks I2C. Workaround is to disable interrupts when accessing i2c devices (undesirable). SD library may also be broken.

Changed in this revision

--- a/RingBuffer.lib	Wed May 14 03:14:57 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/okini3939/code/RingBuffer/#bc363775c3eb

--- a/capture.cpp	Wed May 14 03:14:57 2014 +0000
+++ b/capture.cpp	Fri May 16 23:32:49 2014 +0000
@@ -4,6 +4,9 @@
 #include "helper_3dmath.h"
 #include "SDFileSystem.h"
 #include "debug.h"
+#include "rtos.h"
+#include "shared.h"
+#include "main.h"
 
 // MPU
 MPU6050 mpu;
@@ -38,9 +41,14 @@
 float euler[3];         // [psi, theta, phi]    Euler angle container
 float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
 
+// Thread
+bool threadReady = false;
+osThreadId captureThreadId = 0;
+
 volatile bool mpuInterrupt = false;
 void dataReadyISR() {
-    mpuInterrupt = true;
+    if (threadReady)  // Wait until thread is initialized
+        osSignalSet(captureThreadId, SIG_DATAREADY);
 }
 
 /* Halts program and flashes LED at specified rate */
@@ -55,9 +63,7 @@
 
 /* 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;
@@ -77,9 +83,16 @@
 
 void mpu_init() {
     PC_PRINTLN("Initializing MPU");
+    OLED_PRINTR("Initializing MPU");
+    __disable_irq();
     mpu.initialize();
+    
+    OLED_PRINTR("MPU Initialized");
+
     devStatus = mpu.dmpInitialize();
     
+    OLED_PRINTR("DMP Initialized");
+
     if (devStatus == 0) {
         mpu.setDMPEnabled(true);
         packetSize = mpu.dmpGetFIFOPacketSize();
@@ -91,12 +104,12 @@
         PC_PRINTLNF("Error initializing MPU (code %d)", devStatus);
         die(DMP_ERROR_RATE);
     }
+    __enable_irq();
 }
 
 void captureThrInit() {
-    PC_PRINTLN("Opening log...");
-    log_open();
-    PC_PRINTLN("Initializing MPU...");
+//    PC_PRINTLN("Opening log...");
+//    log_open();
     mpu_init();
 }
 
@@ -105,13 +118,35 @@
     mpu_stop();
 }
 
-/* Requires the log to be open and mpu to be initialized*/
-void captureThr() {
+void capture(void const *args) {
+    OLED_PRINTR("In capture");
+    captureThrInit();
+    OLED_PRINTR("Initialized capture thread");
+
     static uint32_t n_overflows = 0;
+    captureThreadId = osThreadGetId();
+    threadReady = true;
+    OLED_PRINTR("Got thrid");
+    
+    threadReady = true;
+
     while (true) {
-        if (!dmpReady) continue;   // do nothing if dmp not ready
+        // Pause if not in capture mode
+        if (Mode != CAPTURE) {
+//            captureTimer.reset();
+            PC_PRINTLN("Should be waiting...");
+            Thread::signal_wait(SIG_CAPTURESTART, osWaitForever);
+        }
+             
+        if (!dmpReady) {
+            PC_PRINTLN("DMP wasn't initialized properly.  Exiting capture..."); 
+            return;
+        }
         
-        while (!mpuInterrupt && fifoCount < packetSize);
+        if (fifoCount < packetSize)
+            Thread::signal_wait(SIG_DATAREADY);
+            
+//        while (!mpuInterrupt && fifoCount < packetSize);
         
         // Reset interrupt
         mpuInterrupt = false;
@@ -132,21 +167,26 @@
             while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
     
             // read a packet from FIFO
+            // Must disable interrupts to avoid weird behavior
+            //   TODO: Possibly use MODI2C for non-blocking i2c read/writes
+            __disable_irq();
             mpu.getFIFOBytes(fifoBuffer, packetSize);
+            __enable_irq();
+            
             fifoCount -= packetSize;
             
             // Get acceleration data
             mpu.dmpGetAccel(&aa, fifoBuffer);
-            mpu.dmpGetQuaternion(&q, 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);
+            PC_PRINTF("%d, ", aa.x); PC_PRINTF("%d, ", aa.y); PC_PRINTLNF("%d", aa.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);
+//            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

--- a/capture.h	Wed May 14 03:14:57 2014 +0000
+++ b/capture.h	Fri May 16 23:32:49 2014 +0000
@@ -2,9 +2,4 @@
 
 #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
+void capture(void const *args);
\ No newline at end of file

--- a/debug.cpp	Wed May 14 03:14:57 2014 +0000
+++ b/debug.cpp	Fri May 16 23:32:49 2014 +0000
@@ -4,4 +4,10 @@
 
 #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); // DC, RST, CS
 #endif
\ No newline at end of file

--- a/debug.h	Wed May 14 03:14:57 2014 +0000
+++ b/debug.h	Fri May 16 23:32:49 2014 +0000
@@ -3,6 +3,7 @@
 
 #include "mbed.h"
 #include "USBSerial.h"
+#include "Adafruit_SSD1306.h"
 
 #define DMP_ERROR_RATE 1
 #define SD_ERROR_RATE 2
@@ -24,9 +25,52 @@
     #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 Adafruit_SSD1306 oled;
+#endif
+
 #endif // _DEBUG_H
\ No newline at end of file

--- a/main.cpp	Wed May 14 03:14:57 2014 +0000
+++ b/main.cpp	Fri May 16 23:32:49 2014 +0000
@@ -5,28 +5,19 @@
 #include "main.h"
 #include "capture.h"
 #include "debug.h"
-
-#define SIG_IDLE 0x1
-
-// Display
-#ifdef OLED_DEBUG
-SPI spi0(P0_9, NC, P0_10); // mosi, miso, sclk
-Adafruit_SSD1306 oled(spi0, P0_11, P0_12, P0_13); // DC, RST, CS
-#endif
+#include "shared.h"
 
 // Switch
 InterruptIn modeSwitch(P0_16);
 
-// Mode
-enum mode {IDLE, CAPTURE};
-enum mode Mode;
-
 // Timer
 Timer totalTime;
 
 // Main Thread
 osThreadId mainThreadId;
+Thread *captureThread;
 
+// TODO: Enable hold-button to enter sync mode
 void modeSwInterrupt() {
     // used for debouncing
     static int prev_time = 0;
@@ -35,42 +26,42 @@
     // Only change state after an amount of time
     // 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;
+        
+        switch (Mode) {
+            case IDLE:
+                Mode = CAPTURE;
+                captureThread->signal_set(SIG_CAPTURESTART);
+                break;
+            case CAPTURE:
+                Mode = IDLE;
+                osSignalSet(mainThreadId, SIG_IDLESTART);
+                break;
+            case SYNC:
+                Mode = IDLE;
+                break;
+        }
     }
         
     prev_time = curr_time;
 }
 
 int main() {
-    
     Mode = IDLE;
     modeSwitch.mode(PullUp);
     modeSwitch.rise(modeSwInterrupt);   
     
     mainThreadId = osThreadGetId();
-    
+    Thread capThr(capture);
+    captureThread = &capThr;
+
     totalTime.start();
     while (true) {
-        if (Mode == CAPTURE) {
-            PC_PRINTLN("Initializing capture mode...");
-            captureThrInit();
-            
-            PC_PRINTLN("Starting capture...");
-            Thread captureThr(captureThr);
+        // See if mode has changed
+        //if (Mode != IDLE)
+        
+        Thread::signal_wait(SIG_IDLESTART);
             
-            PC_PRINTLN("Capturing...");
-            Thread::signal_wait(SIG_IDLE);
-            
-            PC_PRINTLN("Closing capture thread...");
-            captureThrClose();
-            captureThr.terminate();
-            
-            PC_PRINTLN("Capture complete.");
-        } 
+        // Idling... maybe display something on oled or flash an led
         PC_PRINTLN("Idling...");
     }
 }

--- a/main.h	Wed May 14 03:14:57 2014 +0000
+++ b/main.h	Fri May 16 23:32:49 2014 +0000
@@ -1,37 +0,0 @@
-#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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-src.lib	Fri May 16 23:32:49 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed-src/#718a82ebba31

--- a/mbed.bld	Wed May 14 03:14:57 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/mbed_official/code/mbed/builds/8a40adfe8776
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared.cpp	Fri May 16 23:32:49 2014 +0000
@@ -0,0 +1,3 @@
+#include "shared.h"
+
+enum mode Mode;
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared.h	Fri May 16 23:32:49 2014 +0000
@@ -0,0 +1,16 @@
+#ifndef _SHARED_H
+#define _SHARED_H
+
+/* Signals */
+#define SIG_IDLESTART       0x1
+#define SIG_CAPTURESTART    0x2
+#define SIG_CAPTURESTOP     0x4
+#define SIG_SYNCSTART       0x8
+#define SIG_SYNCSTOP        0x16
+#define SIG_DATAREADY       0x32
+
+// Mode
+enum mode {IDLE, CAPTURE, SYNC};
+extern enum mode Mode;
+
+#endif // _SHARED_H
\ No newline at end of file