Diff: IMUs.cpp

Revision:

7:0e9af5986488

Parent:

6:0ebecfecadc9

Child:

11:366f1186c121

--- a/IMUs.cpp	Tue Feb 26 01:22:53 2019 +0000
+++ b/IMUs.cpp	Tue Feb 26 18:57:16 2019 +0000
@@ -1,4 +1,5 @@
 #include "Structures.h"
+#include "Quaternions.h"
 #include "IMUs.h"
 #include "mbed.h"
 
@@ -50,6 +51,8 @@
      break;   
     }
     
+    this->t.start();     
+    
 }
 
 //void IMU::CFAngle() {
@@ -124,7 +127,175 @@
     localD.Gy = RawData.Gy * gyroSSF;
     localD.Gz = RawData.Gz * gyroSSF;
         
-        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);
+        //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);
     return localD;
 }
-//----------------------------------------------------------------------------------------------------------------------------------------------------------
\ No newline at end of file
+//----------------------------------------------------------------------------------------------------------------------------------------------------------
+
+
+
+
+
+//----------------------------------------------------------------------------------------------------------------------------------------------------------
+IMU_Data IMU::getAngles(IMU_Data SSF_Format_Values, float dt) {
+    
+    IMU_Data newValues, AngleData;
+    
+    newValues = SSF_Format_Values;
+    
+    AngleData.Ax = (atan2f(newValues.Ay, newValues.Az) * 57.29577951f); 
+    AngleData.Ay = (atan2f((-newValues.Ax), sqrt(pow(newValues.Ay, 2) + pow(newValues.Az, 2) )) * 57.29577951f);
+    AngleData.Az = 0;                                                           //Cannot calculate angle for this one as it remains constant all the time
+    
+    AngleData.Gx = (newValues.Gx * dt);
+    AngleData.Gy = (newValues.Gy * dt);
+    AngleData.Gz = (newValues.Gz * dt);
+    
+        //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);
+    return AngleData;
+    
+}
+//----------------------------------------------------------------------------------------------------------------------------------------------------------
+
+
+
+
+
+//----------------------------------------------------------------------------------------------------------------------------------------------------------
+vector IMU::getAccelAngles(vector SSF_Accel) {
+    
+    vector newValues, AngleData;
+    
+    newValues = SSF_Accel;
+    
+    AngleData.x = (atan2f(newValues.y, newValues.z) * 57.29577951f); 
+    AngleData.y = (atan2f((-newValues.x), sqrt(pow(newValues.y, 2) + pow(newValues.z, 2) )) * 57.29577951f);
+    //AngleData.z = 0;                                                           //Cannot calculate angle for this one as it remains constant all the time
+    
+        //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);
+    return AngleData;
+    
+}
+//----------------------------------------------------------------------------------------------------------------------------------------------------------
+
+
+
+
+
+
+
+
+//----------------------------------------------------------------------------------------------------------------------------------------------------------
+IMU_Data IMU::CalculateAngles(int16_t SamplesPieces[12]){
+    
+    IMU_Data ConcatenatedValues, SSFValues, SimpleAngles;
+    static IMU_Data PrevSimpleAngles;
+    float deltaT;
+    
+    ConcatenatedValues = this->concatenateData(SamplesPieces);
+    SSFValues = this->SSFmultiply(ConcatenatedValues);
+    
+    this->t.stop();
+    deltaT = this->t.read();
+    SimpleAngles = this->getAngles(SSFValues, deltaT);
+    
+    PrevSimpleAngles.Gx = PrevSimpleAngles.Gx + SimpleAngles.Gx;
+    PrevSimpleAngles.Gy = PrevSimpleAngles.Gy + SimpleAngles.Gy;
+    PrevSimpleAngles.Gz = PrevSimpleAngles.Gz + SimpleAngles.Gz;
+    
+    this->t.reset();
+    this->t.start();
+    
+        //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);
+    return PrevSimpleAngles;
+}
+//----------------------------------------------------------------------------------------------------------------------------------------------------------
+
+
+vector IMU::CalculateCFAngles(int16_t SamplesPieces[12]) {
+    
+    IMU_Data ConcatenatedValues,NewAngle, SSFValues;
+    float deltaT;
+    static vector CFAngle;
+    
+    
+    this->t.stop();
+    deltaT = this->t.read();
+    ConcatenatedValues = this->concatenateData(SamplesPieces);
+    SSFValues          = this->SSFmultiply(ConcatenatedValues);
+    NewAngle           = this->getAngles(SSFValues, deltaT);
+    
+    CFAngle.x = 0.98f*(CFAngle.x + NewAngle.Gx) + 0.02f*NewAngle.Ax;
+    CFAngle.y = 0.98f*(CFAngle.y + NewAngle.Gy) + 0.02f*NewAngle.Ay;
+    CFAngle.z = 0.98f*(CFAngle.z + NewAngle.Gz) + 0.02f*NewAngle.Az;
+    
+    this->t.reset();
+    this->t.start();
+    
+        //printf("Accel X: %+2f,    Accel Y: %+2f,    Accel Z: %+2f,  dt: %+2f\n\r", CFAngle.x, CFAngle.y, CFAngle.z, deltaT);
+        printf("%+2f,%+2f,%+2f\n\r", CFAngle.x, CFAngle.y, CFAngle.z);
+    return CFAngle;
+    
+}
+
+
+
+vector IMU::CalculateQCFAngles(int16_t SamplesPieces[12]) {
+    
+    IMU_Data ConcatenatedValues,NewAngle, SSFValues;
+    float deltaT;
+    static vector CFAngle;
+    static Quaternion CF = {.w = 1.0f, .x = 0.0001f, .y = 0.0001f, .z = 0.0001f};
+    vector Accel_Angles, Eangles, accelSSFVals, SSFAngularRate, QuaternionAngles;
+    
+       
+    this->t.stop();
+    deltaT = this->t.read();
+    ConcatenatedValues = this->concatenateData(SamplesPieces);
+    SSFValues          = this->SSFmultiply(ConcatenatedValues);                                        
+    
+    //convert to radians per second
+    SSFValues.Gx *= 0.0174533f;
+    SSFValues.Gy *= 0.0174533f; 
+    SSFValues.Gz *= 0.0174533f;  
+    
+    //Angular rates used for Quaternion gyro integration
+    SSFAngularRate.x = SSFValues.Gx;
+    SSFAngularRate.y = SSFValues.Gy;
+    SSFAngularRate.z = SSFValues.Gz;    
+    
+    CF = updateQuaternion(CF, SSFAngularRate, deltaT); 
+    CF = normaliseQuaternion(CF);
+    Eangles = eulerA(CF);
+    
+    
+    //Get accel ssf values into vector format
+    accelSSFVals.x = SSFValues.Ax;
+    accelSSFVals.y = SSFValues.Ay;
+    accelSSFVals.z = SSFValues.Az;
+    Accel_Angles = getAccelAngles(accelSSFVals);                                //Convert Accel data to angle
+        
+    
+    CF.x = 0.98f*(Eangles.x) + 0.02f*Accel_Angles.x;
+    CF.y = 0.98f*(Eangles.y) + 0.02f*Accel_Angles.y;
+    CF.z = 0.98f*(Eangles.z) + 0.02f*Accel_Angles.z;
+   
+    QuaternionAngles.x = CF.x;
+    QuaternionAngles.y = CF.y;
+    QuaternionAngles.z = CF.z;
+    
+    //Convert back to radians 
+    CF.x *= 0.0174533f;             
+    CF.y *= 0.0174533f;
+    CF.z *= 0.0174533f;
+    
+    CF = euler2Quaternion(CF);                                                  //Conver CF angle back to quaternion for next iteration
+    CF = normaliseQuaternion(CF);                                               //Normalise the quaternion
+    
+    this->t.reset();
+    this->t.start();
+    
+    printf("Quat Angle X: %+2f,    Quat Angle Y: %+2f,    Quat Angle Z: %+2f,  dt: %+2f\n\r", QuaternionAngles.x, QuaternionAngles.y, QuaternionAngles.z, deltaT);
+    return QuaternionAngles;
+    
+}   
\ No newline at end of file