Took Kris Winer's code and added some functions for the MPU6050

Dependents:   buffer_algorithm

Fork of MPU6050 by Simon Garfieldsg

Files at this revision

API Documentation at this revision

Comitter:
byiu3
Date:
Sat Mar 14 15:44:48 2015 +0000
Parent:
2:19e22a4eaa18
Commit message:
Updated some of the functions in the MPU library;

Changed in this revision

I2Cdev.cpp Show annotated file Show diff for this revision Revisions of this file
MPU6050.cpp Show annotated file Show diff for this revision Revisions of this file
MPU6050.h Show annotated file Show diff for this revision Revisions of this file
diff -r 19e22a4eaa18 -r 6624faa750c7 I2Cdev.cpp
--- a/I2Cdev.cpp	Wed Feb 11 20:15:11 2015 +0000
+++ b/I2Cdev.cpp	Sat Mar 14 15:44:48 2015 +0000
@@ -9,12 +9,12 @@
 
 I2Cdev::I2Cdev(): debugSerial(USBTX, USBRX), i2c(I2C_SDA,I2C_SCL)
 {
-
+    i2c.frequency(400000);
 }
 
 I2Cdev::I2Cdev(PinName i2cSda, PinName i2cScl): debugSerial(USBTX, USBRX), i2c(i2cSda,i2cScl)
 {
-
+    i2c.frequency(400000);
 }
 
 /** Read a single bit from an 8-bit device register.
diff -r 19e22a4eaa18 -r 6624faa750c7 MPU6050.cpp
--- a/MPU6050.cpp	Wed Feb 11 20:15:11 2015 +0000
+++ b/MPU6050.cpp	Sat Mar 14 15:44:48 2015 +0000
@@ -54,15 +54,19 @@
  */
 MPU6050::MPU6050() : debugSerial(USBTX, USBRX)
 {
+    //i2Cdev.frequency(400000);
     first_update = 0;
     last_update = 0;
     cur_time = 0;
-    GyroMeasError = PI * (60.0f / 180.0f);
+    GyroMeasError = PI * (40.0f / 180.0f);
     beta = sqrt(3.0f / 4.0f) * GyroMeasError;
-    GyroMeasDrift = PI * (1.0f / 180.0f);
+    GyroMeasDrift = PI * (2.0f / 180.0f);
     zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift;
     timer.start();
-    
+    q[0] = 1.0f;
+    q[1] = 0.0f;
+    q[2] = 0.0f;
+    q[3] = 0.0f;
     devAddr = MPU6050_DEFAULT_ADDRESS;
 }
 
@@ -81,6 +85,10 @@
     beta = sqrt(3.0f / 4.0f) * GyroMeasError;
     GyroMeasDrift = PI * (1.0f / 180.0f);
     zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift;
+    q[0] = 1.0f;
+    q[1] = 0.0f;
+    q[2] = 0.0f;
+    q[3] = 0.0f;
     timer.start();
     
     devAddr = address;
@@ -3290,9 +3298,10 @@
  */
 void MPU6050::getAndConvertData(float *ax, float *ay, float *az, 
                                 float *yaw, float *pitch, float *roll,
-                                float *accel_bias, float *gyro_bias) {
+                                float *accel_bias, float *gyro_bias, 
+                                float *gx, float *gy, float *gz) {
     int16_t *tmp_ax, *tmp_ay, *tmp_az, *tmp_gx, *tmp_gy, *tmp_gz;
-    float ares = 0, gres = 0, g[3], q[4] = {1.0f, 0.0f, 0.0f, 0.0f};
+    float ares = 0, gres = 0, g[3];
     
     tmp_ax = (int16_t *) malloc(sizeof(*tmp_ax));
     tmp_ay = (int16_t *) malloc(sizeof(*tmp_ay));
@@ -3324,16 +3333,16 @@
     
     switch (this->getFullScaleGyroRange()) {
         case 0: 
+            gres = (float) 250/MPU6050_ADC_RANGE;
+            break;
+        case 1:
             gres = (float) 500/MPU6050_ADC_RANGE;
             break;
-        case 1:
+        case 2:
             gres = (float) 1000/MPU6050_ADC_RANGE;
             break;
-        case 2:
+        case 3:
             gres = (float) 2000/MPU6050_ADC_RANGE;
-            break;
-        case 3:
-            gres = (float) 4000/MPU6050_ADC_RANGE;
     }
     
 #ifdef useDebugSerial
@@ -3367,19 +3376,23 @@
     *ay = (float) (*tmp_ay) * ares;
     *az = (float) (*tmp_az) * ares;
 
-    g[0] = (float) (*tmp_gx)*gres; // Get actual rotation values
-    g[1] = (float) (*tmp_gy)*gres;
-    g[2] = (float) (*tmp_gz)*gres;
+    *gx = g[0] = (float) (*tmp_gx)*gres; // Get actual rotation values
+    *gy = g[1] = (float) (*tmp_gy)*gres;
+    *gz = g[2] = (float) (*tmp_gz)*gres;
     
 #ifdef useDebugSerial
     //debugSerial.printf("%f %f %f %f %f %f\n\r", *ax, *ay, *az, g[0], g[1], g[2]);
 #endif
 
-    // Function to generate quaternion
-    this->MadgwickQuaternionUpdate(ax, ay, az, g[0]*PI/180.0f, g[1]*PI/180.0f, g[2]*PI/180.0f, q);
+#ifdef useDebugSerial
+    //debugSerial.printf("Madgwick: %f, %f, %f, %f\n\r", q[0], q[1], q[2], q[3]);
+#endif
 
+    // Function to generate quaternion
+    this->MadgwickQuaternionUpdate(ax, ay, az, g[0]*PI/180.0f, g[1]*PI/180.0f, g[2]*PI/180.0f);
+    
 #ifdef useDebugSerial
-    //debugSerial.printf("Madgwick: %d, %d, %d, %d\n\r", q[0], q[1], q[2], q[3]);
+    //debugSerial.printf("Madgwick1: %f, %f, %f, %f\n\r", q[0], q[1], q[2], q[3]);
 #endif
 
     // Generate yaw, pitch, and roll
@@ -3388,9 +3401,9 @@
     *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]);
 
     // Change to degrees
-    //pitch *= 180.0f / PI;
-    //yaw *= 180.0f / PI;
-    //roll *= 180.0f / PI;
+    *pitch *= 180.0f / PI;
+    *yaw *= 180.0f / PI;
+    *roll *= 180.0f / PI;
     
     free(tmp_ax);
     free(tmp_ay);
@@ -3406,9 +3419,8 @@
  * gx, gy, gz won't change
  * q[0], q[1], q[2], q[3] will get populated
  */
-void MPU6050::MadgwickQuaternionUpdate(float *ax, float *ay, float *az, float gx, float gy, float gz, float *q) {
-
-    int cur_time;
+void MPU6050::MadgwickQuaternionUpdate(float *ax, float *ay, float *az, float gx, float gy, float gz) {
+    
     float deltat;
     float tmp_ax = *ax, tmp_ay = *ay, tmp_az = *az;
 
@@ -3433,14 +3445,9 @@
     //debugSerial.printf("MPU6050::Got into Madgwick Quaternion\n\r");
 #endif
     
-    cur_time = this->timer.read_us();
+    cur_time = timer.read_us();
     deltat = (float) ((cur_time - last_update)/1000000.0f); // Integration time by time elapsed since last filter update
     last_update = cur_time;
-
-    if (last_update - first_update > 10000000.0f) {
-        beta = 0.04; // Decrease filter gain after stabilized
-        zeta = 0.015; // Increase bias drift gain after stabilized
-    }
     
 #ifdef useDebugSerial
     //debugSerial.printf("cur_time: %d deltat: %f beta: %f zeta: %f\n\r");
@@ -3495,7 +3502,7 @@
     // Compute the quaternion derivative
     qDot1 = -_halfq2 * gx - _halfq3 * gy - _halfq4 * gz;
     qDot2 = _halfq1 * gx + _halfq3 * gz - _halfq4 * gy;
-    qDot3 = _halfq1 * gy + _halfq2 * gz + _halfq4 * gx;
+    qDot3 = _halfq1 * gy - _halfq2 * gz + _halfq4 * gx;
     qDot4 = _halfq1 * gz + _halfq2 * gy - _halfq3 * gx;
 
     // Compute then integrate estimated quaternion derivative
diff -r 19e22a4eaa18 -r 6624faa750c7 MPU6050.h
--- a/MPU6050.h	Wed Feb 11 20:15:11 2015 +0000
+++ b/MPU6050.h	Sat Mar 14 15:44:48 2015 +0000
@@ -735,8 +735,9 @@
         // ======== UNDOCUMENTED/DMP REGISTERS/METHODS ========
         
         // Get the data and convert it
-        void getAndConvertData(float *ax, float *ay, float *az, float *yaw, float *pitch, float *roll, float *accel_bias, float *gyro_bias);
-        void MadgwickQuaternionUpdate(float *ax, float *ay, float *az, float gx, float gy, float gz, float *q);
+        void getAndConvertData(float *ax, float *ay, float *az, float *yaw, float *pitch, float *roll, 
+                                float *accel_bias, float *gyro_bias, float *gx, float *gy, float *gz);
+        void MadgwickQuaternionUpdate(float *ax, float *ay, float *az, float gx, float gy, float gz);
         
         // XG_OFFS_TC register
         uint8_t getOTPBankValid();
@@ -1042,7 +1043,7 @@
         uint8_t devAddr;
         uint8_t buffer[14];
         
-        float last_update, first_update, cur_time;
+        int last_update, first_update, cur_time;
         
         // Gyroscope meas. error in rad/s (start at 60 deg/s), then reduce
         // after ~10 to 3
@@ -1053,6 +1054,8 @@
         // Compute zeta, the other free parameter in the Madgwick scheme
         // usually set to a small or zero value
         float zeta;
+        
+        float q[4];
 };
 
 #endif /* _MPU6050_H_ */
\ No newline at end of file