Sheila Pham
/
IMU6050Ver1
Added for gyro and testing
Diff: MPU6050.cpp
- Revision:
- 11:9b414412b09e
- Parent:
- 10:bd9665d14241
- Child:
- 13:d66766523196
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);