Swimate V2 without RTOS code
Dependencies: Adafruit_GFX_128x64 DS3231 PinDetect SDFileSystem USBDevice mbed RealtimeMath MODSERIAL
receive_data.cpp
- Committer:
- ellingjp
- Date:
- 2014-06-09
- Revision:
- 24:f2503d1256ad
- Parent:
- 20:294eaeaf2ebb
File content as of revision 24:f2503d1256ad:
#include "mbed.h" #include "MPU6050_6Axis_MotionApps20.h" #include "debug.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 volatile bool mpuInterrupt = false; void dataReadyISR() { mpuInterrupt = true; } /* Returns a pointer to an array containing the most recent data. Will return NULL if unable to get new data (expect this to happen). Requires the mpu to be initialized. */ VectorInt16 *receive_data() { static uint32_t n_overflows = 0; // do nothing if dmp not ready if (!dmpReady) return NULL; 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_PRINTLN("**** FIFO OVERFLOW ****"); n_overflows++; // reset so we can continue cleanly mpu.resetFIFO(); // otherwise, check for DMP data ready interrupt or another packet to process (this should happen frequently) } else if (mpuIntStatus & 0x02 || fifoCount > packetSize) { // 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); return &aa; } return NULL; } bool receive_close() { // Disable DMP and clear fifo mpu.setDMPEnabled(false); mpu.resetFIFO(); return true; } bool receive_init() { PC_PRINTLN("Initializing MPU"); mpu.initialize(); PC_PRINTLN("MPU initialized, initializing DMP"); devStatus = mpu.dmpInitialize(); if (devStatus == 0) { mpu.setDMPEnabled(true); packetSize = mpu.dmpGetFIFOPacketSize(); PC_PRINTLN("DMP Initialized successfully!"); dmpReady = true; dataReady.rise(dataReadyISR); return true; } else { // ERROR PC_PRINTLNF("Error initializing MPU (code %d)", devStatus); DIE(DMP_ERROR_RATE); return false; } PC_PRINTLN("Receive init success, capturing data..."); }