Port of http://dev.qu.tu-berlin.de/projects/sf-razor-9dof-ahrs to an mbed, tested with a 9DOF Sensor Stick, SEN-10724
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
Generated on Sun Jul 17 2022 22:09:51 by 1.7.2