CSE477 / swimate_v2

Swimate V2 without RTOS code

Dependencies:   Adafruit_GFX_128x64 DS3231 PinDetect SDFileSystem USBDevice mbed RealtimeMath MODSERIAL

receive_data.cpp@24:f2503d1256ad, 2014-06-09 (annotated)

Committer:
ellingjp
Date:
Mon Jun 09 04:55:16 2014 +0000
Revision:
24:f2503d1256ad
Parent:
20:294eaeaf2ebb 
Using RTC filenames

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ellingjp 7:33a74adff0ff 1 #include "mbed.h"
ellingjp 7:33a74adff0ff 2 #include "MPU6050_6Axis_MotionApps20.h"
ellingjp 7:33a74adff0ff 3 #include "debug.h"
ellingjp 7:33a74adff0ff 4
ellingjp 7:33a74adff0ff 5 // MPU
ellingjp 7:33a74adff0ff 6 MPU6050 mpu;
ellingjp 7:33a74adff0ff 7 InterruptIn dataReady(P0_15);
ellingjp 7:33a74adff0ff 8
ellingjp 7:33a74adff0ff 9 // MPU control/status vars
ellingjp 7:33a74adff0ff 10 bool dmpReady = false; // set true if DMP init was successful
ellingjp 7:33a74adff0ff 11 uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
ellingjp 7:33a74adff0ff 12 uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
ellingjp 7:33a74adff0ff 13 uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
ellingjp 7:33a74adff0ff 14 uint16_t fifoCount; // count of all bytes currently in FIFO
ellingjp 7:33a74adff0ff 15 uint8_t fifoBuffer[64]; // FIFO storage buffer
ellingjp 7:33a74adff0ff 16
ellingjp 7:33a74adff0ff 17 // orientation/motion vars
ellingjp 8:8430a5c0914c 18 //Quaternion q; // [w, x, y, z] quaternion container
ellingjp 7:33a74adff0ff 19 VectorInt16 aa; // [x, y, z] accel sensor measurements
ellingjp 8:8430a5c0914c 20 //VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
ellingjp 8:8430a5c0914c 21 //VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
ellingjp 8:8430a5c0914c 22 //VectorFloat gravity; // [x, y, z] gravity vector
ellingjp 8:8430a5c0914c 23 //float euler[3]; // [psi, theta, phi] Euler angle container
ellingjp 8:8430a5c0914c 24 //float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
ellingjp 7:33a74adff0ff 25
ellingjp 7:33a74adff0ff 26 volatile bool mpuInterrupt = false;
ellingjp 7:33a74adff0ff 27 void dataReadyISR() {
ellingjp 7:33a74adff0ff 28 mpuInterrupt = true;
ellingjp 7:33a74adff0ff 29 }
ellingjp 7:33a74adff0ff 30
ellingjp 8:8430a5c0914c 31 /* Returns a pointer to an array containing the most recent data. Will return NULL if unable to
ellingjp 8:8430a5c0914c 32 get new data (expect this to happen). Requires the mpu to be initialized. */
ellingjp 8:8430a5c0914c 33 VectorInt16 *receive_data()
ellingjp 7:33a74adff0ff 34 {
ellingjp 7:33a74adff0ff 35 static uint32_t n_overflows = 0;
ellingjp 8:8430a5c0914c 36
ellingjp 8:8430a5c0914c 37 // do nothing if dmp not ready
ellingjp 8:8430a5c0914c 38 if (!dmpReady) return NULL;
ellingjp 7:33a74adff0ff 39
ellingjp 7:33a74adff0ff 40 while (!mpuInterrupt && fifoCount < packetSize);
ellingjp 7:33a74adff0ff 41
ellingjp 7:33a74adff0ff 42 // Reset interrupt
ellingjp 7:33a74adff0ff 43 mpuInterrupt = false;
ellingjp 7:33a74adff0ff 44 mpuIntStatus = mpu.getIntStatus();
ellingjp 7:33a74adff0ff 45
ellingjp 7:33a74adff0ff 46 // get current FIFO count
ellingjp 7:33a74adff0ff 47 fifoCount = mpu.getFIFOCount();
ellingjp 7:33a74adff0ff 48
ellingjp 7:33a74adff0ff 49 // check for overflow (this should never happen unless our code is too inefficient)
ellingjp 7:33a74adff0ff 50 if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
ellingjp 7:33a74adff0ff 51 PC_PRINTLN("**** FIFO OVERFLOW ****");
ellingjp 7:33a74adff0ff 52 n_overflows++;
ellingjp 7:33a74adff0ff 53 // reset so we can continue cleanly
ellingjp 7:33a74adff0ff 54 mpu.resetFIFO();
ellingjp 8:8430a5c0914c 55 // otherwise, check for DMP data ready interrupt or another packet to process (this should happen frequently)
ellingjp 8:8430a5c0914c 56 } else if (mpuIntStatus & 0x02 || fifoCount > packetSize) {
ellingjp 7:33a74adff0ff 57 // Wait for a full packet - should be very short wait
ellingjp 7:33a74adff0ff 58 while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
ellingjp 7:33a74adff0ff 59
ellingjp 7:33a74adff0ff 60 // read a packet from FIFO
ellingjp 7:33a74adff0ff 61 mpu.getFIFOBytes(fifoBuffer, packetSize);
ellingjp 7:33a74adff0ff 62 fifoCount -= packetSize;
ellingjp 7:33a74adff0ff 63
ellingjp 7:33a74adff0ff 64 // Get acceleration data
ellingjp 8:8430a5c0914c 65 mpu.dmpGetAccel(&aa, fifoBuffer);
ellingjp 8:8430a5c0914c 66 return &aa;
ellingjp 7:33a74adff0ff 67 }
ellingjp 8:8430a5c0914c 68
ellingjp 8:8430a5c0914c 69 return NULL;
ellingjp 7:33a74adff0ff 70 }
ellingjp 7:33a74adff0ff 71
ellingjp 8:8430a5c0914c 72 bool receive_close() {
ellingjp 8:8430a5c0914c 73 // Disable DMP and clear fifo
ellingjp 8:8430a5c0914c 74 mpu.setDMPEnabled(false);
ellingjp 8:8430a5c0914c 75 mpu.resetFIFO();
ellingjp 8:8430a5c0914c 76
ellingjp 8:8430a5c0914c 77 return true;
ellingjp 8:8430a5c0914c 78 }
ellingjp 8:8430a5c0914c 79
ellingjp 8:8430a5c0914c 80 bool receive_init() {
ellingjp 7:33a74adff0ff 81 PC_PRINTLN("Initializing MPU");
ellingjp 7:33a74adff0ff 82 mpu.initialize();
ellingjp 24:f2503d1256ad 83 PC_PRINTLN("MPU initialized, initializing DMP");
ellingjp 7:33a74adff0ff 84 devStatus = mpu.dmpInitialize();
ellingjp 7:33a74adff0ff 85
ellingjp 7:33a74adff0ff 86 if (devStatus == 0) {
ellingjp 7:33a74adff0ff 87 mpu.setDMPEnabled(true);
ellingjp 7:33a74adff0ff 88 packetSize = mpu.dmpGetFIFOPacketSize();
ellingjp 7:33a74adff0ff 89
ellingjp 7:33a74adff0ff 90 PC_PRINTLN("DMP Initialized successfully!");
ellingjp 7:33a74adff0ff 91 dmpReady = true;
ellingjp 7:33a74adff0ff 92 dataReady.rise(dataReadyISR);
ellingjp 8:8430a5c0914c 93 return true;
ellingjp 7:33a74adff0ff 94 } else { // ERROR
ellingjp 7:33a74adff0ff 95 PC_PRINTLNF("Error initializing MPU (code %d)", devStatus);
ellingjp 7:33a74adff0ff 96 DIE(DMP_ERROR_RATE);
ellingjp 8:8430a5c0914c 97 return false;
ellingjp 7:33a74adff0ff 98 }
ellingjp 20:294eaeaf2ebb 99
ellingjp 20:294eaeaf2ebb 100 PC_PRINTLN("Receive init success, capturing data...");
ellingjp 7:33a74adff0ff 101 }