ABELI / Mbed 2 deprecated Beschleunigungssensor

Werte auslesen und in Winkel umrechnen

Dependencies:   mbed

helper_3dmath.h@0:d1960beb98fe, 2015-09-30 (annotated)

Committer:
Heidl
Date:
Wed Sep 30 16:08:36 2015 +0000
Revision:
0:d1960beb98fe
Werte von Beschleunigungssensor auslesen

Who changed what in which revision?

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