Zbigniew Druzbacki / Mbed 2 deprecated SPI_Slave

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

Dependencies:   mbed

IMUs.cpp@13:c7e8e277f884, 2019-04-19 (annotated)

Committer:
Zbyszek
Date:
Fri Apr 19 18:43:39 2019 +0000
Revision:
13:c7e8e277f884
Parent:
11:366f1186c121
Child:
14:7bbaafa22f8d
Added IMU data transmission handling

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 6:0ebecfecadc9 9 AccelerometerOffset.x = OffsetAX;
Zbyszek 6:0ebecfecadc9 10 AccelerometerOffset.y = OffsetAY;
Zbyszek 6:0ebecfecadc9 11 AccelerometerOffset.z = OffsetAZ;
Zbyszek 6:0ebecfecadc9 12
Zbyszek 6:0ebecfecadc9 13 GyroscopeOffset.x = OffsetGX;
Zbyszek 6:0ebecfecadc9 14 GyroscopeOffset.y = OffsetGY;
Zbyszek 6:0ebecfecadc9 15 GyroscopeOffset.z = OffsetGZ;
Zbyszek 6:0ebecfecadc9 16
Zbyszek 6:0ebecfecadc9 17
Zbyszek 6:0ebecfecadc9 18 IMU_Identifier = IMU_ID;
Zbyszek 6:0ebecfecadc9 19
Zbyszek 6:0ebecfecadc9 20 switch(SSFA) {
Zbyszek 6:0ebecfecadc9 21 case 0:
Zbyszek 6:0ebecfecadc9 22 accelSSF = 0.00006103515625f;
Zbyszek 6:0ebecfecadc9 23 break;
Zbyszek 6:0ebecfecadc9 24 case 1:
Zbyszek 6:0ebecfecadc9 25 accelSSF = 0.0001220703125f;
Zbyszek 6:0ebecfecadc9 26 break;
Zbyszek 6:0ebecfecadc9 27 case 2:
Zbyszek 6:0ebecfecadc9 28 accelSSF = 0.000244140625f;
Zbyszek 6:0ebecfecadc9 29 break;
Zbyszek 6:0ebecfecadc9 30 case 3:
Zbyszek 6:0ebecfecadc9 31 accelSSF = 0.00048828125f;
Zbyszek 6:0ebecfecadc9 32 break;
Zbyszek 6:0ebecfecadc9 33 default:
Zbyszek 6:0ebecfecadc9 34 break;
Zbyszek 6:0ebecfecadc9 35 }
Zbyszek 6:0ebecfecadc9 36
Zbyszek 6:0ebecfecadc9 37 switch(SSFG) {
Zbyszek 6:0ebecfecadc9 38 case 0:
Zbyszek 6:0ebecfecadc9 39 gyroSSF = 0.00763358778625954198473282442748f;
Zbyszek 6:0ebecfecadc9 40 break;
Zbyszek 6:0ebecfecadc9 41 case 1:
Zbyszek 6:0ebecfecadc9 42 gyroSSF = 0.01526717557251908396946564885496f;
Zbyszek 6:0ebecfecadc9 43 break;
Zbyszek 6:0ebecfecadc9 44 case 2:
Zbyszek 6:0ebecfecadc9 45 gyroSSF = 0.03048780487804878048780487804878f;
Zbyszek 6:0ebecfecadc9 46 break;
Zbyszek 6:0ebecfecadc9 47 case 3:
Zbyszek 6:0ebecfecadc9 48 gyroSSF = 0.06097560975609756097560975609756f;
Zbyszek 6:0ebecfecadc9 49 break;
Zbyszek 6:0ebecfecadc9 50 default:
Zbyszek 6:0ebecfecadc9 51 break;
Zbyszek 6:0ebecfecadc9 52 }
Zbyszek 6:0ebecfecadc9 53
Zbyszek 7:0e9af5986488 54 this->t.start();
Zbyszek 7:0e9af5986488 55
Zbyszek 6:0ebecfecadc9 56 }
Zbyszek 6:0ebecfecadc9 57
Zbyszek 6:0ebecfecadc9 58 //void IMU::CFAngle() {
Zbyszek 6:0ebecfecadc9 59
Zbyszek 6:0ebecfecadc9 60
Zbyszek 6:0ebecfecadc9 61
Zbyszek 6:0ebecfecadc9 62 //}
Zbyszek 6:0ebecfecadc9 63
Zbyszek 6:0ebecfecadc9 64 //----------------------------------------------------------------------------------------------------------------------------------------------------------
Zbyszek 6:0ebecfecadc9 65 IMU_Data IMU::concatenateData(int16_t SamplesPieces[12]) {
Zbyszek 6:0ebecfecadc9 66 //Local-Variables------Local-Variables------Local-Variables------Local-Variables
Zbyszek 6:0ebecfecadc9 67
Zbyszek 6:0ebecfecadc9 68 int16_t MSB = 0; //Store Most Significant Byte of data piece in this variable for processing
Zbyszek 6:0ebecfecadc9 69 int16_t LSB = 0; //Store Least Significant Byte of data piece in this variable for processing
Zbyszek 6:0ebecfecadc9 70 char arrPointer = 0; //Array Pointer
Zbyszek 6:0ebecfecadc9 71
Zbyszek 6:0ebecfecadc9 72 IMU_Data LocalD;
Zbyszek 6:0ebecfecadc9 73
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 6:0ebecfecadc9 86 LocalD.Ax = (MSB + LSB) + AccelerometerOffset.x; //Combine Accelerometer x-axis data
Zbyszek 6:0ebecfecadc9 87 break;
Zbyszek 6:0ebecfecadc9 88 case 1:
Zbyszek 6:0ebecfecadc9 89 LocalD.Ay = (MSB + LSB) + AccelerometerOffset.y; //Combine Accelerometer y-axis data
Zbyszek 6:0ebecfecadc9 90 break;
Zbyszek 6:0ebecfecadc9 91 case 2:
Zbyszek 6:0ebecfecadc9 92 LocalD.Az = (MSB + LSB) + AccelerometerOffset.z; //Combine Accelerometer z-axis data
Zbyszek 6:0ebecfecadc9 93 break;
Zbyszek 6:0ebecfecadc9 94 case 3:
Zbyszek 6:0ebecfecadc9 95 LocalD.Gx = (MSB + LSB) + GyroscopeOffset.x; //Combine Gyroscope x-axis data
Zbyszek 6:0ebecfecadc9 96 break;
Zbyszek 6:0ebecfecadc9 97 case 4:
Zbyszek 6:0ebecfecadc9 98 LocalD.Gy = (MSB + LSB) + GyroscopeOffset.y; //Combine Gyroscope y-axis data
Zbyszek 6:0ebecfecadc9 99 break;
Zbyszek 6:0ebecfecadc9 100 case 5:
Zbyszek 6:0ebecfecadc9 101 LocalD.Gz = (MSB + LSB) + GyroscopeOffset.z; //Combine Gyroscope z-axis data
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 11:366f1186c121 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
Zbyszek 6:0ebecfecadc9 110 return LocalD;
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 //----------------------------------------------------------------------------------------------------------------------------------------------------------
Zbyszek 6:0ebecfecadc9 119 IMU_Data IMU::SSFmultiply(IMU_Data RawData) {
Zbyszek 6:0ebecfecadc9 120
Zbyszek 6:0ebecfecadc9 121 IMU_Data localD;
Zbyszek 6:0ebecfecadc9 122
Zbyszek 6:0ebecfecadc9 123
Zbyszek 6:0ebecfecadc9 124 localD.Ax = RawData.Ax * accelSSF;
Zbyszek 6:0ebecfecadc9 125 localD.Ay = RawData.Ay * accelSSF;
Zbyszek 6:0ebecfecadc9 126 localD.Az = RawData.Az * accelSSF;
Zbyszek 6:0ebecfecadc9 127
Zbyszek 6:0ebecfecadc9 128 localD.Gx = RawData.Gx * gyroSSF;
Zbyszek 6:0ebecfecadc9 129 localD.Gy = RawData.Gy * gyroSSF;
Zbyszek 6:0ebecfecadc9 130 localD.Gz = RawData.Gz * gyroSSF;
Zbyszek 6:0ebecfecadc9 131
Zbyszek 7:0e9af5986488 132 //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 133 return localD;
Zbyszek 6:0ebecfecadc9 134 }
Zbyszek 7:0e9af5986488 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 IMU_Data IMU::getAngles(IMU_Data SSF_Format_Values, float dt) {
Zbyszek 7:0e9af5986488 143
Zbyszek 7:0e9af5986488 144 IMU_Data newValues, AngleData;
Zbyszek 7:0e9af5986488 145
Zbyszek 7:0e9af5986488 146 newValues = SSF_Format_Values;
Zbyszek 7:0e9af5986488 147
Zbyszek 7:0e9af5986488 148 AngleData.Ax = (atan2f(newValues.Ay, newValues.Az) * 57.29577951f);
Zbyszek 7:0e9af5986488 149 AngleData.Ay = (atan2f((-newValues.Ax), sqrt(pow(newValues.Ay, 2) + pow(newValues.Az, 2) )) * 57.29577951f);
Zbyszek 7:0e9af5986488 150 AngleData.Az = 0; //Cannot calculate angle for this one as it remains constant all the time
Zbyszek 7:0e9af5986488 151
Zbyszek 7:0e9af5986488 152 AngleData.Gx = (newValues.Gx * dt);
Zbyszek 7:0e9af5986488 153 AngleData.Gy = (newValues.Gy * dt);
Zbyszek 7:0e9af5986488 154 AngleData.Gz = (newValues.Gz * dt);
Zbyszek 7:0e9af5986488 155
Zbyszek 7:0e9af5986488 156 //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 157 return AngleData;
Zbyszek 7:0e9af5986488 158
Zbyszek 7:0e9af5986488 159 }
Zbyszek 7:0e9af5986488 160 //----------------------------------------------------------------------------------------------------------------------------------------------------------
Zbyszek 7:0e9af5986488 161
Zbyszek 7:0e9af5986488 162
Zbyszek 7:0e9af5986488 163
Zbyszek 7:0e9af5986488 164
Zbyszek 7:0e9af5986488 165
Zbyszek 7:0e9af5986488 166 //----------------------------------------------------------------------------------------------------------------------------------------------------------
Zbyszek 7:0e9af5986488 167 vector IMU::getAccelAngles(vector SSF_Accel) {
Zbyszek 7:0e9af5986488 168
Zbyszek 7:0e9af5986488 169 vector newValues, AngleData;
Zbyszek 7:0e9af5986488 170
Zbyszek 7:0e9af5986488 171 newValues = SSF_Accel;
Zbyszek 7:0e9af5986488 172
Zbyszek 7:0e9af5986488 173 AngleData.x = (atan2f(newValues.y, newValues.z) * 57.29577951f);
Zbyszek 7:0e9af5986488 174 AngleData.y = (atan2f((-newValues.x), sqrt(pow(newValues.y, 2) + pow(newValues.z, 2) )) * 57.29577951f);
Zbyszek 7:0e9af5986488 175 //AngleData.z = 0; //Cannot calculate angle for this one as it remains constant all the time
Zbyszek 7:0e9af5986488 176
Zbyszek 7:0e9af5986488 177 //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 178 return AngleData;
Zbyszek 7:0e9af5986488 179
Zbyszek 7:0e9af5986488 180 }
Zbyszek 7:0e9af5986488 181 //----------------------------------------------------------------------------------------------------------------------------------------------------------
Zbyszek 7:0e9af5986488 182
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 IMU_Data IMU::CalculateAngles(int16_t SamplesPieces[12]){
Zbyszek 7:0e9af5986488 192
Zbyszek 7:0e9af5986488 193 IMU_Data ConcatenatedValues, SSFValues, SimpleAngles;
Zbyszek 7:0e9af5986488 194 static IMU_Data PrevSimpleAngles;
Zbyszek 7:0e9af5986488 195 float deltaT;
Zbyszek 7:0e9af5986488 196
Zbyszek 7:0e9af5986488 197 ConcatenatedValues = this->concatenateData(SamplesPieces);
Zbyszek 7:0e9af5986488 198 SSFValues = this->SSFmultiply(ConcatenatedValues);
Zbyszek 7:0e9af5986488 199
Zbyszek 7:0e9af5986488 200 this->t.stop();
Zbyszek 7:0e9af5986488 201 deltaT = this->t.read();
Zbyszek 7:0e9af5986488 202 SimpleAngles = this->getAngles(SSFValues, deltaT);
Zbyszek 7:0e9af5986488 203
Zbyszek 7:0e9af5986488 204 PrevSimpleAngles.Gx = PrevSimpleAngles.Gx + SimpleAngles.Gx;
Zbyszek 7:0e9af5986488 205 PrevSimpleAngles.Gy = PrevSimpleAngles.Gy + SimpleAngles.Gy;
Zbyszek 7:0e9af5986488 206 PrevSimpleAngles.Gz = PrevSimpleAngles.Gz + SimpleAngles.Gz;
Zbyszek 7:0e9af5986488 207
Zbyszek 7:0e9af5986488 208 this->t.reset();
Zbyszek 7:0e9af5986488 209 this->t.start();
Zbyszek 7:0e9af5986488 210
Zbyszek 7:0e9af5986488 211 //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 212 return PrevSimpleAngles;
Zbyszek 7:0e9af5986488 213 }
Zbyszek 7:0e9af5986488 214 //----------------------------------------------------------------------------------------------------------------------------------------------------------
Zbyszek 7:0e9af5986488 215
Zbyszek 7:0e9af5986488 216
Zbyszek 7:0e9af5986488 217 vector IMU::CalculateCFAngles(int16_t SamplesPieces[12]) {
Zbyszek 7:0e9af5986488 218
Zbyszek 7:0e9af5986488 219 IMU_Data ConcatenatedValues,NewAngle, SSFValues;
Zbyszek 7:0e9af5986488 220 float deltaT;
Zbyszek 7:0e9af5986488 221
Zbyszek 7:0e9af5986488 222
Zbyszek 7:0e9af5986488 223 this->t.stop();
Zbyszek 7:0e9af5986488 224 deltaT = this->t.read();
Zbyszek 7:0e9af5986488 225 ConcatenatedValues = this->concatenateData(SamplesPieces);
Zbyszek 7:0e9af5986488 226 SSFValues = this->SSFmultiply(ConcatenatedValues);
Zbyszek 7:0e9af5986488 227 NewAngle = this->getAngles(SSFValues, deltaT);
Zbyszek 7:0e9af5986488 228
Zbyszek 11:366f1186c121 229 this->CFAngle.x = 0.98f*(CFAngle.x + NewAngle.Gx) + 0.02f*NewAngle.Ax;
Zbyszek 11:366f1186c121 230 this->CFAngle.y = 0.98f*(CFAngle.y + NewAngle.Gy) + 0.02f*NewAngle.Ay;
Zbyszek 11:366f1186c121 231 this->CFAngle.z = 0.98f*(CFAngle.z + NewAngle.Gz) + 0.02f*NewAngle.Az;
Zbyszek 7:0e9af5986488 232
Zbyszek 11:366f1186c121 233 t.reset();
Zbyszek 11:366f1186c121 234 t.start();
Zbyszek 7:0e9af5986488 235
Zbyszek 7:0e9af5986488 236 //printf("Accel X: %+2f, Accel Y: %+2f, Accel Z: %+2f, dt: %+2f\n\r", CFAngle.x, CFAngle.y, CFAngle.z, deltaT);
Zbyszek 7:0e9af5986488 237 printf("%+2f,%+2f,%+2f\n\r", CFAngle.x, CFAngle.y, CFAngle.z);
Zbyszek 7:0e9af5986488 238 return CFAngle;
Zbyszek 7:0e9af5986488 239
Zbyszek 7:0e9af5986488 240 }
Zbyszek 7:0e9af5986488 241
Zbyszek 7:0e9af5986488 242
Zbyszek 7:0e9af5986488 243
Zbyszek 7:0e9af5986488 244 vector IMU::CalculateQCFAngles(int16_t SamplesPieces[12]) {
Zbyszek 7:0e9af5986488 245
Zbyszek 7:0e9af5986488 246 IMU_Data ConcatenatedValues,NewAngle, SSFValues;
Zbyszek 7:0e9af5986488 247 float deltaT;
Zbyszek 7:0e9af5986488 248 static Quaternion CF = {.w = 1.0f, .x = 0.0001f, .y = 0.0001f, .z = 0.0001f};
Zbyszek 7:0e9af5986488 249 vector Accel_Angles, Eangles, accelSSFVals, SSFAngularRate, QuaternionAngles;
Zbyszek 7:0e9af5986488 250
Zbyszek 7:0e9af5986488 251
Zbyszek 7:0e9af5986488 252 this->t.stop();
Zbyszek 7:0e9af5986488 253 deltaT = this->t.read();
Zbyszek 7:0e9af5986488 254 ConcatenatedValues = this->concatenateData(SamplesPieces);
Zbyszek 7:0e9af5986488 255 SSFValues = this->SSFmultiply(ConcatenatedValues);
Zbyszek 7:0e9af5986488 256
Zbyszek 7:0e9af5986488 257 //convert to radians per second
Zbyszek 7:0e9af5986488 258 SSFValues.Gx *= 0.0174533f;
Zbyszek 7:0e9af5986488 259 SSFValues.Gy *= 0.0174533f;
Zbyszek 7:0e9af5986488 260 SSFValues.Gz *= 0.0174533f;
Zbyszek 7:0e9af5986488 261
Zbyszek 7:0e9af5986488 262 //Angular rates used for Quaternion gyro integration
Zbyszek 7:0e9af5986488 263 SSFAngularRate.x = SSFValues.Gx;
Zbyszek 7:0e9af5986488 264 SSFAngularRate.y = SSFValues.Gy;
Zbyszek 7:0e9af5986488 265 SSFAngularRate.z = SSFValues.Gz;
Zbyszek 7:0e9af5986488 266
Zbyszek 7:0e9af5986488 267 CF = updateQuaternion(CF, SSFAngularRate, deltaT);
Zbyszek 7:0e9af5986488 268 CF = normaliseQuaternion(CF);
Zbyszek 7:0e9af5986488 269 Eangles = eulerA(CF);
Zbyszek 7:0e9af5986488 270
Zbyszek 7:0e9af5986488 271
Zbyszek 7:0e9af5986488 272 //Get accel ssf values into vector format
Zbyszek 7:0e9af5986488 273 accelSSFVals.x = SSFValues.Ax;
Zbyszek 7:0e9af5986488 274 accelSSFVals.y = SSFValues.Ay;
Zbyszek 7:0e9af5986488 275 accelSSFVals.z = SSFValues.Az;
Zbyszek 7:0e9af5986488 276 Accel_Angles = getAccelAngles(accelSSFVals); //Convert Accel data to angle
Zbyszek 7:0e9af5986488 277
Zbyszek 7:0e9af5986488 278
Zbyszek 7:0e9af5986488 279 CF.x = 0.98f*(Eangles.x) + 0.02f*Accel_Angles.x;
Zbyszek 7:0e9af5986488 280 CF.y = 0.98f*(Eangles.y) + 0.02f*Accel_Angles.y;
Zbyszek 7:0e9af5986488 281 CF.z = 0.98f*(Eangles.z) + 0.02f*Accel_Angles.z;
Zbyszek 7:0e9af5986488 282
Zbyszek 7:0e9af5986488 283 QuaternionAngles.x = CF.x;
Zbyszek 7:0e9af5986488 284 QuaternionAngles.y = CF.y;
Zbyszek 7:0e9af5986488 285 QuaternionAngles.z = CF.z;
Zbyszek 7:0e9af5986488 286
Zbyszek 7:0e9af5986488 287 //Convert back to radians
Zbyszek 7:0e9af5986488 288 CF.x *= 0.0174533f;
Zbyszek 7:0e9af5986488 289 CF.y *= 0.0174533f;
Zbyszek 7:0e9af5986488 290 CF.z *= 0.0174533f;
Zbyszek 7:0e9af5986488 291
Zbyszek 7:0e9af5986488 292 CF = euler2Quaternion(CF); //Conver CF angle back to quaternion for next iteration
Zbyszek 7:0e9af5986488 293 CF = normaliseQuaternion(CF); //Normalise the quaternion
Zbyszek 7:0e9af5986488 294
Zbyszek 7:0e9af5986488 295 this->t.reset();
Zbyszek 7:0e9af5986488 296 this->t.start();
Zbyszek 7:0e9af5986488 297
Zbyszek 7:0e9af5986488 298 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 299 return QuaternionAngles;
Zbyszek 7:0e9af5986488 300
Zbyszek 7:0e9af5986488 301 }