Mark Vandermeulen / FYDP_Final

Code for our FYDP -only one IMU works right now -RTOS is working

Dependencies:   mbed

drivers/MPU6050-DMP-Ian/Math/helper_3dmath.h@0:964eb6a2ef00, 2015-03-18 (annotated)

Committer:
majik
Date:
Wed Mar 18 22:23:48 2015 +0000
Revision:
0:964eb6a2ef00
This is our FYDP code, but only one IMU works with the RTOS.

Who changed what in which revision?

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