a
Fork of MPU6050 by
Diff: MPU6050.cpp
- Revision:
- 2:3e0dfce73a58
- Parent:
- 0:954f15bd95f1
- Child:
- 3:a173ad187e67
diff -r a248e65a25cc -r 3e0dfce73a58 MPU6050.cpp --- a/MPU6050.cpp Mon Jul 13 13:18:34 2015 +0000 +++ b/MPU6050.cpp Thu Jul 16 13:56:09 2015 +0000 @@ -1,5 +1,5 @@ /* @author: Baser Kandehir -* @date: July 9, 2015 +* @date: July 16, 2015 * @license: Use this code however you'd like */ @@ -115,16 +115,16 @@ void MPU6050::whoAmI() { uint8_t whoAmI = readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Should return 0x68 - ftdi.printf("I AM 0x%x \r\n",whoAmI); + pc.printf("I AM 0x%x \r\n",whoAmI); if(whoAmI==0x68) { - ftdi.printf("MPU6050 is online... \r\n"); + pc.printf("MPU6050 is online... \r\n"); led2=1; } else { - ftdi.printf("Could not connect to MPU6050 \r\nCheck the connections... \r\n"); + pc.printf("Could not connect to MPU6050 \r\nCheck the connections... \r\n"); toggler1.attach(&toggle_led1,0.1); // toggles led1 every 100 ms } } @@ -317,3 +317,35 @@ dest2[1] = gyro_bias[1]*gRes; dest2[2] = gyro_bias[2]*gRes; } + +void MPU6050::complementaryFilter(float* pitch, float* roll) +{ + /* Get actual acc value */ + readAccelData(accelData); + getAres(); + ax = accelData[0]*aRes - accelBias[0]; + ay = accelData[1]*aRes - accelBias[1]; + az = accelData[2]*aRes - accelBias[2]; + + /* Get actual gyro value */ + readGyroData(gyroData); + getGres(); + gx = gyroData[0]*gRes; // - gyroBias[0]; // Results are better without extracting gyroBias[i] + gy = gyroData[1]*gRes; // - gyroBias[1]; + gz = gyroData[2]*gRes; // - gyroBias[2]; + + float pitchAcc, rollAcc; + + /* Integrate the gyro data(deg/s) over time to get angle */ + *pitch += gx * dt; // Angle around the X-axis + *roll -= gy * dt; // Angle around the Y-axis + + /* Turning around the X-axis results in a vector on the Y-axis + whereas turning around the Y-axis results in a vector on the X-axis. */ + pitchAcc = atan2f(accelData[1], accelData[2])*180/PI; + 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; +}