File content as of revision 1:43f8ac7ca6d7:

/* Stabilisation */
#include "Task1.h"
#include "setup.h"

// MPU control/status vars
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 vars
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


#ifndef M_PI
#define M_PI 3.1415
#endif

// ================================================================
// === INTERRUPT DETECTION ROUTINE ===
// ================================================================

volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady()
{
    mpuInterrupt = true;
}
void Task1(void const *argument)
{
    // reset interrupt flag and get INT_STATUS byte
    mpuInterrupt = false;
    mpuIntStatus = imu.getIntStatus();

    // get current FIFO count
    fifoCount = imu.getFIFOCount();

    // check for overflow (this should never happen unless our code is too inefficient)
    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
        // 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();

        // 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] *= 180/M_PI;
        ypr[1] *= 180/M_PI;
        ypr[2] *= 180/M_PI;

        //BT.printf("YPR\t%3.2f\t%3.2f\t%3.2f\n", ypr[0]*180/M_PI, ypr[1]*180/M_PI, ypr[2]*180/M_PI);
        BT.printf("Y%3.2f\nP%3.2f\nR%3.2f\n", ypr[0] - ypr_offset[0], ypr[1] - ypr_offset[1], ypr[2] - ypr_offset[2]);
    }

    led1 = !led1;
}