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@3:6f87862d2d68, 2017-08-11 (annotated)

Committer:
mbedoguz
Date:
Fri Aug 11 08:31:27 2017 +0000
Revision:
3:6f87862d2d68
Parent:
2:c35f8379f2cb 
computeEulerAngles is corrected according to issue https://github.com/sparkfun/SparkFun_MPU-9250-DMP_Arduino_Library/issues/5; Now euler angles are computed in a correct way and output is surprisingly satisfying.

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