Diff: MPU6050.h

Revision:

5:01e1e68309ae

Parent:

4:778bc352c47f

--- a/MPU6050.h	Sun Oct 07 19:40:12 2018 +0000
+++ b/MPU6050.h	Mon Oct 15 18:30:20 2018 +0000
@@ -153,7 +153,7 @@
 int Ascale = AFS_2G;
  
 //Set up I2C, (SDA,SCL)
-I2C i2c(PB_9, PB_8);
+extern I2C i2c;
  
 DigitalOut myled(LED1);
    
@@ -175,7 +175,7 @@
 int count = 0;  // used to control display output rate
  
 // parameters for 6 DoF sensor fusion calculations
-float PI = 3.14159265358979323846f;
+//float PI = 3.14159265358979323846f;
 float GyroMeasError = PI * (60.0f / 180.0f);     // gyroscope measurement error in rads/s (start at 60 deg/s), then reduce after ~10 s to 3
 float beta = sqrt(3.0f / 4.0f) * GyroMeasError;  // compute beta
 float GyroMeasDrift = PI * (1.0f / 180.0f);      // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
@@ -466,8 +466,8 @@
     gyro_bias[1]  /= (int32_t) packet_count;
     gyro_bias[2]  /= (int32_t) packet_count;
     
-  if(accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) accelsensitivity;}  // Remove gravity from the z-axis accelerometer bias calculation
-  else {accel_bias[2] += (int32_t) accelsensitivity;}
+  if(accel_bias[1] > 0L) {accel_bias[1] -= (int32_t) accelsensitivity;}  // Remove gravity from the z-axis accelerometer bias calculation
+  else {accel_bias[1] += (int32_t) accelsensitivity;}
  
 // Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup
   data[0] = (-gyro_bias[0]/4  >> 8) & 0xFF; // Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input format
@@ -526,12 +526,12 @@
   data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers
  
   // Push accelerometer biases to hardware registers
-//  writeByte(MPU6050_ADDRESS, XA_OFFSET_H, data[0]);  
-//  writeByte(MPU6050_ADDRESS, XA_OFFSET_L_TC, data[1]);
-//  writeByte(MPU6050_ADDRESS, YA_OFFSET_H, data[2]);
-//  writeByte(MPU6050_ADDRESS, YA_OFFSET_L_TC, data[3]);  
-//  writeByte(MPU6050_ADDRESS, ZA_OFFSET_H, data[4]);
-//  writeByte(MPU6050_ADDRESS, ZA_OFFSET_L_TC, data[5]);
+  writeByte(MPU6050_ADDRESS, XA_OFFSET_H, data[0]);  
+  writeByte(MPU6050_ADDRESS, XA_OFFSET_L_TC, data[1]);
+  writeByte(MPU6050_ADDRESS, YA_OFFSET_H, data[2]);
+  writeByte(MPU6050_ADDRESS, YA_OFFSET_L_TC, data[3]);  
+  writeByte(MPU6050_ADDRESS, ZA_OFFSET_H, data[4]);
+  writeByte(MPU6050_ADDRESS, ZA_OFFSET_L_TC, data[5]);
  
 // Output scaled accelerometer biases for manual subtraction in the main program
    dest2[0] = (float)accel_bias[0]/(float)accelsensitivity;