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@14:6cb3c2204b9b, 2013-10-16 (annotated)

Committer:
oprospero
Date:
Wed Oct 16 07:49:34 2013 +0000
Revision:
14:6cb3c2204b9b
Parent:
13:6134f21cad37 
Fixed vector copy;

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