SPI slave program to enable communication between the FPGA and the STM32L432 board.
Diff: main.cpp
- Revision:
- 6:0ebecfecadc9
- Parent:
- 5:155d224d855c
- Child:
- 7:0e9af5986488
--- a/main.cpp Sun Feb 24 16:49:06 2019 +0000
+++ b/main.cpp Tue Feb 26 01:22:53 2019 +0000
@@ -1,5 +1,7 @@
#include "mbed.h"
#include "SPI.h"
+#include "IMUs.h"
+#include "Structures.h"
#include "Quaternions.h"
DigitalOut myled(LED1);
Serial pc(USBTX, USBRX);
@@ -13,7 +15,18 @@
int16_t zGyro = 0;
int countx = 0;
int p = 32776;
-double const SSF = 0.00763358778625954198473282442748f; //FSEL = 0: 0.00763358778625954198473282442748, FSEL = 3: 0.06097560975609756097560975609756f
+double const SSF = 0.06097560975609756097560975609756f; //FSEL = 0: 0.00763358778625954198473282442748, FSEL = 3: 0.06097560975609756097560975609756f
+
+int OffsetX = 254;
+int OffsetY = -14;
+int OffsetZ = 81;
+
+int OffsetXA = -306;
+int OffsetYA = -131;
+int OffsetZA = -531;
+
+IMU IMU0 (0, -306.0f, -131.0f, -351.0f, 254.0f, -14.0f, 81.0f, 0, 3);
+
Timer t;
float dTime = 0.0f;
@@ -161,9 +174,10 @@
//reset idx and dataCount
dataCount = 0;
idx = 0;
- ProcessAndPrint();
- //calibrateOffset();
-
+ //ProcessAndPrint();
+ //calibrateOffset();
+ IMU0.concatenateData(IMUarray);
+
t.stop();
dTime = t.read();
t.reset();
@@ -232,29 +246,29 @@
switch(x) {
case 0:
- accelX = MSB + LSB; //Combine Accelerometer x-axis data
+ accelX = (MSB + LSB) + OffsetXA; //Combine Accelerometer x-axis data
faccelX = (float)accelX * 0.00006103515625f; //Multiply the acceleration by sensitivity scale factor to get it into g 1/16,384
break;
case 1:
- accelY = MSB + LSB; //Combine Accelerometer y-axis data
+ accelY = (MSB + LSB) + OffsetYA; //Combine Accelerometer y-axis data
faccelY = (float)accelY * 0.00006103515625f; //Multiply the acceleration by sensitivity scale factor to get it into g 1/16,384
break;
case 2:
- accelZ = MSB + LSB; //Combine Accelerometer z-axis data
+ accelZ = (MSB + LSB) + OffsetZA; //Combine Accelerometer z-axis data
faccelZ = (float)accelZ * 0.00006103515625f; //Multiply the acceleration by sensitivity scale factor to get it into g 1/16,384
break;
case 3:
- gyroX = MSB + LSB; //Combine Gyroscope x-axis data
+ gyroX = (MSB + LSB) + OffsetX; //Combine Gyroscope x-axis data
fgyroX = (float)gyroX * SSF; //Multiply the sample by sensitivity scale factor to get it into degress per second 1/16.4
break;
case 4:
- gyroY = MSB + LSB; //Combine Gyroscope y-axis data
+ gyroY = (MSB + LSB) + OffsetY; //Combine Gyroscope y-axis data
fgyroY = (float)gyroY * SSF; //Multiply the sample by sensitivity scale factor to get it into degress per second 1/16.4
break;
case 5:
- gyroZ = MSB + LSB; //Combine Gyroscope z-axis data
+ gyroZ = (MSB + LSB) + OffsetZ; //Combine Gyroscope z-axis data
fgyroZ = (float)gyroZ * SSF; //Multiply the sample by sensitivity scale factor to get it into degress per second 1/16.4
break;
default:
@@ -265,9 +279,9 @@
//Quaternion-Gyro-Integration--------------------------------------------------------------
//Get the Gyro and Accel values--------------------------------------------------------
//Convert these vales to radians per second and store them in the vector
- GyroVals.x = fgyroX * 0.0174533;
- GyroVals.y = fgyroY * 0.0174533;
- GyroVals.z = fgyroZ * 0.0174533;
+ GyroVals.x = fgyroX * 0.0174533f;
+ GyroVals.y = fgyroY * 0.0174533f;
+ GyroVals.z = fgyroZ * 0.0174533f;
intGyro.x = (fgyroX * dTime);
intGyro.y = (fgyroY * dTime);
@@ -278,9 +292,9 @@
AccelVals.z = faccelZ;
//Get the Gyro and Accel values--------------------------------------------------------
- CF = updateQuaternion(CF, GyroVals, dTime);
- CF = normaliseQuaternion(CF);
- Eangles = eulerA(CF);
+ //CF = updateQuaternion(CF, GyroVals, dTime);
+ //CF = normaliseQuaternion(CF);
+ //Eangles = eulerA(CF);
//Quaternion-Gyro-Integration--------------------------------------------------------------
@@ -296,26 +310,27 @@
//Complementary-Filter---------------------------------------------------------------------
- CF.x = 0.98f*(Eangles.x) + 0.02f*accelTilt.x;
- CF.y = 0.98f*(Eangles.y) + 0.02f*accelTilt.y;
- CF.z = 0.98f*(Eangles.z) + 0.02f*accelTilt.z;
+ //CF.x = 0.98f*(Eangles.x) + 0.02f*accelTilt.x;
+ //CF.y = 0.98f*(Eangles.y) + 0.02f*accelTilt.y;
+ //CF.z = 0.98f*(Eangles.z) + 0.02f*accelTilt.z;
- //CF.x = 0.98f*(CF.x + intGyro.x) + 0.02f*accelTilt.x;
- //CF.y = 0.98f*(CF.y + intGyro.y) + 0.02f*accelTilt.y;
- //CF.z = 0.98f*(CF.z + intGyro.z) + 0.02f*accelTilt.z;
+ CF.x = 0.98f*(CF.x + intGyro.x) + 0.02f*accelTilt.x;
+ CF.y = 0.98f*(CF.y + intGyro.y) + 0.02f*accelTilt.y;
+ CF.z = 0.98f*(CF.z + intGyro.z) + 0.02f*accelTilt.z;
//Complementary-Filter---------------------------------------------------------------------
//Add the correction to gyro readings and update the quaternion------------------------
//pc.printf("%+6f, %+6f, %+6f %+6f\n\r ", xx, yy, zz, dTime);
//pc.printf("%+6f, %+6f, %+6f, %+6f, %+6f, %+6f, %+6f\n\r ", gyroQ.w, gyroQ.x, gyroQ.y, gyroQ.z, CF.x, CF.y, Eangles.z);
- //pc.printf("%+6f, %+6f, %+6f ", CF.x, CF.y, CF.z);
- CF.x *= 0.0174533f;
- CF.y *= 0.0174533f;
- CF.z *= 0.0174533f;
- CF = euler2Quaternion(CF);
- CF = normaliseQuaternion(CF);
+ //pc.printf("%+6f, %+6f, %+6f \n\r", CF.x, CF.y, CF.z);
+ //CF.x *= 0.0174533f;
+ //CF.y *= 0.0174533f;
+ // CF.z *= 0.0174533f;
+ //CF = euler2Quaternion(CF);
+ //CF = normaliseQuaternion(CF);
//pc.printf("%+6f, %+6f, %+6f\n\r ",Eangles.x, Eangles.y, Eangles.z);
pc.printf("Accel X: %+6f, Accel Y: %+6f, Accel Z: %+6f, Gyro X: %+6f, Gyro Y: %+6f, Gyro Z: %+6f\n\r", faccelX, faccelY, faccelZ, fgyroX, fgyroY, fgyroZ);
+ //pc.printf("Accel X: %+6f, Accel Y: %+6f, Accel Z: %+6f, Gyro X: %+6d, Gyro Y: %+6d, Gyro Z: %+6d\n\r", faccelX, faccelY, faccelZ, gyroX, gyroY, gyroZ);
}//void ProcessAndPrint()