File content as of revision 27:18b6580eb0b1:

/* File:        Task1.cpp
 * Author:      Trung Tin Ian HUA
 * Date:        May 2014
 * Purpose:     Thread1: Code to read Yaw Pitch Roll angles from MPU6050 DMP.
 * Settings:    100Hz
 */

#include "Task1.h"
#include "setup.h"

/* MPU6050 control/status variables: */
uint8_t mpuIntStatus;       // holds actual interrupt status byte from MPU
uint16_t fifoCount;         // count of all bytes currently in FIFO
uint8_t fifoBuffer[64];     // FIFO storage buffer

/* Orientation/motion variables: */
Quaternion q;               // [w, x, y, z] quaternion container
VectorFloat gravity;        // [x, y, z] gravity vector
float ypr[3];               // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
float ypr_1[3];

float altitude, temperature;
bool counterTask1 = false;

#ifndef M_PI
#define M_PI 3.1415
#endif

#ifdef ENABLE_COMPASS
//int compassX, compassY, compassZ;
double heading = 0;
#endif

// ================================================================
// === YPR ROUTINE ===
// ================================================================
void Task1(void const *argument)
{
    switch (mode) {
        case RATE:
            break;
            
        case ATTITUDE:
        default:
            // reset interrupt flag and get INT_STATUS byte
            mpuIntStatus = imu.getIntStatus();

            // get current FIFO count
            fifoCount = imu.getFIFOCount();
            //imu.debugSerial.printf("FIFO Count: %d\n", fifoCount);

            // check for overflow
            // Only keep a max of 2 packets in buffer.
            if ((mpuIntStatus & 0x10) || fifoCount > 84) {
                // reset so we can continue cleanly
                imu.resetFIFO();
                imu.debugSerial.printf("FIFO overflow!");

                // otherwise, check for DMP data ready interrupt (this should happen frequently)
            } else if (mpuIntStatus & 0x02) {
                // wait for correct available data length, should be a VERY short wait
                while (fifoCount < packetSize) fifoCount = imu.getFIFOCount();

                while (fifoCount > 41) {
                    // read a packet from FIFO
                    imu.getFIFOBytes(fifoBuffer, packetSize);

                    // track FIFO count here in case there is > 1 packet available
                    // (this lets us immediately read more without waiting for an interrupt)
                    fifoCount -= packetSize;
                }

                // display YPR angles in degrees
                imu.dmpGetQuaternion(&q, fifoBuffer);
                imu.dmpGetGravity(&gravity, &q);
                imu.dmpGetYawPitchRoll(ypr, &q, &gravity);

                ypr[0] = ypr[0] * 180/M_PI;
                ypr[1] = ypr[1] * 180/M_PI;
                ypr[2] = ypr[2] * 180/M_PI;

                /*
                if (compass.getDataReady()) {
                    // compass.getValues(&compass_x, &compass_y, &compass_z);
                    heading = compass.getHeadingXY() * 180/M_PI;
                }

                ypr[0] *= 0.98;
                ypr[0] += 0.02*heading;
                */

                if (box_demo)
                    BT.printf("\nY%3.2f\nP%3.2f\nR%3.2f\n", ypr[0] - ypr_offset[0],
                              ypr[1] - ypr_offset[1], ypr[2] - ypr_offset[2]);

                counterTask1 = true;
            }
            break;
    }
}