File content as of revision 48:9dbdc4144f00:

/* File:        Task2_Master.cpp
 * Author:      Trung Tin Ian HUA
 * Date:        May 2014
 * Purpose:     Thread2M: Master PID control loop (attitude)
 * Functions:   AHRSSample: Read MPU6050 DMP and calculate YPR
 * Settings:    200Hz
 * Timing:
 */
#include "Task2_Slave.h"
#include "setup.h"
#include "PID.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_rad[3];           // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
volatile float ypr[3];      

float altitude, temperature;

#ifndef M_PI
#define M_PI 3.1415
#endif

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

/* YPR Adjust */
volatile float adjust_attitude[3] = {0.0, 0.0, 0.0};




// ===============================
// === YPR SAMPLE & MASTER PID ===
// ===============================
//Timer
void Task2_Master(void const *argument)
{
//Timer
    AHRSSample();

    if (armed) {
        switch (mode) {
            case RATE:
                break;

            case ATTITUDE:
            default:
//Timer
                pitchPIDattitude.setProcessValue((int) (ypr[1] - ypr_offset[1]));
                rollPIDattitude.setProcessValue((int) (ypr[2] - ypr_offset[2]));

                pitchPIDattitude.setSetPoint(inputYPR[1]);
                rollPIDattitude.setSetPoint(inputYPR[2]);

                adjust_attitude[1] = pitchPIDattitude.compute();
                adjust_attitude[2] = rollPIDattitude.compute();
                adjust_attitude[2] *= -1;
//Timer
                break;
        }
//Timer
    }
}




// ************************
// *** Helper functions ***
// ************************
void AHRSSample(void)
{
//Timer
    // reset interrupt flag and get INT_STATUS byte
    mpuIntStatus = imu_nb.getIntStatus();

    // get current FIFO count
    fifoCount = imu_nb.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_nb.resetFIFO();
        //imu.debugSerial.printf("FIFO overflow!");
        //BT.printf("FIFO overflow!\n");

        // 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_nb.getFIFOCount();

        while (fifoCount > 41) {
            // read a packet from FIFO
            imu_nb.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_nb.dmpGetQuaternion(&q, fifoBuffer);
        imu_nb.dmpGetGravity(&gravity, &q);
        imu_nb.dmpGetYawPitchRoll(ypr_rad, &q, &gravity);

        for (int i = 0; i < 3; i++)
            ypr[i] = ypr_rad[i] * 180/M_PI;
    }
//Timer
}