Gurvan PRIEM / Mbed 2 deprecated RaptorControl

An incomplete quadcopter control programme.

Dependencies:   mbed

MPU6050/helper_3dmath.h@0:9cb9445a11f0, 2013-07-17 (annotated)

Committer:
Gurvan
Date:
Wed Jul 17 15:58:25 2013 +0000
Revision:
0:9cb9445a11f0
Pour Zobson, fi(r)st commit.

Who changed what in which revision?

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