File content as of revision 15:791f35b0f220:

#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) {

    this->AccelerometerOffset.x = OffsetAX;
    this->AccelerometerOffset.y = OffsetAY;
    this->AccelerometerOffset.z = OffsetAZ;

    this->GyroscopeOffset.x = OffsetGX;
    this->GyroscopeOffset.y = OffsetGY;
    this->GyroscopeOffset.z = OffsetGZ;
    
    
    IMU_Identifier = IMU_ID;
    
    //Accelerometer Sensitvity Scale Factor reciprocals
    switch(SSFA) {
     case 0:
        accelSSF = 0.00006103515625f;                                           //Sensitivity Scale Factor setting 0 reciprocal  
     break;
     case 1:
        accelSSF = 0.0001220703125f;                                            //Sensitivity Scale Factor setting 1 reciprocal  
     break; 
     case 2:
         accelSSF = 0.000244140625f;                                            //Sensitivity Scale Factor setting 2 reciprocal  
     break;
     case 3:
        accelSSF = 0.00048828125f;                                              //Sensitivity Scale Factor setting 3 reciprocal  
     break;
     default:
     break;   
    }
    
    //Gyroscope Sensitivity Scale Factor reciprocals
    switch(SSFG) {
     case 0:
        gyroSSF = 0.00763358778625954198473282442748f;                          //Sensitivity Scale Factor setting 0 reciprocal  
     break;
     case 1:
        gyroSSF = 0.01526717557251908396946564885496f;                          //Sensitivity Scale Factor setting 1 reciprocal  
     break;
     case 2:
         gyroSSF = 0.03048780487804878048780487804878f;                         //Sensitivity Scale Factor setting 2 reciprocal  
     break;
     case 3:
        gyroSSF = 0.06097560975609756097560975609756f;                          //Sensitivity Scale Factor setting 3 reciprocal  
     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) + this->AccelerometerOffset.x;                 //Combine Accelerometer x-axis data and apply offset cancellation value
            break;
            case 1:
                LocalD.Ay = (MSB + LSB) + this->AccelerometerOffset.y;                 //Combine Accelerometer y-axis data and apply offset cancellation value
            break;
            case 2:
                LocalD.Az = (MSB + LSB) + this->AccelerometerOffset.z;                 //Combine Accelerometer z-axis data and apply offset cancellation value
            break;
            case 3:
                LocalD.Gx = (MSB + LSB) + this->GyroscopeOffset.x;                      //Combine Gyroscope x-axis data and apply offset cancellation value
            break;
            case 4:
                LocalD.Gy = (MSB + LSB) + this->GyroscopeOffset.y;                      //Combine Gyroscope y-axis data and apply offset cancellation value
            break;
            case 5:
                LocalD.Gz = (MSB + LSB) + this->GyroscopeOffset.z;                      //Combine Gyroscope z-axis data and apply offset cancellation value
            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;  
    
    //Multiply the raw data by sensitivity scale factor to convert:
    //Accelerometer data to gravity in g.
    localD.Ax = RawData.Ax * this->accelSSF;
    localD.Ay = RawData.Ay * this->accelSSF;
    localD.Az = RawData.Az * this->accelSSF;
    
    //angular velocity to degrees per second
    localD.Gx = RawData.Gx * this->gyroSSF;
    localD.Gy = RawData.Gy * this->gyroSSF;
    localD.Gz = RawData.Gz * this->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;
    
    //use trig functions to calculate tilt angles based on accelerometer values
    //The function returns data in radains and therefore convert to angle by multiplying by 57.295.....
    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
    
    //Integrate with respect to time to get change in angle.
    AngleData.Gx = (newValues.Gx * dt);
    AngleData.Gy = (newValues.Gy * dt);
    AngleData.Gz = (newValues.Gz * dt);
    
    //return the data   
    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;
}
//----------------------------------------------------------------------------------------------------------------------------------------------------------

//Complementary filter calculation function
vector IMU::CalculateCFAngles(int16_t SamplesPieces[12]) {
    
    IMU_Data ConcatenatedValues,NewAngle, SSFValues;
    float deltaT;
    static vector pureGANGLE;
    
    
    this->t.stop();                                                             //Stop timer
    deltaT = this->t.read();                                                    //read how much time has passed since last sample i.e sample period
    ConcatenatedValues = this->concatenateData(SamplesPieces);                  //Get Sample pieces and join together to from actual useful value
    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
    NewAngle           = this->getAngles(SSFValues, deltaT);                    //calculate the accelerometer angles and the change in angle from the new gyroscope data
    
    this->CFAngle.x = 0.98f*(CFAngle.x + NewAngle.Gx) + 0.02f*NewAngle.Ax;      //Complementary filter for x-axis
    this->CFAngle.y = 0.98f*(CFAngle.y + NewAngle.Gy) + 0.02f*NewAngle.Ay;      //Complementary filter for y-axis
    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

    this->t.reset();                                                            //reset timer to count until next sample arrives
    this->t.start();                                                            //Start the timer again to start counting
    
    return CFAngle;                                                             //Return the calculated complementary filter angles  
}



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;
    
}