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