Amr El-Gendy / Mbed 2 deprecated QUAD_PROJECT

a quadcopter code

Dependencies:   Pulse RangeFinder mbed

helper_3dmath.h@0:4a55d0a21ea9, 2015-11-24 (annotated)

Committer:
Gendy
Date:
Tue Nov 24 19:57:06 2015 +0000
Revision:
0:4a55d0a21ea9
Quad_project PID on angle;

Who changed what in which revision?

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