エラー判定用 '\0' + クォータニオンのfloat型4成分,合計17byteのデータをひたすらバイナリ形式で送り続ける.最初のバイトが '\0' でなかったらズレているので,適当なバイト数見送って先頭が '\0' になるよう合わせること.
MPU9250/MPU9250.cpp@0:2a7221ee9861, 2018-03-21 (annotated)
- Committer:
- daqn
- Date:
- Wed Mar 21 07:58:57 2018 +0000
- Revision:
- 0:2a7221ee9861
i dont know what should i write here
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
daqn | 0:2a7221ee9861 | 1 | /** Daqn change log |
daqn | 0:2a7221ee9861 | 2 | * 2018/02/16 : Replace words "MPU6050" with "MPU9250". |
daqn | 0:2a7221ee9861 | 3 | * 2018/02/16 : L57, flip T/F by adding "!". |
daqn | 0:2a7221ee9861 | 4 | * 2018/02/21 : L |
daqn | 0:2a7221ee9861 | 5 | */ |
daqn | 0:2a7221ee9861 | 6 | |
daqn | 0:2a7221ee9861 | 7 | /** |
daqn | 0:2a7221ee9861 | 8 | * Includes |
daqn | 0:2a7221ee9861 | 9 | */ |
daqn | 0:2a7221ee9861 | 10 | #include "MPU9250.h" |
daqn | 0:2a7221ee9861 | 11 | |
daqn | 0:2a7221ee9861 | 12 | MPU9250::MPU9250(PinName sda, PinName scl) : connection(sda, scl) { |
daqn | 0:2a7221ee9861 | 13 | this->setSleepMode(false); |
daqn | 0:2a7221ee9861 | 14 | |
daqn | 0:2a7221ee9861 | 15 | //Initializations: |
daqn | 0:2a7221ee9861 | 16 | currentGyroRange = 0; |
daqn | 0:2a7221ee9861 | 17 | currentAcceleroRange=0; |
daqn | 0:2a7221ee9861 | 18 | alpha = ALPHA; |
daqn | 0:2a7221ee9861 | 19 | } |
daqn | 0:2a7221ee9861 | 20 | |
daqn | 0:2a7221ee9861 | 21 | //-------------------------------------------------- |
daqn | 0:2a7221ee9861 | 22 | //-------------------General------------------------ |
daqn | 0:2a7221ee9861 | 23 | //-------------------------------------------------- |
daqn | 0:2a7221ee9861 | 24 | |
daqn | 0:2a7221ee9861 | 25 | void MPU9250::write(char address, char data) { |
daqn | 0:2a7221ee9861 | 26 | char temp[2]; |
daqn | 0:2a7221ee9861 | 27 | temp[0]=address; |
daqn | 0:2a7221ee9861 | 28 | temp[1]=data; |
daqn | 0:2a7221ee9861 | 29 | |
daqn | 0:2a7221ee9861 | 30 | connection.write(MPU9250_ADDRESS * 2,temp,2); |
daqn | 0:2a7221ee9861 | 31 | } |
daqn | 0:2a7221ee9861 | 32 | |
daqn | 0:2a7221ee9861 | 33 | char MPU9250::read(char address) { |
daqn | 0:2a7221ee9861 | 34 | char retval; |
daqn | 0:2a7221ee9861 | 35 | connection.write(MPU9250_ADDRESS * 2, &address, 1, true); |
daqn | 0:2a7221ee9861 | 36 | connection.read(MPU9250_ADDRESS * 2, &retval, 1); |
daqn | 0:2a7221ee9861 | 37 | return retval; |
daqn | 0:2a7221ee9861 | 38 | } |
daqn | 0:2a7221ee9861 | 39 | |
daqn | 0:2a7221ee9861 | 40 | void MPU9250::read(char address, char *data, int length) { |
daqn | 0:2a7221ee9861 | 41 | connection.write(MPU9250_ADDRESS * 2, &address, 1, true); |
daqn | 0:2a7221ee9861 | 42 | connection.read(MPU9250_ADDRESS * 2, data, length); |
daqn | 0:2a7221ee9861 | 43 | } |
daqn | 0:2a7221ee9861 | 44 | |
daqn | 0:2a7221ee9861 | 45 | void MPU9250::setSleepMode(bool state) { |
daqn | 0:2a7221ee9861 | 46 | char temp; |
daqn | 0:2a7221ee9861 | 47 | temp = this->read(MPU9250_PWR_MGMT_1_REG); |
daqn | 0:2a7221ee9861 | 48 | if (state == true) |
daqn | 0:2a7221ee9861 | 49 | temp |= 1<<MPU9250_SLP_BIT; |
daqn | 0:2a7221ee9861 | 50 | if (state == false) |
daqn | 0:2a7221ee9861 | 51 | temp &= ~(1<<MPU9250_SLP_BIT); |
daqn | 0:2a7221ee9861 | 52 | this->write(MPU9250_PWR_MGMT_1_REG, temp); |
daqn | 0:2a7221ee9861 | 53 | } |
daqn | 0:2a7221ee9861 | 54 | |
daqn | 0:2a7221ee9861 | 55 | bool MPU9250::testConnection( void ) { |
daqn | 0:2a7221ee9861 | 56 | char temp; |
daqn | 0:2a7221ee9861 | 57 | temp = this->read(MPU9250_WHO_AM_I_REG); |
daqn | 0:2a7221ee9861 | 58 | return !(temp == (MPU9250_ADDRESS & 0xFE)); // Daqn 2018/02/16 |
daqn | 0:2a7221ee9861 | 59 | } |
daqn | 0:2a7221ee9861 | 60 | |
daqn | 0:2a7221ee9861 | 61 | void MPU9250::setBW(char BW) { |
daqn | 0:2a7221ee9861 | 62 | char temp; |
daqn | 0:2a7221ee9861 | 63 | BW=BW & 0x07; |
daqn | 0:2a7221ee9861 | 64 | temp = this->read(MPU9250_CONFIG_REG); |
daqn | 0:2a7221ee9861 | 65 | temp &= 0xF8; |
daqn | 0:2a7221ee9861 | 66 | temp = temp + BW; |
daqn | 0:2a7221ee9861 | 67 | this->write(MPU9250_CONFIG_REG, temp); |
daqn | 0:2a7221ee9861 | 68 | } |
daqn | 0:2a7221ee9861 | 69 | |
daqn | 0:2a7221ee9861 | 70 | void MPU9250::setI2CBypass(bool state) { |
daqn | 0:2a7221ee9861 | 71 | char temp; |
daqn | 0:2a7221ee9861 | 72 | temp = this->read(MPU9250_INT_PIN_CFG); |
daqn | 0:2a7221ee9861 | 73 | if (state == true) |
daqn | 0:2a7221ee9861 | 74 | temp |= 1<<MPU9250_BYPASS_BIT; |
daqn | 0:2a7221ee9861 | 75 | if (state == false) |
daqn | 0:2a7221ee9861 | 76 | temp &= ~(1<<MPU9250_BYPASS_BIT); |
daqn | 0:2a7221ee9861 | 77 | this->write(MPU9250_INT_PIN_CFG, temp); |
daqn | 0:2a7221ee9861 | 78 | } |
daqn | 0:2a7221ee9861 | 79 | |
daqn | 0:2a7221ee9861 | 80 | //-------------------------------------------------- |
daqn | 0:2a7221ee9861 | 81 | //----------------Accelerometer--------------------- |
daqn | 0:2a7221ee9861 | 82 | //-------------------------------------------------- |
daqn | 0:2a7221ee9861 | 83 | |
daqn | 0:2a7221ee9861 | 84 | void MPU9250::setAcceleroRange( char range ) { |
daqn | 0:2a7221ee9861 | 85 | char temp; |
daqn | 0:2a7221ee9861 | 86 | range = range & 0x03; |
daqn | 0:2a7221ee9861 | 87 | currentAcceleroRange = range; |
daqn | 0:2a7221ee9861 | 88 | |
daqn | 0:2a7221ee9861 | 89 | temp = this->read(MPU9250_ACCELERO_CONFIG_REG); |
daqn | 0:2a7221ee9861 | 90 | temp &= ~(3<<3); |
daqn | 0:2a7221ee9861 | 91 | temp = temp + (range<<3); |
daqn | 0:2a7221ee9861 | 92 | this->write(MPU9250_ACCELERO_CONFIG_REG, temp); |
daqn | 0:2a7221ee9861 | 93 | } |
daqn | 0:2a7221ee9861 | 94 | |
daqn | 0:2a7221ee9861 | 95 | int MPU9250::getAcceleroRawX( void ) { |
daqn | 0:2a7221ee9861 | 96 | short retval; |
daqn | 0:2a7221ee9861 | 97 | char data[2]; |
daqn | 0:2a7221ee9861 | 98 | this->read(MPU9250_ACCEL_XOUT_H_REG, data, 2); |
daqn | 0:2a7221ee9861 | 99 | retval = (data[0]<<8) + data[1]; |
daqn | 0:2a7221ee9861 | 100 | return (int)retval; |
daqn | 0:2a7221ee9861 | 101 | } |
daqn | 0:2a7221ee9861 | 102 | |
daqn | 0:2a7221ee9861 | 103 | int MPU9250::getAcceleroRawY( void ) { |
daqn | 0:2a7221ee9861 | 104 | short retval; |
daqn | 0:2a7221ee9861 | 105 | char data[2]; |
daqn | 0:2a7221ee9861 | 106 | this->read(MPU9250_ACCEL_YOUT_H_REG, data, 2); |
daqn | 0:2a7221ee9861 | 107 | retval = (data[0]<<8) + data[1]; |
daqn | 0:2a7221ee9861 | 108 | return (int)retval; |
daqn | 0:2a7221ee9861 | 109 | } |
daqn | 0:2a7221ee9861 | 110 | |
daqn | 0:2a7221ee9861 | 111 | int MPU9250::getAcceleroRawZ( void ) { |
daqn | 0:2a7221ee9861 | 112 | short retval; |
daqn | 0:2a7221ee9861 | 113 | char data[2]; |
daqn | 0:2a7221ee9861 | 114 | this->read(MPU9250_ACCEL_ZOUT_H_REG, data, 2); |
daqn | 0:2a7221ee9861 | 115 | retval = (data[0]<<8) + data[1]; |
daqn | 0:2a7221ee9861 | 116 | return (int)retval; |
daqn | 0:2a7221ee9861 | 117 | } |
daqn | 0:2a7221ee9861 | 118 | |
daqn | 0:2a7221ee9861 | 119 | void MPU9250::getAcceleroRaw( int *data ) { |
daqn | 0:2a7221ee9861 | 120 | char temp[6]; |
daqn | 0:2a7221ee9861 | 121 | this->read(MPU9250_ACCEL_XOUT_H_REG, temp, 6); |
daqn | 0:2a7221ee9861 | 122 | data[0] = (int)(short)((temp[0]<<8) + temp[1]); |
daqn | 0:2a7221ee9861 | 123 | data[1] = (int)(short)((temp[2]<<8) + temp[3]); |
daqn | 0:2a7221ee9861 | 124 | data[2] = (int)(short)((temp[4]<<8) + temp[5]); |
daqn | 0:2a7221ee9861 | 125 | } |
daqn | 0:2a7221ee9861 | 126 | |
daqn | 0:2a7221ee9861 | 127 | void MPU9250::getAccelero( float *data ) { |
daqn | 0:2a7221ee9861 | 128 | int temp[3]; |
daqn | 0:2a7221ee9861 | 129 | this->getAcceleroRaw(temp); |
daqn | 0:2a7221ee9861 | 130 | if (currentAcceleroRange == MPU9250_ACCELERO_RANGE_2G) { |
daqn | 0:2a7221ee9861 | 131 | data[0]=(float)temp[0] / 16384.0 * 9.81; |
daqn | 0:2a7221ee9861 | 132 | data[1]=(float)temp[1] / 16384.0 * 9.81; |
daqn | 0:2a7221ee9861 | 133 | data[2]=(float)temp[2] / 16384.0 * 9.81; |
daqn | 0:2a7221ee9861 | 134 | } |
daqn | 0:2a7221ee9861 | 135 | if (currentAcceleroRange == MPU9250_ACCELERO_RANGE_4G){ |
daqn | 0:2a7221ee9861 | 136 | data[0]=(float)temp[0] / 8192.0 * 9.81; |
daqn | 0:2a7221ee9861 | 137 | data[1]=(float)temp[1] / 8192.0 * 9.81; |
daqn | 0:2a7221ee9861 | 138 | data[2]=(float)temp[2] / 8192.0 * 9.81; |
daqn | 0:2a7221ee9861 | 139 | } |
daqn | 0:2a7221ee9861 | 140 | if (currentAcceleroRange == MPU9250_ACCELERO_RANGE_8G){ |
daqn | 0:2a7221ee9861 | 141 | data[0]=(float)temp[0] / 4096.0 * 9.81; |
daqn | 0:2a7221ee9861 | 142 | data[1]=(float)temp[1] / 4096.0 * 9.81; |
daqn | 0:2a7221ee9861 | 143 | data[2]=(float)temp[2] / 4096.0 * 9.81; |
daqn | 0:2a7221ee9861 | 144 | } |
daqn | 0:2a7221ee9861 | 145 | if (currentAcceleroRange == MPU9250_ACCELERO_RANGE_16G){ |
daqn | 0:2a7221ee9861 | 146 | data[0]=(float)temp[0] / 2048.0 * 9.81; |
daqn | 0:2a7221ee9861 | 147 | data[1]=(float)temp[1] / 2048.0 * 9.81; |
daqn | 0:2a7221ee9861 | 148 | data[2]=(float)temp[2] / 2048.0 * 9.81; |
daqn | 0:2a7221ee9861 | 149 | } |
daqn | 0:2a7221ee9861 | 150 | |
daqn | 0:2a7221ee9861 | 151 | #ifdef DOUBLE_ACCELERO |
daqn | 0:2a7221ee9861 | 152 | data[0]*=2; |
daqn | 0:2a7221ee9861 | 153 | data[1]*=2; |
daqn | 0:2a7221ee9861 | 154 | data[2]*=2; |
daqn | 0:2a7221ee9861 | 155 | #endif |
daqn | 0:2a7221ee9861 | 156 | } |
daqn | 0:2a7221ee9861 | 157 | //-------------------------------------------------- |
daqn | 0:2a7221ee9861 | 158 | //------------------Gyroscope----------------------- |
daqn | 0:2a7221ee9861 | 159 | //-------------------------------------------------- |
daqn | 0:2a7221ee9861 | 160 | void MPU9250::setGyroRange( char range ) { |
daqn | 0:2a7221ee9861 | 161 | char temp; |
daqn | 0:2a7221ee9861 | 162 | currentGyroRange = range; |
daqn | 0:2a7221ee9861 | 163 | range = range & 0x03; |
daqn | 0:2a7221ee9861 | 164 | temp = this->read(MPU9250_GYRO_CONFIG_REG); |
daqn | 0:2a7221ee9861 | 165 | temp &= ~(3<<3); |
daqn | 0:2a7221ee9861 | 166 | temp = temp + range<<3; |
daqn | 0:2a7221ee9861 | 167 | this->write(MPU9250_GYRO_CONFIG_REG, temp); |
daqn | 0:2a7221ee9861 | 168 | } |
daqn | 0:2a7221ee9861 | 169 | |
daqn | 0:2a7221ee9861 | 170 | int MPU9250::getGyroRawX( void ) { |
daqn | 0:2a7221ee9861 | 171 | short retval; |
daqn | 0:2a7221ee9861 | 172 | char data[2]; |
daqn | 0:2a7221ee9861 | 173 | this->read(MPU9250_GYRO_XOUT_H_REG, data, 2); |
daqn | 0:2a7221ee9861 | 174 | retval = (data[0]<<8) + data[1]; |
daqn | 0:2a7221ee9861 | 175 | return (int)retval; |
daqn | 0:2a7221ee9861 | 176 | } |
daqn | 0:2a7221ee9861 | 177 | |
daqn | 0:2a7221ee9861 | 178 | int MPU9250::getGyroRawY( void ) { |
daqn | 0:2a7221ee9861 | 179 | short retval; |
daqn | 0:2a7221ee9861 | 180 | char data[2]; |
daqn | 0:2a7221ee9861 | 181 | this->read(MPU9250_GYRO_YOUT_H_REG, data, 2); |
daqn | 0:2a7221ee9861 | 182 | retval = (data[0]<<8) + data[1]; |
daqn | 0:2a7221ee9861 | 183 | return (int)retval; |
daqn | 0:2a7221ee9861 | 184 | } |
daqn | 0:2a7221ee9861 | 185 | |
daqn | 0:2a7221ee9861 | 186 | int MPU9250::getGyroRawZ( void ) { |
daqn | 0:2a7221ee9861 | 187 | short retval; |
daqn | 0:2a7221ee9861 | 188 | char data[2]; |
daqn | 0:2a7221ee9861 | 189 | this->read(MPU9250_GYRO_ZOUT_H_REG, data, 2); |
daqn | 0:2a7221ee9861 | 190 | retval = (data[0]<<8) + data[1]; |
daqn | 0:2a7221ee9861 | 191 | return (int)retval; |
daqn | 0:2a7221ee9861 | 192 | } |
daqn | 0:2a7221ee9861 | 193 | |
daqn | 0:2a7221ee9861 | 194 | void MPU9250::getGyroRaw( int *data ) { |
daqn | 0:2a7221ee9861 | 195 | char temp[6]; |
daqn | 0:2a7221ee9861 | 196 | this->read(MPU9250_GYRO_XOUT_H_REG, temp, 6); |
daqn | 0:2a7221ee9861 | 197 | data[0] = (int)(short)((temp[0]<<8) + temp[1]); |
daqn | 0:2a7221ee9861 | 198 | data[1] = (int)(short)((temp[2]<<8) + temp[3]); |
daqn | 0:2a7221ee9861 | 199 | data[2] = (int)(short)((temp[4]<<8) + temp[5]); |
daqn | 0:2a7221ee9861 | 200 | } |
daqn | 0:2a7221ee9861 | 201 | |
daqn | 0:2a7221ee9861 | 202 | void MPU9250::getGyro( float *data ) { |
daqn | 0:2a7221ee9861 | 203 | int temp[3]; |
daqn | 0:2a7221ee9861 | 204 | this->getGyroRaw(temp); |
daqn | 0:2a7221ee9861 | 205 | if (currentGyroRange == MPU9250_GYRO_RANGE_250) { |
daqn | 0:2a7221ee9861 | 206 | data[0]=(float)temp[0] / 301.0; |
daqn | 0:2a7221ee9861 | 207 | data[1]=(float)temp[1] / 301.0; |
daqn | 0:2a7221ee9861 | 208 | data[2]=(float)temp[2] / 301.0; |
daqn | 0:2a7221ee9861 | 209 | } //7505.5 |
daqn | 0:2a7221ee9861 | 210 | if (currentGyroRange == MPU9250_GYRO_RANGE_500){ |
daqn | 0:2a7221ee9861 | 211 | data[0]=(float)temp[0] / 3752.9; |
daqn | 0:2a7221ee9861 | 212 | data[1]=(float)temp[1] / 3752.9; |
daqn | 0:2a7221ee9861 | 213 | data[2]=(float)temp[2] / 3752.9; |
daqn | 0:2a7221ee9861 | 214 | } |
daqn | 0:2a7221ee9861 | 215 | if (currentGyroRange == MPU9250_GYRO_RANGE_1000){ |
daqn | 0:2a7221ee9861 | 216 | data[0]=(float)temp[0] / 1879.3;; |
daqn | 0:2a7221ee9861 | 217 | data[1]=(float)temp[1] / 1879.3; |
daqn | 0:2a7221ee9861 | 218 | data[2]=(float)temp[2] / 1879.3; |
daqn | 0:2a7221ee9861 | 219 | } |
daqn | 0:2a7221ee9861 | 220 | if (currentGyroRange == MPU9250_GYRO_RANGE_2000){ |
daqn | 0:2a7221ee9861 | 221 | data[0]=(float)temp[0] / 939.7; |
daqn | 0:2a7221ee9861 | 222 | data[1]=(float)temp[1] / 939.7; |
daqn | 0:2a7221ee9861 | 223 | data[2]=(float)temp[2] / 939.7; |
daqn | 0:2a7221ee9861 | 224 | } |
daqn | 0:2a7221ee9861 | 225 | } |
daqn | 0:2a7221ee9861 | 226 | //-------------------------------------------------- |
daqn | 0:2a7221ee9861 | 227 | //-------------------Temperature-------------------- |
daqn | 0:2a7221ee9861 | 228 | //-------------------------------------------------- |
daqn | 0:2a7221ee9861 | 229 | int MPU9250::getTempRaw( void ) { |
daqn | 0:2a7221ee9861 | 230 | short retval; |
daqn | 0:2a7221ee9861 | 231 | char data[2]; |
daqn | 0:2a7221ee9861 | 232 | this->read(MPU9250_TEMP_H_REG, data, 2); |
daqn | 0:2a7221ee9861 | 233 | retval = (data[0]<<8) + data[1]; |
daqn | 0:2a7221ee9861 | 234 | return (int)retval; |
daqn | 0:2a7221ee9861 | 235 | } |
daqn | 0:2a7221ee9861 | 236 | |
daqn | 0:2a7221ee9861 | 237 | float MPU9250::getTemp( void ) { |
daqn | 0:2a7221ee9861 | 238 | float retval; |
daqn | 0:2a7221ee9861 | 239 | retval=(float)this->getTempRaw(); |
daqn | 0:2a7221ee9861 | 240 | retval=(retval+521.0)/340.0+35.0; |
daqn | 0:2a7221ee9861 | 241 | return retval; |
daqn | 0:2a7221ee9861 | 242 | } |
daqn | 0:2a7221ee9861 | 243 | |
daqn | 0:2a7221ee9861 | 244 | |
daqn | 0:2a7221ee9861 | 245 | //-------------------------------------------------- |
daqn | 0:2a7221ee9861 | 246 | //------------------Magnetometer-------------------- |
daqn | 0:2a7221ee9861 | 247 | //-------------------------------------------------- |
daqn | 0:2a7221ee9861 | 248 | void MPU9250::read(char address, char subaddress, char *data, int length) |
daqn | 0:2a7221ee9861 | 249 | { |
daqn | 0:2a7221ee9861 | 250 | // connection.write(MPU9250_ADDRESS * 2, &address, 1, true); |
daqn | 0:2a7221ee9861 | 251 | // connection.read(MPU9250_ADDRESS * 2, data, length); |
daqn | 0:2a7221ee9861 | 252 | connection.write(address, &subaddress, 1, true); |
daqn | 0:2a7221ee9861 | 253 | connection.read(address, data, length); |
daqn | 0:2a7221ee9861 | 254 | } |
daqn | 0:2a7221ee9861 | 255 | |
daqn | 0:2a7221ee9861 | 256 | //void MPU9250::setMagnetoRange( char range ) { |
daqn | 0:2a7221ee9861 | 257 | // char temp; |
daqn | 0:2a7221ee9861 | 258 | // currentMagnetoRange = range; |
daqn | 0:2a7221ee9861 | 259 | // range = range & 0x03; |
daqn | 0:2a7221ee9861 | 260 | // temp = this->read(MPU9250_MAGNETO_CONFIG_REG); |
daqn | 0:2a7221ee9861 | 261 | // temp &= ~(3<<3); |
daqn | 0:2a7221ee9861 | 262 | // temp = temp + range<<3; |
daqn | 0:2a7221ee9861 | 263 | // this->write(MPU9250_MAGNETO_CONFIG_REG, temp); |
daqn | 0:2a7221ee9861 | 264 | //} |
daqn | 0:2a7221ee9861 | 265 | |
daqn | 0:2a7221ee9861 | 266 | |
daqn | 0:2a7221ee9861 | 267 | |
daqn | 0:2a7221ee9861 | 268 | int MPU9250::getMagnetoRawX( void ) |
daqn | 0:2a7221ee9861 | 269 | { |
daqn | 0:2a7221ee9861 | 270 | short retval; |
daqn | 0:2a7221ee9861 | 271 | char data[2]; |
daqn | 0:2a7221ee9861 | 272 | this->read(AK8963_ADDRESS, AK8963_XOUT_L, data, 2); |
daqn | 0:2a7221ee9861 | 273 | retval = (data[0]<<8) + data[1]; |
daqn | 0:2a7221ee9861 | 274 | return (int)retval; |
daqn | 0:2a7221ee9861 | 275 | } |
daqn | 0:2a7221ee9861 | 276 | |
daqn | 0:2a7221ee9861 | 277 | int MPU9250::getMagnetoRawY( void ) |
daqn | 0:2a7221ee9861 | 278 | { |
daqn | 0:2a7221ee9861 | 279 | short retval; |
daqn | 0:2a7221ee9861 | 280 | char data[2]; |
daqn | 0:2a7221ee9861 | 281 | this->read(AK8963_ADDRESS, AK8963_YOUT_L, data, 2); |
daqn | 0:2a7221ee9861 | 282 | retval = (data[0]<<8) + data[1]; |
daqn | 0:2a7221ee9861 | 283 | return (int)retval; |
daqn | 0:2a7221ee9861 | 284 | } |
daqn | 0:2a7221ee9861 | 285 | // |
daqn | 0:2a7221ee9861 | 286 | int MPU9250::getMagnetoRawZ( void ) |
daqn | 0:2a7221ee9861 | 287 | { |
daqn | 0:2a7221ee9861 | 288 | short retval; |
daqn | 0:2a7221ee9861 | 289 | char data[2]; |
daqn | 0:2a7221ee9861 | 290 | this->read(AK8963_ADDRESS, AK8963_ZOUT_L, data, 2); |
daqn | 0:2a7221ee9861 | 291 | retval = (data[0]<<8) + data[1]; |
daqn | 0:2a7221ee9861 | 292 | return (int)retval; |
daqn | 0:2a7221ee9861 | 293 | } |
daqn | 0:2a7221ee9861 | 294 | |
daqn | 0:2a7221ee9861 | 295 | void MPU9250::getMagnetoRaw( int *data ) { |
daqn | 0:2a7221ee9861 | 296 | char temp[6]; |
daqn | 0:2a7221ee9861 | 297 | this->read(AK8963_ADDRESS, AK8963_XOUT_L, temp, 6); |
daqn | 0:2a7221ee9861 | 298 | data[0] = (int)(short)((temp[1]<<8) + temp[0]); |
daqn | 0:2a7221ee9861 | 299 | data[1] = (int)(short)((temp[3]<<8) + temp[2]); |
daqn | 0:2a7221ee9861 | 300 | data[2] = (int)(short)((temp[5]<<8) + temp[4]); |
daqn | 0:2a7221ee9861 | 301 | } |
daqn | 0:2a7221ee9861 | 302 | |
daqn | 0:2a7221ee9861 | 303 | void MPU9250::getMagneto( float *data ) { |
daqn | 0:2a7221ee9861 | 304 | int temp[3]; |
daqn | 0:2a7221ee9861 | 305 | this->getMagnetoRaw(temp); |
daqn | 0:2a7221ee9861 | 306 | data[0]=(float)temp[0] * .15f; |
daqn | 0:2a7221ee9861 | 307 | data[1]=(float)temp[1] * .15f; |
daqn | 0:2a7221ee9861 | 308 | data[2]=(float)temp[2] * .15f; |
daqn | 0:2a7221ee9861 | 309 | } |
daqn | 0:2a7221ee9861 | 310 | |
daqn | 0:2a7221ee9861 | 311 | /**Additional function added by Montvydas Klumbys, which will allow easy offset, angle calculation and much more. |
daqn | 0:2a7221ee9861 | 312 | function for getting angles in degrees from accelerometer |
daqn | 0:2a7221ee9861 | 313 | */ |
daqn | 0:2a7221ee9861 | 314 | void MPU9250::getAcceleroAngle( float *data ) { |
daqn | 0:2a7221ee9861 | 315 | float temp[3]; |
daqn | 0:2a7221ee9861 | 316 | this->getAccelero(temp); |
daqn | 0:2a7221ee9861 | 317 | |
daqn | 0:2a7221ee9861 | 318 | 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 |
daqn | 0:2a7221ee9861 | 319 | 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 |
daqn | 0:2a7221ee9861 | 320 | 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 |
daqn | 0:2a7221ee9861 | 321 | |
daqn | 0:2a7221ee9861 | 322 | // data[Y_AXIS] = atan2 (temp[Y_AXIS],temp[Z_AXIS]) * RADIANS_TO_DEGREES; //This spits out values between -180 to 180 (360 degrees) |
daqn | 0:2a7221ee9861 | 323 | // 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 |
daqn | 0:2a7221ee9861 | 324 | } |
daqn | 0:2a7221ee9861 | 325 | |
daqn | 0:2a7221ee9861 | 326 | ///function for getting offset values for the gyro & accelerometer |
daqn | 0:2a7221ee9861 | 327 | void MPU9250::getOffset(float *accOffset, float *gyroOffset, int sampleSize){ |
daqn | 0:2a7221ee9861 | 328 | float gyro[3]; |
daqn | 0:2a7221ee9861 | 329 | float accAngle[3]; |
daqn | 0:2a7221ee9861 | 330 | |
daqn | 0:2a7221ee9861 | 331 | for (int i = 0; i < 3; i++) { |
daqn | 0:2a7221ee9861 | 332 | accOffset[i] = 0.0; //initialise offsets to 0.0 |
daqn | 0:2a7221ee9861 | 333 | gyroOffset[i] = 0.0; |
daqn | 0:2a7221ee9861 | 334 | } |
daqn | 0:2a7221ee9861 | 335 | |
daqn | 0:2a7221ee9861 | 336 | for (int i = 0; i < sampleSize; i++){ |
daqn | 0:2a7221ee9861 | 337 | this->getGyro(gyro); //take real life measurements |
daqn | 0:2a7221ee9861 | 338 | this->getAcceleroAngle (accAngle); |
daqn | 0:2a7221ee9861 | 339 | |
daqn | 0:2a7221ee9861 | 340 | for (int j = 0; j < 3; j++){ |
daqn | 0:2a7221ee9861 | 341 | *(accOffset+j) += accAngle[j]/sampleSize; //average measurements |
daqn | 0:2a7221ee9861 | 342 | *(gyroOffset+j) += gyro[j]/sampleSize; |
daqn | 0:2a7221ee9861 | 343 | } |
daqn | 0:2a7221ee9861 | 344 | wait (0.01); //wait between each reading for accuracy |
daqn | 0:2a7221ee9861 | 345 | } |
daqn | 0:2a7221ee9861 | 346 | } |
daqn | 0:2a7221ee9861 | 347 | |
daqn | 0:2a7221ee9861 | 348 | ///function for computing angles for roll, pitch anf yaw |
daqn | 0:2a7221ee9861 | 349 | void MPU9250::computeAngle (float *angle, float *accOffset, float *gyroOffset, float interval){ |
daqn | 0:2a7221ee9861 | 350 | float gyro[3]; |
daqn | 0:2a7221ee9861 | 351 | float accAngle[3]; |
daqn | 0:2a7221ee9861 | 352 | |
daqn | 0:2a7221ee9861 | 353 | this->getGyro(gyro); //get gyro value in rad/s |
daqn | 0:2a7221ee9861 | 354 | this->getAcceleroAngle(accAngle); //get angle from accelerometer |
daqn | 0:2a7221ee9861 | 355 | |
daqn | 0:2a7221ee9861 | 356 | for (int i = 0; i < 3; i++){ |
daqn | 0:2a7221ee9861 | 357 | gyro[i] -= gyroOffset[i]; //substract offset values |
daqn | 0:2a7221ee9861 | 358 | accAngle[i] -= accOffset[i]; |
daqn | 0:2a7221ee9861 | 359 | } |
daqn | 0:2a7221ee9861 | 360 | |
daqn | 0:2a7221ee9861 | 361 | //apply filters on pitch and roll to get accurate angle values |
daqn | 0:2a7221ee9861 | 362 | angle[X_AXIS] = alpha * (angle[X_AXIS] + GYRO_SCALE*gyro[X_AXIS]*interval) + (1-alpha)*accAngle[X_AXIS]; |
daqn | 0:2a7221ee9861 | 363 | angle[Y_AXIS] = alpha * (angle[Y_AXIS] + GYRO_SCALE*gyro[Y_AXIS]*interval) + (1-alpha)*accAngle[Y_AXIS]; |
daqn | 0:2a7221ee9861 | 364 | |
daqn | 0:2a7221ee9861 | 365 | //calculate Yaw using just the gyroscope values - inaccurate |
daqn | 0:2a7221ee9861 | 366 | angle[Z_AXIS] = angle[Z_AXIS] + GYRO_SCALE*gyro[Z_AXIS]*interval; |
daqn | 0:2a7221ee9861 | 367 | } |
daqn | 0:2a7221ee9861 | 368 | |
daqn | 0:2a7221ee9861 | 369 | ///function for setting a different Alpha value, which is used in complemetary filter calculations |
daqn | 0:2a7221ee9861 | 370 | void MPU9250::setAlpha(float val){ |
daqn | 0:2a7221ee9861 | 371 | alpha = val; |
daqn | 0:2a7221ee9861 | 372 | } |
daqn | 0:2a7221ee9861 | 373 | |
daqn | 0:2a7221ee9861 | 374 | ///function for enabling interrupts on MPU9250 INT pin, when the data is ready to take |
daqn | 0:2a7221ee9861 | 375 | void MPU9250::enableInt( void ){ |
daqn | 0:2a7221ee9861 | 376 | char temp; |
daqn | 0:2a7221ee9861 | 377 | temp = this->read(MPU9250_RA_INT_ENABLE); |
daqn | 0:2a7221ee9861 | 378 | temp |= 0x01; |
daqn | 0:2a7221ee9861 | 379 | this->write(MPU9250_RA_INT_ENABLE, temp); |
daqn | 0:2a7221ee9861 | 380 | } |
daqn | 0:2a7221ee9861 | 381 | |
daqn | 0:2a7221ee9861 | 382 | ///function for disbling interrupts on MPU9250 INT pin, when the data is ready to take |
daqn | 0:2a7221ee9861 | 383 | void MPU9250::disableInt ( void ){ |
daqn | 0:2a7221ee9861 | 384 | char temp; |
daqn | 0:2a7221ee9861 | 385 | temp = this->read(MPU9250_RA_INT_ENABLE); |
daqn | 0:2a7221ee9861 | 386 | temp &= 0xFE; |
daqn | 0:2a7221ee9861 | 387 | this->write(MPU9250_RA_INT_ENABLE, temp); |
daqn | 0:2a7221ee9861 | 388 | } |