Zbigniew Druzbacki / Mbed 2 deprecated SPI_Slave

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

Dependencies:   mbed

main.cpp@11:366f1186c121, 2019-03-19 (annotated)

Committer:
Zbyszek
Date:
Tue Mar 19 01:26:11 2019 +0000
Revision:
11:366f1186c121
Parent:
10:5b96211275d4
Child:
13:c7e8e277f884
Several IMUs can now be read using STM32L432.

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 11:366f1186c121 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 11:366f1186c121 30 IMU IMU0 (0, -306.0f, -131.0f, -351.0f, 254.0f, -14.0f, 81.0f, 0, 0);
Zbyszek 11:366f1186c121 31 IMU IMU1 (0, -306.0f, -131.0f, -351.0f, 254.0f, -14.0f, 81.0f, 0, 0);
Zbyszek 7:0e9af5986488 32 IMU_Data Dat;
Zbyszek 11:366f1186c121 33 vector Datt, Datt1;
Zbyszek 10:5b96211275d4 34 int D2T;
Zbyszek 6:0ebecfecadc9 35
Zbyszek 3:e33697420c4a 36
Zbyszek 4:e36c7042d3bb 37 Timer t;
Zbyszek 4:e36c7042d3bb 38 float dTime = 0.0f;
Zbyszek 11:366f1186c121 39
Zbyszek 11:366f1186c121 40 void calibrateOffset();
Zbyszek 4:e36c7042d3bb 41
Zbyszek 9:9ed9dffd602a 42 int flag = 0;
Zbyszek 9:9ed9dffd602a 43 int flag2 = 0;
Zbyszek 9:9ed9dffd602a 44 int rx_data = 0;
Zbyszek 11:366f1186c121 45 int badSampleFlag = 0;
Zbyszek 9:9ed9dffd602a 46 int16_t rxx;
Zbyszek 11:366f1186c121 47 IMUcheck infoIMU;
Zbyszek 4:e36c7042d3bb 48
Zbyszek 3:e33697420c4a 49
Zbyszek 3:e33697420c4a 50 /*
Zbyszek 3:e33697420c4a 51
Zbyszek 3:e33697420c4a 52 */
Zbyszek 3:e33697420c4a 53 //------------------Printing-All-values----------------------//
Zbyszek 3:e33697420c4a 54 int16_t IMUarray[12]; //Store each separate reading in an array
Zbyszek 3:e33697420c4a 55 uint16_t IDarray[12]; //Holds the identification of each data piece
Zbyszek 3:e33697420c4a 56 char idx = 0; //IMUarray Pointer
Zbyszek 3:e33697420c4a 57 char dataCount = 0; //Keeps track of how many data points have been read in using SPI
Zbyszek 11:366f1186c121 58
Zbyszek 3:e33697420c4a 59
Zbyszek 3:e33697420c4a 60 void ProcessAndPrint();
Zbyszek 3:e33697420c4a 61 //------------------Printing-All-values----------------------//
Zbyszek 3:e33697420c4a 62
Zbyszek 4:e36c7042d3bb 63 //-------------Testing-Variables-Remove-Later----------------//
Zbyszek 11:366f1186c121 64 int blinkCounter = 0;
Zbyszek 4:e36c7042d3bb 65 //-------------Testing-Variables-Remove-Later----------------//
Zbyszek 0:8e367d6d8f03 66
Zbyszek 11:366f1186c121 67
Zbyszek 11:366f1186c121 68 //------------------------------------------------------------------------------Function Declarations
Zbyszek 11:366f1186c121 69 IMUcheck checkData(int16_t dataArray[10][12]);
Zbyszek 11:366f1186c121 70 //------------------------------------------------------------------------------Function Declarations
Zbyszek 11:366f1186c121 71
Zbyszek 0:8e367d6d8f03 72 int main() {
Zbyszek 11:366f1186c121 73 pc.baud(115200);
Zbyszek 11:366f1186c121 74
Zbyszek 11:366f1186c121 75
Zbyszek 3:e33697420c4a 76 IDarray[0] = 1;
Zbyszek 3:e33697420c4a 77 IDarray[1] = 0;
Zbyszek 3:e33697420c4a 78 IDarray[2] = 9;
Zbyszek 3:e33697420c4a 79 IDarray[3] = 8;
Zbyszek 3:e33697420c4a 80 IDarray[4] = 17;
Zbyszek 3:e33697420c4a 81 IDarray[5] = 16;
Zbyszek 3:e33697420c4a 82 IDarray[6] = 3;
Zbyszek 4:e36c7042d3bb 83 IDarray[7] = 2; //2
Zbyszek 3:e33697420c4a 84 IDarray[8] = 11;
Zbyszek 3:e33697420c4a 85 IDarray[9] = 10;
Zbyszek 3:e33697420c4a 86 IDarray[10] = 19;
Zbyszek 3:e33697420c4a 87 IDarray[11] = 18;
Zbyszek 3:e33697420c4a 88
Zbyszek 11:366f1186c121 89 /*
Zbyszek 11:366f1186c121 90 IDarray[0] = 1 + 32;
Zbyszek 11:366f1186c121 91 IDarray[1] = 0 + 32;
Zbyszek 11:366f1186c121 92 IDarray[2] = 9 + 32;
Zbyszek 11:366f1186c121 93 IDarray[3] = 8 + 32;
Zbyszek 11:366f1186c121 94 IDarray[4] = 17 + 32;
Zbyszek 11:366f1186c121 95 IDarray[5] = 16 + 32;
Zbyszek 11:366f1186c121 96 IDarray[6] = 3 + 32;
Zbyszek 11:366f1186c121 97 IDarray[7] = 2 + 32; //2
Zbyszek 11:366f1186c121 98 IDarray[8] = 11 + 32;
Zbyszek 11:366f1186c121 99 IDarray[9] = 10 + 32;
Zbyszek 11:366f1186c121 100 IDarray[10] = 19 + 32;
Zbyszek 11:366f1186c121 101 IDarray[11] = 18 + 32;
Zbyszek 11:366f1186c121 102 */
Zbyszek 9:9ed9dffd602a 103 //init_spi1();
Zbyszek 4:e36c7042d3bb 104 t.start();
Zbyszek 9:9ed9dffd602a 105 SPI_DMA_init();
Zbyszek 4:e36c7042d3bb 106
Zbyszek 9:9ed9dffd602a 107
Zbyszek 9:9ed9dffd602a 108 data_to_transmit[0] = 1;
Zbyszek 9:9ed9dffd602a 109 data_to_transmit[1] = 2;
Zbyszek 9:9ed9dffd602a 110 data_to_transmit[2] = 3;
Zbyszek 9:9ed9dffd602a 111 data_to_transmit[3] = 4;
Zbyszek 9:9ed9dffd602a 112 data_to_transmit[4] = 5;
Zbyszek 9:9ed9dffd602a 113 data_to_transmit[5] = 6;
Zbyszek 9:9ed9dffd602a 114 data_to_transmit[6] = 7;
Zbyszek 9:9ed9dffd602a 115 data_to_transmit[7] = 8;
Zbyszek 9:9ed9dffd602a 116 data_to_transmit[8] = 9;
Zbyszek 9:9ed9dffd602a 117 data_to_transmit[9] = 10;
Zbyszek 9:9ed9dffd602a 118 data_to_transmit[10] = 11;
Zbyszek 10:5b96211275d4 119 data_to_transmit[11] = 12;
Zbyszek 4:e36c7042d3bb 120
Zbyszek 9:9ed9dffd602a 121
Zbyszek 0:8e367d6d8f03 122 while(1) {
Zbyszek 9:9ed9dffd602a 123
Zbyszek 9:9ed9dffd602a 124
Zbyszek 11:366f1186c121 125
Zbyszek 9:9ed9dffd602a 126
Zbyszek 11:366f1186c121 127 if(newDataFlag == 1) {
Zbyszek 11:366f1186c121 128 newDataFlag = 0;
Zbyszek 11:366f1186c121 129 infoIMU = checkData(SampleFIFO);
Zbyszek 11:366f1186c121 130 if(infoIMU.errorFlag == 0) {
Zbyszek 11:366f1186c121 131 for(int x = 0; x <= 11; x++) {
Zbyszek 11:366f1186c121 132 IMUarray[x] = SampleFIFO[pointerFS][x];
Zbyszek 11:366f1186c121 133 }
Zbyszek 11:366f1186c121 134 if(infoIMU.id == 0) {
Zbyszek 11:366f1186c121 135 Datt = IMU0.CalculateCFAngles(IMUarray);
Zbyszek 11:366f1186c121 136 //pc.printf("IMU 0\n\r");
Zbyszek 11:366f1186c121 137 }
Zbyszek 11:366f1186c121 138 else {
Zbyszek 11:366f1186c121 139 //pc.printf("IMU 1\n\r");
Zbyszek 11:366f1186c121 140 Datt1 = IMU1.CalculateCFAngles(IMUarray);
Zbyszek 11:366f1186c121 141 }
Zbyszek 11:366f1186c121 142 //pc.printf("IMU 0: X = %+2f, Y = %+2f, Z = %+2f IMU 1: X = %+2f, Y = %+2f, Z = %+2f\n\r", Datt.x, Datt.y, Datt.z, Datt1.x, Datt1.y, Datt1.z);
Zbyszek 9:9ed9dffd602a 143 }
Zbyszek 11:366f1186c121 144 }//if(newDataFlag == 1)
Zbyszek 11:366f1186c121 145
Zbyszek 9:9ed9dffd602a 146
Zbyszek 0:8e367d6d8f03 147 }
Zbyszek 11:366f1186c121 148
Zbyszek 0:8e367d6d8f03 149 }
Zbyszek 3:e33697420c4a 150
Zbyszek 3:e33697420c4a 151
Zbyszek 3:e33697420c4a 152
Zbyszek 4:e36c7042d3bb 153
Zbyszek 11:366f1186c121 154 IMUcheck checkData(int16_t dataArray[10][12]) {
Zbyszek 11:366f1186c121 155 int16_t firstSample, lastSample; //Used to check first and last sample of batch
Zbyszek 11:366f1186c121 156 uint16_t id = 0; //Used to store extracted data ID
Zbyszek 11:366f1186c121 157 IMUcheck dataStatus; //contains IMU id and error status
Zbyszek 11:366f1186c121 158
Zbyszek 11:366f1186c121 159 dataStatus.errorFlag = 0; //Initialise as 0 by default
Zbyszek 11:366f1186c121 160
Zbyszek 11:366f1186c121 161 firstSample = SampleFIFO[pointerFS][0]; //first sample loaded here
Zbyszek 11:366f1186c121 162 lastSample = SampleFIFO[pointerFS][11]; //last sample loaded here
Zbyszek 11:366f1186c121 163
Zbyszek 11:366f1186c121 164 firstSample &= ~(8191); //remove first 13 bits
Zbyszek 11:366f1186c121 165 firstSample = firstSample >> 13; //shift by right by 13
Zbyszek 11:366f1186c121 166 lastSample &= ~(8191); //remove first 13 bits
Zbyszek 11:366f1186c121 167 lastSample = lastSample >> 13; //shift by right by 13
Zbyszek 11:366f1186c121 168
Zbyszek 11:366f1186c121 169 if(firstSample != lastSample) { //Check if the IDs match
Zbyszek 11:366f1186c121 170 dataStatus.errorFlag = 1; //if both sample ID are not equal then batch is wrong
Zbyszek 11:366f1186c121 171 return; //Leave function early if error occured
Zbyszek 11:366f1186c121 172 }
Zbyszek 11:366f1186c121 173 else { //otherwise if both match
Zbyszek 11:366f1186c121 174 dataStatus.id = firstSample; //attach the status to dataStatus id
Zbyszek 11:366f1186c121 175 // printf("%d \n\r", dataStatus.id);
Zbyszek 11:366f1186c121 176 }
Zbyszek 11:366f1186c121 177
Zbyszek 11:366f1186c121 178 for(int x = 0; x <= 11; x++) {
Zbyszek 11:366f1186c121 179 id = SampleFIFO[pointerFS][x]; //Save sample to id for id extraction
Zbyszek 11:366f1186c121 180 id &= ~(255); //Remove the actual data to only be left with the id
Zbyszek 11:366f1186c121 181 id &= ~(57344); //Remove IMU identification data
Zbyszek 11:366f1186c121 182 id = id >> 8; //shift the id to the right for comparison
Zbyszek 11:366f1186c121 183
Zbyszek 11:366f1186c121 184 if(id != IDarray[x]) { //if the data identification does not match
Zbyszek 11:366f1186c121 185 if(blinkCounter == 200) {
Zbyszek 11:366f1186c121 186 myled = !myled; //Toggle LED to signal that error occured
Zbyszek 11:366f1186c121 187 blinkCounter = 0;
Zbyszek 11:366f1186c121 188 }
Zbyszek 11:366f1186c121 189 else {
Zbyszek 11:366f1186c121 190 blinkCounter++;
Zbyszek 11:366f1186c121 191 }//if(blinkCounter == 200)
Zbyszek 11:366f1186c121 192
Zbyszek 11:366f1186c121 193 dataStatus.errorFlag = 1; //Raise errorFlag
Zbyszek 11:366f1186c121 194 break; //break out of the for loop
Zbyszek 11:366f1186c121 195 }//if(id != IDarray[x])
Zbyszek 11:366f1186c121 196 }//for(int x = 0; x <= 11; x++)
Zbyszek 11:366f1186c121 197
Zbyszek 11:366f1186c121 198 return dataStatus;
Zbyszek 11:366f1186c121 199
Zbyszek 11:366f1186c121 200 }//IMUcheck checkData(int16_t dataArray[10][12])
Zbyszek 4:e36c7042d3bb 201
Zbyszek 4:e36c7042d3bb 202
Zbyszek 9:9ed9dffd602a 203 //---------------------------------------------------------------------------OFFSET-CALIBRATION--------------------------------------------------------------------
Zbyszek 5:155d224d855c 204 void calibrateOffset() {
Zbyszek 5:155d224d855c 205
Zbyszek 5:155d224d855c 206 int16_t MSB = 0; //Store Most Significant Byte of data piece in this variable for processing
Zbyszek 5:155d224d855c 207 int16_t LSB = 0; //Store Least Significant Byte of data piece in this variable for processing
Zbyszek 5:155d224d855c 208 char arrPointer = 0; //Array Pointer
Zbyszek 5:155d224d855c 209
Zbyszek 5:155d224d855c 210 //-----------Concatentated-Data-Pieces------------------------------------------
Zbyszek 5:155d224d855c 211 int16_t gyroX = 0;
Zbyszek 5:155d224d855c 212 int16_t gyroY = 0;
Zbyszek 5:155d224d855c 213 int16_t gyroZ = 0;
Zbyszek 5:155d224d855c 214
Zbyszek 5:155d224d855c 215 int16_t accelX = 0;
Zbyszek 5:155d224d855c 216 int16_t accelY = 0;
Zbyszek 5:155d224d855c 217 int16_t accelZ = 0;
Zbyszek 5:155d224d855c 218
Zbyszek 5:155d224d855c 219 vector accelRaw;
Zbyszek 5:155d224d855c 220 vector accelG;
Zbyszek 5:155d224d855c 221 vector gyroRaw;
Zbyszek 5:155d224d855c 222 vector gyroDPS;
Zbyszek 5:155d224d855c 223
Zbyszek 5:155d224d855c 224 static unsigned int sampleCounter = 1;
Zbyszek 5:155d224d855c 225 static vector accelRawAvg;
Zbyszek 5:155d224d855c 226 static vector accelGAvg;
Zbyszek 5:155d224d855c 227 static vector gyroRawAvg;
Zbyszek 5:155d224d855c 228 static vector gyroDPSAvg;
Zbyszek 5:155d224d855c 229
Zbyszek 5:155d224d855c 230
Zbyszek 5:155d224d855c 231 for(char x = 0; x <= 5; x++) {
Zbyszek 5:155d224d855c 232 MSB = IMUarray[arrPointer]; //Odd data pieces are MSBs
Zbyszek 5:155d224d855c 233 MSB &= ~(255<<8); //Mask the MSB bits as they are not part of data
Zbyszek 5:155d224d855c 234 MSB = MSB << 8; //Shift the Value as its the MSB of the data piece
Zbyszek 5:155d224d855c 235 arrPointer++; //Increment array pointer
Zbyszek 5:155d224d855c 236 LSB = IMUarray[arrPointer]; //Even data pieces are LSBs
Zbyszek 5:155d224d855c 237 LSB &= ~(255 << 8); //Mask the MSB bits as they are not part of data
Zbyszek 5:155d224d855c 238 arrPointer++; //Increment array pointer
Zbyszek 5:155d224d855c 239
Zbyszek 5:155d224d855c 240 switch(x) {
Zbyszek 5:155d224d855c 241 case 0:
Zbyszek 5:155d224d855c 242 accelX = MSB + LSB; //Combine Accelerometer x-axis data
Zbyszek 5:155d224d855c 243 accelRaw.x = (double)accelX; //accelRaw
Zbyszek 5:155d224d855c 244 accelG.x = (double)accelX * 0.00006103515625f; //accelSSF
Zbyszek 5:155d224d855c 245
Zbyszek 5:155d224d855c 246 break;
Zbyszek 5:155d224d855c 247 case 1:
Zbyszek 5:155d224d855c 248 accelY = MSB + LSB; //Combine Accelerometer y-axis data
Zbyszek 5:155d224d855c 249 accelRaw.y = (double)accelY;
Zbyszek 5:155d224d855c 250 accelG.y = (double)accelY * 0.00006103515625f;
Zbyszek 5:155d224d855c 251 break;
Zbyszek 5:155d224d855c 252 case 2:
Zbyszek 5:155d224d855c 253 accelZ = MSB + LSB; //Combine Accelerometer z-axis data
Zbyszek 5:155d224d855c 254 accelRaw.z = (double)accelZ;
Zbyszek 5:155d224d855c 255 accelG.z = (double)accelZ * 0.00006103515625f;
Zbyszek 5:155d224d855c 256 break;
Zbyszek 5:155d224d855c 257 case 3:
Zbyszek 5:155d224d855c 258 gyroX = MSB + LSB; //Combine Gyroscope x-axis data
Zbyszek 5:155d224d855c 259 gyroRaw.x = (double)gyroX; //gyroRaw
Zbyszek 5:155d224d855c 260 gyroDPS.x = (double)gyroX * SSF; //gyroSSF
Zbyszek 5:155d224d855c 261
Zbyszek 5:155d224d855c 262 break;
Zbyszek 5:155d224d855c 263 case 4:
Zbyszek 5:155d224d855c 264 gyroY = MSB + LSB; //Combine Gyroscope y-axis data
Zbyszek 5:155d224d855c 265 gyroRaw.y = (double)gyroY;
Zbyszek 5:155d224d855c 266 gyroDPS.y = (double)gyroY * SSF;
Zbyszek 4:e36c7042d3bb 267
Zbyszek 5:155d224d855c 268 break;
Zbyszek 5:155d224d855c 269 case 5:
Zbyszek 5:155d224d855c 270 gyroZ = MSB + LSB; //Combine Gyroscope z-axis data
Zbyszek 5:155d224d855c 271 gyroRaw.z = (double)gyroZ;
Zbyszek 5:155d224d855c 272 gyroDPS.z = (double)gyroZ * SSF;
Zbyszek 5:155d224d855c 273 break;
Zbyszek 5:155d224d855c 274 default:
Zbyszek 5:155d224d855c 275 break;
Zbyszek 5:155d224d855c 276 }//switch(x)
Zbyszek 5:155d224d855c 277 }//for(char x = 0; x <= 5; x++)
Zbyszek 5:155d224d855c 278
Zbyszek 5:155d224d855c 279 //Take-Running-Averages------------------------------------------------------------------------
Zbyszek 5:155d224d855c 280 //Raw accel averages
Zbyszek 5:155d224d855c 281 accelRawAvg.x = accelRawAvg.x + ((accelRaw.x - accelRawAvg.x)/sampleCounter);
Zbyszek 5:155d224d855c 282 accelRawAvg.y = accelRawAvg.y + ((accelRaw.y - accelRawAvg.y)/sampleCounter);
Zbyszek 5:155d224d855c 283 accelRawAvg.z = accelRawAvg.z + ((accelRaw.z - accelRawAvg.z)/sampleCounter);
Zbyszek 5:155d224d855c 284
Zbyszek 5:155d224d855c 285 //SSF accel averages
Zbyszek 5:155d224d855c 286 accelGAvg.x = accelGAvg.x + ((accelG.x - accelGAvg.x)/sampleCounter);
Zbyszek 5:155d224d855c 287 accelGAvg.y = accelGAvg.y + ((accelG.y - accelGAvg.y)/sampleCounter);
Zbyszek 5:155d224d855c 288 accelGAvg.z = accelGAvg.z + ((accelG.z - accelGAvg.z)/sampleCounter);
Zbyszek 5:155d224d855c 289
Zbyszek 5:155d224d855c 290 //Raw gyroo averages
Zbyszek 5:155d224d855c 291 gyroRawAvg.x = gyroRawAvg.x + ((gyroRaw.x - gyroRawAvg.x)/sampleCounter);
Zbyszek 5:155d224d855c 292 gyroRawAvg.y = gyroRawAvg.y + ((gyroRaw.y - gyroRawAvg.y)/sampleCounter);
Zbyszek 5:155d224d855c 293 gyroRawAvg.z = gyroRawAvg.z + ((gyroRaw.z - gyroRawAvg.z)/sampleCounter);
Zbyszek 5:155d224d855c 294
Zbyszek 5:155d224d855c 295 //SSF gyro averages
Zbyszek 5:155d224d855c 296 gyroDPSAvg.x = gyroDPSAvg.x + ((gyroDPS.x - gyroDPSAvg.x)/sampleCounter);
Zbyszek 5:155d224d855c 297 gyroDPSAvg.y = gyroDPSAvg.y + ((gyroDPS.y - gyroDPSAvg.y)/sampleCounter);
Zbyszek 5:155d224d855c 298 gyroDPSAvg.z = gyroDPSAvg.z + ((gyroDPS.z - gyroDPSAvg.z)/sampleCounter);
Zbyszek 5:155d224d855c 299 //Take-Running-Averages------------------------------------------------------------------------
Zbyszek 4:e36c7042d3bb 300
Zbyszek 5:155d224d855c 301 //Print Messages-------------------------------------------------------------------------------
Zbyszek 5:155d224d855c 302 if(sampleCounter == 1) {
Zbyszek 5:155d224d855c 303 pc.printf("Collecting Raw and Sensitivity Scale Factor multiplied Gyroscope and Accelerometer Offsets...\n\r");
Zbyszek 5:155d224d855c 304 };
Zbyszek 5:155d224d855c 305
Zbyszek 5:155d224d855c 306
Zbyszek 5:155d224d855c 307 if(sampleCounter == 5000) {
Zbyszek 5:155d224d855c 308 pc.printf("RawAX RawAY RawAZ RawGX RawGY RawGZ SampleN\n\r");
Zbyszek 5:155d224d855c 309 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 310 pc.printf("\n\r");
Zbyszek 5:155d224d855c 311 pc.printf("\n\r");
Zbyszek 5:155d224d855c 312 pc.printf("\n\r");
Zbyszek 5:155d224d855c 313
Zbyszek 5:155d224d855c 314 //Add the sensitivity scale factor multiplied data
Zbyszek 5:155d224d855c 315 pc.printf("SSFAX SSFAY SSFAZ SSFGX SSFGY SSFGZ SampleN\n\r");
Zbyszek 5:155d224d855c 316 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 317
Zbyszek 5:155d224d855c 318 };
Zbyszek 5:155d224d855c 319 sampleCounter++;
Zbyszek 5:155d224d855c 320 //Print Messages-------------------------------------------------------------------------------
Zbyszek 5:155d224d855c 321 }
Zbyszek 9:9ed9dffd602a 322 //---------------------------------------------------------------------------OFFSET-CALIBRATION--------------------------------------------------------------------
Zbyszek 4:e36c7042d3bb 323
Zbyszek 4:e36c7042d3bb 324
Zbyszek 4:e36c7042d3bb 325
Zbyszek 4:e36c7042d3bb 326
Zbyszek 4:e36c7042d3bb 327
Zbyszek 4:e36c7042d3bb 328
Zbyszek 4:e36c7042d3bb 329
Zbyszek 4:e36c7042d3bb 330 //------------------------------------------Artifacts----------------------------------------
Zbyszek 4:e36c7042d3bb 331
Zbyszek 4:e36c7042d3bb 332 //----------------------Insert Whole---------------------------------------------------
Zbyszek 4:e36c7042d3bb 333 //Rotate the accel data Global reference frame by qvq*---------------------------------
Zbyszek 4:e36c7042d3bb 334 //globalAccel = rotateGlobal(AccelVals, gyroQ);
Zbyszek 4:e36c7042d3bb 335 //Rotate the accel data Global reference frame by qvq*---------------------------------
Zbyszek 4:e36c7042d3bb 336
Zbyszek 4:e36c7042d3bb 337 //get the correction values and rotate back to IMU reference---------------------------
Zbyszek 4:e36c7042d3bb 338 // correctionGlobalAccel = crossProduct(globalAccel, vertical);
Zbyszek 4:e36c7042d3bb 339 // correctionBodyAccel = rotateLocal(correctionGlobalAccel, gyroQ);
Zbyszek 4:e36c7042d3bb 340 //get the correction values and rotate back to IMU reference---------------------------
Zbyszek 4:e36c7042d3bb 341
Zbyszek 4:e36c7042d3bb 342 //Add the correction to gyro readings and update the quaternion------------------------
Zbyszek 4:e36c7042d3bb 343 //GyroVals = sumVector(GyroVals, correctionBodyAccel);
Zbyszek 4:e36c7042d3bb 344 //incRot = toQuaternionNotation123(GyroVals, dTime);
Zbyszek 4:e36c7042d3bb 345 //gyroQ = getQuaternionProduct(gyroQ, incRot);
Zbyszek 4:e36c7042d3bb 346 //gyroQ = normaliseQuaternion(gyroQ);
Zbyszek 4:e36c7042d3bb 347 //----------------------Insert Whole---------------------------------------------------
Zbyszek 4:e36c7042d3bb 348
Zbyszek 4:e36c7042d3bb 349 //------------------------------------------Artifacts----------------------------------------