-
Notifications
You must be signed in to change notification settings - Fork 3
/
DCM.ino
127 lines (100 loc) · 4.56 KB
/
DCM.ino
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
/* This file is part of the Razor AHRS Firmware */
// DCM algorithm
/**************************************************/
void Normalize(void)
{
float error=0;
float temporary[3][3];
float renorm=0;
error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19
Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19
Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19
Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]);//eq.19
Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]);//eq.19
Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20
renorm= .5 *(3 - Vector_Dot_Product(&temporary[0][0],&temporary[0][0])); //eq.21
Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
renorm= .5 *(3 - Vector_Dot_Product(&temporary[1][0],&temporary[1][0])); //eq.21
Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
renorm= .5 *(3 - Vector_Dot_Product(&temporary[2][0],&temporary[2][0])); //eq.21
Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
}
/**************************************************/
void Drift_correction(void)
{
float mag_heading_x;
float mag_heading_y;
float errorCourse;
//Compensation the Roll, Pitch and Yaw drift.
static float Scaled_Omega_P[3];
static float Scaled_Omega_I[3];
float Accel_magnitude;
float Accel_weight;
//*****Roll and Pitch***************
// Calculate the magnitude of the accelerometer vector
Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]);
Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
// Dynamic weighting of accelerometer info (reliability filter)
// Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1); //
Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference
Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);
//*****YAW***************
// We make the gyro YAW drift correction based on compass magnetic heading
mag_heading_x = cos(MAG_Heading);
mag_heading_y = sin(MAG_Heading);
errorCourse=(DCM_Matrix[0][0]*mag_heading_y) - (DCM_Matrix[1][0]*mag_heading_x); //Calculating YAW error
Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);//.01proportional of YAW.
Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);//.00001Integrator
Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
}
void Matrix_update(void)
{
Gyro_Vector[0]=GYRO_SCALED_RAD(gyro[0]); //gyro x roll
Gyro_Vector[1]=GYRO_SCALED_RAD(gyro[1]); //gyro y pitch
Gyro_Vector[2]=GYRO_SCALED_RAD(gyro[2]); //gyro z yaw
Accel_Vector[0]=accel[0];
Accel_Vector[1]=accel[1];
Accel_Vector[2]=accel[2];
Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]); //adding proportional term
Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
#if DEBUG__NO_DRIFT_CORRECTION == true // Do not use drift correction
Update_Matrix[0][0]=0;
Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z
Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y
Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z
Update_Matrix[1][1]=0;
Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0];
Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1];
Update_Matrix[2][1]=G_Dt*Gyro_Vector[0];
Update_Matrix[2][2]=0;
#else // Use drift correction
Update_Matrix[0][0]=0;
Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
Update_Matrix[1][1]=0;
Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x
Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
Update_Matrix[2][2]=0;
#endif
Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c
for(int x=0; x<3; x++) //Matrix Addition (update)
{
for(int y=0; y<3; y++)
{
DCM_Matrix[x][y]+=Temporary_Matrix[x][y];
}
}
}
void Euler_angles(void)
{
pitch = -asin(DCM_Matrix[2][0]);
roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
}