Zbigniew Druzbacki / Mbed 2 deprecated SPI_Slave

SPI slave program to enable communication between the FPGA and the STM32L432 board.

Dependencies:   mbed

Quaternions.cpp@4:e36c7042d3bb, 2019-02-23 (annotated)

Committer:
Zbyszek
Date:
Sat Feb 23 01:20:08 2019 +0000
Revision:
4:e36c7042d3bb
Child:
5:155d224d855c
-Added quaternion integration; -added accel based angle calculations

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Zbyszek 4:e36c7042d3bb 1 #include "Quaternions.h"
Zbyszek 4:e36c7042d3bb 2 #include <math.h>
Zbyszek 4:e36c7042d3bb 3
Zbyszek 4:e36c7042d3bb 4
Zbyszek 4:e36c7042d3bb 5 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 6 /*
Zbyszek 4:e36c7042d3bb 7 *Takes the angular rate in roll, pitch, yaw form and converts it into quaternion notation
Zbyszek 4:e36c7042d3bb 8 */
Zbyszek 4:e36c7042d3bb 9 Quaternion toQuaternionNotation(vector AngularRate, double dt) {
Zbyszek 4:e36c7042d3bb 10 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 11 Quaternion q;
Zbyszek 4:e36c7042d3bb 12 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 13
Zbyszek 4:e36c7042d3bb 14
Zbyszek 4:e36c7042d3bb 15 double l = sqrt(pow(AngularRate.x, 2) + pow(AngularRate.y, 2) + pow(AngularRate.z, 2)); //Normalise the quaternion such that it a unit quaternion between 0 and 1.
Zbyszek 4:e36c7042d3bb 16 double theta = (dt * l) / 2; //Get Theta in quaternions
Zbyszek 4:e36c7042d3bb 17
Zbyszek 4:e36c7042d3bb 18 //double thetaRad = theta * 0.01745; // 0.01745 = pi/180
Zbyszek 4:e36c7042d3bb 19 //Convert each component of rotation
Zbyszek 4:e36c7042d3bb 20 q.w = cosf(theta); //* Magnitude
Zbyszek 4:e36c7042d3bb 21 q.x = sinf(theta) * (AngularRate.x / l); //*
Zbyszek 4:e36c7042d3bb 22 q.y = sinf(theta) * (AngularRate.y / l); //*
Zbyszek 4:e36c7042d3bb 23 q.z = sinf(theta) * (AngularRate.z / l); //*
Zbyszek 4:e36c7042d3bb 24 //Convert each component of rotation
Zbyszek 4:e36c7042d3bb 25 return q;
Zbyszek 4:e36c7042d3bb 26 }
Zbyszek 4:e36c7042d3bb 27 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 28
Zbyszek 4:e36c7042d3bb 29
Zbyszek 4:e36c7042d3bb 30 Quaternion toQuaternionNotation123(vector AngularRate, double dt) {
Zbyszek 4:e36c7042d3bb 31 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 32 Quaternion Q;
Zbyszek 4:e36c7042d3bb 33 float Sin_Mag;
Zbyszek 4:e36c7042d3bb 34 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 35
Zbyszek 4:e36c7042d3bb 36 Q.x = AngularRate.x * dt;
Zbyszek 4:e36c7042d3bb 37 Q.y = AngularRate.y * dt;
Zbyszek 4:e36c7042d3bb 38 Q.z = AngularRate.z * dt;
Zbyszek 4:e36c7042d3bb 39 Q.w = 1.0f - 0.5f * (Q.x * Q.x +Q.y * Q.y + Q.z * Q.z);
Zbyszek 4:e36c7042d3bb 40
Zbyszek 4:e36c7042d3bb 41
Zbyszek 4:e36c7042d3bb 42
Zbyszek 4:e36c7042d3bb 43 double l = sqrt(pow(AngularRate.x, 2) + pow(AngularRate.y, 2) + pow(AngularRate.z, 2)); //Normalise the quaternion such that it a unit quaternion between 0 and 1.
Zbyszek 4:e36c7042d3bb 44 double theta = l / 2; //Get Theta in quaternions
Zbyszek 4:e36c7042d3bb 45
Zbyszek 4:e36c7042d3bb 46 //double thetaRad = theta * 0.01745; // 0.01745 = pi/180
Zbyszek 4:e36c7042d3bb 47
Zbyszek 4:e36c7042d3bb 48 Sin_Mag = sin(theta) / l;
Zbyszek 4:e36c7042d3bb 49 //Convert each component of rotation
Zbyszek 4:e36c7042d3bb 50 Q.x = AngularRate.x * Sin_Mag;
Zbyszek 4:e36c7042d3bb 51 Q.y = AngularRate.y * Sin_Mag;
Zbyszek 4:e36c7042d3bb 52 Q.z = AngularRate.z * Sin_Mag;
Zbyszek 4:e36c7042d3bb 53 Q.w = cos(theta);
Zbyszek 4:e36c7042d3bb 54
Zbyszek 4:e36c7042d3bb 55 return Q;
Zbyszek 4:e36c7042d3bb 56 }
Zbyszek 4:e36c7042d3bb 57 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 58
Zbyszek 4:e36c7042d3bb 59
Zbyszek 4:e36c7042d3bb 60
Zbyszek 4:e36c7042d3bb 61
Zbyszek 4:e36c7042d3bb 62
Zbyszek 4:e36c7042d3bb 63
Zbyszek 4:e36c7042d3bb 64 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 65 /*
Zbyszek 4:e36c7042d3bb 66 *This function updates the old orientation by rotating the reference frame by
Zbyszek 4:e36c7042d3bb 67 *the change in angle. All this is done in quaternion form.
Zbyszek 4:e36c7042d3bb 68 */
Zbyszek 4:e36c7042d3bb 69 Quaternion getQuaternionProduct(Quaternion Q1, Quaternion Q2) {
Zbyszek 4:e36c7042d3bb 70 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 71 Quaternion currentQ;
Zbyszek 4:e36c7042d3bb 72 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 73
Zbyszek 4:e36c7042d3bb 74
Zbyszek 4:e36c7042d3bb 75 // Piecewise matrix multiplication
Zbyszek 4:e36c7042d3bb 76 currentQ.w = (Q1.w*Q2.w - Q1.x*Q2.x - Q1.y*Q2.y - Q1.z*Q2.z);
Zbyszek 4:e36c7042d3bb 77 currentQ.x = (Q1.w*Q2.x + Q1.x*Q2.w + Q1.y*Q2.z - Q1.z*Q2.y);
Zbyszek 4:e36c7042d3bb 78 currentQ.y = (Q1.w*Q2.y - Q1.x*Q2.z + Q1.y*Q2.w + Q1.z*Q2.x);
Zbyszek 4:e36c7042d3bb 79 currentQ.z = (Q1.w*Q2.z + Q1.x*Q2.y - Q1.y*Q2.x + Q1.z*Q2.w);
Zbyszek 4:e36c7042d3bb 80
Zbyszek 4:e36c7042d3bb 81 return currentQ;
Zbyszek 4:e36c7042d3bb 82
Zbyszek 4:e36c7042d3bb 83 }
Zbyszek 4:e36c7042d3bb 84 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 85
Zbyszek 4:e36c7042d3bb 86
Zbyszek 4:e36c7042d3bb 87
Zbyszek 4:e36c7042d3bb 88 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 89 /*
Zbyszek 4:e36c7042d3bb 90 *This function updates the old orientation by rotating the reference frame by
Zbyszek 4:e36c7042d3bb 91 *the change in angle. All this is done in quaternion form.
Zbyszek 4:e36c7042d3bb 92 */
Zbyszek 4:e36c7042d3bb 93 Quaternion getQVproduct(Quaternion Q, vector v) {
Zbyszek 4:e36c7042d3bb 94 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 95 Quaternion currentQ;
Zbyszek 4:e36c7042d3bb 96 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 97
Zbyszek 4:e36c7042d3bb 98
Zbyszek 4:e36c7042d3bb 99 // Piecewise matrix multiplication
Zbyszek 4:e36c7042d3bb 100 currentQ.x = (Q.w*v.x + Q.z*v.y - Q.y*v.z);
Zbyszek 4:e36c7042d3bb 101 currentQ.y = (Q.w*v.y + Q.x*v.z - Q.z*v.x);
Zbyszek 4:e36c7042d3bb 102 currentQ.z = (Q.y*v.x - Q.x*v.y + Q.w*v.z);
Zbyszek 4:e36c7042d3bb 103 currentQ.w = (-Q.x*v.x - Q.y*v.y - Q.z*v.z);
Zbyszek 4:e36c7042d3bb 104
Zbyszek 4:e36c7042d3bb 105 return currentQ;
Zbyszek 4:e36c7042d3bb 106
Zbyszek 4:e36c7042d3bb 107 }
Zbyszek 4:e36c7042d3bb 108 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 109
Zbyszek 4:e36c7042d3bb 110
Zbyszek 4:e36c7042d3bb 111
Zbyszek 4:e36c7042d3bb 112
Zbyszek 4:e36c7042d3bb 113
Zbyszek 4:e36c7042d3bb 114
Zbyszek 4:e36c7042d3bb 115
Zbyszek 4:e36c7042d3bb 116 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 117 Quaternion quaternionConjugate(Quaternion Q) {
Zbyszek 4:e36c7042d3bb 118 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 119 Quaternion localQ;
Zbyszek 4:e36c7042d3bb 120 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 121
Zbyszek 4:e36c7042d3bb 122 localQ.w = Q.w;
Zbyszek 4:e36c7042d3bb 123 localQ.x = -Q.x;
Zbyszek 4:e36c7042d3bb 124 localQ.y = -Q.y;
Zbyszek 4:e36c7042d3bb 125 localQ.z = -Q.z;
Zbyszek 4:e36c7042d3bb 126
Zbyszek 4:e36c7042d3bb 127 return localQ;
Zbyszek 4:e36c7042d3bb 128
Zbyszek 4:e36c7042d3bb 129 }
Zbyszek 4:e36c7042d3bb 130 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 131
Zbyszek 4:e36c7042d3bb 132 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 133 Quaternion sumQuaternion(Quaternion Q1, Quaternion Q2) {
Zbyszek 4:e36c7042d3bb 134 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 135 Quaternion localQ;
Zbyszek 4:e36c7042d3bb 136 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 137
Zbyszek 4:e36c7042d3bb 138 localQ.w = Q1.w + Q2.w;
Zbyszek 4:e36c7042d3bb 139 localQ.x = Q1.x + Q2.x;
Zbyszek 4:e36c7042d3bb 140 localQ.y = Q1.y + Q2.y;
Zbyszek 4:e36c7042d3bb 141 localQ.z = Q1.z + Q2.z;
Zbyszek 4:e36c7042d3bb 142
Zbyszek 4:e36c7042d3bb 143 return localQ;
Zbyszek 4:e36c7042d3bb 144
Zbyszek 4:e36c7042d3bb 145 }
Zbyszek 4:e36c7042d3bb 146 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 147
Zbyszek 4:e36c7042d3bb 148
Zbyszek 4:e36c7042d3bb 149 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 150 vector sumVector(vector v1, vector v2) {
Zbyszek 4:e36c7042d3bb 151 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 152 vector localv;
Zbyszek 4:e36c7042d3bb 153 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 154
Zbyszek 4:e36c7042d3bb 155 localv.x = v1.x + v2.x;
Zbyszek 4:e36c7042d3bb 156 localv.y = v1.y + v2.y;
Zbyszek 4:e36c7042d3bb 157 localv.z = v1.z + v2.z;
Zbyszek 4:e36c7042d3bb 158
Zbyszek 4:e36c7042d3bb 159 return localv;
Zbyszek 4:e36c7042d3bb 160
Zbyszek 4:e36c7042d3bb 161 }
Zbyszek 4:e36c7042d3bb 162 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 163
Zbyszek 4:e36c7042d3bb 164
Zbyszek 4:e36c7042d3bb 165
Zbyszek 4:e36c7042d3bb 166
Zbyszek 4:e36c7042d3bb 167
Zbyszek 4:e36c7042d3bb 168
Zbyszek 4:e36c7042d3bb 169
Zbyszek 4:e36c7042d3bb 170 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 171 vector rotateVector(Quaternion Q, vector v) {
Zbyszek 4:e36c7042d3bb 172 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 173 Quaternion vectorQ;
Zbyszek 4:e36c7042d3bb 174 Quaternion Qconjugate;
Zbyszek 4:e36c7042d3bb 175 Quaternion rotationQ;
Zbyszek 4:e36c7042d3bb 176 vector localv;
Zbyszek 4:e36c7042d3bb 177 vector rotatedVector;
Zbyszek 4:e36c7042d3bb 178 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 179
Zbyszek 4:e36c7042d3bb 180 localv = v;
Zbyszek 4:e36c7042d3bb 181 vectorQ = vector2Quaternion(localv);
Zbyszek 4:e36c7042d3bb 182 Qconjugate = quaternionConjugate(Q);
Zbyszek 4:e36c7042d3bb 183
Zbyszek 4:e36c7042d3bb 184 //q*p*q*. Our point is our rotation axis therefore q*v*q*
Zbyszek 4:e36c7042d3bb 185 rotationQ = getQuaternionProduct(Q, vectorQ);
Zbyszek 4:e36c7042d3bb 186 rotationQ = getQuaternionProduct(rotationQ, Qconjugate);
Zbyszek 4:e36c7042d3bb 187 rotatedVector = quaternion2Vector(rotationQ);
Zbyszek 4:e36c7042d3bb 188
Zbyszek 4:e36c7042d3bb 189 return rotatedVector;
Zbyszek 4:e36c7042d3bb 190
Zbyszek 4:e36c7042d3bb 191 }
Zbyszek 4:e36c7042d3bb 192 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 193
Zbyszek 4:e36c7042d3bb 194
Zbyszek 4:e36c7042d3bb 195
Zbyszek 4:e36c7042d3bb 196
Zbyszek 4:e36c7042d3bb 197
Zbyszek 4:e36c7042d3bb 198
Zbyszek 4:e36c7042d3bb 199
Zbyszek 4:e36c7042d3bb 200
Zbyszek 4:e36c7042d3bb 201 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 202 Quaternion vector2Quaternion(vector v) {
Zbyszek 4:e36c7042d3bb 203 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 204 Quaternion localQ;
Zbyszek 4:e36c7042d3bb 205 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 206
Zbyszek 4:e36c7042d3bb 207 localQ.w = 0;
Zbyszek 4:e36c7042d3bb 208 localQ.x = v.x;
Zbyszek 4:e36c7042d3bb 209 localQ.y = v.y;
Zbyszek 4:e36c7042d3bb 210 localQ.z = v.z;
Zbyszek 4:e36c7042d3bb 211
Zbyszek 4:e36c7042d3bb 212 return localQ;
Zbyszek 4:e36c7042d3bb 213 }
Zbyszek 4:e36c7042d3bb 214 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 215
Zbyszek 4:e36c7042d3bb 216
Zbyszek 4:e36c7042d3bb 217
Zbyszek 4:e36c7042d3bb 218
Zbyszek 4:e36c7042d3bb 219
Zbyszek 4:e36c7042d3bb 220
Zbyszek 4:e36c7042d3bb 221
Zbyszek 4:e36c7042d3bb 222
Zbyszek 4:e36c7042d3bb 223 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 224 vector quaternion2Vector(Quaternion Q) {
Zbyszek 4:e36c7042d3bb 225 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 226 vector localv;
Zbyszek 4:e36c7042d3bb 227 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 228
Zbyszek 4:e36c7042d3bb 229 localv.x = Q.x;
Zbyszek 4:e36c7042d3bb 230 localv.y = Q.y;
Zbyszek 4:e36c7042d3bb 231 localv.z = Q.z;
Zbyszek 4:e36c7042d3bb 232
Zbyszek 4:e36c7042d3bb 233 return localv;
Zbyszek 4:e36c7042d3bb 234 }
Zbyszek 4:e36c7042d3bb 235 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 236
Zbyszek 4:e36c7042d3bb 237
Zbyszek 4:e36c7042d3bb 238
Zbyszek 4:e36c7042d3bb 239
Zbyszek 4:e36c7042d3bb 240
Zbyszek 4:e36c7042d3bb 241
Zbyszek 4:e36c7042d3bb 242 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 243 Quaternion normaliseQuaternion(Quaternion Q) {
Zbyszek 4:e36c7042d3bb 244 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 245 Quaternion localQ;
Zbyszek 4:e36c7042d3bb 246 double l;
Zbyszek 4:e36c7042d3bb 247 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 248 localQ = Q;
Zbyszek 4:e36c7042d3bb 249 l = getQuaternionNorm(localQ);
Zbyszek 4:e36c7042d3bb 250
Zbyszek 4:e36c7042d3bb 251 localQ.w = localQ.w / l;
Zbyszek 4:e36c7042d3bb 252 localQ.x = localQ.x / l;
Zbyszek 4:e36c7042d3bb 253 localQ.y = localQ.y / l;
Zbyszek 4:e36c7042d3bb 254 localQ.z = localQ.z / l;
Zbyszek 4:e36c7042d3bb 255
Zbyszek 4:e36c7042d3bb 256 return localQ;
Zbyszek 4:e36c7042d3bb 257 }
Zbyszek 4:e36c7042d3bb 258 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 259
Zbyszek 4:e36c7042d3bb 260
Zbyszek 4:e36c7042d3bb 261
Zbyszek 4:e36c7042d3bb 262
Zbyszek 4:e36c7042d3bb 263
Zbyszek 4:e36c7042d3bb 264
Zbyszek 4:e36c7042d3bb 265
Zbyszek 4:e36c7042d3bb 266
Zbyszek 4:e36c7042d3bb 267 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 268 double getQuaternionNorm(Quaternion Q) {
Zbyszek 4:e36c7042d3bb 269 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 270 double l;
Zbyszek 4:e36c7042d3bb 271 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 272
Zbyszek 4:e36c7042d3bb 273 l = sqrt(pow(Q.w, 2) + pow(Q.x, 2) + pow(Q.y, 2) + pow(Q.z, 2)); //Normalise the quaternion such that it a unit quaternion between 0 and 1.
Zbyszek 4:e36c7042d3bb 274
Zbyszek 4:e36c7042d3bb 275 return l;
Zbyszek 4:e36c7042d3bb 276 }
Zbyszek 4:e36c7042d3bb 277 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 278
Zbyszek 4:e36c7042d3bb 279
Zbyszek 4:e36c7042d3bb 280
Zbyszek 4:e36c7042d3bb 281 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 282 Quaternion getQaDelta(Quaternion Q) {
Zbyszek 4:e36c7042d3bb 283 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 284 Quaternion localQ;
Zbyszek 4:e36c7042d3bb 285 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 286
Zbyszek 4:e36c7042d3bb 287 localQ.w = sqrt((Q.z+1)/2);
Zbyszek 4:e36c7042d3bb 288 localQ.x = -(Q.y/sqrt(2*(Q.z +1 )));
Zbyszek 4:e36c7042d3bb 289 localQ.y = Q.x/sqrt(2*(Q.z +1 ));
Zbyszek 4:e36c7042d3bb 290 localQ.z = 0;
Zbyszek 4:e36c7042d3bb 291
Zbyszek 4:e36c7042d3bb 292 return localQ;
Zbyszek 4:e36c7042d3bb 293 }
Zbyszek 4:e36c7042d3bb 294 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 295
Zbyszek 4:e36c7042d3bb 296
Zbyszek 4:e36c7042d3bb 297
Zbyszek 4:e36c7042d3bb 298
Zbyszek 4:e36c7042d3bb 299 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 300 vector crossProduct(vector v1, vector v2) {
Zbyszek 4:e36c7042d3bb 301 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 302 vector localv;
Zbyszek 4:e36c7042d3bb 303 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 304
Zbyszek 4:e36c7042d3bb 305
Zbyszek 4:e36c7042d3bb 306 localv.x = v1.y * v2.z - v1.z * v2.y;
Zbyszek 4:e36c7042d3bb 307 localv.y = v1.z * v2.x - v1.x * v2.z;
Zbyszek 4:e36c7042d3bb 308 localv.z = v1.x * v2.y - v1.y * v2.x;
Zbyszek 4:e36c7042d3bb 309
Zbyszek 4:e36c7042d3bb 310 return localv;
Zbyszek 4:e36c7042d3bb 311 }
Zbyszek 4:e36c7042d3bb 312 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 313
Zbyszek 4:e36c7042d3bb 314 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 315 vector rotateGlobal(vector v1, Quaternion Q) {
Zbyszek 4:e36c7042d3bb 316 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 317 vector localv;
Zbyszek 4:e36c7042d3bb 318 Quaternion localQ1;
Zbyszek 4:e36c7042d3bb 319 Quaternion localQ2;
Zbyszek 4:e36c7042d3bb 320 Quaternion localQconjugate;
Zbyszek 4:e36c7042d3bb 321 vector r;
Zbyszek 4:e36c7042d3bb 322 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 323
Zbyszek 4:e36c7042d3bb 324 localQconjugate = quaternionConjugate(Q);
Zbyszek 4:e36c7042d3bb 325 localQ1 = getQVproduct(Q, v1);
Zbyszek 4:e36c7042d3bb 326 localQ2 = getQuaternionProduct(localQ1, localQconjugate);
Zbyszek 4:e36c7042d3bb 327 localv = quaternion2Vector(localQ2);
Zbyszek 4:e36c7042d3bb 328
Zbyszek 4:e36c7042d3bb 329 return localv;
Zbyszek 4:e36c7042d3bb 330
Zbyszek 4:e36c7042d3bb 331 //r.x = -Q.x;
Zbyszek 4:e36c7042d3bb 332 //r.y = -Q.y;
Zbyszek 4:e36c7042d3bb 333 //r.z = -Q.z;
Zbyszek 4:e36c7042d3bb 334 //return sumVector(v1, crossProduct(sumVector(r, r), sumVector(crossProduct(r, v1), mul(Q.w, v1))));
Zbyszek 4:e36c7042d3bb 335 }
Zbyszek 4:e36c7042d3bb 336 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 337
Zbyszek 4:e36c7042d3bb 338 vector mul(float b, vector a) {
Zbyszek 4:e36c7042d3bb 339
Zbyszek 4:e36c7042d3bb 340
Zbyszek 4:e36c7042d3bb 341
Zbyszek 4:e36c7042d3bb 342 a.x *= b;
Zbyszek 4:e36c7042d3bb 343 a.y *= b;
Zbyszek 4:e36c7042d3bb 344 a.z *= b;
Zbyszek 4:e36c7042d3bb 345
Zbyszek 4:e36c7042d3bb 346 return a;
Zbyszek 4:e36c7042d3bb 347 }
Zbyszek 4:e36c7042d3bb 348
Zbyszek 4:e36c7042d3bb 349
Zbyszek 4:e36c7042d3bb 350 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 351 vector rotateLocal(vector v1, Quaternion Q) {
Zbyszek 4:e36c7042d3bb 352 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 353 vector localv;
Zbyszek 4:e36c7042d3bb 354 Quaternion localQ1;
Zbyszek 4:e36c7042d3bb 355 Quaternion localQ2;
Zbyszek 4:e36c7042d3bb 356 Quaternion localQconjugate;
Zbyszek 4:e36c7042d3bb 357 vector r;
Zbyszek 4:e36c7042d3bb 358 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 359
Zbyszek 4:e36c7042d3bb 360 localQconjugate = quaternionConjugate(Q);
Zbyszek 4:e36c7042d3bb 361 localQ1 = getQVproduct(localQconjugate, v1);
Zbyszek 4:e36c7042d3bb 362 localQ2 = getQuaternionProduct(localQ1, Q);
Zbyszek 4:e36c7042d3bb 363 localv = quaternion2Vector(localQ2);
Zbyszek 4:e36c7042d3bb 364
Zbyszek 4:e36c7042d3bb 365 return localv;
Zbyszek 4:e36c7042d3bb 366
Zbyszek 4:e36c7042d3bb 367
Zbyszek 4:e36c7042d3bb 368 //r.x = Q.x;
Zbyszek 4:e36c7042d3bb 369 //r.y = Q.y;
Zbyszek 4:e36c7042d3bb 370 //r.z = Q.z;
Zbyszek 4:e36c7042d3bb 371
Zbyszek 4:e36c7042d3bb 372 //return sumVector(v1, crossProduct(sumVector(r, r), sumVector(crossProduct(r, v1), mul(Q.w, v1))));
Zbyszek 4:e36c7042d3bb 373 }
Zbyszek 4:e36c7042d3bb 374 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 375
Zbyszek 4:e36c7042d3bb 376
Zbyszek 4:e36c7042d3bb 377
Zbyszek 4:e36c7042d3bb 378
Zbyszek 4:e36c7042d3bb 379 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 380 Quaternion updateQuaternion(Quaternion Qprevious, vector angularRate, double deltaTime)
Zbyszek 4:e36c7042d3bb 381 {
Zbyszek 4:e36c7042d3bb 382 vector AngularRates;
Zbyszek 4:e36c7042d3bb 383 Quaternion localQ, Qprime;
Zbyszek 4:e36c7042d3bb 384
Zbyszek 4:e36c7042d3bb 385 //The angular rates will be sent from each IMU object in form a structure and not an array and therefroe the angularRate[] will have to be chnaged
Zbyszek 4:e36c7042d3bb 386 AngularRates.x = angularRate.x;
Zbyszek 4:e36c7042d3bb 387 AngularRates.y = angularRate.y;
Zbyszek 4:e36c7042d3bb 388 AngularRates.z = angularRate.z;
Zbyszek 4:e36c7042d3bb 389
Zbyszek 4:e36c7042d3bb 390 //Need to add variable delta_t when I add the function that gets me the time between samples
Zbyszek 4:e36c7042d3bb 391 localQ = toQuaternionNotation(AngularRates, deltaTime);
Zbyszek 4:e36c7042d3bb 392 Qprime = getQuaternionProduct(Qprevious, localQ); // Perform the rotation on the old quaternion
Zbyszek 4:e36c7042d3bb 393
Zbyszek 4:e36c7042d3bb 394 return Qprime;
Zbyszek 4:e36c7042d3bb 395 }
Zbyszek 4:e36c7042d3bb 396 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 397
Zbyszek 4:e36c7042d3bb 398
Zbyszek 4:e36c7042d3bb 399
Zbyszek 4:e36c7042d3bb 400 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 401 vector eulerA(Quaternion Q)
Zbyszek 4:e36c7042d3bb 402 {
Zbyszek 4:e36c7042d3bb 403 vector EulerAngles;
Zbyszek 4:e36c7042d3bb 404
Zbyszek 4:e36c7042d3bb 405
Zbyszek 4:e36c7042d3bb 406
Zbyszek 4:e36c7042d3bb 407 EulerAngles.x = atan2((2*(Q.w*Q.x + Q.y*Q.z)), (pow(Q.w,2)-pow(Q.x,2)-pow(Q.y,2)+pow(Q.z,2))) * (180.0f/3.141592654f); //atan2 returns the angle between -π and π radians (equivalent to -180 and 180 degrees)
Zbyszek 4:e36c7042d3bb 408 EulerAngles.y = -asin(2*(Q.x*Q.z - Q.w*Q.y)) * (180.0f/3.141592654f); //asin returns the angle between -π/2 and π/2 radians (equivalent to -90 and 90 degrees)
Zbyszek 4:e36c7042d3bb 409 EulerAngles.z = atan2((2*(Q.w*Q.z + Q.x*Q.y)), (pow(Q.w,2)+pow(Q.x,2)-pow(Q.y,2)-pow(Q.z,2))) * (180.0f/3.141592654f);
Zbyszek 4:e36c7042d3bb 410
Zbyszek 4:e36c7042d3bb 411 return EulerAngles;
Zbyszek 4:e36c7042d3bb 412 }
Zbyszek 4:e36c7042d3bb 413 //----------------------------------------------------------------------------------------------------------------//