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
Diff: IMU/IMU_Filter.cpp
- Revision:
- 30:021e13b62575
- Parent:
- 29:8b7362a2ee14
- Child:
- 31:872d8b8c7812
--- a/IMU/IMU_Filter.cpp Sat Dec 15 08:42:36 2012 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,55 +0,0 @@
-#include "IMU_Filter.h"
-
-IMU_Filter::IMU_Filter()
-{
- for(int i=0; i<3; i++)
- angle[i]=0;
-}
-
-void IMU_Filter::compute(unsigned long dt, const float * Gyro_data, const int * Acc_data)
-{
- // calculate angles for each sensor
- for(int i = 0; i < 3; i++)
- d_Gyro_angle[i] = Gyro_data[i] *dt/15000000.0;
- get_Acc_angle(Acc_data);
-
- // Complementary Filter
- #if 1 // (formula from http://diydrones.com/m/discussion?id=705844%3ATopic%3A669858)
- angle[0] = (0.99*(angle[0] + d_Gyro_angle[0]))+(0.01*(Acc_angle[0]));
- angle[1] = (0.99*(angle[1] + d_Gyro_angle[1]))+(0.01*(Acc_angle[1] + 3)); // TODO Offset accelerometer einstellen
- angle[2] += d_Gyro_angle[2]; // gyro only here TODO: Compass + 3D
- #endif
-
- #if 0 // alte berechnung, vielleicht Accelerometer zu stark gewichtet
- angle[0] += (Acc.angle[0] - angle[0])/50 + d_Gyro_angle[0];
- angle[1] += (Acc.angle[1] - angle[1])/50 + d_Gyro_angle[1];// TODO Offset accelerometer einstellen
- //tempangle += (Comp.get_angle() - tempangle)/50 + Gyro.data[2] *dt/15000000.0;
- angle[2] = Gyro_angle[2]; // gyro only here
- #endif
-
- #if 0 // neuer Test 2 (funktioniert wahrscheinlich nicht, denkfehler)
- angle[0] += Gyro_angle[0] * 0.98 + Acc.angle[0] * 0.02;
- angle[1] += Gyro_angle[1] * 0.98 + (Acc.angle[1] + 3) * 0.02; // TODO: Calibrierung Acc
- angle[2] = Gyro_angle[2]; // gyro only here
- #endif
-
- #if 0 // all gyro only
- for(int i = 0; i < 3; i++)
- angle[i] += d_Gyro_angle[i];
- #endif
-}
-
-void IMU_Filter::get_Acc_angle(const int * Acc_data)
-{
- // calculate the angles for roll and pitch (0,1)
- float R = sqrt(pow((float)Acc_data[0],2) + pow((float)Acc_data[1],2) + pow((float)Acc_data[2],2));
- float temp[3];
-
- temp[0] = -(Rad2Deg * acos((float)Acc_data[1] / R)-90);
- temp[1] = Rad2Deg * acos((float)Acc_data[0] / R)-90;
- temp[2] = Rad2Deg * acos((float)Acc_data[2] / R);
-
- for(int i = 0;i < 3; i++)
- if (temp[i] > -360 && temp[i] < 360)
- Acc_angle[i] = temp[i];
-}
\ No newline at end of file
Matthias Grob
