Matthias Grob / Mbed 2 deprecated FlyBed1

NOT FINISHED YET!!! My first try to get a self built fully working Quadrocopter based on an mbed, a self built frame and some other more or less cheap parts.

Dependencies:   mbed MODI2C

IMU_Filter/IMU_Filter.cpp@30:021e13b62575, 2013-02-10 (annotated)

Committer:
maetugr
Date:
Sun Feb 10 22:08:10 2013 +0000
Revision:
30:021e13b62575
Child:
31:872d8b8c7812
newest changes because of the needed faster refresh rate for the ESCs 495Hz and the new build of the same hardware in X-configuration.; ; RC angle steering not yet finished

Who changed what in which revision?

UserRevisionLine numberNew contents of line
maetugr 30:021e13b62575 1 #include "IMU_Filter.h"
maetugr 30:021e13b62575 2
maetugr 30:021e13b62575 3 IMU_Filter::IMU_Filter()
maetugr 30:021e13b62575 4 {
maetugr 30:021e13b62575 5 for(int i=0; i<3; i++)
maetugr 30:021e13b62575 6 angle[i]=0;
maetugr 30:021e13b62575 7 }
maetugr 30:021e13b62575 8
maetugr 30:021e13b62575 9 void IMU_Filter::compute(unsigned long dt, const float * Gyro_data, const int * Acc_data)
maetugr 30:021e13b62575 10 {
maetugr 30:021e13b62575 11 // calculate angles for each sensor
maetugr 30:021e13b62575 12 for(int i = 0; i < 3; i++)
maetugr 30:021e13b62575 13 d_Gyro_angle[i] = Gyro_data[i] *dt/15000000.0;
maetugr 30:021e13b62575 14 get_Acc_angle(Acc_data);
maetugr 30:021e13b62575 15
maetugr 30:021e13b62575 16 // Complementary Filter
maetugr 30:021e13b62575 17 #if 1 // (formula from http://diydrones.com/m/discussion?id=705844%3ATopic%3A669858)
maetugr 30:021e13b62575 18 angle[0] = (0.999*(angle[0] + d_Gyro_angle[0]))+(0.001*(Acc_angle[0]));
maetugr 30:021e13b62575 19 angle[1] = (0.999*(angle[1] + d_Gyro_angle[1]))+(0.001*(Acc_angle[1] + 3)); // TODO Offset accelerometer einstellen
maetugr 30:021e13b62575 20 angle[2] += d_Gyro_angle[2]; // gyro only here TODO: Compass + 3D
maetugr 30:021e13b62575 21 #endif
maetugr 30:021e13b62575 22
maetugr 30:021e13b62575 23 #if 0 // alte berechnung, vielleicht Accelerometer zu stark gewichtet
maetugr 30:021e13b62575 24 angle[0] += (Acc.angle[0] - angle[0])/50 + d_Gyro_angle[0];
maetugr 30:021e13b62575 25 angle[1] += (Acc.angle[1] - angle[1])/50 + d_Gyro_angle[1];// TODO Offset accelerometer einstellen
maetugr 30:021e13b62575 26 //tempangle += (Comp.get_angle() - tempangle)/50 + Gyro.data[2] *dt/15000000.0;
maetugr 30:021e13b62575 27 angle[2] = Gyro_angle[2]; // gyro only here
maetugr 30:021e13b62575 28 #endif
maetugr 30:021e13b62575 29
maetugr 30:021e13b62575 30 #if 0 // neuer Test 2 (funktioniert wahrscheinlich nicht, denkfehler)
maetugr 30:021e13b62575 31 angle[0] += Gyro_angle[0] * 0.98 + Acc.angle[0] * 0.02;
maetugr 30:021e13b62575 32 angle[1] += Gyro_angle[1] * 0.98 + (Acc.angle[1] + 3) * 0.02; // TODO: Calibrierung Acc
maetugr 30:021e13b62575 33 angle[2] = Gyro_angle[2]; // gyro only here
maetugr 30:021e13b62575 34 #endif
maetugr 30:021e13b62575 35
maetugr 30:021e13b62575 36 #if 0 // all gyro only
maetugr 30:021e13b62575 37 for(int i = 0; i < 3; i++)
maetugr 30:021e13b62575 38 angle[i] += d_Gyro_angle[i];
maetugr 30:021e13b62575 39 #endif
maetugr 30:021e13b62575 40
maetugr 30:021e13b62575 41
maetugr 30:021e13b62575 42 }
maetugr 30:021e13b62575 43
maetugr 30:021e13b62575 44 void IMU_Filter::get_Acc_angle(const int * Acc_data)
maetugr 30:021e13b62575 45 {
maetugr 30:021e13b62575 46 // calculate the angles for roll and pitch (0,1)
maetugr 30:021e13b62575 47 float R = sqrt(pow((float)Acc_data[0],2) + pow((float)Acc_data[1],2) + pow((float)Acc_data[2],2));
maetugr 30:021e13b62575 48 float temp[3];
maetugr 30:021e13b62575 49
maetugr 30:021e13b62575 50 temp[0] = -(Rad2Deg * acos((float)Acc_data[1] / R)-90);
maetugr 30:021e13b62575 51 temp[1] = Rad2Deg * acos((float)Acc_data[0] / R)-90;
maetugr 30:021e13b62575 52 temp[2] = Rad2Deg * acos((float)Acc_data[2] / R);
maetugr 30:021e13b62575 53
maetugr 30:021e13b62575 54 for(int i = 0;i < 3; i++)
maetugr 30:021e13b62575 55 if (temp[i] > -360 && temp[i] < 360)
maetugr 30:021e13b62575 56 Acc_angle[i] = temp[i];
maetugr 30:021e13b62575 57 }