DCM.cpp

00001 /* This file is part of the Razor AHRS Firmware */
00002 #include "Razor_AHRS.h"
00003 #include <math.h>
00004 
00005 // DCM algorithm
00006 
00007 /**************************************************/
00008 void IMU::Normalize(void)
00009 {
00010   float error=0;
00011   float temporary[3][3];
00012   float renorm=0;
00013   
00014   error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19
00015 
00016   Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19
00017   Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19
00018   
00019   Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]);//eq.19
00020   Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]);//eq.19
00021   
00022   Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20
00023   
00024   renorm= .5 *(3 - Vector_Dot_Product(&temporary[0][0],&temporary[0][0])); //eq.21
00025   Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
00026   
00027   renorm= .5 *(3 - Vector_Dot_Product(&temporary[1][0],&temporary[1][0])); //eq.21
00028   Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
00029   
00030   renorm= .5 *(3 - Vector_Dot_Product(&temporary[2][0],&temporary[2][0])); //eq.21
00031   Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
00032 }
00033 
00034 /**************************************************/
00035 void IMU::Drift_correction(void)
00036 {
00037   float mag_heading_x;
00038   float mag_heading_y;
00039   float errorCourse;
00040   //Compensation the Roll, Pitch and Yaw drift. 
00041   static float Scaled_Omega_P[3];
00042   static float Scaled_Omega_I[3];
00043   float Accel_magnitude;
00044   float Accel_weight;
00045   
00046   
00047   //*****Roll and Pitch***************
00048 
00049   // Calculate the magnitude of the accelerometer vector
00050   Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]);
00051   Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
00052   // Dynamic weighting of accelerometer info (reliability filter)
00053   // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
00054   Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1);  //  
00055 
00056   Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference
00057   Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
00058   
00059   Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
00060   Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);     
00061   
00062   //*****YAW***************
00063   // We make the gyro YAW drift correction based on compass magnetic heading
00064  
00065   mag_heading_x = cos(MAG_Heading);
00066   mag_heading_y = sin(MAG_Heading);
00067   errorCourse=(DCM_Matrix[0][0]*mag_heading_y) - (DCM_Matrix[1][0]*mag_heading_x);  //Calculating YAW error
00068   Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
00069   
00070   Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);//.01proportional of YAW.
00071   Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding  Proportional.
00072   
00073   Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);//.00001Integrator
00074   Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
00075 }
00076 
00077 void IMU::Matrix_update(void)
00078 {
00079   Gyro_Vector[0]=GYRO_SCALED_RAD(gyro[0]); //gyro x roll
00080   Gyro_Vector[1]=GYRO_SCALED_RAD(gyro[1]); //gyro y pitch
00081   Gyro_Vector[2]=GYRO_SCALED_RAD(gyro[2]); //gyro z yaw
00082   
00083   Accel_Vector[0]=accel[0];
00084   Accel_Vector[1]=accel[1];
00085   Accel_Vector[2]=accel[2];
00086     
00087   Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]);  //adding proportional term
00088   Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
00089   
00090 #if DEBUG__NO_DRIFT_CORRECTION == true // Do not use drift correction
00091   Update_Matrix[0][0]=0;
00092   Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z
00093   Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y
00094   Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z
00095   Update_Matrix[1][1]=0;
00096   Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0];
00097   Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1];
00098   Update_Matrix[2][1]=G_Dt*Gyro_Vector[0];
00099   Update_Matrix[2][2]=0;
00100 #else // Use drift correction
00101   Update_Matrix[0][0]=0;
00102   Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
00103   Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
00104   Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
00105   Update_Matrix[1][1]=0;
00106   Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x
00107   Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
00108   Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
00109   Update_Matrix[2][2]=0;
00110 #endif
00111 
00112   Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c
00113 
00114   for(int x=0; x<3; x++) //Matrix Addition (update)
00115   {
00116     for(int y=0; y<3; y++)
00117     {
00118       DCM_Matrix[x][y]+=Temporary_Matrix[x][y];
00119     } 
00120   }
00121 }
00122 
00123 void IMU::Euler_angles(void)
00124 {
00125   pitch = -asin(DCM_Matrix[2][0]);
00126   roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
00127   yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
00128 }
00129