Diff: MPU6050.cpp

Revision:

1:8f364d5295d8

Parent:

0:ffb90bdc9bb5

--- a/MPU6050.cpp	Thu Apr 28 23:13:10 2022 +0000
+++ b/MPU6050.cpp	Sun May 08 16:32:45 2022 +0000
@@ -67,7 +67,7 @@
 
 // Specify sensor full scale range
 int Ascale = AFS_2G;
-int Gscale = GFS_250DPS;
+int Gscale = GFS_500DPS;
 
 // Scale resolutions per LSB for the sensors
 float aRes, gRes; 
@@ -149,12 +149,10 @@
     if(whoAmI==0x68)
     {
         pc.printf("MPU6050 is online... \r\n");  
-        led2=1;
     }
     else
     {
         pc.printf("Could not connect to MPU6050 \r\nCheck the connections... \r\n");  
-        toggler1.attach(&toggle_led1,0.1);     // toggles led1 every 100 ms
     }  
 }
 
@@ -181,7 +179,7 @@
     
     /* Set sample rate = gyroscope output rate/(1+SMPLRT_DIV) */
     // SMPLRT_DIV=4 and sample rate=200 Hz (compatible with config above)
-    writeByte(MPU6050_ADDRESS, SMPLRT_DIV, 0x04);
+    writeByte(MPU6050_ADDRESS, SMPLRT_DIV, 0x00);
     
     /* Accelerometer configuration */
     uint8_t temp = readByte(MPU6050_ADDRESS, ACCEL_CONFIG);
@@ -222,7 +220,7 @@
     /* Turn the MSB LSB into signed 16-bit value */
     dest[0] = (int16_t)(((int16_t)rawData[0]<<8) | rawData[1]);  // GYRO_XOUT
     dest[1] = (int16_t)(((int16_t)rawData[2]<<8) | rawData[3]);  // GYRO_YOUT
-    dest[2] = (int16_t)(((int16_t)rawData[4]<<8) | rawData[5]);  // GYRO_ZOUT    
+    dest[2] = (int16_t)(((int16_t)rawData[4]<<8) | rawData[5]) * -1;  // GYRO_ZOUT    
 }
     
 int16_t MPU6050::readTempData()
@@ -377,7 +375,7 @@
     rollAcc  = atan2f(accelData[0], accelData[2])*180/PI;
       
     /* Apply Complementary Filter */
-    *pitch = *pitch * 0.98 + pitchAcc * 0.02;
-    *roll  = *roll  * 0.98 + rollAcc  * 0.02;
+    *pitch = *pitch * 0.995 + pitchAcc * 0.005;
+    *roll  = *roll  * 0.995 + rollAcc  * 0.005;
       
 }