root / quad1 / AeroQuad / FlightAngle.h @ 9240aaa3
History | View | Annotate | Download (23.1 KB)
1 |
/*
|
---|---|
2 |
AeroQuad v2.1 - October 2010
|
3 |
www.AeroQuad.com
|
4 |
Copyright (c) 2010 Ted Carancho. All rights reserved.
|
5 |
An Open Source Arduino based quadrocopter.
|
6 |
|
7 |
This program is free software: you can redistribute it and/or modify
|
8 |
it under the terms of the GNU General Public License as published by
|
9 |
the Free Software Foundation, either version 3 of the License, or
|
10 |
(at your option) any later version.
|
11 |
|
12 |
This program is distributed in the hope that it will be useful,
|
13 |
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
14 |
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
15 |
GNU General Public License for more details.
|
16 |
|
17 |
You should have received a copy of the GNU General Public License
|
18 |
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
19 |
*/
|
20 |
|
21 |
// This class is responsible for calculating vehicle attitude
|
22 |
class FlightAngle { |
23 |
public:
|
24 |
#define CF 0 |
25 |
#define KF 1 |
26 |
#define DCM 2 |
27 |
#define IMU 3 |
28 |
byte type; |
29 |
float angle[3]; |
30 |
float gyroAngle[2]; |
31 |
|
32 |
FlightAngle(void) {
|
33 |
angle[ROLL] = 0;
|
34 |
angle[PITCH] = 0;
|
35 |
angle[YAW] = 0;
|
36 |
gyroAngle[ROLL] = 0;
|
37 |
gyroAngle[PITCH] = 0;
|
38 |
} |
39 |
|
40 |
virtual void initialize();
|
41 |
virtual void calculate();
|
42 |
virtual float getGyroAngle(byte axis);
|
43 |
|
44 |
const float getData(byte axis) { |
45 |
return angle[axis];
|
46 |
} |
47 |
|
48 |
const byte getType(void) { |
49 |
// This is set in each subclass to identify which algorithm used
|
50 |
return type;
|
51 |
} |
52 |
}; |
53 |
|
54 |
/******************************************************/
|
55 |
/*************** Complementary Filter *****************/
|
56 |
/******************************************************/
|
57 |
// Originally authored by RoyLB
|
58 |
// http://www.rcgroups.com/forums/showpost.php?p=12082524&postcount=1286
|
59 |
class FlightAngle_CompFilter : public FlightAngle { |
60 |
private:
|
61 |
float previousAngle[2]; |
62 |
float filterTerm0[2]; |
63 |
float filterTerm1[2]; |
64 |
float filterTerm2[2]; |
65 |
float timeConstantCF;
|
66 |
|
67 |
void _initialize(byte axis) {
|
68 |
previousAngle[axis] = accel.angleDeg(axis); |
69 |
filterTerm2[axis] = gyro.rateDegPerSec(axis); |
70 |
timeConstantCF = timeConstant; // timeConstant is a global variable read in from EEPROM
|
71 |
// timeConstantCF should have been read in from set method, but needed common way for CF and KF to be initialized
|
72 |
// Will take care of better OO implementation in future revision
|
73 |
} |
74 |
|
75 |
float _calculate(byte axis, float newAngle, float newRate) { |
76 |
filterTerm0[axis] = (newAngle - previousAngle[axis]) * timeConstantCF * timeConstantCF; |
77 |
filterTerm2[axis] += filterTerm0[axis] * G_Dt; |
78 |
filterTerm1[axis] = filterTerm2[axis] + (newAngle - previousAngle[axis]) * 2 * timeConstantCF + newRate;
|
79 |
previousAngle[axis] = (filterTerm1[axis] * G_Dt) + previousAngle[axis]; |
80 |
return previousAngle[axis]; // This is actually the current angle, but is stored for the next iteration |
81 |
} |
82 |
|
83 |
public:
|
84 |
FlightAngle_CompFilter() : FlightAngle() { |
85 |
filterTerm0[ROLL] = 0;
|
86 |
filterTerm1[ROLL] = 0;
|
87 |
filterTerm0[PITCH] = 0;
|
88 |
filterTerm1[PITCH] = 0;
|
89 |
type = CF; |
90 |
} |
91 |
|
92 |
void initialize(void) { |
93 |
for (axis = ROLL; axis < YAW; axis++)
|
94 |
_initialize(axis); |
95 |
} |
96 |
|
97 |
void calculate(void) { |
98 |
angle[ROLL] = _calculate(ROLL, accel.angleDeg(ROLL), gyro.rateDegPerSec(ROLL)); |
99 |
angle[PITCH] = _calculate(PITCH, accel.angleDeg(PITCH), gyro.rateDegPerSec(PITCH)); |
100 |
} |
101 |
|
102 |
float getGyroAngle(byte axis) {
|
103 |
gyroAngle[axis] += gyro.rateDegPerSec(axis) * G_Dt; |
104 |
} |
105 |
}; |
106 |
|
107 |
/******************************************************/
|
108 |
/****************** Kalman Filter *********************/
|
109 |
/******************************************************/
|
110 |
// Originally authored by Tom Pycke
|
111 |
// http://tom.pycke.be/mav/71/kalman-filtering-of-imu-data
|
112 |
class FlightAngle_KalmanFilter : public FlightAngle { |
113 |
private:
|
114 |
float x_angle[2], x_bias[2]; |
115 |
float P_00[2], P_01[2], P_10[2], P_11[2]; |
116 |
float Q_angle, Q_gyro;
|
117 |
float R_angle;
|
118 |
float y, S;
|
119 |
float K_0, K_1;
|
120 |
|
121 |
float _calculate(byte axis, float newAngle, float newRate) { |
122 |
x_angle[axis] += G_Dt * (newRate - x_bias[axis]); |
123 |
P_00[axis] += - G_Dt * (P_10[axis] + P_01[axis]) + Q_angle * G_Dt; |
124 |
P_01[axis] += - G_Dt * P_11[axis]; |
125 |
P_10[axis] += - G_Dt * P_11[axis]; |
126 |
P_11[axis] += + Q_gyro * G_Dt; |
127 |
|
128 |
y = newAngle - x_angle[axis]; |
129 |
S = P_00[axis] + R_angle; |
130 |
K_0 = P_00[axis] / S; |
131 |
K_1 = P_10[axis] / S; |
132 |
|
133 |
x_angle[axis] += K_0 * y; |
134 |
x_bias[axis] += K_1 * y; |
135 |
P_00[axis] -= K_0 * P_00[axis]; |
136 |
P_01[axis] -= K_0 * P_01[axis]; |
137 |
P_10[axis] -= K_1 * P_00[axis]; |
138 |
P_11[axis] -= K_1 * P_01[axis]; |
139 |
|
140 |
return x_angle[axis];
|
141 |
} |
142 |
|
143 |
public:
|
144 |
FlightAngle_KalmanFilter() : FlightAngle() { |
145 |
for (axis = ROLL; axis ++; axis < YAW) {
|
146 |
x_angle[axis] = 0;
|
147 |
x_bias[axis] = 0;
|
148 |
P_00[axis] = 0;
|
149 |
P_01[axis] = 0;
|
150 |
P_10[axis] = 0;
|
151 |
P_11[axis] = 0;
|
152 |
} |
153 |
type = KF; |
154 |
} |
155 |
|
156 |
void initialize(void) { |
157 |
Q_angle = 0.001; |
158 |
Q_gyro = 0.003; |
159 |
R_angle = 0.03; |
160 |
} |
161 |
|
162 |
void calculate(void) { |
163 |
angle[ROLL] = _calculate(ROLL, accel.angleDeg(ROLL), gyro.rateDegPerSec(ROLL)); |
164 |
angle[PITCH] = _calculate(PITCH, accel.angleDeg(PITCH), gyro.rateDegPerSec(PITCH)); |
165 |
} |
166 |
|
167 |
float getGyroAngle(byte axis) {
|
168 |
gyroAngle[axis] += gyro.rateDegPerSec(axis) * G_Dt; |
169 |
} |
170 |
}; |
171 |
|
172 |
/******************************************************/
|
173 |
/*********************** DCM **************************/
|
174 |
/******************************************************/
|
175 |
// Written by William Premerlani
|
176 |
// Modified by Jose Julio for multicopters
|
177 |
// http://diydrones.com/profiles/blogs/dcm-imu-theory-first-draft
|
178 |
class FlightAngle_DCM : public FlightAngle { |
179 |
private:
|
180 |
float dt;
|
181 |
float Gyro_Gain_X;
|
182 |
float Gyro_Gain_Y;
|
183 |
float Gyro_Gain_Z;
|
184 |
float DCM_Matrix[3][3]; |
185 |
float Update_Matrix[3][3]; |
186 |
float Temporary_Matrix[3][3]; |
187 |
float Accel_Vector[3]; |
188 |
float Accel_Vector_unfiltered[3]; |
189 |
float Gyro_Vector[3]; |
190 |
float Omega_Vector[3]; |
191 |
float Omega_P[3]; |
192 |
float Omega_I[3]; |
193 |
float Omega[3]; |
194 |
float errorRollPitch[3]; |
195 |
float errorYaw[3]; |
196 |
float errorCourse;
|
197 |
float COGX; //Course overground X axis |
198 |
float COGY; //Course overground Y axis |
199 |
float Kp_ROLLPITCH;
|
200 |
float Ki_ROLLPITCH;
|
201 |
|
202 |
//Computes the dot product of two vectors
|
203 |
float Vector_Dot_Product(float vector1[3],float vector2[3]) { |
204 |
float op=0; |
205 |
|
206 |
for(int c=0; c<3; c++) |
207 |
op+=vector1[c]*vector2[c]; |
208 |
return op;
|
209 |
} |
210 |
|
211 |
//Computes the cross product of two vectors
|
212 |
void Vector_Cross_Product(float vectorOut[3], float v1[3],float v2[3]) { |
213 |
vectorOut[0]= (v1[1]*v2[2]) - (v1[2]*v2[1]); |
214 |
vectorOut[1]= (v1[2]*v2[0]) - (v1[0]*v2[2]); |
215 |
vectorOut[2]= (v1[0]*v2[1]) - (v1[1]*v2[0]); |
216 |
} |
217 |
|
218 |
//Multiply the vector by a scalar.
|
219 |
void Vector_Scale(float vectorOut[3],float vectorIn[3], float scale2) { |
220 |
for(int c=0; c<3; c++) |
221 |
vectorOut[c]=vectorIn[c]*scale2; |
222 |
} |
223 |
|
224 |
void Vector_Add(float vectorOut[3],float vectorIn1[3], float vectorIn2[3]){ |
225 |
for(int c=0; c<3; c++) |
226 |
vectorOut[c]=vectorIn1[c]+vectorIn2[c]; |
227 |
} |
228 |
|
229 |
/********* MATRIX FUNCTIONS *****************************************/
|
230 |
//Multiply two 3x3 matrixs. This function developed by Jordi can be easily adapted to multiple n*n matrix's. (Pero me da flojera!).
|
231 |
void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3]) { |
232 |
float op[3]; |
233 |
for(int x=0; x<3; x++) { |
234 |
for(int y=0; y<3; y++) { |
235 |
for(int w=0; w<3; w++) |
236 |
op[w]=a[x][w]*b[w][y]; |
237 |
mat[x][y]=0;
|
238 |
mat[x][y]=op[0]+op[1]+op[2]; |
239 |
float test=mat[x][y];
|
240 |
} |
241 |
} |
242 |
} |
243 |
|
244 |
void Normalize(void) { |
245 |
float error=0; |
246 |
float temporary[3][3]; |
247 |
float renorm=0; |
248 |
|
249 |
error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19 |
250 |
|
251 |
Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19 |
252 |
Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19 |
253 |
|
254 |
Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]);//eq.19 |
255 |
Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]);//eq.19 |
256 |
|
257 |
Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20 |
258 |
|
259 |
renorm= .5 *(3 - Vector_Dot_Product(&temporary[0][0],&temporary[0][0])); //eq.21 |
260 |
Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm); |
261 |
|
262 |
renorm= .5 *(3 - Vector_Dot_Product(&temporary[1][0],&temporary[1][0])); //eq.21 |
263 |
Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm); |
264 |
|
265 |
renorm= .5 *(3 - Vector_Dot_Product(&temporary[2][0],&temporary[2][0])); //eq.21 |
266 |
Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm); |
267 |
} |
268 |
|
269 |
void Drift_correction(void) { |
270 |
//Compensation the Roll, Pitch and Yaw drift.
|
271 |
float errorCourse;
|
272 |
static float Scaled_Omega_P[3]; |
273 |
static float Scaled_Omega_I[3]; |
274 |
float Accel_magnitude;
|
275 |
float Accel_weight;
|
276 |
|
277 |
//*****Roll and Pitch***************
|
278 |
|
279 |
// Calculate the magnitude of the accelerometer vector
|
280 |
// Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]);
|
281 |
// Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
|
282 |
// Weight for accelerometer info (<0.75G = 0.0, 1G = 1.0 , >1.25G = 0.0)
|
283 |
// Accel_weight = constrain(1 - 4*abs(1 - Accel_magnitude),0,1);
|
284 |
// Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
|
285 |
// Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1);
|
286 |
Accel_weight = 1.0; |
287 |
|
288 |
Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference |
289 |
Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight); |
290 |
|
291 |
Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight); |
292 |
Vector_Add(Omega_I,Omega_I,Scaled_Omega_I); |
293 |
|
294 |
//*****YAW***************
|
295 |
// We make the gyro YAW drift correction based on compass magnetic heading
|
296 |
/*if (MAGNETOMETER == 1) {
|
297 |
errorCourse= (DCM_Matrix[0][0]*APM_Compass.Heading_Y) - (DCM_Matrix[1][0]*APM_Compass.Heading_X); //Calculating YAW error
|
298 |
Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
|
299 |
|
300 |
Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);
|
301 |
Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
|
302 |
|
303 |
Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);
|
304 |
Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
|
305 |
}*/
|
306 |
} |
307 |
|
308 |
/*void Accel_adjust(void) {
|
309 |
// ADC : Voltage reference 3.0V / 10bits(1024 steps) => 2.93mV/ADC step
|
310 |
// ADXL335 Sensitivity(from datasheet) => 330mV/g, 2.93mV/ADC step => 330/0.8 = 102
|
311 |
#define GRAVITY 102 //this equivalent to 1G in the raw data coming from the accelerometer
|
312 |
#define Accel_Scale(x) x*(GRAVITY/9.81)//Scaling the raw data of the accel to actual acceleration in meters for seconds square
|
313 |
|
314 |
Accel_Vector[1] += Accel_Scale(speed_3d*Omega[2]); // Centrifugal force on Acc_y = GPS_speed*GyroZ
|
315 |
Accel_Vector[2] -= Accel_Scale(speed_3d*Omega[1]); // Centrifugal force on Acc_z = GPS_speed*GyroY
|
316 |
}*/
|
317 |
|
318 |
void Matrix_update(void) { |
319 |
Gyro_Vector[0]=Gyro_Gain_X * -gyro.getData(PITCH); //gyro x roll |
320 |
Gyro_Vector[1]=Gyro_Gain_Y * gyro.getData(ROLL); //gyro y pitch |
321 |
Gyro_Vector[2]=Gyro_Gain_Z * gyro.getData(YAW); //gyro Z yaw |
322 |
|
323 |
Accel_Vector[0]=-accel.getFlightData(ROLL); // acc x |
324 |
Accel_Vector[1]=accel.getFlightData(PITCH); // acc y |
325 |
Accel_Vector[2]=accel.getFlightData(ZAXIS); // acc z |
326 |
|
327 |
// Low pass filter on accelerometer data (to filter vibrations)
|
328 |
//Accel_Vector[0]=Accel_Vector[0]*0.5 + (float)read_adc(3)*0.5; // acc x
|
329 |
//Accel_Vector[1]=Accel_Vector[1]*0.5 + (float)read_adc(4)*0.5; // acc y
|
330 |
//Accel_Vector[2]=Accel_Vector[2]*0.5 + (float)read_adc(5)*0.5; // acc z
|
331 |
|
332 |
Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]);//adding integrator |
333 |
Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]);//adding proportional |
334 |
|
335 |
//Accel_adjust();//adjusting centrifugal acceleration. // Not used for quadcopter
|
336 |
|
337 |
Update_Matrix[0][0]=0; |
338 |
Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z |
339 |
Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y |
340 |
Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z |
341 |
Update_Matrix[1][1]=0; |
342 |
Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x |
343 |
Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y |
344 |
Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x |
345 |
Update_Matrix[2][2]=0; |
346 |
|
347 |
//Serial.println(DCM_Matrix[2][0], 6);
|
348 |
|
349 |
Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c
|
350 |
|
351 |
for(int x=0; x<3; x++) { //Matrix Addition (update) |
352 |
for(int y=0; y<3; y++) |
353 |
DCM_Matrix[x][y]+=Temporary_Matrix[x][y]; |
354 |
} |
355 |
} |
356 |
|
357 |
void Euler_angles(void) { |
358 |
angle[ROLL] = degrees(asin(-DCM_Matrix[2][0])); |
359 |
angle[PITCH] = degrees(atan2(DCM_Matrix[2][1],DCM_Matrix[2][2])); |
360 |
angle[YAW] = degrees(atan2(DCM_Matrix[1][0],DCM_Matrix[0][0])); |
361 |
} |
362 |
|
363 |
public:
|
364 |
FlightAngle_DCM():FlightAngle() {} |
365 |
|
366 |
void initialize(void) { |
367 |
for (byte i=0; i<3; i++) { |
368 |
Accel_Vector[i] = 0; //Store the acceleration in a vector |
369 |
Accel_Vector_unfiltered[i] = 0; //Store the acceleration in a vector |
370 |
Gyro_Vector[i] = 0;//Store the gyros rutn rate in a vector |
371 |
Omega_Vector[i] = 0; //Corrected Gyro_Vector data |
372 |
Omega_P[i] = 0;//Omega Proportional correction |
373 |
Omega_I[i] = 0;//Omega Integrator |
374 |
Omega[i] = 0;
|
375 |
errorRollPitch[i]= 0;
|
376 |
errorYaw[i]= 0;
|
377 |
} |
378 |
DCM_Matrix[0][0] = 1; |
379 |
DCM_Matrix[0][1] = 0; |
380 |
DCM_Matrix[0][2] = 0; |
381 |
DCM_Matrix[1][0] = 0; |
382 |
DCM_Matrix[1][1] = 1; |
383 |
DCM_Matrix[1][2] = 0; |
384 |
DCM_Matrix[2][0] = 0; |
385 |
DCM_Matrix[2][1] = 0; |
386 |
DCM_Matrix[2][2] = 1; |
387 |
Update_Matrix[0][0] = 0; |
388 |
Update_Matrix[0][1] = 1; |
389 |
Update_Matrix[0][2] = 2; |
390 |
Update_Matrix[1][0] = 3; |
391 |
Update_Matrix[1][1] = 4; |
392 |
Update_Matrix[1][2] = 5; |
393 |
Update_Matrix[2][0] = 6; |
394 |
Update_Matrix[2][1] = 7; |
395 |
Update_Matrix[2][2] = 8; |
396 |
Temporary_Matrix[0][0] = 0; |
397 |
Temporary_Matrix[0][1] = 0; |
398 |
Temporary_Matrix[0][2] = 0; |
399 |
Temporary_Matrix[1][0] = 0; |
400 |
Temporary_Matrix[1][1] = 0; |
401 |
Temporary_Matrix[1][2] = 0; |
402 |
Temporary_Matrix[2][0] = 0; |
403 |
Temporary_Matrix[2][1] = 0; |
404 |
Temporary_Matrix[2][2] = 0; |
405 |
errorCourse = 0;
|
406 |
COGX = 0; //Course overground X axis |
407 |
COGY = 1; //Course overground Y axis |
408 |
dt = 0;
|
409 |
Gyro_Gain_X = gyro.getScaleFactor() * 0.0174532925; // convert to rad/sec |
410 |
Gyro_Gain_Y = gyro.getScaleFactor() * 0.0174532925; |
411 |
Gyro_Gain_Z = gyro.getScaleFactor() * 0.0174532925; |
412 |
type = DCM; |
413 |
#ifdef ArduCopter
|
414 |
Kp_ROLLPITCH = 0.025; |
415 |
Ki_ROLLPITCH = 0.00000015; |
416 |
#endif
|
417 |
#ifdef AeroQuadMega_Wii
|
418 |
Kp_ROLLPITCH = 0.060; |
419 |
Ki_ROLLPITCH = 0.0000005; |
420 |
#endif
|
421 |
#ifdef AeroQuadMega_v1
|
422 |
Kp_ROLLPITCH = 0.3423; |
423 |
Ki_ROLLPITCH = 0.00234; |
424 |
#endif
|
425 |
#if !defined(ArduCopter) & !defined(AeroQuadMega_Wii) & !defined(AeroQuadMega_v1)
|
426 |
Kp_ROLLPITCH = 0.010; |
427 |
Ki_ROLLPITCH = 0.0000005; |
428 |
#endif
|
429 |
} |
430 |
|
431 |
void calculate(void) { |
432 |
Matrix_update(); |
433 |
Normalize(); |
434 |
Drift_correction(); |
435 |
Euler_angles(); |
436 |
} |
437 |
|
438 |
float getGyroAngle(byte axis) {
|
439 |
if (axis == ROLL)
|
440 |
return degrees(Omega[1]); |
441 |
if (axis == PITCH)
|
442 |
return degrees(-Omega[0]); |
443 |
} |
444 |
}; |
445 |
|
446 |
/******************************************************/
|
447 |
/*********************** IMU **************************/
|
448 |
/******************************************************/
|
449 |
// Written by Sebastian O.H. Madgwick
|
450 |
// http://code.google.com/p/imumargalgorithm30042010sohm/
|
451 |
|
452 |
// Do not use, experimental code
|
453 |
|
454 |
class FlightAngle_IMU : public FlightAngle { |
455 |
private:
|
456 |
// System constants
|
457 |
#define gyroMeasError 3.14159265358979f * (75.0f / 180.0f) // gyroscope measurement error in rad/s (shown as 5 deg/s) |
458 |
#define beta sqrt(3.0f / 4.0f) * gyroMeasError // compute beta |
459 |
float SEq_1, SEq_2, SEq_3, SEq_4; // estimated orientation quaternion elements with initial conditions |
460 |
|
461 |
void filterUpdate(float w_x, float w_y, float w_z, float a_x, float a_y, float a_z) { |
462 |
// Local system variables
|
463 |
float norm; // vector norm |
464 |
float SEqDot_omega_1, SEqDot_omega_2, SEqDot_omega_3, SEqDot_omega_4; // quaternion derrivative from gyroscopes elements |
465 |
float f_1, f_2, f_3; // objective function elements |
466 |
float J_11or24, J_12or23, J_13or22, J_14or21, J_32, J_33; // objective function Jacobian elements |
467 |
float SEqHatDot_1, SEqHatDot_2, SEqHatDot_3, SEqHatDot_4; // estimated direction of the gyroscope error |
468 |
// Axulirary variables to avoid repeated calcualtions
|
469 |
float halfSEq_1 = 0.5f * SEq_1; |
470 |
float halfSEq_2 = 0.5f * SEq_2; |
471 |
float halfSEq_3 = 0.5f * SEq_3; |
472 |
float halfSEq_4 = 0.5f * SEq_4; |
473 |
float twoSEq_1 = 2.0f * SEq_1; |
474 |
float twoSEq_2 = 2.0f * SEq_2; |
475 |
float twoSEq_3 = 2.0f * SEq_3; |
476 |
// Normalise the accelerometer measurement
|
477 |
norm = sqrt(a_x * a_x + a_y * a_y + a_z * a_z); |
478 |
a_x /= norm; |
479 |
a_y /= norm; |
480 |
a_z /= norm; |
481 |
// Compute the objective function and Jacobian
|
482 |
f_1 = twoSEq_2 * SEq_4 - twoSEq_1 * SEq_3 - a_x; |
483 |
f_2 = twoSEq_1 * SEq_2 + twoSEq_3 * SEq_4 - a_y; |
484 |
f_3 = 1.0f - twoSEq_2 * SEq_2 - twoSEq_3 * SEq_3 - a_z; |
485 |
J_11or24 = twoSEq_3; // J_11 negated in matrix multiplication
|
486 |
J_12or23 = 2.0f * SEq_4; |
487 |
J_13or22 = twoSEq_1; // J_12 negated in matrix multiplication
|
488 |
J_14or21 = twoSEq_2; |
489 |
J_32 = 2.0f * J_14or21; // negated in matrix multiplication |
490 |
J_33 = 2.0f * J_11or24; // negated in matrix multiplication |
491 |
// Compute the gradient (matrix multiplication)
|
492 |
SEqHatDot_1 = J_14or21 * f_2 - J_11or24 * f_1; |
493 |
SEqHatDot_2 = J_12or23 * f_1 + J_13or22 * f_2 - J_32 * f_3; |
494 |
SEqHatDot_3 = J_12or23 * f_2 - J_33 * f_3 - J_13or22 * f_1; |
495 |
SEqHatDot_4 = J_14or21 * f_1 + J_11or24 * f_2; |
496 |
// Normalise the gradient
|
497 |
norm = sqrt(SEqHatDot_1 * SEqHatDot_1 + SEqHatDot_2 * SEqHatDot_2 + SEqHatDot_3 * SEqHatDot_3 + SEqHatDot_4 * SEqHatDot_4); |
498 |
SEqHatDot_1 /= norm; |
499 |
SEqHatDot_2 /= norm; |
500 |
SEqHatDot_3 /= norm; |
501 |
SEqHatDot_4 /= norm; |
502 |
// Compute the quaternion derrivative measured by gyroscopes
|
503 |
SEqDot_omega_1 = -halfSEq_2 * w_x - halfSEq_3 * w_y - halfSEq_4 * w_z; |
504 |
SEqDot_omega_2 = halfSEq_1 * w_x + halfSEq_3 * w_z - halfSEq_4 * w_y; |
505 |
SEqDot_omega_3 = halfSEq_1 * w_y - halfSEq_2 * w_z + halfSEq_4 * w_x; |
506 |
SEqDot_omega_4 = halfSEq_1 * w_z + halfSEq_2 * w_y - halfSEq_3 * w_x; |
507 |
// Compute then integrate the estimated quaternion derrivative
|
508 |
SEq_1 += (SEqDot_omega_1 - (beta * SEqHatDot_1)) * G_Dt; |
509 |
SEq_2 += (SEqDot_omega_2 - (beta * SEqHatDot_2)) * G_Dt; |
510 |
SEq_3 += (SEqDot_omega_3 - (beta * SEqHatDot_3)) * G_Dt; |
511 |
SEq_4 += (SEqDot_omega_4 - (beta * SEqHatDot_4)) * G_Dt; |
512 |
// Normalise quaternion
|
513 |
norm = sqrt(SEq_1 * SEq_1 + SEq_2 * SEq_2 + SEq_3 * SEq_3 + SEq_4 * SEq_4); |
514 |
SEq_1 /= norm; |
515 |
SEq_2 /= norm; |
516 |
SEq_3 /= norm; |
517 |
SEq_4 /= norm; |
518 |
} |
519 |
|
520 |
public:
|
521 |
FlightAngle_IMU() : FlightAngle() {} |
522 |
|
523 |
void initialize(void) { |
524 |
// estimated orientation quaternion elements with initial conditions
|
525 |
SEq_1 = 1.0f; // w |
526 |
SEq_2 = 0.0f; // x |
527 |
SEq_3 = 0.0f; // y |
528 |
SEq_4 = 0.0f; // z |
529 |
} |
530 |
|
531 |
void calculate(void) { |
532 |
filterUpdate(gyro.rateRadPerSec(ROLL), gyro.rateRadPerSec(PITCH), gyro.rateRadPerSec(YAW), accel.getRaw(XAXIS), accel.getRaw(YAXIS), accel.getRaw(ZAXIS)); |
533 |
angle[ROLL] = degrees(-asin((2 * SEq_2 * SEq_4) + (2 * SEq_1 * SEq_3))); |
534 |
angle[PITCH] = degrees(atan2((2 * SEq_3 * SEq_4) - (2 *SEq_1 * SEq_2), (2 * SEq_1 * SEq_1) + (2 *SEq_4 * SEq_4) - 1)); |
535 |
angle[YAW] = degrees(atan2((2 * SEq_2 * SEq_3) - (2 * SEq_1 * SEq_4), (2 * SEq_1 * SEq_1) + (2 * SEq_2 * SEq_2) -1)); |
536 |
} |
537 |
|
538 |
float getGyroAngle(byte axis) {
|
539 |
gyroAngle[axis] += gyro.rateDegPerSec(axis) * G_Dt; |
540 |
} |
541 |
}; |
542 |
|
543 |
// ***********************************************************************
|
544 |
// ********************* MultiWii Kalman Filter **************************
|
545 |
// ***********************************************************************
|
546 |
// Original code by Alex at: http://radio-commande.com/international/triwiicopter-design/
|
547 |
// ************************************
|
548 |
// simplified IMU based on Kalman Filter
|
549 |
// inspired from http://starlino.com/imu_guide.html
|
550 |
// and http://www.starlino.com/imu_kalman_arduino.html
|
551 |
// with this algorithm, we can get absolute angles for a stable mode integration
|
552 |
// ************************************
|
553 |
|
554 |
class FlightAngle_MultiWii : public FlightAngle { |
555 |
private:
|
556 |
int8_t signRzGyro; |
557 |
float R;
|
558 |
float RxEst; // init acc in stable mode |
559 |
float RyEst;
|
560 |
float RzEst;
|
561 |
float Axz,Ayz; //angles between projection of R on XZ/YZ plane and Z axis (in Radian) |
562 |
float RxAcc,RyAcc,RzAcc; //projection of normalized gravitation force vector on x/y/z axis, as measured by accelerometer |
563 |
float RxGyro,RyGyro,RzGyro; //R obtained from last estimated value and gyro movement |
564 |
float wGyro; // gyro weight/smooting factor |
565 |
float atanx,atany;
|
566 |
float gyroFactor;
|
567 |
//float meanTime; // **** Need to update this ***
|
568 |
|
569 |
public:
|
570 |
FlightAngle_MultiWii() : FlightAngle() { |
571 |
RxEst = 0; // init acc in stable mode |
572 |
RyEst = 0;
|
573 |
RzEst = 1;
|
574 |
wGyro = 50.0f; // gyro weight/smooting factor |
575 |
} |
576 |
|
577 |
// ***********************************************************
|
578 |
// Define all the virtual functions declared in the main class
|
579 |
// ***********************************************************
|
580 |
void initialize(void) {} |
581 |
|
582 |
void calculate(void) { |
583 |
//get accelerometer readings in g, gives us RAcc vector
|
584 |
RxAcc = accel.getRaw(ROLL); |
585 |
RyAcc = accel.getRaw(PITCH); |
586 |
RzAcc = accel.getRaw(YAW); |
587 |
|
588 |
//normalize vector (convert to a vector with same direction and with length 1)
|
589 |
R = sqrt(square(RxAcc) + square(RyAcc) + square(RzAcc)); |
590 |
RxAcc /= R; |
591 |
RyAcc /= R; |
592 |
RzAcc /= R; |
593 |
|
594 |
gyroFactor = G_Dt/83e6; //empirical, depends on WMP on IDG datasheet, tied of deg/ms sensibility |
595 |
|
596 |
//evaluate R Gyro vector
|
597 |
if(abs(RzEst) < 0.1f) { |
598 |
//Rz is too small and because it is used as reference for computing Axz, Ayz it's error fluctuations will amplify leading to bad results
|
599 |
//in this case skip the gyro data and just use previous estimate
|
600 |
RxGyro = RxEst; |
601 |
RyGyro = RyEst; |
602 |
RzGyro = RzEst; |
603 |
} |
604 |
else {
|
605 |
//get angles between projection of R on ZX/ZY plane and Z axis, based on last REst
|
606 |
//Convert ADC value for to physical units
|
607 |
//For gyro it will return deg/ms (rate of rotation)
|
608 |
atanx = atan2(RxEst,RzEst); |
609 |
atany = atan2(RyEst,RzEst); |
610 |
|
611 |
Axz = atanx + gyro.getRaw(ROLL) * gyroFactor; // convert ADC value for to physical units
|
612 |
Ayz = atany + gyro.getRaw(PITCH) * gyroFactor; // and get updated angle according to gyro movement
|
613 |
|
614 |
//estimate sign of RzGyro by looking in what qudrant the angle Axz is,
|
615 |
signRzGyro = ( cos(Axz) >=0 ) ? 1 : -1; |
616 |
|
617 |
//reverse calculation of RwGyro from Awz angles, for formulas deductions see http://starlino.com/imu_guide.html
|
618 |
RxGyro = sin(Axz) / sqrt( 1 + square(cos(Axz)) * square(tan(Ayz)) );
|
619 |
RyGyro = sin(Ayz) / sqrt( 1 + square(cos(Ayz)) * square(tan(Axz)) );
|
620 |
RzGyro = signRzGyro * sqrt(1 - square(RxGyro) - square(RyGyro));
|
621 |
} |
622 |
|
623 |
//combine Accelerometer and gyro readings
|
624 |
RxEst = (RxAcc + wGyro* RxGyro) / (1.0 + wGyro); |
625 |
RyEst = (RyAcc + wGyro* RyGyro) / (1.0 + wGyro); |
626 |
RzEst = (RzAcc + wGyro* RzGyro) / (1.0 + wGyro); |
627 |
|
628 |
angle[ROLL] = 180/PI * Axz;
|
629 |
angle[PITCH] = 180/PI * Ayz;
|
630 |
} |
631 |
|
632 |
float getGyroAngle(byte axis) {
|
633 |
gyroAngle[axis] += gyro.rateDegPerSec(axis) * G_Dt; |
634 |
} |
635 |
}; |
636 |
|