Isabella Gomez Torres
/
IMU6050
additional edits to James’ code
Diff: MPU6050.cpp
- Revision:
- 10:bd9665d14241
- Parent:
- 9:898effccce30
- Child:
- 11:9b414412b09e
diff -r 898effccce30 -r bd9665d14241 MPU6050.cpp --- 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