Diff: MPU6050.cpp

Revision:

10:bd9665d14241

Parent:

9:898effccce30

Child:

11:9b414412b09e

--- a/MPU6050.cpp	Thu Feb 19 00:15:52 2015 +0000
+++ b/MPU6050.cpp	Thu Feb 19 23:56:16 2015 +0000
@@ -9,7 +9,7 @@
     //Initializations:
     currentGyroRange = 0;
     currentAcceleroRange=0;
-    alpha = 0.97;
+    alpha = ALPHA;
 }
 
 //--------------------------------------------------
@@ -148,82 +148,6 @@
         data[2]*=2;
     #endif   
 }
-
-//function for getting angles from accelerometer
-void MPU6050::getAcceleroAngle( float *data ) {
-    float temp[3];
-    this->getAccelero(temp);
-    
-    data[X_AXIS] = atan (temp[Y_AXIS]/sqrt(pow(temp[X_AXIS], 2) + pow(temp[Z_AXIS], 2))) * RADIANS_TO_DEGREES; //calculate angle x(pitch/roll?) from accellerometer reading
-    data[Y_AXIS] = atan (-1*temp[X_AXIS]/sqrt(pow(temp[Y_AXIS], 2) + pow(temp[Z_AXIS], 2))) * RADIANS_TO_DEGREES; //calculate angle x(pitch/roll?) from accellerometer reading
-    data[Z_AXIS] = atan (sqrt(pow(temp[X_AXIS], 2) + pow(temp[Y_AXIS], 2))/temp[Z_AXIS]) * RADIANS_TO_DEGREES; //This one is not used anywhere later on
-    
-//    data[Y_AXIS] = atan2 (temp[Y_AXIS],temp[Z_AXIS]) * RADIANS_TO_DEGREES;  //This spits out values between -180 to 180 (360 degrees)
-//    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
-}
-void MPU6050::getOffset(float *accOffset, float *gyroOffset, int sampleSize){
-    float gyro[3];      
-    float accAngle[3];
-    
-    for (int i = 0; i < 3; i++) //initialise to 0.0 offsets
-    {
-        accOffset[i] = 0.0;
-        gyroOffset[i] = 0.0;
-    }
-    
-    for (int i = 0; i < sampleSize; i++)
-    {
-        this->getGyro(gyro); //take real life measurements  
-        this->getAcceleroAngle (accAngle);
-        
-        for (int j = 0; j < 3; j++)
-        {
-            *(accOffset+j) += accAngle[j]/sampleSize;    //average measurements
-            *(gyroOffset+j) += gyro[j]/sampleSize;
-        }
-        wait (0.01);    //wait between each reading for accuracy (maybe will not need later on) ****))#$(#@)$@#)(#@(%)@#(%@)(#%#@$--------------
-    } 
-}
-
-void MPU6050::computeAngle (float *angle, float *accOffset, float *gyroOffset, float *currTime, float *prevTime)
-{
-    float gyro[3];
-    float accAngle[3];
-    
-    this->getGyro(gyro);
-    this->getAcceleroAngle(accAngle);
-    
-    for (int i = 0; i < 3; i++)
-    {
-        gyro[i] -= gyroOffset[i];
-        accAngle[i] -= accOffset[i];
-    }
-    
-    angle[X_AXIS] = alpha * (angle[X_AXIS] + GYRO_SCALE*gyro[X_AXIS]*(*currTime-*prevTime)) + (1-alpha)*accAngle[X_AXIS];  //apply filter on both reading to get all angles
-    angle[Y_AXIS] = alpha * (angle[Y_AXIS] + GYRO_SCALE*gyro[Y_AXIS]*(*currTime-*prevTime)) + (1-alpha)*accAngle[Y_AXIS];
-  //angle[Z_AXIS] = alpha * (angle[Z_AXIS] + GYRO_SCALE*gyro[Z_AXIS]*(*currTime-*prevTime)) + (1-alpha)*accAngle[Z_AXIS];
-    angle[Z_AXIS] = angle[Z_AXIS] + GYRO_SCALE*gyro[Z_AXIS]*(*currTime-*prevTime);  //this is Yaw    
-}
-
-void MPU6050::setAlpha(float val)
-{
-    alpha = val;   
-}
-
-void MPU6050::enableInt( void )
-{
-    char temp;
-    temp = this->read(MPU6050_RA_INT_ENABLE);
-    temp |= 0x01;
-    this->write(MPU6050_RA_INT_ENABLE, temp);
-}
-void MPU6050::disableInt ( void )
-{
-    char temp;
-    temp = this->read(MPU6050_RA_INT_ENABLE);
-    temp &= 0xFE;
-    this->write(MPU6050_RA_INT_ENABLE, temp);
-}
 //--------------------------------------------------
 //------------------Gyroscope-----------------------
 //--------------------------------------------------
@@ -311,3 +235,80 @@
     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
+void MPU6050::getAcceleroAngle( float *data ) {
+    float temp[3];
+    this->getAccelero(temp);
+    
+    data[X_AXIS] = atan (temp[Y_AXIS]/sqrt(pow(temp[X_AXIS], 2) + pow(temp[Z_AXIS], 2))) * RADIANS_TO_DEGREES; //calculate angle x(pitch/roll?) from accellerometer reading
+    data[Y_AXIS] = atan (-1*temp[X_AXIS]/sqrt(pow(temp[Y_AXIS], 2) + pow(temp[Z_AXIS], 2))) * RADIANS_TO_DEGREES; //calculate angle x(pitch/roll?) from accellerometer reading
+    data[Z_AXIS] = atan (sqrt(pow(temp[X_AXIS], 2) + pow(temp[Y_AXIS], 2))/temp[Z_AXIS]) * RADIANS_TO_DEGREES; //This one is not used anywhere later on
+    
+//    data[Y_AXIS] = atan2 (temp[Y_AXIS],temp[Z_AXIS]) * RADIANS_TO_DEGREES;  //This spits out values between -180 to 180 (360 degrees)
+//    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
+void MPU6050::getOffset(float *accOffset, float *gyroOffset, int sampleSize){
+    float gyro[3];      
+    float accAngle[3];
+    
+    for (int i = 0; i < 3; i++) {
+        accOffset[i] = 0.0;     //initialise offsets to 0.0
+        gyroOffset[i] = 0.0;
+    }
+    
+    for (int i = 0; i < sampleSize; i++){
+        this->getGyro(gyro); //take real life measurements  
+        this->getAcceleroAngle (accAngle);
+        
+        for (int j = 0; j < 3; j++){
+            *(accOffset+j) += accAngle[j]/sampleSize;    //average measurements
+            *(gyroOffset+j) += gyro[j]/sampleSize;
+        }
+        wait (0.01);    //wait between each reading for accuracy 
+    } 
+}
+
+//function for computing angles for roll, pitch anf yaw
+void MPU6050::computeAngle (float *angle, float *accOffset, float *gyroOffset, float *currTime, float *prevTime){
+    float gyro[3];
+    float accAngle[3];
+    
+    this->getGyro(gyro);                //get gyro value in rad/s
+    this->getAcceleroAngle(accAngle);   //get angle from accelerometer
+    
+    for (int i = 0; i < 3; i++){
+        gyro[i] -= gyroOffset[i];       //substract offset values
+        accAngle[i] -= accOffset[i];    
+    }
+    
+    //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];
+    
+    //calculate Yaw using just the gyroscope values - inaccurate
+    angle[Z_AXIS] = angle[Z_AXIS] + GYRO_SCALE*gyro[Z_AXIS]*(*currTime-*prevTime);     
+}
+
+//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
+void MPU6050::enableInt( void ){
+    char temp;
+    temp = this->read(MPU6050_RA_INT_ENABLE);
+    temp |= 0x01;
+    this->write(MPU6050_RA_INT_ENABLE, temp);
+}
+
+//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);
+    temp &= 0xFE;
+    this->write(MPU6050_RA_INT_ENABLE, temp);
+}
\ No newline at end of file