File content as of revision 0:2b4ec92bff8b:

#include "DCM.h"

DCM::DCM(float dt)
{
    for(int i = 0; i < 3; i++)
    {
        Accel_Vector[i] = 0; // Store the acceleration in a vector
        Gyro_Vector[i] = 0; // Store the gyros turn rate in a vector
        Omega_Vector[i] = 0; // Corrected Gyro_Vector data
        Omega_P[i] = 0; // Omega Proportional correction
        Omega_I[i] = 0; // Omega Integrator
        Omega[i]= 0;
        errorRollPitch[i] = 0;
        errorYaw[i] = 0;
    }
    
    int k = 1;
    
    for(int i = 0; i < 3; i++)
    {
        for(int j = 0; j < 3; j++)
        {
            DCM_Matrix[i][j] = 0;
            Update_Matrix[i][j] = k;
            Temporary_Matrix[i][j] = 0;
            k++;
        }
        k++;
    }
    
    G_Dt = dt;    
    
    reset_sensor_fusion();    
}

void DCM::FilterInit(int rawAccX, int rawAccY, int rawAccZ, int rawGyroX, int rawGyroY, int rawGyroZ, int rawMagX, int rawMagY, int rawMagZ)
{
    setAccelRawX(rawAccX);
    setAccelRawY(rawAccY);
    setAccelRawZ(rawAccZ);
    
    setGyroRawX(rawGyroX);
    setGyroRawY(rawGyroY);
    setGyroRawZ(rawGyroZ);
    
    setMagnetRawX(rawMagX);
    setMagnetRawY(rawMagY);
    setMagnetRawZ(rawMagZ);
    
    reset_sensor_fusion();
}

void DCM::init_rotation_matrix(float m[3][3], float yaw, float pitch, float roll)
{
  float c1 = cos(roll);
  float s1 = sin(roll);
  float c2 = cos(pitch);
  float s2 = sin(pitch);
  float c3 = cos(yaw);
  float s3 = sin(yaw);

  // Euler angles, right-handed, intrinsic, XYZ convention
  // (which means: rotate around body axes Z, Y', X'') 
  m[0][0] = c2 * c3;
  m[0][1] = c3 * s1 * s2 - c1 * s3;
  m[0][2] = s1 * s3 + c1 * c3 * s2;

  m[1][0] = c2 * s3;
  m[1][1] = c1 * c3 + s1 * s2 * s3;
  m[1][2] = c1 * s2 * s3 - c3 * s1;

  m[2][0] = -s2;
  m[2][1] = c2 * s1;
  m[2][2] = c1 * c2;
}

void DCM::ReadMagnetometer()
{
    rawMagnet[0] = ((int16_t)magnetRaw[0]);
    rawMagnet[1] = ((int16_t)magnetRaw[1]);
    rawMagnet[2] = ((int16_t)magnetRaw[2]);
    
    magnet[0] = ((float)(rawMagnet[0] - MAGN_X_OFFSET) * MAGN_X_SCALE);
    magnet[1] = ((float)(rawMagnet[1] - MAGN_Y_OFFSET) * MAGN_Y_SCALE);
    magnet[2] = ((float)(rawMagnet[2] - MAGN_Z_OFFSET) * MAGN_Z_SCALE);
}

void DCM::ReadAccelerometer()
{
    accel[0] = (((int16_t)accelRaw[0]) - ACCEL_X_OFFSET) * ACCEL_X_SCALE;
    accel[1] = (((int16_t)accelRaw[1]) - ACCEL_Y_OFFSET) * ACCEL_Y_SCALE;
    accel[2] = (((int16_t)accelRaw[2]) - ACCEL_Z_OFFSET) * ACCEL_Z_SCALE;
}

void DCM::ReadGyroscope()
{
    gyro[0] = (gyroRaw[0] - GYRO_BIAS_X);
    gyro[1] = (gyroRaw[1] - GYRO_BIAS_Y);   
    gyro[2] = (gyroRaw[2] - GYRO_BIAS_Z);       
}

float DCM::constrain(float in, float min, float max)
{
    in = in > max ? max : in;
    in = in < min ? min : in;
    return in;
}

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 = magnet[0]*cos_pitch + magnet[1]*sin_roll*sin_pitch + magnet[2]*cos_roll*sin_pitch;
  // Tilt compensated magnetic field Y
  mag_y = magnet[1]*cos_roll - magnet[2]*sin_roll;
  // Magnetic Heading
  MAG_Heading = atan2(-mag_y, mag_x);
}

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

  Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19
  Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19
  
  Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]);//eq.19
  Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[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_Matrix[0][0], &temporary[0][0], renorm);
  
  renorm= .5 *(3 - Vector_Dot_Product(&temporary[1][0],&temporary[1][0])); //eq.21
  Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
  
  renorm= .5 *(3 - Vector_Dot_Product(&temporary[2][0],&temporary[2][0])); //eq.21
  Vector_Scale(&DCM_Matrix[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_Matrix[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_Matrix[0][0]*mag_heading_y) - (DCM_Matrix[1][0]*mag_heading_x);  //Calculating YAW error
  Vector_Scale(errorYaw,&DCM_Matrix[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)
{
  Gyro_Vector[0]=GYRO_SCALED_RAD(gyro[0]); //gyro x roll
  Gyro_Vector[1]=GYRO_SCALED_RAD(gyro[1]); //gyro y pitch
  Gyro_Vector[2]=GYRO_SCALED_RAD(gyro[2]); //gyro z yaw
  
  Accel_Vector[0]=accel[0];
  Accel_Vector[1]=accel[1];
  Accel_Vector[2]=accel[2];
    
  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
  
  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];
  Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];
  Update_Matrix[2][1]=G_Dt*Omega_Vector[0];
  Update_Matrix[2][2]=0;

  Matrix_Multiply(Temporary_Matrix,DCM_Matrix,Update_Matrix); //a*b=c

  for(int x=0; x<3; x++) //Matrix Addition (update)
  {
    for(int y=0; y<3; y++)
    {
      DCM_Matrix[x][y]+=Temporary_Matrix[x][y];
    } 
  }
}

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

void DCM::Sensors()
{
    ReadAccelerometer();
    ReadGyroscope();
    ReadMagnetometer();
    Compass_Heading();    
}

void DCM::UpdateFilter()
{
    Sensors();
    Matrix_update();
    Normalize();
    Drift_correction(); 
    Euler_angles();    
}

void DCM::reset_sensor_fusion() {
  float temp1[3];
  float temp2[3];
  float xAxis[] = {1.0f, 0.0f, 0.0f};

  Sensors();
    
  // GET PITCH
  // Using y-z-plane-component/x-component of gravity vector
  pitch = -atan2(accel[0], sqrt(accel[1] * accel[1] + accel[2] * accel[2]));
    
  Vector_Cross_Product(temp1, accel, xAxis);
  Vector_Cross_Product(temp2, xAxis, temp1);
  roll = atan2(temp2[1], temp2[2]);
  
  // GET YAW
  Compass_Heading();
  yaw = MAG_Heading;
  
  // Init rotation matrix
  init_rotation_matrix(DCM_Matrix, yaw, pitch, roll);
}

float DCM::getYaw()
{
    return TO_DEG(yaw);
}

float DCM::getPitch()
{
    return TO_DEG(pitch);
}

float DCM::getRoll()
{
    return TO_DEG(roll);
}

void DCM::setAccelRawX(int accelRawX)
{
    accelRaw[0] = (uint16_t)accelRawX;    
}

void DCM::setAccelRawY(int accelRawY)
{
    accelRaw[1] = (uint16_t)accelRawY;    
}

void DCM::setAccelRawZ(int accelRawZ)
{
    accelRaw[2] = (uint16_t)accelRawZ;    
}

void DCM::setGyroRawX(int gyroRawX)
{
    gyroRaw[0] = gyroRawX;    
}

void DCM::setGyroRawY(int gyroRawY)
{
    gyroRaw[1] = gyroRawY;    
}

void DCM::setGyroRawZ(int gyroRawZ)
{
    gyroRaw[2] = gyroRawZ;    
}

void DCM::setMagnetRawX(int magnetRawX)
{
    magnetRaw[0] = (uint16_t)magnetRawX;    
}

void DCM::setMagnetRawY(int magnetRawY)
{
    magnetRaw[1] = (uint16_t)magnetRawY;    
}

void DCM::setMagnetRawZ(int magnetRawZ)
{
    magnetRaw[2] = (uint16_t)magnetRawZ;    
}

float DCM::getTestVariable()
{
    return MAG_Heading;    
}