Diff: main.cpp
- Revision:
- 13:c7e8e277f884
- Parent:
- 11:366f1186c121
- Child:
- 14:7bbaafa22f8d
--- a/main.cpp Mon Mar 25 02:15:44 2019 +0000
+++ b/main.cpp Fri Apr 19 18:43:39 2019 +0000
@@ -1,13 +1,16 @@
#include "mbed.h"
#include "SPI.h"
#include "IMUs.h"
-#include "Structures.h"
+#include "CustomDatatypes.h"
#include "Quaternions.h"
#include "DMA_SPI.h"
DigitalOut myled(LED1);
Serial pc(USBTX, USBRX);
+InterruptIn dataRequest(PA_0);
+
+
int masterRx = 0;
int16_t slaveRx = 0;
int i = 2;
@@ -31,6 +34,7 @@
IMU IMU1 (0, -306.0f, -131.0f, -351.0f, 254.0f, -14.0f, 81.0f, 0, 0);
IMU_Data Dat;
vector Datt, Datt1;
+vector IMU0_Data, IMU1_Data, IMU2_Data;
int D2T;
@@ -47,6 +51,9 @@
IMUcheck infoIMU;
+char IMU_Data_Pointer = 0; //Points to the IMU whose turn it is to send data.
+
+
/*
*/
@@ -69,7 +76,72 @@
IMUcheck checkData(int16_t dataArray[10][12]);
//------------------------------------------------------------------------------Function Declarations
+void requestISR(void) {
+ dataLoadedFlag = 0;
+ dataRequestFlag = 1; //Let L432 know that data has been requested
+ myled = !myled; //Flash LED to indicate
+
+ if(IMU_Data_Pointer == 2) { //Was previous data sent IMU2?
+ IMU_Data_Pointer = 0; //Yes - reset back to IMU0
+ }
+ else {
+ IMU_Data_Pointer++; //No - point to the next IMU data to be sent.
+ }
+}
+
+//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
+
+ for(int x = 0; x <= 11; x++) { //Use the for loop to load data into correct places in the array
+ switch(x) {
+ case 0:
+ IMU_Data_Array[x] = imuID;
+ break;
+
+ case 1:
+ data.f = IMUn.x; //Place x-axis oreintation data into the union float
+ IMU_Data_Array[x] = data.n[0]; //save MSB 16 bits to array
+ break;
+
+ case 2:
+ data.f = IMUn.x; //Place x-axis oreintation data into the union float
+ IMU_Data_Array[x] = data.n[1]; //save LSB 16 bits to array
+ break;
+
+ case 3:
+ data.f = IMUn.y; //Place y-axis oreintation data into the union float
+ IMU_Data_Array[x] = data.n[0]; //save MSB 16 bits to array
+ break;
+
+ case 4:
+ data.f = IMUn.y; //Place y-axis oreintation data into the union float
+ IMU_Data_Array[x] = data.n[1]; //save LSB 16 bits to array
+ break;
+
+ case 5:
+ data.f = IMUn.z; //Place z-axis oreintation data into the union float
+ IMU_Data_Array[x] = data.n[0]; //save MSB 16 bits to array
+ break;
+
+ case 6:
+ data.f = IMUn.z; //Place z-axis oreintation data into the union float
+ IMU_Data_Array[x] = data.n[1]; //save LSB 16 bits to array
+ break;
+
+ default:
+ IMU_Data_Array[x] = 0;
+ break;
+ }//switch(x)
+ }//for(int x = 0; x <= 11; x++) {
+}//void prepareSPItx
+
int main() {
+
+ IMU_Data_Pointer = 2; //Intially 2 but will be reset to zero when first request signal comes in.
+
+ dataRequest.rise(&requestISR);
+
pc.baud(115200);
@@ -133,20 +205,37 @@
}
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);
}
}//if(newDataFlag == 1)
-
+ //Load appropriate data into array for transmission
+ if(dataLoadedFlag == 0) { //if data hasnt been loaded when request occured then load data
+ switch(IMU_Data_Pointer) {
+ case 0:
+ prepareSPItx(0, IMU0_Data); //IMU0 Data
+ break;
+
+ case 1:
+ prepareSPItx(1, IMU1_Data); //IMU1 Data
+ break;
+
+ case 2:
+ prepareSPItx(2, IMU2_Data); //IMU2 Data
+ break;
+
+ default:
+ break;
+ }//switch(IMU_Data_Pointer)
+ dataLoadedFlag = 1;
}
-
-}
+
+ }//while(1)
+}//int main()
@@ -183,7 +272,7 @@
if(id != IDarray[x]) { //if the data identification does not match
if(blinkCounter == 200) {
- myled = !myled; //Toggle LED to signal that error occured
+ // myled = !myled; //Toggle LED to signal that error occured
blinkCounter = 0;
}
else {
@@ -320,30 +409,3 @@
//Print Messages-------------------------------------------------------------------------------
}
//---------------------------------------------------------------------------OFFSET-CALIBRATION--------------------------------------------------------------------
-
-
-
-
-
-
-
-//------------------------------------------Artifacts----------------------------------------
-
- //----------------------Insert Whole---------------------------------------------------
- //Rotate the accel data Global reference frame by qvq*---------------------------------
- //globalAccel = rotateGlobal(AccelVals, gyroQ);
- //Rotate the accel data Global reference frame by qvq*---------------------------------
-
- //get the correction values and rotate back to IMU reference---------------------------
- // correctionGlobalAccel = crossProduct(globalAccel, vertical);
- // correctionBodyAccel = rotateLocal(correctionGlobalAccel, gyroQ);
- //get the correction values and rotate back to IMU reference---------------------------
-
- //Add the correction to gyro readings and update the quaternion------------------------
- //GyroVals = sumVector(GyroVals, correctionBodyAccel);
- //incRot = toQuaternionNotation123(GyroVals, dTime);
- //gyroQ = getQuaternionProduct(gyroQ, incRot);
- //gyroQ = normaliseQuaternion(gyroQ);
- //----------------------Insert Whole---------------------------------------------------
-
-//------------------------------------------Artifacts----------------------------------------
\ No newline at end of file