Zbigniew Druzbacki / Mbed 2 deprecated SPI_Slave

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

Dependencies:   mbed

main.cpp@15:791f35b0f220, 2019-05-15 (annotated)

Committer:
Zbyszek
Date:
Wed May 15 22:56:20 2019 +0000
Revision:
15:791f35b0f220
Parent:
14:7bbaafa22f8d 
Official Code used on the 15/05/2019

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