Zbigniew Druzbacki
SPI slave program to enable communication between the FPGA and the STM32L432 board.
Diff: main.cpp
- Revision:
- 4:e36c7042d3bb
- Parent:
- 3:e33697420c4a
- Child:
- 5:155d224d855c
--- a/main.cpp Tue Feb 19 01:44:53 2019 +0000
+++ b/main.cpp Sat Feb 23 01:20:08 2019 +0000
@@ -1,5 +1,6 @@
#include "mbed.h"
#include "SPI.h"
+#include "Quaternions.h"
DigitalOut myled(LED1);
Serial pc(USBTX, USBRX);
@@ -13,6 +14,22 @@
int countx = 0;
int p = 32776;
+Timer t;
+float dTime = 0.0f;
+
+vector vertical;
+vector globalAccel;
+vector correctionGlobalAccel;
+vector correctionBodyAccel;
+Quaternion gyroQ;
+vector Eangles;
+
+
+float xx;
+float yy;
+float zz;
+
+
/*
@@ -27,9 +44,12 @@
void ProcessAndPrint();
//------------------Printing-All-values----------------------//
+//-------------Testing-Variables-Remove-Later----------------//
+int blinkCounter = 0;
+//-------------Testing-Variables-Remove-Later----------------//
int main() {
-
+ pc.baud (115200);
IDarray[0] = 1;
IDarray[1] = 0;
IDarray[2] = 9;
@@ -37,20 +57,29 @@
IDarray[4] = 17;
IDarray[5] = 16;
IDarray[6] = 3;
- IDarray[7] = 2;
+ IDarray[7] = 2; //2
IDarray[8] = 11;
IDarray[9] = 10;
IDarray[10] = 19;
IDarray[11] = 18;
+ init_spi1();
+ t.start();
- init_spi1();
+ gyroQ.w = 1;
+ gyroQ.x = 0.0001;
+ gyroQ.y = 0.0001;
+ gyroQ.z = 0.0001;
+
+
+
+ gyroQ = normaliseQuaternion(gyroQ);
+
+ vertical.x = 0.0f;
+ vertical.y = 0.0f;
+ vertical.z = 1.0f;
+
pc.printf("Begin \n\r");
- // pc.printf("%+d \n\r",p);
- // p = p & ~(255<< 8);
- // pc.printf("%+d \n\r",p);
- // p = p >> 1;
- // pc.printf("%d \n\r",p);
while(1) {
if(i == 1) {
@@ -100,28 +129,47 @@
//------------------------------------------------------------------------------
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);
- 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
+ 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;
- ProcessAndPrint();
+ ProcessAndPrint();
+
+ t.stop();
+ dTime = t.read();
+ t.reset();
+ t.start();
}//if(dataCount == 12)
}//if(IDarray[dataCount] == id)
- else {
- // pc.printf("Failed at: %d \n\r", dataCount);
- dataCount = 0;
- idx = 0;
- }
+ 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)
@@ -131,10 +179,19 @@
+
+/*
+ -The purpose of this function
+*/
void ProcessAndPrint() {
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
+
+ vector GyroVals;
+ vector AccelVals;
+ vector accelTilt;
+ Quaternion incRot;
//-----------Concatentated-Data-Pieces------------------------------------------
int16_t gyroX = 0;
int16_t gyroY = 0;
@@ -143,41 +200,120 @@
int16_t accelX = 0;
int16_t accelY = 0;
int16_t accelZ = 0;
+
+ float faccelX = 0.0f;
+ float faccelY = 0.0f;
+ float faccelZ = 0.0f;
+
+ float fgyroX = 0.0f;
+ float fgyroY = 0.0f;
+ float fgyroZ = 0.0f;
+
//-----------Concatentated-Data-Pieces------------------------------------------
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++;
- LSB = IMUarray[arrPointer]; //Even data pieces are LSBs
- LSB &= ~(255 << 8); //Mask the MSB bits as they are not part of data
- arrPointer++;
+ 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
+ accelX = MSB + LSB; //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; //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; //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; //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
+
break;
case 4:
- gyroY = MSB + LSB; //Combine Gyroscope y-axis data
+ 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
+
break;
case 5:
- gyroZ = MSB + LSB; //Combine Gyroscope z-axis data
+ 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
break;
default:
break;
}//switch(x)
}//for(char x = 0; x <= 5; x++)
- pc.printf("\r");
- pc.printf("Accel X: %+6d, Accel Y: %+6d, Accel Z: %+6d, Gyro X: %+6d, Gyro Y: %+6d, Gyro Z: %+6d", accelX, accelY, accelZ, gyroX, gyroY, gyroZ);
+
+//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;
+
+ 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);
+//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);
+ //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------------------------------------------------
+
+ //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\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);
}//void ProcessAndPrint()
+
+
+
+
+
+
+
+
+
+
+
+
+//------------------------------------------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