Zbigniew Druzbacki / Mbed 2 deprecated SPI_Slave

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

Dependencies:   mbed

main.cpp@9:9ed9dffd602a, 2019-03-07 (annotated)

Committer:
Zbyszek
Date:
Thu Mar 07 01:16:48 2019 +0000
Revision:
9:9ed9dffd602a
Parent:
8:e87027349167
Child:
10:5b96211275d4
Got the DMA working to some extent. Disabling the PINC bit solves the transmission problem to some extent but still facing problems such as getting 100% of the data to send properly.

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 9:9ed9dffd602a 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 9:9ed9dffd602a 54 int stateRXNE = 0;
Zbyszek 9:9ed9dffd602a 55 int stateTXE = 0;
Zbyszek 9:9ed9dffd602a 56 int stateBSY = 0;
Zbyszek 9:9ed9dffd602a 57 int DMA1_CH2_Transfer_Complete_Flag = 0;
Zbyszek 9:9ed9dffd602a 58 int DMA1_CH3_Transfer_Complete_Flag = 0;
Zbyszek 9:9ed9dffd602a 59 int DMA1_CH2_Transfer_Error_Flag = 0;
Zbyszek 9:9ed9dffd602a 60 int DMA1_CH3_Transfer_Error_Flag = 0;
Zbyszek 9:9ed9dffd602a 61 int stateDMAch2interrupt = 0;
Zbyszek 9:9ed9dffd602a 62 int stateDMAch3interrupt = 0;
Zbyszek 9:9ed9dffd602a 63 int flag = 0;
Zbyszek 9:9ed9dffd602a 64 int flag2 = 0;
Zbyszek 9:9ed9dffd602a 65 int rx_data = 0;
Zbyszek 9:9ed9dffd602a 66 int16_t rxx;
Zbyszek 4:e36c7042d3bb 67
Zbyszek 3:e33697420c4a 68
Zbyszek 3:e33697420c4a 69 /*
Zbyszek 3:e33697420c4a 70
Zbyszek 3:e33697420c4a 71 */
Zbyszek 3:e33697420c4a 72 //------------------Printing-All-values----------------------//
Zbyszek 3:e33697420c4a 73 int16_t IMUarray[12]; //Store each separate reading in an array
Zbyszek 3:e33697420c4a 74 uint16_t IDarray[12]; //Holds the identification of each data piece
Zbyszek 3:e33697420c4a 75 char idx = 0; //IMUarray Pointer
Zbyszek 3:e33697420c4a 76 char dataCount = 0; //Keeps track of how many data points have been read in using SPI
Zbyszek 3:e33697420c4a 77 uint16_t id = 0; //Used to store extracted data ID
Zbyszek 3:e33697420c4a 78
Zbyszek 3:e33697420c4a 79 void ProcessAndPrint();
Zbyszek 3:e33697420c4a 80 //------------------Printing-All-values----------------------//
Zbyszek 3:e33697420c4a 81
Zbyszek 4:e36c7042d3bb 82 //-------------Testing-Variables-Remove-Later----------------//
Zbyszek 9:9ed9dffd602a 83 //int blinkCounter = 0;
Zbyszek 4:e36c7042d3bb 84 //-------------Testing-Variables-Remove-Later----------------//
Zbyszek 0:8e367d6d8f03 85
Zbyszek 0:8e367d6d8f03 86 int main() {
Zbyszek 4:e36c7042d3bb 87 pc.baud (115200);
Zbyszek 3:e33697420c4a 88 IDarray[0] = 1;
Zbyszek 3:e33697420c4a 89 IDarray[1] = 0;
Zbyszek 3:e33697420c4a 90 IDarray[2] = 9;
Zbyszek 3:e33697420c4a 91 IDarray[3] = 8;
Zbyszek 3:e33697420c4a 92 IDarray[4] = 17;
Zbyszek 3:e33697420c4a 93 IDarray[5] = 16;
Zbyszek 3:e33697420c4a 94 IDarray[6] = 3;
Zbyszek 4:e36c7042d3bb 95 IDarray[7] = 2; //2
Zbyszek 3:e33697420c4a 96 IDarray[8] = 11;
Zbyszek 3:e33697420c4a 97 IDarray[9] = 10;
Zbyszek 3:e33697420c4a 98 IDarray[10] = 19;
Zbyszek 3:e33697420c4a 99 IDarray[11] = 18;
Zbyszek 3:e33697420c4a 100
Zbyszek 9:9ed9dffd602a 101 //init_spi1();
Zbyszek 4:e36c7042d3bb 102 t.start();
Zbyszek 9:9ed9dffd602a 103 SPI_DMA_init();
Zbyszek 4:e36c7042d3bb 104 gyroQ.w = 1;
Zbyszek 4:e36c7042d3bb 105 gyroQ.x = 0.0001;
Zbyszek 4:e36c7042d3bb 106 gyroQ.y = 0.0001;
Zbyszek 4:e36c7042d3bb 107 gyroQ.z = 0.0001;
Zbyszek 4:e36c7042d3bb 108
Zbyszek 5:155d224d855c 109 CF.w = 1;
Zbyszek 5:155d224d855c 110 CF.x = 0.0001;
Zbyszek 5:155d224d855c 111 CF.y = 0.0001;
Zbyszek 5:155d224d855c 112 CF.z = 0.0001;
Zbyszek 4:e36c7042d3bb 113
Zbyszek 4:e36c7042d3bb 114 gyroQ = normaliseQuaternion(gyroQ);
Zbyszek 5:155d224d855c 115 CF = normaliseQuaternion(CF);
Zbyszek 4:e36c7042d3bb 116
Zbyszek 4:e36c7042d3bb 117 vertical.x = 0.0f;
Zbyszek 4:e36c7042d3bb 118 vertical.y = 0.0f;
Zbyszek 4:e36c7042d3bb 119 vertical.z = 1.0f;
Zbyszek 9:9ed9dffd602a 120
Zbyszek 9:9ed9dffd602a 121
Zbyszek 9:9ed9dffd602a 122 data_to_transmit[0] = 1;
Zbyszek 9:9ed9dffd602a 123 data_to_transmit[1] = 2;
Zbyszek 9:9ed9dffd602a 124 data_to_transmit[2] = 3;
Zbyszek 9:9ed9dffd602a 125 data_to_transmit[3] = 4;
Zbyszek 9:9ed9dffd602a 126 data_to_transmit[4] = 5;
Zbyszek 9:9ed9dffd602a 127 data_to_transmit[5] = 6;
Zbyszek 9:9ed9dffd602a 128 data_to_transmit[6] = 7;
Zbyszek 9:9ed9dffd602a 129 data_to_transmit[7] = 8;
Zbyszek 9:9ed9dffd602a 130 data_to_transmit[8] = 9;
Zbyszek 9:9ed9dffd602a 131 data_to_transmit[9] = 10;
Zbyszek 9:9ed9dffd602a 132 data_to_transmit[10] = 11;
Zbyszek 9:9ed9dffd602a 133 data_to_transmit[11] = 16;
Zbyszek 9:9ed9dffd602a 134 data_to_transmit[12] = 13;
Zbyszek 4:e36c7042d3bb 135
Zbyszek 9:9ed9dffd602a 136
Zbyszek 0:8e367d6d8f03 137 while(1) {
Zbyszek 9:9ed9dffd602a 138
Zbyszek 9:9ed9dffd602a 139
Zbyszek 9:9ed9dffd602a 140
Zbyszek 9:9ed9dffd602a 141 stateRXNE = SPI1->SR&0x01;
Zbyszek 9:9ed9dffd602a 142 stateTXE = SPI1->SR&0x02;
Zbyszek 9:9ed9dffd602a 143 stateBSY = SPI1->SR&(1u<<7);
Zbyszek 9:9ed9dffd602a 144
Zbyszek 9:9ed9dffd602a 145 DMA1_CH2_Transfer_Complete_Flag = DMA1->ISR&(1u<<5);
Zbyszek 9:9ed9dffd602a 146 DMA1_CH3_Transfer_Complete_Flag = DMA1->ISR&(1u<<9);
Zbyszek 9:9ed9dffd602a 147 DMA1_CH2_Transfer_Error_Flag = DMA1->ISR&(1u<<7);
Zbyszek 9:9ed9dffd602a 148 DMA1_CH3_Transfer_Error_Flag = DMA1->ISR&(1u<<11);
Zbyszek 9:9ed9dffd602a 149
Zbyszek 9:9ed9dffd602a 150 pc.printf("RXNE: %d, TXE: %d, BSY: %d, CH2 Transfer Complete: %d, CH2 Transfer Complete: %d, CH2 Transfer Error: %d, CH3 Transfer Error: %d\n\r",stateRXNE, stateTXE, stateBSY, DMA1_CH2_Transfer_Complete_Flag, DMA1_CH3_Transfer_Complete_Flag, DMA1_CH2_Transfer_Error_Flag, DMA1_CH3_Transfer_Error_Flag);
Zbyszek 9:9ed9dffd602a 151
Zbyszek 9:9ed9dffd602a 152 if(DMA1->ISR&(1u<<5)) { //Check whether data read transfer is complete
Zbyszek 9:9ed9dffd602a 153 for(int x = 0; x <= 11; x++) {
Zbyszek 9:9ed9dffd602a 154 data_to_transmit[x] = x;
Zbyszek 0:8e367d6d8f03 155 }
Zbyszek 9:9ed9dffd602a 156 CLEAR_DMA1_CH2_IFCR_GFLAG(); //Clear global channel interrupt flag for channel 2
Zbyszek 9:9ed9dffd602a 157 }
Zbyszek 9:9ed9dffd602a 158
Zbyszek 9:9ed9dffd602a 159 if(DMA1->ISR&(1u<<9)) { //Check whteher data transmit transfer is complete
Zbyszek 9:9ed9dffd602a 160 //Read data from the array that stores received data
Zbyszek 9:9ed9dffd602a 161 for(int x = 0; x <= 11; x++) {
Zbyszek 9:9ed9dffd602a 162 IMUarray[x] = received_data[x];
Zbyszek 9:9ed9dffd602a 163 }
Zbyszek 9:9ed9dffd602a 164 CLEAR_DMA1_CH3_IFCR_GFLAG(); //Clear global channel interrupt flag for channel 3
Zbyszek 9:9ed9dffd602a 165 }
Zbyszek 9:9ed9dffd602a 166 /*
Zbyszek 9:9ed9dffd602a 167 if(DMA1->ISR&(1u<<11) && flag == 0) {
Zbyszek 9:9ed9dffd602a 168 if(SPI1->SR&(1u<<7) == 0) { //Check whether SPI is busy before disabling
Zbyszek 9:9ed9dffd602a 169 SPI1->CR1 &= ~SPI_CR1_SPE; //Disable the SPI
Zbyszek 9:9ed9dffd602a 170 DMA1->IFCR |= (1u << (4*(C3S - 1)));
Zbyszek 9:9ed9dffd602a 171 DMA1_Channel3->CCR |= (0x01<<CCR_EN);
Zbyszek 9:9ed9dffd602a 172 SPI1->CR1 |= SPI_CR1_SPE; //Enable SPI
Zbyszek 9:9ed9dffd602a 173 flag = 1;
Zbyszek 9:9ed9dffd602a 174 }
Zbyszek 9:9ed9dffd602a 175 }
Zbyszek 9:9ed9dffd602a 176
Zbyszek 9:9ed9dffd602a 177 if(DMA1->ISR&(1u<<7) && flag2 == 0) {
Zbyszek 9:9ed9dffd602a 178 DMA1->IFCR |= (15u << (4*(C2S - 1)));
Zbyszek 9:9ed9dffd602a 179 flag2 = 1;
Zbyszek 9:9ed9dffd602a 180 }
Zbyszek 9:9ed9dffd602a 181
Zbyszek 9:9ed9dffd602a 182 if(DMA1->ISR&(1u<<9) && flag == 0) {
Zbyszek 9:9ed9dffd602a 183 DMA1->IFCR |= (1u << (4*(C3S - 1)));
Zbyszek 3:e33697420c4a 184
Zbyszek 9:9ed9dffd602a 185 }
Zbyszek 9:9ed9dffd602a 186
Zbyszek 9:9ed9dffd602a 187 if(DMA1->ISR&(1u<<5) && flag2 == 0) {
Zbyszek 9:9ed9dffd602a 188 DMA1->IFCR |= (15u << (4*(C2S - 1)));
Zbyszek 9:9ed9dffd602a 189 for(int x = 0; x <= 10; x++) {
Zbyszek 9:9ed9dffd602a 190 rxx = received_data[x];
Zbyszek 9:9ed9dffd602a 191 }
Zbyszek 9:9ed9dffd602a 192 }
Zbyszek 9:9ed9dffd602a 193
Zbyszek 9:9ed9dffd602a 194 */
Zbyszek 9:9ed9dffd602a 195
Zbyszek 9:9ed9dffd602a 196
Zbyszek 9:9ed9dffd602a 197
Zbyszek 9:9ed9dffd602a 198
Zbyszek 9:9ed9dffd602a 199
Zbyszek 9:9ed9dffd602a 200
Zbyszek 9:9ed9dffd602a 201
Zbyszek 9:9ed9dffd602a 202
Zbyszek 9:9ed9dffd602a 203
Zbyszek 9:9ed9dffd602a 204
Zbyszek 9:9ed9dffd602a 205
Zbyszek 9:9ed9dffd602a 206
Zbyszek 9:9ed9dffd602a 207
Zbyszek 9:9ed9dffd602a 208
Zbyszek 9:9ed9dffd602a 209
Zbyszek 9:9ed9dffd602a 210
Zbyszek 9:9ed9dffd602a 211
Zbyszek 9:9ed9dffd602a 212
Zbyszek 9:9ed9dffd602a 213
Zbyszek 9:9ed9dffd602a 214
Zbyszek 9:9ed9dffd602a 215
Zbyszek 9:9ed9dffd602a 216
Zbyszek 9:9ed9dffd602a 217
Zbyszek 9:9ed9dffd602a 218
Zbyszek 9:9ed9dffd602a 219
Zbyszek 9:9ed9dffd602a 220
Zbyszek 3:e33697420c4a 221 //------------------------------------------------------------------------------
Zbyszek 9:9ed9dffd602a 222 /*
Zbyszek 3:e33697420c4a 223 if(i == 2) {
Zbyszek 9:9ed9dffd602a 224 //slaveRx = transfer_spi_slave(10); //get IMU data
Zbyszek 4:e36c7042d3bb 225
Zbyszek 4:e36c7042d3bb 226
Zbyszek 4:e36c7042d3bb 227 id = slaveRx; //Save sample to id for id extraction
Zbyszek 4:e36c7042d3bb 228 id &= ~(255); //get rid of the actual data to only be left with the id
Zbyszek 4:e36c7042d3bb 229 id = id >> 8; //shift the id to the right for comparison
Zbyszek 4:e36c7042d3bb 230
Zbyszek 4:e36c7042d3bb 231 //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 232
Zbyszek 4:e36c7042d3bb 233 if(IDarray[dataCount] == id) { //compare if the order of data is right and if not wait until it is.
Zbyszek 4:e36c7042d3bb 234 dataCount++; //Increase dataCount as new value has been read in.
Zbyszek 4:e36c7042d3bb 235 IMUarray[idx] = slaveRx; //save the newly read value to current free space in the array
Zbyszek 4:e36c7042d3bb 236 idx++; //increment the pointer to point to next free space in the array
Zbyszek 3:e33697420c4a 237
Zbyszek 3:e33697420c4a 238 if(dataCount == 12) {
Zbyszek 3:e33697420c4a 239 //reset idx and dataCount
Zbyszek 3:e33697420c4a 240 dataCount = 0;
Zbyszek 3:e33697420c4a 241 idx = 0;
Zbyszek 6:0ebecfecadc9 242 //calibrateOffset();
Zbyszek 7:0e9af5986488 243 //IMU0.concatenateData(IMUarray);
Zbyszek 7:0e9af5986488 244 Datt = IMU0.CalculateQCFAngles(IMUarray);
Zbyszek 7:0e9af5986488 245
Zbyszek 6:0ebecfecadc9 246
Zbyszek 4:e36c7042d3bb 247 t.stop();
Zbyszek 4:e36c7042d3bb 248 dTime = t.read();
Zbyszek 4:e36c7042d3bb 249 t.reset();
Zbyszek 4:e36c7042d3bb 250 t.start();
Zbyszek 3:e33697420c4a 251 }//if(dataCount == 12)
Zbyszek 3:e33697420c4a 252 }//if(IDarray[dataCount] == id)
Zbyszek 4:e36c7042d3bb 253 else {
Zbyszek 4:e36c7042d3bb 254 //-----Code-Used-For-Testing-----//
Zbyszek 4:e36c7042d3bb 255 //pc.printf("Failed at: %d \n\r", dataCount); //Print an error if there is one
Zbyszek 9:9ed9dffd602a 256 // if(blinkCounter == 10) { //Slow the blinking down to make it visible if there are errors
Zbyszek 9:9ed9dffd602a 257 // myled = !myled; //Change state of the LED if error occurs
Zbyszek 9:9ed9dffd602a 258 // blinkCounter = 0; //Reset Blink counter
Zbyszek 9:9ed9dffd602a 259 // }
Zbyszek 9:9ed9dffd602a 260 // else {
Zbyszek 9:9ed9dffd602a 261 // blinkCounter++;
Zbyszek 9:9ed9dffd602a 262 //}
Zbyszek 4:e36c7042d3bb 263 //-----Code-Used-For-Testing-----//
Zbyszek 4:e36c7042d3bb 264
Zbyszek 4:e36c7042d3bb 265 dataCount = 0; //ID sequence is worng so reset the counter
Zbyszek 4:e36c7042d3bb 266 idx = 0; //ID sequence is worng so reset the counter
Zbyszek 4:e36c7042d3bb 267 }
Zbyszek 3:e33697420c4a 268 }//if(i == 2)
Zbyszek 3:e33697420c4a 269
Zbyszek 9:9ed9dffd602a 270 */
Zbyszek 3:e33697420c4a 271
Zbyszek 0:8e367d6d8f03 272 }
Zbyszek 0:8e367d6d8f03 273 }
Zbyszek 3:e33697420c4a 274
Zbyszek 3:e33697420c4a 275
Zbyszek 3:e33697420c4a 276
Zbyszek 4:e36c7042d3bb 277
Zbyszek 3:e33697420c4a 278
Zbyszek 4:e36c7042d3bb 279
Zbyszek 4:e36c7042d3bb 280
Zbyszek 9:9ed9dffd602a 281 //---------------------------------------------------------------------------OFFSET-CALIBRATION--------------------------------------------------------------------
Zbyszek 5:155d224d855c 282 void calibrateOffset() {
Zbyszek 5:155d224d855c 283
Zbyszek 5:155d224d855c 284 int16_t MSB = 0; //Store Most Significant Byte of data piece in this variable for processing
Zbyszek 5:155d224d855c 285 int16_t LSB = 0; //Store Least Significant Byte of data piece in this variable for processing
Zbyszek 5:155d224d855c 286 char arrPointer = 0; //Array Pointer
Zbyszek 5:155d224d855c 287
Zbyszek 5:155d224d855c 288 //-----------Concatentated-Data-Pieces------------------------------------------
Zbyszek 5:155d224d855c 289 int16_t gyroX = 0;
Zbyszek 5:155d224d855c 290 int16_t gyroY = 0;
Zbyszek 5:155d224d855c 291 int16_t gyroZ = 0;
Zbyszek 5:155d224d855c 292
Zbyszek 5:155d224d855c 293 int16_t accelX = 0;
Zbyszek 5:155d224d855c 294 int16_t accelY = 0;
Zbyszek 5:155d224d855c 295 int16_t accelZ = 0;
Zbyszek 5:155d224d855c 296
Zbyszek 5:155d224d855c 297 vector accelRaw;
Zbyszek 5:155d224d855c 298 vector accelG;
Zbyszek 5:155d224d855c 299 vector gyroRaw;
Zbyszek 5:155d224d855c 300 vector gyroDPS;
Zbyszek 5:155d224d855c 301
Zbyszek 5:155d224d855c 302 static unsigned int sampleCounter = 1;
Zbyszek 5:155d224d855c 303 static vector accelRawAvg;
Zbyszek 5:155d224d855c 304 static vector accelGAvg;
Zbyszek 5:155d224d855c 305 static vector gyroRawAvg;
Zbyszek 5:155d224d855c 306 static vector gyroDPSAvg;
Zbyszek 5:155d224d855c 307
Zbyszek 5:155d224d855c 308
Zbyszek 5:155d224d855c 309 for(char x = 0; x <= 5; x++) {
Zbyszek 5:155d224d855c 310 MSB = IMUarray[arrPointer]; //Odd data pieces are MSBs
Zbyszek 5:155d224d855c 311 MSB &= ~(255<<8); //Mask the MSB bits as they are not part of data
Zbyszek 5:155d224d855c 312 MSB = MSB << 8; //Shift the Value as its the MSB of the data piece
Zbyszek 5:155d224d855c 313 arrPointer++; //Increment array pointer
Zbyszek 5:155d224d855c 314 LSB = IMUarray[arrPointer]; //Even data pieces are LSBs
Zbyszek 5:155d224d855c 315 LSB &= ~(255 << 8); //Mask the MSB bits as they are not part of data
Zbyszek 5:155d224d855c 316 arrPointer++; //Increment array pointer
Zbyszek 5:155d224d855c 317
Zbyszek 5:155d224d855c 318 switch(x) {
Zbyszek 5:155d224d855c 319 case 0:
Zbyszek 5:155d224d855c 320 accelX = MSB + LSB; //Combine Accelerometer x-axis data
Zbyszek 5:155d224d855c 321 accelRaw.x = (double)accelX; //accelRaw
Zbyszek 5:155d224d855c 322 accelG.x = (double)accelX * 0.00006103515625f; //accelSSF
Zbyszek 5:155d224d855c 323
Zbyszek 5:155d224d855c 324 break;
Zbyszek 5:155d224d855c 325 case 1:
Zbyszek 5:155d224d855c 326 accelY = MSB + LSB; //Combine Accelerometer y-axis data
Zbyszek 5:155d224d855c 327 accelRaw.y = (double)accelY;
Zbyszek 5:155d224d855c 328 accelG.y = (double)accelY * 0.00006103515625f;
Zbyszek 5:155d224d855c 329 break;
Zbyszek 5:155d224d855c 330 case 2:
Zbyszek 5:155d224d855c 331 accelZ = MSB + LSB; //Combine Accelerometer z-axis data
Zbyszek 5:155d224d855c 332 accelRaw.z = (double)accelZ;
Zbyszek 5:155d224d855c 333 accelG.z = (double)accelZ * 0.00006103515625f;
Zbyszek 5:155d224d855c 334 break;
Zbyszek 5:155d224d855c 335 case 3:
Zbyszek 5:155d224d855c 336 gyroX = MSB + LSB; //Combine Gyroscope x-axis data
Zbyszek 5:155d224d855c 337 gyroRaw.x = (double)gyroX; //gyroRaw
Zbyszek 5:155d224d855c 338 gyroDPS.x = (double)gyroX * SSF; //gyroSSF
Zbyszek 5:155d224d855c 339
Zbyszek 5:155d224d855c 340 break;
Zbyszek 5:155d224d855c 341 case 4:
Zbyszek 5:155d224d855c 342 gyroY = MSB + LSB; //Combine Gyroscope y-axis data
Zbyszek 5:155d224d855c 343 gyroRaw.y = (double)gyroY;
Zbyszek 5:155d224d855c 344 gyroDPS.y = (double)gyroY * SSF;
Zbyszek 4:e36c7042d3bb 345
Zbyszek 5:155d224d855c 346 break;
Zbyszek 5:155d224d855c 347 case 5:
Zbyszek 5:155d224d855c 348 gyroZ = MSB + LSB; //Combine Gyroscope z-axis data
Zbyszek 5:155d224d855c 349 gyroRaw.z = (double)gyroZ;
Zbyszek 5:155d224d855c 350 gyroDPS.z = (double)gyroZ * SSF;
Zbyszek 5:155d224d855c 351 break;
Zbyszek 5:155d224d855c 352 default:
Zbyszek 5:155d224d855c 353 break;
Zbyszek 5:155d224d855c 354 }//switch(x)
Zbyszek 5:155d224d855c 355 }//for(char x = 0; x <= 5; x++)
Zbyszek 5:155d224d855c 356
Zbyszek 5:155d224d855c 357 //Take-Running-Averages------------------------------------------------------------------------
Zbyszek 5:155d224d855c 358 //Raw accel averages
Zbyszek 5:155d224d855c 359 accelRawAvg.x = accelRawAvg.x + ((accelRaw.x - accelRawAvg.x)/sampleCounter);
Zbyszek 5:155d224d855c 360 accelRawAvg.y = accelRawAvg.y + ((accelRaw.y - accelRawAvg.y)/sampleCounter);
Zbyszek 5:155d224d855c 361 accelRawAvg.z = accelRawAvg.z + ((accelRaw.z - accelRawAvg.z)/sampleCounter);
Zbyszek 5:155d224d855c 362
Zbyszek 5:155d224d855c 363 //SSF accel averages
Zbyszek 5:155d224d855c 364 accelGAvg.x = accelGAvg.x + ((accelG.x - accelGAvg.x)/sampleCounter);
Zbyszek 5:155d224d855c 365 accelGAvg.y = accelGAvg.y + ((accelG.y - accelGAvg.y)/sampleCounter);
Zbyszek 5:155d224d855c 366 accelGAvg.z = accelGAvg.z + ((accelG.z - accelGAvg.z)/sampleCounter);
Zbyszek 5:155d224d855c 367
Zbyszek 5:155d224d855c 368 //Raw gyroo averages
Zbyszek 5:155d224d855c 369 gyroRawAvg.x = gyroRawAvg.x + ((gyroRaw.x - gyroRawAvg.x)/sampleCounter);
Zbyszek 5:155d224d855c 370 gyroRawAvg.y = gyroRawAvg.y + ((gyroRaw.y - gyroRawAvg.y)/sampleCounter);
Zbyszek 5:155d224d855c 371 gyroRawAvg.z = gyroRawAvg.z + ((gyroRaw.z - gyroRawAvg.z)/sampleCounter);
Zbyszek 5:155d224d855c 372
Zbyszek 5:155d224d855c 373 //SSF gyro averages
Zbyszek 5:155d224d855c 374 gyroDPSAvg.x = gyroDPSAvg.x + ((gyroDPS.x - gyroDPSAvg.x)/sampleCounter);
Zbyszek 5:155d224d855c 375 gyroDPSAvg.y = gyroDPSAvg.y + ((gyroDPS.y - gyroDPSAvg.y)/sampleCounter);
Zbyszek 5:155d224d855c 376 gyroDPSAvg.z = gyroDPSAvg.z + ((gyroDPS.z - gyroDPSAvg.z)/sampleCounter);
Zbyszek 5:155d224d855c 377 //Take-Running-Averages------------------------------------------------------------------------
Zbyszek 4:e36c7042d3bb 378
Zbyszek 5:155d224d855c 379 //Print Messages-------------------------------------------------------------------------------
Zbyszek 5:155d224d855c 380 if(sampleCounter == 1) {
Zbyszek 5:155d224d855c 381 pc.printf("Collecting Raw and Sensitivity Scale Factor multiplied Gyroscope and Accelerometer Offsets...\n\r");
Zbyszek 5:155d224d855c 382 };
Zbyszek 5:155d224d855c 383
Zbyszek 5:155d224d855c 384
Zbyszek 5:155d224d855c 385 if(sampleCounter == 5000) {
Zbyszek 5:155d224d855c 386 pc.printf("RawAX RawAY RawAZ RawGX RawGY RawGZ SampleN\n\r");
Zbyszek 5:155d224d855c 387 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 388 pc.printf("\n\r");
Zbyszek 5:155d224d855c 389 pc.printf("\n\r");
Zbyszek 5:155d224d855c 390 pc.printf("\n\r");
Zbyszek 5:155d224d855c 391
Zbyszek 5:155d224d855c 392 //Add the sensitivity scale factor multiplied data
Zbyszek 5:155d224d855c 393 pc.printf("SSFAX SSFAY SSFAZ SSFGX SSFGY SSFGZ SampleN\n\r");
Zbyszek 5:155d224d855c 394 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 395
Zbyszek 5:155d224d855c 396 };
Zbyszek 5:155d224d855c 397 sampleCounter++;
Zbyszek 5:155d224d855c 398 //Print Messages-------------------------------------------------------------------------------
Zbyszek 5:155d224d855c 399 }
Zbyszek 9:9ed9dffd602a 400 //---------------------------------------------------------------------------OFFSET-CALIBRATION--------------------------------------------------------------------
Zbyszek 4:e36c7042d3bb 401
Zbyszek 4:e36c7042d3bb 402
Zbyszek 4:e36c7042d3bb 403
Zbyszek 4:e36c7042d3bb 404
Zbyszek 4:e36c7042d3bb 405
Zbyszek 4:e36c7042d3bb 406
Zbyszek 4:e36c7042d3bb 407
Zbyszek 4:e36c7042d3bb 408 //------------------------------------------Artifacts----------------------------------------
Zbyszek 4:e36c7042d3bb 409
Zbyszek 4:e36c7042d3bb 410 //----------------------Insert Whole---------------------------------------------------
Zbyszek 4:e36c7042d3bb 411 //Rotate the accel data Global reference frame by qvq*---------------------------------
Zbyszek 4:e36c7042d3bb 412 //globalAccel = rotateGlobal(AccelVals, gyroQ);
Zbyszek 4:e36c7042d3bb 413 //Rotate the accel data Global reference frame by qvq*---------------------------------
Zbyszek 4:e36c7042d3bb 414
Zbyszek 4:e36c7042d3bb 415 //get the correction values and rotate back to IMU reference---------------------------
Zbyszek 4:e36c7042d3bb 416 // correctionGlobalAccel = crossProduct(globalAccel, vertical);
Zbyszek 4:e36c7042d3bb 417 // correctionBodyAccel = rotateLocal(correctionGlobalAccel, gyroQ);
Zbyszek 4:e36c7042d3bb 418 //get the correction values and rotate back to IMU reference---------------------------
Zbyszek 4:e36c7042d3bb 419
Zbyszek 4:e36c7042d3bb 420 //Add the correction to gyro readings and update the quaternion------------------------
Zbyszek 4:e36c7042d3bb 421 //GyroVals = sumVector(GyroVals, correctionBodyAccel);
Zbyszek 4:e36c7042d3bb 422 //incRot = toQuaternionNotation123(GyroVals, dTime);
Zbyszek 4:e36c7042d3bb 423 //gyroQ = getQuaternionProduct(gyroQ, incRot);
Zbyszek 4:e36c7042d3bb 424 //gyroQ = normaliseQuaternion(gyroQ);
Zbyszek 4:e36c7042d3bb 425 //----------------------Insert Whole---------------------------------------------------
Zbyszek 4:e36c7042d3bb 426
Zbyszek 4:e36c7042d3bb 427 //------------------------------------------Artifacts----------------------------------------