-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathDCM.ino
172 lines (136 loc) · 6.31 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
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
/*
MinIMU-9-Arduino-AHRS
Pololu MinIMU-9 + Arduino AHRS (Attitude and Heading Reference System)
Copyright (c) 2011 Pololu Corporation.
http://www.pololu.com/
MinIMU-9-Arduino-AHRS is based on sf9domahrs by Doug Weibel and Jose Julio:
http://code.google.com/p/sf9domahrs/
sf9domahrs is based on ArduIMU v1.5 by Jordi Munoz and William Premerlani, Jose
Julio and Doug Weibel:
http://code.google.com/p/ardu-imu/
MinIMU-9-Arduino-AHRS is free software: you can redistribute it and/or modify it
under the terms of the GNU Lesser General Public License as published by the
Free Software Foundation, either version 3 of the License, or (at your option)
any later version.
MinIMU-9-Arduino-AHRS is distributed in the hope that it will be useful, but
WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for
more details.
You should have received a copy of the GNU Lesser General Public License along
with MinIMU-9-Arduino-AHRS. If not, see <http://www.gnu.org/licenses/>.
*/
/**************************************************/
#if Compass == 0
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); // pulls mag values out of compass.read() function for purposes of calculating yaw
mag_heading_y = sin(MAG_Heading);
#if LSMLib == 1
if (abs(roll) < PI / 2.0) { // 2/24/24: maintains tilt compensation if IMU upside down
mag_heading_x = -mag_heading_x;
mag_heading_y = -mag_heading_y; }
#endif
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 Accel_adjust(void)
{
Accel_Vector[1] += Accel_Scale(speed_3d*Omega[2]); // Centrifugal force on Acc_y = GPS_speed*GyroZ
Accel_Vector[2] -= Accel_Scale(speed_3d*Omega[1]); // Centrifugal force on Acc_z = GPS_speed*GyroY
}
*/
/**************************************************/
void Matrix_update(void)
{
Gyro_Vector[0]=Gyro_Scaled_X(gyro_x); //gyro x roll
Gyro_Vector[1]=Gyro_Scaled_Y(gyro_y); //gyro y pitch
Gyro_Vector[2]=Gyro_Scaled_Z(gyro_z); //gyro Z yaw
Accel_Vector[0]=accel_x;
Accel_Vector[1]=accel_y;
Accel_Vector[2]=accel_z;
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
//Accel_adjust(); //Remove centrifugal acceleration. We are not using this function in this version - we have no speed measurement
#if OUTPUTMODE==1
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;
#else // Uncorrected data (no 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;
#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]);
}
#endif