Ian Hua / MPU6050-DMP-Ian

Rearranged original code port/fork to: * Make library compatible with TiltyQuad IMU; * Prevent multiple definition, and added inclusion guard; * Cleaner access to library functions and file structure; and * "Broke out" code to control Sampling Rate and FIFO buffer update rate. By Trung Tin Ian HUA 2014. Credit to Jeff Rowberg for his original code, the best DMP implementation thus far; and szymon gaertig for porting the arduino library to mbed.

Dependents:   MPU6050-DMP_test

Fork of MPU6050 by Shundo Kishi

Math/helper_3dmath.h@14:86afc8447df9, 2014-05-14 (annotated)

Committer:
pHysiX
Date:
Wed May 14 12:41:19 2014 +0000
Revision:
14:86afc8447df9
Parent:
6:2dc23167c8d8 
Moved configuration to file named "config.h"

Who changed what in which revision?

UserRevisionLine numberNew contents of line
garfieldsg 0:662207e34fba 1 // I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class, 3D math helper
garfieldsg 0:662207e34fba 2 // 6/5/2012 by Jeff Rowberg <jeff@rowberg.net>
garfieldsg 0:662207e34fba 3 // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
garfieldsg 0:662207e34fba 4 //
garfieldsg 0:662207e34fba 5 // Changelog:
garfieldsg 0:662207e34fba 6 // 2012-06-05 - add 3D math helper file to DMP6 example sketch
garfieldsg 0:662207e34fba 7
garfieldsg 0:662207e34fba 8 /* ============================================
garfieldsg 0:662207e34fba 9 I2Cdev device library code is placed under the MIT license
garfieldsg 0:662207e34fba 10 Copyright (c) 2012 Jeff Rowberg
garfieldsg 0:662207e34fba 11
garfieldsg 0:662207e34fba 12 Permission is hereby granted, free of charge, to any person obtaining a copy
garfieldsg 0:662207e34fba 13 of this software and associated documentation files (the "Software"), to deal
garfieldsg 0:662207e34fba 14 in the Software without restriction, including without limitation the rights
garfieldsg 0:662207e34fba 15 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
garfieldsg 0:662207e34fba 16 copies of the Software, and to permit persons to whom the Software is
garfieldsg 0:662207e34fba 17 furnished to do so, subject to the following conditions:
garfieldsg 0:662207e34fba 18
garfieldsg 0:662207e34fba 19 The above copyright notice and this permission notice shall be included in
garfieldsg 0:662207e34fba 20 all copies or substantial portions of the Software.
garfieldsg 0:662207e34fba 21
garfieldsg 0:662207e34fba 22 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
garfieldsg 0:662207e34fba 23 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
garfieldsg 0:662207e34fba 24 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
garfieldsg 0:662207e34fba 25 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
garfieldsg 0:662207e34fba 26 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
garfieldsg 0:662207e34fba 27 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
garfieldsg 0:662207e34fba 28 THE SOFTWARE.
garfieldsg 0:662207e34fba 29 ===============================================
garfieldsg 0:662207e34fba 30 */
garfieldsg 0:662207e34fba 31
garfieldsg 0:662207e34fba 32 #ifndef _HELPER_3DMATH_H_
garfieldsg 0:662207e34fba 33 #define _HELPER_3DMATH_H_
garfieldsg 0:662207e34fba 34
garfieldsg 0:662207e34fba 35 class Quaternion {
garfieldsg 0:662207e34fba 36 public:
garfieldsg 0:662207e34fba 37 float w;
garfieldsg 0:662207e34fba 38 float x;
garfieldsg 0:662207e34fba 39 float y;
garfieldsg 0:662207e34fba 40 float z;
garfieldsg 0:662207e34fba 41
garfieldsg 0:662207e34fba 42 Quaternion() {
garfieldsg 0:662207e34fba 43 w = 1.0f;
garfieldsg 0:662207e34fba 44 x = 0.0f;
garfieldsg 0:662207e34fba 45 y = 0.0f;
garfieldsg 0:662207e34fba 46 z = 0.0f;
garfieldsg 0:662207e34fba 47 }
garfieldsg 0:662207e34fba 48
garfieldsg 0:662207e34fba 49 Quaternion(float nw, float nx, float ny, float nz) {
garfieldsg 0:662207e34fba 50 w = nw;
garfieldsg 0:662207e34fba 51 x = nx;
garfieldsg 0:662207e34fba 52 y = ny;
garfieldsg 0:662207e34fba 53 z = nz;
garfieldsg 0:662207e34fba 54 }
garfieldsg 0:662207e34fba 55
garfieldsg 0:662207e34fba 56 Quaternion getProduct(Quaternion q) {
garfieldsg 0:662207e34fba 57 // Quaternion multiplication is defined by:
garfieldsg 0:662207e34fba 58 // (Q1 * Q2).w = (w1w2 - x1x2 - y1y2 - z1z2)
garfieldsg 0:662207e34fba 59 // (Q1 * Q2).x = (w1x2 + x1w2 + y1z2 - z1y2)
garfieldsg 0:662207e34fba 60 // (Q1 * Q2).y = (w1y2 - x1z2 + y1w2 + z1x2)
garfieldsg 0:662207e34fba 61 // (Q1 * Q2).z = (w1z2 + x1y2 - y1x2 + z1w2
garfieldsg 0:662207e34fba 62 return Quaternion(
garfieldsg 0:662207e34fba 63 w*q.w - x*q.x - y*q.y - z*q.z, // new w
garfieldsg 0:662207e34fba 64 w*q.x + x*q.w + y*q.z - z*q.y, // new x
garfieldsg 0:662207e34fba 65 w*q.y - x*q.z + y*q.w + z*q.x, // new y
garfieldsg 0:662207e34fba 66 w*q.z + x*q.y - y*q.x + z*q.w); // new z
garfieldsg 0:662207e34fba 67 }
garfieldsg 0:662207e34fba 68
garfieldsg 0:662207e34fba 69 Quaternion getConjugate() {
garfieldsg 0:662207e34fba 70 return Quaternion(w, -x, -y, -z);
garfieldsg 0:662207e34fba 71 }
garfieldsg 0:662207e34fba 72
garfieldsg 0:662207e34fba 73 float getMagnitude() {
garfieldsg 0:662207e34fba 74 return sqrt(w*w + x*x + y*y + z*z);
garfieldsg 0:662207e34fba 75 }
garfieldsg 0:662207e34fba 76
garfieldsg 0:662207e34fba 77 void normalize() {
garfieldsg 0:662207e34fba 78 float m = getMagnitude();
garfieldsg 0:662207e34fba 79 w /= m;
garfieldsg 0:662207e34fba 80 x /= m;
garfieldsg 0:662207e34fba 81 y /= m;
garfieldsg 0:662207e34fba 82 z /= m;
garfieldsg 0:662207e34fba 83 }
garfieldsg 0:662207e34fba 84
garfieldsg 0:662207e34fba 85 Quaternion getNormalized() {
garfieldsg 0:662207e34fba 86 Quaternion r(w, x, y, z);
garfieldsg 0:662207e34fba 87 r.normalize();
garfieldsg 0:662207e34fba 88 return r;
garfieldsg 0:662207e34fba 89 }
garfieldsg 0:662207e34fba 90 };
garfieldsg 0:662207e34fba 91
garfieldsg 0:662207e34fba 92 class VectorInt16 {
garfieldsg 0:662207e34fba 93 public:
garfieldsg 0:662207e34fba 94 int16_t x;
garfieldsg 0:662207e34fba 95 int16_t y;
garfieldsg 0:662207e34fba 96 int16_t z;
garfieldsg 0:662207e34fba 97
garfieldsg 0:662207e34fba 98 VectorInt16() {
garfieldsg 0:662207e34fba 99 x = 0;
garfieldsg 0:662207e34fba 100 y = 0;
garfieldsg 0:662207e34fba 101 z = 0;
garfieldsg 0:662207e34fba 102 }
garfieldsg 0:662207e34fba 103
garfieldsg 0:662207e34fba 104 VectorInt16(int16_t nx, int16_t ny, int16_t nz) {
garfieldsg 0:662207e34fba 105 x = nx;
garfieldsg 0:662207e34fba 106 y = ny;
garfieldsg 0:662207e34fba 107 z = nz;
garfieldsg 0:662207e34fba 108 }
garfieldsg 0:662207e34fba 109
garfieldsg 0:662207e34fba 110 float getMagnitude() {
garfieldsg 0:662207e34fba 111 return sqrt((float)(x*x + y*y + z*z));
garfieldsg 0:662207e34fba 112 }
garfieldsg 0:662207e34fba 113
garfieldsg 0:662207e34fba 114 void normalize() {
garfieldsg 0:662207e34fba 115 float m = getMagnitude();
garfieldsg 0:662207e34fba 116 x /= m;
garfieldsg 0:662207e34fba 117 y /= m;
garfieldsg 0:662207e34fba 118 z /= m;
garfieldsg 0:662207e34fba 119 }
garfieldsg 0:662207e34fba 120
garfieldsg 0:662207e34fba 121 VectorInt16 getNormalized() {
garfieldsg 0:662207e34fba 122 VectorInt16 r(x, y, z);
garfieldsg 0:662207e34fba 123 r.normalize();
garfieldsg 0:662207e34fba 124 return r;
garfieldsg 0:662207e34fba 125 }
garfieldsg 0:662207e34fba 126
garfieldsg 0:662207e34fba 127 void rotate(Quaternion *q) {
garfieldsg 0:662207e34fba 128 // http://www.cprogramming.com/tutorial/3d/quaternions.html
garfieldsg 0:662207e34fba 129 // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm
garfieldsg 0:662207e34fba 130 // http://content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation
garfieldsg 0:662207e34fba 131 // ^ or: http://webcache.googleusercontent.com/search?q=cache:xgJAp3bDNhQJ:content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation&hl=en&gl=us&strip=1
garfieldsg 0:662207e34fba 132
garfieldsg 0:662207e34fba 133 // P_out = q * P_in * conj(q)
garfieldsg 0:662207e34fba 134 // - P_out is the output vector
garfieldsg 0:662207e34fba 135 // - q is the orientation quaternion
garfieldsg 0:662207e34fba 136 // - P_in is the input vector (a*aReal)
garfieldsg 0:662207e34fba 137 // - conj(q) is the conjugate of the orientation quaternion (q=[w,x,y,z], q*=[w,-x,-y,-z])
garfieldsg 0:662207e34fba 138 Quaternion p(0, x, y, z);
garfieldsg 0:662207e34fba 139
garfieldsg 0:662207e34fba 140 // quaternion multiplication: q * p, stored back in p
garfieldsg 0:662207e34fba 141 p = q -> getProduct(p);
garfieldsg 0:662207e34fba 142
garfieldsg 0:662207e34fba 143 // quaternion multiplication: p * conj(q), stored back in p
garfieldsg 0:662207e34fba 144 p = p.getProduct(q -> getConjugate());
garfieldsg 0:662207e34fba 145
garfieldsg 0:662207e34fba 146 // p quaternion is now [0, x', y', z']
garfieldsg 0:662207e34fba 147 x = p.x;
garfieldsg 0:662207e34fba 148 y = p.y;
garfieldsg 0:662207e34fba 149 z = p.z;
garfieldsg 0:662207e34fba 150 }
garfieldsg 0:662207e34fba 151
garfieldsg 0:662207e34fba 152 VectorInt16 getRotated(Quaternion *q) {
garfieldsg 0:662207e34fba 153 VectorInt16 r(x, y, z);
garfieldsg 0:662207e34fba 154 r.rotate(q);
garfieldsg 0:662207e34fba 155 return r;
garfieldsg 0:662207e34fba 156 }
garfieldsg 0:662207e34fba 157 };
garfieldsg 0:662207e34fba 158
garfieldsg 0:662207e34fba 159 class VectorFloat {
garfieldsg 0:662207e34fba 160 public:
garfieldsg 0:662207e34fba 161 float x;
garfieldsg 0:662207e34fba 162 float y;
garfieldsg 0:662207e34fba 163 float z;
garfieldsg 0:662207e34fba 164
garfieldsg 0:662207e34fba 165 VectorFloat() {
garfieldsg 0:662207e34fba 166 x = 0;
garfieldsg 0:662207e34fba 167 y = 0;
garfieldsg 0:662207e34fba 168 z = 0;
garfieldsg 0:662207e34fba 169 }
garfieldsg 0:662207e34fba 170
garfieldsg 0:662207e34fba 171 VectorFloat(float nx, float ny, float nz) {
garfieldsg 0:662207e34fba 172 x = nx;
garfieldsg 0:662207e34fba 173 y = ny;
garfieldsg 0:662207e34fba 174 z = nz;
garfieldsg 0:662207e34fba 175 }
garfieldsg 0:662207e34fba 176
garfieldsg 0:662207e34fba 177 float getMagnitude() {
garfieldsg 0:662207e34fba 178 return sqrt(x*x + y*y + z*z);
garfieldsg 0:662207e34fba 179 }
garfieldsg 0:662207e34fba 180
garfieldsg 0:662207e34fba 181 void normalize() {
garfieldsg 0:662207e34fba 182 float m = getMagnitude();
garfieldsg 0:662207e34fba 183 x /= m;
garfieldsg 0:662207e34fba 184 y /= m;
garfieldsg 0:662207e34fba 185 z /= m;
garfieldsg 0:662207e34fba 186 }
garfieldsg 0:662207e34fba 187
garfieldsg 0:662207e34fba 188 VectorFloat getNormalized() {
garfieldsg 0:662207e34fba 189 VectorFloat r(x, y, z);
garfieldsg 0:662207e34fba 190 r.normalize();
garfieldsg 0:662207e34fba 191 return r;
garfieldsg 0:662207e34fba 192 }
garfieldsg 0:662207e34fba 193
garfieldsg 0:662207e34fba 194 void rotate(Quaternion *q) {
garfieldsg 0:662207e34fba 195 Quaternion p(0, x, y, z);
garfieldsg 0:662207e34fba 196
garfieldsg 0:662207e34fba 197 // quaternion multiplication: q * p, stored back in p
garfieldsg 0:662207e34fba 198 p = q -> getProduct(p);
garfieldsg 0:662207e34fba 199
garfieldsg 0:662207e34fba 200 // quaternion multiplication: p * conj(q), stored back in p
garfieldsg 0:662207e34fba 201 p = p.getProduct(q -> getConjugate());
garfieldsg 0:662207e34fba 202
garfieldsg 0:662207e34fba 203 // p quaternion is now [0, x', y', z']
garfieldsg 0:662207e34fba 204 x = p.x;
garfieldsg 0:662207e34fba 205 y = p.y;
garfieldsg 0:662207e34fba 206 z = p.z;
garfieldsg 0:662207e34fba 207 }
garfieldsg 0:662207e34fba 208
garfieldsg 0:662207e34fba 209 VectorFloat getRotated(Quaternion *q) {
garfieldsg 0:662207e34fba 210 VectorFloat r(x, y, z);
garfieldsg 0:662207e34fba 211 r.rotate(q);
garfieldsg 0:662207e34fba 212 return r;
garfieldsg 0:662207e34fba 213 }
garfieldsg 0:662207e34fba 214 };
garfieldsg 0:662207e34fba 215
garfieldsg 0:662207e34fba 216 #endif /* _HELPER_3DMATH_H_ */