File content as of revision 30:d9b988f8d84f:

/* 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
 * Timing:      1440us
 */
#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

#ifdef TIME_TASK1
Timer _t1;
#endif
// ================================================================
// === YPR THREAD ===
// ================================================================
void Task1(void const *argument)
{
    switch (box_demo) {
        case true:
            AHRSSample();
            
            BT.printf("
            break;

        case false:
#ifdef TIME_TASK1
            _t1.reset();
            _t1.start();
#endif
            if (armed) {
                switch (mode) {
                    case RATE:
                        break;

                    case ATTITUDE:
                    default:
                        AHRSSample();

                        counterTask1 = true;
                        break;
                }
            }
    }
}

// ================================================================
// === Helper functions ===
// ================================================================

void AHRSSample(void)
{
    // 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;
        */
    }
}