Diff: AngularDataAcquizition.h

Revision:

0:8fe8d6dd7cd6

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AngularDataAcquizition.h	Tue Jun 23 14:47:52 2015 +0000
@@ -0,0 +1,181 @@
+#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
+