Diff: MPU6050.cpp

Revision:

11:9b414412b09e

Parent:

10:bd9665d14241

Child:

13:d66766523196

--- a/MPU6050.cpp	Thu Feb 19 23:56:16 2015 +0000
+++ b/MPU6050.cpp	Fri Feb 20 00:13:01 2015 +0000
@@ -235,8 +235,9 @@
     return retval;
 }
 
-//Additional function added by Montvydas Klumbys, which will allow easy offset, angle calculation and much more.
-//function for getting angles in degrees from accelerometer
+/**Additional function added by Montvydas Klumbys, which will allow easy offset, angle calculation and much more.
+ function for getting angles in degrees from accelerometer
+*/
 void MPU6050::getAcceleroAngle( float *data ) {
     float temp[3];
     this->getAccelero(temp);
@@ -249,7 +250,7 @@
 //    data[X_AXIS] = atan2 (-1*temp[X_AXIS], temp[Z_AXIS]) * RADIANS_TO_DEGREES;    //but it takes longer and system gets unstable when angles ~90 degrees
 }
 
-//function for getting offset values for the gyro & accelerometer
+///function for getting offset values for the gyro & accelerometer
 void MPU6050::getOffset(float *accOffset, float *gyroOffset, int sampleSize){
     float gyro[3];      
     float accAngle[3];
@@ -271,8 +272,8 @@
     } 
 }
 
-//function for computing angles for roll, pitch anf yaw
-void MPU6050::computeAngle (float *angle, float *accOffset, float *gyroOffset, float *currTime, float *prevTime){
+///function for computing angles for roll, pitch anf yaw
+void MPU6050::computeAngle (float *angle, float *accOffset, float *gyroOffset, float interval){
     float gyro[3];
     float accAngle[3];
     
@@ -285,19 +286,19 @@
     }
     
     //apply filters on pitch and roll to get accurate angle values
-    angle[X_AXIS] = alpha * (angle[X_AXIS] + GYRO_SCALE*gyro[X_AXIS]*(*currTime-*prevTime)) + (1-alpha)*accAngle[X_AXIS]; 
-    angle[Y_AXIS] = alpha * (angle[Y_AXIS] + GYRO_SCALE*gyro[Y_AXIS]*(*currTime-*prevTime)) + (1-alpha)*accAngle[Y_AXIS];
+    angle[X_AXIS] = alpha * (angle[X_AXIS] + GYRO_SCALE*gyro[X_AXIS]*interval) + (1-alpha)*accAngle[X_AXIS]; 
+    angle[Y_AXIS] = alpha * (angle[Y_AXIS] + GYRO_SCALE*gyro[Y_AXIS]*interval) + (1-alpha)*accAngle[Y_AXIS];
     
     //calculate Yaw using just the gyroscope values - inaccurate
-    angle[Z_AXIS] = angle[Z_AXIS] + GYRO_SCALE*gyro[Z_AXIS]*(*currTime-*prevTime);     
+    angle[Z_AXIS] = angle[Z_AXIS] + GYRO_SCALE*gyro[Z_AXIS]*interval;     
 }
 
-//function for setting a different Alpha value, which is used in complemetary filter calculations
+///function for setting a different Alpha value, which is used in complemetary filter calculations
 void MPU6050::setAlpha(float val){
     alpha = val;   
 }
 
-//function for enabling interrupts on MPU6050 INT pin, when the data is ready to take
+///function for enabling interrupts on MPU6050 INT pin, when the data is ready to take
 void MPU6050::enableInt( void ){
     char temp;
     temp = this->read(MPU6050_RA_INT_ENABLE);
@@ -305,7 +306,7 @@
     this->write(MPU6050_RA_INT_ENABLE, temp);
 }
 
-//function for disbling interrupts on MPU6050 INT pin, when the data is ready to take
+///function for disbling interrupts on MPU6050 INT pin, when the data is ready to take
 void MPU6050::disableInt ( void ){
     char temp;
     temp = this->read(MPU6050_RA_INT_ENABLE);