Swimate V2 without RTOS code
Dependencies: Adafruit_GFX_128x64 DS3231 PinDetect SDFileSystem USBDevice mbed RealtimeMath MODSERIAL
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