Zbigniew Druzbacki
SPI slave program to enable communication between the FPGA and the STM32L432 board.
main.cpp
- Committer:
- Zbyszek
- Date:
- 2019-02-27
- Revision:
- 8:e87027349167
- Parent:
- 7:0e9af5986488
- Child:
- 9:9ed9dffd602a
File content as of revision 8:e87027349167:
#include "mbed.h"
#include "SPI.h"
#include "IMUs.h"
#include "Structures.h"
#include "Quaternions.h"
#include "DMA_SPI.h"
DigitalOut myled(LED1);
Serial pc(USBTX, USBRX);
int masterRx = 0;
int16_t slaveRx = 0;
int i = 2;
int k = 0;
int16_t zgHigher = 0;
int16_t zgLower = 0;
int16_t zGyro = 0;
int countx = 0;
int p = 32776;
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);
IMU_Data Dat;
vector Datt;
Timer t;
float dTime = 0.0f;
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();
float xx;
float yy;
float zz;
/*
*/
//------------------Printing-All-values----------------------//
int16_t IMUarray[12]; //Store each separate reading in an array
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;
//-------------Testing-Variables-Remove-Later----------------//
int main() {
pc.baud (115200);
IDarray[0] = 1;
IDarray[1] = 0;
IDarray[2] = 9;
IDarray[3] = 8;
IDarray[4] = 17;
IDarray[5] = 16;
IDarray[6] = 3;
IDarray[7] = 2; //2
IDarray[8] = 11;
IDarray[9] = 10;
IDarray[10] = 19;
IDarray[11] = 18;
init_spi1();
t.start();
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;
pc.printf("Begin \n\r");
while(1) {
if(i == 1) {
for(int x = 1; x < 128; x *= 2) {
slaveRx = transfer_spi_slave(x);
//pc.putc(slaveRx);
pc.printf("%d \n\r",slaveRx);
}
for(int x = 128; x > 1; x /= 2) {
slaveRx = transfer_spi_slave(x);
//pc.putc(slaveRx);
pc.printf("%d \n\r",slaveRx);
}
}//if(i == 1)
//------------------------------------------------------------------------------
if(i == 0) {
slaveRx = transfer_spi_slave(10);
countx++;
if(countx == 1) {
k = slaveRx;
k &= ~(255 << 8);
k = k << 8;
// if(k == 19) {
zgHigher = k;
//zgHigher = zgHigher << 8;
//}//if(k == 19)
}//if(count == 11)
else if(countx == 2) {
k = slaveRx;
k &= ~(255 << 8);
//k = k << 8;
//if(k == 18) {
zgLower = k;
//zgLower = zgLower << 8;
zGyro = zgHigher + zgLower;
pc.printf("%+d \n\r",zGyro);
//}//if(k == 19)
countx = 0;
}//else if(count == 12)
}//if(i == 0)
//------------------------------------------------------------------------------
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;
//ProcessAndPrint();
//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)
}
}
/*
-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
//-----------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;
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++; //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) + 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) + 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) + 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) + 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) + 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) + 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:
break;
}//switch(x)
}//for(char x = 0; x <= 5; x++)
//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.0174533f;
GyroVals.y = fgyroY * 0.0174533f;
GyroVals.z = fgyroZ * 0.0174533f;
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--------------------------------------------------------
//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 = (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------------------------------------------------
//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, CF.x, CF.y, Eangles.z);
//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()
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-------------------------------------------------------------------------------
}
//------------------------------------------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----------------------------------------