Alan Huchin Herrera / Mbed 2 deprecated Nucleo_MPU_9250_

Quadrirotor

Dependencies:   CommonTypes ESC Matrix PID Servo kalman mbed-rtos mbed

Fork of Nucleo_MPU_9250 by Alan Huchin Herrera

CID10DOF/helper_3dmath.h@0:89cf0851969b, 2018-06-26 (annotated)

Committer:
AlanHuchin
Date:
Tue Jun 26 18:24:45 2018 +0000
Revision:
0:89cf0851969b
hello

Who changed what in which revision?

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