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@5:d5ecdb82c17b, 2013-07-26 (annotated)

Committer:
oprospero
Date:
Fri Jul 26 04:02:53 2013 +0000
Revision:
5:d5ecdb82c17b
Parent:
4:4c6ffd1a88ce
Child:
6:49cbf2acc4e6
clean up

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