File content as of revision 0:8fe8d6dd7cd6:

#ifndef MBED_AngularDataAcquizition_H
#define MBED_AngularDataAcquizition_H

#include "I2Cdev.h"
#include "mbed.h"
#include "MPU6050_6Axis_MotionApps20.h" // works
#include <math.h>

#ifndef M_PI
#define M_PI 3.1415
#endif
#define D_BAUDRATE            115200

class AngularDataAcquizition{
    public:
        AngularDataAcquizition(PinName D_SDA, PinName D_SCL);
        void BeginInterrupt(float InterruptDuration);
        void StopInterrupt();
        void UpdateTheta();
        void MeanValue(float *FilteredVal, int num);
        void FilterVariablesAquizition(float *variable,int number);
        void callMeanFilteredReadings();

        float Meanypr[3];        
        bool dmpReady; // set true if DMP init was successful

    private:
    I2C _i2c;
    MPU6050 mpuLocal;
    Ticker timer;
    uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
    uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
    uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
    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
    VectorInt16 aa; // [x, y, z] accel sensor measurements
    VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
    VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
    VectorFloat gravity; // [x, y, z] gravity vector
    float euler[3]; // [psi, theta, phi] Euler angle container
    float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
    Serial pc;
    // packet structure for InvenSense teapot demo
    uint8_t teapotPacket[15];

    };







AngularDataAcquizition::AngularDataAcquizition(PinName D_SDA, PinName D_SCL): _i2c(D_SDA, D_SCL),pc(USBTX, USBRX) {
    dmpReady = false;
    teapotPacket[0] = '$',teapotPacket[1] =  0x02,teapotPacket[2] =  0,teapotPacket[3] = 0,teapotPacket[4] =  0,teapotPacket[5] = 0,teapotPacket[6] =  0,
    teapotPacket[7] = 0,teapotPacket[8] =  0,teapotPacket[9] = 0,teapotPacket[10] =  0x00,teapotPacket[11] =  0x00,teapotPacket[12] =  '\r',teapotPacket[13] =  '\n',teapotPacket[14] = 0 ;
    //Serial pc(USBTX, USBRX); // tx, rx

    pc.baud(D_BAUDRATE);
    // initialize device
    pc.printf("Initializing I2C devices...\n");
    mpuLocal.initialize();
   // verify connection
    pc.printf("Testing device connections...\n");
    
    bool mpu6050TestResult = mpuLocal.testConnection();
    if(mpu6050TestResult){
        pc.printf("mpuLocalLocalLocal6050 test passed \n");
    } else{
        pc.printf("mpuLocalLocalLocal6050 test failed \n");
    }  

    // wait for ready
    pc.printf("\nSend any character to begin DMP programming and demo: ");

    //while(!pc.readable());
      //      pc.getc();
    pc.printf("\n");
     
    // load and configure the DMP
    pc.printf("Initializing DMP...\n");
    devStatus = mpuLocal.dmpInitialize();

    // supply your own gyro offsets here, scaled for min sensitivity
    mpuLocal.setXGyroOffset(-61);
    mpuLocal.setYGyroOffset(-127);
    mpuLocal.setZGyroOffset(19);
    mpuLocal.setZAccelOffset(16282); // 1688 factory default for my test chip

    // make sure it worked (returns 0 if so)
    if (devStatus == 0) {
        // turn on the DMP, now that it's ready
        pc.printf("Enabling DMP...\n");
        mpuLocal.setDMPEnabled(true);

        // enable Arduino interrupt detection
        pc.printf("Enabling interrupt detection (Arduino external interrupt 0)...\n");
//        attachInterrupt(0, dmpDataReady, RISING);
        mpuIntStatus = mpuLocal.getIntStatus();

        // set our DMP Ready flag so the main loop() function knows it's okay to use it
        pc.printf("DMP ready! Waiting for first interrupt...\n");
        dmpReady = true;

        // get expected DMP packet size for later comparison
        packetSize = mpuLocal.dmpGetFIFOPacketSize();
    } else {
        // ERROR!
        // 1 = initial memory load failed
        // 2 = DMP configuration updates failed
        // (if it's going to break, usually the code will be 1)
        pc.printf("DMP Initialization failed (code ");
        pc.printf("%u",devStatus);
        pc.printf(")\n");
    }


}

void AngularDataAcquizition::UpdateTheta(){
            // reset interrupt flag and get INT_STATUS byte
        mpuIntStatus = mpuLocal.getIntStatus();
    
        // get current FIFO count
        fifoCount = mpuLocal.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
            mpuLocal.resetFIFO();
            pc.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 = mpuLocal.getFIFOCount();
    
            // read a packet from FIFO
            mpuLocal.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 Euler angles in degrees
            mpuLocal.dmpGetQuaternion(&q, fifoBuffer);
            mpuLocal.dmpGetGravity(&gravity, &q);
            mpuLocal.dmpGetYawPitchRoll(ypr, &q, &gravity);
        }
}
void AngularDataAcquizition::MeanValue(float *FilteredVal, int num){
        int i,j;
        for(j=0;j<3;j++)
                FilteredVal[j]=0;
        for (i=0;i<num;i++){
            UpdateTheta();
            for(j=0;j<3;j++)
                FilteredVal[j]=FilteredVal[j]+ypr[j];
            }
        for(j=0;j<3;j++)
            FilteredVal[j]=FilteredVal[j]/num;
}
void AngularDataAcquizition::callMeanFilteredReadings(){
    MeanValue(Meanypr,20);
    Meanypr[1]=Meanypr[1]-(1.19284-0.00716-0.7679)*3.14/180;
    Meanypr[2]=Meanypr[2]-(-0.96533+0.021043+0.107681)*3.14/180;

}
void AngularDataAcquizition::BeginInterrupt(float InterruptDuration){
    if(dmpReady)
        timer.attach(this,&AngularDataAcquizition::callMeanFilteredReadings, InterruptDuration);
    }
void AngularDataAcquizition::StopInterrupt(){
    if(dmpReady)
        timer.detach();
    }

#endif