Revision 11:9b414412b09e, committed 2015-02-20

Comitter:

moklumbys

Date:

Fri Feb 20 00:13:01 2015 +0000

Parent:

10:bd9665d14241

Commit message:

I modified some small things in the library and additionally added some functions for calculating actual angle values from the one read by the sensor.

Changed in this revision

diff -r bd9665d14241 -r 9b414412b09e MPU6050.cpp
--- 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);

diff -r bd9665d14241 -r 9b414412b09e MPU6050.h
--- a/MPU6050.h	Thu Feb 19 23:56:16 2015 +0000
+++ b/MPU6050.h	Fri Feb 20 00:13:01 2015 +0000
@@ -269,31 +269,54 @@
      */
      void read( char adress, char *data, int length);
      
-    //function for calculating the angle from the accelerometer.
-    //it takes 3 values which correspond acceleration in X, Y and Z direction and calculates angles in degrees
-    //for pitch, roll and one more direction.. (NOT YAW!)
-     void getAcceleroAngle( float *data );
+    /**
+    * function for calculating the angle from the accelerometer.
+    * it takes 3 values which correspond acceleration in X, Y and Z direction and calculates angles in degrees
+    * for pitch, roll and one more direction.. (NOT YAW!)
+    *
+    * @param data - angle calculated using only accelerometer
+    *
+    */ 
+    void getAcceleroAngle( float *data );
      
      
-     //function which allows to produce the offset values for gyro and accelerometer.
-     //offset for gyro is simply a value, which needs to be substracted from original gyro rad/sec speed
-     //but offset for accelerometer is calculated in angles... later on might change that
-     //function simply takes the number of samples to be taken and calculated the average
+     /**function which allows to produce the offset values for gyro and accelerometer.
+     * offset for gyro is simply a value, which needs to be substracted from original gyro rad/sec speed
+     * but offset for accelerometer is calculated in angles... later on might change that
+     * function simply takes the number of samples to be taken and calculated the average
+     * 
+     * @param accOffset - accelerometer offset in angle
+     * @param gyroOffset - gyroscope offset in rad/s
+     * @param sampleSize - number of samples to be taken for calculating offsets
+     *
+     */
      void getOffset(float *accOffset, float *gyroOffset, int sampleSize);
      
-     //function for computing the angle, when accelerometer angle offset and gyro offset are both known.
-     //we also need to know how much time passed from previous calculation to now
-     //it produces the angle in degrees. However angles are produced from -90.0 to 90.0 degrees
-     //if anyone need smth different, they can update this library...
-     void computeAngle (float *angle, float *accOffset, float *gyroOffset, float *currTime, float *prevTime);
+     /**
+     * function for computing the angle, when accelerometer angle offset and gyro offset are both known.
+     * we also need to know how much time passed from previous calculation to now
+     * it produces the angle in degrees. However angles are produced from -90.0 to 90.0 degrees
+     * if anyone need smth different, they can update this library...
+     *
+     * @param angle - calculated accurate angle from complemetary filter
+     * @param accOffset - offset in angle for the accelerometer
+     * @param gyroOffset - offset in rad/s for the gyroscope
+     * @param interval - time before previous angle calculation and now
+     *
+     */
+     void computeAngle (float *angle, float *accOffset, float *gyroOffset, float interval);
      
-     //function, which enables interrupts on MPU6050 INT pin
+     ///function, which enables interrupts on MPU6050 INT pin
      void enableInt( void );
      
-     //disables interrupts
+     ///disables interrupts
      void disableInt( void );
      
-     //function which sets the alpha value - constant for the complementary filter. default alpha = 0.97
+     /**function which sets the alpha value - constant for the complementary filter. default alpha = 0.97
+     *
+     * @param val - value the alpha (complementary filter constant) should be set to
+     *
+     */
      void setAlpha(float val);
      
      private: