Zbigniew Druzbacki / Mbed 2 deprecated SPI_Slave

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

Dependencies:   mbed

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

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

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Zbyszek 0:8e367d6d8f03 1 #include "mbed.h"
Zbyszek 0:8e367d6d8f03 2 #include "SPI.h"
Zbyszek 4:e36c7042d3bb 3 #include "Quaternions.h"
Zbyszek 0:8e367d6d8f03 4 DigitalOut myled(LED1);
Zbyszek 0:8e367d6d8f03 5 Serial pc(USBTX, USBRX);
Zbyszek 0:8e367d6d8f03 6
Zbyszek 0:8e367d6d8f03 7 int masterRx = 0;
Zbyszek 3:e33697420c4a 8 int16_t slaveRx = 0;
Zbyszek 3:e33697420c4a 9 int i = 2;
Zbyszek 3:e33697420c4a 10 int k = 0;
Zbyszek 3:e33697420c4a 11 int16_t zgHigher = 0;
Zbyszek 3:e33697420c4a 12 int16_t zgLower = 0;
Zbyszek 3:e33697420c4a 13 int16_t zGyro = 0;
Zbyszek 3:e33697420c4a 14 int countx = 0;
Zbyszek 3:e33697420c4a 15 int p = 32776;
Zbyszek 3:e33697420c4a 16
Zbyszek 4:e36c7042d3bb 17 Timer t;
Zbyszek 4:e36c7042d3bb 18 float dTime = 0.0f;
Zbyszek 4:e36c7042d3bb 19
Zbyszek 4:e36c7042d3bb 20 vector vertical;
Zbyszek 4:e36c7042d3bb 21 vector globalAccel;
Zbyszek 4:e36c7042d3bb 22 vector correctionGlobalAccel;
Zbyszek 4:e36c7042d3bb 23 vector correctionBodyAccel;
Zbyszek 4:e36c7042d3bb 24 Quaternion gyroQ;
Zbyszek 4:e36c7042d3bb 25 vector Eangles;
Zbyszek 4:e36c7042d3bb 26
Zbyszek 4:e36c7042d3bb 27
Zbyszek 4:e36c7042d3bb 28 float xx;
Zbyszek 4:e36c7042d3bb 29 float yy;
Zbyszek 4:e36c7042d3bb 30 float zz;
Zbyszek 4:e36c7042d3bb 31
Zbyszek 4:e36c7042d3bb 32
Zbyszek 3:e33697420c4a 33
Zbyszek 3:e33697420c4a 34 /*
Zbyszek 3:e33697420c4a 35
Zbyszek 3:e33697420c4a 36 */
Zbyszek 3:e33697420c4a 37 //------------------Printing-All-values----------------------//
Zbyszek 3:e33697420c4a 38 int16_t IMUarray[12]; //Store each separate reading in an array
Zbyszek 3:e33697420c4a 39 uint16_t IDarray[12]; //Holds the identification of each data piece
Zbyszek 3:e33697420c4a 40 char idx = 0; //IMUarray Pointer
Zbyszek 3:e33697420c4a 41 char dataCount = 0; //Keeps track of how many data points have been read in using SPI
Zbyszek 3:e33697420c4a 42 uint16_t id = 0; //Used to store extracted data ID
Zbyszek 3:e33697420c4a 43
Zbyszek 3:e33697420c4a 44 void ProcessAndPrint();
Zbyszek 3:e33697420c4a 45 //------------------Printing-All-values----------------------//
Zbyszek 3:e33697420c4a 46
Zbyszek 4:e36c7042d3bb 47 //-------------Testing-Variables-Remove-Later----------------//
Zbyszek 4:e36c7042d3bb 48 int blinkCounter = 0;
Zbyszek 4:e36c7042d3bb 49 //-------------Testing-Variables-Remove-Later----------------//
Zbyszek 0:8e367d6d8f03 50
Zbyszek 0:8e367d6d8f03 51 int main() {
Zbyszek 4:e36c7042d3bb 52 pc.baud (115200);
Zbyszek 3:e33697420c4a 53 IDarray[0] = 1;
Zbyszek 3:e33697420c4a 54 IDarray[1] = 0;
Zbyszek 3:e33697420c4a 55 IDarray[2] = 9;
Zbyszek 3:e33697420c4a 56 IDarray[3] = 8;
Zbyszek 3:e33697420c4a 57 IDarray[4] = 17;
Zbyszek 3:e33697420c4a 58 IDarray[5] = 16;
Zbyszek 3:e33697420c4a 59 IDarray[6] = 3;
Zbyszek 4:e36c7042d3bb 60 IDarray[7] = 2; //2
Zbyszek 3:e33697420c4a 61 IDarray[8] = 11;
Zbyszek 3:e33697420c4a 62 IDarray[9] = 10;
Zbyszek 3:e33697420c4a 63 IDarray[10] = 19;
Zbyszek 3:e33697420c4a 64 IDarray[11] = 18;
Zbyszek 3:e33697420c4a 65
Zbyszek 4:e36c7042d3bb 66 init_spi1();
Zbyszek 4:e36c7042d3bb 67 t.start();
Zbyszek 3:e33697420c4a 68
Zbyszek 4:e36c7042d3bb 69 gyroQ.w = 1;
Zbyszek 4:e36c7042d3bb 70 gyroQ.x = 0.0001;
Zbyszek 4:e36c7042d3bb 71 gyroQ.y = 0.0001;
Zbyszek 4:e36c7042d3bb 72 gyroQ.z = 0.0001;
Zbyszek 4:e36c7042d3bb 73
Zbyszek 4:e36c7042d3bb 74
Zbyszek 4:e36c7042d3bb 75
Zbyszek 4:e36c7042d3bb 76 gyroQ = normaliseQuaternion(gyroQ);
Zbyszek 4:e36c7042d3bb 77
Zbyszek 4:e36c7042d3bb 78 vertical.x = 0.0f;
Zbyszek 4:e36c7042d3bb 79 vertical.y = 0.0f;
Zbyszek 4:e36c7042d3bb 80 vertical.z = 1.0f;
Zbyszek 4:e36c7042d3bb 81
Zbyszek 3:e33697420c4a 82 pc.printf("Begin \n\r");
Zbyszek 0:8e367d6d8f03 83
Zbyszek 0:8e367d6d8f03 84 while(1) {
Zbyszek 3:e33697420c4a 85 if(i == 1) {
Zbyszek 0:8e367d6d8f03 86 for(int x = 1; x < 128; x *= 2) {
Zbyszek 0:8e367d6d8f03 87 slaveRx = transfer_spi_slave(x);
Zbyszek 3:e33697420c4a 88 //pc.putc(slaveRx);
Zbyszek 3:e33697420c4a 89 pc.printf("%d \n\r",slaveRx);
Zbyszek 0:8e367d6d8f03 90 }
Zbyszek 0:8e367d6d8f03 91 for(int x = 128; x > 1; x /= 2) {
Zbyszek 0:8e367d6d8f03 92 slaveRx = transfer_spi_slave(x);
Zbyszek 3:e33697420c4a 93 //pc.putc(slaveRx);
Zbyszek 3:e33697420c4a 94 pc.printf("%d \n\r",slaveRx);
Zbyszek 2:4cc880ea466d 95 }
Zbyszek 3:e33697420c4a 96 }//if(i == 1)
Zbyszek 3:e33697420c4a 97
Zbyszek 3:e33697420c4a 98
Zbyszek 3:e33697420c4a 99
Zbyszek 3:e33697420c4a 100 //------------------------------------------------------------------------------
Zbyszek 3:e33697420c4a 101 if(i == 0) {
Zbyszek 3:e33697420c4a 102 slaveRx = transfer_spi_slave(10);
Zbyszek 3:e33697420c4a 103 countx++;
Zbyszek 3:e33697420c4a 104 if(countx == 1) {
Zbyszek 3:e33697420c4a 105 k = slaveRx;
Zbyszek 3:e33697420c4a 106 k &= ~(255 << 8);
Zbyszek 3:e33697420c4a 107 k = k << 8;
Zbyszek 3:e33697420c4a 108 // if(k == 19) {
Zbyszek 3:e33697420c4a 109 zgHigher = k;
Zbyszek 3:e33697420c4a 110 //zgHigher = zgHigher << 8;
Zbyszek 3:e33697420c4a 111 //}//if(k == 19)
Zbyszek 3:e33697420c4a 112 }//if(count == 11)
Zbyszek 3:e33697420c4a 113
Zbyszek 3:e33697420c4a 114
Zbyszek 3:e33697420c4a 115 else if(countx == 2) {
Zbyszek 3:e33697420c4a 116 k = slaveRx;
Zbyszek 3:e33697420c4a 117 k &= ~(255 << 8);
Zbyszek 3:e33697420c4a 118 //k = k << 8;
Zbyszek 3:e33697420c4a 119 //if(k == 18) {
Zbyszek 3:e33697420c4a 120 zgLower = k;
Zbyszek 3:e33697420c4a 121 //zgLower = zgLower << 8;
Zbyszek 3:e33697420c4a 122 zGyro = zgHigher + zgLower;
Zbyszek 3:e33697420c4a 123 pc.printf("%+d \n\r",zGyro);
Zbyszek 3:e33697420c4a 124 //}//if(k == 19)
Zbyszek 3:e33697420c4a 125 countx = 0;
Zbyszek 3:e33697420c4a 126 }//else if(count == 12)
Zbyszek 3:e33697420c4a 127
Zbyszek 3:e33697420c4a 128 }//if(i == 0)
Zbyszek 3:e33697420c4a 129 //------------------------------------------------------------------------------
Zbyszek 3:e33697420c4a 130
Zbyszek 3:e33697420c4a 131 if(i == 2) {
Zbyszek 4:e36c7042d3bb 132 slaveRx = transfer_spi_slave(10); //get IMU data
Zbyszek 4:e36c7042d3bb 133
Zbyszek 4:e36c7042d3bb 134
Zbyszek 4:e36c7042d3bb 135 id = slaveRx; //Save sample to id for id extraction
Zbyszek 4:e36c7042d3bb 136 id &= ~(255); //get rid of the actual data to only be left with the id
Zbyszek 4:e36c7042d3bb 137 id = id >> 8; //shift the id to the right for comparison
Zbyszek 4:e36c7042d3bb 138
Zbyszek 4:e36c7042d3bb 139 //pc.printf("ID: %d \n\r", id); //Print each id to see what sequence is obtained. Only the correct sequence will make the code run/
Zbyszek 4:e36c7042d3bb 140
Zbyszek 4:e36c7042d3bb 141 if(IDarray[dataCount] == id) { //compare if the order of data is right and if not wait until it is.
Zbyszek 4:e36c7042d3bb 142 dataCount++; //Increase dataCount as new value has been read in.
Zbyszek 4:e36c7042d3bb 143 IMUarray[idx] = slaveRx; //save the newly read value to current free space in the array
Zbyszek 4:e36c7042d3bb 144 idx++; //increment the pointer to point to next free space in the array
Zbyszek 3:e33697420c4a 145
Zbyszek 3:e33697420c4a 146 if(dataCount == 12) {
Zbyszek 3:e33697420c4a 147 //reset idx and dataCount
Zbyszek 3:e33697420c4a 148 dataCount = 0;
Zbyszek 3:e33697420c4a 149 idx = 0;
Zbyszek 4:e36c7042d3bb 150 ProcessAndPrint();
Zbyszek 4:e36c7042d3bb 151
Zbyszek 4:e36c7042d3bb 152 t.stop();
Zbyszek 4:e36c7042d3bb 153 dTime = t.read();
Zbyszek 4:e36c7042d3bb 154 t.reset();
Zbyszek 4:e36c7042d3bb 155 t.start();
Zbyszek 3:e33697420c4a 156 }//if(dataCount == 12)
Zbyszek 3:e33697420c4a 157 }//if(IDarray[dataCount] == id)
Zbyszek 4:e36c7042d3bb 158 else {
Zbyszek 4:e36c7042d3bb 159 //-----Code-Used-For-Testing-----//
Zbyszek 4:e36c7042d3bb 160 //pc.printf("Failed at: %d \n\r", dataCount); //Print an error if there is one
Zbyszek 4:e36c7042d3bb 161 if(blinkCounter == 10) { //Slow the blinking down to make it visible if there are errors
Zbyszek 4:e36c7042d3bb 162 myled = !myled; //Change state of the LED if error occurs
Zbyszek 4:e36c7042d3bb 163 blinkCounter = 0; //Reset Blink counter
Zbyszek 4:e36c7042d3bb 164 }
Zbyszek 4:e36c7042d3bb 165 else {
Zbyszek 4:e36c7042d3bb 166 blinkCounter++;
Zbyszek 4:e36c7042d3bb 167 }
Zbyszek 4:e36c7042d3bb 168 //-----Code-Used-For-Testing-----//
Zbyszek 4:e36c7042d3bb 169
Zbyszek 4:e36c7042d3bb 170 dataCount = 0; //ID sequence is worng so reset the counter
Zbyszek 4:e36c7042d3bb 171 idx = 0; //ID sequence is worng so reset the counter
Zbyszek 4:e36c7042d3bb 172 }
Zbyszek 3:e33697420c4a 173 }//if(i == 2)
Zbyszek 3:e33697420c4a 174
Zbyszek 3:e33697420c4a 175
Zbyszek 3:e33697420c4a 176
Zbyszek 0:8e367d6d8f03 177 }
Zbyszek 0:8e367d6d8f03 178 }
Zbyszek 3:e33697420c4a 179
Zbyszek 3:e33697420c4a 180
Zbyszek 3:e33697420c4a 181
Zbyszek 4:e36c7042d3bb 182
Zbyszek 4:e36c7042d3bb 183 /*
Zbyszek 4:e36c7042d3bb 184 -The purpose of this function
Zbyszek 4:e36c7042d3bb 185 */
Zbyszek 3:e33697420c4a 186 void ProcessAndPrint() {
Zbyszek 3:e33697420c4a 187 int16_t MSB = 0; //Store Most Significant Byte of data piece in this variable for processing
Zbyszek 3:e33697420c4a 188 int16_t LSB = 0; //Store Least Significant Byte of data piece in this variable for processing
Zbyszek 3:e33697420c4a 189 char arrPointer = 0; //Array Pointer
Zbyszek 4:e36c7042d3bb 190
Zbyszek 4:e36c7042d3bb 191 vector GyroVals;
Zbyszek 4:e36c7042d3bb 192 vector AccelVals;
Zbyszek 4:e36c7042d3bb 193 vector accelTilt;
Zbyszek 4:e36c7042d3bb 194 Quaternion incRot;
Zbyszek 3:e33697420c4a 195 //-----------Concatentated-Data-Pieces------------------------------------------
Zbyszek 3:e33697420c4a 196 int16_t gyroX = 0;
Zbyszek 3:e33697420c4a 197 int16_t gyroY = 0;
Zbyszek 3:e33697420c4a 198 int16_t gyroZ = 0;
Zbyszek 3:e33697420c4a 199
Zbyszek 3:e33697420c4a 200 int16_t accelX = 0;
Zbyszek 3:e33697420c4a 201 int16_t accelY = 0;
Zbyszek 3:e33697420c4a 202 int16_t accelZ = 0;
Zbyszek 4:e36c7042d3bb 203
Zbyszek 4:e36c7042d3bb 204 float faccelX = 0.0f;
Zbyszek 4:e36c7042d3bb 205 float faccelY = 0.0f;
Zbyszek 4:e36c7042d3bb 206 float faccelZ = 0.0f;
Zbyszek 4:e36c7042d3bb 207
Zbyszek 4:e36c7042d3bb 208 float fgyroX = 0.0f;
Zbyszek 4:e36c7042d3bb 209 float fgyroY = 0.0f;
Zbyszek 4:e36c7042d3bb 210 float fgyroZ = 0.0f;
Zbyszek 4:e36c7042d3bb 211
Zbyszek 3:e33697420c4a 212 //-----------Concatentated-Data-Pieces------------------------------------------
Zbyszek 3:e33697420c4a 213 for(char x = 0; x <= 5; x++) {
Zbyszek 4:e36c7042d3bb 214 MSB = IMUarray[arrPointer]; //Odd data pieces are MSBs
Zbyszek 4:e36c7042d3bb 215 MSB &= ~(255<<8); //Mask the MSB bits as they are not part of data
Zbyszek 4:e36c7042d3bb 216 MSB = MSB << 8; //Shift the Value as its the MSB of the data piece
Zbyszek 4:e36c7042d3bb 217 arrPointer++; //Increment array pointer
Zbyszek 4:e36c7042d3bb 218 LSB = IMUarray[arrPointer]; //Even data pieces are LSBs
Zbyszek 4:e36c7042d3bb 219 LSB &= ~(255 << 8); //Mask the MSB bits as they are not part of data
Zbyszek 4:e36c7042d3bb 220 arrPointer++; //Increment array pointer
Zbyszek 3:e33697420c4a 221
Zbyszek 3:e33697420c4a 222 switch(x) {
Zbyszek 3:e33697420c4a 223 case 0:
Zbyszek 4:e36c7042d3bb 224 accelX = MSB + LSB; //Combine Accelerometer x-axis data
Zbyszek 4:e36c7042d3bb 225 faccelX = (float)accelX * 0.00006103515625f; //Multiply the acceleration by sensitivity scale factor to get it into g 1/16,384
Zbyszek 3:e33697420c4a 226 break;
Zbyszek 3:e33697420c4a 227 case 1:
Zbyszek 4:e36c7042d3bb 228 accelY = MSB + LSB; //Combine Accelerometer y-axis data
Zbyszek 4:e36c7042d3bb 229 faccelY = (float)accelY * 0.00006103515625f; //Multiply the acceleration by sensitivity scale factor to get it into g 1/16,384
Zbyszek 3:e33697420c4a 230 break;
Zbyszek 3:e33697420c4a 231 case 2:
Zbyszek 4:e36c7042d3bb 232 accelZ = MSB + LSB; //Combine Accelerometer z-axis data
Zbyszek 4:e36c7042d3bb 233 faccelZ = (float)accelZ * 0.00006103515625f; //Multiply the acceleration by sensitivity scale factor to get it into g 1/16,384
Zbyszek 3:e33697420c4a 234 break;
Zbyszek 3:e33697420c4a 235 case 3:
Zbyszek 4:e36c7042d3bb 236 gyroX = MSB + LSB; //Combine Gyroscope x-axis data
Zbyszek 4:e36c7042d3bb 237 fgyroX = (float)gyroX * 0.06097560975609756097560975609756f; //Multiply the sample by sensitivity scale factor to get it into degress per second 1/16.4
Zbyszek 4:e36c7042d3bb 238
Zbyszek 3:e33697420c4a 239 break;
Zbyszek 3:e33697420c4a 240 case 4:
Zbyszek 4:e36c7042d3bb 241 gyroY = MSB + LSB; //Combine Gyroscope y-axis data
Zbyszek 4:e36c7042d3bb 242 fgyroY = (float)gyroY * 0.06097560975609756097560975609756f; //Multiply the sample by sensitivity scale factor to get it into degress per second 1/16.4
Zbyszek 4:e36c7042d3bb 243
Zbyszek 3:e33697420c4a 244 break;
Zbyszek 3:e33697420c4a 245 case 5:
Zbyszek 4:e36c7042d3bb 246 gyroZ = MSB + LSB; //Combine Gyroscope z-axis data
Zbyszek 4:e36c7042d3bb 247 fgyroZ = (float)gyroZ * 0.06097560975609756097560975609756f; //Multiply the sample by sensitivity scale factor to get it into degress per second 1/16.4
Zbyszek 3:e33697420c4a 248 break;
Zbyszek 3:e33697420c4a 249 default:
Zbyszek 3:e33697420c4a 250 break;
Zbyszek 3:e33697420c4a 251 }//switch(x)
Zbyszek 3:e33697420c4a 252 }//for(char x = 0; x <= 5; x++)
Zbyszek 4:e36c7042d3bb 253
Zbyszek 4:e36c7042d3bb 254 //Quaternion-Gyro-Integration--------------------------------------------------------------
Zbyszek 4:e36c7042d3bb 255 //Get the Gyro and Accel values--------------------------------------------------------
Zbyszek 4:e36c7042d3bb 256 //Convert these vales to radians per second and store them in the vector
Zbyszek 4:e36c7042d3bb 257 GyroVals.x = fgyroX * 0.0174533;
Zbyszek 4:e36c7042d3bb 258 GyroVals.y = fgyroY * 0.0174533;
Zbyszek 4:e36c7042d3bb 259 GyroVals.z = fgyroZ * 0.0174533;
Zbyszek 4:e36c7042d3bb 260
Zbyszek 4:e36c7042d3bb 261 AccelVals.x = faccelX;
Zbyszek 4:e36c7042d3bb 262 AccelVals.y = faccelY;
Zbyszek 4:e36c7042d3bb 263 AccelVals.z = faccelZ;
Zbyszek 4:e36c7042d3bb 264 //Get the Gyro and Accel values--------------------------------------------------------
Zbyszek 4:e36c7042d3bb 265
Zbyszek 4:e36c7042d3bb 266 gyroQ = updateQuaternion(gyroQ, GyroVals, dTime);
Zbyszek 4:e36c7042d3bb 267 gyroQ = normaliseQuaternion(gyroQ);
Zbyszek 4:e36c7042d3bb 268 Eangles = eulerA(gyroQ);
Zbyszek 4:e36c7042d3bb 269 //Quaternion-Gyro-Integration--------------------------------------------------------------
Zbyszek 4:e36c7042d3bb 270
Zbyszek 4:e36c7042d3bb 271
Zbyszek 4:e36c7042d3bb 272 //Angle-Calculations-Based-on-Accalerometer------------------------------------------------
Zbyszek 4:e36c7042d3bb 273 //https://www.dfrobot.com/wiki/index.php/How_to_Use_a_Three-Axis_Accelerometer_for_Tilt_Sensing
Zbyszek 4:e36c7042d3bb 274
Zbyszek 4:e36c7042d3bb 275 accelTilt.x = (atan2(AccelVals.y, AccelVals.z) * 57.29577951f);
Zbyszek 4:e36c7042d3bb 276 accelTilt.y = (atan2((-AccelVals.x), sqrt(pow(AccelVals.y, 2) + pow(AccelVals.z, 2) ))) * 57.29577951f);
Zbyszek 4:e36c7042d3bb 277 //Canntot calculate accel tilt for Z. Gyro date in combination with magnetometer have to be used to obtain reliable z-axis tilt.
Zbyszek 4:e36c7042d3bb 278 //Angle-Calculations-Based-on-Accalerometer------------------------------------------------
Zbyszek 4:e36c7042d3bb 279
Zbyszek 4:e36c7042d3bb 280 //Add the correction to gyro readings and update the quaternion------------------------
Zbyszek 4:e36c7042d3bb 281 //pc.printf("%+6f, %+6f, %+6f %+6f\n\r ", xx, yy, zz, dTime);
Zbyszek 4:e36c7042d3bb 282 pc.printf("%+6f, %+6f, %+6f, %+6f, %+6f, %+6f, %+6f\n\r ", gyroQ.w, gyroQ.x, gyroQ.y, gyroQ.z, Eangles.x, Eangles.y, Eangles.z);
Zbyszek 4:e36c7042d3bb 283 //pc.printf("%+6f, %+6f, %+6f\n\r ",Eangles.x, Eangles.y, Eangles.z);
Zbyszek 4:e36c7042d3bb 284 //pc.printf("Accel X: %+6f, Accel Y: %+6f, Accel Z: %+6f, Gyro X: %+6f, Gyro Y: %+6f, Gyro Z: %+6f\n\r", faccelX, faccelY, faccelZ, fgyroX, fgyroY, fgyroZ);
Zbyszek 3:e33697420c4a 285
Zbyszek 3:e33697420c4a 286 }//void ProcessAndPrint()
Zbyszek 3:e33697420c4a 287
Zbyszek 4:e36c7042d3bb 288
Zbyszek 4:e36c7042d3bb 289
Zbyszek 4:e36c7042d3bb 290
Zbyszek 4:e36c7042d3bb 291
Zbyszek 4:e36c7042d3bb 292
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 //------------------------------------------Artifacts----------------------------------------
Zbyszek 4:e36c7042d3bb 301
Zbyszek 4:e36c7042d3bb 302 //----------------------Insert Whole---------------------------------------------------
Zbyszek 4:e36c7042d3bb 303 //Rotate the accel data Global reference frame by qvq*---------------------------------
Zbyszek 4:e36c7042d3bb 304 //globalAccel = rotateGlobal(AccelVals, gyroQ);
Zbyszek 4:e36c7042d3bb 305 //Rotate the accel data Global reference frame by qvq*---------------------------------
Zbyszek 4:e36c7042d3bb 306
Zbyszek 4:e36c7042d3bb 307 //get the correction values and rotate back to IMU reference---------------------------
Zbyszek 4:e36c7042d3bb 308 // correctionGlobalAccel = crossProduct(globalAccel, vertical);
Zbyszek 4:e36c7042d3bb 309 // correctionBodyAccel = rotateLocal(correctionGlobalAccel, gyroQ);
Zbyszek 4:e36c7042d3bb 310 //get the correction values and rotate back to IMU reference---------------------------
Zbyszek 4:e36c7042d3bb 311
Zbyszek 4:e36c7042d3bb 312 //Add the correction to gyro readings and update the quaternion------------------------
Zbyszek 4:e36c7042d3bb 313 //GyroVals = sumVector(GyroVals, correctionBodyAccel);
Zbyszek 4:e36c7042d3bb 314 //incRot = toQuaternionNotation123(GyroVals, dTime);
Zbyszek 4:e36c7042d3bb 315 //gyroQ = getQuaternionProduct(gyroQ, incRot);
Zbyszek 4:e36c7042d3bb 316 //gyroQ = normaliseQuaternion(gyroQ);
Zbyszek 4:e36c7042d3bb 317 //----------------------Insert Whole---------------------------------------------------
Zbyszek 4:e36c7042d3bb 318
Zbyszek 4:e36c7042d3bb 319 //------------------------------------------Artifacts----------------------------------------