Oğuz Özdemir / SparkFunMPU9250-DMP

It is the library published by sparkfun, edited accordingly to make it work under mbed platform.

Dependents:   MPU9250-dmp-bluepill MPU9250-dmp

SparkFunMPU9250-DMP.cpp@1:80a269cb9d4d, 2017-08-08 (annotated)

Committer:
mbedoguz
Date:
Tue Aug 08 14:14:58 2017 +0000
Revision:
1:80a269cb9d4d
Parent:
0:d1f0ae13f4a7
Child:
2:c35f8379f2cb
changes are made to succesfully transmit and receive data by i2c

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mbedoguz 0:d1f0ae13f4a7 1 /******************************************************************************
mbedoguz 0:d1f0ae13f4a7 2 SparkFunMPU9250-DMP.cpp - MPU-9250 Digital Motion Processor Arduino Library
mbedoguz 0:d1f0ae13f4a7 3 Jim Lindblom @ SparkFun Electronics
mbedoguz 0:d1f0ae13f4a7 4 original creation date: November 23, 2016
mbedoguz 0:d1f0ae13f4a7 5 https://github.com/sparkfun/SparkFun_MPU9250_DMP_Arduino_Library
mbedoguz 0:d1f0ae13f4a7 6
mbedoguz 0:d1f0ae13f4a7 7 This library implements motion processing functions of Invensense's MPU-9250.
mbedoguz 0:d1f0ae13f4a7 8 It is based on their Emedded MotionDriver 6.12 library.
mbedoguz 0:d1f0ae13f4a7 9 https://www.invensense.com/developers/software-downloads/
mbedoguz 0:d1f0ae13f4a7 10
mbedoguz 0:d1f0ae13f4a7 11 Development environment specifics:
mbedoguz 0:d1f0ae13f4a7 12 Arduino IDE 1.6.12
mbedoguz 0:d1f0ae13f4a7 13 SparkFun 9DoF Razor IMU M0
mbedoguz 0:d1f0ae13f4a7 14
mbedoguz 0:d1f0ae13f4a7 15 Supported Platforms:
mbedoguz 0:d1f0ae13f4a7 16 - ATSAMD21 (Arduino Zero, SparkFun SAMD21 Breakouts)
mbedoguz 0:d1f0ae13f4a7 17 ******************************************************************************/
mbedoguz 0:d1f0ae13f4a7 18 #include "SparkFunMPU9250-DMP.h"
mbedoguz 0:d1f0ae13f4a7 19 #include "MPU9250_RegisterMap.h"
mbedoguz 0:d1f0ae13f4a7 20 #include "mdcompat.h"
mbedoguz 0:d1f0ae13f4a7 21 extern "C" {
mbedoguz 0:d1f0ae13f4a7 22 #include "inv_mpu.h"
mbedoguz 0:d1f0ae13f4a7 23 }
mbedoguz 0:d1f0ae13f4a7 24 static unsigned char mpu9250_orientation;
mbedoguz 0:d1f0ae13f4a7 25 static unsigned char tap_count;
mbedoguz 0:d1f0ae13f4a7 26 static unsigned char tap_direction;
mbedoguz 0:d1f0ae13f4a7 27 static bool _tap_available;
mbedoguz 0:d1f0ae13f4a7 28 static void orient_cb(unsigned char orient);
mbedoguz 0:d1f0ae13f4a7 29 static void tap_cb(unsigned char direction, unsigned char count);
mbedoguz 0:d1f0ae13f4a7 30
mbedoguz 0:d1f0ae13f4a7 31 MPU9250_DMP::MPU9250_DMP()
mbedoguz 0:d1f0ae13f4a7 32 {
mbedoguz 0:d1f0ae13f4a7 33 _mSense = 6.665f; // Constant - 4915 / 32760
mbedoguz 0:d1f0ae13f4a7 34 _aSense = 0.0f; // Updated after accel FSR is set
mbedoguz 0:d1f0ae13f4a7 35 _gSense = 0.0f; // Updated after gyro FSR is set
mbedoguz 0:d1f0ae13f4a7 36 }
mbedoguz 0:d1f0ae13f4a7 37
mbedoguz 0:d1f0ae13f4a7 38 inv_error_t MPU9250_DMP::begin(void)
mbedoguz 0:d1f0ae13f4a7 39 {
mbedoguz 0:d1f0ae13f4a7 40 inv_error_t result;
mbedoguz 0:d1f0ae13f4a7 41 struct int_param_s int_param;
mbedoguz 0:d1f0ae13f4a7 42 imu_init();
mbedoguz 0:d1f0ae13f4a7 43 result = mpu_init(&int_param);
mbedoguz 0:d1f0ae13f4a7 44 if (result)
mbedoguz 0:d1f0ae13f4a7 45 return result;
mbedoguz 0:d1f0ae13f4a7 46
mbedoguz 0:d1f0ae13f4a7 47 mpu_set_bypass(1); // Place all slaves (including compass) on primary bus
mbedoguz 0:d1f0ae13f4a7 48
mbedoguz 0:d1f0ae13f4a7 49 setSensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS);
mbedoguz 0:d1f0ae13f4a7 50
mbedoguz 0:d1f0ae13f4a7 51 _gSense = getGyroSens();
mbedoguz 0:d1f0ae13f4a7 52 _aSense = getAccelSens();
mbedoguz 0:d1f0ae13f4a7 53
mbedoguz 0:d1f0ae13f4a7 54 return result;
mbedoguz 0:d1f0ae13f4a7 55 }
mbedoguz 0:d1f0ae13f4a7 56
mbedoguz 0:d1f0ae13f4a7 57 inv_error_t MPU9250_DMP::enableInterrupt(unsigned char enable)
mbedoguz 0:d1f0ae13f4a7 58 {
mbedoguz 0:d1f0ae13f4a7 59 return set_int_enable(enable);
mbedoguz 0:d1f0ae13f4a7 60 }
mbedoguz 0:d1f0ae13f4a7 61
mbedoguz 0:d1f0ae13f4a7 62 inv_error_t MPU9250_DMP::setIntLevel(unsigned char active_low)
mbedoguz 0:d1f0ae13f4a7 63 {
mbedoguz 0:d1f0ae13f4a7 64 return mpu_set_int_level(active_low);
mbedoguz 0:d1f0ae13f4a7 65 }
mbedoguz 0:d1f0ae13f4a7 66
mbedoguz 0:d1f0ae13f4a7 67 inv_error_t MPU9250_DMP::setIntLatched(unsigned char enable)
mbedoguz 0:d1f0ae13f4a7 68 {
mbedoguz 0:d1f0ae13f4a7 69 return mpu_set_int_latched(enable);
mbedoguz 0:d1f0ae13f4a7 70 }
mbedoguz 0:d1f0ae13f4a7 71
mbedoguz 0:d1f0ae13f4a7 72 short MPU9250_DMP::getIntStatus(void)
mbedoguz 0:d1f0ae13f4a7 73 {
mbedoguz 0:d1f0ae13f4a7 74 short status;
mbedoguz 0:d1f0ae13f4a7 75 if (mpu_get_int_status(&status) == INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 76 {
mbedoguz 0:d1f0ae13f4a7 77 return status;
mbedoguz 0:d1f0ae13f4a7 78 }
mbedoguz 0:d1f0ae13f4a7 79 return 0;
mbedoguz 0:d1f0ae13f4a7 80 }
mbedoguz 0:d1f0ae13f4a7 81
mbedoguz 0:d1f0ae13f4a7 82 // Accelerometer Low-Power Mode. Rate options:
mbedoguz 0:d1f0ae13f4a7 83 // 1.25 (1), 2.5 (2), 5, 10, 20, 40,
mbedoguz 0:d1f0ae13f4a7 84 // 80, 160, 320, or 640 Hz
mbedoguz 0:d1f0ae13f4a7 85 // Disables compass and gyro
mbedoguz 0:d1f0ae13f4a7 86 inv_error_t MPU9250_DMP::lowPowerAccel(unsigned short rate)
mbedoguz 0:d1f0ae13f4a7 87 {
mbedoguz 0:d1f0ae13f4a7 88 return mpu_lp_accel_mode(rate);
mbedoguz 0:d1f0ae13f4a7 89 }
mbedoguz 0:d1f0ae13f4a7 90
mbedoguz 0:d1f0ae13f4a7 91 inv_error_t MPU9250_DMP::setGyroFSR(unsigned short fsr)
mbedoguz 0:d1f0ae13f4a7 92 {
mbedoguz 0:d1f0ae13f4a7 93 inv_error_t err;
mbedoguz 0:d1f0ae13f4a7 94 err = mpu_set_gyro_fsr(fsr);
mbedoguz 0:d1f0ae13f4a7 95 if (err == INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 96 {
mbedoguz 0:d1f0ae13f4a7 97 _gSense = getGyroSens();
mbedoguz 0:d1f0ae13f4a7 98 }
mbedoguz 0:d1f0ae13f4a7 99 return err;
mbedoguz 0:d1f0ae13f4a7 100 }
mbedoguz 0:d1f0ae13f4a7 101
mbedoguz 0:d1f0ae13f4a7 102 inv_error_t MPU9250_DMP::setAccelFSR(unsigned char fsr)
mbedoguz 0:d1f0ae13f4a7 103 {
mbedoguz 0:d1f0ae13f4a7 104 inv_error_t err;
mbedoguz 0:d1f0ae13f4a7 105 err = mpu_set_accel_fsr(fsr);
mbedoguz 0:d1f0ae13f4a7 106 if (err == INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 107 {
mbedoguz 0:d1f0ae13f4a7 108 _aSense = getAccelSens();
mbedoguz 0:d1f0ae13f4a7 109 }
mbedoguz 0:d1f0ae13f4a7 110 return err;
mbedoguz 0:d1f0ae13f4a7 111 }
mbedoguz 0:d1f0ae13f4a7 112
mbedoguz 0:d1f0ae13f4a7 113 unsigned short MPU9250_DMP::getGyroFSR(void)
mbedoguz 0:d1f0ae13f4a7 114 {
mbedoguz 0:d1f0ae13f4a7 115 unsigned short tmp;
mbedoguz 0:d1f0ae13f4a7 116 if (mpu_get_gyro_fsr(&tmp) == INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 117 {
mbedoguz 0:d1f0ae13f4a7 118 return tmp;
mbedoguz 0:d1f0ae13f4a7 119 }
mbedoguz 0:d1f0ae13f4a7 120 return 0;
mbedoguz 0:d1f0ae13f4a7 121 }
mbedoguz 0:d1f0ae13f4a7 122
mbedoguz 0:d1f0ae13f4a7 123 unsigned char MPU9250_DMP::getAccelFSR(void)
mbedoguz 0:d1f0ae13f4a7 124 {
mbedoguz 0:d1f0ae13f4a7 125 unsigned char tmp;
mbedoguz 0:d1f0ae13f4a7 126 if (mpu_get_accel_fsr(&tmp) == INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 127 {
mbedoguz 0:d1f0ae13f4a7 128 return tmp;
mbedoguz 0:d1f0ae13f4a7 129 }
mbedoguz 0:d1f0ae13f4a7 130 return 0;
mbedoguz 0:d1f0ae13f4a7 131 }
mbedoguz 0:d1f0ae13f4a7 132
mbedoguz 0:d1f0ae13f4a7 133 unsigned short MPU9250_DMP::getMagFSR(void)
mbedoguz 0:d1f0ae13f4a7 134 {
mbedoguz 0:d1f0ae13f4a7 135 unsigned short tmp;
mbedoguz 0:d1f0ae13f4a7 136 if (mpu_get_compass_fsr(&tmp) == INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 137 {
mbedoguz 0:d1f0ae13f4a7 138 return tmp;
mbedoguz 0:d1f0ae13f4a7 139 }
mbedoguz 0:d1f0ae13f4a7 140 return 0;
mbedoguz 0:d1f0ae13f4a7 141 }
mbedoguz 0:d1f0ae13f4a7 142
mbedoguz 0:d1f0ae13f4a7 143 inv_error_t MPU9250_DMP::setLPF(unsigned short lpf)
mbedoguz 0:d1f0ae13f4a7 144 {
mbedoguz 0:d1f0ae13f4a7 145 return mpu_set_lpf(lpf);
mbedoguz 0:d1f0ae13f4a7 146 }
mbedoguz 0:d1f0ae13f4a7 147
mbedoguz 0:d1f0ae13f4a7 148 unsigned short MPU9250_DMP::getLPF(void)
mbedoguz 0:d1f0ae13f4a7 149 {
mbedoguz 0:d1f0ae13f4a7 150 unsigned short tmp;
mbedoguz 0:d1f0ae13f4a7 151 if (mpu_get_lpf(&tmp) == INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 152 {
mbedoguz 0:d1f0ae13f4a7 153 return tmp;
mbedoguz 0:d1f0ae13f4a7 154 }
mbedoguz 0:d1f0ae13f4a7 155 return 0;
mbedoguz 0:d1f0ae13f4a7 156 }
mbedoguz 0:d1f0ae13f4a7 157
mbedoguz 0:d1f0ae13f4a7 158 inv_error_t MPU9250_DMP::setSampleRate(unsigned short rate)
mbedoguz 0:d1f0ae13f4a7 159 {
mbedoguz 0:d1f0ae13f4a7 160 return mpu_set_sample_rate(rate);
mbedoguz 0:d1f0ae13f4a7 161 }
mbedoguz 0:d1f0ae13f4a7 162
mbedoguz 0:d1f0ae13f4a7 163 unsigned short MPU9250_DMP::getSampleRate(void)
mbedoguz 0:d1f0ae13f4a7 164 {
mbedoguz 0:d1f0ae13f4a7 165 unsigned short tmp;
mbedoguz 0:d1f0ae13f4a7 166 if (mpu_get_sample_rate(&tmp) == INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 167 {
mbedoguz 0:d1f0ae13f4a7 168 return tmp;
mbedoguz 0:d1f0ae13f4a7 169 }
mbedoguz 0:d1f0ae13f4a7 170 return 0;
mbedoguz 0:d1f0ae13f4a7 171 }
mbedoguz 0:d1f0ae13f4a7 172
mbedoguz 0:d1f0ae13f4a7 173 inv_error_t MPU9250_DMP::setCompassSampleRate(unsigned short rate)
mbedoguz 0:d1f0ae13f4a7 174 {
mbedoguz 0:d1f0ae13f4a7 175 return mpu_set_compass_sample_rate(rate);
mbedoguz 0:d1f0ae13f4a7 176 }
mbedoguz 0:d1f0ae13f4a7 177
mbedoguz 0:d1f0ae13f4a7 178 unsigned short MPU9250_DMP::getCompassSampleRate(void)
mbedoguz 0:d1f0ae13f4a7 179 {
mbedoguz 0:d1f0ae13f4a7 180 unsigned short tmp;
mbedoguz 0:d1f0ae13f4a7 181 if (mpu_get_compass_sample_rate(&tmp) == INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 182 {
mbedoguz 0:d1f0ae13f4a7 183 return tmp;
mbedoguz 0:d1f0ae13f4a7 184 }
mbedoguz 0:d1f0ae13f4a7 185
mbedoguz 0:d1f0ae13f4a7 186 return 0;
mbedoguz 0:d1f0ae13f4a7 187 }
mbedoguz 0:d1f0ae13f4a7 188
mbedoguz 0:d1f0ae13f4a7 189 float MPU9250_DMP::getGyroSens(void)
mbedoguz 0:d1f0ae13f4a7 190 {
mbedoguz 0:d1f0ae13f4a7 191 float sens;
mbedoguz 0:d1f0ae13f4a7 192 if (mpu_get_gyro_sens(&sens) == INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 193 {
mbedoguz 0:d1f0ae13f4a7 194 return sens;
mbedoguz 0:d1f0ae13f4a7 195 }
mbedoguz 0:d1f0ae13f4a7 196 return 0;
mbedoguz 0:d1f0ae13f4a7 197 }
mbedoguz 0:d1f0ae13f4a7 198
mbedoguz 0:d1f0ae13f4a7 199 unsigned short MPU9250_DMP::getAccelSens(void)
mbedoguz 0:d1f0ae13f4a7 200 {
mbedoguz 0:d1f0ae13f4a7 201 unsigned short sens;
mbedoguz 0:d1f0ae13f4a7 202 if (mpu_get_accel_sens(&sens) == INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 203 {
mbedoguz 0:d1f0ae13f4a7 204 return sens;
mbedoguz 0:d1f0ae13f4a7 205 }
mbedoguz 0:d1f0ae13f4a7 206 return 0;
mbedoguz 0:d1f0ae13f4a7 207 }
mbedoguz 0:d1f0ae13f4a7 208
mbedoguz 0:d1f0ae13f4a7 209 float MPU9250_DMP::getMagSens(void)
mbedoguz 0:d1f0ae13f4a7 210 {
mbedoguz 0:d1f0ae13f4a7 211 return 0.15; // Static, 4915/32760
mbedoguz 0:d1f0ae13f4a7 212 }
mbedoguz 0:d1f0ae13f4a7 213
mbedoguz 0:d1f0ae13f4a7 214 unsigned char MPU9250_DMP::getFifoConfig(void)
mbedoguz 0:d1f0ae13f4a7 215 {
mbedoguz 0:d1f0ae13f4a7 216 unsigned char sensors;
mbedoguz 0:d1f0ae13f4a7 217 if (mpu_get_fifo_config(&sensors) == INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 218 {
mbedoguz 0:d1f0ae13f4a7 219 return sensors;
mbedoguz 0:d1f0ae13f4a7 220 }
mbedoguz 0:d1f0ae13f4a7 221 return 0;
mbedoguz 0:d1f0ae13f4a7 222 }
mbedoguz 0:d1f0ae13f4a7 223
mbedoguz 0:d1f0ae13f4a7 224 inv_error_t MPU9250_DMP::configureFifo(unsigned char sensors)
mbedoguz 0:d1f0ae13f4a7 225 {
mbedoguz 0:d1f0ae13f4a7 226 return mpu_configure_fifo(sensors);
mbedoguz 0:d1f0ae13f4a7 227 }
mbedoguz 0:d1f0ae13f4a7 228
mbedoguz 0:d1f0ae13f4a7 229 inv_error_t MPU9250_DMP::resetFifo(void)
mbedoguz 0:d1f0ae13f4a7 230 {
mbedoguz 0:d1f0ae13f4a7 231 return mpu_reset_fifo();
mbedoguz 0:d1f0ae13f4a7 232 }
mbedoguz 0:d1f0ae13f4a7 233
mbedoguz 0:d1f0ae13f4a7 234 unsigned short MPU9250_DMP::fifoAvailable(void)
mbedoguz 0:d1f0ae13f4a7 235 {
mbedoguz 0:d1f0ae13f4a7 236 unsigned char fifoH, fifoL;
mbedoguz 0:d1f0ae13f4a7 237 if (mpu_read_reg(MPU9250_FIFO_COUNTH, &fifoH) != INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 238 return 0;
mbedoguz 0:d1f0ae13f4a7 239 if (mpu_read_reg(MPU9250_FIFO_COUNTL, &fifoL) != INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 240 return 0;
mbedoguz 0:d1f0ae13f4a7 241
mbedoguz 0:d1f0ae13f4a7 242 return (fifoH << 8 ) | fifoL;
mbedoguz 0:d1f0ae13f4a7 243 }
mbedoguz 0:d1f0ae13f4a7 244
mbedoguz 0:d1f0ae13f4a7 245 inv_error_t MPU9250_DMP::updateFifo(void)
mbedoguz 0:d1f0ae13f4a7 246 {
mbedoguz 0:d1f0ae13f4a7 247 short gyro[3], accel[3];
mbedoguz 0:d1f0ae13f4a7 248 unsigned long timestamp;
mbedoguz 0:d1f0ae13f4a7 249 unsigned char sensors, more;
mbedoguz 0:d1f0ae13f4a7 250
mbedoguz 0:d1f0ae13f4a7 251 if (mpu_read_fifo(gyro, accel, &timestamp, &sensors, &more) != INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 252 return INV_ERROR;
mbedoguz 0:d1f0ae13f4a7 253
mbedoguz 0:d1f0ae13f4a7 254 if (sensors & INV_XYZ_ACCEL)
mbedoguz 0:d1f0ae13f4a7 255 {
mbedoguz 0:d1f0ae13f4a7 256 ax = accel[X_AXIS];
mbedoguz 0:d1f0ae13f4a7 257 ay = accel[Y_AXIS];
mbedoguz 0:d1f0ae13f4a7 258 az = accel[Z_AXIS];
mbedoguz 0:d1f0ae13f4a7 259 }
mbedoguz 0:d1f0ae13f4a7 260 if (sensors & INV_X_GYRO)
mbedoguz 0:d1f0ae13f4a7 261 gx = gyro[X_AXIS];
mbedoguz 0:d1f0ae13f4a7 262 if (sensors & INV_Y_GYRO)
mbedoguz 0:d1f0ae13f4a7 263 gy = gyro[Y_AXIS];
mbedoguz 0:d1f0ae13f4a7 264 if (sensors & INV_Z_GYRO)
mbedoguz 0:d1f0ae13f4a7 265 gz = gyro[Z_AXIS];
mbedoguz 0:d1f0ae13f4a7 266
mbedoguz 0:d1f0ae13f4a7 267 time = timestamp;
mbedoguz 0:d1f0ae13f4a7 268
mbedoguz 0:d1f0ae13f4a7 269 return INV_SUCCESS;
mbedoguz 0:d1f0ae13f4a7 270 }
mbedoguz 0:d1f0ae13f4a7 271
mbedoguz 0:d1f0ae13f4a7 272 inv_error_t MPU9250_DMP::setSensors(unsigned char sensors)
mbedoguz 0:d1f0ae13f4a7 273 {
mbedoguz 0:d1f0ae13f4a7 274 return mpu_set_sensors(sensors);
mbedoguz 0:d1f0ae13f4a7 275 }
mbedoguz 0:d1f0ae13f4a7 276
mbedoguz 0:d1f0ae13f4a7 277 bool MPU9250_DMP::dataReady()
mbedoguz 0:d1f0ae13f4a7 278 {
mbedoguz 0:d1f0ae13f4a7 279 unsigned char intStatusReg;
mbedoguz 0:d1f0ae13f4a7 280
mbedoguz 0:d1f0ae13f4a7 281 if (mpu_read_reg(MPU9250_INT_STATUS, &intStatusReg) == INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 282 {
mbedoguz 0:d1f0ae13f4a7 283 return (intStatusReg & (1<<INT_STATUS_RAW_DATA_RDY_INT));
mbedoguz 0:d1f0ae13f4a7 284 }
mbedoguz 0:d1f0ae13f4a7 285 return false;
mbedoguz 0:d1f0ae13f4a7 286 }
mbedoguz 0:d1f0ae13f4a7 287
mbedoguz 0:d1f0ae13f4a7 288 inv_error_t MPU9250_DMP::update(unsigned char sensors)
mbedoguz 0:d1f0ae13f4a7 289 {
mbedoguz 0:d1f0ae13f4a7 290 inv_error_t aErr = INV_SUCCESS;
mbedoguz 0:d1f0ae13f4a7 291 inv_error_t gErr = INV_SUCCESS;
mbedoguz 0:d1f0ae13f4a7 292 inv_error_t mErr = INV_SUCCESS;
mbedoguz 0:d1f0ae13f4a7 293 inv_error_t tErr = INV_SUCCESS;
mbedoguz 0:d1f0ae13f4a7 294
mbedoguz 0:d1f0ae13f4a7 295 if (sensors & UPDATE_ACCEL)
mbedoguz 0:d1f0ae13f4a7 296 aErr = updateAccel();
mbedoguz 0:d1f0ae13f4a7 297 if (sensors & UPDATE_GYRO)
mbedoguz 0:d1f0ae13f4a7 298 gErr = updateGyro();
mbedoguz 0:d1f0ae13f4a7 299 if (sensors & UPDATE_COMPASS)
mbedoguz 0:d1f0ae13f4a7 300 mErr = updateCompass();
mbedoguz 0:d1f0ae13f4a7 301 if (sensors & UPDATE_TEMP)
mbedoguz 0:d1f0ae13f4a7 302 tErr = updateTemperature();
mbedoguz 0:d1f0ae13f4a7 303
mbedoguz 0:d1f0ae13f4a7 304 return aErr | gErr | mErr | tErr;
mbedoguz 0:d1f0ae13f4a7 305 }
mbedoguz 0:d1f0ae13f4a7 306
mbedoguz 0:d1f0ae13f4a7 307 int MPU9250_DMP::updateAccel(void)
mbedoguz 0:d1f0ae13f4a7 308 {
mbedoguz 0:d1f0ae13f4a7 309 short data[3];
mbedoguz 0:d1f0ae13f4a7 310
mbedoguz 0:d1f0ae13f4a7 311 if (mpu_get_accel_reg(data, &time))
mbedoguz 0:d1f0ae13f4a7 312 {
mbedoguz 0:d1f0ae13f4a7 313 return INV_ERROR;
mbedoguz 0:d1f0ae13f4a7 314 }
mbedoguz 0:d1f0ae13f4a7 315 ax = data[X_AXIS];
mbedoguz 0:d1f0ae13f4a7 316 ay = data[Y_AXIS];
mbedoguz 0:d1f0ae13f4a7 317 az = data[Z_AXIS];
mbedoguz 0:d1f0ae13f4a7 318 return INV_SUCCESS;
mbedoguz 0:d1f0ae13f4a7 319 }
mbedoguz 0:d1f0ae13f4a7 320
mbedoguz 0:d1f0ae13f4a7 321 int MPU9250_DMP::updateGyro(void)
mbedoguz 0:d1f0ae13f4a7 322 {
mbedoguz 0:d1f0ae13f4a7 323 short data[3];
mbedoguz 0:d1f0ae13f4a7 324
mbedoguz 0:d1f0ae13f4a7 325 if (mpu_get_gyro_reg(data, &time))
mbedoguz 0:d1f0ae13f4a7 326 {
mbedoguz 0:d1f0ae13f4a7 327 return INV_ERROR;
mbedoguz 0:d1f0ae13f4a7 328 }
mbedoguz 0:d1f0ae13f4a7 329 gx = data[X_AXIS];
mbedoguz 0:d1f0ae13f4a7 330 gy = data[Y_AXIS];
mbedoguz 0:d1f0ae13f4a7 331 gz = data[Z_AXIS];
mbedoguz 0:d1f0ae13f4a7 332 return INV_SUCCESS;
mbedoguz 0:d1f0ae13f4a7 333 }
mbedoguz 0:d1f0ae13f4a7 334
mbedoguz 0:d1f0ae13f4a7 335 int MPU9250_DMP::updateCompass(void)
mbedoguz 0:d1f0ae13f4a7 336 {
mbedoguz 0:d1f0ae13f4a7 337 short data[3];
mbedoguz 0:d1f0ae13f4a7 338
mbedoguz 0:d1f0ae13f4a7 339 if (mpu_get_compass_reg(data, &time))
mbedoguz 0:d1f0ae13f4a7 340 {
mbedoguz 0:d1f0ae13f4a7 341 return INV_ERROR;
mbedoguz 0:d1f0ae13f4a7 342 }
mbedoguz 0:d1f0ae13f4a7 343 mx = data[X_AXIS];
mbedoguz 0:d1f0ae13f4a7 344 my = data[Y_AXIS];
mbedoguz 0:d1f0ae13f4a7 345 mz = data[Z_AXIS];
mbedoguz 0:d1f0ae13f4a7 346 return INV_SUCCESS;
mbedoguz 0:d1f0ae13f4a7 347 }
mbedoguz 0:d1f0ae13f4a7 348
mbedoguz 0:d1f0ae13f4a7 349 inv_error_t MPU9250_DMP::updateTemperature(void)
mbedoguz 0:d1f0ae13f4a7 350 {
mbedoguz 0:d1f0ae13f4a7 351 return mpu_get_temperature(&temperature, &time);
mbedoguz 0:d1f0ae13f4a7 352 }
mbedoguz 0:d1f0ae13f4a7 353
mbedoguz 0:d1f0ae13f4a7 354 int MPU9250_DMP::selfTest(unsigned char debug)
mbedoguz 0:d1f0ae13f4a7 355 {
mbedoguz 0:d1f0ae13f4a7 356 long gyro[3], accel[3];
mbedoguz 0:d1f0ae13f4a7 357 return mpu_run_self_test(gyro, accel);
mbedoguz 0:d1f0ae13f4a7 358 }
mbedoguz 0:d1f0ae13f4a7 359
mbedoguz 0:d1f0ae13f4a7 360 inv_error_t MPU9250_DMP::dmpBegin(unsigned short features, unsigned short fifoRate)
mbedoguz 0:d1f0ae13f4a7 361 {
mbedoguz 0:d1f0ae13f4a7 362 unsigned short feat = features;
mbedoguz 0:d1f0ae13f4a7 363 unsigned short rate = fifoRate;
mbedoguz 0:d1f0ae13f4a7 364
mbedoguz 0:d1f0ae13f4a7 365 if (dmpLoad() != INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 366 return INV_ERROR;
mbedoguz 0:d1f0ae13f4a7 367
mbedoguz 0:d1f0ae13f4a7 368 // 3-axis and 6-axis LP quat are mutually exclusive.
mbedoguz 0:d1f0ae13f4a7 369 // If both are selected, default to 3-axis
mbedoguz 0:d1f0ae13f4a7 370 if (feat & DMP_FEATURE_LP_QUAT)
mbedoguz 0:d1f0ae13f4a7 371 {
mbedoguz 0:d1f0ae13f4a7 372 feat &= ~(DMP_FEATURE_6X_LP_QUAT);
mbedoguz 0:d1f0ae13f4a7 373 dmp_enable_lp_quat(1);
mbedoguz 0:d1f0ae13f4a7 374 }
mbedoguz 0:d1f0ae13f4a7 375 else if (feat & DMP_FEATURE_6X_LP_QUAT)
mbedoguz 0:d1f0ae13f4a7 376 dmp_enable_6x_lp_quat(1);
mbedoguz 0:d1f0ae13f4a7 377
mbedoguz 0:d1f0ae13f4a7 378 if (feat & DMP_FEATURE_GYRO_CAL)
mbedoguz 0:d1f0ae13f4a7 379 dmp_enable_gyro_cal(1);
mbedoguz 0:d1f0ae13f4a7 380
mbedoguz 0:d1f0ae13f4a7 381 if (dmpEnableFeatures(feat) != INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 382 return INV_ERROR;
mbedoguz 0:d1f0ae13f4a7 383
mbedoguz 0:d1f0ae13f4a7 384 rate = constrain(rate, 1, 200);
mbedoguz 0:d1f0ae13f4a7 385 if (dmpSetFifoRate(rate) != INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 386 return INV_ERROR;
mbedoguz 0:d1f0ae13f4a7 387
mbedoguz 0:d1f0ae13f4a7 388 return mpu_set_dmp_state(1);
mbedoguz 0:d1f0ae13f4a7 389 }
mbedoguz 0:d1f0ae13f4a7 390
mbedoguz 0:d1f0ae13f4a7 391 inv_error_t MPU9250_DMP::dmpLoad(void)
mbedoguz 0:d1f0ae13f4a7 392 {
mbedoguz 0:d1f0ae13f4a7 393 return dmp_load_motion_driver_firmware();
mbedoguz 0:d1f0ae13f4a7 394 }
mbedoguz 0:d1f0ae13f4a7 395
mbedoguz 0:d1f0ae13f4a7 396 unsigned short MPU9250_DMP::dmpGetFifoRate(void)
mbedoguz 0:d1f0ae13f4a7 397 {
mbedoguz 0:d1f0ae13f4a7 398 unsigned short rate;
mbedoguz 0:d1f0ae13f4a7 399 if (dmp_get_fifo_rate(&rate) == INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 400 return rate;
mbedoguz 0:d1f0ae13f4a7 401
mbedoguz 0:d1f0ae13f4a7 402 return 0;
mbedoguz 0:d1f0ae13f4a7 403 }
mbedoguz 0:d1f0ae13f4a7 404
mbedoguz 0:d1f0ae13f4a7 405 inv_error_t MPU9250_DMP::dmpSetFifoRate(unsigned short rate)
mbedoguz 0:d1f0ae13f4a7 406 {
mbedoguz 0:d1f0ae13f4a7 407 if (rate > MAX_DMP_SAMPLE_RATE) rate = MAX_DMP_SAMPLE_RATE;
mbedoguz 0:d1f0ae13f4a7 408 return dmp_set_fifo_rate(rate);
mbedoguz 0:d1f0ae13f4a7 409 }
mbedoguz 0:d1f0ae13f4a7 410
mbedoguz 0:d1f0ae13f4a7 411 inv_error_t MPU9250_DMP::dmpUpdateFifo(long DEBUG[4])
mbedoguz 0:d1f0ae13f4a7 412 {
mbedoguz 0:d1f0ae13f4a7 413 short gyro[3];
mbedoguz 0:d1f0ae13f4a7 414 short accel[3];
mbedoguz 0:d1f0ae13f4a7 415 long quat[4];
mbedoguz 0:d1f0ae13f4a7 416 unsigned long timestamp;
mbedoguz 0:d1f0ae13f4a7 417 short sensors;
mbedoguz 0:d1f0ae13f4a7 418 unsigned char more;
mbedoguz 0:d1f0ae13f4a7 419
mbedoguz 0:d1f0ae13f4a7 420 if (dmp_read_fifo(gyro, accel, quat, &timestamp, &sensors, &more)
mbedoguz 0:d1f0ae13f4a7 421 != INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 422 {
mbedoguz 0:d1f0ae13f4a7 423 return INV_ERROR;
mbedoguz 0:d1f0ae13f4a7 424 }
mbedoguz 0:d1f0ae13f4a7 425
mbedoguz 0:d1f0ae13f4a7 426 if (sensors & INV_XYZ_ACCEL)
mbedoguz 0:d1f0ae13f4a7 427 {
mbedoguz 0:d1f0ae13f4a7 428 ax = accel[X_AXIS];
mbedoguz 0:d1f0ae13f4a7 429 ay = accel[Y_AXIS];
mbedoguz 0:d1f0ae13f4a7 430 az = accel[Z_AXIS];
mbedoguz 0:d1f0ae13f4a7 431 }
mbedoguz 0:d1f0ae13f4a7 432 if (sensors & INV_X_GYRO)
mbedoguz 0:d1f0ae13f4a7 433 gx = gyro[X_AXIS];
mbedoguz 0:d1f0ae13f4a7 434 if (sensors & INV_Y_GYRO)
mbedoguz 0:d1f0ae13f4a7 435 gy = gyro[Y_AXIS];
mbedoguz 0:d1f0ae13f4a7 436 if (sensors & INV_Z_GYRO)
mbedoguz 0:d1f0ae13f4a7 437 gz = gyro[Z_AXIS];
mbedoguz 0:d1f0ae13f4a7 438 if (sensors & INV_WXYZ_QUAT)
mbedoguz 0:d1f0ae13f4a7 439 {
mbedoguz 0:d1f0ae13f4a7 440 qw = quat[0];
mbedoguz 0:d1f0ae13f4a7 441 qx = quat[1];
mbedoguz 0:d1f0ae13f4a7 442 qy = quat[2];
mbedoguz 0:d1f0ae13f4a7 443 qz = quat[3];
mbedoguz 0:d1f0ae13f4a7 444 }
mbedoguz 0:d1f0ae13f4a7 445 DEBUG[0]=quat[0];
mbedoguz 0:d1f0ae13f4a7 446 DEBUG[1]=quat[1];
mbedoguz 0:d1f0ae13f4a7 447 DEBUG[2]=quat[2];
mbedoguz 0:d1f0ae13f4a7 448 DEBUG[3]=quat[3];
mbedoguz 0:d1f0ae13f4a7 449
mbedoguz 0:d1f0ae13f4a7 450 time = timestamp;
mbedoguz 0:d1f0ae13f4a7 451
mbedoguz 0:d1f0ae13f4a7 452 return INV_SUCCESS;
mbedoguz 0:d1f0ae13f4a7 453 }
mbedoguz 0:d1f0ae13f4a7 454
mbedoguz 0:d1f0ae13f4a7 455 inv_error_t MPU9250_DMP::dmpEnableFeatures(unsigned short mask)
mbedoguz 0:d1f0ae13f4a7 456 {
mbedoguz 0:d1f0ae13f4a7 457 unsigned short enMask = 0;
mbedoguz 0:d1f0ae13f4a7 458 enMask |= mask;
mbedoguz 0:d1f0ae13f4a7 459 // Combat known issue where fifo sample rate is incorrect
mbedoguz 0:d1f0ae13f4a7 460 // unless tap is enabled in the DMP.
mbedoguz 0:d1f0ae13f4a7 461 enMask |= DMP_FEATURE_TAP;
mbedoguz 0:d1f0ae13f4a7 462 return dmp_enable_feature(enMask);
mbedoguz 0:d1f0ae13f4a7 463 }
mbedoguz 0:d1f0ae13f4a7 464
mbedoguz 0:d1f0ae13f4a7 465 unsigned short MPU9250_DMP::dmpGetEnabledFeatures(void)
mbedoguz 0:d1f0ae13f4a7 466 {
mbedoguz 0:d1f0ae13f4a7 467 unsigned short mask;
mbedoguz 0:d1f0ae13f4a7 468 if (dmp_get_enabled_features(&mask) == INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 469 return mask;
mbedoguz 0:d1f0ae13f4a7 470 return 0;
mbedoguz 0:d1f0ae13f4a7 471 }
mbedoguz 0:d1f0ae13f4a7 472
mbedoguz 0:d1f0ae13f4a7 473 inv_error_t MPU9250_DMP::dmpSetTap(
mbedoguz 0:d1f0ae13f4a7 474 unsigned short xThresh, unsigned short yThresh, unsigned short zThresh,
mbedoguz 0:d1f0ae13f4a7 475 unsigned char taps, unsigned short tapTime, unsigned short tapMulti)
mbedoguz 0:d1f0ae13f4a7 476 {
mbedoguz 0:d1f0ae13f4a7 477 unsigned char axes = 0;
mbedoguz 0:d1f0ae13f4a7 478 if (xThresh > 0)
mbedoguz 0:d1f0ae13f4a7 479 {
mbedoguz 0:d1f0ae13f4a7 480 axes |= TAP_X;
mbedoguz 0:d1f0ae13f4a7 481 xThresh = constrain(xThresh, 1, 1600);
mbedoguz 0:d1f0ae13f4a7 482 if (dmp_set_tap_thresh(1<<X_AXIS, xThresh) != INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 483 return INV_ERROR;
mbedoguz 0:d1f0ae13f4a7 484 }
mbedoguz 0:d1f0ae13f4a7 485 if (yThresh > 0)
mbedoguz 0:d1f0ae13f4a7 486 {
mbedoguz 0:d1f0ae13f4a7 487 axes |= TAP_Y;
mbedoguz 0:d1f0ae13f4a7 488 yThresh = constrain(yThresh, 1, 1600);
mbedoguz 0:d1f0ae13f4a7 489 if (dmp_set_tap_thresh(1<<Y_AXIS, yThresh) != INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 490 return INV_ERROR;
mbedoguz 0:d1f0ae13f4a7 491 }
mbedoguz 0:d1f0ae13f4a7 492 if (zThresh > 0)
mbedoguz 0:d1f0ae13f4a7 493 {
mbedoguz 0:d1f0ae13f4a7 494 axes |= TAP_Z;
mbedoguz 0:d1f0ae13f4a7 495 zThresh = constrain(zThresh, 1, 1600);
mbedoguz 0:d1f0ae13f4a7 496 if (dmp_set_tap_thresh(1<<Z_AXIS, zThresh) != INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 497 return INV_ERROR;
mbedoguz 0:d1f0ae13f4a7 498 }
mbedoguz 0:d1f0ae13f4a7 499 if (dmp_set_tap_axes(axes) != INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 500 return INV_ERROR;
mbedoguz 0:d1f0ae13f4a7 501 if (dmp_set_tap_count(taps) != INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 502 return INV_ERROR;
mbedoguz 0:d1f0ae13f4a7 503 if (dmp_set_tap_time(tapTime) != INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 504 return INV_ERROR;
mbedoguz 0:d1f0ae13f4a7 505 if (dmp_set_tap_time_multi(tapMulti) != INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 506 return INV_ERROR;
mbedoguz 0:d1f0ae13f4a7 507
mbedoguz 0:d1f0ae13f4a7 508 dmp_register_tap_cb(tap_cb);
mbedoguz 0:d1f0ae13f4a7 509
mbedoguz 0:d1f0ae13f4a7 510 return INV_SUCCESS;
mbedoguz 0:d1f0ae13f4a7 511 }
mbedoguz 0:d1f0ae13f4a7 512
mbedoguz 0:d1f0ae13f4a7 513 unsigned char MPU9250_DMP::getTapDir(void)
mbedoguz 0:d1f0ae13f4a7 514 {
mbedoguz 0:d1f0ae13f4a7 515 _tap_available = false;
mbedoguz 0:d1f0ae13f4a7 516 return tap_direction;
mbedoguz 0:d1f0ae13f4a7 517 }
mbedoguz 0:d1f0ae13f4a7 518
mbedoguz 0:d1f0ae13f4a7 519 unsigned char MPU9250_DMP::getTapCount(void)
mbedoguz 0:d1f0ae13f4a7 520 {
mbedoguz 0:d1f0ae13f4a7 521 _tap_available = false;
mbedoguz 0:d1f0ae13f4a7 522 return tap_count;
mbedoguz 0:d1f0ae13f4a7 523 }
mbedoguz 0:d1f0ae13f4a7 524
mbedoguz 0:d1f0ae13f4a7 525 bool MPU9250_DMP::tapAvailable(void)
mbedoguz 0:d1f0ae13f4a7 526 {
mbedoguz 0:d1f0ae13f4a7 527 return _tap_available;
mbedoguz 0:d1f0ae13f4a7 528 }
mbedoguz 0:d1f0ae13f4a7 529
mbedoguz 0:d1f0ae13f4a7 530 inv_error_t MPU9250_DMP::dmpSetOrientation(const signed char * orientationMatrix)
mbedoguz 0:d1f0ae13f4a7 531 {
mbedoguz 0:d1f0ae13f4a7 532 unsigned short scalar;
mbedoguz 0:d1f0ae13f4a7 533 scalar = orientation_row_2_scale(orientationMatrix);
mbedoguz 0:d1f0ae13f4a7 534 scalar |= orientation_row_2_scale(orientationMatrix + 3) << 3;
mbedoguz 0:d1f0ae13f4a7 535 scalar |= orientation_row_2_scale(orientationMatrix + 6) << 6;
mbedoguz 0:d1f0ae13f4a7 536
mbedoguz 0:d1f0ae13f4a7 537 dmp_register_android_orient_cb(orient_cb);
mbedoguz 0:d1f0ae13f4a7 538
mbedoguz 0:d1f0ae13f4a7 539 return dmp_set_orientation(scalar);
mbedoguz 0:d1f0ae13f4a7 540 }
mbedoguz 0:d1f0ae13f4a7 541
mbedoguz 0:d1f0ae13f4a7 542 unsigned char MPU9250_DMP::dmpGetOrientation(void)
mbedoguz 0:d1f0ae13f4a7 543 {
mbedoguz 0:d1f0ae13f4a7 544 return mpu9250_orientation;
mbedoguz 0:d1f0ae13f4a7 545 }
mbedoguz 0:d1f0ae13f4a7 546
mbedoguz 0:d1f0ae13f4a7 547 inv_error_t MPU9250_DMP::dmpEnable3Quat(void)
mbedoguz 0:d1f0ae13f4a7 548 {
mbedoguz 0:d1f0ae13f4a7 549 unsigned short dmpFeatures;
mbedoguz 0:d1f0ae13f4a7 550
mbedoguz 0:d1f0ae13f4a7 551 // 3-axis and 6-axis quat are mutually exclusive
mbedoguz 0:d1f0ae13f4a7 552 dmpFeatures = dmpGetEnabledFeatures();
mbedoguz 0:d1f0ae13f4a7 553 dmpFeatures &= ~(DMP_FEATURE_6X_LP_QUAT);
mbedoguz 0:d1f0ae13f4a7 554 dmpFeatures |= DMP_FEATURE_LP_QUAT;
mbedoguz 0:d1f0ae13f4a7 555
mbedoguz 0:d1f0ae13f4a7 556 if (dmpEnableFeatures(dmpFeatures) != INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 557 return INV_ERROR;
mbedoguz 0:d1f0ae13f4a7 558
mbedoguz 0:d1f0ae13f4a7 559 return dmp_enable_lp_quat(1);
mbedoguz 0:d1f0ae13f4a7 560 }
mbedoguz 0:d1f0ae13f4a7 561
mbedoguz 0:d1f0ae13f4a7 562 unsigned long MPU9250_DMP::dmpGetPedometerSteps(void)
mbedoguz 0:d1f0ae13f4a7 563 {
mbedoguz 0:d1f0ae13f4a7 564 unsigned long steps;
mbedoguz 0:d1f0ae13f4a7 565 if (dmp_get_pedometer_step_count(&steps) == INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 566 {
mbedoguz 0:d1f0ae13f4a7 567 return steps;
mbedoguz 0:d1f0ae13f4a7 568 }
mbedoguz 0:d1f0ae13f4a7 569 return 0;
mbedoguz 0:d1f0ae13f4a7 570 }
mbedoguz 0:d1f0ae13f4a7 571
mbedoguz 0:d1f0ae13f4a7 572 inv_error_t MPU9250_DMP::dmpSetPedometerSteps(unsigned long steps)
mbedoguz 0:d1f0ae13f4a7 573 {
mbedoguz 0:d1f0ae13f4a7 574 return dmp_set_pedometer_step_count(steps);
mbedoguz 0:d1f0ae13f4a7 575 }
mbedoguz 0:d1f0ae13f4a7 576
mbedoguz 0:d1f0ae13f4a7 577 unsigned long MPU9250_DMP::dmpGetPedometerTime(void)
mbedoguz 0:d1f0ae13f4a7 578 {
mbedoguz 0:d1f0ae13f4a7 579 unsigned long walkTime;
mbedoguz 0:d1f0ae13f4a7 580 if (dmp_get_pedometer_walk_time(&walkTime) == INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 581 {
mbedoguz 0:d1f0ae13f4a7 582 return walkTime;
mbedoguz 0:d1f0ae13f4a7 583 }
mbedoguz 0:d1f0ae13f4a7 584 return 0;
mbedoguz 0:d1f0ae13f4a7 585 }
mbedoguz 0:d1f0ae13f4a7 586
mbedoguz 0:d1f0ae13f4a7 587 inv_error_t MPU9250_DMP::dmpSetPedometerTime(unsigned long time)
mbedoguz 0:d1f0ae13f4a7 588 {
mbedoguz 0:d1f0ae13f4a7 589 return dmp_set_pedometer_walk_time(time);
mbedoguz 0:d1f0ae13f4a7 590 }
mbedoguz 0:d1f0ae13f4a7 591
mbedoguz 0:d1f0ae13f4a7 592 float MPU9250_DMP::calcAccel(int axis)
mbedoguz 0:d1f0ae13f4a7 593 {
mbedoguz 0:d1f0ae13f4a7 594 return (float) axis / (float) _aSense;
mbedoguz 0:d1f0ae13f4a7 595 }
mbedoguz 0:d1f0ae13f4a7 596
mbedoguz 0:d1f0ae13f4a7 597 float MPU9250_DMP::calcGyro(int axis)
mbedoguz 0:d1f0ae13f4a7 598 {
mbedoguz 0:d1f0ae13f4a7 599 return (float) axis / (float) _gSense;
mbedoguz 0:d1f0ae13f4a7 600 }
mbedoguz 0:d1f0ae13f4a7 601
mbedoguz 0:d1f0ae13f4a7 602 float MPU9250_DMP::calcMag(int axis)
mbedoguz 0:d1f0ae13f4a7 603 {
mbedoguz 0:d1f0ae13f4a7 604 return (float) axis / (float) _mSense;
mbedoguz 0:d1f0ae13f4a7 605 }
mbedoguz 0:d1f0ae13f4a7 606
mbedoguz 0:d1f0ae13f4a7 607 float MPU9250_DMP::calcQuat(long axis)
mbedoguz 0:d1f0ae13f4a7 608 {
mbedoguz 0:d1f0ae13f4a7 609 return qToFloat(axis, 30);
mbedoguz 0:d1f0ae13f4a7 610 }
mbedoguz 0:d1f0ae13f4a7 611
mbedoguz 0:d1f0ae13f4a7 612 float MPU9250_DMP::qToFloat(long number, unsigned char q)
mbedoguz 0:d1f0ae13f4a7 613 {
mbedoguz 0:d1f0ae13f4a7 614 unsigned long mask;
mbedoguz 0:d1f0ae13f4a7 615 for (int i=0; i<q; i++)
mbedoguz 0:d1f0ae13f4a7 616 {
mbedoguz 0:d1f0ae13f4a7 617 mask |= (1<<i);
mbedoguz 0:d1f0ae13f4a7 618 }
mbedoguz 0:d1f0ae13f4a7 619 return (number >> q) + ((number & mask) / (float) (2<<(q-1)));
mbedoguz 0:d1f0ae13f4a7 620 }
mbedoguz 0:d1f0ae13f4a7 621
mbedoguz 0:d1f0ae13f4a7 622 void MPU9250_DMP::computeEulerAngles(bool degrees)
mbedoguz 0:d1f0ae13f4a7 623 {
mbedoguz 0:d1f0ae13f4a7 624 double dqw = qToFloat(qw, 30);
mbedoguz 0:d1f0ae13f4a7 625 double dqx = qToFloat(qx, 30);
mbedoguz 0:d1f0ae13f4a7 626 double dqy = qToFloat(qy, 30);
mbedoguz 0:d1f0ae13f4a7 627 double dqz = qToFloat(qz, 30);
mbedoguz 0:d1f0ae13f4a7 628
mbedoguz 0:d1f0ae13f4a7 629 double ysqr = dqy * dqy;
mbedoguz 0:d1f0ae13f4a7 630 double t0 = -2.0f * (ysqr + dqz * dqz) + 1.0f;
mbedoguz 0:d1f0ae13f4a7 631 double t1 = +2.0f * (dqx * dqy - dqw * dqz);
mbedoguz 0:d1f0ae13f4a7 632 double t2 = -2.0f * (dqx * dqz + dqw * dqy);
mbedoguz 0:d1f0ae13f4a7 633 double t3 = +2.0f * (dqy * dqz - dqw * dqx);
mbedoguz 0:d1f0ae13f4a7 634 double t4 = -2.0f * (dqx * dqx + ysqr) + 1.0f;
mbedoguz 0:d1f0ae13f4a7 635
mbedoguz 0:d1f0ae13f4a7 636 // Keep t2 within range of asin (-1, 1)
mbedoguz 0:d1f0ae13f4a7 637 t2 = t2 > 1.0f ? 1.0f : t2;
mbedoguz 0:d1f0ae13f4a7 638 t2 = t2 < -1.0f ? -1.0f : t2;
mbedoguz 0:d1f0ae13f4a7 639
mbedoguz 0:d1f0ae13f4a7 640 pitch = asin(t2) * 2;
mbedoguz 0:d1f0ae13f4a7 641 roll = atan2(t3, t4);
mbedoguz 0:d1f0ae13f4a7 642 yaw = atan2(t1, t0);
mbedoguz 0:d1f0ae13f4a7 643
mbedoguz 0:d1f0ae13f4a7 644 if (degrees)
mbedoguz 0:d1f0ae13f4a7 645 {
mbedoguz 0:d1f0ae13f4a7 646 pitch *= (180.0 / PI);
mbedoguz 0:d1f0ae13f4a7 647 roll *= (180.0 / PI);
mbedoguz 0:d1f0ae13f4a7 648 yaw *= (180.0 / PI);
mbedoguz 0:d1f0ae13f4a7 649 if (pitch < 0) pitch = 360.0 + pitch;
mbedoguz 0:d1f0ae13f4a7 650 if (roll < 0) roll = 360.0 + roll;
mbedoguz 0:d1f0ae13f4a7 651 if (yaw < 0) yaw = 360.0 + yaw;
mbedoguz 0:d1f0ae13f4a7 652 }
mbedoguz 0:d1f0ae13f4a7 653 }
mbedoguz 0:d1f0ae13f4a7 654
mbedoguz 0:d1f0ae13f4a7 655 float MPU9250_DMP::computeCompassHeading(void)
mbedoguz 0:d1f0ae13f4a7 656 {
mbedoguz 0:d1f0ae13f4a7 657 if (my == 0)
mbedoguz 0:d1f0ae13f4a7 658 heading = (mx < 0) ? 180.0 : 0;
mbedoguz 0:d1f0ae13f4a7 659 else
mbedoguz 0:d1f0ae13f4a7 660 heading = atan2((double)mx, (double)my);
mbedoguz 0:d1f0ae13f4a7 661
mbedoguz 0:d1f0ae13f4a7 662 if (heading > PI) heading -= (2 * PI);
mbedoguz 0:d1f0ae13f4a7 663 else if (heading < -PI) heading += (2 * PI);
mbedoguz 0:d1f0ae13f4a7 664 else if (heading < 0) heading += 2 * PI;
mbedoguz 0:d1f0ae13f4a7 665
mbedoguz 0:d1f0ae13f4a7 666 heading*= 180.0 / PI;
mbedoguz 0:d1f0ae13f4a7 667
mbedoguz 0:d1f0ae13f4a7 668 return heading;
mbedoguz 0:d1f0ae13f4a7 669 }
mbedoguz 0:d1f0ae13f4a7 670
mbedoguz 0:d1f0ae13f4a7 671 unsigned short MPU9250_DMP::orientation_row_2_scale(const signed char *row)
mbedoguz 0:d1f0ae13f4a7 672 {
mbedoguz 0:d1f0ae13f4a7 673 unsigned short b;
mbedoguz 0:d1f0ae13f4a7 674
mbedoguz 0:d1f0ae13f4a7 675 if (row[0] > 0)
mbedoguz 0:d1f0ae13f4a7 676 b = 0;
mbedoguz 0:d1f0ae13f4a7 677 else if (row[0] < 0)
mbedoguz 0:d1f0ae13f4a7 678 b = 4;
mbedoguz 0:d1f0ae13f4a7 679 else if (row[1] > 0)
mbedoguz 0:d1f0ae13f4a7 680 b = 1;
mbedoguz 0:d1f0ae13f4a7 681 else if (row[1] < 0)
mbedoguz 0:d1f0ae13f4a7 682 b = 5;
mbedoguz 0:d1f0ae13f4a7 683 else if (row[2] > 0)
mbedoguz 0:d1f0ae13f4a7 684 b = 2;
mbedoguz 0:d1f0ae13f4a7 685 else if (row[2] < 0)
mbedoguz 0:d1f0ae13f4a7 686 b = 6;
mbedoguz 0:d1f0ae13f4a7 687 else
mbedoguz 0:d1f0ae13f4a7 688 b = 7; // error
mbedoguz 0:d1f0ae13f4a7 689 return b;
mbedoguz 0:d1f0ae13f4a7 690 }
mbedoguz 0:d1f0ae13f4a7 691
mbedoguz 0:d1f0ae13f4a7 692 static void tap_cb(unsigned char direction, unsigned char count)
mbedoguz 0:d1f0ae13f4a7 693 {
mbedoguz 0:d1f0ae13f4a7 694 _tap_available = true;
mbedoguz 0:d1f0ae13f4a7 695 tap_count = count;
mbedoguz 0:d1f0ae13f4a7 696 tap_direction = direction;
mbedoguz 0:d1f0ae13f4a7 697 }
mbedoguz 0:d1f0ae13f4a7 698
mbedoguz 0:d1f0ae13f4a7 699 static void orient_cb(unsigned char orient)
mbedoguz 0:d1f0ae13f4a7 700 {
mbedoguz 0:d1f0ae13f4a7 701 mpu9250_orientation = orient;
mbedoguz 0:d1f0ae13f4a7 702 }