C:/Documents and Settings/gw/My Documents/Other projects/flightos/Libraries/DCM.h
Go to the documentation of this file.
00001 float DCM_Matrix[3][3]= { 00002 { 00003 1,0,0 } 00004 ,{ 00005 0,1,0 } 00006 ,{ 00007 0,0,1 } 00008 }; 00009 float Update_Matrix[3][3]={{0,1,2},{3,4,5},{6,7,8}}; //Gyros here 00010 00011 00012 float Temporary_Matrix[3][3]={ 00013 { 00014 0,0,0 } 00015 ,{ 00016 0,0,0 } 00017 ,{ 00018 0,0,0 } 00019 }; 00020 00021 float Omega_Vector[3]= {0,0,0}; //Corrected Gyro_Vector data 00022 float Omega_P[3]= {0,0,0};//Omega Proportional correction 00023 float Omega_I[3]= {0,0,0};//Omega Integrator 00024 float Omega[3]= {0,0,0}; 00025 00026 float errorRollPitch[3]= {0,0,0}; 00027 float errorYaw[3]= {0,0,0}; 00028 00029 //#define Kp_ROLLPITCH 0.02 00030 //#define Ki_ROLLPITCH 0.00002 00031 //#define Kp_ROLLPITCH 0.4 00032 //#define Ki_ROLLPITCH 0.0002 00033 #define Kp_YAW 1.2 00034 #define Ki_YAW 0.00002 00035 00036 #define OUTPUTMODE 1 00037 00038 void DCM_reference(void) 00039 { 00040 #if 0 00041 for(int ct=0; ct< MAX_AXES; ct++) 00042 { 00043 /* for(int ct2 = 0; ct2 < MAX_AXES; ct2++) 00044 { 00045 DCM_Matrix[ct][ct2] = 0; 00046 } 00047 DCM_Matrix[ct][ct] = 1;*/ 00048 Omega_P[ct] = 0; 00049 Omega_I[ct] = 0; 00050 Omega[ct] = 0; 00051 } 00052 /* DCM_Matrix[0][0]=0; 00053 DCM_Matrix[0][1]=Accel_Vector[2];//-z 00054 DCM_Matrix[0][2]=-Accel_Vector[1];//y 00055 DCM_Matrix[1][0]=-Accel_Vector[2];//z 00056 DCM_Matrix[1][1]=0; 00057 DCM_Matrix[1][2]=-Accel_Vector[0];//-x 00058 DCM_Matrix[2][0]=Accel_Vector[1];//-y 00059 DCM_Matrix[2][1]=Accel_Vector[0];//x 00060 DCM_Matrix[2][2]=0;*/ 00061 #endif 00062 } 00063 00064 /**************************************************/ 00065 void Normalize(void) 00066 { 00067 float error=0; 00068 float temporary[3][3]; 00069 float renorm=0; 00070 00071 error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19 00072 00073 Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19 00074 Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19 00075 00076 Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]);//eq.19 00077 Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]);//eq.19 00078 00079 Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20 00080 00081 renorm= .5 *(3 - Vector_Dot_Product(&temporary[0][0],&temporary[0][0])); //eq.21 00082 Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm); 00083 00084 renorm= .5 *(3 - Vector_Dot_Product(&temporary[1][0],&temporary[1][0])); //eq.21 00085 Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm); 00086 00087 renorm= .5 *(3 - Vector_Dot_Product(&temporary[2][0],&temporary[2][0])); //eq.21 00088 Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm); 00089 } 00090 00091 /**************************************************/ 00092 void Drift_correction(void) 00093 { 00094 float mag_heading_x; 00095 float mag_heading_y; 00096 float errorCourse; 00097 //Compensation the Roll, Pitch and Yaw drift. 00098 static float Scaled_Omega_P[3]; 00099 static float Scaled_Omega_I[3]; 00100 float Accel_magnitude; 00101 float Accel_weight; 00102 00103 00104 //*****Roll and Pitch*************** 00105 00106 // Calculate the magnitude of the accelerometer vector 00107 Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]); 00108 00109 //Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity. 00110 // Dynamic weighting of accelerometer info (reliability filter) 00111 // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0) 00112 // Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1); // 00113 // Weight for accelerometer info (<0.75G = 0.0, 1G = 1.0 , >1.25G = 0.0) 00114 Accel_weight = constrain(1 - 4*abs(1 - Accel_magnitude),0,1); // 00115 00116 float tmp[3]; 00117 tmp[0] = Accel_Vector[0]; 00118 tmp[1] = -Accel_Vector[1]; 00119 tmp[2] = -Accel_Vector[2]; 00120 00121 00122 Vector_Cross_Product(&errorRollPitch[0],&tmp[0],&DCM_Matrix[2][0]); //adjust the ground of reference 00123 Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH * Accel_weight); 00124 Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH * Accel_weight); 00125 Vector_Add(Omega_I,Omega_I,Scaled_Omega_I); 00126 00127 //*****YAW*************** 00128 // We make the gyro YAW drift correction based on compass magnetic heading 00129 00130 mag_heading_x = cos(MAG_Heading); 00131 mag_heading_y = sin(MAG_Heading); 00132 errorCourse=(DCM_Matrix[0][0]*mag_heading_y) - (DCM_Matrix[1][0]*mag_heading_x); //Calculating YAW error 00133 Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position. 00134 00135 Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);//.01proportional of YAW. 00136 Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional. 00137 00138 Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);//.00001Integrator 00139 Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I 00140 } 00141 /**************************************************/ 00142 /* 00143 void Accel_adjust(void) 00144 { 00145 Accel_Vector[1] += Accel_Scale(speed_3d*Omega[2]); // Centrifugal force on Acc_y = GPS_speed*GyroZ 00146 Accel_Vector[2] -= Accel_Scale(speed_3d*Omega[1]); // Centrifugal force on Acc_z = GPS_speed*GyroY 00147 } 00148 */ 00149 /**************************************************/ 00150 00151 void Matrix_update(void) 00152 { 00153 // Gyro_Vector[0]=Gyro_Scaled_X(read_adc(0)); //gyro x roll 00154 // Gyro_Vector[1]=Gyro_Scaled_Y(read_adc(1)); //gyro y pitch 00155 // Gyro_Vector[2]=Gyro_Scaled_Z(read_adc(2)); //gyro Z yaw 00156 00157 // Accel_Vector[0]=accel_x; 00158 // Accel_Vector[1]=accel_y; 00159 // Accel_Vector[2]=accel_z; 00160 00161 float tmp[3]; 00162 tmp[0] = Gyro_Vector[0]; 00163 tmp[1] = Gyro_Vector[1]; 00164 tmp[2] = Gyro_Vector[2]; 00165 00166 Vector_Add(&Omega[0], &tmp[0], &Omega_I[0]); //adding proportional term 00167 Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term 00168 00169 //Accel_adjust(); //Remove centrifugal acceleration. We are not using this function in this version - we have no speed measurement 00170 00171 #if OUTPUTMODE==1 00172 Update_Matrix[0][0]=0; 00173 Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z 00174 Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y 00175 Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z 00176 Update_Matrix[1][1]=0; 00177 Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x 00178 Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y 00179 Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x 00180 Update_Matrix[2][2]=0; 00181 #else // Uncorrected data (no drift correction) 00182 Update_Matrix[0][0]=0; 00183 Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z 00184 Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y 00185 Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z 00186 Update_Matrix[1][1]=0; 00187 Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0]; 00188 Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1]; 00189 Update_Matrix[2][1]=G_Dt*Gyro_Vector[0]; 00190 Update_Matrix[2][2]=0; 00191 #endif 00192 00193 Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c 00194 00195 for(int x=0; x<3; x++) //Matrix Addition (update) 00196 { 00197 for(int y=0; y<3; y++) 00198 { 00199 DCM_Matrix[x][y]+=Temporary_Matrix[x][y]; 00200 } 00201 } 00202 } 00203 00204 void Euler_angles(void) 00205 { 00206 pitch = -atan2(DCM_Matrix[2][0],DCM_Matrix[2][2]); 00207 roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]); 00208 yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]); 00209 00210 } 00211
