SPI slave program to enable communication between the FPGA and the STM32L432 board.
Diff: main.cpp
- Revision:
- 5:155d224d855c
- Parent:
- 4:e36c7042d3bb
- Child:
- 6:0ebecfecadc9
--- a/main.cpp Sat Feb 23 01:20:08 2019 +0000
+++ b/main.cpp Sun Feb 24 16:49:06 2019 +0000
@@ -13,6 +13,7 @@
int16_t zGyro = 0;
int countx = 0;
int p = 32776;
+double const SSF = 0.00763358778625954198473282442748f; //FSEL = 0: 0.00763358778625954198473282442748, FSEL = 3: 0.06097560975609756097560975609756f
Timer t;
float dTime = 0.0f;
@@ -23,6 +24,14 @@
vector correctionBodyAccel;
Quaternion gyroQ;
vector Eangles;
+Quaternion CF;
+vector intGyro;
+ vector GyroVals;
+ vector AccelVals;
+ vector accelTilt;
+ Quaternion accelQ;
+
+ void calibrateOffset();
float xx;
@@ -71,9 +80,14 @@
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;
@@ -147,7 +161,8 @@
//reset idx and dataCount
dataCount = 0;
idx = 0;
- ProcessAndPrint();
+ ProcessAndPrint();
+ //calibrateOffset();
t.stop();
dTime = t.read();
@@ -188,10 +203,6 @@
int16_t LSB = 0; //Store Least Significant Byte of data piece in this variable for processing
char arrPointer = 0; //Array Pointer
- vector GyroVals;
- vector AccelVals;
- vector accelTilt;
- Quaternion incRot;
//-----------Concatentated-Data-Pieces------------------------------------------
int16_t gyroX = 0;
int16_t gyroY = 0;
@@ -234,17 +245,17 @@
break;
case 3:
gyroX = MSB + LSB; //Combine Gyroscope x-axis data
- fgyroX = (float)gyroX * 0.06097560975609756097560975609756f; //Multiply the sample by sensitivity scale factor to get it into degress per second 1/16.4
+ 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
- fgyroY = (float)gyroY * 0.06097560975609756097560975609756f; //Multiply the sample by sensitivity scale factor to get it into degress per second 1/16.4
+ 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
- fgyroZ = (float)gyroZ * 0.06097560975609756097560975609756f; //Multiply the sample by sensitivity scale factor to get it into degress per second 1/16.4
+ fgyroZ = (float)gyroZ * SSF; //Multiply the sample by sensitivity scale factor to get it into degress per second 1/16.4
break;
default:
break;
@@ -258,37 +269,176 @@
GyroVals.y = fgyroY * 0.0174533;
GyroVals.z = fgyroZ * 0.0174533;
+ intGyro.x = (fgyroX * dTime);
+ intGyro.y = (fgyroY * dTime);
+ intGyro.z = (fgyroZ * dTime);
+
AccelVals.x = faccelX;
AccelVals.y = faccelY;
AccelVals.z = faccelZ;
//Get the Gyro and Accel values--------------------------------------------------------
- gyroQ = updateQuaternion(gyroQ, GyroVals, dTime);
- gyroQ = normaliseQuaternion(gyroQ);
- Eangles = eulerA(gyroQ);
+ CF = updateQuaternion(CF, GyroVals, dTime);
+ CF = normaliseQuaternion(CF);
+ Eangles = eulerA(CF);
//Quaternion-Gyro-Integration--------------------------------------------------------------
//Angle-Calculations-Based-on-Accalerometer------------------------------------------------
//https://www.dfrobot.com/wiki/index.php/How_to_Use_a_Three-Axis_Accelerometer_for_Tilt_Sensing
- accelTilt.x = (atan2(AccelVals.y, AccelVals.z) * 57.29577951f);
- accelTilt.y = (atan2((-AccelVals.x), sqrt(pow(AccelVals.y, 2) + pow(AccelVals.z, 2) ))) * 57.29577951f);
+ accelTilt.x = (atan2f(AccelVals.y, AccelVals.z) * 57.29577951f);
+ accelTilt.y = (atan2f((-AccelVals.x), sqrt(pow(AccelVals.y, 2) + pow(AccelVals.z, 2) )) * 57.29577951f);
+ //pc.printf("%+6f, %+6f ", accelTilt.x, accelTilt.y);
//Canntot calculate accel tilt for Z. Gyro date in combination with magnetometer have to be used to obtain reliable z-axis tilt.
-//Angle-Calculations-Based-on-Accalerometer------------------------------------------------
+//Angle-Calculations-Based-on-Accalerometer------------------------------------------------
+ //pc.printf("%+6f, %+6f, %+6f ", CF.x, CF.y, CF.z);
+
+
+//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*(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, Eangles.x, Eangles.y, Eangles.z);
+ //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 ",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: %+6f, Gyro Y: %+6f, Gyro Z: %+6f\n\r", faccelX, faccelY, faccelZ, fgyroX, fgyroY, fgyroZ);
}//void ProcessAndPrint()
+void calibrateOffset() {
+
+ 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
+
+//-----------Concatentated-Data-Pieces------------------------------------------
+ int16_t gyroX = 0;
+ int16_t gyroY = 0;
+ int16_t gyroZ = 0;
+
+ int16_t accelX = 0;
+ int16_t accelY = 0;
+ int16_t accelZ = 0;
+
+ vector accelRaw;
+ vector accelG;
+ vector gyroRaw;
+ vector gyroDPS;
+
+ static unsigned int sampleCounter = 1;
+ static vector accelRawAvg;
+ static vector accelGAvg;
+ static vector gyroRawAvg;
+ static vector gyroDPSAvg;
+
+
+ for(char x = 0; x <= 5; x++) {
+ MSB = IMUarray[arrPointer]; //Odd data pieces are MSBs
+ MSB &= ~(255<<8); //Mask the MSB bits as they are not part of data
+ MSB = MSB << 8; //Shift the Value as its the MSB of the data piece
+ arrPointer++; //Increment array pointer
+ LSB = IMUarray[arrPointer]; //Even data pieces are LSBs
+ LSB &= ~(255 << 8); //Mask the MSB bits as they are not part of data
+ arrPointer++; //Increment array pointer
+
+ switch(x) {
+ case 0:
+ accelX = MSB + LSB; //Combine Accelerometer x-axis data
+ accelRaw.x = (double)accelX; //accelRaw
+ accelG.x = (double)accelX * 0.00006103515625f; //accelSSF
+
+ break;
+ case 1:
+ accelY = MSB + LSB; //Combine Accelerometer y-axis data
+ accelRaw.y = (double)accelY;
+ accelG.y = (double)accelY * 0.00006103515625f;
+ break;
+ case 2:
+ accelZ = MSB + LSB; //Combine Accelerometer z-axis data
+ accelRaw.z = (double)accelZ;
+ accelG.z = (double)accelZ * 0.00006103515625f;
+ break;
+ case 3:
+ gyroX = MSB + LSB; //Combine Gyroscope x-axis data
+ gyroRaw.x = (double)gyroX; //gyroRaw
+ gyroDPS.x = (double)gyroX * SSF; //gyroSSF
+
+ break;
+ case 4:
+ gyroY = MSB + LSB; //Combine Gyroscope y-axis data
+ gyroRaw.y = (double)gyroY;
+ gyroDPS.y = (double)gyroY * SSF;
+ break;
+ case 5:
+ gyroZ = MSB + LSB; //Combine Gyroscope z-axis data
+ gyroRaw.z = (double)gyroZ;
+ gyroDPS.z = (double)gyroZ * SSF;
+ break;
+ default:
+ break;
+ }//switch(x)
+ }//for(char x = 0; x <= 5; x++)
+
+//Take-Running-Averages------------------------------------------------------------------------
+ //Raw accel averages
+ accelRawAvg.x = accelRawAvg.x + ((accelRaw.x - accelRawAvg.x)/sampleCounter);
+ accelRawAvg.y = accelRawAvg.y + ((accelRaw.y - accelRawAvg.y)/sampleCounter);
+ accelRawAvg.z = accelRawAvg.z + ((accelRaw.z - accelRawAvg.z)/sampleCounter);
+
+ //SSF accel averages
+ accelGAvg.x = accelGAvg.x + ((accelG.x - accelGAvg.x)/sampleCounter);
+ accelGAvg.y = accelGAvg.y + ((accelG.y - accelGAvg.y)/sampleCounter);
+ accelGAvg.z = accelGAvg.z + ((accelG.z - accelGAvg.z)/sampleCounter);
+
+ //Raw gyroo averages
+ gyroRawAvg.x = gyroRawAvg.x + ((gyroRaw.x - gyroRawAvg.x)/sampleCounter);
+ gyroRawAvg.y = gyroRawAvg.y + ((gyroRaw.y - gyroRawAvg.y)/sampleCounter);
+ gyroRawAvg.z = gyroRawAvg.z + ((gyroRaw.z - gyroRawAvg.z)/sampleCounter);
+
+ //SSF gyro averages
+ gyroDPSAvg.x = gyroDPSAvg.x + ((gyroDPS.x - gyroDPSAvg.x)/sampleCounter);
+ gyroDPSAvg.y = gyroDPSAvg.y + ((gyroDPS.y - gyroDPSAvg.y)/sampleCounter);
+ gyroDPSAvg.z = gyroDPSAvg.z + ((gyroDPS.z - gyroDPSAvg.z)/sampleCounter);
+//Take-Running-Averages------------------------------------------------------------------------
+//Print Messages-------------------------------------------------------------------------------
+ if(sampleCounter == 1) {
+ pc.printf("Collecting Raw and Sensitivity Scale Factor multiplied Gyroscope and Accelerometer Offsets...\n\r");
+ };
+
+
+ if(sampleCounter == 5000) {
+ pc.printf("RawAX RawAY RawAZ RawGX RawGY RawGZ SampleN\n\r");
+ 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);
+ pc.printf("\n\r");
+ pc.printf("\n\r");
+ pc.printf("\n\r");
+
+ //Add the sensitivity scale factor multiplied data
+ pc.printf("SSFAX SSFAY SSFAZ SSFGX SSFGY SSFGZ SampleN\n\r");
+ 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);
+
+ };
+ sampleCounter++;
+//Print Messages-------------------------------------------------------------------------------
+}