quadcopter cufe / Mbed 2 deprecated MPU_6050_nadafa

mpu6050 nadafa

Dependencies:   mbed

helper_3dmath.h@0:8fe8d6dd7cd6, 2015-06-23 (annotated)

Committer:
khaledelmadawi
Date:
Tue Jun 23 14:47:52 2015 +0000
Revision:
0:8fe8d6dd7cd6
mpu nadafa

Who changed what in which revision?

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