Zbigniew Druzbacki / Mbed 2 deprecated SPI_Slave

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

Dependencies:   mbed

main.cpp@13:c7e8e277f884, 2019-04-19 (annotated)

Committer:
Zbyszek
Date:
Fri Apr 19 18:43:39 2019 +0000
Revision:
13:c7e8e277f884
Parent:
11:366f1186c121
Child:
14:7bbaafa22f8d
Added IMU data transmission handling

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 13:c7e8e277f884 4 #include "CustomDatatypes.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 13:c7e8e277f884 11 InterruptIn dataRequest(PA_0);
Zbyszek 13:c7e8e277f884 12
Zbyszek 13:c7e8e277f884 13
Zbyszek 0:8e367d6d8f03 14 int masterRx = 0;
Zbyszek 3:e33697420c4a 15 int16_t slaveRx = 0;
Zbyszek 3:e33697420c4a 16 int i = 2;
Zbyszek 3:e33697420c4a 17 int k = 0;
Zbyszek 3:e33697420c4a 18 int16_t zgHigher = 0;
Zbyszek 3:e33697420c4a 19 int16_t zgLower = 0;
Zbyszek 3:e33697420c4a 20 int16_t zGyro = 0;
Zbyszek 3:e33697420c4a 21 int countx = 0;
Zbyszek 3:e33697420c4a 22 int p = 32776;
Zbyszek 6:0ebecfecadc9 23 double const SSF = 0.06097560975609756097560975609756f; //FSEL = 0: 0.00763358778625954198473282442748, FSEL = 3: 0.06097560975609756097560975609756f
Zbyszek 6:0ebecfecadc9 24
Zbyszek 6:0ebecfecadc9 25 int OffsetX = 254;
Zbyszek 6:0ebecfecadc9 26 int OffsetY = -14;
Zbyszek 6:0ebecfecadc9 27 int OffsetZ = 81;
Zbyszek 6:0ebecfecadc9 28
Zbyszek 6:0ebecfecadc9 29 int OffsetXA = -306;
Zbyszek 6:0ebecfecadc9 30 int OffsetYA = -131;
Zbyszek 6:0ebecfecadc9 31 int OffsetZA = -531;
Zbyszek 6:0ebecfecadc9 32
Zbyszek 11:366f1186c121 33 IMU IMU0 (0, -306.0f, -131.0f, -351.0f, 254.0f, -14.0f, 81.0f, 0, 0);
Zbyszek 11:366f1186c121 34 IMU IMU1 (0, -306.0f, -131.0f, -351.0f, 254.0f, -14.0f, 81.0f, 0, 0);
Zbyszek 7:0e9af5986488 35 IMU_Data Dat;
Zbyszek 11:366f1186c121 36 vector Datt, Datt1;
Zbyszek 13:c7e8e277f884 37 vector IMU0_Data, IMU1_Data, IMU2_Data;
Zbyszek 10:5b96211275d4 38 int D2T;
Zbyszek 6:0ebecfecadc9 39
Zbyszek 3:e33697420c4a 40
Zbyszek 4:e36c7042d3bb 41 Timer t;
Zbyszek 4:e36c7042d3bb 42 float dTime = 0.0f;
Zbyszek 11:366f1186c121 43
Zbyszek 11:366f1186c121 44 void calibrateOffset();
Zbyszek 4:e36c7042d3bb 45
Zbyszek 9:9ed9dffd602a 46 int flag = 0;
Zbyszek 9:9ed9dffd602a 47 int flag2 = 0;
Zbyszek 9:9ed9dffd602a 48 int rx_data = 0;
Zbyszek 11:366f1186c121 49 int badSampleFlag = 0;
Zbyszek 9:9ed9dffd602a 50 int16_t rxx;
Zbyszek 11:366f1186c121 51 IMUcheck infoIMU;
Zbyszek 4:e36c7042d3bb 52
Zbyszek 3:e33697420c4a 53
Zbyszek 13:c7e8e277f884 54 char IMU_Data_Pointer = 0; //Points to the IMU whose turn it is to send data.
Zbyszek 13:c7e8e277f884 55
Zbyszek 13:c7e8e277f884 56
Zbyszek 3:e33697420c4a 57 /*
Zbyszek 3:e33697420c4a 58
Zbyszek 3:e33697420c4a 59 */
Zbyszek 3:e33697420c4a 60 //------------------Printing-All-values----------------------//
Zbyszek 3:e33697420c4a 61 int16_t IMUarray[12]; //Store each separate reading in an array
Zbyszek 3:e33697420c4a 62 uint16_t IDarray[12]; //Holds the identification of each data piece
Zbyszek 3:e33697420c4a 63 char idx = 0; //IMUarray Pointer
Zbyszek 3:e33697420c4a 64 char dataCount = 0; //Keeps track of how many data points have been read in using SPI
Zbyszek 11:366f1186c121 65
Zbyszek 3:e33697420c4a 66
Zbyszek 3:e33697420c4a 67 void ProcessAndPrint();
Zbyszek 3:e33697420c4a 68 //------------------Printing-All-values----------------------//
Zbyszek 3:e33697420c4a 69
Zbyszek 4:e36c7042d3bb 70 //-------------Testing-Variables-Remove-Later----------------//
Zbyszek 11:366f1186c121 71 int blinkCounter = 0;
Zbyszek 4:e36c7042d3bb 72 //-------------Testing-Variables-Remove-Later----------------//
Zbyszek 0:8e367d6d8f03 73
Zbyszek 11:366f1186c121 74
Zbyszek 11:366f1186c121 75 //------------------------------------------------------------------------------Function Declarations
Zbyszek 11:366f1186c121 76 IMUcheck checkData(int16_t dataArray[10][12]);
Zbyszek 11:366f1186c121 77 //------------------------------------------------------------------------------Function Declarations
Zbyszek 11:366f1186c121 78
Zbyszek 13:c7e8e277f884 79 void requestISR(void) {
Zbyszek 13:c7e8e277f884 80 dataLoadedFlag = 0;
Zbyszek 13:c7e8e277f884 81 dataRequestFlag = 1; //Let L432 know that data has been requested
Zbyszek 13:c7e8e277f884 82 myled = !myled; //Flash LED to indicate
Zbyszek 13:c7e8e277f884 83
Zbyszek 13:c7e8e277f884 84 if(IMU_Data_Pointer == 2) { //Was previous data sent IMU2?
Zbyszek 13:c7e8e277f884 85 IMU_Data_Pointer = 0; //Yes - reset back to IMU0
Zbyszek 13:c7e8e277f884 86 }
Zbyszek 13:c7e8e277f884 87 else {
Zbyszek 13:c7e8e277f884 88 IMU_Data_Pointer++; //No - point to the next IMU data to be sent.
Zbyszek 13:c7e8e277f884 89 }
Zbyszek 13:c7e8e277f884 90 }
Zbyszek 13:c7e8e277f884 91
Zbyszek 13:c7e8e277f884 92 //Used to split data into SPI managable chunks
Zbyszek 13:c7e8e277f884 93 void prepareSPItx(int imuID, vector IMUn) {
Zbyszek 13:c7e8e277f884 94 txSPIUnion data; //Use union to split float into 2 16-bit values
Zbyszek 13:c7e8e277f884 95
Zbyszek 13:c7e8e277f884 96 for(int x = 0; x <= 11; x++) { //Use the for loop to load data into correct places in the array
Zbyszek 13:c7e8e277f884 97 switch(x) {
Zbyszek 13:c7e8e277f884 98 case 0:
Zbyszek 13:c7e8e277f884 99 IMU_Data_Array[x] = imuID;
Zbyszek 13:c7e8e277f884 100 break;
Zbyszek 13:c7e8e277f884 101
Zbyszek 13:c7e8e277f884 102 case 1:
Zbyszek 13:c7e8e277f884 103 data.f = IMUn.x; //Place x-axis oreintation data into the union float
Zbyszek 13:c7e8e277f884 104 IMU_Data_Array[x] = data.n[0]; //save MSB 16 bits to array
Zbyszek 13:c7e8e277f884 105 break;
Zbyszek 13:c7e8e277f884 106
Zbyszek 13:c7e8e277f884 107 case 2:
Zbyszek 13:c7e8e277f884 108 data.f = IMUn.x; //Place x-axis oreintation data into the union float
Zbyszek 13:c7e8e277f884 109 IMU_Data_Array[x] = data.n[1]; //save LSB 16 bits to array
Zbyszek 13:c7e8e277f884 110 break;
Zbyszek 13:c7e8e277f884 111
Zbyszek 13:c7e8e277f884 112 case 3:
Zbyszek 13:c7e8e277f884 113 data.f = IMUn.y; //Place y-axis oreintation data into the union float
Zbyszek 13:c7e8e277f884 114 IMU_Data_Array[x] = data.n[0]; //save MSB 16 bits to array
Zbyszek 13:c7e8e277f884 115 break;
Zbyszek 13:c7e8e277f884 116
Zbyszek 13:c7e8e277f884 117 case 4:
Zbyszek 13:c7e8e277f884 118 data.f = IMUn.y; //Place y-axis oreintation data into the union float
Zbyszek 13:c7e8e277f884 119 IMU_Data_Array[x] = data.n[1]; //save LSB 16 bits to array
Zbyszek 13:c7e8e277f884 120 break;
Zbyszek 13:c7e8e277f884 121
Zbyszek 13:c7e8e277f884 122 case 5:
Zbyszek 13:c7e8e277f884 123 data.f = IMUn.z; //Place z-axis oreintation data into the union float
Zbyszek 13:c7e8e277f884 124 IMU_Data_Array[x] = data.n[0]; //save MSB 16 bits to array
Zbyszek 13:c7e8e277f884 125 break;
Zbyszek 13:c7e8e277f884 126
Zbyszek 13:c7e8e277f884 127 case 6:
Zbyszek 13:c7e8e277f884 128 data.f = IMUn.z; //Place z-axis oreintation data into the union float
Zbyszek 13:c7e8e277f884 129 IMU_Data_Array[x] = data.n[1]; //save LSB 16 bits to array
Zbyszek 13:c7e8e277f884 130 break;
Zbyszek 13:c7e8e277f884 131
Zbyszek 13:c7e8e277f884 132 default:
Zbyszek 13:c7e8e277f884 133 IMU_Data_Array[x] = 0;
Zbyszek 13:c7e8e277f884 134 break;
Zbyszek 13:c7e8e277f884 135 }//switch(x)
Zbyszek 13:c7e8e277f884 136 }//for(int x = 0; x <= 11; x++) {
Zbyszek 13:c7e8e277f884 137 }//void prepareSPItx
Zbyszek 13:c7e8e277f884 138
Zbyszek 0:8e367d6d8f03 139 int main() {
Zbyszek 13:c7e8e277f884 140
Zbyszek 13:c7e8e277f884 141 IMU_Data_Pointer = 2; //Intially 2 but will be reset to zero when first request signal comes in.
Zbyszek 13:c7e8e277f884 142
Zbyszek 13:c7e8e277f884 143 dataRequest.rise(&requestISR);
Zbyszek 13:c7e8e277f884 144
Zbyszek 11:366f1186c121 145 pc.baud(115200);
Zbyszek 11:366f1186c121 146
Zbyszek 11:366f1186c121 147
Zbyszek 3:e33697420c4a 148 IDarray[0] = 1;
Zbyszek 3:e33697420c4a 149 IDarray[1] = 0;
Zbyszek 3:e33697420c4a 150 IDarray[2] = 9;
Zbyszek 3:e33697420c4a 151 IDarray[3] = 8;
Zbyszek 3:e33697420c4a 152 IDarray[4] = 17;
Zbyszek 3:e33697420c4a 153 IDarray[5] = 16;
Zbyszek 3:e33697420c4a 154 IDarray[6] = 3;
Zbyszek 4:e36c7042d3bb 155 IDarray[7] = 2; //2
Zbyszek 3:e33697420c4a 156 IDarray[8] = 11;
Zbyszek 3:e33697420c4a 157 IDarray[9] = 10;
Zbyszek 3:e33697420c4a 158 IDarray[10] = 19;
Zbyszek 3:e33697420c4a 159 IDarray[11] = 18;
Zbyszek 3:e33697420c4a 160
Zbyszek 11:366f1186c121 161 /*
Zbyszek 11:366f1186c121 162 IDarray[0] = 1 + 32;
Zbyszek 11:366f1186c121 163 IDarray[1] = 0 + 32;
Zbyszek 11:366f1186c121 164 IDarray[2] = 9 + 32;
Zbyszek 11:366f1186c121 165 IDarray[3] = 8 + 32;
Zbyszek 11:366f1186c121 166 IDarray[4] = 17 + 32;
Zbyszek 11:366f1186c121 167 IDarray[5] = 16 + 32;
Zbyszek 11:366f1186c121 168 IDarray[6] = 3 + 32;
Zbyszek 11:366f1186c121 169 IDarray[7] = 2 + 32; //2
Zbyszek 11:366f1186c121 170 IDarray[8] = 11 + 32;
Zbyszek 11:366f1186c121 171 IDarray[9] = 10 + 32;
Zbyszek 11:366f1186c121 172 IDarray[10] = 19 + 32;
Zbyszek 11:366f1186c121 173 IDarray[11] = 18 + 32;
Zbyszek 11:366f1186c121 174 */
Zbyszek 9:9ed9dffd602a 175 //init_spi1();
Zbyszek 4:e36c7042d3bb 176 t.start();
Zbyszek 9:9ed9dffd602a 177 SPI_DMA_init();
Zbyszek 4:e36c7042d3bb 178
Zbyszek 9:9ed9dffd602a 179
Zbyszek 9:9ed9dffd602a 180 data_to_transmit[0] = 1;
Zbyszek 9:9ed9dffd602a 181 data_to_transmit[1] = 2;
Zbyszek 9:9ed9dffd602a 182 data_to_transmit[2] = 3;
Zbyszek 9:9ed9dffd602a 183 data_to_transmit[3] = 4;
Zbyszek 9:9ed9dffd602a 184 data_to_transmit[4] = 5;
Zbyszek 9:9ed9dffd602a 185 data_to_transmit[5] = 6;
Zbyszek 9:9ed9dffd602a 186 data_to_transmit[6] = 7;
Zbyszek 9:9ed9dffd602a 187 data_to_transmit[7] = 8;
Zbyszek 9:9ed9dffd602a 188 data_to_transmit[8] = 9;
Zbyszek 9:9ed9dffd602a 189 data_to_transmit[9] = 10;
Zbyszek 9:9ed9dffd602a 190 data_to_transmit[10] = 11;
Zbyszek 10:5b96211275d4 191 data_to_transmit[11] = 12;
Zbyszek 4:e36c7042d3bb 192
Zbyszek 9:9ed9dffd602a 193
Zbyszek 0:8e367d6d8f03 194 while(1) {
Zbyszek 9:9ed9dffd602a 195
Zbyszek 9:9ed9dffd602a 196
Zbyszek 11:366f1186c121 197
Zbyszek 9:9ed9dffd602a 198
Zbyszek 11:366f1186c121 199 if(newDataFlag == 1) {
Zbyszek 11:366f1186c121 200 newDataFlag = 0;
Zbyszek 11:366f1186c121 201 infoIMU = checkData(SampleFIFO);
Zbyszek 11:366f1186c121 202 if(infoIMU.errorFlag == 0) {
Zbyszek 11:366f1186c121 203 for(int x = 0; x <= 11; x++) {
Zbyszek 11:366f1186c121 204 IMUarray[x] = SampleFIFO[pointerFS][x];
Zbyszek 11:366f1186c121 205 }
Zbyszek 11:366f1186c121 206 if(infoIMU.id == 0) {
Zbyszek 11:366f1186c121 207 Datt = IMU0.CalculateCFAngles(IMUarray);
Zbyszek 11:366f1186c121 208 }
Zbyszek 11:366f1186c121 209 else {
Zbyszek 11:366f1186c121 210 Datt1 = IMU1.CalculateCFAngles(IMUarray);
Zbyszek 11:366f1186c121 211 }
Zbyszek 11:366f1186c121 212 //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 213 }
Zbyszek 11:366f1186c121 214 }//if(newDataFlag == 1)
Zbyszek 11:366f1186c121 215
Zbyszek 13:c7e8e277f884 216 //Load appropriate data into array for transmission
Zbyszek 13:c7e8e277f884 217 if(dataLoadedFlag == 0) { //if data hasnt been loaded when request occured then load data
Zbyszek 13:c7e8e277f884 218 switch(IMU_Data_Pointer) {
Zbyszek 13:c7e8e277f884 219 case 0:
Zbyszek 13:c7e8e277f884 220 prepareSPItx(0, IMU0_Data); //IMU0 Data
Zbyszek 13:c7e8e277f884 221 break;
Zbyszek 13:c7e8e277f884 222
Zbyszek 13:c7e8e277f884 223 case 1:
Zbyszek 13:c7e8e277f884 224 prepareSPItx(1, IMU1_Data); //IMU1 Data
Zbyszek 13:c7e8e277f884 225 break;
Zbyszek 13:c7e8e277f884 226
Zbyszek 13:c7e8e277f884 227 case 2:
Zbyszek 13:c7e8e277f884 228 prepareSPItx(2, IMU2_Data); //IMU2 Data
Zbyszek 13:c7e8e277f884 229 break;
Zbyszek 13:c7e8e277f884 230
Zbyszek 13:c7e8e277f884 231 default:
Zbyszek 13:c7e8e277f884 232 break;
Zbyszek 13:c7e8e277f884 233 }//switch(IMU_Data_Pointer)
Zbyszek 13:c7e8e277f884 234 dataLoadedFlag = 1;
Zbyszek 0:8e367d6d8f03 235 }
Zbyszek 13:c7e8e277f884 236
Zbyszek 13:c7e8e277f884 237 }//while(1)
Zbyszek 13:c7e8e277f884 238 }//int main()
Zbyszek 3:e33697420c4a 239
Zbyszek 3:e33697420c4a 240
Zbyszek 3:e33697420c4a 241
Zbyszek 4:e36c7042d3bb 242
Zbyszek 11:366f1186c121 243 IMUcheck checkData(int16_t dataArray[10][12]) {
Zbyszek 11:366f1186c121 244 int16_t firstSample, lastSample; //Used to check first and last sample of batch
Zbyszek 11:366f1186c121 245 uint16_t id = 0; //Used to store extracted data ID
Zbyszek 11:366f1186c121 246 IMUcheck dataStatus; //contains IMU id and error status
Zbyszek 11:366f1186c121 247
Zbyszek 11:366f1186c121 248 dataStatus.errorFlag = 0; //Initialise as 0 by default
Zbyszek 11:366f1186c121 249
Zbyszek 11:366f1186c121 250 firstSample = SampleFIFO[pointerFS][0]; //first sample loaded here
Zbyszek 11:366f1186c121 251 lastSample = SampleFIFO[pointerFS][11]; //last sample loaded here
Zbyszek 11:366f1186c121 252
Zbyszek 11:366f1186c121 253 firstSample &= ~(8191); //remove first 13 bits
Zbyszek 11:366f1186c121 254 firstSample = firstSample >> 13; //shift by right by 13
Zbyszek 11:366f1186c121 255 lastSample &= ~(8191); //remove first 13 bits
Zbyszek 11:366f1186c121 256 lastSample = lastSample >> 13; //shift by right by 13
Zbyszek 11:366f1186c121 257
Zbyszek 11:366f1186c121 258 if(firstSample != lastSample) { //Check if the IDs match
Zbyszek 11:366f1186c121 259 dataStatus.errorFlag = 1; //if both sample ID are not equal then batch is wrong
Zbyszek 11:366f1186c121 260 return; //Leave function early if error occured
Zbyszek 11:366f1186c121 261 }
Zbyszek 11:366f1186c121 262 else { //otherwise if both match
Zbyszek 11:366f1186c121 263 dataStatus.id = firstSample; //attach the status to dataStatus id
Zbyszek 11:366f1186c121 264 // printf("%d \n\r", dataStatus.id);
Zbyszek 11:366f1186c121 265 }
Zbyszek 11:366f1186c121 266
Zbyszek 11:366f1186c121 267 for(int x = 0; x <= 11; x++) {
Zbyszek 11:366f1186c121 268 id = SampleFIFO[pointerFS][x]; //Save sample to id for id extraction
Zbyszek 11:366f1186c121 269 id &= ~(255); //Remove the actual data to only be left with the id
Zbyszek 11:366f1186c121 270 id &= ~(57344); //Remove IMU identification data
Zbyszek 11:366f1186c121 271 id = id >> 8; //shift the id to the right for comparison
Zbyszek 11:366f1186c121 272
Zbyszek 11:366f1186c121 273 if(id != IDarray[x]) { //if the data identification does not match
Zbyszek 11:366f1186c121 274 if(blinkCounter == 200) {
Zbyszek 13:c7e8e277f884 275 // myled = !myled; //Toggle LED to signal that error occured
Zbyszek 11:366f1186c121 276 blinkCounter = 0;
Zbyszek 11:366f1186c121 277 }
Zbyszek 11:366f1186c121 278 else {
Zbyszek 11:366f1186c121 279 blinkCounter++;
Zbyszek 11:366f1186c121 280 }//if(blinkCounter == 200)
Zbyszek 11:366f1186c121 281
Zbyszek 11:366f1186c121 282 dataStatus.errorFlag = 1; //Raise errorFlag
Zbyszek 11:366f1186c121 283 break; //break out of the for loop
Zbyszek 11:366f1186c121 284 }//if(id != IDarray[x])
Zbyszek 11:366f1186c121 285 }//for(int x = 0; x <= 11; x++)
Zbyszek 11:366f1186c121 286
Zbyszek 11:366f1186c121 287 return dataStatus;
Zbyszek 11:366f1186c121 288
Zbyszek 11:366f1186c121 289 }//IMUcheck checkData(int16_t dataArray[10][12])
Zbyszek 4:e36c7042d3bb 290
Zbyszek 4:e36c7042d3bb 291
Zbyszek 9:9ed9dffd602a 292 //---------------------------------------------------------------------------OFFSET-CALIBRATION--------------------------------------------------------------------
Zbyszek 5:155d224d855c 293 void calibrateOffset() {
Zbyszek 5:155d224d855c 294
Zbyszek 5:155d224d855c 295 int16_t MSB = 0; //Store Most Significant Byte of data piece in this variable for processing
Zbyszek 5:155d224d855c 296 int16_t LSB = 0; //Store Least Significant Byte of data piece in this variable for processing
Zbyszek 5:155d224d855c 297 char arrPointer = 0; //Array Pointer
Zbyszek 5:155d224d855c 298
Zbyszek 5:155d224d855c 299 //-----------Concatentated-Data-Pieces------------------------------------------
Zbyszek 5:155d224d855c 300 int16_t gyroX = 0;
Zbyszek 5:155d224d855c 301 int16_t gyroY = 0;
Zbyszek 5:155d224d855c 302 int16_t gyroZ = 0;
Zbyszek 5:155d224d855c 303
Zbyszek 5:155d224d855c 304 int16_t accelX = 0;
Zbyszek 5:155d224d855c 305 int16_t accelY = 0;
Zbyszek 5:155d224d855c 306 int16_t accelZ = 0;
Zbyszek 5:155d224d855c 307
Zbyszek 5:155d224d855c 308 vector accelRaw;
Zbyszek 5:155d224d855c 309 vector accelG;
Zbyszek 5:155d224d855c 310 vector gyroRaw;
Zbyszek 5:155d224d855c 311 vector gyroDPS;
Zbyszek 5:155d224d855c 312
Zbyszek 5:155d224d855c 313 static unsigned int sampleCounter = 1;
Zbyszek 5:155d224d855c 314 static vector accelRawAvg;
Zbyszek 5:155d224d855c 315 static vector accelGAvg;
Zbyszek 5:155d224d855c 316 static vector gyroRawAvg;
Zbyszek 5:155d224d855c 317 static vector gyroDPSAvg;
Zbyszek 5:155d224d855c 318
Zbyszek 5:155d224d855c 319
Zbyszek 5:155d224d855c 320 for(char x = 0; x <= 5; x++) {
Zbyszek 5:155d224d855c 321 MSB = IMUarray[arrPointer]; //Odd data pieces are MSBs
Zbyszek 5:155d224d855c 322 MSB &= ~(255<<8); //Mask the MSB bits as they are not part of data
Zbyszek 5:155d224d855c 323 MSB = MSB << 8; //Shift the Value as its the MSB of the data piece
Zbyszek 5:155d224d855c 324 arrPointer++; //Increment array pointer
Zbyszek 5:155d224d855c 325 LSB = IMUarray[arrPointer]; //Even data pieces are LSBs
Zbyszek 5:155d224d855c 326 LSB &= ~(255 << 8); //Mask the MSB bits as they are not part of data
Zbyszek 5:155d224d855c 327 arrPointer++; //Increment array pointer
Zbyszek 5:155d224d855c 328
Zbyszek 5:155d224d855c 329 switch(x) {
Zbyszek 5:155d224d855c 330 case 0:
Zbyszek 5:155d224d855c 331 accelX = MSB + LSB; //Combine Accelerometer x-axis data
Zbyszek 5:155d224d855c 332 accelRaw.x = (double)accelX; //accelRaw
Zbyszek 5:155d224d855c 333 accelG.x = (double)accelX * 0.00006103515625f; //accelSSF
Zbyszek 5:155d224d855c 334
Zbyszek 5:155d224d855c 335 break;
Zbyszek 5:155d224d855c 336 case 1:
Zbyszek 5:155d224d855c 337 accelY = MSB + LSB; //Combine Accelerometer y-axis data
Zbyszek 5:155d224d855c 338 accelRaw.y = (double)accelY;
Zbyszek 5:155d224d855c 339 accelG.y = (double)accelY * 0.00006103515625f;
Zbyszek 5:155d224d855c 340 break;
Zbyszek 5:155d224d855c 341 case 2:
Zbyszek 5:155d224d855c 342 accelZ = MSB + LSB; //Combine Accelerometer z-axis data
Zbyszek 5:155d224d855c 343 accelRaw.z = (double)accelZ;
Zbyszek 5:155d224d855c 344 accelG.z = (double)accelZ * 0.00006103515625f;
Zbyszek 5:155d224d855c 345 break;
Zbyszek 5:155d224d855c 346 case 3:
Zbyszek 5:155d224d855c 347 gyroX = MSB + LSB; //Combine Gyroscope x-axis data
Zbyszek 5:155d224d855c 348 gyroRaw.x = (double)gyroX; //gyroRaw
Zbyszek 5:155d224d855c 349 gyroDPS.x = (double)gyroX * SSF; //gyroSSF
Zbyszek 5:155d224d855c 350
Zbyszek 5:155d224d855c 351 break;
Zbyszek 5:155d224d855c 352 case 4:
Zbyszek 5:155d224d855c 353 gyroY = MSB + LSB; //Combine Gyroscope y-axis data
Zbyszek 5:155d224d855c 354 gyroRaw.y = (double)gyroY;
Zbyszek 5:155d224d855c 355 gyroDPS.y = (double)gyroY * SSF;
Zbyszek 4:e36c7042d3bb 356
Zbyszek 5:155d224d855c 357 break;
Zbyszek 5:155d224d855c 358 case 5:
Zbyszek 5:155d224d855c 359 gyroZ = MSB + LSB; //Combine Gyroscope z-axis data
Zbyszek 5:155d224d855c 360 gyroRaw.z = (double)gyroZ;
Zbyszek 5:155d224d855c 361 gyroDPS.z = (double)gyroZ * SSF;
Zbyszek 5:155d224d855c 362 break;
Zbyszek 5:155d224d855c 363 default:
Zbyszek 5:155d224d855c 364 break;
Zbyszek 5:155d224d855c 365 }//switch(x)
Zbyszek 5:155d224d855c 366 }//for(char x = 0; x <= 5; x++)
Zbyszek 5:155d224d855c 367
Zbyszek 5:155d224d855c 368 //Take-Running-Averages------------------------------------------------------------------------
Zbyszek 5:155d224d855c 369 //Raw accel averages
Zbyszek 5:155d224d855c 370 accelRawAvg.x = accelRawAvg.x + ((accelRaw.x - accelRawAvg.x)/sampleCounter);
Zbyszek 5:155d224d855c 371 accelRawAvg.y = accelRawAvg.y + ((accelRaw.y - accelRawAvg.y)/sampleCounter);
Zbyszek 5:155d224d855c 372 accelRawAvg.z = accelRawAvg.z + ((accelRaw.z - accelRawAvg.z)/sampleCounter);
Zbyszek 5:155d224d855c 373
Zbyszek 5:155d224d855c 374 //SSF accel averages
Zbyszek 5:155d224d855c 375 accelGAvg.x = accelGAvg.x + ((accelG.x - accelGAvg.x)/sampleCounter);
Zbyszek 5:155d224d855c 376 accelGAvg.y = accelGAvg.y + ((accelG.y - accelGAvg.y)/sampleCounter);
Zbyszek 5:155d224d855c 377 accelGAvg.z = accelGAvg.z + ((accelG.z - accelGAvg.z)/sampleCounter);
Zbyszek 5:155d224d855c 378
Zbyszek 5:155d224d855c 379 //Raw gyroo averages
Zbyszek 5:155d224d855c 380 gyroRawAvg.x = gyroRawAvg.x + ((gyroRaw.x - gyroRawAvg.x)/sampleCounter);
Zbyszek 5:155d224d855c 381 gyroRawAvg.y = gyroRawAvg.y + ((gyroRaw.y - gyroRawAvg.y)/sampleCounter);
Zbyszek 5:155d224d855c 382 gyroRawAvg.z = gyroRawAvg.z + ((gyroRaw.z - gyroRawAvg.z)/sampleCounter);
Zbyszek 5:155d224d855c 383
Zbyszek 5:155d224d855c 384 //SSF gyro averages
Zbyszek 5:155d224d855c 385 gyroDPSAvg.x = gyroDPSAvg.x + ((gyroDPS.x - gyroDPSAvg.x)/sampleCounter);
Zbyszek 5:155d224d855c 386 gyroDPSAvg.y = gyroDPSAvg.y + ((gyroDPS.y - gyroDPSAvg.y)/sampleCounter);
Zbyszek 5:155d224d855c 387 gyroDPSAvg.z = gyroDPSAvg.z + ((gyroDPS.z - gyroDPSAvg.z)/sampleCounter);
Zbyszek 5:155d224d855c 388 //Take-Running-Averages------------------------------------------------------------------------
Zbyszek 4:e36c7042d3bb 389
Zbyszek 5:155d224d855c 390 //Print Messages-------------------------------------------------------------------------------
Zbyszek 5:155d224d855c 391 if(sampleCounter == 1) {
Zbyszek 5:155d224d855c 392 pc.printf("Collecting Raw and Sensitivity Scale Factor multiplied Gyroscope and Accelerometer Offsets...\n\r");
Zbyszek 5:155d224d855c 393 };
Zbyszek 5:155d224d855c 394
Zbyszek 5:155d224d855c 395
Zbyszek 5:155d224d855c 396 if(sampleCounter == 5000) {
Zbyszek 5:155d224d855c 397 pc.printf("RawAX RawAY RawAZ RawGX RawGY RawGZ SampleN\n\r");
Zbyszek 5:155d224d855c 398 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 399 pc.printf("\n\r");
Zbyszek 5:155d224d855c 400 pc.printf("\n\r");
Zbyszek 5:155d224d855c 401 pc.printf("\n\r");
Zbyszek 5:155d224d855c 402
Zbyszek 5:155d224d855c 403 //Add the sensitivity scale factor multiplied data
Zbyszek 5:155d224d855c 404 pc.printf("SSFAX SSFAY SSFAZ SSFGX SSFGY SSFGZ SampleN\n\r");
Zbyszek 5:155d224d855c 405 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 406
Zbyszek 5:155d224d855c 407 };
Zbyszek 5:155d224d855c 408 sampleCounter++;
Zbyszek 5:155d224d855c 409 //Print Messages-------------------------------------------------------------------------------
Zbyszek 5:155d224d855c 410 }
Zbyszek 9:9ed9dffd602a 411 //---------------------------------------------------------------------------OFFSET-CALIBRATION--------------------------------------------------------------------