Zbigniew Druzbacki / Mbed 2 deprecated SPI_Slave

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

Dependencies:   mbed

Quaternions.cpp@6:0ebecfecadc9, 2019-02-26 (annotated)

Committer:
Zbyszek
Date:
Tue Feb 26 01:22:53 2019 +0000
Revision:
6:0ebecfecadc9
Parent:
5:155d224d855c
Child:
13:c7e8e277f884
Placing Code into class to make program tidy and prepare it for multiple IMUs

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Zbyszek 4:e36c7042d3bb 1 #include "Quaternions.h"
Zbyszek 6:0ebecfecadc9 2 #include "Structures.h"
Zbyszek 4:e36c7042d3bb 3 #include <math.h>
Zbyszek 4:e36c7042d3bb 4
Zbyszek 4:e36c7042d3bb 5
Zbyszek 4:e36c7042d3bb 6 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 7 /*
Zbyszek 4:e36c7042d3bb 8 *Takes the angular rate in roll, pitch, yaw form and converts it into quaternion notation
Zbyszek 4:e36c7042d3bb 9 */
Zbyszek 4:e36c7042d3bb 10 Quaternion toQuaternionNotation(vector AngularRate, double dt) {
Zbyszek 4:e36c7042d3bb 11 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 12 Quaternion q;
Zbyszek 4:e36c7042d3bb 13 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 14
Zbyszek 4:e36c7042d3bb 15
Zbyszek 4:e36c7042d3bb 16 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 17 double theta = (dt * l) / 2; //Get Theta in quaternions
Zbyszek 4:e36c7042d3bb 18
Zbyszek 4:e36c7042d3bb 19 //double thetaRad = theta * 0.01745; // 0.01745 = pi/180
Zbyszek 4:e36c7042d3bb 20 //Convert each component of rotation
Zbyszek 4:e36c7042d3bb 21 q.w = cosf(theta); //* Magnitude
Zbyszek 4:e36c7042d3bb 22 q.x = sinf(theta) * (AngularRate.x / l); //*
Zbyszek 4:e36c7042d3bb 23 q.y = sinf(theta) * (AngularRate.y / l); //*
Zbyszek 4:e36c7042d3bb 24 q.z = sinf(theta) * (AngularRate.z / l); //*
Zbyszek 4:e36c7042d3bb 25 //Convert each component of rotation
Zbyszek 4:e36c7042d3bb 26 return q;
Zbyszek 4:e36c7042d3bb 27 }
Zbyszek 4:e36c7042d3bb 28 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 29
Zbyszek 4:e36c7042d3bb 30
Zbyszek 4:e36c7042d3bb 31 Quaternion toQuaternionNotation123(vector AngularRate, double dt) {
Zbyszek 4:e36c7042d3bb 32 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 33 Quaternion Q;
Zbyszek 4:e36c7042d3bb 34 float Sin_Mag;
Zbyszek 4:e36c7042d3bb 35 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 36
Zbyszek 4:e36c7042d3bb 37 Q.x = AngularRate.x * dt;
Zbyszek 4:e36c7042d3bb 38 Q.y = AngularRate.y * dt;
Zbyszek 4:e36c7042d3bb 39 Q.z = AngularRate.z * dt;
Zbyszek 4:e36c7042d3bb 40 Q.w = 1.0f - 0.5f * (Q.x * Q.x +Q.y * Q.y + Q.z * Q.z);
Zbyszek 4:e36c7042d3bb 41
Zbyszek 4:e36c7042d3bb 42
Zbyszek 4:e36c7042d3bb 43
Zbyszek 4:e36c7042d3bb 44 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 45 double theta = l / 2; //Get Theta in quaternions
Zbyszek 4:e36c7042d3bb 46
Zbyszek 4:e36c7042d3bb 47 //double thetaRad = theta * 0.01745; // 0.01745 = pi/180
Zbyszek 4:e36c7042d3bb 48
Zbyszek 4:e36c7042d3bb 49 Sin_Mag = sin(theta) / l;
Zbyszek 4:e36c7042d3bb 50 //Convert each component of rotation
Zbyszek 4:e36c7042d3bb 51 Q.x = AngularRate.x * Sin_Mag;
Zbyszek 4:e36c7042d3bb 52 Q.y = AngularRate.y * Sin_Mag;
Zbyszek 4:e36c7042d3bb 53 Q.z = AngularRate.z * Sin_Mag;
Zbyszek 4:e36c7042d3bb 54 Q.w = cos(theta);
Zbyszek 4:e36c7042d3bb 55
Zbyszek 4:e36c7042d3bb 56 return Q;
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 /*
Zbyszek 4:e36c7042d3bb 67 *This function updates the old orientation by rotating the reference frame by
Zbyszek 4:e36c7042d3bb 68 *the change in angle. All this is done in quaternion form.
Zbyszek 4:e36c7042d3bb 69 */
Zbyszek 4:e36c7042d3bb 70 Quaternion getQuaternionProduct(Quaternion Q1, Quaternion Q2) {
Zbyszek 4:e36c7042d3bb 71 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 72 Quaternion currentQ;
Zbyszek 4:e36c7042d3bb 73 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 74
Zbyszek 4:e36c7042d3bb 75
Zbyszek 4:e36c7042d3bb 76 // Piecewise matrix multiplication
Zbyszek 4:e36c7042d3bb 77 currentQ.w = (Q1.w*Q2.w - Q1.x*Q2.x - Q1.y*Q2.y - Q1.z*Q2.z);
Zbyszek 4:e36c7042d3bb 78 currentQ.x = (Q1.w*Q2.x + Q1.x*Q2.w + Q1.y*Q2.z - Q1.z*Q2.y);
Zbyszek 4:e36c7042d3bb 79 currentQ.y = (Q1.w*Q2.y - Q1.x*Q2.z + Q1.y*Q2.w + Q1.z*Q2.x);
Zbyszek 4:e36c7042d3bb 80 currentQ.z = (Q1.w*Q2.z + Q1.x*Q2.y - Q1.y*Q2.x + Q1.z*Q2.w);
Zbyszek 4:e36c7042d3bb 81
Zbyszek 4:e36c7042d3bb 82 return currentQ;
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 /*
Zbyszek 4:e36c7042d3bb 91 *This function updates the old orientation by rotating the reference frame by
Zbyszek 4:e36c7042d3bb 92 *the change in angle. All this is done in quaternion form.
Zbyszek 4:e36c7042d3bb 93 */
Zbyszek 4:e36c7042d3bb 94 Quaternion getQVproduct(Quaternion Q, vector v) {
Zbyszek 4:e36c7042d3bb 95 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 96 Quaternion currentQ;
Zbyszek 4:e36c7042d3bb 97 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 98
Zbyszek 4:e36c7042d3bb 99
Zbyszek 4:e36c7042d3bb 100 // Piecewise matrix multiplication
Zbyszek 4:e36c7042d3bb 101 currentQ.x = (Q.w*v.x + Q.z*v.y - Q.y*v.z);
Zbyszek 4:e36c7042d3bb 102 currentQ.y = (Q.w*v.y + Q.x*v.z - Q.z*v.x);
Zbyszek 4:e36c7042d3bb 103 currentQ.z = (Q.y*v.x - Q.x*v.y + Q.w*v.z);
Zbyszek 4:e36c7042d3bb 104 currentQ.w = (-Q.x*v.x - Q.y*v.y - Q.z*v.z);
Zbyszek 4:e36c7042d3bb 105
Zbyszek 4:e36c7042d3bb 106 return currentQ;
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 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 118 Quaternion quaternionConjugate(Quaternion Q) {
Zbyszek 4:e36c7042d3bb 119 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 120 Quaternion localQ;
Zbyszek 4:e36c7042d3bb 121 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 122
Zbyszek 4:e36c7042d3bb 123 localQ.w = Q.w;
Zbyszek 4:e36c7042d3bb 124 localQ.x = -Q.x;
Zbyszek 4:e36c7042d3bb 125 localQ.y = -Q.y;
Zbyszek 4:e36c7042d3bb 126 localQ.z = -Q.z;
Zbyszek 4:e36c7042d3bb 127
Zbyszek 4:e36c7042d3bb 128 return localQ;
Zbyszek 4:e36c7042d3bb 129
Zbyszek 4:e36c7042d3bb 130 }
Zbyszek 4:e36c7042d3bb 131 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 132
Zbyszek 4:e36c7042d3bb 133 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 134 Quaternion sumQuaternion(Quaternion Q1, Quaternion Q2) {
Zbyszek 4:e36c7042d3bb 135 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 136 Quaternion localQ;
Zbyszek 4:e36c7042d3bb 137 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 138
Zbyszek 4:e36c7042d3bb 139 localQ.w = Q1.w + Q2.w;
Zbyszek 4:e36c7042d3bb 140 localQ.x = Q1.x + Q2.x;
Zbyszek 4:e36c7042d3bb 141 localQ.y = Q1.y + Q2.y;
Zbyszek 4:e36c7042d3bb 142 localQ.z = Q1.z + Q2.z;
Zbyszek 4:e36c7042d3bb 143
Zbyszek 4:e36c7042d3bb 144 return localQ;
Zbyszek 4:e36c7042d3bb 145
Zbyszek 4:e36c7042d3bb 146 }
Zbyszek 4:e36c7042d3bb 147 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 148
Zbyszek 4:e36c7042d3bb 149
Zbyszek 4:e36c7042d3bb 150 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 151 vector sumVector(vector v1, vector v2) {
Zbyszek 4:e36c7042d3bb 152 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 153 vector localv;
Zbyszek 4:e36c7042d3bb 154 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 155
Zbyszek 4:e36c7042d3bb 156 localv.x = v1.x + v2.x;
Zbyszek 4:e36c7042d3bb 157 localv.y = v1.y + v2.y;
Zbyszek 4:e36c7042d3bb 158 localv.z = v1.z + v2.z;
Zbyszek 4:e36c7042d3bb 159
Zbyszek 4:e36c7042d3bb 160 return localv;
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 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 172 vector rotateVector(Quaternion Q, vector v) {
Zbyszek 4:e36c7042d3bb 173 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 174 Quaternion vectorQ;
Zbyszek 4:e36c7042d3bb 175 Quaternion Qconjugate;
Zbyszek 4:e36c7042d3bb 176 Quaternion rotationQ;
Zbyszek 4:e36c7042d3bb 177 vector localv;
Zbyszek 4:e36c7042d3bb 178 vector rotatedVector;
Zbyszek 4:e36c7042d3bb 179 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 180
Zbyszek 4:e36c7042d3bb 181 localv = v;
Zbyszek 4:e36c7042d3bb 182 vectorQ = vector2Quaternion(localv);
Zbyszek 4:e36c7042d3bb 183 Qconjugate = quaternionConjugate(Q);
Zbyszek 4:e36c7042d3bb 184
Zbyszek 4:e36c7042d3bb 185 //q*p*q*. Our point is our rotation axis therefore q*v*q*
Zbyszek 4:e36c7042d3bb 186 rotationQ = getQuaternionProduct(Q, vectorQ);
Zbyszek 4:e36c7042d3bb 187 rotationQ = getQuaternionProduct(rotationQ, Qconjugate);
Zbyszek 4:e36c7042d3bb 188 rotatedVector = quaternion2Vector(rotationQ);
Zbyszek 4:e36c7042d3bb 189
Zbyszek 4:e36c7042d3bb 190 return rotatedVector;
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 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 203 Quaternion vector2Quaternion(vector v) {
Zbyszek 4:e36c7042d3bb 204 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 205 Quaternion localQ;
Zbyszek 4:e36c7042d3bb 206 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 207
Zbyszek 4:e36c7042d3bb 208 localQ.w = 0;
Zbyszek 4:e36c7042d3bb 209 localQ.x = v.x;
Zbyszek 4:e36c7042d3bb 210 localQ.y = v.y;
Zbyszek 4:e36c7042d3bb 211 localQ.z = v.z;
Zbyszek 4:e36c7042d3bb 212
Zbyszek 4:e36c7042d3bb 213 return localQ;
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 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 225 vector quaternion2Vector(Quaternion Q) {
Zbyszek 4:e36c7042d3bb 226 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 227 vector localv;
Zbyszek 4:e36c7042d3bb 228 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 229
Zbyszek 4:e36c7042d3bb 230 localv.x = Q.x;
Zbyszek 4:e36c7042d3bb 231 localv.y = Q.y;
Zbyszek 4:e36c7042d3bb 232 localv.z = Q.z;
Zbyszek 4:e36c7042d3bb 233
Zbyszek 4:e36c7042d3bb 234 return localv;
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 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 244 Quaternion normaliseQuaternion(Quaternion Q) {
Zbyszek 4:e36c7042d3bb 245 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 246 Quaternion localQ;
Zbyszek 4:e36c7042d3bb 247 double l;
Zbyszek 4:e36c7042d3bb 248 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 249 localQ = Q;
Zbyszek 4:e36c7042d3bb 250 l = getQuaternionNorm(localQ);
Zbyszek 4:e36c7042d3bb 251
Zbyszek 4:e36c7042d3bb 252 localQ.w = localQ.w / l;
Zbyszek 4:e36c7042d3bb 253 localQ.x = localQ.x / l;
Zbyszek 4:e36c7042d3bb 254 localQ.y = localQ.y / l;
Zbyszek 4:e36c7042d3bb 255 localQ.z = localQ.z / l;
Zbyszek 4:e36c7042d3bb 256
Zbyszek 4:e36c7042d3bb 257 return localQ;
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 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 269 double getQuaternionNorm(Quaternion Q) {
Zbyszek 4:e36c7042d3bb 270 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 271 double l;
Zbyszek 4:e36c7042d3bb 272 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 273
Zbyszek 4:e36c7042d3bb 274 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 275
Zbyszek 4:e36c7042d3bb 276 return l;
Zbyszek 4:e36c7042d3bb 277 }
Zbyszek 4:e36c7042d3bb 278 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 279
Zbyszek 4:e36c7042d3bb 280
Zbyszek 4:e36c7042d3bb 281
Zbyszek 4:e36c7042d3bb 282 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 283 Quaternion getQaDelta(Quaternion Q) {
Zbyszek 4:e36c7042d3bb 284 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 285 Quaternion localQ;
Zbyszek 4:e36c7042d3bb 286 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 287
Zbyszek 4:e36c7042d3bb 288 localQ.w = sqrt((Q.z+1)/2);
Zbyszek 4:e36c7042d3bb 289 localQ.x = -(Q.y/sqrt(2*(Q.z +1 )));
Zbyszek 4:e36c7042d3bb 290 localQ.y = Q.x/sqrt(2*(Q.z +1 ));
Zbyszek 4:e36c7042d3bb 291 localQ.z = 0;
Zbyszek 4:e36c7042d3bb 292
Zbyszek 4:e36c7042d3bb 293 return localQ;
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 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 301 vector crossProduct(vector v1, vector v2) {
Zbyszek 4:e36c7042d3bb 302 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 303 vector localv;
Zbyszek 4:e36c7042d3bb 304 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 305
Zbyszek 4:e36c7042d3bb 306
Zbyszek 4:e36c7042d3bb 307 localv.x = v1.y * v2.z - v1.z * v2.y;
Zbyszek 4:e36c7042d3bb 308 localv.y = v1.z * v2.x - v1.x * v2.z;
Zbyszek 4:e36c7042d3bb 309 localv.z = v1.x * v2.y - v1.y * v2.x;
Zbyszek 4:e36c7042d3bb 310
Zbyszek 4:e36c7042d3bb 311 return localv;
Zbyszek 4:e36c7042d3bb 312 }
Zbyszek 4:e36c7042d3bb 313 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 314
Zbyszek 4:e36c7042d3bb 315 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 316 vector rotateGlobal(vector v1, Quaternion Q) {
Zbyszek 4:e36c7042d3bb 317 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 318 vector localv;
Zbyszek 4:e36c7042d3bb 319 Quaternion localQ1;
Zbyszek 4:e36c7042d3bb 320 Quaternion localQ2;
Zbyszek 4:e36c7042d3bb 321 Quaternion localQconjugate;
Zbyszek 4:e36c7042d3bb 322 vector r;
Zbyszek 4:e36c7042d3bb 323 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 324
Zbyszek 4:e36c7042d3bb 325 localQconjugate = quaternionConjugate(Q);
Zbyszek 4:e36c7042d3bb 326 localQ1 = getQVproduct(Q, v1);
Zbyszek 4:e36c7042d3bb 327 localQ2 = getQuaternionProduct(localQ1, localQconjugate);
Zbyszek 4:e36c7042d3bb 328 localv = quaternion2Vector(localQ2);
Zbyszek 4:e36c7042d3bb 329
Zbyszek 4:e36c7042d3bb 330 return localv;
Zbyszek 4:e36c7042d3bb 331
Zbyszek 4:e36c7042d3bb 332 //r.x = -Q.x;
Zbyszek 4:e36c7042d3bb 333 //r.y = -Q.y;
Zbyszek 4:e36c7042d3bb 334 //r.z = -Q.z;
Zbyszek 4:e36c7042d3bb 335 //return sumVector(v1, crossProduct(sumVector(r, r), sumVector(crossProduct(r, v1), mul(Q.w, v1))));
Zbyszek 4:e36c7042d3bb 336 }
Zbyszek 4:e36c7042d3bb 337 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 338
Zbyszek 4:e36c7042d3bb 339 vector mul(float b, vector a) {
Zbyszek 4:e36c7042d3bb 340
Zbyszek 4:e36c7042d3bb 341
Zbyszek 4:e36c7042d3bb 342
Zbyszek 4:e36c7042d3bb 343 a.x *= b;
Zbyszek 4:e36c7042d3bb 344 a.y *= b;
Zbyszek 4:e36c7042d3bb 345 a.z *= b;
Zbyszek 4:e36c7042d3bb 346
Zbyszek 4:e36c7042d3bb 347 return a;
Zbyszek 4:e36c7042d3bb 348 }
Zbyszek 4:e36c7042d3bb 349
Zbyszek 4:e36c7042d3bb 350
Zbyszek 4:e36c7042d3bb 351 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 352 vector rotateLocal(vector v1, Quaternion Q) {
Zbyszek 4:e36c7042d3bb 353 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 354 vector localv;
Zbyszek 4:e36c7042d3bb 355 Quaternion localQ1;
Zbyszek 4:e36c7042d3bb 356 Quaternion localQ2;
Zbyszek 4:e36c7042d3bb 357 Quaternion localQconjugate;
Zbyszek 4:e36c7042d3bb 358 vector r;
Zbyszek 4:e36c7042d3bb 359 //--------------Variables--------------//
Zbyszek 4:e36c7042d3bb 360
Zbyszek 4:e36c7042d3bb 361 localQconjugate = quaternionConjugate(Q);
Zbyszek 4:e36c7042d3bb 362 localQ1 = getQVproduct(localQconjugate, v1);
Zbyszek 4:e36c7042d3bb 363 localQ2 = getQuaternionProduct(localQ1, Q);
Zbyszek 4:e36c7042d3bb 364 localv = quaternion2Vector(localQ2);
Zbyszek 4:e36c7042d3bb 365
Zbyszek 4:e36c7042d3bb 366 return localv;
Zbyszek 4:e36c7042d3bb 367
Zbyszek 4:e36c7042d3bb 368
Zbyszek 4:e36c7042d3bb 369 //r.x = Q.x;
Zbyszek 4:e36c7042d3bb 370 //r.y = Q.y;
Zbyszek 4:e36c7042d3bb 371 //r.z = Q.z;
Zbyszek 4:e36c7042d3bb 372
Zbyszek 4:e36c7042d3bb 373 //return sumVector(v1, crossProduct(sumVector(r, r), sumVector(crossProduct(r, v1), mul(Q.w, v1))));
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 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 381 Quaternion updateQuaternion(Quaternion Qprevious, vector angularRate, double deltaTime)
Zbyszek 4:e36c7042d3bb 382 {
Zbyszek 4:e36c7042d3bb 383 vector AngularRates;
Zbyszek 4:e36c7042d3bb 384 Quaternion localQ, Qprime;
Zbyszek 4:e36c7042d3bb 385
Zbyszek 4:e36c7042d3bb 386 //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 387 AngularRates.x = angularRate.x;
Zbyszek 4:e36c7042d3bb 388 AngularRates.y = angularRate.y;
Zbyszek 4:e36c7042d3bb 389 AngularRates.z = angularRate.z;
Zbyszek 4:e36c7042d3bb 390
Zbyszek 4:e36c7042d3bb 391 //Need to add variable delta_t when I add the function that gets me the time between samples
Zbyszek 4:e36c7042d3bb 392 localQ = toQuaternionNotation(AngularRates, deltaTime);
Zbyszek 4:e36c7042d3bb 393 Qprime = getQuaternionProduct(Qprevious, localQ); // Perform the rotation on the old quaternion
Zbyszek 4:e36c7042d3bb 394
Zbyszek 4:e36c7042d3bb 395 return Qprime;
Zbyszek 4:e36c7042d3bb 396 }
Zbyszek 4:e36c7042d3bb 397 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 398
Zbyszek 4:e36c7042d3bb 399
Zbyszek 4:e36c7042d3bb 400
Zbyszek 4:e36c7042d3bb 401 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 4:e36c7042d3bb 402 vector eulerA(Quaternion Q)
Zbyszek 4:e36c7042d3bb 403 {
Zbyszek 4:e36c7042d3bb 404 vector EulerAngles;
Zbyszek 4:e36c7042d3bb 405
Zbyszek 4:e36c7042d3bb 406
Zbyszek 4:e36c7042d3bb 407
Zbyszek 4:e36c7042d3bb 408 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 409 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 410 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 411
Zbyszek 4:e36c7042d3bb 412 return EulerAngles;
Zbyszek 4:e36c7042d3bb 413 }
Zbyszek 5:155d224d855c 414 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 5:155d224d855c 415
Zbyszek 5:155d224d855c 416
Zbyszek 5:155d224d855c 417
Zbyszek 5:155d224d855c 418 //----------------------------------------------------------------------------------------------------------------//
Zbyszek 5:155d224d855c 419 Quaternion euler2Quaternion(Quaternion Q)
Zbyszek 5:155d224d855c 420 {
Zbyszek 5:155d224d855c 421
Zbyszek 5:155d224d855c 422
Zbyszek 5:155d224d855c 423
Zbyszek 5:155d224d855c 424 // Abbreviations for the various angular functions
Zbyszek 5:155d224d855c 425 double cy = cosf(Q.z * 0.5f);
Zbyszek 5:155d224d855c 426 double sy = sinf(Q.z * 0.5f);
Zbyszek 5:155d224d855c 427 double cp = cosf(Q.y * 0.5f);
Zbyszek 5:155d224d855c 428 double sp = sinf(Q.y * 0.5f);
Zbyszek 5:155d224d855c 429 double cr = cosf(Q.x * 0.5f);
Zbyszek 5:155d224d855c 430 double sr = sinf(Q.x * 0.5f);
Zbyszek 5:155d224d855c 431
Zbyszek 5:155d224d855c 432 Quaternion q;
Zbyszek 5:155d224d855c 433 q.w = cy * cp * cr + sy * sp * sr;
Zbyszek 5:155d224d855c 434 q.x = cy * cp * sr - sy * sp * cr;
Zbyszek 5:155d224d855c 435 q.y = sy * cp * sr + cy * sp * cr;
Zbyszek 5:155d224d855c 436 q.z = sy * cp * cr - cy * sp * sr;
Zbyszek 5:155d224d855c 437
Zbyszek 5:155d224d855c 438 return q;
Zbyszek 5:155d224d855c 439
Zbyszek 5:155d224d855c 440 }
Zbyszek 4:e36c7042d3bb 441 //----------------------------------------------------------------------------------------------------------------//