Revision 2:8b8ea2b74666, committed 2015-02-20

Comitter:

Reimerguns

Date:

Fri Feb 20 20:04:37 2015 +0000

Parent:

1:91a599a98b76

Commit message:

Changed in this revision

--- a/MPU6050IMU.lib	Sun Feb 15 17:42:16 2015 +0000
+++ b/MPU6050IMU.lib	Fri Feb 20 20:04:37 2015 +0000
@@ -1,1 +1,1 @@
-http://developer.mbed.org/teams/The-A-Team/code/MPU6050IMU/#c9d3c37326f0
+http://developer.mbed.org/teams/The-A-Team/code/MPU6050IMU/#09b08e19167c

--- a/main.cpp	Sun Feb 15 17:42:16 2015 +0000
+++ b/main.cpp	Fri Feb 20 20:04:37 2015 +0000
@@ -1,11 +1,10 @@
-// SDA --- A4
-// SCL --- A5
+// SDA --- PTE25
+// SCL --- PTE24
 
 #include "mbed.h"
 #include "MPU6050.h"
 
-float sum = 0;
-uint32_t sumCount = 0;
+float dt,Yaw,gyroZ=0, lgyroZ = 0;
 
 MPU6050 mpu6050;
 Timer t;
@@ -28,25 +27,6 @@
         wait(1);
 
         mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
-        pc.printf("x-axis self test: acceleration trim within : ");
-        pc.printf("%f", SelfTest[0]);
-        pc.printf("% of factory value \n\r");
-        pc.printf("y-axis self test: acceleration trim within : ");
-        pc.printf("%f", SelfTest[1]);
-        pc.printf("% of factory value \n\r");
-        pc.printf("z-axis self test: acceleration trim within : ");
-        pc.printf("%f", SelfTest[2]);
-        pc.printf("% of factory value \n\r");
-        pc.printf("x-axis self test: gyration trim within : ");
-        pc.printf("%f", SelfTest[3]);
-        pc.printf("% of factory value \n\r");
-        pc.printf("y-axis self test: gyration trim within : ");
-        pc.printf("%f", SelfTest[4]);
-        pc.printf("% of factory value \n\r");
-        pc.printf("z-axis self test: gyration trim within : ");
-        pc.printf("%f", SelfTest[5]);
-        pc.printf("% of factory value \n\r");
-        wait(1);
 
         if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f) {
             mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
@@ -67,59 +47,18 @@
 
         // If data ready bit set, all data registers have new data
         if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) {  // check if data ready interrupt
-            mpu6050.readAccelData(accelCount);  // Read the x/y/z adc values
-            mpu6050.getAres();
-
-            // Now we'll calculate the accleration value into actual g's
-            ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
-            ay = (float)accelCount[1]*aRes - accelBias[1];
-            az = (float)accelCount[2]*aRes - accelBias[2];
-
             mpu6050.readGyroData(gyroCount);  // Read the x/y/z adc values
             mpu6050.getGres();
-
-            // Calculate the gyro value into actual degrees per second
-            gx = (float)gyroCount[0]*gRes; // - gyroBias[0];  // get actual gyro value, this depends on scale being set
-            gy = (float)gyroCount[1]*gRes; // - gyroBias[1];
-            gz = (float)gyroCount[2]*gRes; // - gyroBias[2];
-
-            tempCount = mpu6050.readTempData();  // Read the x/y/z adc values
-            temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
+            gyroZ = (float)gyroCount[2]*gRes; // - gyroBias[2];
         }
 
         Now = t.read_us();
-        deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
+        dt = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
         lastUpdate = Now;
-
-        sum += deltat;
-        sumCount++;
-
-        if(lastUpdate - firstUpdate > 10000000.0f) {
-            beta = 0.04;  // decrease filter gain after stabilized
-            zeta = 0.015; // increasey bias drift gain after stabilized
-        }
-
-        // Pass gyro rate as rad/s
-        mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
-
-        // Serial print and/or display at 0.5 s rate independent of data rates
-        delt_t = t.read_ms() - count;
-        if (delt_t > 500) { // update LCD once per half-second independent of read rate
-
-            yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
-            pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
-            roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
-            pitch *= 180.0f / PI;
-            yaw   *= 180.0f / PI;
-            roll  *= 180.0f / PI;
-
-            pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
-            pc.printf("average rate = %f\n\r", (float) sumCount/sum);
-
-            count = t.read_ms();
-            sum = 0;
-            sumCount = 0;
-        }
+        
+        Yaw += ((lgyroZ+gyroZ)/2)*dt;
+        lgyroZ = gyroZ;  
+        pc.printf("Yaw: %f\n\r", Yaw);
     }
 
 }
\ No newline at end of file