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@6:49cbf2acc4e6, 2013-07-26 (annotated)

Committer:
oprospero
Date:
Fri Jul 26 18:44:33 2013 +0000
Revision:
6:49cbf2acc4e6
Parent:
5:d5ecdb82c17b
Child:
7:d22702f7ce89
Added reset

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
oprospero 6:49cbf2acc4e6 9
oprospero 6:49cbf2acc4e6 10 #include "mbed.h"
oprospero 6:49cbf2acc4e6 11 Serial p(USBTX, USBRX); // tx, rx
oprospero 6:49cbf2acc4e6 12
oprospero 6:49cbf2acc4e6 13 void DCM::pv(float a[3])
shimniok 0:62284d27d75e 14 {
oprospero 6:49cbf2acc4e6 15 p.printf("%g,\t%g,\t%g \n\r",a[0],a[1],a[2]);
shimniok 0:62284d27d75e 16 }
shimniok 0:62284d27d75e 17
oprospero 6:49cbf2acc4e6 18 void DCM::pm(float a[3][3])
oprospero 6:49cbf2acc4e6 19 {
oprospero 6:49cbf2acc4e6 20 p.printf("\t%g,\t%g,\t%g \n\r",a[0][0],a[0][1],a[0][2]);
oprospero 6:49cbf2acc4e6 21 p.printf("\t%g,\t%g,\t%g \n\r",a[1][0],a[1][1],a[1][2]);
oprospero 6:49cbf2acc4e6 22 p.printf("\t%g,\t%g,\t%g \n\r",a[2][0],a[2][1],a[2][2]);
oprospero 6:49cbf2acc4e6 23 }
shimniok 0:62284d27d75e 24
shimniok 0:62284d27d75e 25 DCM::DCM(void):
oprospero 2:486018bc6acd 26 G_Dt(0.02), update_count(MAG_SKIP)
shimniok 0:62284d27d75e 27 {
oprospero 1:115cf0a84a9d 28 Accel_Init();
oprospero 1:115cf0a84a9d 29 Gyro_Init();
oprospero 1:115cf0a84a9d 30 Magn_Init();
shimniok 0:62284d27d75e 31 for (int m=0; m < 3; m++) {
shimniok 0:62284d27d75e 32 Accel_Vector[m] = 0;
shimniok 0:62284d27d75e 33 Gyro_Vector[m] = 0;
shimniok 0:62284d27d75e 34 Omega_Vector[m] = 0;
shimniok 0:62284d27d75e 35 Omega_P[m] = 0;
shimniok 0:62284d27d75e 36 Omega_I[m] = 0;
shimniok 0:62284d27d75e 37 Omega[m] = 0;
shimniok 0:62284d27d75e 38 errorRollPitch[m] = 0;
shimniok 0:62284d27d75e 39 errorYaw[m] = 0;
shimniok 0:62284d27d75e 40 for (int n=0; n < 3; n++) {
shimniok 0:62284d27d75e 41 dcm[m][n] = (m == n) ? 1 : 0; // dcm starts as identity matrix
shimniok 0:62284d27d75e 42 }
oprospero 6:49cbf2acc4e6 43 reset_sensor_fusion();
shimniok 0:62284d27d75e 44 }
shimniok 0:62284d27d75e 45 }
shimniok 0:62284d27d75e 46
shimniok 0:62284d27d75e 47
shimniok 0:62284d27d75e 48 /**************************************************/
shimniok 0:62284d27d75e 49 void DCM::Normalize(void)
shimniok 0:62284d27d75e 50 {
oprospero 6:49cbf2acc4e6 51 float error=0;
oprospero 6:49cbf2acc4e6 52 float temporary[3][3];
oprospero 6:49cbf2acc4e6 53 float renorm=0;
oprospero 6:49cbf2acc4e6 54
oprospero 1:115cf0a84a9d 55 error= -Vector_Dot_Product(&dcm[0][0], &dcm[1][0])*.5; //eq.19
oprospero 6:49cbf2acc4e6 56
oprospero 6:49cbf2acc4e6 57 Vector_Scale(&temporary[0][0], &dcm[1][0], error); //eq.19
oprospero 6:49cbf2acc4e6 58 Vector_Scale(&temporary[1][0], &dcm[0][0], error); //eq.19
oprospero 1:115cf0a84a9d 59
oprospero 6:49cbf2acc4e6 60 Vector_Add(&temporary[0][0], &temporary[0][0], &dcm[0][0]);//eq.19
oprospero 6:49cbf2acc4e6 61 Vector_Add(&temporary[1][0], &temporary[1][0], &dcm[1][0]);//eq.19
oprospero 6:49cbf2acc4e6 62
oprospero 6:49cbf2acc4e6 63 Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20
oprospero 6:49cbf2acc4e6 64
oprospero 6:49cbf2acc4e6 65 renorm= .5 *(3 - Vector_Dot_Product(&temporary[0][0],&temporary[0][0])); //eq.21
oprospero 6:49cbf2acc4e6 66 Vector_Scale(&dcm[0][0], &temporary[0][0], renorm);
oprospero 6:49cbf2acc4e6 67
oprospero 6:49cbf2acc4e6 68 renorm= .5 *(3 - Vector_Dot_Product(&temporary[1][0],&temporary[1][0])); //eq.21
oprospero 6:49cbf2acc4e6 69 Vector_Scale(&dcm[1][0], &temporary[1][0], renorm);
oprospero 6:49cbf2acc4e6 70
oprospero 6:49cbf2acc4e6 71 renorm= .5 *(3 - Vector_Dot_Product(&temporary[2][0],&temporary[2][0])); //eq.21
oprospero 6:49cbf2acc4e6 72 Vector_Scale(&dcm[2][0], &temporary[2][0], renorm);
shimniok 0:62284d27d75e 73 }
shimniok 0:62284d27d75e 74
shimniok 0:62284d27d75e 75 /**************************************************/
shimniok 0:62284d27d75e 76 void DCM::Drift_correction(void)
shimniok 0:62284d27d75e 77 {
shimniok 0:62284d27d75e 78 float mag_heading_x;
shimniok 0:62284d27d75e 79 float mag_heading_y;
shimniok 0:62284d27d75e 80 float errorCourse;
shimniok 0:62284d27d75e 81 //Compensation the Roll, Pitch and Yaw drift.
shimniok 0:62284d27d75e 82 static float Scaled_Omega_P[3];
shimniok 0:62284d27d75e 83 static float Scaled_Omega_I[3];
shimniok 0:62284d27d75e 84 float Accel_magnitude;
shimniok 0:62284d27d75e 85 float Accel_weight;
shimniok 0:62284d27d75e 86
shimniok 0:62284d27d75e 87
shimniok 0:62284d27d75e 88 //*****Roll and Pitch***************
shimniok 0:62284d27d75e 89
shimniok 0:62284d27d75e 90 // Calculate the magnitude of the accelerometer vector
shimniok 0:62284d27d75e 91 Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]);
shimniok 0:62284d27d75e 92 Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
shimniok 0:62284d27d75e 93 // Dynamic weighting of accelerometer info (reliability filter)
shimniok 0:62284d27d75e 94 // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
shimniok 0:62284d27d75e 95 Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1); //
oprospero 1:115cf0a84a9d 96
shimniok 0:62284d27d75e 97 Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&dcm[2][0]); //adjust the ground of reference
shimniok 0:62284d27d75e 98 Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
shimniok 0:62284d27d75e 99
shimniok 0:62284d27d75e 100 Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
oprospero 1:115cf0a84a9d 101 Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);
shimniok 0:62284d27d75e 102
shimniok 0:62284d27d75e 103 //*****YAW***************
shimniok 0:62284d27d75e 104 // We make the gyro YAW drift correction based on compass magnetic heading
oprospero 1:115cf0a84a9d 105 mag_heading_x = cos(MAG_Heading);
oprospero 1:115cf0a84a9d 106 mag_heading_y = sin(MAG_Heading);
oprospero 1:115cf0a84a9d 107 errorCourse=(dcm[0][0]*mag_heading_y) - (dcm[1][0]*mag_heading_x); //Calculating YAW error
oprospero 1:115cf0a84a9d 108 Vector_Scale(errorYaw,&dcm[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
oprospero 1:115cf0a84a9d 109
oprospero 1:115cf0a84a9d 110 Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);//.01proportional of YAW.
oprospero 1:115cf0a84a9d 111 Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
oprospero 1:115cf0a84a9d 112
oprospero 1:115cf0a84a9d 113 Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);//.00001Integrator
oprospero 1:115cf0a84a9d 114 Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
shimniok 0:62284d27d75e 115
shimniok 0:62284d27d75e 116 }
shimniok 0:62284d27d75e 117
shimniok 0:62284d27d75e 118
shimniok 0:62284d27d75e 119 /**************************************************/
shimniok 0:62284d27d75e 120 void DCM::Matrix_update(void)
shimniok 0:62284d27d75e 121 {
oprospero 6:49cbf2acc4e6 122 // p.printf("Omega: ");pv(Omega);
oprospero 1:115cf0a84a9d 123 Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]); //adding proportional term
oprospero 1:115cf0a84a9d 124 Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
oprospero 6:49cbf2acc4e6 125 // p.printf("Omega_Vector: ");pv(Omega_Vector);
oprospero 1:115cf0a84a9d 126 #if OUTPUTMODE==1
oprospero 1:115cf0a84a9d 127 Update_Matrix[0][0]=0;
oprospero 1:115cf0a84a9d 128 Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
oprospero 1:115cf0a84a9d 129 Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
oprospero 1:115cf0a84a9d 130 Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
oprospero 1:115cf0a84a9d 131 Update_Matrix[1][1]=0;
oprospero 1:115cf0a84a9d 132 Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x
oprospero 1:115cf0a84a9d 133 Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
oprospero 1:115cf0a84a9d 134 Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
oprospero 1:115cf0a84a9d 135 Update_Matrix[2][2]=0;
oprospero 6:49cbf2acc4e6 136 // p.printf("Update: \n\r");pm(Update_Matrix);
oprospero 1:115cf0a84a9d 137 #else // Uncorrected data (no drift correction)
oprospero 1:115cf0a84a9d 138 Update_Matrix[0][0]=0;
oprospero 1:115cf0a84a9d 139 Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z
oprospero 1:115cf0a84a9d 140 Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y
oprospero 1:115cf0a84a9d 141 Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z
oprospero 1:115cf0a84a9d 142 Update_Matrix[1][1]=0;
oprospero 1:115cf0a84a9d 143 Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0];
oprospero 1:115cf0a84a9d 144 Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1];
oprospero 1:115cf0a84a9d 145 Update_Matrix[2][1]=G_Dt*Gyro_Vector[0];
oprospero 1:115cf0a84a9d 146 Update_Matrix[2][2]=0;
oprospero 1:115cf0a84a9d 147 #endif
oprospero 1:115cf0a84a9d 148
oprospero 6:49cbf2acc4e6 149 // p.printf("Temp: \n\r");pm(Temporary_Matrix);
oprospero 6:49cbf2acc4e6 150 // p.printf("Update: \n\r");pm(Update_Matrix);
oprospero 1:115cf0a84a9d 151 Matrix_Multiply(Temporary_Matrix, dcm, Update_Matrix); //c=a*b
oprospero 1:115cf0a84a9d 152 // ??? Matrix_Add(dcm, dcm, Temporary_Matrix); ???
oprospero 1:115cf0a84a9d 153 for(int x=0; x<3; x++) { //Matrix Addition (update)
oprospero 1:115cf0a84a9d 154 for(int y=0; y<3; y++) {
oprospero 1:115cf0a84a9d 155 dcm[x][y] += Temporary_Matrix[x][y];
oprospero 1:115cf0a84a9d 156 }
oprospero 1:115cf0a84a9d 157 }
shimniok 0:62284d27d75e 158 }
shimniok 0:62284d27d75e 159
shimniok 0:62284d27d75e 160 void DCM::Euler_angles(void)
shimniok 0:62284d27d75e 161 {
oprospero 5:d5ecdb82c17b 162 pitch = RAD2DEG(-asin(dcm[2][0]));
oprospero 5:d5ecdb82c17b 163 roll = RAD2DEG(atan2(dcm[2][1],dcm[2][2]));
oprospero 5:d5ecdb82c17b 164 yaw = RAD2DEG(atan2(dcm[1][0],dcm[0][0]));
shimniok 0:62284d27d75e 165 }
shimniok 0:62284d27d75e 166
oprospero 4:4c6ffd1a88ce 167 void DCM::Update_step(float dt)
oprospero 1:115cf0a84a9d 168 {
oprospero 4:4c6ffd1a88ce 169 G_Dt = dt;
oprospero 3:ef027800a8d5 170 Read_Accel(Accel_Vector);
oprospero 3:ef027800a8d5 171 Read_Gyro(Gyro_Vector);
shimniok 0:62284d27d75e 172 if (--update_count == 0) {
shimniok 0:62284d27d75e 173 Update_mag();
shimniok 0:62284d27d75e 174 update_count = MAG_SKIP;
shimniok 0:62284d27d75e 175 }
shimniok 0:62284d27d75e 176 Matrix_update();
shimniok 0:62284d27d75e 177 Normalize();
shimniok 0:62284d27d75e 178 Drift_correction();
shimniok 0:62284d27d75e 179 Euler_angles();
shimniok 0:62284d27d75e 180 }
shimniok 0:62284d27d75e 181
shimniok 0:62284d27d75e 182 void DCM::Update_mag()
shimniok 0:62284d27d75e 183 {
oprospero 3:ef027800a8d5 184 Read_Magn(Magn_Vector);
shimniok 0:62284d27d75e 185 Compass_Heading();
shimniok 0:62284d27d75e 186 }
oprospero 1:115cf0a84a9d 187
oprospero 1:115cf0a84a9d 188 void DCM::Compass_Heading()
oprospero 1:115cf0a84a9d 189 {
oprospero 1:115cf0a84a9d 190 float mag_x;
oprospero 1:115cf0a84a9d 191 float mag_y;
oprospero 1:115cf0a84a9d 192 float cos_roll;
oprospero 1:115cf0a84a9d 193 float sin_roll;
oprospero 1:115cf0a84a9d 194 float cos_pitch;
oprospero 1:115cf0a84a9d 195 float sin_pitch;
oprospero 1:115cf0a84a9d 196
oprospero 1:115cf0a84a9d 197 cos_roll = cos(roll);
oprospero 1:115cf0a84a9d 198 sin_roll = sin(roll);
oprospero 1:115cf0a84a9d 199 cos_pitch = cos(pitch);
oprospero 1:115cf0a84a9d 200 sin_pitch = sin(pitch);
oprospero 1:115cf0a84a9d 201
oprospero 1:115cf0a84a9d 202 // Tilt compensated magnetic field X
oprospero 1:115cf0a84a9d 203 mag_x = Magn_Vector[0]*cos_pitch + Magn_Vector[1]*sin_roll*sin_pitch + Magn_Vector[2]*cos_roll*sin_pitch;
oprospero 1:115cf0a84a9d 204 // Tilt compensated magnetic field Y
oprospero 1:115cf0a84a9d 205 mag_y = Magn_Vector[1]*cos_roll - Magn_Vector[2]*sin_roll;
oprospero 1:115cf0a84a9d 206 // Magnetic Heading
oprospero 1:115cf0a84a9d 207 MAG_Heading = atan2(-mag_y, mag_x);
oprospero 1:115cf0a84a9d 208 }
oprospero 1:115cf0a84a9d 209
oprospero 6:49cbf2acc4e6 210 void DCM::reset_sensor_fusion()
oprospero 6:49cbf2acc4e6 211 {
oprospero 6:49cbf2acc4e6 212 float temp1[3];
oprospero 6:49cbf2acc4e6 213 float temp2[3];
oprospero 6:49cbf2acc4e6 214 float xAxis[] = {1.0f, 0.0f, 0.0f};
oprospero 6:49cbf2acc4e6 215
oprospero 6:49cbf2acc4e6 216 Read_Accel(Accel_Vector);
oprospero 6:49cbf2acc4e6 217 Read_Gyro(Gyro_Vector);
oprospero 6:49cbf2acc4e6 218
oprospero 6:49cbf2acc4e6 219 // timestamp = timer.read_ms();
oprospero 6:49cbf2acc4e6 220
oprospero 6:49cbf2acc4e6 221 // GET PITCH
oprospero 6:49cbf2acc4e6 222 // Using y-z-plane-component/x-component of gravity vector
oprospero 6:49cbf2acc4e6 223 pitch = -atan2(Accel_Vector[0], sqrt(Accel_Vector[1] * Accel_Vector[1] + Accel_Vector[2] * Accel_Vector[2]));
oprospero 6:49cbf2acc4e6 224
oprospero 6:49cbf2acc4e6 225 // GET ROLL
oprospero 6:49cbf2acc4e6 226 // Compensate pitch of gravity vector
oprospero 6:49cbf2acc4e6 227 Vector_Cross_Product(temp1, Accel_Vector, xAxis);
oprospero 6:49cbf2acc4e6 228 Vector_Cross_Product(temp2, xAxis, temp1);
oprospero 6:49cbf2acc4e6 229 // Normally using x-z-plane-component/y-component of compensated gravity vector
oprospero 6:49cbf2acc4e6 230 // roll = atan2(temp2[1], sqrt(temp2[0] * temp2[0] + temp2[2] * temp2[2]));
oprospero 6:49cbf2acc4e6 231 // Since we compensated for pitch, x-z-plane-component equals z-component:
oprospero 6:49cbf2acc4e6 232 roll = atan2(temp2[1], temp2[2]);
oprospero 6:49cbf2acc4e6 233
oprospero 6:49cbf2acc4e6 234 // GET YAW
oprospero 6:49cbf2acc4e6 235 Update_mag();
oprospero 6:49cbf2acc4e6 236 yaw = MAG_Heading;
oprospero 6:49cbf2acc4e6 237
oprospero 6:49cbf2acc4e6 238 // Init rotation matrix
oprospero 6:49cbf2acc4e6 239 init_rotation_matrix(dcm, yaw, pitch, roll);
oprospero 6:49cbf2acc4e6 240 }
oprospero 6:49cbf2acc4e6 241
oprospero 6:49cbf2acc4e6 242 float DCM::constrain(float x, float a, float b)
oprospero 6:49cbf2acc4e6 243 {
oprospero 6:49cbf2acc4e6 244 float result = x;
oprospero 6:49cbf2acc4e6 245
oprospero 6:49cbf2acc4e6 246 if (x < a) result = a;
oprospero 6:49cbf2acc4e6 247 if (x > b) result = b;
oprospero 6:49cbf2acc4e6 248
oprospero 6:49cbf2acc4e6 249 return result;
oprospero 6:49cbf2acc4e6 250 }