Zbigniew Druzbacki / Mbed 2 deprecated SPI_Slave

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

Dependencies:   mbed

main.cpp@10:5b96211275d4, 2019-03-08 (annotated)

Committer:
Zbyszek
Date:
Fri Mar 08 01:33:51 2019 +0000
Revision:
10:5b96211275d4
Parent:
9:9ed9dffd602a
Child:
11:366f1186c121
Solved the problem with Interrupts not working with DMA. Needed to add "extern "C"" in fornt of the interrupt service routines.

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