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@11:8d46121bd255, 2013-09-26 (annotated)

Committer:
oprospero
Date:
Thu Sep 26 05:19:13 2013 +0000
Revision:
11:8d46121bd255
Parent:
8:195d53710158
Child:
12:e85008094132
tweeks and test

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