![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
additional edits to James’ code
MPU6050.cpp@11:9b414412b09e, 2015-02-20 (annotated)
- Committer:
- moklumbys
- Date:
- Fri Feb 20 00:13:01 2015 +0000
- Revision:
- 11:9b414412b09e
- Parent:
- 10:bd9665d14241
I modified some small things in the library and additionally added some functions for calculating actual angle values from the one read by the sensor.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Sissors | 0:6757f7363a9f | 1 | /** |
Sissors | 0:6757f7363a9f | 2 | * Includes |
Sissors | 0:6757f7363a9f | 3 | */ |
Sissors | 0:6757f7363a9f | 4 | #include "MPU6050.h" |
Sissors | 0:6757f7363a9f | 5 | |
Sissors | 0:6757f7363a9f | 6 | MPU6050::MPU6050(PinName sda, PinName scl) : connection(sda, scl) { |
Sissors | 0:6757f7363a9f | 7 | this->setSleepMode(false); |
Sissors | 0:6757f7363a9f | 8 | |
Sissors | 0:6757f7363a9f | 9 | //Initializations: |
Sissors | 0:6757f7363a9f | 10 | currentGyroRange = 0; |
Sissors | 0:6757f7363a9f | 11 | currentAcceleroRange=0; |
moklumbys | 10:bd9665d14241 | 12 | alpha = ALPHA; |
Sissors | 0:6757f7363a9f | 13 | } |
Sissors | 0:6757f7363a9f | 14 | |
Sissors | 0:6757f7363a9f | 15 | //-------------------------------------------------- |
Sissors | 0:6757f7363a9f | 16 | //-------------------General------------------------ |
Sissors | 0:6757f7363a9f | 17 | //-------------------------------------------------- |
Sissors | 0:6757f7363a9f | 18 | |
Sissors | 0:6757f7363a9f | 19 | void MPU6050::write(char address, char data) { |
Sissors | 0:6757f7363a9f | 20 | char temp[2]; |
Sissors | 0:6757f7363a9f | 21 | temp[0]=address; |
Sissors | 0:6757f7363a9f | 22 | temp[1]=data; |
Sissors | 0:6757f7363a9f | 23 | |
Sissors | 0:6757f7363a9f | 24 | connection.write(MPU6050_ADDRESS * 2,temp,2); |
Sissors | 0:6757f7363a9f | 25 | } |
Sissors | 0:6757f7363a9f | 26 | |
Sissors | 0:6757f7363a9f | 27 | char MPU6050::read(char address) { |
Sissors | 0:6757f7363a9f | 28 | char retval; |
Sissors | 0:6757f7363a9f | 29 | connection.write(MPU6050_ADDRESS * 2, &address, 1, true); |
Sissors | 0:6757f7363a9f | 30 | connection.read(MPU6050_ADDRESS * 2, &retval, 1); |
Sissors | 0:6757f7363a9f | 31 | return retval; |
Sissors | 0:6757f7363a9f | 32 | } |
Sissors | 0:6757f7363a9f | 33 | |
Sissors | 0:6757f7363a9f | 34 | void MPU6050::read(char address, char *data, int length) { |
Sissors | 0:6757f7363a9f | 35 | connection.write(MPU6050_ADDRESS * 2, &address, 1, true); |
Sissors | 0:6757f7363a9f | 36 | connection.read(MPU6050_ADDRESS * 2, data, length); |
Sissors | 0:6757f7363a9f | 37 | } |
Sissors | 0:6757f7363a9f | 38 | |
Sissors | 0:6757f7363a9f | 39 | void MPU6050::setSleepMode(bool state) { |
Sissors | 0:6757f7363a9f | 40 | char temp; |
Sissors | 0:6757f7363a9f | 41 | temp = this->read(MPU6050_PWR_MGMT_1_REG); |
Sissors | 0:6757f7363a9f | 42 | if (state == true) |
Sissors | 0:6757f7363a9f | 43 | temp |= 1<<MPU6050_SLP_BIT; |
Sissors | 0:6757f7363a9f | 44 | if (state == false) |
Sissors | 0:6757f7363a9f | 45 | temp &= ~(1<<MPU6050_SLP_BIT); |
Sissors | 0:6757f7363a9f | 46 | this->write(MPU6050_PWR_MGMT_1_REG, temp); |
Sissors | 0:6757f7363a9f | 47 | } |
Sissors | 0:6757f7363a9f | 48 | |
Sissors | 0:6757f7363a9f | 49 | bool MPU6050::testConnection( void ) { |
Sissors | 0:6757f7363a9f | 50 | char temp; |
Sissors | 0:6757f7363a9f | 51 | temp = this->read(MPU6050_WHO_AM_I_REG); |
Sissors | 0:6757f7363a9f | 52 | return (temp == (MPU6050_ADDRESS & 0xFE)); |
Sissors | 0:6757f7363a9f | 53 | } |
Sissors | 0:6757f7363a9f | 54 | |
Sissors | 0:6757f7363a9f | 55 | void MPU6050::setBW(char BW) { |
Sissors | 0:6757f7363a9f | 56 | char temp; |
Sissors | 0:6757f7363a9f | 57 | BW=BW & 0x07; |
Sissors | 0:6757f7363a9f | 58 | temp = this->read(MPU6050_CONFIG_REG); |
Sissors | 0:6757f7363a9f | 59 | temp &= 0xF8; |
Sissors | 0:6757f7363a9f | 60 | temp = temp + BW; |
Sissors | 0:6757f7363a9f | 61 | this->write(MPU6050_CONFIG_REG, temp); |
Sissors | 0:6757f7363a9f | 62 | } |
Sissors | 0:6757f7363a9f | 63 | |
Sissors | 0:6757f7363a9f | 64 | void MPU6050::setI2CBypass(bool state) { |
Sissors | 0:6757f7363a9f | 65 | char temp; |
Sissors | 0:6757f7363a9f | 66 | temp = this->read(MPU6050_INT_PIN_CFG); |
Sissors | 0:6757f7363a9f | 67 | if (state == true) |
Sissors | 0:6757f7363a9f | 68 | temp |= 1<<MPU6050_BYPASS_BIT; |
Sissors | 0:6757f7363a9f | 69 | if (state == false) |
Sissors | 0:6757f7363a9f | 70 | temp &= ~(1<<MPU6050_BYPASS_BIT); |
Sissors | 0:6757f7363a9f | 71 | this->write(MPU6050_INT_PIN_CFG, temp); |
Sissors | 0:6757f7363a9f | 72 | } |
Sissors | 0:6757f7363a9f | 73 | |
Sissors | 0:6757f7363a9f | 74 | //-------------------------------------------------- |
Sissors | 0:6757f7363a9f | 75 | //----------------Accelerometer--------------------- |
Sissors | 0:6757f7363a9f | 76 | //-------------------------------------------------- |
Sissors | 0:6757f7363a9f | 77 | |
Sissors | 0:6757f7363a9f | 78 | void MPU6050::setAcceleroRange( char range ) { |
Sissors | 0:6757f7363a9f | 79 | char temp; |
Sissors | 0:6757f7363a9f | 80 | range = range & 0x03; |
Sissors | 0:6757f7363a9f | 81 | currentAcceleroRange = range; |
Sissors | 0:6757f7363a9f | 82 | |
Sissors | 0:6757f7363a9f | 83 | temp = this->read(MPU6050_ACCELERO_CONFIG_REG); |
Sissors | 0:6757f7363a9f | 84 | temp &= ~(3<<3); |
Sissors | 0:6757f7363a9f | 85 | temp = temp + (range<<3); |
Sissors | 0:6757f7363a9f | 86 | this->write(MPU6050_ACCELERO_CONFIG_REG, temp); |
Sissors | 0:6757f7363a9f | 87 | } |
Sissors | 0:6757f7363a9f | 88 | |
Sissors | 0:6757f7363a9f | 89 | int MPU6050::getAcceleroRawX( void ) { |
Sissors | 0:6757f7363a9f | 90 | short retval; |
Sissors | 0:6757f7363a9f | 91 | char data[2]; |
Sissors | 0:6757f7363a9f | 92 | this->read(MPU6050_ACCEL_XOUT_H_REG, data, 2); |
Sissors | 0:6757f7363a9f | 93 | retval = (data[0]<<8) + data[1]; |
Sissors | 0:6757f7363a9f | 94 | return (int)retval; |
Sissors | 0:6757f7363a9f | 95 | } |
Sissors | 0:6757f7363a9f | 96 | |
Sissors | 0:6757f7363a9f | 97 | int MPU6050::getAcceleroRawY( void ) { |
Sissors | 0:6757f7363a9f | 98 | short retval; |
Sissors | 0:6757f7363a9f | 99 | char data[2]; |
Sissors | 0:6757f7363a9f | 100 | this->read(MPU6050_ACCEL_YOUT_H_REG, data, 2); |
Sissors | 0:6757f7363a9f | 101 | retval = (data[0]<<8) + data[1]; |
Sissors | 0:6757f7363a9f | 102 | return (int)retval; |
Sissors | 0:6757f7363a9f | 103 | } |
Sissors | 0:6757f7363a9f | 104 | |
Sissors | 0:6757f7363a9f | 105 | int MPU6050::getAcceleroRawZ( void ) { |
Sissors | 0:6757f7363a9f | 106 | short retval; |
Sissors | 0:6757f7363a9f | 107 | char data[2]; |
Sissors | 0:6757f7363a9f | 108 | this->read(MPU6050_ACCEL_ZOUT_H_REG, data, 2); |
Sissors | 0:6757f7363a9f | 109 | retval = (data[0]<<8) + data[1]; |
Sissors | 0:6757f7363a9f | 110 | return (int)retval; |
Sissors | 0:6757f7363a9f | 111 | } |
Sissors | 0:6757f7363a9f | 112 | |
Sissors | 0:6757f7363a9f | 113 | void MPU6050::getAcceleroRaw( int *data ) { |
Sissors | 0:6757f7363a9f | 114 | char temp[6]; |
Sissors | 0:6757f7363a9f | 115 | this->read(MPU6050_ACCEL_XOUT_H_REG, temp, 6); |
Sissors | 0:6757f7363a9f | 116 | data[0] = (int)(short)((temp[0]<<8) + temp[1]); |
Sissors | 0:6757f7363a9f | 117 | data[1] = (int)(short)((temp[2]<<8) + temp[3]); |
Sissors | 0:6757f7363a9f | 118 | data[2] = (int)(short)((temp[4]<<8) + temp[5]); |
Sissors | 0:6757f7363a9f | 119 | } |
Sissors | 0:6757f7363a9f | 120 | |
Sissors | 0:6757f7363a9f | 121 | void MPU6050::getAccelero( float *data ) { |
Sissors | 0:6757f7363a9f | 122 | int temp[3]; |
Sissors | 0:6757f7363a9f | 123 | this->getAcceleroRaw(temp); |
Sissors | 0:6757f7363a9f | 124 | if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_2G) { |
Sissors | 0:6757f7363a9f | 125 | data[0]=(float)temp[0] / 16384.0 * 9.81; |
Sissors | 0:6757f7363a9f | 126 | data[1]=(float)temp[1] / 16384.0 * 9.81; |
Sissors | 0:6757f7363a9f | 127 | data[2]=(float)temp[2] / 16384.0 * 9.81; |
Sissors | 0:6757f7363a9f | 128 | } |
Sissors | 0:6757f7363a9f | 129 | if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_4G){ |
Sissors | 0:6757f7363a9f | 130 | data[0]=(float)temp[0] / 8192.0 * 9.81; |
Sissors | 0:6757f7363a9f | 131 | data[1]=(float)temp[1] / 8192.0 * 9.81; |
Sissors | 0:6757f7363a9f | 132 | data[2]=(float)temp[2] / 8192.0 * 9.81; |
Sissors | 0:6757f7363a9f | 133 | } |
Sissors | 0:6757f7363a9f | 134 | if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_8G){ |
Sissors | 0:6757f7363a9f | 135 | data[0]=(float)temp[0] / 4096.0 * 9.81; |
Sissors | 0:6757f7363a9f | 136 | data[1]=(float)temp[1] / 4096.0 * 9.81; |
Sissors | 0:6757f7363a9f | 137 | data[2]=(float)temp[2] / 4096.0 * 9.81; |
Sissors | 0:6757f7363a9f | 138 | } |
Sissors | 0:6757f7363a9f | 139 | if (currentAcceleroRange == MPU6050_ACCELERO_RANGE_16G){ |
Sissors | 0:6757f7363a9f | 140 | data[0]=(float)temp[0] / 2048.0 * 9.81; |
Sissors | 0:6757f7363a9f | 141 | data[1]=(float)temp[1] / 2048.0 * 9.81; |
Sissors | 0:6757f7363a9f | 142 | data[2]=(float)temp[2] / 2048.0 * 9.81; |
Sissors | 0:6757f7363a9f | 143 | } |
Sissors | 0:6757f7363a9f | 144 | |
Sissors | 0:6757f7363a9f | 145 | #ifdef DOUBLE_ACCELERO |
Sissors | 0:6757f7363a9f | 146 | data[0]*=2; |
Sissors | 0:6757f7363a9f | 147 | data[1]*=2; |
Sissors | 0:6757f7363a9f | 148 | data[2]*=2; |
Sissors | 0:6757f7363a9f | 149 | #endif |
Sissors | 0:6757f7363a9f | 150 | } |
Sissors | 0:6757f7363a9f | 151 | //-------------------------------------------------- |
Sissors | 0:6757f7363a9f | 152 | //------------------Gyroscope----------------------- |
Sissors | 0:6757f7363a9f | 153 | //-------------------------------------------------- |
Sissors | 0:6757f7363a9f | 154 | void MPU6050::setGyroRange( char range ) { |
Sissors | 0:6757f7363a9f | 155 | char temp; |
Sissors | 0:6757f7363a9f | 156 | currentGyroRange = range; |
Sissors | 0:6757f7363a9f | 157 | range = range & 0x03; |
Sissors | 0:6757f7363a9f | 158 | temp = this->read(MPU6050_GYRO_CONFIG_REG); |
Sissors | 0:6757f7363a9f | 159 | temp &= ~(3<<3); |
Sissors | 0:6757f7363a9f | 160 | temp = temp + range<<3; |
Sissors | 0:6757f7363a9f | 161 | this->write(MPU6050_GYRO_CONFIG_REG, temp); |
Sissors | 0:6757f7363a9f | 162 | } |
Sissors | 0:6757f7363a9f | 163 | |
Sissors | 0:6757f7363a9f | 164 | int MPU6050::getGyroRawX( void ) { |
Sissors | 0:6757f7363a9f | 165 | short retval; |
Sissors | 0:6757f7363a9f | 166 | char data[2]; |
Sissors | 0:6757f7363a9f | 167 | this->read(MPU6050_GYRO_XOUT_H_REG, data, 2); |
Sissors | 0:6757f7363a9f | 168 | retval = (data[0]<<8) + data[1]; |
Sissors | 0:6757f7363a9f | 169 | return (int)retval; |
Sissors | 0:6757f7363a9f | 170 | } |
Sissors | 0:6757f7363a9f | 171 | |
Sissors | 0:6757f7363a9f | 172 | int MPU6050::getGyroRawY( void ) { |
Sissors | 0:6757f7363a9f | 173 | short retval; |
Sissors | 0:6757f7363a9f | 174 | char data[2]; |
Sissors | 0:6757f7363a9f | 175 | this->read(MPU6050_GYRO_YOUT_H_REG, data, 2); |
Sissors | 0:6757f7363a9f | 176 | retval = (data[0]<<8) + data[1]; |
Sissors | 0:6757f7363a9f | 177 | return (int)retval; |
Sissors | 0:6757f7363a9f | 178 | } |
Sissors | 0:6757f7363a9f | 179 | |
Sissors | 0:6757f7363a9f | 180 | int MPU6050::getGyroRawZ( void ) { |
Sissors | 0:6757f7363a9f | 181 | short retval; |
Sissors | 0:6757f7363a9f | 182 | char data[2]; |
Sissors | 0:6757f7363a9f | 183 | this->read(MPU6050_GYRO_ZOUT_H_REG, data, 2); |
Sissors | 0:6757f7363a9f | 184 | retval = (data[0]<<8) + data[1]; |
Sissors | 0:6757f7363a9f | 185 | return (int)retval; |
Sissors | 0:6757f7363a9f | 186 | } |
Sissors | 0:6757f7363a9f | 187 | |
Sissors | 0:6757f7363a9f | 188 | void MPU6050::getGyroRaw( int *data ) { |
Sissors | 0:6757f7363a9f | 189 | char temp[6]; |
Sissors | 0:6757f7363a9f | 190 | this->read(MPU6050_GYRO_XOUT_H_REG, temp, 6); |
Sissors | 0:6757f7363a9f | 191 | data[0] = (int)(short)((temp[0]<<8) + temp[1]); |
Sissors | 0:6757f7363a9f | 192 | data[1] = (int)(short)((temp[2]<<8) + temp[3]); |
Sissors | 0:6757f7363a9f | 193 | data[2] = (int)(short)((temp[4]<<8) + temp[5]); |
Sissors | 0:6757f7363a9f | 194 | } |
Sissors | 0:6757f7363a9f | 195 | |
Sissors | 0:6757f7363a9f | 196 | void MPU6050::getGyro( float *data ) { |
Sissors | 0:6757f7363a9f | 197 | int temp[3]; |
Sissors | 0:6757f7363a9f | 198 | this->getGyroRaw(temp); |
Sissors | 1:a3366f09e95c | 199 | if (currentGyroRange == MPU6050_GYRO_RANGE_250) { |
moklumbys | 3:187152513f8d | 200 | data[0]=(float)temp[0] / 301.0; |
moklumbys | 3:187152513f8d | 201 | data[1]=(float)temp[1] / 301.0; |
moklumbys | 3:187152513f8d | 202 | data[2]=(float)temp[2] / 301.0; |
moklumbys | 3:187152513f8d | 203 | } //7505.5 |
Sissors | 1:a3366f09e95c | 204 | if (currentGyroRange == MPU6050_GYRO_RANGE_500){ |
Sissors | 0:6757f7363a9f | 205 | data[0]=(float)temp[0] / 3752.9; |
Sissors | 0:6757f7363a9f | 206 | data[1]=(float)temp[1] / 3752.9; |
Sissors | 0:6757f7363a9f | 207 | data[2]=(float)temp[2] / 3752.9; |
Sissors | 0:6757f7363a9f | 208 | } |
Sissors | 1:a3366f09e95c | 209 | if (currentGyroRange == MPU6050_GYRO_RANGE_1000){ |
Sissors | 0:6757f7363a9f | 210 | data[0]=(float)temp[0] / 1879.3;; |
Sissors | 0:6757f7363a9f | 211 | data[1]=(float)temp[1] / 1879.3; |
Sissors | 0:6757f7363a9f | 212 | data[2]=(float)temp[2] / 1879.3; |
Sissors | 0:6757f7363a9f | 213 | } |
Sissors | 1:a3366f09e95c | 214 | if (currentGyroRange == MPU6050_GYRO_RANGE_2000){ |
Sissors | 0:6757f7363a9f | 215 | data[0]=(float)temp[0] / 939.7; |
Sissors | 0:6757f7363a9f | 216 | data[1]=(float)temp[1] / 939.7; |
Sissors | 0:6757f7363a9f | 217 | data[2]=(float)temp[2] / 939.7; |
Sissors | 0:6757f7363a9f | 218 | } |
Sissors | 0:6757f7363a9f | 219 | } |
Sissors | 0:6757f7363a9f | 220 | //-------------------------------------------------- |
Sissors | 0:6757f7363a9f | 221 | //-------------------Temperature-------------------- |
Sissors | 0:6757f7363a9f | 222 | //-------------------------------------------------- |
Sissors | 0:6757f7363a9f | 223 | int MPU6050::getTempRaw( void ) { |
Sissors | 0:6757f7363a9f | 224 | short retval; |
Sissors | 0:6757f7363a9f | 225 | char data[2]; |
Sissors | 0:6757f7363a9f | 226 | this->read(MPU6050_TEMP_H_REG, data, 2); |
Sissors | 0:6757f7363a9f | 227 | retval = (data[0]<<8) + data[1]; |
Sissors | 0:6757f7363a9f | 228 | return (int)retval; |
Sissors | 0:6757f7363a9f | 229 | } |
Sissors | 0:6757f7363a9f | 230 | |
Sissors | 0:6757f7363a9f | 231 | float MPU6050::getTemp( void ) { |
Sissors | 0:6757f7363a9f | 232 | float retval; |
Sissors | 0:6757f7363a9f | 233 | retval=(float)this->getTempRaw(); |
Sissors | 0:6757f7363a9f | 234 | retval=(retval+521.0)/340.0+35.0; |
Sissors | 0:6757f7363a9f | 235 | return retval; |
Sissors | 0:6757f7363a9f | 236 | } |
Sissors | 0:6757f7363a9f | 237 | |
moklumbys | 11:9b414412b09e | 238 | /**Additional function added by Montvydas Klumbys, which will allow easy offset, angle calculation and much more. |
moklumbys | 11:9b414412b09e | 239 | function for getting angles in degrees from accelerometer |
moklumbys | 11:9b414412b09e | 240 | */ |
moklumbys | 10:bd9665d14241 | 241 | void MPU6050::getAcceleroAngle( float *data ) { |
moklumbys | 10:bd9665d14241 | 242 | float temp[3]; |
moklumbys | 10:bd9665d14241 | 243 | this->getAccelero(temp); |
moklumbys | 10:bd9665d14241 | 244 | |
moklumbys | 10:bd9665d14241 | 245 | 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 |
moklumbys | 10:bd9665d14241 | 246 | 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 |
moklumbys | 10:bd9665d14241 | 247 | 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 |
moklumbys | 10:bd9665d14241 | 248 | |
moklumbys | 10:bd9665d14241 | 249 | // data[Y_AXIS] = atan2 (temp[Y_AXIS],temp[Z_AXIS]) * RADIANS_TO_DEGREES; //This spits out values between -180 to 180 (360 degrees) |
moklumbys | 10:bd9665d14241 | 250 | // 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 |
moklumbys | 10:bd9665d14241 | 251 | } |
moklumbys | 10:bd9665d14241 | 252 | |
moklumbys | 11:9b414412b09e | 253 | ///function for getting offset values for the gyro & accelerometer |
moklumbys | 10:bd9665d14241 | 254 | void MPU6050::getOffset(float *accOffset, float *gyroOffset, int sampleSize){ |
moklumbys | 10:bd9665d14241 | 255 | float gyro[3]; |
moklumbys | 10:bd9665d14241 | 256 | float accAngle[3]; |
moklumbys | 10:bd9665d14241 | 257 | |
moklumbys | 10:bd9665d14241 | 258 | for (int i = 0; i < 3; i++) { |
moklumbys | 10:bd9665d14241 | 259 | accOffset[i] = 0.0; //initialise offsets to 0.0 |
moklumbys | 10:bd9665d14241 | 260 | gyroOffset[i] = 0.0; |
moklumbys | 10:bd9665d14241 | 261 | } |
moklumbys | 10:bd9665d14241 | 262 | |
moklumbys | 10:bd9665d14241 | 263 | for (int i = 0; i < sampleSize; i++){ |
moklumbys | 10:bd9665d14241 | 264 | this->getGyro(gyro); //take real life measurements |
moklumbys | 10:bd9665d14241 | 265 | this->getAcceleroAngle (accAngle); |
moklumbys | 10:bd9665d14241 | 266 | |
moklumbys | 10:bd9665d14241 | 267 | for (int j = 0; j < 3; j++){ |
moklumbys | 10:bd9665d14241 | 268 | *(accOffset+j) += accAngle[j]/sampleSize; //average measurements |
moklumbys | 10:bd9665d14241 | 269 | *(gyroOffset+j) += gyro[j]/sampleSize; |
moklumbys | 10:bd9665d14241 | 270 | } |
moklumbys | 10:bd9665d14241 | 271 | wait (0.01); //wait between each reading for accuracy |
moklumbys | 10:bd9665d14241 | 272 | } |
moklumbys | 10:bd9665d14241 | 273 | } |
moklumbys | 10:bd9665d14241 | 274 | |
moklumbys | 11:9b414412b09e | 275 | ///function for computing angles for roll, pitch anf yaw |
moklumbys | 11:9b414412b09e | 276 | void MPU6050::computeAngle (float *angle, float *accOffset, float *gyroOffset, float interval){ |
moklumbys | 10:bd9665d14241 | 277 | float gyro[3]; |
moklumbys | 10:bd9665d14241 | 278 | float accAngle[3]; |
moklumbys | 10:bd9665d14241 | 279 | |
moklumbys | 10:bd9665d14241 | 280 | this->getGyro(gyro); //get gyro value in rad/s |
moklumbys | 10:bd9665d14241 | 281 | this->getAcceleroAngle(accAngle); //get angle from accelerometer |
moklumbys | 10:bd9665d14241 | 282 | |
moklumbys | 10:bd9665d14241 | 283 | for (int i = 0; i < 3; i++){ |
moklumbys | 10:bd9665d14241 | 284 | gyro[i] -= gyroOffset[i]; //substract offset values |
moklumbys | 10:bd9665d14241 | 285 | accAngle[i] -= accOffset[i]; |
moklumbys | 10:bd9665d14241 | 286 | } |
moklumbys | 10:bd9665d14241 | 287 | |
moklumbys | 10:bd9665d14241 | 288 | //apply filters on pitch and roll to get accurate angle values |
moklumbys | 11:9b414412b09e | 289 | angle[X_AXIS] = alpha * (angle[X_AXIS] + GYRO_SCALE*gyro[X_AXIS]*interval) + (1-alpha)*accAngle[X_AXIS]; |
moklumbys | 11:9b414412b09e | 290 | angle[Y_AXIS] = alpha * (angle[Y_AXIS] + GYRO_SCALE*gyro[Y_AXIS]*interval) + (1-alpha)*accAngle[Y_AXIS]; |
moklumbys | 10:bd9665d14241 | 291 | |
moklumbys | 10:bd9665d14241 | 292 | //calculate Yaw using just the gyroscope values - inaccurate |
moklumbys | 11:9b414412b09e | 293 | angle[Z_AXIS] = angle[Z_AXIS] + GYRO_SCALE*gyro[Z_AXIS]*interval; |
moklumbys | 10:bd9665d14241 | 294 | } |
moklumbys | 10:bd9665d14241 | 295 | |
moklumbys | 11:9b414412b09e | 296 | ///function for setting a different Alpha value, which is used in complemetary filter calculations |
moklumbys | 10:bd9665d14241 | 297 | void MPU6050::setAlpha(float val){ |
moklumbys | 10:bd9665d14241 | 298 | alpha = val; |
moklumbys | 10:bd9665d14241 | 299 | } |
moklumbys | 10:bd9665d14241 | 300 | |
moklumbys | 11:9b414412b09e | 301 | ///function for enabling interrupts on MPU6050 INT pin, when the data is ready to take |
moklumbys | 10:bd9665d14241 | 302 | void MPU6050::enableInt( void ){ |
moklumbys | 10:bd9665d14241 | 303 | char temp; |
moklumbys | 10:bd9665d14241 | 304 | temp = this->read(MPU6050_RA_INT_ENABLE); |
moklumbys | 10:bd9665d14241 | 305 | temp |= 0x01; |
moklumbys | 10:bd9665d14241 | 306 | this->write(MPU6050_RA_INT_ENABLE, temp); |
moklumbys | 10:bd9665d14241 | 307 | } |
moklumbys | 10:bd9665d14241 | 308 | |
moklumbys | 11:9b414412b09e | 309 | ///function for disbling interrupts on MPU6050 INT pin, when the data is ready to take |
moklumbys | 10:bd9665d14241 | 310 | void MPU6050::disableInt ( void ){ |
moklumbys | 10:bd9665d14241 | 311 | char temp; |
moklumbys | 10:bd9665d14241 | 312 | temp = this->read(MPU6050_RA_INT_ENABLE); |
moklumbys | 10:bd9665d14241 | 313 | temp &= 0xFE; |
moklumbys | 10:bd9665d14241 | 314 | this->write(MPU6050_RA_INT_ENABLE, temp); |
moklumbys | 10:bd9665d14241 | 315 | } |