Swimate V2 without RTOS code
Dependencies: Adafruit_GFX_128x64 DS3231 PinDetect SDFileSystem USBDevice mbed RealtimeMath MODSERIAL
main.cpp
- Committer:
- ellingjp
- Date:
- 2014-05-08
- Revision:
- 2:a28cc676a454
- Parent:
- 0:cd1fe4f0ed39
- Child:
- 3:9aa7c37212b5
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); } }