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