Diff: MPU6050.cpp

Revision:

3:6624faa750c7

Parent:

2:19e22a4eaa18

--- 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