Sheila Pham
/
IMU6050Ver1
Added for gyro and testing
Diff: MPU6050.cpp
- Revision:
- 7:56e591a74939
- Parent:
- 6:502448484f91
- Child:
- 8:b1570b99df9e
diff -r 502448484f91 -r 56e591a74939 MPU6050.cpp --- a/MPU6050.cpp Thu Feb 12 21:01:38 2015 +0000 +++ b/MPU6050.cpp Fri Feb 13 01:04:17 2015 +0000 @@ -181,6 +181,27 @@ 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 hopefully :D + //Y = atan2(rawY,rawZ) * 180 / PI; //This spits out values between -180 to 180 (360 degrees) +} //-------------------------------------------------- //------------------Gyroscope----------------------- //--------------------------------------------------