Zbigniew Druzbacki
SPI slave program to enable communication between the FPGA and the STM32L432 board.
Diff: main.cpp
- Revision:
- 11:366f1186c121
- Parent:
- 10:5b96211275d4
- Child:
- 13:c7e8e277f884
--- a/main.cpp Fri Mar 08 01:33:51 2019 +0000
+++ b/main.cpp Tue Mar 19 01:26:11 2019 +0000
@@ -5,7 +5,7 @@
#include "Quaternions.h"
#include "DMA_SPI.h"
-//DigitalOut myled(LED1);
+DigitalOut myled(LED1);
Serial pc(USBTX, USBRX);
int masterRx = 0;
@@ -27,44 +27,24 @@
int OffsetYA = -131;
int OffsetZA = -531;
-IMU IMU0 (0, -306.0f, -131.0f, -351.0f, 254.0f, -14.0f, 81.0f, 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_Data Dat;
-vector Datt;
+vector Datt, Datt1;
int D2T;
Timer t;
float dTime = 0.0f;
+
+void calibrateOffset();
-vector vertical;
-vector globalAccel;
-vector correctionGlobalAccel;
-vector correctionBodyAccel;
-Quaternion gyroQ;
-vector Eangles;
-Quaternion CF;
-vector intGyro;
- vector GyroVals;
- vector AccelVals;
- vector accelTilt;
- Quaternion accelQ;
-
- void calibrateOffset();
-
-
-int stateRXNE = 0;
-int stateTXE = 0;
-int stateBSY = 0;
-int DMA1_CH2_Transfer_Complete_Flag = 0;
-int DMA1_CH3_Transfer_Complete_Flag = 0;
-int DMA1_CH2_Transfer_Error_Flag = 0;
-int DMA1_CH3_Transfer_Error_Flag = 0;
-int stateDMAch2interrupt = 0;
-int stateDMAch3interrupt = 0;
int flag = 0;
int flag2 = 0;
int rx_data = 0;
+int badSampleFlag = 0;
int16_t rxx;
+IMUcheck infoIMU;
/*
@@ -75,17 +55,24 @@
uint16_t IDarray[12]; //Holds the identification of each data piece
char idx = 0; //IMUarray Pointer
char dataCount = 0; //Keeps track of how many data points have been read in using SPI
-uint16_t id = 0; //Used to store extracted data ID
+
void ProcessAndPrint();
//------------------Printing-All-values----------------------//
//-------------Testing-Variables-Remove-Later----------------//
-//int blinkCounter = 0;
+int blinkCounter = 0;
//-------------Testing-Variables-Remove-Later----------------//
+
+//------------------------------------------------------------------------------Function Declarations
+IMUcheck checkData(int16_t dataArray[10][12]);
+//------------------------------------------------------------------------------Function Declarations
+
int main() {
- pc.baud (115200);
+ pc.baud(115200);
+
+
IDarray[0] = 1;
IDarray[1] = 0;
IDarray[2] = 9;
@@ -99,26 +86,24 @@
IDarray[10] = 19;
IDarray[11] = 18;
+ /*
+ IDarray[0] = 1 + 32;
+ IDarray[1] = 0 + 32;
+ IDarray[2] = 9 + 32;
+ IDarray[3] = 8 + 32;
+ IDarray[4] = 17 + 32;
+ IDarray[5] = 16 + 32;
+ IDarray[6] = 3 + 32;
+ IDarray[7] = 2 + 32; //2
+ IDarray[8] = 11 + 32;
+ IDarray[9] = 10 + 32;
+ IDarray[10] = 19 + 32;
+ IDarray[11] = 18 + 32;
+ */
//init_spi1();
t.start();
SPI_DMA_init();
- gyroQ.w = 1;
- gyroQ.x = 0.0001;
- gyroQ.y = 0.0001;
- gyroQ.z = 0.0001;
- CF.w = 1;
- CF.x = 0.0001;
- CF.y = 0.0001;
- CF.z = 0.0001;
-
- gyroQ = normaliseQuaternion(gyroQ);
- CF = normaliseQuaternion(CF);
-
- vertical.x = 0.0f;
- vertical.y = 0.0f;
- vertical.z = 1.0f;
-
data_to_transmit[0] = 1;
data_to_transmit[1] = 2;
@@ -132,152 +117,87 @@
data_to_transmit[9] = 10;
data_to_transmit[10] = 11;
data_to_transmit[11] = 12;
- //data_to_transmit[12] = 13;
while(1) {
-
- stateRXNE = SPI1->SR&0x01;
- stateTXE = SPI1->SR&0x02;
- stateBSY = SPI1->SR&(1u<<7);
-
- DMA1_CH2_Transfer_Complete_Flag = DMA1->ISR&(1u<<5);
- DMA1_CH3_Transfer_Complete_Flag = DMA1->ISR&(1u<<9);
- DMA1_CH2_Transfer_Error_Flag = DMA1->ISR&(1u<<7);
- DMA1_CH3_Transfer_Error_Flag = DMA1->ISR&(1u<<11);
- //D2T
+
- 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);
-
- if(DMA1->ISR&(1u<<5)) { //Check whether data read transfer is complete
- for(int x = 0; x <= 12; x++) {
- IMUarray[x] = received_data[x];
- }
- CLEAR_DMA1_CH2_IFCR_GFLAG(); //Clear global channel interrupt flag for channel 2
- }
-
- if(DMA1->ISR&(1u<<9)) { //Check whteher data transmit transfer is complete
- //Read data from the array that stores received data
- for(int x = 0; x <= 12; x++) {
- data_to_transmit[x] = x+1;
+ if(newDataFlag == 1) {
+ newDataFlag = 0;
+ infoIMU = checkData(SampleFIFO);
+ if(infoIMU.errorFlag == 0) {
+ for(int x = 0; x <= 11; x++) {
+ IMUarray[x] = SampleFIFO[pointerFS][x];
+ }
+ if(infoIMU.id == 0) {
+ Datt = IMU0.CalculateCFAngles(IMUarray);
+ //pc.printf("IMU 0\n\r");
+ }
+ else {
+ //pc.printf("IMU 1\n\r");
+ Datt1 = IMU1.CalculateCFAngles(IMUarray);
+ }
+ //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);
}
- CLEAR_DMA1_CH3_IFCR_GFLAG(); //Clear global channel interrupt flag for channel 3
- }
- /*
- if(DMA1->ISR&(1u<<11) && flag == 0) {
- if(SPI1->SR&(1u<<7) == 0) { //Check whether SPI is busy before disabling
- SPI1->CR1 &= ~SPI_CR1_SPE; //Disable the SPI
- DMA1->IFCR |= (1u << (4*(C3S - 1)));
- DMA1_Channel3->CCR |= (0x01<<CCR_EN);
- SPI1->CR1 |= SPI_CR1_SPE; //Enable SPI
- flag = 1;
- }
- }
-
- if(DMA1->ISR&(1u<<7) && flag2 == 0) {
- DMA1->IFCR |= (15u << (4*(C2S - 1)));
- flag2 = 1;
- }
-
- if(DMA1->ISR&(1u<<9) && flag == 0) {
- DMA1->IFCR |= (1u << (4*(C3S - 1)));
-
- }
-
- if(DMA1->ISR&(1u<<5) && flag2 == 0) {
- DMA1->IFCR |= (15u << (4*(C2S - 1)));
- for(int x = 0; x <= 10; x++) {
- rxx = received_data[x];
- }
- }
-
-*/
-
-
-
-
-
-
-
-
-
-
+ }//if(newDataFlag == 1)
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-//------------------------------------------------------------------------------
- /*
- if(i == 2) {
- //slaveRx = transfer_spi_slave(10); //get IMU data
-
-
- id = slaveRx; //Save sample to id for id extraction
- id &= ~(255); //get rid of the actual data to only be left with the id
- id = id >> 8; //shift the id to the right for comparison
-
- //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/
-
- if(IDarray[dataCount] == id) { //compare if the order of data is right and if not wait until it is.
- dataCount++; //Increase dataCount as new value has been read in.
- IMUarray[idx] = slaveRx; //save the newly read value to current free space in the array
- idx++; //increment the pointer to point to next free space in the array
-
- if(dataCount == 12) {
- //reset idx and dataCount
- dataCount = 0;
- idx = 0;
- //calibrateOffset();
- //IMU0.concatenateData(IMUarray);
- Datt = IMU0.CalculateQCFAngles(IMUarray);
-
-
- t.stop();
- dTime = t.read();
- t.reset();
- t.start();
- }//if(dataCount == 12)
- }//if(IDarray[dataCount] == id)
- else {
- //-----Code-Used-For-Testing-----//
- //pc.printf("Failed at: %d \n\r", dataCount); //Print an error if there is one
- // if(blinkCounter == 10) { //Slow the blinking down to make it visible if there are errors
- // myled = !myled; //Change state of the LED if error occurs
- // blinkCounter = 0; //Reset Blink counter
- // }
- // else {
- // blinkCounter++;
- //}
- //-----Code-Used-For-Testing-----//
-
- dataCount = 0; //ID sequence is worng so reset the counter
- idx = 0; //ID sequence is worng so reset the counter
- }
- }//if(i == 2)
-
- */
-
}
+
}
-
+IMUcheck checkData(int16_t dataArray[10][12]) {
+ int16_t firstSample, lastSample; //Used to check first and last sample of batch
+ uint16_t id = 0; //Used to store extracted data ID
+ IMUcheck dataStatus; //contains IMU id and error status
+
+ dataStatus.errorFlag = 0; //Initialise as 0 by default
+
+ firstSample = SampleFIFO[pointerFS][0]; //first sample loaded here
+ lastSample = SampleFIFO[pointerFS][11]; //last sample loaded here
+
+ firstSample &= ~(8191); //remove first 13 bits
+ firstSample = firstSample >> 13; //shift by right by 13
+ lastSample &= ~(8191); //remove first 13 bits
+ lastSample = lastSample >> 13; //shift by right by 13
+
+ if(firstSample != lastSample) { //Check if the IDs match
+ dataStatus.errorFlag = 1; //if both sample ID are not equal then batch is wrong
+ return; //Leave function early if error occured
+ }
+ else { //otherwise if both match
+ dataStatus.id = firstSample; //attach the status to dataStatus id
+ // printf("%d \n\r", dataStatus.id);
+ }
+
+ 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
+ id &= ~(57344); //Remove IMU identification data
+ id = id >> 8; //shift the id to the right for comparison
+
+ if(id != IDarray[x]) { //if the data identification does not match
+ if(blinkCounter == 200) {
+ myled = !myled; //Toggle LED to signal that error occured
+ blinkCounter = 0;
+ }
+ else {
+ blinkCounter++;
+ }//if(blinkCounter == 200)
+
+ dataStatus.errorFlag = 1; //Raise errorFlag
+ break; //break out of the for loop
+ }//if(id != IDarray[x])
+ }//for(int x = 0; x <= 11; x++)
+
+ return dataStatus;
+
+}//IMUcheck checkData(int16_t dataArray[10][12])
//---------------------------------------------------------------------------OFFSET-CALIBRATION--------------------------------------------------------------------