CSE477 / swimate_v2

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   Adafruit_GFX_128x64 DS3231 PinDetect SDFileSystem USBDevice mbed RealtimeMath MODSERIAL

receive_data.cpp@7:33a74adff0ff, 2014-05-17 (annotated)

Committer:
ellingjp
Date:
Sat May 17 01:20:31 2014 +0000
Revision:
7:33a74adff0ff
Child:
8:8430a5c0914c
Refactor -- introduced bug that causes garbage to be received from mpu

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 7:33a74adff0ff 18 Quaternion q; // [w, x, y, z] quaternion container
ellingjp 7:33a74adff0ff 19 VectorInt16 aa; // [x, y, z] accel sensor measurements
ellingjp 7:33a74adff0ff 20 VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
ellingjp 7:33a74adff0ff 21 VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
ellingjp 7:33a74adff0ff 22 VectorFloat gravity; // [x, y, z] gravity vector
ellingjp 7:33a74adff0ff 23 float euler[3]; // [psi, theta, phi] Euler angle container
ellingjp 7:33a74adff0ff 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 7:33a74adff0ff 31 /* Returns a pointer to an array containing the most recent data, otherwise NULL. Requires the mpu to be initialized. */
ellingjp 7:33a74adff0ff 32 void receive_data()
ellingjp 7:33a74adff0ff 33 {
ellingjp 7:33a74adff0ff 34 static uint32_t n_overflows = 0;
ellingjp 7:33a74adff0ff 35 // while (true) {
ellingjp 7:33a74adff0ff 36 //if (!dmpReady) break; // do nothing if dmp not ready
ellingjp 7:33a74adff0ff 37 if (!dmpReady) return;
ellingjp 7:33a74adff0ff 38
ellingjp 7:33a74adff0ff 39 while (!mpuInterrupt && fifoCount < packetSize);
ellingjp 7:33a74adff0ff 40
ellingjp 7:33a74adff0ff 41 // Reset interrupt
ellingjp 7:33a74adff0ff 42 mpuInterrupt = false;
ellingjp 7:33a74adff0ff 43 mpuIntStatus = mpu.getIntStatus();
ellingjp 7:33a74adff0ff 44
ellingjp 7:33a74adff0ff 45 // get current FIFO count
ellingjp 7:33a74adff0ff 46 fifoCount = mpu.getFIFOCount();
ellingjp 7:33a74adff0ff 47
ellingjp 7:33a74adff0ff 48 // PC_PRINTF("%d, ", fifoCount); PC_PRINTF("%x, ", mpuIntStatus);
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 7:33a74adff0ff 55 // otherwise, check for DMP data ready interrupt (this should happen frequently)
ellingjp 7:33a74adff0ff 56 } else if (mpuIntStatus & 0x02) {
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 7:33a74adff0ff 65 mpu.dmpGetAccel(&aa, fifoBuffer);
ellingjp 7:33a74adff0ff 66 // mpu.dmpGetQuaternion(&q, fifoBuffer);
ellingjp 7:33a74adff0ff 67 // mpu.dmpGetGravity(&gravity, &q);
ellingjp 7:33a74adff0ff 68 // mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
ellingjp 7:33a74adff0ff 69 // mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
ellingjp 7:33a74adff0ff 70
ellingjp 7:33a74adff0ff 71 PC_PRINTF("%d, ", aa.x);
ellingjp 7:33a74adff0ff 72 PC_PRINTF("%d, ", aa.y);
ellingjp 7:33a74adff0ff 73 PC_PRINTLNF("%d", aa.z);
ellingjp 7:33a74adff0ff 74 // OLED_SETCURS(0, 10); OLED_PRINTF("%d, ", aaWorld.x); OLED_PRINTF("%d, ", aaWorld.y); OLED_PRINTLNF("%d", aaWorld.z);
ellingjp 7:33a74adff0ff 75
ellingjp 7:33a74adff0ff 76 // 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);
ellingjp 7:33a74adff0ff 77 // }
ellingjp 7:33a74adff0ff 78 }
ellingjp 7:33a74adff0ff 79 }
ellingjp 7:33a74adff0ff 80
ellingjp 7:33a74adff0ff 81 void receive_init() {
ellingjp 7:33a74adff0ff 82 PC_PRINTLN("Initializing MPU");
ellingjp 7:33a74adff0ff 83 mpu.initialize();
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 7:33a74adff0ff 93 } else { // ERROR
ellingjp 7:33a74adff0ff 94 PC_PRINTLNF("Error initializing MPU (code %d)", devStatus);
ellingjp 7:33a74adff0ff 95 DIE(DMP_ERROR_RATE);
ellingjp 7:33a74adff0ff 96 }
ellingjp 7:33a74adff0ff 97 }