Zbigniew Druzbacki / Mbed 2 deprecated SPI_Slave

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

Dependencies:   mbed

main.cpp@8:e87027349167, 2019-02-27 (annotated)

Committer:
Zbyszek
Date:
Wed Feb 27 23:35:51 2019 +0000
Revision:
8:e87027349167
Parent:
7:0e9af5986488
Child:
9:9ed9dffd602a
Exporting To Mbed studio

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 6:0ebecfecadc9 3 #include "IMUs.h"
Zbyszek 6:0ebecfecadc9 4 #include "Structures.h"
Zbyszek 4:e36c7042d3bb 5 #include "Quaternions.h"
Zbyszek 8:e87027349167 6 #include "DMA_SPI.h"
Zbyszek 8:e87027349167 7
Zbyszek 0:8e367d6d8f03 8 DigitalOut myled(LED1);
Zbyszek 0:8e367d6d8f03 9 Serial pc(USBTX, USBRX);
Zbyszek 0:8e367d6d8f03 10
Zbyszek 0:8e367d6d8f03 11 int masterRx = 0;
Zbyszek 3:e33697420c4a 12 int16_t slaveRx = 0;
Zbyszek 3:e33697420c4a 13 int i = 2;
Zbyszek 3:e33697420c4a 14 int k = 0;
Zbyszek 3:e33697420c4a 15 int16_t zgHigher = 0;
Zbyszek 3:e33697420c4a 16 int16_t zgLower = 0;
Zbyszek 3:e33697420c4a 17 int16_t zGyro = 0;
Zbyszek 3:e33697420c4a 18 int countx = 0;
Zbyszek 3:e33697420c4a 19 int p = 32776;
Zbyszek 6:0ebecfecadc9 20 double const SSF = 0.06097560975609756097560975609756f; //FSEL = 0: 0.00763358778625954198473282442748, FSEL = 3: 0.06097560975609756097560975609756f
Zbyszek 6:0ebecfecadc9 21
Zbyszek 6:0ebecfecadc9 22 int OffsetX = 254;
Zbyszek 6:0ebecfecadc9 23 int OffsetY = -14;
Zbyszek 6:0ebecfecadc9 24 int OffsetZ = 81;
Zbyszek 6:0ebecfecadc9 25
Zbyszek 6:0ebecfecadc9 26 int OffsetXA = -306;
Zbyszek 6:0ebecfecadc9 27 int OffsetYA = -131;
Zbyszek 6:0ebecfecadc9 28 int OffsetZA = -531;
Zbyszek 6:0ebecfecadc9 29
Zbyszek 6:0ebecfecadc9 30 IMU IMU0 (0, -306.0f, -131.0f, -351.0f, 254.0f, -14.0f, 81.0f, 0, 3);
Zbyszek 7:0e9af5986488 31 IMU_Data Dat;
Zbyszek 7:0e9af5986488 32 vector Datt;
Zbyszek 6:0ebecfecadc9 33
Zbyszek 3:e33697420c4a 34
Zbyszek 4:e36c7042d3bb 35 Timer t;
Zbyszek 4:e36c7042d3bb 36 float dTime = 0.0f;
Zbyszek 4:e36c7042d3bb 37
Zbyszek 4:e36c7042d3bb 38 vector vertical;
Zbyszek 4:e36c7042d3bb 39 vector globalAccel;
Zbyszek 4:e36c7042d3bb 40 vector correctionGlobalAccel;
Zbyszek 4:e36c7042d3bb 41 vector correctionBodyAccel;
Zbyszek 4:e36c7042d3bb 42 Quaternion gyroQ;
Zbyszek 4:e36c7042d3bb 43 vector Eangles;
Zbyszek 5:155d224d855c 44 Quaternion CF;
Zbyszek 5:155d224d855c 45 vector intGyro;
Zbyszek 5:155d224d855c 46 vector GyroVals;
Zbyszek 5:155d224d855c 47 vector AccelVals;
Zbyszek 5:155d224d855c 48 vector accelTilt;
Zbyszek 5:155d224d855c 49 Quaternion accelQ;
Zbyszek 5:155d224d855c 50
Zbyszek 5:155d224d855c 51 void calibrateOffset();
Zbyszek 4:e36c7042d3bb 52
Zbyszek 4:e36c7042d3bb 53
Zbyszek 4:e36c7042d3bb 54 float xx;
Zbyszek 4:e36c7042d3bb 55 float yy;
Zbyszek 4:e36c7042d3bb 56 float zz;
Zbyszek 4:e36c7042d3bb 57
Zbyszek 4:e36c7042d3bb 58
Zbyszek 3:e33697420c4a 59
Zbyszek 3:e33697420c4a 60 /*
Zbyszek 3:e33697420c4a 61
Zbyszek 3:e33697420c4a 62 */
Zbyszek 3:e33697420c4a 63 //------------------Printing-All-values----------------------//
Zbyszek 3:e33697420c4a 64 int16_t IMUarray[12]; //Store each separate reading in an array
Zbyszek 3:e33697420c4a 65 uint16_t IDarray[12]; //Holds the identification of each data piece
Zbyszek 3:e33697420c4a 66 char idx = 0; //IMUarray Pointer
Zbyszek 3:e33697420c4a 67 char dataCount = 0; //Keeps track of how many data points have been read in using SPI
Zbyszek 3:e33697420c4a 68 uint16_t id = 0; //Used to store extracted data ID
Zbyszek 3:e33697420c4a 69
Zbyszek 3:e33697420c4a 70 void ProcessAndPrint();
Zbyszek 3:e33697420c4a 71 //------------------Printing-All-values----------------------//
Zbyszek 3:e33697420c4a 72
Zbyszek 4:e36c7042d3bb 73 //-------------Testing-Variables-Remove-Later----------------//
Zbyszek 4:e36c7042d3bb 74 int blinkCounter = 0;
Zbyszek 4:e36c7042d3bb 75 //-------------Testing-Variables-Remove-Later----------------//
Zbyszek 0:8e367d6d8f03 76
Zbyszek 0:8e367d6d8f03 77 int main() {
Zbyszek 4:e36c7042d3bb 78 pc.baud (115200);
Zbyszek 3:e33697420c4a 79 IDarray[0] = 1;
Zbyszek 3:e33697420c4a 80 IDarray[1] = 0;
Zbyszek 3:e33697420c4a 81 IDarray[2] = 9;
Zbyszek 3:e33697420c4a 82 IDarray[3] = 8;
Zbyszek 3:e33697420c4a 83 IDarray[4] = 17;
Zbyszek 3:e33697420c4a 84 IDarray[5] = 16;
Zbyszek 3:e33697420c4a 85 IDarray[6] = 3;
Zbyszek 4:e36c7042d3bb 86 IDarray[7] = 2; //2
Zbyszek 3:e33697420c4a 87 IDarray[8] = 11;
Zbyszek 3:e33697420c4a 88 IDarray[9] = 10;
Zbyszek 3:e33697420c4a 89 IDarray[10] = 19;
Zbyszek 3:e33697420c4a 90 IDarray[11] = 18;
Zbyszek 3:e33697420c4a 91
Zbyszek 4:e36c7042d3bb 92 init_spi1();
Zbyszek 4:e36c7042d3bb 93 t.start();
Zbyszek 3:e33697420c4a 94
Zbyszek 4:e36c7042d3bb 95 gyroQ.w = 1;
Zbyszek 4:e36c7042d3bb 96 gyroQ.x = 0.0001;
Zbyszek 4:e36c7042d3bb 97 gyroQ.y = 0.0001;
Zbyszek 4:e36c7042d3bb 98 gyroQ.z = 0.0001;
Zbyszek 4:e36c7042d3bb 99
Zbyszek 5:155d224d855c 100 CF.w = 1;
Zbyszek 5:155d224d855c 101 CF.x = 0.0001;
Zbyszek 5:155d224d855c 102 CF.y = 0.0001;
Zbyszek 5:155d224d855c 103 CF.z = 0.0001;
Zbyszek 4:e36c7042d3bb 104
Zbyszek 4:e36c7042d3bb 105
Zbyszek 4:e36c7042d3bb 106 gyroQ = normaliseQuaternion(gyroQ);
Zbyszek 5:155d224d855c 107 CF = normaliseQuaternion(CF);
Zbyszek 4:e36c7042d3bb 108
Zbyszek 4:e36c7042d3bb 109 vertical.x = 0.0f;
Zbyszek 4:e36c7042d3bb 110 vertical.y = 0.0f;
Zbyszek 4:e36c7042d3bb 111 vertical.z = 1.0f;
Zbyszek 4:e36c7042d3bb 112
Zbyszek 3:e33697420c4a 113 pc.printf("Begin \n\r");
Zbyszek 0:8e367d6d8f03 114
Zbyszek 0:8e367d6d8f03 115 while(1) {
Zbyszek 3:e33697420c4a 116 if(i == 1) {
Zbyszek 0:8e367d6d8f03 117 for(int x = 1; x < 128; x *= 2) {
Zbyszek 0:8e367d6d8f03 118 slaveRx = transfer_spi_slave(x);
Zbyszek 3:e33697420c4a 119 //pc.putc(slaveRx);
Zbyszek 3:e33697420c4a 120 pc.printf("%d \n\r",slaveRx);
Zbyszek 0:8e367d6d8f03 121 }
Zbyszek 0:8e367d6d8f03 122 for(int x = 128; x > 1; x /= 2) {
Zbyszek 0:8e367d6d8f03 123 slaveRx = transfer_spi_slave(x);
Zbyszek 3:e33697420c4a 124 //pc.putc(slaveRx);
Zbyszek 3:e33697420c4a 125 pc.printf("%d \n\r",slaveRx);
Zbyszek 2:4cc880ea466d 126 }
Zbyszek 3:e33697420c4a 127 }//if(i == 1)
Zbyszek 3:e33697420c4a 128
Zbyszek 3:e33697420c4a 129
Zbyszek 3:e33697420c4a 130
Zbyszek 3:e33697420c4a 131 //------------------------------------------------------------------------------
Zbyszek 3:e33697420c4a 132 if(i == 0) {
Zbyszek 3:e33697420c4a 133 slaveRx = transfer_spi_slave(10);
Zbyszek 3:e33697420c4a 134 countx++;
Zbyszek 3:e33697420c4a 135 if(countx == 1) {
Zbyszek 3:e33697420c4a 136 k = slaveRx;
Zbyszek 3:e33697420c4a 137 k &= ~(255 << 8);
Zbyszek 3:e33697420c4a 138 k = k << 8;
Zbyszek 3:e33697420c4a 139 // if(k == 19) {
Zbyszek 3:e33697420c4a 140 zgHigher = k;
Zbyszek 3:e33697420c4a 141 //zgHigher = zgHigher << 8;
Zbyszek 3:e33697420c4a 142 //}//if(k == 19)
Zbyszek 3:e33697420c4a 143 }//if(count == 11)
Zbyszek 3:e33697420c4a 144
Zbyszek 3:e33697420c4a 145
Zbyszek 3:e33697420c4a 146 else if(countx == 2) {
Zbyszek 3:e33697420c4a 147 k = slaveRx;
Zbyszek 3:e33697420c4a 148 k &= ~(255 << 8);
Zbyszek 3:e33697420c4a 149 //k = k << 8;
Zbyszek 3:e33697420c4a 150 //if(k == 18) {
Zbyszek 3:e33697420c4a 151 zgLower = k;
Zbyszek 3:e33697420c4a 152 //zgLower = zgLower << 8;
Zbyszek 3:e33697420c4a 153 zGyro = zgHigher + zgLower;
Zbyszek 3:e33697420c4a 154 pc.printf("%+d \n\r",zGyro);
Zbyszek 3:e33697420c4a 155 //}//if(k == 19)
Zbyszek 3:e33697420c4a 156 countx = 0;
Zbyszek 3:e33697420c4a 157 }//else if(count == 12)
Zbyszek 3:e33697420c4a 158
Zbyszek 3:e33697420c4a 159 }//if(i == 0)
Zbyszek 3:e33697420c4a 160 //------------------------------------------------------------------------------
Zbyszek 3:e33697420c4a 161
Zbyszek 3:e33697420c4a 162 if(i == 2) {
Zbyszek 4:e36c7042d3bb 163 slaveRx = transfer_spi_slave(10); //get IMU data
Zbyszek 4:e36c7042d3bb 164
Zbyszek 4:e36c7042d3bb 165
Zbyszek 4:e36c7042d3bb 166 id = slaveRx; //Save sample to id for id extraction
Zbyszek 4:e36c7042d3bb 167 id &= ~(255); //get rid of the actual data to only be left with the id
Zbyszek 4:e36c7042d3bb 168 id = id >> 8; //shift the id to the right for comparison
Zbyszek 4:e36c7042d3bb 169
Zbyszek 4:e36c7042d3bb 170 //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 171
Zbyszek 4:e36c7042d3bb 172 if(IDarray[dataCount] == id) { //compare if the order of data is right and if not wait until it is.
Zbyszek 4:e36c7042d3bb 173 dataCount++; //Increase dataCount as new value has been read in.
Zbyszek 4:e36c7042d3bb 174 IMUarray[idx] = slaveRx; //save the newly read value to current free space in the array
Zbyszek 4:e36c7042d3bb 175 idx++; //increment the pointer to point to next free space in the array
Zbyszek 3:e33697420c4a 176
Zbyszek 3:e33697420c4a 177 if(dataCount == 12) {
Zbyszek 3:e33697420c4a 178 //reset idx and dataCount
Zbyszek 3:e33697420c4a 179 dataCount = 0;
Zbyszek 3:e33697420c4a 180 idx = 0;
Zbyszek 6:0ebecfecadc9 181 //ProcessAndPrint();
Zbyszek 6:0ebecfecadc9 182 //calibrateOffset();
Zbyszek 7:0e9af5986488 183 //IMU0.concatenateData(IMUarray);
Zbyszek 7:0e9af5986488 184 Datt = IMU0.CalculateQCFAngles(IMUarray);
Zbyszek 7:0e9af5986488 185
Zbyszek 6:0ebecfecadc9 186
Zbyszek 4:e36c7042d3bb 187 t.stop();
Zbyszek 4:e36c7042d3bb 188 dTime = t.read();
Zbyszek 4:e36c7042d3bb 189 t.reset();
Zbyszek 4:e36c7042d3bb 190 t.start();
Zbyszek 3:e33697420c4a 191 }//if(dataCount == 12)
Zbyszek 3:e33697420c4a 192 }//if(IDarray[dataCount] == id)
Zbyszek 4:e36c7042d3bb 193 else {
Zbyszek 4:e36c7042d3bb 194 //-----Code-Used-For-Testing-----//
Zbyszek 4:e36c7042d3bb 195 //pc.printf("Failed at: %d \n\r", dataCount); //Print an error if there is one
Zbyszek 4:e36c7042d3bb 196 if(blinkCounter == 10) { //Slow the blinking down to make it visible if there are errors
Zbyszek 4:e36c7042d3bb 197 myled = !myled; //Change state of the LED if error occurs
Zbyszek 4:e36c7042d3bb 198 blinkCounter = 0; //Reset Blink counter
Zbyszek 4:e36c7042d3bb 199 }
Zbyszek 4:e36c7042d3bb 200 else {
Zbyszek 4:e36c7042d3bb 201 blinkCounter++;
Zbyszek 4:e36c7042d3bb 202 }
Zbyszek 4:e36c7042d3bb 203 //-----Code-Used-For-Testing-----//
Zbyszek 4:e36c7042d3bb 204
Zbyszek 4:e36c7042d3bb 205 dataCount = 0; //ID sequence is worng so reset the counter
Zbyszek 4:e36c7042d3bb 206 idx = 0; //ID sequence is worng so reset the counter
Zbyszek 4:e36c7042d3bb 207 }
Zbyszek 3:e33697420c4a 208 }//if(i == 2)
Zbyszek 3:e33697420c4a 209
Zbyszek 3:e33697420c4a 210
Zbyszek 3:e33697420c4a 211
Zbyszek 0:8e367d6d8f03 212 }
Zbyszek 0:8e367d6d8f03 213 }
Zbyszek 3:e33697420c4a 214
Zbyszek 3:e33697420c4a 215
Zbyszek 3:e33697420c4a 216
Zbyszek 4:e36c7042d3bb 217
Zbyszek 4:e36c7042d3bb 218 /*
Zbyszek 4:e36c7042d3bb 219 -The purpose of this function
Zbyszek 4:e36c7042d3bb 220 */
Zbyszek 3:e33697420c4a 221 void ProcessAndPrint() {
Zbyszek 3:e33697420c4a 222 int16_t MSB = 0; //Store Most Significant Byte of data piece in this variable for processing
Zbyszek 3:e33697420c4a 223 int16_t LSB = 0; //Store Least Significant Byte of data piece in this variable for processing
Zbyszek 3:e33697420c4a 224 char arrPointer = 0; //Array Pointer
Zbyszek 4:e36c7042d3bb 225
Zbyszek 3:e33697420c4a 226 //-----------Concatentated-Data-Pieces------------------------------------------
Zbyszek 3:e33697420c4a 227 int16_t gyroX = 0;
Zbyszek 3:e33697420c4a 228 int16_t gyroY = 0;
Zbyszek 3:e33697420c4a 229 int16_t gyroZ = 0;
Zbyszek 3:e33697420c4a 230
Zbyszek 3:e33697420c4a 231 int16_t accelX = 0;
Zbyszek 3:e33697420c4a 232 int16_t accelY = 0;
Zbyszek 3:e33697420c4a 233 int16_t accelZ = 0;
Zbyszek 4:e36c7042d3bb 234
Zbyszek 4:e36c7042d3bb 235 float faccelX = 0.0f;
Zbyszek 4:e36c7042d3bb 236 float faccelY = 0.0f;
Zbyszek 4:e36c7042d3bb 237 float faccelZ = 0.0f;
Zbyszek 4:e36c7042d3bb 238
Zbyszek 4:e36c7042d3bb 239 float fgyroX = 0.0f;
Zbyszek 4:e36c7042d3bb 240 float fgyroY = 0.0f;
Zbyszek 4:e36c7042d3bb 241 float fgyroZ = 0.0f;
Zbyszek 4:e36c7042d3bb 242
Zbyszek 3:e33697420c4a 243 //-----------Concatentated-Data-Pieces------------------------------------------
Zbyszek 3:e33697420c4a 244 for(char x = 0; x <= 5; x++) {
Zbyszek 4:e36c7042d3bb 245 MSB = IMUarray[arrPointer]; //Odd data pieces are MSBs
Zbyszek 4:e36c7042d3bb 246 MSB &= ~(255<<8); //Mask the MSB bits as they are not part of data
Zbyszek 4:e36c7042d3bb 247 MSB = MSB << 8; //Shift the Value as its the MSB of the data piece
Zbyszek 4:e36c7042d3bb 248 arrPointer++; //Increment array pointer
Zbyszek 4:e36c7042d3bb 249 LSB = IMUarray[arrPointer]; //Even data pieces are LSBs
Zbyszek 4:e36c7042d3bb 250 LSB &= ~(255 << 8); //Mask the MSB bits as they are not part of data
Zbyszek 4:e36c7042d3bb 251 arrPointer++; //Increment array pointer
Zbyszek 3:e33697420c4a 252
Zbyszek 3:e33697420c4a 253 switch(x) {
Zbyszek 3:e33697420c4a 254 case 0:
Zbyszek 6:0ebecfecadc9 255 accelX = (MSB + LSB) + OffsetXA; //Combine Accelerometer x-axis data
Zbyszek 4:e36c7042d3bb 256 faccelX = (float)accelX * 0.00006103515625f; //Multiply the acceleration by sensitivity scale factor to get it into g 1/16,384
Zbyszek 3:e33697420c4a 257 break;
Zbyszek 3:e33697420c4a 258 case 1:
Zbyszek 6:0ebecfecadc9 259 accelY = (MSB + LSB) + OffsetYA; //Combine Accelerometer y-axis data
Zbyszek 4:e36c7042d3bb 260 faccelY = (float)accelY * 0.00006103515625f; //Multiply the acceleration by sensitivity scale factor to get it into g 1/16,384
Zbyszek 3:e33697420c4a 261 break;
Zbyszek 3:e33697420c4a 262 case 2:
Zbyszek 6:0ebecfecadc9 263 accelZ = (MSB + LSB) + OffsetZA; //Combine Accelerometer z-axis data
Zbyszek 4:e36c7042d3bb 264 faccelZ = (float)accelZ * 0.00006103515625f; //Multiply the acceleration by sensitivity scale factor to get it into g 1/16,384
Zbyszek 3:e33697420c4a 265 break;
Zbyszek 3:e33697420c4a 266 case 3:
Zbyszek 6:0ebecfecadc9 267 gyroX = (MSB + LSB) + OffsetX; //Combine Gyroscope x-axis data
Zbyszek 5:155d224d855c 268 fgyroX = (float)gyroX * SSF; //Multiply the sample by sensitivity scale factor to get it into degress per second 1/16.4
Zbyszek 4:e36c7042d3bb 269
Zbyszek 3:e33697420c4a 270 break;
Zbyszek 3:e33697420c4a 271 case 4:
Zbyszek 6:0ebecfecadc9 272 gyroY = (MSB + LSB) + OffsetY; //Combine Gyroscope y-axis data
Zbyszek 5:155d224d855c 273 fgyroY = (float)gyroY * SSF; //Multiply the sample by sensitivity scale factor to get it into degress per second 1/16.4
Zbyszek 4:e36c7042d3bb 274
Zbyszek 3:e33697420c4a 275 break;
Zbyszek 3:e33697420c4a 276 case 5:
Zbyszek 6:0ebecfecadc9 277 gyroZ = (MSB + LSB) + OffsetZ; //Combine Gyroscope z-axis data
Zbyszek 5:155d224d855c 278 fgyroZ = (float)gyroZ * SSF; //Multiply the sample by sensitivity scale factor to get it into degress per second 1/16.4
Zbyszek 3:e33697420c4a 279 break;
Zbyszek 3:e33697420c4a 280 default:
Zbyszek 3:e33697420c4a 281 break;
Zbyszek 3:e33697420c4a 282 }//switch(x)
Zbyszek 3:e33697420c4a 283 }//for(char x = 0; x <= 5; x++)
Zbyszek 4:e36c7042d3bb 284
Zbyszek 4:e36c7042d3bb 285 //Quaternion-Gyro-Integration--------------------------------------------------------------
Zbyszek 4:e36c7042d3bb 286 //Get the Gyro and Accel values--------------------------------------------------------
Zbyszek 4:e36c7042d3bb 287 //Convert these vales to radians per second and store them in the vector
Zbyszek 6:0ebecfecadc9 288 GyroVals.x = fgyroX * 0.0174533f;
Zbyszek 6:0ebecfecadc9 289 GyroVals.y = fgyroY * 0.0174533f;
Zbyszek 6:0ebecfecadc9 290 GyroVals.z = fgyroZ * 0.0174533f;
Zbyszek 4:e36c7042d3bb 291
Zbyszek 5:155d224d855c 292 intGyro.x = (fgyroX * dTime);
Zbyszek 5:155d224d855c 293 intGyro.y = (fgyroY * dTime);
Zbyszek 5:155d224d855c 294 intGyro.z = (fgyroZ * dTime);
Zbyszek 5:155d224d855c 295
Zbyszek 4:e36c7042d3bb 296 AccelVals.x = faccelX;
Zbyszek 4:e36c7042d3bb 297 AccelVals.y = faccelY;
Zbyszek 4:e36c7042d3bb 298 AccelVals.z = faccelZ;
Zbyszek 4:e36c7042d3bb 299 //Get the Gyro and Accel values--------------------------------------------------------
Zbyszek 4:e36c7042d3bb 300
Zbyszek 6:0ebecfecadc9 301 //CF = updateQuaternion(CF, GyroVals, dTime);
Zbyszek 6:0ebecfecadc9 302 //CF = normaliseQuaternion(CF);
Zbyszek 6:0ebecfecadc9 303 //Eangles = eulerA(CF);
Zbyszek 4:e36c7042d3bb 304 //Quaternion-Gyro-Integration--------------------------------------------------------------
Zbyszek 4:e36c7042d3bb 305
Zbyszek 4:e36c7042d3bb 306
Zbyszek 4:e36c7042d3bb 307 //Angle-Calculations-Based-on-Accalerometer------------------------------------------------
Zbyszek 4:e36c7042d3bb 308 //https://www.dfrobot.com/wiki/index.php/How_to_Use_a_Three-Axis_Accelerometer_for_Tilt_Sensing
Zbyszek 4:e36c7042d3bb 309
Zbyszek 5:155d224d855c 310 accelTilt.x = (atan2f(AccelVals.y, AccelVals.z) * 57.29577951f);
Zbyszek 5:155d224d855c 311 accelTilt.y = (atan2f((-AccelVals.x), sqrt(pow(AccelVals.y, 2) + pow(AccelVals.z, 2) )) * 57.29577951f);
Zbyszek 5:155d224d855c 312 //pc.printf("%+6f, %+6f ", accelTilt.x, accelTilt.y);
Zbyszek 4:e36c7042d3bb 313 //Canntot calculate accel tilt for Z. Gyro date in combination with magnetometer have to be used to obtain reliable z-axis tilt.
Zbyszek 5:155d224d855c 314 //Angle-Calculations-Based-on-Accalerometer------------------------------------------------
Zbyszek 5:155d224d855c 315 //pc.printf("%+6f, %+6f, %+6f ", CF.x, CF.y, CF.z);
Zbyszek 5:155d224d855c 316
Zbyszek 5:155d224d855c 317
Zbyszek 5:155d224d855c 318 //Complementary-Filter---------------------------------------------------------------------
Zbyszek 6:0ebecfecadc9 319 //CF.x = 0.98f*(Eangles.x) + 0.02f*accelTilt.x;
Zbyszek 6:0ebecfecadc9 320 //CF.y = 0.98f*(Eangles.y) + 0.02f*accelTilt.y;
Zbyszek 6:0ebecfecadc9 321 //CF.z = 0.98f*(Eangles.z) + 0.02f*accelTilt.z;
Zbyszek 5:155d224d855c 322
Zbyszek 6:0ebecfecadc9 323 CF.x = 0.98f*(CF.x + intGyro.x) + 0.02f*accelTilt.x;
Zbyszek 6:0ebecfecadc9 324 CF.y = 0.98f*(CF.y + intGyro.y) + 0.02f*accelTilt.y;
Zbyszek 6:0ebecfecadc9 325 CF.z = 0.98f*(CF.z + intGyro.z) + 0.02f*accelTilt.z;
Zbyszek 5:155d224d855c 326 //Complementary-Filter---------------------------------------------------------------------
Zbyszek 4:e36c7042d3bb 327
Zbyszek 4:e36c7042d3bb 328 //Add the correction to gyro readings and update the quaternion------------------------
Zbyszek 4:e36c7042d3bb 329 //pc.printf("%+6f, %+6f, %+6f %+6f\n\r ", xx, yy, zz, dTime);
Zbyszek 5:155d224d855c 330 //pc.printf("%+6f, %+6f, %+6f, %+6f, %+6f, %+6f, %+6f\n\r ", gyroQ.w, gyroQ.x, gyroQ.y, gyroQ.z, CF.x, CF.y, Eangles.z);
Zbyszek 6:0ebecfecadc9 331 //pc.printf("%+6f, %+6f, %+6f \n\r", CF.x, CF.y, CF.z);
Zbyszek 6:0ebecfecadc9 332 //CF.x *= 0.0174533f;
Zbyszek 6:0ebecfecadc9 333 //CF.y *= 0.0174533f;
Zbyszek 6:0ebecfecadc9 334 // CF.z *= 0.0174533f;
Zbyszek 6:0ebecfecadc9 335 //CF = euler2Quaternion(CF);
Zbyszek 6:0ebecfecadc9 336 //CF = normaliseQuaternion(CF);
Zbyszek 4:e36c7042d3bb 337 //pc.printf("%+6f, %+6f, %+6f\n\r ",Eangles.x, Eangles.y, Eangles.z);
Zbyszek 5:155d224d855c 338 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 6:0ebecfecadc9 339 //pc.printf("Accel X: %+6f, Accel Y: %+6f, Accel Z: %+6f, Gyro X: %+6d, Gyro Y: %+6d, Gyro Z: %+6d\n\r", faccelX, faccelY, faccelZ, gyroX, gyroY, gyroZ);
Zbyszek 3:e33697420c4a 340
Zbyszek 3:e33697420c4a 341 }//void ProcessAndPrint()
Zbyszek 3:e33697420c4a 342
Zbyszek 4:e36c7042d3bb 343
Zbyszek 4:e36c7042d3bb 344
Zbyszek 5:155d224d855c 345 void calibrateOffset() {
Zbyszek 5:155d224d855c 346
Zbyszek 5:155d224d855c 347 int16_t MSB = 0; //Store Most Significant Byte of data piece in this variable for processing
Zbyszek 5:155d224d855c 348 int16_t LSB = 0; //Store Least Significant Byte of data piece in this variable for processing
Zbyszek 5:155d224d855c 349 char arrPointer = 0; //Array Pointer
Zbyszek 5:155d224d855c 350
Zbyszek 5:155d224d855c 351 //-----------Concatentated-Data-Pieces------------------------------------------
Zbyszek 5:155d224d855c 352 int16_t gyroX = 0;
Zbyszek 5:155d224d855c 353 int16_t gyroY = 0;
Zbyszek 5:155d224d855c 354 int16_t gyroZ = 0;
Zbyszek 5:155d224d855c 355
Zbyszek 5:155d224d855c 356 int16_t accelX = 0;
Zbyszek 5:155d224d855c 357 int16_t accelY = 0;
Zbyszek 5:155d224d855c 358 int16_t accelZ = 0;
Zbyszek 5:155d224d855c 359
Zbyszek 5:155d224d855c 360 vector accelRaw;
Zbyszek 5:155d224d855c 361 vector accelG;
Zbyszek 5:155d224d855c 362 vector gyroRaw;
Zbyszek 5:155d224d855c 363 vector gyroDPS;
Zbyszek 5:155d224d855c 364
Zbyszek 5:155d224d855c 365 static unsigned int sampleCounter = 1;
Zbyszek 5:155d224d855c 366 static vector accelRawAvg;
Zbyszek 5:155d224d855c 367 static vector accelGAvg;
Zbyszek 5:155d224d855c 368 static vector gyroRawAvg;
Zbyszek 5:155d224d855c 369 static vector gyroDPSAvg;
Zbyszek 5:155d224d855c 370
Zbyszek 5:155d224d855c 371
Zbyszek 5:155d224d855c 372 for(char x = 0; x <= 5; x++) {
Zbyszek 5:155d224d855c 373 MSB = IMUarray[arrPointer]; //Odd data pieces are MSBs
Zbyszek 5:155d224d855c 374 MSB &= ~(255<<8); //Mask the MSB bits as they are not part of data
Zbyszek 5:155d224d855c 375 MSB = MSB << 8; //Shift the Value as its the MSB of the data piece
Zbyszek 5:155d224d855c 376 arrPointer++; //Increment array pointer
Zbyszek 5:155d224d855c 377 LSB = IMUarray[arrPointer]; //Even data pieces are LSBs
Zbyszek 5:155d224d855c 378 LSB &= ~(255 << 8); //Mask the MSB bits as they are not part of data
Zbyszek 5:155d224d855c 379 arrPointer++; //Increment array pointer
Zbyszek 5:155d224d855c 380
Zbyszek 5:155d224d855c 381 switch(x) {
Zbyszek 5:155d224d855c 382 case 0:
Zbyszek 5:155d224d855c 383 accelX = MSB + LSB; //Combine Accelerometer x-axis data
Zbyszek 5:155d224d855c 384 accelRaw.x = (double)accelX; //accelRaw
Zbyszek 5:155d224d855c 385 accelG.x = (double)accelX * 0.00006103515625f; //accelSSF
Zbyszek 5:155d224d855c 386
Zbyszek 5:155d224d855c 387 break;
Zbyszek 5:155d224d855c 388 case 1:
Zbyszek 5:155d224d855c 389 accelY = MSB + LSB; //Combine Accelerometer y-axis data
Zbyszek 5:155d224d855c 390 accelRaw.y = (double)accelY;
Zbyszek 5:155d224d855c 391 accelG.y = (double)accelY * 0.00006103515625f;
Zbyszek 5:155d224d855c 392 break;
Zbyszek 5:155d224d855c 393 case 2:
Zbyszek 5:155d224d855c 394 accelZ = MSB + LSB; //Combine Accelerometer z-axis data
Zbyszek 5:155d224d855c 395 accelRaw.z = (double)accelZ;
Zbyszek 5:155d224d855c 396 accelG.z = (double)accelZ * 0.00006103515625f;
Zbyszek 5:155d224d855c 397 break;
Zbyszek 5:155d224d855c 398 case 3:
Zbyszek 5:155d224d855c 399 gyroX = MSB + LSB; //Combine Gyroscope x-axis data
Zbyszek 5:155d224d855c 400 gyroRaw.x = (double)gyroX; //gyroRaw
Zbyszek 5:155d224d855c 401 gyroDPS.x = (double)gyroX * SSF; //gyroSSF
Zbyszek 5:155d224d855c 402
Zbyszek 5:155d224d855c 403 break;
Zbyszek 5:155d224d855c 404 case 4:
Zbyszek 5:155d224d855c 405 gyroY = MSB + LSB; //Combine Gyroscope y-axis data
Zbyszek 5:155d224d855c 406 gyroRaw.y = (double)gyroY;
Zbyszek 5:155d224d855c 407 gyroDPS.y = (double)gyroY * SSF;
Zbyszek 4:e36c7042d3bb 408
Zbyszek 5:155d224d855c 409 break;
Zbyszek 5:155d224d855c 410 case 5:
Zbyszek 5:155d224d855c 411 gyroZ = MSB + LSB; //Combine Gyroscope z-axis data
Zbyszek 5:155d224d855c 412 gyroRaw.z = (double)gyroZ;
Zbyszek 5:155d224d855c 413 gyroDPS.z = (double)gyroZ * SSF;
Zbyszek 5:155d224d855c 414 break;
Zbyszek 5:155d224d855c 415 default:
Zbyszek 5:155d224d855c 416 break;
Zbyszek 5:155d224d855c 417 }//switch(x)
Zbyszek 5:155d224d855c 418 }//for(char x = 0; x <= 5; x++)
Zbyszek 5:155d224d855c 419
Zbyszek 5:155d224d855c 420 //Take-Running-Averages------------------------------------------------------------------------
Zbyszek 5:155d224d855c 421 //Raw accel averages
Zbyszek 5:155d224d855c 422 accelRawAvg.x = accelRawAvg.x + ((accelRaw.x - accelRawAvg.x)/sampleCounter);
Zbyszek 5:155d224d855c 423 accelRawAvg.y = accelRawAvg.y + ((accelRaw.y - accelRawAvg.y)/sampleCounter);
Zbyszek 5:155d224d855c 424 accelRawAvg.z = accelRawAvg.z + ((accelRaw.z - accelRawAvg.z)/sampleCounter);
Zbyszek 5:155d224d855c 425
Zbyszek 5:155d224d855c 426 //SSF accel averages
Zbyszek 5:155d224d855c 427 accelGAvg.x = accelGAvg.x + ((accelG.x - accelGAvg.x)/sampleCounter);
Zbyszek 5:155d224d855c 428 accelGAvg.y = accelGAvg.y + ((accelG.y - accelGAvg.y)/sampleCounter);
Zbyszek 5:155d224d855c 429 accelGAvg.z = accelGAvg.z + ((accelG.z - accelGAvg.z)/sampleCounter);
Zbyszek 5:155d224d855c 430
Zbyszek 5:155d224d855c 431 //Raw gyroo averages
Zbyszek 5:155d224d855c 432 gyroRawAvg.x = gyroRawAvg.x + ((gyroRaw.x - gyroRawAvg.x)/sampleCounter);
Zbyszek 5:155d224d855c 433 gyroRawAvg.y = gyroRawAvg.y + ((gyroRaw.y - gyroRawAvg.y)/sampleCounter);
Zbyszek 5:155d224d855c 434 gyroRawAvg.z = gyroRawAvg.z + ((gyroRaw.z - gyroRawAvg.z)/sampleCounter);
Zbyszek 5:155d224d855c 435
Zbyszek 5:155d224d855c 436 //SSF gyro averages
Zbyszek 5:155d224d855c 437 gyroDPSAvg.x = gyroDPSAvg.x + ((gyroDPS.x - gyroDPSAvg.x)/sampleCounter);
Zbyszek 5:155d224d855c 438 gyroDPSAvg.y = gyroDPSAvg.y + ((gyroDPS.y - gyroDPSAvg.y)/sampleCounter);
Zbyszek 5:155d224d855c 439 gyroDPSAvg.z = gyroDPSAvg.z + ((gyroDPS.z - gyroDPSAvg.z)/sampleCounter);
Zbyszek 5:155d224d855c 440 //Take-Running-Averages------------------------------------------------------------------------
Zbyszek 4:e36c7042d3bb 441
Zbyszek 5:155d224d855c 442 //Print Messages-------------------------------------------------------------------------------
Zbyszek 5:155d224d855c 443 if(sampleCounter == 1) {
Zbyszek 5:155d224d855c 444 pc.printf("Collecting Raw and Sensitivity Scale Factor multiplied Gyroscope and Accelerometer Offsets...\n\r");
Zbyszek 5:155d224d855c 445 };
Zbyszek 5:155d224d855c 446
Zbyszek 5:155d224d855c 447
Zbyszek 5:155d224d855c 448 if(sampleCounter == 5000) {
Zbyszek 5:155d224d855c 449 pc.printf("RawAX RawAY RawAZ RawGX RawGY RawGZ SampleN\n\r");
Zbyszek 5:155d224d855c 450 pc.printf("%+-6.2f %+-6.2f %+-6.2f %+-6.2f %+-6.2f %+-6.2f %-10d\n\r", accelRawAvg.x, accelRawAvg.y, accelRawAvg.z, gyroRawAvg.x, gyroRawAvg.y, gyroRawAvg.z, sampleCounter);
Zbyszek 5:155d224d855c 451 pc.printf("\n\r");
Zbyszek 5:155d224d855c 452 pc.printf("\n\r");
Zbyszek 5:155d224d855c 453 pc.printf("\n\r");
Zbyszek 5:155d224d855c 454
Zbyszek 5:155d224d855c 455 //Add the sensitivity scale factor multiplied data
Zbyszek 5:155d224d855c 456 pc.printf("SSFAX SSFAY SSFAZ SSFGX SSFGY SSFGZ SampleN\n\r");
Zbyszek 5:155d224d855c 457 pc.printf("%+-6.2f %+-6.2f %+-6.2f %+-6.2f %+-6.2f %+-6.2f %-10d\n\r", accelGAvg.x, accelGAvg.y, accelGAvg.z, gyroDPSAvg.x, gyroDPSAvg.y, gyroDPSAvg.z, sampleCounter);
Zbyszek 5:155d224d855c 458
Zbyszek 5:155d224d855c 459 };
Zbyszek 5:155d224d855c 460 sampleCounter++;
Zbyszek 5:155d224d855c 461 //Print Messages-------------------------------------------------------------------------------
Zbyszek 5:155d224d855c 462 }
Zbyszek 4:e36c7042d3bb 463
Zbyszek 4:e36c7042d3bb 464
Zbyszek 4:e36c7042d3bb 465
Zbyszek 4:e36c7042d3bb 466
Zbyszek 4:e36c7042d3bb 467
Zbyszek 4:e36c7042d3bb 468
Zbyszek 4:e36c7042d3bb 469
Zbyszek 4:e36c7042d3bb 470
Zbyszek 4:e36c7042d3bb 471 //------------------------------------------Artifacts----------------------------------------
Zbyszek 4:e36c7042d3bb 472
Zbyszek 4:e36c7042d3bb 473 //----------------------Insert Whole---------------------------------------------------
Zbyszek 4:e36c7042d3bb 474 //Rotate the accel data Global reference frame by qvq*---------------------------------
Zbyszek 4:e36c7042d3bb 475 //globalAccel = rotateGlobal(AccelVals, gyroQ);
Zbyszek 4:e36c7042d3bb 476 //Rotate the accel data Global reference frame by qvq*---------------------------------
Zbyszek 4:e36c7042d3bb 477
Zbyszek 4:e36c7042d3bb 478 //get the correction values and rotate back to IMU reference---------------------------
Zbyszek 4:e36c7042d3bb 479 // correctionGlobalAccel = crossProduct(globalAccel, vertical);
Zbyszek 4:e36c7042d3bb 480 // correctionBodyAccel = rotateLocal(correctionGlobalAccel, gyroQ);
Zbyszek 4:e36c7042d3bb 481 //get the correction values and rotate back to IMU reference---------------------------
Zbyszek 4:e36c7042d3bb 482
Zbyszek 4:e36c7042d3bb 483 //Add the correction to gyro readings and update the quaternion------------------------
Zbyszek 4:e36c7042d3bb 484 //GyroVals = sumVector(GyroVals, correctionBodyAccel);
Zbyszek 4:e36c7042d3bb 485 //incRot = toQuaternionNotation123(GyroVals, dTime);
Zbyszek 4:e36c7042d3bb 486 //gyroQ = getQuaternionProduct(gyroQ, incRot);
Zbyszek 4:e36c7042d3bb 487 //gyroQ = normaliseQuaternion(gyroQ);
Zbyszek 4:e36c7042d3bb 488 //----------------------Insert Whole---------------------------------------------------
Zbyszek 4:e36c7042d3bb 489
Zbyszek 4:e36c7042d3bb 490 //------------------------------------------Artifacts----------------------------------------