CIA / Mbed 2 deprecated MPU9250_Quaternion_Binary_Serial

エラー判定用 '\0' + クォータニオンのfloat型4成分，合計17byteのデータをひたすらバイナリ形式で送り続ける．最初のバイトが '\0' でなかったらズレているので，適当なバイト数見送って先頭が '\0' になるよう合わせること．

Dependencies:   Eigen mbed

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?

UserRevisionLine numberNew 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 }