Zbigniew Druzbacki / Mbed 2 deprecated SPI_Slave

SPI slave program to enable communication between the FPGA and the STM32L432 board.

Dependencies:   mbed

IMUs.cpp@15:791f35b0f220, 2019-05-15 (annotated)

Committer:
Zbyszek
Date:
Wed May 15 22:56:20 2019 +0000
Revision:
15:791f35b0f220
Parent:
14:7bbaafa22f8d 
Official Code used on the 15/05/2019

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Zbyszek 13:c7e8e277f884 1 #include "CustomDatatypes.h"
Zbyszek 7:0e9af5986488 2 #include "Quaternions.h"
Zbyszek 6:0ebecfecadc9 3 #include "IMUs.h"
Zbyszek 6:0ebecfecadc9 4 #include "mbed.h"
Zbyszek 6:0ebecfecadc9 5
Zbyszek 6:0ebecfecadc9 6 //IMU Class Constructor
Zbyszek 6:0ebecfecadc9 7 IMU::IMU(char IMU_ID, double OffsetAX, double OffsetAY, double OffsetAZ, double OffsetGX, double OffsetGY, double OffsetGZ, char SSFA, char SSFG) {
Zbyszek 6:0ebecfecadc9 8
Zbyszek 15:791f35b0f220 9 this->AccelerometerOffset.x = OffsetAX;
Zbyszek 15:791f35b0f220 10 this->AccelerometerOffset.y = OffsetAY;
Zbyszek 15:791f35b0f220 11 this->AccelerometerOffset.z = OffsetAZ;
Zbyszek 6:0ebecfecadc9 12
Zbyszek 15:791f35b0f220 13 this->GyroscopeOffset.x = OffsetGX;
Zbyszek 15:791f35b0f220 14 this->GyroscopeOffset.y = OffsetGY;
Zbyszek 15:791f35b0f220 15 this->GyroscopeOffset.z = OffsetGZ;
Zbyszek 6:0ebecfecadc9 16
Zbyszek 6:0ebecfecadc9 17
Zbyszek 6:0ebecfecadc9 18 IMU_Identifier = IMU_ID;
Zbyszek 6:0ebecfecadc9 19
Zbyszek 15:791f35b0f220 20 //Accelerometer Sensitvity Scale Factor reciprocals
Zbyszek 6:0ebecfecadc9 21 switch(SSFA) {
Zbyszek 6:0ebecfecadc9 22 case 0:
Zbyszek 15:791f35b0f220 23 accelSSF = 0.00006103515625f; //Sensitivity Scale Factor setting 0 reciprocal
Zbyszek 6:0ebecfecadc9 24 break;
Zbyszek 6:0ebecfecadc9 25 case 1:
Zbyszek 15:791f35b0f220 26 accelSSF = 0.0001220703125f; //Sensitivity Scale Factor setting 1 reciprocal
Zbyszek 15:791f35b0f220 27 break;
Zbyszek 6:0ebecfecadc9 28 case 2:
Zbyszek 15:791f35b0f220 29 accelSSF = 0.000244140625f; //Sensitivity Scale Factor setting 2 reciprocal
Zbyszek 6:0ebecfecadc9 30 break;
Zbyszek 6:0ebecfecadc9 31 case 3:
Zbyszek 15:791f35b0f220 32 accelSSF = 0.00048828125f; //Sensitivity Scale Factor setting 3 reciprocal
Zbyszek 6:0ebecfecadc9 33 break;
Zbyszek 6:0ebecfecadc9 34 default:
Zbyszek 6:0ebecfecadc9 35 break;
Zbyszek 6:0ebecfecadc9 36 }
Zbyszek 6:0ebecfecadc9 37
Zbyszek 15:791f35b0f220 38 //Gyroscope Sensitivity Scale Factor reciprocals
Zbyszek 6:0ebecfecadc9 39 switch(SSFG) {
Zbyszek 6:0ebecfecadc9 40 case 0:
Zbyszek 15:791f35b0f220 41 gyroSSF = 0.00763358778625954198473282442748f; //Sensitivity Scale Factor setting 0 reciprocal
Zbyszek 6:0ebecfecadc9 42 break;
Zbyszek 6:0ebecfecadc9 43 case 1:
Zbyszek 15:791f35b0f220 44 gyroSSF = 0.01526717557251908396946564885496f; //Sensitivity Scale Factor setting 1 reciprocal
Zbyszek 6:0ebecfecadc9 45 break;
Zbyszek 6:0ebecfecadc9 46 case 2:
Zbyszek 15:791f35b0f220 47 gyroSSF = 0.03048780487804878048780487804878f; //Sensitivity Scale Factor setting 2 reciprocal
Zbyszek 6:0ebecfecadc9 48 break;
Zbyszek 6:0ebecfecadc9 49 case 3:
Zbyszek 15:791f35b0f220 50 gyroSSF = 0.06097560975609756097560975609756f; //Sensitivity Scale Factor setting 3 reciprocal
Zbyszek 6:0ebecfecadc9 51 break;
Zbyszek 6:0ebecfecadc9 52 default:
Zbyszek 6:0ebecfecadc9 53 break;
Zbyszek 6:0ebecfecadc9 54 }
Zbyszek 6:0ebecfecadc9 55
Zbyszek 7:0e9af5986488 56 this->t.start();
Zbyszek 7:0e9af5986488 57
Zbyszek 6:0ebecfecadc9 58 }
Zbyszek 6:0ebecfecadc9 59
Zbyszek 6:0ebecfecadc9 60 //void IMU::CFAngle() {
Zbyszek 6:0ebecfecadc9 61
Zbyszek 6:0ebecfecadc9 62
Zbyszek 6:0ebecfecadc9 63
Zbyszek 6:0ebecfecadc9 64 //}
Zbyszek 6:0ebecfecadc9 65
Zbyszek 6:0ebecfecadc9 66 //----------------------------------------------------------------------------------------------------------------------------------------------------------
Zbyszek 6:0ebecfecadc9 67 IMU_Data IMU::concatenateData(int16_t SamplesPieces[12]) {
Zbyszek 6:0ebecfecadc9 68 //Local-Variables------Local-Variables------Local-Variables------Local-Variables
Zbyszek 6:0ebecfecadc9 69
Zbyszek 6:0ebecfecadc9 70 int16_t MSB = 0; //Store Most Significant Byte of data piece in this variable for processing
Zbyszek 6:0ebecfecadc9 71 int16_t LSB = 0; //Store Least Significant Byte of data piece in this variable for processing
Zbyszek 6:0ebecfecadc9 72 char arrPointer = 0; //Array Pointer
Zbyszek 6:0ebecfecadc9 73 IMU_Data LocalD;
Zbyszek 6:0ebecfecadc9 74 //Procedure--------------Procedure------------Procedure-------------Procedure---
Zbyszek 6:0ebecfecadc9 75 for(char x = 0; x <= 5; x++) {
Zbyszek 6:0ebecfecadc9 76 MSB = SamplesPieces[arrPointer]; //Odd data pieces are MSBs
Zbyszek 6:0ebecfecadc9 77 MSB &= ~(255<<8); //Mask the MSB bits as they are not part of data
Zbyszek 6:0ebecfecadc9 78 MSB = MSB << 8; //Shift the Value as its the MSB of the data piece
Zbyszek 6:0ebecfecadc9 79 arrPointer++; //Increment array pointer
Zbyszek 6:0ebecfecadc9 80 LSB = SamplesPieces[arrPointer]; //Even data pieces are LSBs
Zbyszek 6:0ebecfecadc9 81 LSB &= ~(255 << 8); //Mask the MSB bits as they are not part of data
Zbyszek 6:0ebecfecadc9 82 arrPointer++; //Increment array pointer
Zbyszek 6:0ebecfecadc9 83
Zbyszek 6:0ebecfecadc9 84 switch(x) {
Zbyszek 6:0ebecfecadc9 85 case 0:
Zbyszek 15:791f35b0f220 86 LocalD.Ax = (MSB + LSB) + this->AccelerometerOffset.x; //Combine Accelerometer x-axis data and apply offset cancellation value
Zbyszek 6:0ebecfecadc9 87 break;
Zbyszek 6:0ebecfecadc9 88 case 1:
Zbyszek 15:791f35b0f220 89 LocalD.Ay = (MSB + LSB) + this->AccelerometerOffset.y; //Combine Accelerometer y-axis data and apply offset cancellation value
Zbyszek 6:0ebecfecadc9 90 break;
Zbyszek 6:0ebecfecadc9 91 case 2:
Zbyszek 15:791f35b0f220 92 LocalD.Az = (MSB + LSB) + this->AccelerometerOffset.z; //Combine Accelerometer z-axis data and apply offset cancellation value
Zbyszek 6:0ebecfecadc9 93 break;
Zbyszek 6:0ebecfecadc9 94 case 3:
Zbyszek 15:791f35b0f220 95 LocalD.Gx = (MSB + LSB) + this->GyroscopeOffset.x; //Combine Gyroscope x-axis data and apply offset cancellation value
Zbyszek 6:0ebecfecadc9 96 break;
Zbyszek 6:0ebecfecadc9 97 case 4:
Zbyszek 15:791f35b0f220 98 LocalD.Gy = (MSB + LSB) + this->GyroscopeOffset.y; //Combine Gyroscope y-axis data and apply offset cancellation value
Zbyszek 6:0ebecfecadc9 99 break;
Zbyszek 6:0ebecfecadc9 100 case 5:
Zbyszek 15:791f35b0f220 101 LocalD.Gz = (MSB + LSB) + this->GyroscopeOffset.z; //Combine Gyroscope z-axis data and apply offset cancellation value
Zbyszek 6:0ebecfecadc9 102 break;
Zbyszek 6:0ebecfecadc9 103 default:
Zbyszek 6:0ebecfecadc9 104 break;
Zbyszek 6:0ebecfecadc9 105 }//switch(x)
Zbyszek 6:0ebecfecadc9 106 }//for(char x = 0; x <= 5; x++)
Zbyszek 11:366f1186c121 107
Zbyszek 15:791f35b0f220 108 //printf("Accel X: %+2f, Accel Y: %+2f, Accel Z: %+2f, Gyro X: %+2f, Gyro Y: %+2f, Gyro Z: %+2f\n\r", LocalD.Ax, LocalD.Ay, LocalD.Az, LocalD.Gx, LocalD.Gy, LocalD.Gz);
Zbyszek 6:0ebecfecadc9 109 return LocalD;
Zbyszek 6:0ebecfecadc9 110 }
Zbyszek 6:0ebecfecadc9 111 //----------------------------------------------------------------------------------------------------------------------------------------------------------
Zbyszek 6:0ebecfecadc9 112
Zbyszek 6:0ebecfecadc9 113
Zbyszek 6:0ebecfecadc9 114
Zbyszek 6:0ebecfecadc9 115
Zbyszek 6:0ebecfecadc9 116
Zbyszek 6:0ebecfecadc9 117 //----------------------------------------------------------------------------------------------------------------------------------------------------------
Zbyszek 6:0ebecfecadc9 118 IMU_Data IMU::SSFmultiply(IMU_Data RawData) {
Zbyszek 6:0ebecfecadc9 119
Zbyszek 6:0ebecfecadc9 120 IMU_Data localD;
Zbyszek 6:0ebecfecadc9 121
Zbyszek 15:791f35b0f220 122 //Multiply the raw data by sensitivity scale factor to convert:
Zbyszek 15:791f35b0f220 123 //Accelerometer data to gravity in g.
Zbyszek 15:791f35b0f220 124 localD.Ax = RawData.Ax * this->accelSSF;
Zbyszek 15:791f35b0f220 125 localD.Ay = RawData.Ay * this->accelSSF;
Zbyszek 15:791f35b0f220 126 localD.Az = RawData.Az * this->accelSSF;
Zbyszek 6:0ebecfecadc9 127
Zbyszek 15:791f35b0f220 128 //angular velocity to degrees per second
Zbyszek 15:791f35b0f220 129 localD.Gx = RawData.Gx * this->gyroSSF;
Zbyszek 15:791f35b0f220 130 localD.Gy = RawData.Gy * this->gyroSSF;
Zbyszek 15:791f35b0f220 131 localD.Gz = RawData.Gz * this->gyroSSF;
Zbyszek 6:0ebecfecadc9 132
Zbyszek 7:0e9af5986488 133 //printf("Accel X: %+2f, Accel Y: %+2f, Accel Z: %+2f, Gyro X: %+2f, Gyro Y: %+2f, Gyro Z: %+2f\n\r", LocalD.Ax, LocalD.Ay, LocalD.Az, LocalD.Gx, LocalD.Gy, LocalD.Gz);
Zbyszek 6:0ebecfecadc9 134 return localD;
Zbyszek 6:0ebecfecadc9 135 }
Zbyszek 7:0e9af5986488 136 //----------------------------------------------------------------------------------------------------------------------------------------------------------
Zbyszek 7:0e9af5986488 137
Zbyszek 7:0e9af5986488 138
Zbyszek 7:0e9af5986488 139
Zbyszek 7:0e9af5986488 140
Zbyszek 7:0e9af5986488 141
Zbyszek 7:0e9af5986488 142 //----------------------------------------------------------------------------------------------------------------------------------------------------------
Zbyszek 7:0e9af5986488 143 IMU_Data IMU::getAngles(IMU_Data SSF_Format_Values, float dt) {
Zbyszek 7:0e9af5986488 144
Zbyszek 7:0e9af5986488 145 IMU_Data newValues, AngleData;
Zbyszek 7:0e9af5986488 146
Zbyszek 7:0e9af5986488 147 newValues = SSF_Format_Values;
Zbyszek 7:0e9af5986488 148
Zbyszek 15:791f35b0f220 149 //use trig functions to calculate tilt angles based on accelerometer values
Zbyszek 15:791f35b0f220 150 //The function returns data in radains and therefore convert to angle by multiplying by 57.295.....
Zbyszek 7:0e9af5986488 151 AngleData.Ax = (atan2f(newValues.Ay, newValues.Az) * 57.29577951f);
Zbyszek 7:0e9af5986488 152 AngleData.Ay = (atan2f((-newValues.Ax), sqrt(pow(newValues.Ay, 2) + pow(newValues.Az, 2) )) * 57.29577951f);
Zbyszek 7:0e9af5986488 153 AngleData.Az = 0; //Cannot calculate angle for this one as it remains constant all the time
Zbyszek 7:0e9af5986488 154
Zbyszek 15:791f35b0f220 155 //Integrate with respect to time to get change in angle.
Zbyszek 7:0e9af5986488 156 AngleData.Gx = (newValues.Gx * dt);
Zbyszek 7:0e9af5986488 157 AngleData.Gy = (newValues.Gy * dt);
Zbyszek 7:0e9af5986488 158 AngleData.Gz = (newValues.Gz * dt);
Zbyszek 7:0e9af5986488 159
Zbyszek 15:791f35b0f220 160 //return the data
Zbyszek 7:0e9af5986488 161 return AngleData;
Zbyszek 7:0e9af5986488 162
Zbyszek 7:0e9af5986488 163 }
Zbyszek 7:0e9af5986488 164 //----------------------------------------------------------------------------------------------------------------------------------------------------------
Zbyszek 7:0e9af5986488 165
Zbyszek 7:0e9af5986488 166
Zbyszek 7:0e9af5986488 167
Zbyszek 7:0e9af5986488 168
Zbyszek 7:0e9af5986488 169
Zbyszek 7:0e9af5986488 170 //----------------------------------------------------------------------------------------------------------------------------------------------------------
Zbyszek 7:0e9af5986488 171 vector IMU::getAccelAngles(vector SSF_Accel) {
Zbyszek 7:0e9af5986488 172
Zbyszek 7:0e9af5986488 173 vector newValues, AngleData;
Zbyszek 7:0e9af5986488 174
Zbyszek 7:0e9af5986488 175 newValues = SSF_Accel;
Zbyszek 7:0e9af5986488 176
Zbyszek 7:0e9af5986488 177 AngleData.x = (atan2f(newValues.y, newValues.z) * 57.29577951f);
Zbyszek 7:0e9af5986488 178 AngleData.y = (atan2f((-newValues.x), sqrt(pow(newValues.y, 2) + pow(newValues.z, 2) )) * 57.29577951f);
Zbyszek 7:0e9af5986488 179 //AngleData.z = 0; //Cannot calculate angle for this one as it remains constant all the time
Zbyszek 7:0e9af5986488 180
Zbyszek 7:0e9af5986488 181 //printf("Accel X: %+2f, Accel Y: %+2f, Accel Z: %+2f, Gyro X: %+2f, Gyro Y: %+2f, Gyro Z: %+2f dt: %2f\n\r", AngleData.Ax, AngleData.Ay, AngleData.Az, AngleData.Gx, AngleData.Gy, AngleData.Gz, dt);
Zbyszek 7:0e9af5986488 182 return AngleData;
Zbyszek 7:0e9af5986488 183
Zbyszek 7:0e9af5986488 184 }
Zbyszek 7:0e9af5986488 185 //----------------------------------------------------------------------------------------------------------------------------------------------------------
Zbyszek 7:0e9af5986488 186
Zbyszek 7:0e9af5986488 187
Zbyszek 7:0e9af5986488 188
Zbyszek 7:0e9af5986488 189
Zbyszek 7:0e9af5986488 190
Zbyszek 7:0e9af5986488 191
Zbyszek 7:0e9af5986488 192
Zbyszek 7:0e9af5986488 193
Zbyszek 7:0e9af5986488 194 //----------------------------------------------------------------------------------------------------------------------------------------------------------
Zbyszek 7:0e9af5986488 195 IMU_Data IMU::CalculateAngles(int16_t SamplesPieces[12]){
Zbyszek 7:0e9af5986488 196
Zbyszek 7:0e9af5986488 197 IMU_Data ConcatenatedValues, SSFValues, SimpleAngles;
Zbyszek 7:0e9af5986488 198 static IMU_Data PrevSimpleAngles;
Zbyszek 7:0e9af5986488 199 float deltaT;
Zbyszek 7:0e9af5986488 200
Zbyszek 7:0e9af5986488 201 ConcatenatedValues = this->concatenateData(SamplesPieces);
Zbyszek 7:0e9af5986488 202 SSFValues = this->SSFmultiply(ConcatenatedValues);
Zbyszek 7:0e9af5986488 203
Zbyszek 7:0e9af5986488 204 this->t.stop();
Zbyszek 7:0e9af5986488 205 deltaT = this->t.read();
Zbyszek 7:0e9af5986488 206 SimpleAngles = this->getAngles(SSFValues, deltaT);
Zbyszek 7:0e9af5986488 207
Zbyszek 7:0e9af5986488 208 PrevSimpleAngles.Gx = PrevSimpleAngles.Gx + SimpleAngles.Gx;
Zbyszek 7:0e9af5986488 209 PrevSimpleAngles.Gy = PrevSimpleAngles.Gy + SimpleAngles.Gy;
Zbyszek 7:0e9af5986488 210 PrevSimpleAngles.Gz = PrevSimpleAngles.Gz + SimpleAngles.Gz;
Zbyszek 7:0e9af5986488 211
Zbyszek 7:0e9af5986488 212 this->t.reset();
Zbyszek 7:0e9af5986488 213 this->t.start();
Zbyszek 7:0e9af5986488 214
Zbyszek 7:0e9af5986488 215 //printf("Accel X: %+2f, Accel Y: %+2f, Accel Z: %+2f, Gyro X: %+2f, Gyro Y: %+2f, Gyro Z: %+2f\n\r", PrevSimpleAngles.Ax, PrevSimpleAngles.Ay, PrevSimpleAngles.Az, PrevSimpleAngles.Gx, PrevSimpleAngles.Gy, PrevSimpleAngles.Gz);
Zbyszek 7:0e9af5986488 216 return PrevSimpleAngles;
Zbyszek 7:0e9af5986488 217 }
Zbyszek 7:0e9af5986488 218 //----------------------------------------------------------------------------------------------------------------------------------------------------------
Zbyszek 7:0e9af5986488 219
Zbyszek 15:791f35b0f220 220 //Complementary filter calculation function
Zbyszek 7:0e9af5986488 221 vector IMU::CalculateCFAngles(int16_t SamplesPieces[12]) {
Zbyszek 7:0e9af5986488 222
Zbyszek 7:0e9af5986488 223 IMU_Data ConcatenatedValues,NewAngle, SSFValues;
Zbyszek 7:0e9af5986488 224 float deltaT;
Zbyszek 15:791f35b0f220 225 static vector pureGANGLE;
Zbyszek 7:0e9af5986488 226
Zbyszek 7:0e9af5986488 227
Zbyszek 15:791f35b0f220 228 this->t.stop(); //Stop timer
Zbyszek 15:791f35b0f220 229 deltaT = this->t.read(); //read how much time has passed since last sample i.e sample period
Zbyszek 15:791f35b0f220 230 ConcatenatedValues = this->concatenateData(SamplesPieces); //Get Sample pieces and join together to from actual useful value
Zbyszek 15:791f35b0f220 231 SSFValues = this->SSFmultiply(ConcatenatedValues); //Multiply the values by sensitivity scale factor to get angular velocity in deg per sec and acceleration of gravity in g
Zbyszek 15:791f35b0f220 232 NewAngle = this->getAngles(SSFValues, deltaT); //calculate the accelerometer angles and the change in angle from the new gyroscope data
Zbyszek 7:0e9af5986488 233
Zbyszek 15:791f35b0f220 234 this->CFAngle.x = 0.98f*(CFAngle.x + NewAngle.Gx) + 0.02f*NewAngle.Ax; //Complementary filter for x-axis
Zbyszek 15:791f35b0f220 235 this->CFAngle.y = 0.98f*(CFAngle.y + NewAngle.Gy) + 0.02f*NewAngle.Ay; //Complementary filter for y-axis
Zbyszek 15:791f35b0f220 236 this->CFAngle.z = 0.98f*(CFAngle.z + NewAngle.Gz) + 0.02f*NewAngle.Az; //This in fact will just return the z-axis angle to zero all the time as accelerometer tilt in this axis is zero
Zbyszek 15:791f35b0f220 237
Zbyszek 15:791f35b0f220 238 this->t.reset(); //reset timer to count until next sample arrives
Zbyszek 15:791f35b0f220 239 this->t.start(); //Start the timer again to start counting
Zbyszek 7:0e9af5986488 240
Zbyszek 15:791f35b0f220 241 return CFAngle; //Return the calculated complementary filter angles
Zbyszek 7:0e9af5986488 242 }
Zbyszek 7:0e9af5986488 243
Zbyszek 7:0e9af5986488 244
Zbyszek 7:0e9af5986488 245
Zbyszek 7:0e9af5986488 246 vector IMU::CalculateQCFAngles(int16_t SamplesPieces[12]) {
Zbyszek 7:0e9af5986488 247
Zbyszek 7:0e9af5986488 248 IMU_Data ConcatenatedValues,NewAngle, SSFValues;
Zbyszek 7:0e9af5986488 249 float deltaT;
Zbyszek 7:0e9af5986488 250 static Quaternion CF = {.w = 1.0f, .x = 0.0001f, .y = 0.0001f, .z = 0.0001f};
Zbyszek 7:0e9af5986488 251 vector Accel_Angles, Eangles, accelSSFVals, SSFAngularRate, QuaternionAngles;
Zbyszek 7:0e9af5986488 252
Zbyszek 7:0e9af5986488 253
Zbyszek 7:0e9af5986488 254 this->t.stop();
Zbyszek 7:0e9af5986488 255 deltaT = this->t.read();
Zbyszek 7:0e9af5986488 256 ConcatenatedValues = this->concatenateData(SamplesPieces);
Zbyszek 7:0e9af5986488 257 SSFValues = this->SSFmultiply(ConcatenatedValues);
Zbyszek 7:0e9af5986488 258
Zbyszek 7:0e9af5986488 259 //convert to radians per second
Zbyszek 7:0e9af5986488 260 SSFValues.Gx *= 0.0174533f;
Zbyszek 7:0e9af5986488 261 SSFValues.Gy *= 0.0174533f;
Zbyszek 7:0e9af5986488 262 SSFValues.Gz *= 0.0174533f;
Zbyszek 7:0e9af5986488 263
Zbyszek 7:0e9af5986488 264 //Angular rates used for Quaternion gyro integration
Zbyszek 7:0e9af5986488 265 SSFAngularRate.x = SSFValues.Gx;
Zbyszek 7:0e9af5986488 266 SSFAngularRate.y = SSFValues.Gy;
Zbyszek 7:0e9af5986488 267 SSFAngularRate.z = SSFValues.Gz;
Zbyszek 7:0e9af5986488 268
Zbyszek 7:0e9af5986488 269 CF = updateQuaternion(CF, SSFAngularRate, deltaT);
Zbyszek 7:0e9af5986488 270 CF = normaliseQuaternion(CF);
Zbyszek 7:0e9af5986488 271 Eangles = eulerA(CF);
Zbyszek 7:0e9af5986488 272
Zbyszek 7:0e9af5986488 273
Zbyszek 7:0e9af5986488 274 //Get accel ssf values into vector format
Zbyszek 7:0e9af5986488 275 accelSSFVals.x = SSFValues.Ax;
Zbyszek 7:0e9af5986488 276 accelSSFVals.y = SSFValues.Ay;
Zbyszek 7:0e9af5986488 277 accelSSFVals.z = SSFValues.Az;
Zbyszek 7:0e9af5986488 278 Accel_Angles = getAccelAngles(accelSSFVals); //Convert Accel data to angle
Zbyszek 7:0e9af5986488 279
Zbyszek 7:0e9af5986488 280
Zbyszek 7:0e9af5986488 281 CF.x = 0.98f*(Eangles.x) + 0.02f*Accel_Angles.x;
Zbyszek 7:0e9af5986488 282 CF.y = 0.98f*(Eangles.y) + 0.02f*Accel_Angles.y;
Zbyszek 7:0e9af5986488 283 CF.z = 0.98f*(Eangles.z) + 0.02f*Accel_Angles.z;
Zbyszek 7:0e9af5986488 284
Zbyszek 7:0e9af5986488 285 QuaternionAngles.x = CF.x;
Zbyszek 7:0e9af5986488 286 QuaternionAngles.y = CF.y;
Zbyszek 7:0e9af5986488 287 QuaternionAngles.z = CF.z;
Zbyszek 7:0e9af5986488 288
Zbyszek 7:0e9af5986488 289 //Convert back to radians
Zbyszek 7:0e9af5986488 290 CF.x *= 0.0174533f;
Zbyszek 7:0e9af5986488 291 CF.y *= 0.0174533f;
Zbyszek 7:0e9af5986488 292 CF.z *= 0.0174533f;
Zbyszek 7:0e9af5986488 293
Zbyszek 7:0e9af5986488 294 CF = euler2Quaternion(CF); //Conver CF angle back to quaternion for next iteration
Zbyszek 7:0e9af5986488 295 CF = normaliseQuaternion(CF); //Normalise the quaternion
Zbyszek 7:0e9af5986488 296
Zbyszek 7:0e9af5986488 297 this->t.reset();
Zbyszek 7:0e9af5986488 298 this->t.start();
Zbyszek 7:0e9af5986488 299
Zbyszek 7:0e9af5986488 300 printf("Quat Angle X: %+2f, Quat Angle Y: %+2f, Quat Angle Z: %+2f, dt: %+2f\n\r", QuaternionAngles.x, QuaternionAngles.y, QuaternionAngles.z, deltaT);
Zbyszek 7:0e9af5986488 301 return QuaternionAngles;
Zbyszek 7:0e9af5986488 302
Zbyszek 7:0e9af5986488 303 }