File content as of revision 1:115cf0a84a9d:

#include "DCM.h"
#include "Matrix.h"
#include "math.h"
#include "Sensors.h"

#include "mbed.h"
//Serial pc3(USBTX, USBRX);

#define MAG_SKIP 1

float DCM::constrain(float x, float a, float b) 
{
    float result = x;
    
    if (x < a) result = a;
    if (x > b) result = b;

    return result;  
}


DCM::DCM(void):
    G_Dt(0.25), update_count(MAG_SKIP)
{
    Accel_Init();
    Gyro_Init();
    Magn_Init();
    for (int m=0; m < 3; m++) {
        Accel_Vector[m] = 0;
        Gyro_Vector[m] = 0;
        Omega_Vector[m] = 0;
        Omega_P[m] = 0;
        Omega_I[m] = 0;
        Omega[m] = 0;
        errorRollPitch[m] = 0; 
        errorYaw[m] = 0;
        for (int n=0; n < 3; n++) {
            dcm[m][n] = (m == n) ? 1 : 0; // dcm starts as identity matrix
        }
    }
    
}


/**************************************************/
void DCM::Normalize(void)
{
  float error=0;
  float temporary[3][3];
  float renorm=0;
  
    error= -Vector_Dot_Product(&dcm[0][0], &dcm[1][0])*.5; //eq.19

  Vector_Scale(&temporary[0][0], &dcm[1][0], error); //eq.19
  Vector_Scale(&temporary[1][0], &dcm[0][0], error); //eq.19
    
  Vector_Add(&temporary[0][0], &temporary[0][0], &dcm[0][0]);//eq.19
  Vector_Add(&temporary[1][0], &temporary[1][0], &dcm[1][0]);//eq.19
  
  Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20
  
  renorm= .5 *(3 - Vector_Dot_Product(&temporary[0][0],&temporary[0][0])); //eq.21
  Vector_Scale(&dcm[0][0], &temporary[0][0], renorm);
  
  renorm= .5 *(3 - Vector_Dot_Product(&temporary[1][0],&temporary[1][0])); //eq.21
  Vector_Scale(&dcm[1][0], &temporary[1][0], renorm);
  
  renorm= .5 *(3 - Vector_Dot_Product(&temporary[2][0],&temporary[2][0])); //eq.21
  Vector_Scale(&dcm[2][0], &temporary[2][0], renorm);
}

/**************************************************/
void DCM::Drift_correction(void)
{
  float mag_heading_x;
  float mag_heading_y;
  float errorCourse;
  //Compensation the Roll, Pitch and Yaw drift. 
  static float Scaled_Omega_P[3];
  static float Scaled_Omega_I[3];
  float Accel_magnitude;
  float Accel_weight;
  
  
  //*****Roll and Pitch***************

  // Calculate the magnitude of the accelerometer vector
  Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]);
  Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
  // Dynamic weighting of accelerometer info (reliability filter)
  // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
  Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1);  //  
    
  Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&dcm[2][0]); //adjust the ground of reference
  Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
  
  Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
  Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);  
  
  //*****YAW***************
  // We make the gyro YAW drift correction based on compass magnetic heading
    mag_heading_x = cos(MAG_Heading);
    mag_heading_y = sin(MAG_Heading);
    errorCourse=(dcm[0][0]*mag_heading_y) - (dcm[1][0]*mag_heading_x);  //Calculating YAW error
    Vector_Scale(errorYaw,&dcm[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
    
    Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);//.01proportional of YAW.
    Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding  Proportional.
    
    Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);//.00001Integrator
    Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I

}


/**************************************************/
void DCM::Matrix_update(void)
{

    Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]);  //adding proportional term
    Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term

    #if OUTPUTMODE==1         
    Update_Matrix[0][0]=0;
    Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
    Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
    Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
    Update_Matrix[1][1]=0;
    Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x
    Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
    Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
    Update_Matrix[2][2]=0;
    #else                    // Uncorrected data (no drift correction)
    Update_Matrix[0][0]=0;
    Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z
    Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y
    Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z
    Update_Matrix[1][1]=0;
    Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0];
    Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1];
    Update_Matrix[2][1]=G_Dt*Gyro_Vector[0];
    Update_Matrix[2][2]=0;
    #endif
    
    Matrix_Multiply(Temporary_Matrix, dcm, Update_Matrix); //c=a*b
    // ???  Matrix_Add(dcm, dcm, Temporary_Matrix); ???
    for(int x=0; x<3; x++) { //Matrix Addition (update)
        for(int y=0; y<3; y++) {
            dcm[x][y] += Temporary_Matrix[x][y];
        } 
    }
}

void DCM::Euler_angles(void)
{
  pitch = RAD2DEG(-asin(dcm[2][0]));
  roll = RAD2DEG(atan2(dcm[2][1],dcm[2][2]));
  yaw = RAD2DEG(atan2(dcm[1][0],dcm[0][0]));
}

void DCM::Update_step()
{
    Read_Accel();         
    Read_Gyro();
    if (--update_count == 0) {
        Update_mag();
        update_count = MAG_SKIP;
    }
    Matrix_update();
    Normalize();
    Drift_correction();
    Euler_angles();
}

void DCM::Update_mag()
{
    Read_Magn();
    Compass_Heading();
}

void DCM::Compass_Heading()
{
    float mag_x;
    float mag_y;
    float cos_roll;
    float sin_roll;
    float cos_pitch;
    float sin_pitch;
  
    cos_roll = cos(roll);
    sin_roll = sin(roll);
    cos_pitch = cos(pitch);
    sin_pitch = sin(pitch);
  
    // Tilt compensated magnetic field X
    mag_x = Magn_Vector[0]*cos_pitch + Magn_Vector[1]*sin_roll*sin_pitch + Magn_Vector[2]*cos_roll*sin_pitch;
    // Tilt compensated magnetic field Y
    mag_y = Magn_Vector[1]*cos_roll - Magn_Vector[2]*sin_roll;
    // Magnetic Heading
    MAG_Heading = atan2(-mag_y, mag_x);
}