Prosper Van / DCM_AHRS_GY80

AHRS based on MatrixPilot DCM algorithm; ported from Pololu MinIMU-9 example code in turn based on ArduPilot 1.5 built for sensor gy_80

Dependents:   quadCommand2

Fork of DCM_AHRS by Michael Shimniok

DCM.cpp@4:4c6ffd1a88ce, 2013-07-26 (annotated)

Committer:
oprospero
Date:
Fri Jul 26 02:42:14 2013 +0000
Revision:
4:4c6ffd1a88ce
Parent:
3:ef027800a8d5
Child:
5:d5ecdb82c17b
7/25 update;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimniok 0:62284d27d75e 1 #include "DCM.h"
shimniok 0:62284d27d75e 2 #include "Matrix.h"
shimniok 0:62284d27d75e 3 #include "math.h"
shimniok 0:62284d27d75e 4 #include "Sensors.h"
shimniok 0:62284d27d75e 5
oprospero 1:115cf0a84a9d 6
oprospero 1:115cf0a84a9d 7 #define MAG_SKIP 1
shimniok 0:62284d27d75e 8
shimniok 0:62284d27d75e 9 float DCM::constrain(float x, float a, float b)
shimniok 0:62284d27d75e 10 {
shimniok 0:62284d27d75e 11 float result = x;
shimniok 0:62284d27d75e 12
shimniok 0:62284d27d75e 13 if (x < a) result = a;
shimniok 0:62284d27d75e 14 if (x > b) result = b;
shimniok 0:62284d27d75e 15
shimniok 0:62284d27d75e 16 return result;
shimniok 0:62284d27d75e 17 }
shimniok 0:62284d27d75e 18
shimniok 0:62284d27d75e 19
shimniok 0:62284d27d75e 20 DCM::DCM(void):
oprospero 2:486018bc6acd 21 G_Dt(0.02), update_count(MAG_SKIP)
shimniok 0:62284d27d75e 22 {
oprospero 1:115cf0a84a9d 23 Accel_Init();
oprospero 1:115cf0a84a9d 24 Gyro_Init();
oprospero 1:115cf0a84a9d 25 Magn_Init();
shimniok 0:62284d27d75e 26 for (int m=0; m < 3; m++) {
shimniok 0:62284d27d75e 27 Accel_Vector[m] = 0;
shimniok 0:62284d27d75e 28 Gyro_Vector[m] = 0;
shimniok 0:62284d27d75e 29 Omega_Vector[m] = 0;
shimniok 0:62284d27d75e 30 Omega_P[m] = 0;
shimniok 0:62284d27d75e 31 Omega_I[m] = 0;
shimniok 0:62284d27d75e 32 Omega[m] = 0;
shimniok 0:62284d27d75e 33 errorRollPitch[m] = 0;
shimniok 0:62284d27d75e 34 errorYaw[m] = 0;
shimniok 0:62284d27d75e 35 for (int n=0; n < 3; n++) {
shimniok 0:62284d27d75e 36 dcm[m][n] = (m == n) ? 1 : 0; // dcm starts as identity matrix
shimniok 0:62284d27d75e 37 }
shimniok 0:62284d27d75e 38 }
shimniok 0:62284d27d75e 39
shimniok 0:62284d27d75e 40 }
shimniok 0:62284d27d75e 41
shimniok 0:62284d27d75e 42
shimniok 0:62284d27d75e 43 /**************************************************/
shimniok 0:62284d27d75e 44 void DCM::Normalize(void)
shimniok 0:62284d27d75e 45 {
shimniok 0:62284d27d75e 46 float error=0;
shimniok 0:62284d27d75e 47 float temporary[3][3];
shimniok 0:62284d27d75e 48 float renorm=0;
shimniok 0:62284d27d75e 49
oprospero 1:115cf0a84a9d 50 error= -Vector_Dot_Product(&dcm[0][0], &dcm[1][0])*.5; //eq.19
shimniok 0:62284d27d75e 51
shimniok 0:62284d27d75e 52 Vector_Scale(&temporary[0][0], &dcm[1][0], error); //eq.19
shimniok 0:62284d27d75e 53 Vector_Scale(&temporary[1][0], &dcm[0][0], error); //eq.19
oprospero 1:115cf0a84a9d 54
shimniok 0:62284d27d75e 55 Vector_Add(&temporary[0][0], &temporary[0][0], &dcm[0][0]);//eq.19
shimniok 0:62284d27d75e 56 Vector_Add(&temporary[1][0], &temporary[1][0], &dcm[1][0]);//eq.19
shimniok 0:62284d27d75e 57
shimniok 0:62284d27d75e 58 Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20
shimniok 0:62284d27d75e 59
shimniok 0:62284d27d75e 60 renorm= .5 *(3 - Vector_Dot_Product(&temporary[0][0],&temporary[0][0])); //eq.21
shimniok 0:62284d27d75e 61 Vector_Scale(&dcm[0][0], &temporary[0][0], renorm);
shimniok 0:62284d27d75e 62
shimniok 0:62284d27d75e 63 renorm= .5 *(3 - Vector_Dot_Product(&temporary[1][0],&temporary[1][0])); //eq.21
shimniok 0:62284d27d75e 64 Vector_Scale(&dcm[1][0], &temporary[1][0], renorm);
shimniok 0:62284d27d75e 65
shimniok 0:62284d27d75e 66 renorm= .5 *(3 - Vector_Dot_Product(&temporary[2][0],&temporary[2][0])); //eq.21
shimniok 0:62284d27d75e 67 Vector_Scale(&dcm[2][0], &temporary[2][0], renorm);
shimniok 0:62284d27d75e 68 }
shimniok 0:62284d27d75e 69
shimniok 0:62284d27d75e 70 /**************************************************/
shimniok 0:62284d27d75e 71 void DCM::Drift_correction(void)
shimniok 0:62284d27d75e 72 {
shimniok 0:62284d27d75e 73 float mag_heading_x;
shimniok 0:62284d27d75e 74 float mag_heading_y;
shimniok 0:62284d27d75e 75 float errorCourse;
shimniok 0:62284d27d75e 76 //Compensation the Roll, Pitch and Yaw drift.
shimniok 0:62284d27d75e 77 static float Scaled_Omega_P[3];
shimniok 0:62284d27d75e 78 static float Scaled_Omega_I[3];
shimniok 0:62284d27d75e 79 float Accel_magnitude;
shimniok 0:62284d27d75e 80 float Accel_weight;
shimniok 0:62284d27d75e 81
shimniok 0:62284d27d75e 82
shimniok 0:62284d27d75e 83 //*****Roll and Pitch***************
shimniok 0:62284d27d75e 84
shimniok 0:62284d27d75e 85 // Calculate the magnitude of the accelerometer vector
shimniok 0:62284d27d75e 86 Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]);
shimniok 0:62284d27d75e 87 Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
shimniok 0:62284d27d75e 88 // Dynamic weighting of accelerometer info (reliability filter)
shimniok 0:62284d27d75e 89 // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
shimniok 0:62284d27d75e 90 Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1); //
oprospero 1:115cf0a84a9d 91
shimniok 0:62284d27d75e 92 Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&dcm[2][0]); //adjust the ground of reference
shimniok 0:62284d27d75e 93 Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
shimniok 0:62284d27d75e 94
shimniok 0:62284d27d75e 95 Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
oprospero 1:115cf0a84a9d 96 Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);
shimniok 0:62284d27d75e 97
shimniok 0:62284d27d75e 98 //*****YAW***************
shimniok 0:62284d27d75e 99 // We make the gyro YAW drift correction based on compass magnetic heading
oprospero 1:115cf0a84a9d 100 mag_heading_x = cos(MAG_Heading);
oprospero 1:115cf0a84a9d 101 mag_heading_y = sin(MAG_Heading);
oprospero 1:115cf0a84a9d 102 errorCourse=(dcm[0][0]*mag_heading_y) - (dcm[1][0]*mag_heading_x); //Calculating YAW error
oprospero 1:115cf0a84a9d 103 Vector_Scale(errorYaw,&dcm[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
oprospero 1:115cf0a84a9d 104
oprospero 1:115cf0a84a9d 105 Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);//.01proportional of YAW.
oprospero 1:115cf0a84a9d 106 Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
oprospero 1:115cf0a84a9d 107
oprospero 1:115cf0a84a9d 108 Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);//.00001Integrator
oprospero 1:115cf0a84a9d 109 Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
shimniok 0:62284d27d75e 110
shimniok 0:62284d27d75e 111 }
shimniok 0:62284d27d75e 112
shimniok 0:62284d27d75e 113
shimniok 0:62284d27d75e 114 /**************************************************/
shimniok 0:62284d27d75e 115 void DCM::Matrix_update(void)
shimniok 0:62284d27d75e 116 {
shimniok 0:62284d27d75e 117
oprospero 1:115cf0a84a9d 118 Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]); //adding proportional term
oprospero 1:115cf0a84a9d 119 Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
shimniok 0:62284d27d75e 120
oprospero 1:115cf0a84a9d 121 #if OUTPUTMODE==1
oprospero 1:115cf0a84a9d 122 Update_Matrix[0][0]=0;
oprospero 1:115cf0a84a9d 123 Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
oprospero 1:115cf0a84a9d 124 Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
oprospero 1:115cf0a84a9d 125 Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
oprospero 1:115cf0a84a9d 126 Update_Matrix[1][1]=0;
oprospero 1:115cf0a84a9d 127 Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x
oprospero 1:115cf0a84a9d 128 Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
oprospero 1:115cf0a84a9d 129 Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
oprospero 1:115cf0a84a9d 130 Update_Matrix[2][2]=0;
oprospero 1:115cf0a84a9d 131 #else // Uncorrected data (no drift correction)
oprospero 1:115cf0a84a9d 132 Update_Matrix[0][0]=0;
oprospero 1:115cf0a84a9d 133 Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z
oprospero 1:115cf0a84a9d 134 Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y
oprospero 1:115cf0a84a9d 135 Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z
oprospero 1:115cf0a84a9d 136 Update_Matrix[1][1]=0;
oprospero 1:115cf0a84a9d 137 Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0];
oprospero 1:115cf0a84a9d 138 Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1];
oprospero 1:115cf0a84a9d 139 Update_Matrix[2][1]=G_Dt*Gyro_Vector[0];
oprospero 1:115cf0a84a9d 140 Update_Matrix[2][2]=0;
oprospero 1:115cf0a84a9d 141 #endif
oprospero 1:115cf0a84a9d 142
oprospero 1:115cf0a84a9d 143 Matrix_Multiply(Temporary_Matrix, dcm, Update_Matrix); //c=a*b
oprospero 1:115cf0a84a9d 144 // ??? Matrix_Add(dcm, dcm, Temporary_Matrix); ???
oprospero 1:115cf0a84a9d 145 for(int x=0; x<3; x++) { //Matrix Addition (update)
oprospero 1:115cf0a84a9d 146 for(int y=0; y<3; y++) {
oprospero 1:115cf0a84a9d 147 dcm[x][y] += Temporary_Matrix[x][y];
oprospero 1:115cf0a84a9d 148 }
oprospero 1:115cf0a84a9d 149 }
shimniok 0:62284d27d75e 150 }
shimniok 0:62284d27d75e 151
shimniok 0:62284d27d75e 152 void DCM::Euler_angles(void)
shimniok 0:62284d27d75e 153 {
oprospero 1:115cf0a84a9d 154 pitch = RAD2DEG(-asin(dcm[2][0]));
oprospero 1:115cf0a84a9d 155 roll = RAD2DEG(atan2(dcm[2][1],dcm[2][2]));
oprospero 1:115cf0a84a9d 156 yaw = RAD2DEG(atan2(dcm[1][0],dcm[0][0]));
shimniok 0:62284d27d75e 157 }
shimniok 0:62284d27d75e 158
oprospero 4:4c6ffd1a88ce 159 void DCM::Update_step(float dt)
oprospero 1:115cf0a84a9d 160 {
oprospero 4:4c6ffd1a88ce 161 G_Dt = dt;
oprospero 3:ef027800a8d5 162 Read_Accel(Accel_Vector);
oprospero 3:ef027800a8d5 163 Read_Gyro(Gyro_Vector);
shimniok 0:62284d27d75e 164 if (--update_count == 0) {
shimniok 0:62284d27d75e 165 Update_mag();
shimniok 0:62284d27d75e 166 update_count = MAG_SKIP;
shimniok 0:62284d27d75e 167 }
shimniok 0:62284d27d75e 168 Matrix_update();
shimniok 0:62284d27d75e 169 Normalize();
shimniok 0:62284d27d75e 170 Drift_correction();
shimniok 0:62284d27d75e 171 Euler_angles();
shimniok 0:62284d27d75e 172 }
shimniok 0:62284d27d75e 173
shimniok 0:62284d27d75e 174 void DCM::Update_mag()
shimniok 0:62284d27d75e 175 {
oprospero 3:ef027800a8d5 176 Read_Magn(Magn_Vector);
shimniok 0:62284d27d75e 177 Compass_Heading();
shimniok 0:62284d27d75e 178 }
oprospero 1:115cf0a84a9d 179
oprospero 1:115cf0a84a9d 180 void DCM::Compass_Heading()
oprospero 1:115cf0a84a9d 181 {
oprospero 1:115cf0a84a9d 182 float mag_x;
oprospero 1:115cf0a84a9d 183 float mag_y;
oprospero 1:115cf0a84a9d 184 float cos_roll;
oprospero 1:115cf0a84a9d 185 float sin_roll;
oprospero 1:115cf0a84a9d 186 float cos_pitch;
oprospero 1:115cf0a84a9d 187 float sin_pitch;
oprospero 1:115cf0a84a9d 188
oprospero 1:115cf0a84a9d 189 cos_roll = cos(roll);
oprospero 1:115cf0a84a9d 190 sin_roll = sin(roll);
oprospero 1:115cf0a84a9d 191 cos_pitch = cos(pitch);
oprospero 1:115cf0a84a9d 192 sin_pitch = sin(pitch);
oprospero 1:115cf0a84a9d 193
oprospero 1:115cf0a84a9d 194 // Tilt compensated magnetic field X
oprospero 1:115cf0a84a9d 195 mag_x = Magn_Vector[0]*cos_pitch + Magn_Vector[1]*sin_roll*sin_pitch + Magn_Vector[2]*cos_roll*sin_pitch;
oprospero 1:115cf0a84a9d 196 // Tilt compensated magnetic field Y
oprospero 1:115cf0a84a9d 197 mag_y = Magn_Vector[1]*cos_roll - Magn_Vector[2]*sin_roll;
oprospero 1:115cf0a84a9d 198 // Magnetic Heading
oprospero 1:115cf0a84a9d 199 MAG_Heading = atan2(-mag_y, mag_x);
oprospero 1:115cf0a84a9d 200 }
oprospero 1:115cf0a84a9d 201