CIA / Mbed 2 deprecated AttitudeEstimation_usingTicker

MPU6050/9250で姿勢を推定するプログラム ・ジャイロ積算のみ（update()） ・ジャイロ積算後，加速度で補正（update_correction()） の2パターンの関数がある．

Dependencies:   Eigen mbed

MPU9250/MPU9250.cpp@0:29dce55dbcfe, 2018-03-20 (annotated)

Committer:
daqn
Date:
Tue Mar 20 13:46:54 2018 +0000
Revision:
0:29dce55dbcfe
??????????????; ??????????????; ?????????

Who changed what in which revision?

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