File content as of revision 8:195d53710158:

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


#define MAG_SKIP 1


//#include "mbed.h"
//Serial p(USBTX, USBRX); // tx, rx
//
//void DCM::pv(float a[3])
//{
//    p.printf("%g,\t%g,\t%g \n\r",a[0],a[1],a[2]);
//}
//
//void DCM::pm(float a[3][3])
//{
//    p.printf("\t%g,\t%g,\t%g \n\r",a[0][0],a[0][1],a[0][2]);
//    p.printf("\t%g,\t%g,\t%g \n\r",a[1][0],a[1][1],a[1][2]);
//    p.printf("\t%g,\t%g,\t%g \n\r",a[2][0],a[2][1],a[2][2]);
//}

DCM::DCM(void):
    G_Dt(0.02), 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
        }
    reset_sensor_fusion();
    }
}


/**************************************************/
void DCM::Normalize(void)
{
//    p.printf("NORMLALIZE:----\n\r");
    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);
//    p.printf("dcm: \n\r");pm(dcm);
    
}

/**************************************************/
void DCM::Drift_correction(void)
{
//    p.printf("DRIFT CORRECTION:----\n\r");
  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);  //  
  
//  p.printf("Accel_W: %g", Accel_weight);
  Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&dcm[2][0]); //adjust the ground of reference
//  p.printf("errorRollPitch: ");pv(errorRollPitch);
  Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
//  p.printf("Omega_P: ");pv(Omega_P);
  
  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

//    p.printf("dcm: \n\r");pm(dcm);
}


/**************************************************/
void DCM::Matrix_update(void)
{
//    p.printf("MATRIX UPDATE:----\n\r");
//    p.printf("Omega: ");pv(Omega);
    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
//    p.printf("Omega_Vector: ");pv(Omega_Vector);
    #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;
//    p.printf("Update: \n\r");pm(Update_Matrix);
    #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
    
 //   p.printf("Temp: \n\r");pm(Temporary_Matrix);
//    p.printf("Update: \n\r");pm(Update_Matrix);
    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];
        } 
    }
//    p.printf("dcm: \n\r");pm(dcm);
}

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(float dt)
{
    G_Dt = dt;
    Read_Accel(Accel_Vector);         
    Read_Gyro(Gyro_Vector);
    if (--update_count == 0) {
        Update_mag();
        update_count = MAG_SKIP;
    }
    Matrix_update();
    Normalize();
    Drift_correction();
    Euler_angles();
}

void DCM::Update_mag()
{
    Read_Magn(Magn_Vector);
    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);
}

void DCM::reset_sensor_fusion()
{
    float temp1[3];
    float temp2[3];
    float xAxis[] = {1.0f, 0.0f, 0.0f};
    
    Read_Accel(Accel_Vector);         
    Read_Gyro(Gyro_Vector);
    
    //  timestamp = timer.read_ms();
    
    // GET PITCH
    // Using y-z-plane-component/x-component of gravity vector
    pitch = -atan2(Accel_Vector[0], sqrt(Accel_Vector[1] * Accel_Vector[1] + Accel_Vector[2] * Accel_Vector[2]));
    
    // GET ROLL
    // Compensate pitch of gravity vector 
    Vector_Cross_Product(temp1, Accel_Vector, xAxis);
    Vector_Cross_Product(temp2, xAxis, temp1);
    // Normally using x-z-plane-component/y-component of compensated gravity vector
    // roll = atan2(temp2[1], sqrt(temp2[0] * temp2[0] + temp2[2] * temp2[2]));
    // Since we compensated for pitch, x-z-plane-component equals z-component:
    roll = atan2(temp2[1], temp2[2]);
    
    // GET YAW
    Update_mag();
    yaw = MAG_Heading;
    
    // Init rotation matrix
    init_rotation_matrix(dcm, yaw, pitch, roll);
}

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

    return result;  
}