File content as of revision 2:a28cc676a454:

#include "main.h"
#include "mbed.h"
#include "USBSerial.h"
#include "Adafruit_SSD1306.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "SDFileSystem.h"
#include "misc.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);

// 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 *rawlog;
FILE *linlog;
FILE *worldlog;
FILE *quatlog;
FILE *gravlog;
FILE *splitlog;

// 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

// Real time split detection
RingBuffer accel_buf(32);

// Forward declarations
void die(int flash_rate_s);
bool log_open(FILE *lf, char *path);
void log_acceleration(VectorInt16 data);
void log_close(FILE *lf);
void mpu_init();
void get_data();

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

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(rawlog, RAW_ACCEL_FILE) ||
                    !log_open(linlog, LIN_ACCEL_FILE) ||
                    !log_open(worldlog, WORLD_ACCEL_FILE) ||
                    !log_open(quatlog, QUAT_FILE) ||
                    !log_open(gravlog, GRAV_FILE) ||
                    !log_open(splitlog, SPLITS_FILE)) {
                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_open(RAW_ACCEL_FILE) ||
            log_open(LIN_ACCEL_FILE) ||
            log_open(worldlog, WORLD_ACCEL_FILE) ||
            log_open(quatlog, QUAT_FILE) ||
            log_open(gravlog, GRAV_FILE) ||
            log_open(splitlog, SPLITS_FILE)) {
                
            captureTime.stop();
        }
    
        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 */
FILE *log_open(FILE *lf, char *path) {
    lf = fopen(path, "a");
    if (lf == NULL) {
        PC_PRINTLNF("SD card initialization error: Failed to open %s", LOG_FILE);
        return lf;
    }
    fprintf(lf, "---- BEGIN NEW DATASET ----\n");
    return lf;
}

void log_close(FILE *lf) {
    if (lf != NULL)
        fclose(lf);
}


//void log_acceleration(VectorInt16 data) {
//    fprintf(logFile, "%d, %d,%d,%d\n", captureTime.read_ms(), data.x, data.y, data.z);
//}

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() {
    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);
        
        accel_buf.put(aa.x);
  
        int currtime = captureTime.read_ms();
        
        if (mov_avg(accel_buf))
            fprintf(splitlog, "%d", currtime);
        fprintf(rawlog, "%d,%d,%d,%d", currtime, aa.x, aa.y, aa.z);
        fprintf(linlog, "%d,%d,%d,%d", currtime, aaReal.x, aaReal.y, aaReal.z);
        fprintf(worldlog, "%d,%d,%d,%d", currtime, aaWorld.x, aaWorld.y, aaWorld.z);       
        fprintf(quatlog, "%d,%f,%f,%f,%f", currtime, q.x, q.y, q.z, q.w);   
        fprintf(gravlog, "%d,%f,%f,%f", currtime, gravity.x, gravity.y, gravity.z);

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