James Hutchinson
/
IMU6050
Header file and functions
Diff: MPU6050.cpp
- Revision:
- 9:898effccce30
- Parent:
- 8:b1570b99df9e
- Child:
- 10:bd9665d14241
diff -r b1570b99df9e -r 898effccce30 MPU6050.cpp --- 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 )