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@0:d1f0ae13f4a7, 2017-08-07 (annotated)

Committer:
mbedoguz
Date:
Mon Aug 07 13:50:23 2017 +0000
Revision:
0:d1f0ae13f4a7
Child:
1:80a269cb9d4d
It is modified so that it works with mbed platform

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
mbedoguz 0:d1f0ae13f4a7 238 if (mpu_read_reg(MPU9250_FIFO_COUNTH, &fifoH) != INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 239 return 0;
mbedoguz 0:d1f0ae13f4a7 240 if (mpu_read_reg(MPU9250_FIFO_COUNTL, &fifoL) != INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 241 return 0;
mbedoguz 0:d1f0ae13f4a7 242
mbedoguz 0:d1f0ae13f4a7 243 return (fifoH << 8 ) | fifoL;
mbedoguz 0:d1f0ae13f4a7 244 }
mbedoguz 0:d1f0ae13f4a7 245
mbedoguz 0:d1f0ae13f4a7 246 inv_error_t MPU9250_DMP::updateFifo(void)
mbedoguz 0:d1f0ae13f4a7 247 {
mbedoguz 0:d1f0ae13f4a7 248 short gyro[3], accel[3];
mbedoguz 0:d1f0ae13f4a7 249 unsigned long timestamp;
mbedoguz 0:d1f0ae13f4a7 250 unsigned char sensors, more;
mbedoguz 0:d1f0ae13f4a7 251
mbedoguz 0:d1f0ae13f4a7 252 if (mpu_read_fifo(gyro, accel, &timestamp, &sensors, &more) != INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 253 return INV_ERROR;
mbedoguz 0:d1f0ae13f4a7 254
mbedoguz 0:d1f0ae13f4a7 255 if (sensors & INV_XYZ_ACCEL)
mbedoguz 0:d1f0ae13f4a7 256 {
mbedoguz 0:d1f0ae13f4a7 257 ax = accel[X_AXIS];
mbedoguz 0:d1f0ae13f4a7 258 ay = accel[Y_AXIS];
mbedoguz 0:d1f0ae13f4a7 259 az = accel[Z_AXIS];
mbedoguz 0:d1f0ae13f4a7 260 }
mbedoguz 0:d1f0ae13f4a7 261 if (sensors & INV_X_GYRO)
mbedoguz 0:d1f0ae13f4a7 262 gx = gyro[X_AXIS];
mbedoguz 0:d1f0ae13f4a7 263 if (sensors & INV_Y_GYRO)
mbedoguz 0:d1f0ae13f4a7 264 gy = gyro[Y_AXIS];
mbedoguz 0:d1f0ae13f4a7 265 if (sensors & INV_Z_GYRO)
mbedoguz 0:d1f0ae13f4a7 266 gz = gyro[Z_AXIS];
mbedoguz 0:d1f0ae13f4a7 267
mbedoguz 0:d1f0ae13f4a7 268 time = timestamp;
mbedoguz 0:d1f0ae13f4a7 269
mbedoguz 0:d1f0ae13f4a7 270 return INV_SUCCESS;
mbedoguz 0:d1f0ae13f4a7 271 }
mbedoguz 0:d1f0ae13f4a7 272
mbedoguz 0:d1f0ae13f4a7 273 inv_error_t MPU9250_DMP::setSensors(unsigned char sensors)
mbedoguz 0:d1f0ae13f4a7 274 {
mbedoguz 0:d1f0ae13f4a7 275 return mpu_set_sensors(sensors);
mbedoguz 0:d1f0ae13f4a7 276 }
mbedoguz 0:d1f0ae13f4a7 277
mbedoguz 0:d1f0ae13f4a7 278 bool MPU9250_DMP::dataReady()
mbedoguz 0:d1f0ae13f4a7 279 {
mbedoguz 0:d1f0ae13f4a7 280 unsigned char intStatusReg;
mbedoguz 0:d1f0ae13f4a7 281
mbedoguz 0:d1f0ae13f4a7 282 if (mpu_read_reg(MPU9250_INT_STATUS, &intStatusReg) == INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 283 {
mbedoguz 0:d1f0ae13f4a7 284 return (intStatusReg & (1<<INT_STATUS_RAW_DATA_RDY_INT));
mbedoguz 0:d1f0ae13f4a7 285 }
mbedoguz 0:d1f0ae13f4a7 286 return false;
mbedoguz 0:d1f0ae13f4a7 287 }
mbedoguz 0:d1f0ae13f4a7 288
mbedoguz 0:d1f0ae13f4a7 289 inv_error_t MPU9250_DMP::update(unsigned char sensors)
mbedoguz 0:d1f0ae13f4a7 290 {
mbedoguz 0:d1f0ae13f4a7 291 inv_error_t aErr = INV_SUCCESS;
mbedoguz 0:d1f0ae13f4a7 292 inv_error_t gErr = INV_SUCCESS;
mbedoguz 0:d1f0ae13f4a7 293 inv_error_t mErr = INV_SUCCESS;
mbedoguz 0:d1f0ae13f4a7 294 inv_error_t tErr = INV_SUCCESS;
mbedoguz 0:d1f0ae13f4a7 295
mbedoguz 0:d1f0ae13f4a7 296 if (sensors & UPDATE_ACCEL)
mbedoguz 0:d1f0ae13f4a7 297 aErr = updateAccel();
mbedoguz 0:d1f0ae13f4a7 298 if (sensors & UPDATE_GYRO)
mbedoguz 0:d1f0ae13f4a7 299 gErr = updateGyro();
mbedoguz 0:d1f0ae13f4a7 300 if (sensors & UPDATE_COMPASS)
mbedoguz 0:d1f0ae13f4a7 301 mErr = updateCompass();
mbedoguz 0:d1f0ae13f4a7 302 if (sensors & UPDATE_TEMP)
mbedoguz 0:d1f0ae13f4a7 303 tErr = updateTemperature();
mbedoguz 0:d1f0ae13f4a7 304
mbedoguz 0:d1f0ae13f4a7 305 return aErr | gErr | mErr | tErr;
mbedoguz 0:d1f0ae13f4a7 306 }
mbedoguz 0:d1f0ae13f4a7 307
mbedoguz 0:d1f0ae13f4a7 308 int MPU9250_DMP::updateAccel(void)
mbedoguz 0:d1f0ae13f4a7 309 {
mbedoguz 0:d1f0ae13f4a7 310 short data[3];
mbedoguz 0:d1f0ae13f4a7 311
mbedoguz 0:d1f0ae13f4a7 312 if (mpu_get_accel_reg(data, &time))
mbedoguz 0:d1f0ae13f4a7 313 {
mbedoguz 0:d1f0ae13f4a7 314 return INV_ERROR;
mbedoguz 0:d1f0ae13f4a7 315 }
mbedoguz 0:d1f0ae13f4a7 316 ax = data[X_AXIS];
mbedoguz 0:d1f0ae13f4a7 317 ay = data[Y_AXIS];
mbedoguz 0:d1f0ae13f4a7 318 az = data[Z_AXIS];
mbedoguz 0:d1f0ae13f4a7 319 return INV_SUCCESS;
mbedoguz 0:d1f0ae13f4a7 320 }
mbedoguz 0:d1f0ae13f4a7 321
mbedoguz 0:d1f0ae13f4a7 322 int MPU9250_DMP::updateGyro(void)
mbedoguz 0:d1f0ae13f4a7 323 {
mbedoguz 0:d1f0ae13f4a7 324 short data[3];
mbedoguz 0:d1f0ae13f4a7 325
mbedoguz 0:d1f0ae13f4a7 326 if (mpu_get_gyro_reg(data, &time))
mbedoguz 0:d1f0ae13f4a7 327 {
mbedoguz 0:d1f0ae13f4a7 328 return INV_ERROR;
mbedoguz 0:d1f0ae13f4a7 329 }
mbedoguz 0:d1f0ae13f4a7 330 gx = data[X_AXIS];
mbedoguz 0:d1f0ae13f4a7 331 gy = data[Y_AXIS];
mbedoguz 0:d1f0ae13f4a7 332 gz = data[Z_AXIS];
mbedoguz 0:d1f0ae13f4a7 333 return INV_SUCCESS;
mbedoguz 0:d1f0ae13f4a7 334 }
mbedoguz 0:d1f0ae13f4a7 335
mbedoguz 0:d1f0ae13f4a7 336 int MPU9250_DMP::updateCompass(void)
mbedoguz 0:d1f0ae13f4a7 337 {
mbedoguz 0:d1f0ae13f4a7 338 short data[3];
mbedoguz 0:d1f0ae13f4a7 339
mbedoguz 0:d1f0ae13f4a7 340 if (mpu_get_compass_reg(data, &time))
mbedoguz 0:d1f0ae13f4a7 341 {
mbedoguz 0:d1f0ae13f4a7 342 return INV_ERROR;
mbedoguz 0:d1f0ae13f4a7 343 }
mbedoguz 0:d1f0ae13f4a7 344 mx = data[X_AXIS];
mbedoguz 0:d1f0ae13f4a7 345 my = data[Y_AXIS];
mbedoguz 0:d1f0ae13f4a7 346 mz = data[Z_AXIS];
mbedoguz 0:d1f0ae13f4a7 347 return INV_SUCCESS;
mbedoguz 0:d1f0ae13f4a7 348 }
mbedoguz 0:d1f0ae13f4a7 349
mbedoguz 0:d1f0ae13f4a7 350 inv_error_t MPU9250_DMP::updateTemperature(void)
mbedoguz 0:d1f0ae13f4a7 351 {
mbedoguz 0:d1f0ae13f4a7 352 return mpu_get_temperature(&temperature, &time);
mbedoguz 0:d1f0ae13f4a7 353 }
mbedoguz 0:d1f0ae13f4a7 354
mbedoguz 0:d1f0ae13f4a7 355 int MPU9250_DMP::selfTest(unsigned char debug)
mbedoguz 0:d1f0ae13f4a7 356 {
mbedoguz 0:d1f0ae13f4a7 357 long gyro[3], accel[3];
mbedoguz 0:d1f0ae13f4a7 358 return mpu_run_self_test(gyro, accel);
mbedoguz 0:d1f0ae13f4a7 359 }
mbedoguz 0:d1f0ae13f4a7 360
mbedoguz 0:d1f0ae13f4a7 361 inv_error_t MPU9250_DMP::dmpBegin(unsigned short features, unsigned short fifoRate)
mbedoguz 0:d1f0ae13f4a7 362 {
mbedoguz 0:d1f0ae13f4a7 363 unsigned short feat = features;
mbedoguz 0:d1f0ae13f4a7 364 unsigned short rate = fifoRate;
mbedoguz 0:d1f0ae13f4a7 365
mbedoguz 0:d1f0ae13f4a7 366 if (dmpLoad() != INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 367 return INV_ERROR;
mbedoguz 0:d1f0ae13f4a7 368
mbedoguz 0:d1f0ae13f4a7 369 // 3-axis and 6-axis LP quat are mutually exclusive.
mbedoguz 0:d1f0ae13f4a7 370 // If both are selected, default to 3-axis
mbedoguz 0:d1f0ae13f4a7 371 if (feat & DMP_FEATURE_LP_QUAT)
mbedoguz 0:d1f0ae13f4a7 372 {
mbedoguz 0:d1f0ae13f4a7 373 feat &= ~(DMP_FEATURE_6X_LP_QUAT);
mbedoguz 0:d1f0ae13f4a7 374 dmp_enable_lp_quat(1);
mbedoguz 0:d1f0ae13f4a7 375 }
mbedoguz 0:d1f0ae13f4a7 376 else if (feat & DMP_FEATURE_6X_LP_QUAT)
mbedoguz 0:d1f0ae13f4a7 377 dmp_enable_6x_lp_quat(1);
mbedoguz 0:d1f0ae13f4a7 378
mbedoguz 0:d1f0ae13f4a7 379 if (feat & DMP_FEATURE_GYRO_CAL)
mbedoguz 0:d1f0ae13f4a7 380 dmp_enable_gyro_cal(1);
mbedoguz 0:d1f0ae13f4a7 381
mbedoguz 0:d1f0ae13f4a7 382 if (dmpEnableFeatures(feat) != INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 383 return INV_ERROR;
mbedoguz 0:d1f0ae13f4a7 384
mbedoguz 0:d1f0ae13f4a7 385 rate = constrain(rate, 1, 200);
mbedoguz 0:d1f0ae13f4a7 386 if (dmpSetFifoRate(rate) != INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 387 return INV_ERROR;
mbedoguz 0:d1f0ae13f4a7 388
mbedoguz 0:d1f0ae13f4a7 389 return mpu_set_dmp_state(1);
mbedoguz 0:d1f0ae13f4a7 390 }
mbedoguz 0:d1f0ae13f4a7 391
mbedoguz 0:d1f0ae13f4a7 392 inv_error_t MPU9250_DMP::dmpLoad(void)
mbedoguz 0:d1f0ae13f4a7 393 {
mbedoguz 0:d1f0ae13f4a7 394 return dmp_load_motion_driver_firmware();
mbedoguz 0:d1f0ae13f4a7 395 }
mbedoguz 0:d1f0ae13f4a7 396
mbedoguz 0:d1f0ae13f4a7 397 unsigned short MPU9250_DMP::dmpGetFifoRate(void)
mbedoguz 0:d1f0ae13f4a7 398 {
mbedoguz 0:d1f0ae13f4a7 399 unsigned short rate;
mbedoguz 0:d1f0ae13f4a7 400 if (dmp_get_fifo_rate(&rate) == INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 401 return rate;
mbedoguz 0:d1f0ae13f4a7 402
mbedoguz 0:d1f0ae13f4a7 403 return 0;
mbedoguz 0:d1f0ae13f4a7 404 }
mbedoguz 0:d1f0ae13f4a7 405
mbedoguz 0:d1f0ae13f4a7 406 inv_error_t MPU9250_DMP::dmpSetFifoRate(unsigned short rate)
mbedoguz 0:d1f0ae13f4a7 407 {
mbedoguz 0:d1f0ae13f4a7 408 if (rate > MAX_DMP_SAMPLE_RATE) rate = MAX_DMP_SAMPLE_RATE;
mbedoguz 0:d1f0ae13f4a7 409 return dmp_set_fifo_rate(rate);
mbedoguz 0:d1f0ae13f4a7 410 }
mbedoguz 0:d1f0ae13f4a7 411
mbedoguz 0:d1f0ae13f4a7 412 inv_error_t MPU9250_DMP::dmpUpdateFifo(long DEBUG[4])
mbedoguz 0:d1f0ae13f4a7 413 {
mbedoguz 0:d1f0ae13f4a7 414 short gyro[3];
mbedoguz 0:d1f0ae13f4a7 415 short accel[3];
mbedoguz 0:d1f0ae13f4a7 416 long quat[4];
mbedoguz 0:d1f0ae13f4a7 417 unsigned long timestamp;
mbedoguz 0:d1f0ae13f4a7 418 short sensors;
mbedoguz 0:d1f0ae13f4a7 419 unsigned char more;
mbedoguz 0:d1f0ae13f4a7 420
mbedoguz 0:d1f0ae13f4a7 421 if (dmp_read_fifo(gyro, accel, quat, &timestamp, &sensors, &more)
mbedoguz 0:d1f0ae13f4a7 422 != INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 423 {
mbedoguz 0:d1f0ae13f4a7 424 return INV_ERROR;
mbedoguz 0:d1f0ae13f4a7 425 }
mbedoguz 0:d1f0ae13f4a7 426
mbedoguz 0:d1f0ae13f4a7 427 if (sensors & INV_XYZ_ACCEL)
mbedoguz 0:d1f0ae13f4a7 428 {
mbedoguz 0:d1f0ae13f4a7 429 ax = accel[X_AXIS];
mbedoguz 0:d1f0ae13f4a7 430 ay = accel[Y_AXIS];
mbedoguz 0:d1f0ae13f4a7 431 az = accel[Z_AXIS];
mbedoguz 0:d1f0ae13f4a7 432 }
mbedoguz 0:d1f0ae13f4a7 433 if (sensors & INV_X_GYRO)
mbedoguz 0:d1f0ae13f4a7 434 gx = gyro[X_AXIS];
mbedoguz 0:d1f0ae13f4a7 435 if (sensors & INV_Y_GYRO)
mbedoguz 0:d1f0ae13f4a7 436 gy = gyro[Y_AXIS];
mbedoguz 0:d1f0ae13f4a7 437 if (sensors & INV_Z_GYRO)
mbedoguz 0:d1f0ae13f4a7 438 gz = gyro[Z_AXIS];
mbedoguz 0:d1f0ae13f4a7 439 if (sensors & INV_WXYZ_QUAT)
mbedoguz 0:d1f0ae13f4a7 440 {
mbedoguz 0:d1f0ae13f4a7 441 qw = quat[0];
mbedoguz 0:d1f0ae13f4a7 442 qx = quat[1];
mbedoguz 0:d1f0ae13f4a7 443 qy = quat[2];
mbedoguz 0:d1f0ae13f4a7 444 qz = quat[3];
mbedoguz 0:d1f0ae13f4a7 445 }
mbedoguz 0:d1f0ae13f4a7 446 DEBUG[0]=quat[0];
mbedoguz 0:d1f0ae13f4a7 447 DEBUG[1]=quat[1];
mbedoguz 0:d1f0ae13f4a7 448 DEBUG[2]=quat[2];
mbedoguz 0:d1f0ae13f4a7 449 DEBUG[3]=quat[3];
mbedoguz 0:d1f0ae13f4a7 450
mbedoguz 0:d1f0ae13f4a7 451 time = timestamp;
mbedoguz 0:d1f0ae13f4a7 452
mbedoguz 0:d1f0ae13f4a7 453 return INV_SUCCESS;
mbedoguz 0:d1f0ae13f4a7 454 }
mbedoguz 0:d1f0ae13f4a7 455
mbedoguz 0:d1f0ae13f4a7 456 inv_error_t MPU9250_DMP::dmpEnableFeatures(unsigned short mask)
mbedoguz 0:d1f0ae13f4a7 457 {
mbedoguz 0:d1f0ae13f4a7 458 unsigned short enMask = 0;
mbedoguz 0:d1f0ae13f4a7 459 enMask |= mask;
mbedoguz 0:d1f0ae13f4a7 460 // Combat known issue where fifo sample rate is incorrect
mbedoguz 0:d1f0ae13f4a7 461 // unless tap is enabled in the DMP.
mbedoguz 0:d1f0ae13f4a7 462 enMask |= DMP_FEATURE_TAP;
mbedoguz 0:d1f0ae13f4a7 463 return dmp_enable_feature(enMask);
mbedoguz 0:d1f0ae13f4a7 464 }
mbedoguz 0:d1f0ae13f4a7 465
mbedoguz 0:d1f0ae13f4a7 466 unsigned short MPU9250_DMP::dmpGetEnabledFeatures(void)
mbedoguz 0:d1f0ae13f4a7 467 {
mbedoguz 0:d1f0ae13f4a7 468 unsigned short mask;
mbedoguz 0:d1f0ae13f4a7 469 if (dmp_get_enabled_features(&mask) == INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 470 return mask;
mbedoguz 0:d1f0ae13f4a7 471 return 0;
mbedoguz 0:d1f0ae13f4a7 472 }
mbedoguz 0:d1f0ae13f4a7 473
mbedoguz 0:d1f0ae13f4a7 474 inv_error_t MPU9250_DMP::dmpSetTap(
mbedoguz 0:d1f0ae13f4a7 475 unsigned short xThresh, unsigned short yThresh, unsigned short zThresh,
mbedoguz 0:d1f0ae13f4a7 476 unsigned char taps, unsigned short tapTime, unsigned short tapMulti)
mbedoguz 0:d1f0ae13f4a7 477 {
mbedoguz 0:d1f0ae13f4a7 478 unsigned char axes = 0;
mbedoguz 0:d1f0ae13f4a7 479 if (xThresh > 0)
mbedoguz 0:d1f0ae13f4a7 480 {
mbedoguz 0:d1f0ae13f4a7 481 axes |= TAP_X;
mbedoguz 0:d1f0ae13f4a7 482 xThresh = constrain(xThresh, 1, 1600);
mbedoguz 0:d1f0ae13f4a7 483 if (dmp_set_tap_thresh(1<<X_AXIS, xThresh) != INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 484 return INV_ERROR;
mbedoguz 0:d1f0ae13f4a7 485 }
mbedoguz 0:d1f0ae13f4a7 486 if (yThresh > 0)
mbedoguz 0:d1f0ae13f4a7 487 {
mbedoguz 0:d1f0ae13f4a7 488 axes |= TAP_Y;
mbedoguz 0:d1f0ae13f4a7 489 yThresh = constrain(yThresh, 1, 1600);
mbedoguz 0:d1f0ae13f4a7 490 if (dmp_set_tap_thresh(1<<Y_AXIS, yThresh) != INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 491 return INV_ERROR;
mbedoguz 0:d1f0ae13f4a7 492 }
mbedoguz 0:d1f0ae13f4a7 493 if (zThresh > 0)
mbedoguz 0:d1f0ae13f4a7 494 {
mbedoguz 0:d1f0ae13f4a7 495 axes |= TAP_Z;
mbedoguz 0:d1f0ae13f4a7 496 zThresh = constrain(zThresh, 1, 1600);
mbedoguz 0:d1f0ae13f4a7 497 if (dmp_set_tap_thresh(1<<Z_AXIS, zThresh) != INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 498 return INV_ERROR;
mbedoguz 0:d1f0ae13f4a7 499 }
mbedoguz 0:d1f0ae13f4a7 500 if (dmp_set_tap_axes(axes) != INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 501 return INV_ERROR;
mbedoguz 0:d1f0ae13f4a7 502 if (dmp_set_tap_count(taps) != INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 503 return INV_ERROR;
mbedoguz 0:d1f0ae13f4a7 504 if (dmp_set_tap_time(tapTime) != INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 505 return INV_ERROR;
mbedoguz 0:d1f0ae13f4a7 506 if (dmp_set_tap_time_multi(tapMulti) != INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 507 return INV_ERROR;
mbedoguz 0:d1f0ae13f4a7 508
mbedoguz 0:d1f0ae13f4a7 509 dmp_register_tap_cb(tap_cb);
mbedoguz 0:d1f0ae13f4a7 510
mbedoguz 0:d1f0ae13f4a7 511 return INV_SUCCESS;
mbedoguz 0:d1f0ae13f4a7 512 }
mbedoguz 0:d1f0ae13f4a7 513
mbedoguz 0:d1f0ae13f4a7 514 unsigned char MPU9250_DMP::getTapDir(void)
mbedoguz 0:d1f0ae13f4a7 515 {
mbedoguz 0:d1f0ae13f4a7 516 _tap_available = false;
mbedoguz 0:d1f0ae13f4a7 517 return tap_direction;
mbedoguz 0:d1f0ae13f4a7 518 }
mbedoguz 0:d1f0ae13f4a7 519
mbedoguz 0:d1f0ae13f4a7 520 unsigned char MPU9250_DMP::getTapCount(void)
mbedoguz 0:d1f0ae13f4a7 521 {
mbedoguz 0:d1f0ae13f4a7 522 _tap_available = false;
mbedoguz 0:d1f0ae13f4a7 523 return tap_count;
mbedoguz 0:d1f0ae13f4a7 524 }
mbedoguz 0:d1f0ae13f4a7 525
mbedoguz 0:d1f0ae13f4a7 526 bool MPU9250_DMP::tapAvailable(void)
mbedoguz 0:d1f0ae13f4a7 527 {
mbedoguz 0:d1f0ae13f4a7 528 return _tap_available;
mbedoguz 0:d1f0ae13f4a7 529 }
mbedoguz 0:d1f0ae13f4a7 530
mbedoguz 0:d1f0ae13f4a7 531 inv_error_t MPU9250_DMP::dmpSetOrientation(const signed char * orientationMatrix)
mbedoguz 0:d1f0ae13f4a7 532 {
mbedoguz 0:d1f0ae13f4a7 533 unsigned short scalar;
mbedoguz 0:d1f0ae13f4a7 534 scalar = orientation_row_2_scale(orientationMatrix);
mbedoguz 0:d1f0ae13f4a7 535 scalar |= orientation_row_2_scale(orientationMatrix + 3) << 3;
mbedoguz 0:d1f0ae13f4a7 536 scalar |= orientation_row_2_scale(orientationMatrix + 6) << 6;
mbedoguz 0:d1f0ae13f4a7 537
mbedoguz 0:d1f0ae13f4a7 538 dmp_register_android_orient_cb(orient_cb);
mbedoguz 0:d1f0ae13f4a7 539
mbedoguz 0:d1f0ae13f4a7 540 return dmp_set_orientation(scalar);
mbedoguz 0:d1f0ae13f4a7 541 }
mbedoguz 0:d1f0ae13f4a7 542
mbedoguz 0:d1f0ae13f4a7 543 unsigned char MPU9250_DMP::dmpGetOrientation(void)
mbedoguz 0:d1f0ae13f4a7 544 {
mbedoguz 0:d1f0ae13f4a7 545 return mpu9250_orientation;
mbedoguz 0:d1f0ae13f4a7 546 }
mbedoguz 0:d1f0ae13f4a7 547
mbedoguz 0:d1f0ae13f4a7 548 inv_error_t MPU9250_DMP::dmpEnable3Quat(void)
mbedoguz 0:d1f0ae13f4a7 549 {
mbedoguz 0:d1f0ae13f4a7 550 unsigned short dmpFeatures;
mbedoguz 0:d1f0ae13f4a7 551
mbedoguz 0:d1f0ae13f4a7 552 // 3-axis and 6-axis quat are mutually exclusive
mbedoguz 0:d1f0ae13f4a7 553 dmpFeatures = dmpGetEnabledFeatures();
mbedoguz 0:d1f0ae13f4a7 554 dmpFeatures &= ~(DMP_FEATURE_6X_LP_QUAT);
mbedoguz 0:d1f0ae13f4a7 555 dmpFeatures |= DMP_FEATURE_LP_QUAT;
mbedoguz 0:d1f0ae13f4a7 556
mbedoguz 0:d1f0ae13f4a7 557 if (dmpEnableFeatures(dmpFeatures) != INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 558 return INV_ERROR;
mbedoguz 0:d1f0ae13f4a7 559
mbedoguz 0:d1f0ae13f4a7 560 return dmp_enable_lp_quat(1);
mbedoguz 0:d1f0ae13f4a7 561 }
mbedoguz 0:d1f0ae13f4a7 562
mbedoguz 0:d1f0ae13f4a7 563 unsigned long MPU9250_DMP::dmpGetPedometerSteps(void)
mbedoguz 0:d1f0ae13f4a7 564 {
mbedoguz 0:d1f0ae13f4a7 565 unsigned long steps;
mbedoguz 0:d1f0ae13f4a7 566 if (dmp_get_pedometer_step_count(&steps) == INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 567 {
mbedoguz 0:d1f0ae13f4a7 568 return steps;
mbedoguz 0:d1f0ae13f4a7 569 }
mbedoguz 0:d1f0ae13f4a7 570 return 0;
mbedoguz 0:d1f0ae13f4a7 571 }
mbedoguz 0:d1f0ae13f4a7 572
mbedoguz 0:d1f0ae13f4a7 573 inv_error_t MPU9250_DMP::dmpSetPedometerSteps(unsigned long steps)
mbedoguz 0:d1f0ae13f4a7 574 {
mbedoguz 0:d1f0ae13f4a7 575 return dmp_set_pedometer_step_count(steps);
mbedoguz 0:d1f0ae13f4a7 576 }
mbedoguz 0:d1f0ae13f4a7 577
mbedoguz 0:d1f0ae13f4a7 578 unsigned long MPU9250_DMP::dmpGetPedometerTime(void)
mbedoguz 0:d1f0ae13f4a7 579 {
mbedoguz 0:d1f0ae13f4a7 580 unsigned long walkTime;
mbedoguz 0:d1f0ae13f4a7 581 if (dmp_get_pedometer_walk_time(&walkTime) == INV_SUCCESS)
mbedoguz 0:d1f0ae13f4a7 582 {
mbedoguz 0:d1f0ae13f4a7 583 return walkTime;
mbedoguz 0:d1f0ae13f4a7 584 }
mbedoguz 0:d1f0ae13f4a7 585 return 0;
mbedoguz 0:d1f0ae13f4a7 586 }
mbedoguz 0:d1f0ae13f4a7 587
mbedoguz 0:d1f0ae13f4a7 588 inv_error_t MPU9250_DMP::dmpSetPedometerTime(unsigned long time)
mbedoguz 0:d1f0ae13f4a7 589 {
mbedoguz 0:d1f0ae13f4a7 590 return dmp_set_pedometer_walk_time(time);
mbedoguz 0:d1f0ae13f4a7 591 }
mbedoguz 0:d1f0ae13f4a7 592
mbedoguz 0:d1f0ae13f4a7 593 float MPU9250_DMP::calcAccel(int axis)
mbedoguz 0:d1f0ae13f4a7 594 {
mbedoguz 0:d1f0ae13f4a7 595 return (float) axis / (float) _aSense;
mbedoguz 0:d1f0ae13f4a7 596 }
mbedoguz 0:d1f0ae13f4a7 597
mbedoguz 0:d1f0ae13f4a7 598 float MPU9250_DMP::calcGyro(int axis)
mbedoguz 0:d1f0ae13f4a7 599 {
mbedoguz 0:d1f0ae13f4a7 600 return (float) axis / (float) _gSense;
mbedoguz 0:d1f0ae13f4a7 601 }
mbedoguz 0:d1f0ae13f4a7 602
mbedoguz 0:d1f0ae13f4a7 603 float MPU9250_DMP::calcMag(int axis)
mbedoguz 0:d1f0ae13f4a7 604 {
mbedoguz 0:d1f0ae13f4a7 605 return (float) axis / (float) _mSense;
mbedoguz 0:d1f0ae13f4a7 606 }
mbedoguz 0:d1f0ae13f4a7 607
mbedoguz 0:d1f0ae13f4a7 608 float MPU9250_DMP::calcQuat(long axis)
mbedoguz 0:d1f0ae13f4a7 609 {
mbedoguz 0:d1f0ae13f4a7 610 return qToFloat(axis, 30);
mbedoguz 0:d1f0ae13f4a7 611 }
mbedoguz 0:d1f0ae13f4a7 612
mbedoguz 0:d1f0ae13f4a7 613 float MPU9250_DMP::qToFloat(long number, unsigned char q)
mbedoguz 0:d1f0ae13f4a7 614 {
mbedoguz 0:d1f0ae13f4a7 615 unsigned long mask;
mbedoguz 0:d1f0ae13f4a7 616 for (int i=0; i<q; i++)
mbedoguz 0:d1f0ae13f4a7 617 {
mbedoguz 0:d1f0ae13f4a7 618 mask |= (1<<i);
mbedoguz 0:d1f0ae13f4a7 619 }
mbedoguz 0:d1f0ae13f4a7 620 return (number >> q) + ((number & mask) / (float) (2<<(q-1)));
mbedoguz 0:d1f0ae13f4a7 621 }
mbedoguz 0:d1f0ae13f4a7 622
mbedoguz 0:d1f0ae13f4a7 623 void MPU9250_DMP::computeEulerAngles(bool degrees)
mbedoguz 0:d1f0ae13f4a7 624 {
mbedoguz 0:d1f0ae13f4a7 625 double dqw = qToFloat(qw, 30);
mbedoguz 0:d1f0ae13f4a7 626 double dqx = qToFloat(qx, 30);
mbedoguz 0:d1f0ae13f4a7 627 double dqy = qToFloat(qy, 30);
mbedoguz 0:d1f0ae13f4a7 628 double dqz = qToFloat(qz, 30);
mbedoguz 0:d1f0ae13f4a7 629
mbedoguz 0:d1f0ae13f4a7 630 double ysqr = dqy * dqy;
mbedoguz 0:d1f0ae13f4a7 631 double t0 = -2.0f * (ysqr + dqz * dqz) + 1.0f;
mbedoguz 0:d1f0ae13f4a7 632 double t1 = +2.0f * (dqx * dqy - dqw * dqz);
mbedoguz 0:d1f0ae13f4a7 633 double t2 = -2.0f * (dqx * dqz + dqw * dqy);
mbedoguz 0:d1f0ae13f4a7 634 double t3 = +2.0f * (dqy * dqz - dqw * dqx);
mbedoguz 0:d1f0ae13f4a7 635 double t4 = -2.0f * (dqx * dqx + ysqr) + 1.0f;
mbedoguz 0:d1f0ae13f4a7 636
mbedoguz 0:d1f0ae13f4a7 637 // Keep t2 within range of asin (-1, 1)
mbedoguz 0:d1f0ae13f4a7 638 t2 = t2 > 1.0f ? 1.0f : t2;
mbedoguz 0:d1f0ae13f4a7 639 t2 = t2 < -1.0f ? -1.0f : t2;
mbedoguz 0:d1f0ae13f4a7 640
mbedoguz 0:d1f0ae13f4a7 641 pitch = asin(t2) * 2;
mbedoguz 0:d1f0ae13f4a7 642 roll = atan2(t3, t4);
mbedoguz 0:d1f0ae13f4a7 643 yaw = atan2(t1, t0);
mbedoguz 0:d1f0ae13f4a7 644
mbedoguz 0:d1f0ae13f4a7 645 if (degrees)
mbedoguz 0:d1f0ae13f4a7 646 {
mbedoguz 0:d1f0ae13f4a7 647 pitch *= (180.0 / PI);
mbedoguz 0:d1f0ae13f4a7 648 roll *= (180.0 / PI);
mbedoguz 0:d1f0ae13f4a7 649 yaw *= (180.0 / PI);
mbedoguz 0:d1f0ae13f4a7 650 if (pitch < 0) pitch = 360.0 + pitch;
mbedoguz 0:d1f0ae13f4a7 651 if (roll < 0) roll = 360.0 + roll;
mbedoguz 0:d1f0ae13f4a7 652 if (yaw < 0) yaw = 360.0 + yaw;
mbedoguz 0:d1f0ae13f4a7 653 }
mbedoguz 0:d1f0ae13f4a7 654 }
mbedoguz 0:d1f0ae13f4a7 655
mbedoguz 0:d1f0ae13f4a7 656 float MPU9250_DMP::computeCompassHeading(void)
mbedoguz 0:d1f0ae13f4a7 657 {
mbedoguz 0:d1f0ae13f4a7 658 if (my == 0)
mbedoguz 0:d1f0ae13f4a7 659 heading = (mx < 0) ? 180.0 : 0;
mbedoguz 0:d1f0ae13f4a7 660 else
mbedoguz 0:d1f0ae13f4a7 661 heading = atan2((double)mx, (double)my);
mbedoguz 0:d1f0ae13f4a7 662
mbedoguz 0:d1f0ae13f4a7 663 if (heading > PI) heading -= (2 * PI);
mbedoguz 0:d1f0ae13f4a7 664 else if (heading < -PI) heading += (2 * PI);
mbedoguz 0:d1f0ae13f4a7 665 else if (heading < 0) heading += 2 * PI;
mbedoguz 0:d1f0ae13f4a7 666
mbedoguz 0:d1f0ae13f4a7 667 heading*= 180.0 / PI;
mbedoguz 0:d1f0ae13f4a7 668
mbedoguz 0:d1f0ae13f4a7 669 return heading;
mbedoguz 0:d1f0ae13f4a7 670 }
mbedoguz 0:d1f0ae13f4a7 671
mbedoguz 0:d1f0ae13f4a7 672 unsigned short MPU9250_DMP::orientation_row_2_scale(const signed char *row)
mbedoguz 0:d1f0ae13f4a7 673 {
mbedoguz 0:d1f0ae13f4a7 674 unsigned short b;
mbedoguz 0:d1f0ae13f4a7 675
mbedoguz 0:d1f0ae13f4a7 676 if (row[0] > 0)
mbedoguz 0:d1f0ae13f4a7 677 b = 0;
mbedoguz 0:d1f0ae13f4a7 678 else if (row[0] < 0)
mbedoguz 0:d1f0ae13f4a7 679 b = 4;
mbedoguz 0:d1f0ae13f4a7 680 else if (row[1] > 0)
mbedoguz 0:d1f0ae13f4a7 681 b = 1;
mbedoguz 0:d1f0ae13f4a7 682 else if (row[1] < 0)
mbedoguz 0:d1f0ae13f4a7 683 b = 5;
mbedoguz 0:d1f0ae13f4a7 684 else if (row[2] > 0)
mbedoguz 0:d1f0ae13f4a7 685 b = 2;
mbedoguz 0:d1f0ae13f4a7 686 else if (row[2] < 0)
mbedoguz 0:d1f0ae13f4a7 687 b = 6;
mbedoguz 0:d1f0ae13f4a7 688 else
mbedoguz 0:d1f0ae13f4a7 689 b = 7; // error
mbedoguz 0:d1f0ae13f4a7 690 return b;
mbedoguz 0:d1f0ae13f4a7 691 }
mbedoguz 0:d1f0ae13f4a7 692
mbedoguz 0:d1f0ae13f4a7 693 static void tap_cb(unsigned char direction, unsigned char count)
mbedoguz 0:d1f0ae13f4a7 694 {
mbedoguz 0:d1f0ae13f4a7 695 _tap_available = true;
mbedoguz 0:d1f0ae13f4a7 696 tap_count = count;
mbedoguz 0:d1f0ae13f4a7 697 tap_direction = direction;
mbedoguz 0:d1f0ae13f4a7 698 }
mbedoguz 0:d1f0ae13f4a7 699
mbedoguz 0:d1f0ae13f4a7 700 static void orient_cb(unsigned char orient)
mbedoguz 0:d1f0ae13f4a7 701 {
mbedoguz 0:d1f0ae13f4a7 702 mpu9250_orientation = orient;
mbedoguz 0:d1f0ae13f4a7 703 }