Montvydas Klumbys
Thanks Erik Olieman for his beta library, that saved me a huge amount of time when getting Raw data from MPU6050 module! I was able to update this library by adding additional functions, which would allow a fast angle calculation by using complementary filter. I will probably be updating this library more soon to add additional functionality or make some changes that would look sensible.
Dependents: QuadcopterProgram 3DTracking ControlYokutan2017 Gyro ... more
Revision 9:898effccce30, committed 2015-02-19
- Comitter:
- moklumbys
- Date:
- Thu Feb 19 00:15:52 2015 +0000
- Parent:
- 8:b1570b99df9e
- Child:
- 10:bd9665d14241
- Commit message:
- additional fnctions - done.
Changed in this revision
| MPU6050.cpp | Show annotated file Show diff for this revision Revisions of this file |
| MPU6050.h | Show annotated file Show diff for this revision Revisions of this file |
--- a/MPU6050.cpp Fri Feb 13 01:16:00 2015 +0000
+++ b/MPU6050.cpp Thu Feb 19 00:15:52 2015 +0000
@@ -9,6 +9,7 @@
//Initializations:
currentGyroRange = 0;
currentAcceleroRange=0;
+ alpha = 0.97;
}
//--------------------------------------------------
@@ -153,10 +154,12 @@
float temp[3];
this->getAccelero(temp);
- data[X_AXIS] = -1*atan (-1*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] = -1*atan (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; //not sure what is this one :D
+ 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];
@@ -196,11 +199,15 @@
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 hopefully :D
- //Y = atan2(rawY,rawZ) * 180 / PI; //This spits out values between -180 to 180 (360 degrees)
+ 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 )
--- a/MPU6050.h Fri Feb 13 01:16:00 2015 +0000
+++ b/MPU6050.h Thu Feb 19 00:15:52 2015 +0000
@@ -83,9 +83,9 @@
#define RADIANS_TO_DEGREES 180/3.1415926536
-#define ALPHA 0.97 //filter constant
+#define ALPHA 0.96 //filter constant
-#define GYRO_SCALE 2.31 //scale the gyro
+#define GYRO_SCALE 2.7176 //scale the gyro
/** MPU6050 IMU library.
*
@@ -276,13 +276,14 @@
void computeAngle (float *angle, float *accOffset, float *gyroOffset, float *currTime, float *prevTime);
void enableInt( void );
void disableInt( void );
-
+ void setAlpha(float val);
private:
I2C connection;
char currentAcceleroRange;
char currentGyroRange;
+ float alpha;
};