File content as of revision 6:3b9b4e2c29bf:

#include "mbed.h"
#include "rtos.h"
#include "USBSerial.h"
#include "Adafruit_SSD1306.h"
#include "main.h"
#include "capture.h"
#include "debug.h"
#include "shared.h"

// Switch
InterruptIn modeSwitch(P0_16);

// 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;
    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) {
        
        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) {
        // See if mode has changed
        //if (Mode != IDLE)
        
        Thread::signal_wait(SIG_IDLESTART);
            
        // Idling... maybe display something on oled or flash an led
        PC_PRINTLN("Idling...");
    }
}

////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);
//}