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@3:ef027800a8d5, 2013-07-24 (annotated)

Committer:
oprospero
Date:
Wed Jul 24 02:52:28 2013 +0000
Revision:
3:ef027800a8d5
Parent:
2:486018bc6acd
Child:
4:4c6ffd1a88ce
update sensors

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 #include "mbed.h"
oprospero 1:115cf0a84a9d 7 //Serial pc3(USBTX, USBRX);
oprospero 1:115cf0a84a9d 8
oprospero 1:115cf0a84a9d 9 #define MAG_SKIP 1
shimniok 0:62284d27d75e 10
shimniok 0:62284d27d75e 11 float DCM::constrain(float x, float a, float b)
shimniok 0:62284d27d75e 12 {
shimniok 0:62284d27d75e 13 float result = x;
shimniok 0:62284d27d75e 14
shimniok 0:62284d27d75e 15 if (x < a) result = a;
shimniok 0:62284d27d75e 16 if (x > b) result = b;
shimniok 0:62284d27d75e 17
shimniok 0:62284d27d75e 18 return result;
shimniok 0:62284d27d75e 19 }
shimniok 0:62284d27d75e 20
shimniok 0:62284d27d75e 21
shimniok 0:62284d27d75e 22 DCM::DCM(void):
oprospero 2:486018bc6acd 23 G_Dt(0.02), update_count(MAG_SKIP)
shimniok 0:62284d27d75e 24 {
oprospero 1:115cf0a84a9d 25 Accel_Init();
oprospero 1:115cf0a84a9d 26 Gyro_Init();
oprospero 1:115cf0a84a9d 27 Magn_Init();
shimniok 0:62284d27d75e 28 for (int m=0; m < 3; m++) {
shimniok 0:62284d27d75e 29 Accel_Vector[m] = 0;
shimniok 0:62284d27d75e 30 Gyro_Vector[m] = 0;
shimniok 0:62284d27d75e 31 Omega_Vector[m] = 0;
shimniok 0:62284d27d75e 32 Omega_P[m] = 0;
shimniok 0:62284d27d75e 33 Omega_I[m] = 0;
shimniok 0:62284d27d75e 34 Omega[m] = 0;
shimniok 0:62284d27d75e 35 errorRollPitch[m] = 0;
shimniok 0:62284d27d75e 36 errorYaw[m] = 0;
shimniok 0:62284d27d75e 37 for (int n=0; n < 3; n++) {
shimniok 0:62284d27d75e 38 dcm[m][n] = (m == n) ? 1 : 0; // dcm starts as identity matrix
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 /**************************************************/
shimniok 0:62284d27d75e 46 void DCM::Normalize(void)
shimniok 0:62284d27d75e 47 {
shimniok 0:62284d27d75e 48 float error=0;
shimniok 0:62284d27d75e 49 float temporary[3][3];
shimniok 0:62284d27d75e 50 float renorm=0;
shimniok 0:62284d27d75e 51
oprospero 1:115cf0a84a9d 52 error= -Vector_Dot_Product(&dcm[0][0], &dcm[1][0])*.5; //eq.19
shimniok 0:62284d27d75e 53
shimniok 0:62284d27d75e 54 Vector_Scale(&temporary[0][0], &dcm[1][0], error); //eq.19
shimniok 0:62284d27d75e 55 Vector_Scale(&temporary[1][0], &dcm[0][0], error); //eq.19
oprospero 1:115cf0a84a9d 56
shimniok 0:62284d27d75e 57 Vector_Add(&temporary[0][0], &temporary[0][0], &dcm[0][0]);//eq.19
shimniok 0:62284d27d75e 58 Vector_Add(&temporary[1][0], &temporary[1][0], &dcm[1][0]);//eq.19
shimniok 0:62284d27d75e 59
shimniok 0:62284d27d75e 60 Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20
shimniok 0:62284d27d75e 61
shimniok 0:62284d27d75e 62 renorm= .5 *(3 - Vector_Dot_Product(&temporary[0][0],&temporary[0][0])); //eq.21
shimniok 0:62284d27d75e 63 Vector_Scale(&dcm[0][0], &temporary[0][0], renorm);
shimniok 0:62284d27d75e 64
shimniok 0:62284d27d75e 65 renorm= .5 *(3 - Vector_Dot_Product(&temporary[1][0],&temporary[1][0])); //eq.21
shimniok 0:62284d27d75e 66 Vector_Scale(&dcm[1][0], &temporary[1][0], renorm);
shimniok 0:62284d27d75e 67
shimniok 0:62284d27d75e 68 renorm= .5 *(3 - Vector_Dot_Product(&temporary[2][0],&temporary[2][0])); //eq.21
shimniok 0:62284d27d75e 69 Vector_Scale(&dcm[2][0], &temporary[2][0], renorm);
shimniok 0:62284d27d75e 70 }
shimniok 0:62284d27d75e 71
shimniok 0:62284d27d75e 72 /**************************************************/
shimniok 0:62284d27d75e 73 void DCM::Drift_correction(void)
shimniok 0:62284d27d75e 74 {
shimniok 0:62284d27d75e 75 float mag_heading_x;
shimniok 0:62284d27d75e 76 float mag_heading_y;
shimniok 0:62284d27d75e 77 float errorCourse;
shimniok 0:62284d27d75e 78 //Compensation the Roll, Pitch and Yaw drift.
shimniok 0:62284d27d75e 79 static float Scaled_Omega_P[3];
shimniok 0:62284d27d75e 80 static float Scaled_Omega_I[3];
shimniok 0:62284d27d75e 81 float Accel_magnitude;
shimniok 0:62284d27d75e 82 float Accel_weight;
shimniok 0:62284d27d75e 83
shimniok 0:62284d27d75e 84
shimniok 0:62284d27d75e 85 //*****Roll and Pitch***************
shimniok 0:62284d27d75e 86
shimniok 0:62284d27d75e 87 // Calculate the magnitude of the accelerometer vector
shimniok 0:62284d27d75e 88 Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]);
shimniok 0:62284d27d75e 89 Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
shimniok 0:62284d27d75e 90 // Dynamic weighting of accelerometer info (reliability filter)
shimniok 0:62284d27d75e 91 // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
shimniok 0:62284d27d75e 92 Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1); //
oprospero 1:115cf0a84a9d 93
shimniok 0:62284d27d75e 94 Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&dcm[2][0]); //adjust the ground of reference
shimniok 0:62284d27d75e 95 Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
shimniok 0:62284d27d75e 96
shimniok 0:62284d27d75e 97 Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
oprospero 1:115cf0a84a9d 98 Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);
shimniok 0:62284d27d75e 99
shimniok 0:62284d27d75e 100 //*****YAW***************
shimniok 0:62284d27d75e 101 // We make the gyro YAW drift correction based on compass magnetic heading
oprospero 1:115cf0a84a9d 102 mag_heading_x = cos(MAG_Heading);
oprospero 1:115cf0a84a9d 103 mag_heading_y = sin(MAG_Heading);
oprospero 1:115cf0a84a9d 104 errorCourse=(dcm[0][0]*mag_heading_y) - (dcm[1][0]*mag_heading_x); //Calculating YAW error
oprospero 1:115cf0a84a9d 105 Vector_Scale(errorYaw,&dcm[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
oprospero 1:115cf0a84a9d 106
oprospero 1:115cf0a84a9d 107 Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);//.01proportional of YAW.
oprospero 1:115cf0a84a9d 108 Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
oprospero 1:115cf0a84a9d 109
oprospero 1:115cf0a84a9d 110 Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);//.00001Integrator
oprospero 1:115cf0a84a9d 111 Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
shimniok 0:62284d27d75e 112
shimniok 0:62284d27d75e 113 }
shimniok 0:62284d27d75e 114
shimniok 0:62284d27d75e 115
shimniok 0:62284d27d75e 116 /**************************************************/
shimniok 0:62284d27d75e 117 void DCM::Matrix_update(void)
shimniok 0:62284d27d75e 118 {
shimniok 0:62284d27d75e 119
oprospero 1:115cf0a84a9d 120 Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]); //adding proportional term
oprospero 1:115cf0a84a9d 121 Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
shimniok 0:62284d27d75e 122
oprospero 1:115cf0a84a9d 123 #if OUTPUTMODE==1
oprospero 1:115cf0a84a9d 124 Update_Matrix[0][0]=0;
oprospero 1:115cf0a84a9d 125 Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
oprospero 1:115cf0a84a9d 126 Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
oprospero 1:115cf0a84a9d 127 Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
oprospero 1:115cf0a84a9d 128 Update_Matrix[1][1]=0;
oprospero 1:115cf0a84a9d 129 Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x
oprospero 1:115cf0a84a9d 130 Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
oprospero 1:115cf0a84a9d 131 Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
oprospero 1:115cf0a84a9d 132 Update_Matrix[2][2]=0;
oprospero 1:115cf0a84a9d 133 #else // Uncorrected data (no drift correction)
oprospero 1:115cf0a84a9d 134 Update_Matrix[0][0]=0;
oprospero 1:115cf0a84a9d 135 Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z
oprospero 1:115cf0a84a9d 136 Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y
oprospero 1:115cf0a84a9d 137 Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z
oprospero 1:115cf0a84a9d 138 Update_Matrix[1][1]=0;
oprospero 1:115cf0a84a9d 139 Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0];
oprospero 1:115cf0a84a9d 140 Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1];
oprospero 1:115cf0a84a9d 141 Update_Matrix[2][1]=G_Dt*Gyro_Vector[0];
oprospero 1:115cf0a84a9d 142 Update_Matrix[2][2]=0;
oprospero 1:115cf0a84a9d 143 #endif
oprospero 1:115cf0a84a9d 144
oprospero 1:115cf0a84a9d 145 Matrix_Multiply(Temporary_Matrix, dcm, Update_Matrix); //c=a*b
oprospero 1:115cf0a84a9d 146 // ??? Matrix_Add(dcm, dcm, Temporary_Matrix); ???
oprospero 1:115cf0a84a9d 147 for(int x=0; x<3; x++) { //Matrix Addition (update)
oprospero 1:115cf0a84a9d 148 for(int y=0; y<3; y++) {
oprospero 1:115cf0a84a9d 149 dcm[x][y] += Temporary_Matrix[x][y];
oprospero 1:115cf0a84a9d 150 }
oprospero 1:115cf0a84a9d 151 }
shimniok 0:62284d27d75e 152 }
shimniok 0:62284d27d75e 153
shimniok 0:62284d27d75e 154 void DCM::Euler_angles(void)
shimniok 0:62284d27d75e 155 {
oprospero 1:115cf0a84a9d 156 pitch = RAD2DEG(-asin(dcm[2][0]));
oprospero 1:115cf0a84a9d 157 roll = RAD2DEG(atan2(dcm[2][1],dcm[2][2]));
oprospero 1:115cf0a84a9d 158 yaw = RAD2DEG(atan2(dcm[1][0],dcm[0][0]));
shimniok 0:62284d27d75e 159 }
shimniok 0:62284d27d75e 160
shimniok 0:62284d27d75e 161 void DCM::Update_step()
oprospero 1:115cf0a84a9d 162 {
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