Zbigniew Druzbacki
SPI slave program to enable communication between the FPGA and the STM32L432 board.
Revision 15:791f35b0f220, committed 2019-05-15
- Comitter:
- Zbyszek
- Date:
- Wed May 15 22:56:20 2019 +0000
- Parent:
- 14:7bbaafa22f8d
- Commit message:
- Official Code used on the 15/05/2019
Changed in this revision
--- a/DMA_SPI.cpp Sun May 05 01:08:22 2019 +0000
+++ b/DMA_SPI.cpp Wed May 15 22:56:20 2019 +0000
@@ -30,6 +30,7 @@
startCommunication();
}
+//-----------------------------------------------Step-5-----------------------------------------------------------------------------------------------------------
/* Starting DMA communication according to STM32L432 Reference Manual p1317-p1318*/
void startCommunication() {
@@ -149,91 +150,97 @@
void SPI_DMA_SLAVE_init() {
- RCC->AHB2ENR|= (RCC_AHB2ENR_GPIOAEN); //GPIO A clock enable
- RCC->AHB2ENR |= (RCC_AHB2ENR_GPIOBEN);
- RCC->APB2ENR|=RCC_APB2ENR_SPI1EN; //Enable SPI1 Clock
+ //-----------------------------------------------Step-1-----------------------------------------------------------------------------------------------------------
+ RCC->AHB2ENR|= (RCC_AHB2ENR_GPIOAEN); //GPIO A clock enable by setting the GPIOAEN bit in the RCC_AHB2ENR register
+ RCC->AHB2ENR |= (RCC_AHB2ENR_GPIOBEN); //GPIO B clock enable by setting the GPIOBEN bit in the RCC_AHB2ENR register
+ RCC->APB2ENR|=RCC_APB2ENR_SPI1EN; //Enable SPI1 Clock by setting the SPI1EN bit in the RCC_APB2ENR register
- //SET SCK, MISO, MOSI and CS pins
+ //-----------------------------------------------Step-2-----------------------------------------------------------------------------------------------------------
+ //SET SCK, MISO, MOSI and CS pins
GPIOA->MODER|=(
- (2u<<(2*SCK_slave))
- |(2u<<(2*MISO_slave))
- |(2u<<(2*MOSI_slave))
+ (2u<<(2*SCK_slave)) //Set pin 1 in the MODER register of port A as alternate function
+ |(2u<<(2*MISO_slave)) //Set pin 6 in the MODER register of port A as alternate function
+ |(2u<<(2*MOSI_slave)) //Set pin 7 in the MODER register of port A as alternate function
//|(2u<<(2*4))
);
//Set PB_0 to alternate function
- GPIOB->MODER |= (2u<<0);
-
+ GPIOB->MODER |= (2u<<0); //Set pin 0 in the MODER register of port B as alternate function
+
+ //-----------------------------------------------Step-3-----------------------------------------------------------------------------------------------------------
//SET pins to function as SPI pins
- GPIOA->AFR[0]|=(
- (5u<<(4*SCK_slave))
- |(5u<<(4*MISO_slave))
- |(5u<<(4*MOSI_slave))
+ GPIOA->AFR[0]|=(
+ (5u<<(4*SCK_slave)) //Set the alternate function of pin 1 as SPI clock pin by writing 1001 to the lower part of the AFR register of port A.
+ |(5u<<(4*MISO_slave)) //Set the alternate function of pin 6 as SPI MISO pin by writing 1001 to the lower part of the AFR register of port A.
+ |(5u<<(4*MOSI_slave)) //Set the alternate function of pin 7 as SPI clock pin by writing 1001 to the lower part of the AFR register of port A.
//|(5u<<(4*4))
);
//Select SPI1_SSEL alternate function
- GPIOB->AFR[0] |= (5u<<(4*0));
-
+ GPIOB->AFR[0] |= (5u<<(4*0)); //Set the alternate function of pin 0 as SPI slave select pin by writing 1001 to the lower part of the AFR register of port B.
+
+ //-----------------------------------------------Step-4-----------------------------------------------------------------------------------------------------------
SET_SPI1_CR1_BR_BITS(); //baud rate bits set 1/16 giving 1MHz SCK frequency
+ //-----------------------------------------------Step-5-----------------------------------------------------------------------------------------------------------
SET_SPI1_CR1_CPOL_BIT(); //CPOL = 1
SET_SPI1_CR1_CPHA_BIT(); //CPHA = 1
-
+ //-----------------------------------------------Step-6-----------------------------------------------------------------------------------------------------------
SET_SPI1_CR2_DS_BITS(); //Data Size = 16 bits
// SET_SPI1_CR2_RXDMAEN_BIT(); //Rx buffer DMA enable
- // SET_SPI1_CR2_TXDMAEN_BIT(); //Tx buffer DMA enable
-
-
+ // SET_SPI1_CR2_TXDMAEN_BIT(); //Tx buffer DMA enable
}
-
-
+ //SET_DMA1_CH2_CCR_PINC_BIT(); //Peripheral increment mode
+ // SET_DMA1_CH3_CCR_PINC_BIT(); //Peripheral increment mode
+
void initDMA() {
+ //-----------------------------------------------Step-1-----------------------------------------------------------------------------------------------------------
RCC->AHB1ENR|= (RCC_AHB1ENR_DMA1EN); //Enable the DMA1 clock
+ //-----------------------------------------------Step-2-----------------------------------------------------------------------------------------------------------
DMA1_CH2_DISABLE(); //Disable DMA channel 2
DMA1_CH3_DISABLE(); //Disable DMA channel 3
SET_DMA1_SPI1RX_CSELR_BITS(); //Select SPI1_Rx on DMA1 Channel 2
SET_DMA1_SPI1TX_CSELR_BITS(); //Select SPI1_Tx on DMA1 Channel 3
-
+
+ //-----------------------------------------------Step-3-----------------------------------------------------------------------------------------------------------
//-----------------------------------------------Receive-----------------------------------------------------
- CLEAR_DMA1_CH2_CCR_DIR_BIT(); //Peripheral->Memory
- SET_DMA1_CH2_CCR_PSIZE_BITS(); //16 bits
- SET_DMA1_CH2_CCR_MSIZE_BITS(); //16 bits
- SET_DMA1_CH2_CCR_MINC_BIT(); //Memory increment mode
- //SET_DMA1_CH2_CCR_PINC_BIT(); //Peripheral increment mode
- SET_DMA1_CH2_CCR_TCIE_BIT(); //Transfer complete interrupt enable
- SET_DMA1_CH2_CCR_CIRC_BIT(); //Circular Buffer mode
+ CLEAR_DMA1_CH2_CCR_DIR_BIT(); //Direction bit: 0 = Peripheral->Memory. Data will be loaded from SPI data register to the specified variable
+ SET_DMA1_CH2_CCR_PSIZE_BITS(); //peripheral size bit: 1 = 16 bits. The size is set to 16 bits as thats the data size SPI will work in.
+ SET_DMA1_CH2_CCR_MSIZE_BITS(); //Memory size bit: 1 = 16 bits. The size is set to 16 bits as data coming from SPI is 16 bits wide
+ SET_DMA1_CH2_CCR_MINC_BIT(); //Memory increment mode but: 1 = Increment memory after each transaction. Used when multiple transaction are done before interrupt occurs
+ SET_DMA1_CH2_CCR_TCIE_BIT(); //Transfer complete interrupt enable: 1 = enabled. Allows DMA to interrupt software when number of transaction has occured.
+ SET_DMA1_CH2_CCR_CIRC_BIT(); //Circular Buffer mode: 1 = enabled. After n transaction, the address is reset to starting memory address automatically.
SET_DMA1_CH2_CCR_PL_BITS(); //Priority Level = Highest
- DMA1_Channel2->CNDTR = 12; //number of data to transfer from the peripheral to memory.
+ DMA1_Channel2->CNDTR = 12; //number of data to transfer from the peripheral to memory before interrupt occurs.
DMA1_Channel2->CPAR = (int32_t)&SPI1->DR; //Source Adddress = SPI data register
- DMA1_Channel2->CMAR = (int32_t)&received_data[0]; //Destination address = received_data array
+ DMA1_Channel2->CMAR = (int32_t)&received_data[0]; //Destination address = received_data array.
//-----------------------------------------------Receive-----------------------------------------------------
+
+
//-----------------------------------------------Transmission------------------------------------------------
- SET_DMA1_CH3_CCR_DIR_BIT(); //Memory->Peripheral
- SET_DMA1_CH3_CCR_PSIZE_BITS(); //16 bits
- SET_DMA1_CH3_CCR_MSIZE_BITS(); //16 bits
+ SET_DMA1_CH3_CCR_DIR_BIT(); //Direction bit: 1 = Memory->Peripheral. Data will be sent from variable to SPI data register for transmission
+ SET_DMA1_CH3_CCR_PSIZE_BITS(); //peripheral size bit: 1 = 16 bits. The size is set to 16 bits as thats the data size SPI will work in.
+ SET_DMA1_CH3_CCR_MSIZE_BITS(); //Memory size bit: 1 = 16 bits. The size is set to 16 bits as data coming from SPI is 16 bits wide
SET_DMA1_CH3_CCR_MINC_BIT(); //Memory increment mode
- // SET_DMA1_CH3_CCR_PINC_BIT(); //Peripheral increment mode
SET_DMA1_CH3_CCR_TCIE_BIT(); //Transfer complete interrupt enable
SET_DMA1_CH3_CCR_CIRC_BIT(); //Circular Buffer mode
SET_DMA1_CH3_CCR_PL_BITS(); //Priority Level = Highest
- DMA1_Channel3->CNDTR = 12; //number of data to transfer from memory to the peripheral
+ DMA1_Channel3->CNDTR = 12; //number of data to transfer from memory to the peripheral to be transmitted.
DMA1_Channel3->CPAR = (int32_t)&SPI1->DR; //Destination address = SPI data register
- DMA1_Channel3->CMAR = (int32_t)&data_to_transmit[0]; //Source address = data_to_transmit
+ DMA1_Channel3->CMAR = (int32_t)&data_to_transmit[0]; //Source address = data_to_transmit: Address from which data will be loaded to the SPI data register for transmission
//-----------------------------------------------Transmission------------------------------------------------
-
-
- NVIC->ISER[0] |= (1u<<12); //Enable DMA1 channel 2 interrupt
- NVIC->ISER[0] |= (1u<<13); //Enable DMA1 channel 3 interrupt
+//-----------------------------------------------Step-4-----------------------------------------------------------------------------------------------------------
+ //Enable DMA1 channel 2 and 3 interrupts
+ NVIC->ISER[0] |= (1u<<12);
+ NVIC->ISER[0] |= (1u<<13);
NVIC_EnableIRQ(DMA1_Channel2_IRQn);
- NVIC_EnableIRQ(DMA1_Channel3_IRQn);
-
+ NVIC_EnableIRQ(DMA1_Channel3_IRQn);
}
@@ -241,16 +248,11 @@
extern "C" void DMA1_Channel2_IRQHandler(void) {
uint16_t n;
- CLEAR_DMA1_CH2_IFCR_GFLAG(); //Clear Global Interrupt flag
- for(int x = 0; x <= 11; x++) {
-
- n = received_data[x];
- n &= ~(8191); //remove first 13 bits
- n = n >> 13; //shift by right by 13
- SampleFIFO[pointerFS][x] = received_data[x];
- // printf("%d \n\r", n);
+ CLEAR_DMA1_CH2_IFCR_GFLAG(); //Clear Global Interrupt flag so that the nterrupt can occur again once a new batch of data is received.
+ for(int x = 0; x <= 11; x++) { //run the for loop to read new data from the array that the SPI uses to save the received data to a new array to avoid overwritting
+ SampleFIFO[pointerFS][x] = received_data[x]; //Just move the data from one array to the other.
}
- newDataFlag = 1;
+ newDataFlag = 1; //Once new data has been read set this flag to notify software that new data is received so that it proceeds with processing it.
}
--- a/DMA_SPI.h Sun May 05 01:08:22 2019 +0000 +++ b/DMA_SPI.h Wed May 15 22:56:20 2019 +0000 @@ -18,7 +18,7 @@ #define CCR_MEM2MEM 14 -#define SPI1_DISABLE() SPI1->CR1 &= ~SPI_CR1_SPE +#define SPI1_DISABLE() SPI1->CR1 &= ~SPI_CR1_SPE //Clear SPE bit in SPI1 CR1 register #define SPI1_ENABLE() SPI1->CR1 |= SPI_CR1_SPE #define CLEAR_SPI1_CR1_CRC_BIT() SPI1->CR1 &= ~SPI_CR1_CRCEN
--- a/IMUs.cpp Sun May 05 01:08:22 2019 +0000
+++ b/IMUs.cpp Wed May 15 22:56:20 2019 +0000
@@ -6,46 +6,48 @@
//IMU Class Constructor
IMU::IMU(char IMU_ID, double OffsetAX, double OffsetAY, double OffsetAZ, double OffsetGX, double OffsetGY, double OffsetGZ, char SSFA, char SSFG) {
- AccelerometerOffset.x = OffsetAX;
- AccelerometerOffset.y = OffsetAY;
- AccelerometerOffset.z = OffsetAZ;
+ this->AccelerometerOffset.x = OffsetAX;
+ this->AccelerometerOffset.y = OffsetAY;
+ this->AccelerometerOffset.z = OffsetAZ;
- GyroscopeOffset.x = OffsetGX;
- GyroscopeOffset.y = OffsetGY;
- GyroscopeOffset.z = OffsetGZ;
+ this->GyroscopeOffset.x = OffsetGX;
+ this->GyroscopeOffset.y = OffsetGY;
+ this->GyroscopeOffset.z = OffsetGZ;
IMU_Identifier = IMU_ID;
+ //Accelerometer Sensitvity Scale Factor reciprocals
switch(SSFA) {
case 0:
- accelSSF = 0.00006103515625f;
+ accelSSF = 0.00006103515625f; //Sensitivity Scale Factor setting 0 reciprocal
break;
case 1:
- accelSSF = 0.0001220703125f;
- break;
+ accelSSF = 0.0001220703125f; //Sensitivity Scale Factor setting 1 reciprocal
+ break;
case 2:
- accelSSF = 0.000244140625f;
+ accelSSF = 0.000244140625f; //Sensitivity Scale Factor setting 2 reciprocal
break;
case 3:
- accelSSF = 0.00048828125f;
+ accelSSF = 0.00048828125f; //Sensitivity Scale Factor setting 3 reciprocal
break;
default:
break;
}
+ //Gyroscope Sensitivity Scale Factor reciprocals
switch(SSFG) {
case 0:
- gyroSSF = 0.00763358778625954198473282442748f;
+ gyroSSF = 0.00763358778625954198473282442748f; //Sensitivity Scale Factor setting 0 reciprocal
break;
case 1:
- gyroSSF = 0.01526717557251908396946564885496f;
+ gyroSSF = 0.01526717557251908396946564885496f; //Sensitivity Scale Factor setting 1 reciprocal
break;
case 2:
- gyroSSF = 0.03048780487804878048780487804878f;
+ gyroSSF = 0.03048780487804878048780487804878f; //Sensitivity Scale Factor setting 2 reciprocal
break;
case 3:
- gyroSSF = 0.06097560975609756097560975609756f;
+ gyroSSF = 0.06097560975609756097560975609756f; //Sensitivity Scale Factor setting 3 reciprocal
break;
default:
break;
@@ -68,9 +70,7 @@
int16_t MSB = 0; //Store Most Significant Byte of data piece in this variable for processing
int16_t LSB = 0; //Store Least Significant Byte of data piece in this variable for processing
char arrPointer = 0; //Array Pointer
-
IMU_Data LocalD;
-
//Procedure--------------Procedure------------Procedure-------------Procedure---
for(char x = 0; x <= 5; x++) {
MSB = SamplesPieces[arrPointer]; //Odd data pieces are MSBs
@@ -83,30 +83,29 @@
switch(x) {
case 0:
- LocalD.Ax = (MSB + LSB) + AccelerometerOffset.x; //Combine Accelerometer x-axis data
+ LocalD.Ax = (MSB + LSB) + this->AccelerometerOffset.x; //Combine Accelerometer x-axis data and apply offset cancellation value
break;
case 1:
- LocalD.Ay = (MSB + LSB) + AccelerometerOffset.y; //Combine Accelerometer y-axis data
+ LocalD.Ay = (MSB + LSB) + this->AccelerometerOffset.y; //Combine Accelerometer y-axis data and apply offset cancellation value
break;
case 2:
- LocalD.Az = (MSB + LSB) + AccelerometerOffset.z; //Combine Accelerometer z-axis data
+ LocalD.Az = (MSB + LSB) + this->AccelerometerOffset.z; //Combine Accelerometer z-axis data and apply offset cancellation value
break;
case 3:
- LocalD.Gx = (MSB + LSB) + GyroscopeOffset.x; //Combine Gyroscope x-axis data
+ LocalD.Gx = (MSB + LSB) + this->GyroscopeOffset.x; //Combine Gyroscope x-axis data and apply offset cancellation value
break;
case 4:
- LocalD.Gy = (MSB + LSB) + GyroscopeOffset.y; //Combine Gyroscope y-axis data
+ LocalD.Gy = (MSB + LSB) + this->GyroscopeOffset.y; //Combine Gyroscope y-axis data and apply offset cancellation value
break;
case 5:
- LocalD.Gz = (MSB + LSB) + GyroscopeOffset.z; //Combine Gyroscope z-axis data
+ LocalD.Gz = (MSB + LSB) + this->GyroscopeOffset.z; //Combine Gyroscope z-axis data and apply offset cancellation value
break;
default:
break;
}//switch(x)
}//for(char x = 0; x <= 5; x++)
- //printf("Accel X: %+2f, Accel Y: %+2f, Accel Z: %+2f, Gyro X: %+2f, Gyro Y: %+2f, Gyro Z: %+2f\n\r", LocalD.Ax, LocalD.Ay, LocalD.Az, LocalD.Gx, LocalD.Gy, LocalD.Gz);
-
+ //printf("Accel X: %+2f, Accel Y: %+2f, Accel Z: %+2f, Gyro X: %+2f, Gyro Y: %+2f, Gyro Z: %+2f\n\r", LocalD.Ax, LocalD.Ay, LocalD.Az, LocalD.Gx, LocalD.Gy, LocalD.Gz);
return LocalD;
}
//----------------------------------------------------------------------------------------------------------------------------------------------------------
@@ -120,14 +119,16 @@
IMU_Data localD;
-
- localD.Ax = RawData.Ax * accelSSF;
- localD.Ay = RawData.Ay * accelSSF;
- localD.Az = RawData.Az * accelSSF;
+ //Multiply the raw data by sensitivity scale factor to convert:
+ //Accelerometer data to gravity in g.
+ localD.Ax = RawData.Ax * this->accelSSF;
+ localD.Ay = RawData.Ay * this->accelSSF;
+ localD.Az = RawData.Az * this->accelSSF;
- localD.Gx = RawData.Gx * gyroSSF;
- localD.Gy = RawData.Gy * gyroSSF;
- localD.Gz = RawData.Gz * gyroSSF;
+ //angular velocity to degrees per second
+ localD.Gx = RawData.Gx * this->gyroSSF;
+ localD.Gy = RawData.Gy * this->gyroSSF;
+ localD.Gz = RawData.Gz * this->gyroSSF;
//printf("Accel X: %+2f, Accel Y: %+2f, Accel Z: %+2f, Gyro X: %+2f, Gyro Y: %+2f, Gyro Z: %+2f\n\r", LocalD.Ax, LocalD.Ay, LocalD.Az, LocalD.Gx, LocalD.Gy, LocalD.Gz);
return localD;
@@ -145,15 +146,18 @@
newValues = SSF_Format_Values;
+ //use trig functions to calculate tilt angles based on accelerometer values
+ //The function returns data in radains and therefore convert to angle by multiplying by 57.295.....
AngleData.Ax = (atan2f(newValues.Ay, newValues.Az) * 57.29577951f);
AngleData.Ay = (atan2f((-newValues.Ax), sqrt(pow(newValues.Ay, 2) + pow(newValues.Az, 2) )) * 57.29577951f);
AngleData.Az = 0; //Cannot calculate angle for this one as it remains constant all the time
+ //Integrate with respect to time to get change in angle.
AngleData.Gx = (newValues.Gx * dt);
AngleData.Gy = (newValues.Gy * dt);
AngleData.Gz = (newValues.Gz * dt);
- //printf("Accel X: %+2f, Accel Y: %+2f, Accel Z: %+2f, Gyro X: %+2f, Gyro Y: %+2f, Gyro Z: %+2f dt: %2f\n\r", AngleData.Ax, AngleData.Ay, AngleData.Az, AngleData.Gx, AngleData.Gy, AngleData.Gz, dt);
+ //return the data
return AngleData;
}
@@ -213,30 +217,28 @@
}
//----------------------------------------------------------------------------------------------------------------------------------------------------------
-
+//Complementary filter calculation function
vector IMU::CalculateCFAngles(int16_t SamplesPieces[12]) {
IMU_Data ConcatenatedValues,NewAngle, SSFValues;
float deltaT;
+ static vector pureGANGLE;
- this->t.stop();
- deltaT = this->t.read();
- ConcatenatedValues = this->concatenateData(SamplesPieces);
- SSFValues = this->SSFmultiply(ConcatenatedValues);
- NewAngle = this->getAngles(SSFValues, deltaT);
+ this->t.stop(); //Stop timer
+ deltaT = this->t.read(); //read how much time has passed since last sample i.e sample period
+ ConcatenatedValues = this->concatenateData(SamplesPieces); //Get Sample pieces and join together to from actual useful value
+ SSFValues = this->SSFmultiply(ConcatenatedValues); //Multiply the values by sensitivity scale factor to get angular velocity in deg per sec and acceleration of gravity in g
+ NewAngle = this->getAngles(SSFValues, deltaT); //calculate the accelerometer angles and the change in angle from the new gyroscope data
- this->CFAngle.x = 0.98f*(CFAngle.x + NewAngle.Gx) + 0.02f*NewAngle.Ax;
- this->CFAngle.y = 0.98f*(CFAngle.y + NewAngle.Gy) + 0.02f*NewAngle.Ay;
- this->CFAngle.z = 0.98f*(CFAngle.z + NewAngle.Gz) + 0.02f*NewAngle.Az;
+ this->CFAngle.x = 0.98f*(CFAngle.x + NewAngle.Gx) + 0.02f*NewAngle.Ax; //Complementary filter for x-axis
+ this->CFAngle.y = 0.98f*(CFAngle.y + NewAngle.Gy) + 0.02f*NewAngle.Ay; //Complementary filter for y-axis
+ this->CFAngle.z = 0.98f*(CFAngle.z + NewAngle.Gz) + 0.02f*NewAngle.Az; //This in fact will just return the z-axis angle to zero all the time as accelerometer tilt in this axis is zero
+
+ this->t.reset(); //reset timer to count until next sample arrives
+ this->t.start(); //Start the timer again to start counting
- t.reset();
- t.start();
-
- //printf("Accel X: %+2f, Accel Y: %+2f, Accel Z: %+2f, dt: %+2f\n\r", CFAngle.x, CFAngle.y, CFAngle.z, deltaT);
- //printf("%+2f,%+2f,%+2f\n\r", CFAngle.x, CFAngle.y, CFAngle.z);
- return CFAngle;
-
+ return CFAngle; //Return the calculated complementary filter angles
}
--- a/main.cpp Sun May 05 01:08:22 2019 +0000
+++ b/main.cpp Wed May 15 22:56:20 2019 +0000
@@ -22,9 +22,9 @@
***Accelerometer Sensitivity Scale Factor selection number 0 - 3
***Gyroscope Sensitivity Scale Factor selection number 0 - 3
*/
-IMU IMU0 (0, -306.0f, -131.0f, -351.0f, 254.0f, -14.0f, 81.0f, 0, 0);
-IMU IMU1 (0, -306.0f, -131.0f, -351.0f, 254.0f, -14.0f, 81.0f, 0, 0);
-IMU IMU2 (0, -306.0f, -131.0f, -351.0f, 254.0f, -14.0f, 81.0f, 0, 0);
+IMU IMU0 (0, 2354.0f, 3128.0f, 1674.0f, -737.0f, -609.0f, -135.0f, 0, 0);
+IMU IMU1 (0, 7738.0f, -6734.0f, 2333.0f, -1364.0f, 234.0f, 49.0f, 0, 0);
+IMU IMU2 (0, -107.0f, -494.0f, -631.0f, -19.0f, -137.0f, -71.0f, 0, 0);
//---------------------------IMU-Objects-Initialisation-----------------------------------------//
//These typedefs contain x,y,z orientation data
@@ -77,7 +77,7 @@
//Used to split data into SPI managable chunks
void prepareSPItx(int imuID, vector IMUn) {
- txSPIUnion data; //Use union to split float into 2 16-bit values
+ txSPIUnion data; //Use a union data type to split float into 2 16-bit values
for(int x = 0; x <= 11; x++) { //Use the for loop to load data into correct places in the array
@@ -148,7 +148,7 @@
IMU2_Data.z = 53;
//Used to identify each incoming data piece.
- //Also checks whether order is correct which is important.
+ //Also checks whether order is correct and no corruption has occured.
IDarray[0] = 1;
IDarray[1] = 0;
IDarray[2] = 9;
@@ -193,7 +193,7 @@
break;
}
//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);
- pc.printf("IMU 0: X = %+2f, Y = %+2f, Z = %+2f\n\r", IMU1_Data.x, IMU1_Data.y, IMU1_Data.z);
+ //pc.printf("IMU 0: X = %+2f, Y = %+2f, Z = %+2f\n\r", IMU1_Data.x, IMU1_Data.y, IMU1_Data.z);
}
}//if(newDataFlag == 1)
@@ -234,6 +234,7 @@
firstSample = SampleFIFO[pointerFS][0]; //first sample loaded here
lastSample = SampleFIFO[pointerFS][11]; //last sample loaded here
+ //Get ID number
firstSample &= ~(8191); //remove first 13 bits
firstSample = firstSample >> 13; //shift by right by 13
lastSample &= ~(8191); //remove first 13 bits
@@ -247,6 +248,7 @@
dataStatus.id = firstSample; //attach the status to dataStatus id
}
+ //Check each data piece
for(int x = 0; x <= 11; x++) {
id = SampleFIFO[pointerFS][x]; //Save sample to id for id extraction
id &= ~(255); //Remove the actual data to only be left with the id
@@ -260,7 +262,6 @@
}//for(int x = 0; x <= 11; x++)
return dataStatus;
-
}//IMUcheck checkData(int16_t dataArray[10][12])