File content as of revision 14:7bbaafa22f8d:

#include "CustomDatatypes.h"
#include "Quaternions.h"
#include "IMUs.h"
#include "mbed.h"

//IMU Class Constructor
IMU::IMU(char IMU_ID, double OffsetAX, double OffsetAY, double OffsetAZ, double OffsetGX, double OffsetGY, double OffsetGZ, char SSFA, char SSFG) {

    AccelerometerOffset.x = OffsetAX;
    AccelerometerOffset.y = OffsetAY;
    AccelerometerOffset.z = OffsetAZ;

    GyroscopeOffset.x = OffsetGX;
    GyroscopeOffset.y = OffsetGY;
    GyroscopeOffset.z = OffsetGZ;
    
    
    IMU_Identifier = IMU_ID;
    
    switch(SSFA) {
     case 0:
        accelSSF = 0.00006103515625f;     
     break;
     case 1:
        accelSSF = 0.0001220703125f;
     break;
     case 2:
         accelSSF = 0.000244140625f; 
     break;
     case 3:
        accelSSF = 0.00048828125f; 
     break;
     default:
     break;   
    }
    
    switch(SSFG) {
     case 0:
        gyroSSF = 0.00763358778625954198473282442748f;     
     break;
     case 1:
        gyroSSF = 0.01526717557251908396946564885496f;
     break;
     case 2:
         gyroSSF = 0.03048780487804878048780487804878f; 
     break;
     case 3:
        gyroSSF = 0.06097560975609756097560975609756f; 
     break;
     default:
     break;   
    }
    
    this->t.start();     
    
}

//void IMU::CFAngle() {
    
    
    
//}

//----------------------------------------------------------------------------------------------------------------------------------------------------------
IMU_Data IMU::concatenateData(int16_t SamplesPieces[12]) {
//Local-Variables------Local-Variables------Local-Variables------Local-Variables

    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
    
    IMU_Data LocalD;
    
//Procedure--------------Procedure------------Procedure-------------Procedure---      
        for(char x = 0; x <= 5; x++) {
        MSB = SamplesPieces[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 = SamplesPieces[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:
                LocalD.Ax = (MSB + LSB) + AccelerometerOffset.x;                 //Combine Accelerometer x-axis data
            break;
            case 1:
                LocalD.Ay = (MSB + LSB) + AccelerometerOffset.y;                 //Combine Accelerometer y-axis data
            break;
            case 2:
                LocalD.Az = (MSB + LSB) + AccelerometerOffset.z;                 //Combine Accelerometer z-axis data
            break;
            case 3:
                LocalD.Gx = (MSB + LSB) + GyroscopeOffset.x;                      //Combine Gyroscope x-axis data
            break;
            case 4:
                LocalD.Gy = (MSB + LSB) + GyroscopeOffset.y;                      //Combine Gyroscope y-axis data
            break;
            case 5:
                LocalD.Gz = (MSB + LSB) + GyroscopeOffset.z;                      //Combine Gyroscope z-axis data
            break;
            default:
            break; 
        }//switch(x) 
    }//for(char x = 0; x <= 5; x++)
    
    //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;
}
//----------------------------------------------------------------------------------------------------------------------------------------------------------





//----------------------------------------------------------------------------------------------------------------------------------------------------------
IMU_Data IMU::SSFmultiply(IMU_Data RawData) {
    
    IMU_Data localD;  
    

    localD.Ax = RawData.Ax * accelSSF;
    localD.Ay = RawData.Ay * accelSSF;
    localD.Az = RawData.Az * accelSSF;
    
    localD.Gx = RawData.Gx * gyroSSF;
    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);
    return localD;
}
//----------------------------------------------------------------------------------------------------------------------------------------------------------





//----------------------------------------------------------------------------------------------------------------------------------------------------------
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;
    
    
    this->t.stop();
    deltaT = this->t.read();
    ConcatenatedValues = this->concatenateData(SamplesPieces);
    SSFValues          = this->SSFmultiply(ConcatenatedValues);
    NewAngle           = this->getAngles(SSFValues, deltaT);
    
    this->CFAngle.x = 0.98f*(CFAngle.x + NewAngle.Gx) + 0.02f*NewAngle.Ax;
    this->CFAngle.y = 0.98f*(CFAngle.y + NewAngle.Gy) + 0.02f*NewAngle.Ay;
    this->CFAngle.z = 0.98f*(CFAngle.z + NewAngle.Gz) + 0.02f*NewAngle.Az;
    
    t.reset();
    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 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;
    
}