Statistics
| Branch: | Revision:

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