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@40:2ca410923691, 2014-02-14 (annotated)

Committer:
maetugr
Date:
Fri Feb 14 14:17:32 2014 +0000
Revision:
40:2ca410923691
Parent:
37:34917f7c10ae 
now with MPU6050 before taking it too FlyBed2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
maetugr 34:3aa1cbcde59d 1 #include "IMU_Filter.h"
maetugr 34:3aa1cbcde59d 2
maetugr 34:3aa1cbcde59d 3 // MARG
maetugr 34:3aa1cbcde59d 4 #define PI 3.1415926535897932384626433832795
maetugr 34:3aa1cbcde59d 5 #define Kp 2.0f // proportional gain governs rate of convergence to accelerometer/magnetometer
maetugr 34:3aa1cbcde59d 6 #define Ki 0.005f // integral gain governs rate of convergence of gyroscope biases
maetugr 34:3aa1cbcde59d 7
maetugr 34:3aa1cbcde59d 8 IMU_Filter::IMU_Filter()
maetugr 34:3aa1cbcde59d 9 {
maetugr 34:3aa1cbcde59d 10 for(int i=0; i<3; i++)
maetugr 34:3aa1cbcde59d 11 angle[i]=0;
maetugr 34:3aa1cbcde59d 12
maetugr 34:3aa1cbcde59d 13 // MARG
maetugr 34:3aa1cbcde59d 14 q0 = 1; q1 = 0; q2 = 0; q3 = 0;
maetugr 34:3aa1cbcde59d 15 exInt = 0; eyInt = 0; ezInt = 0;
maetugr 34:3aa1cbcde59d 16 }
maetugr 34:3aa1cbcde59d 17
maetugr 34:3aa1cbcde59d 18 void IMU_Filter::compute(float dt, const float * Gyro_data, const float * Acc_data)
maetugr 34:3aa1cbcde59d 19 {
maetugr 34:3aa1cbcde59d 20 // calculate angles for each sensor
maetugr 34:3aa1cbcde59d 21 for(int i = 0; i < 3; i++)
maetugr 34:3aa1cbcde59d 22 d_Gyro_angle[i] = Gyro_data[i] *dt;
maetugr 34:3aa1cbcde59d 23 get_Acc_angle(Acc_data);
maetugr 34:3aa1cbcde59d 24
maetugr 34:3aa1cbcde59d 25 // Complementary Filter
maetugr 34:3aa1cbcde59d 26 #if 0 // (formula from http://diydrones.com/m/discussion?id=705844%3ATopic%3A669858)
maetugr 34:3aa1cbcde59d 27 angle[0] = (0.999*(angle[0] + d_Gyro_angle[0]))+(0.001*(Acc_angle[0]));
maetugr 34:3aa1cbcde59d 28 angle[1] = (0.999*(angle[1] + d_Gyro_angle[1]))+(0.001*(Acc_angle[1]));// + 3)); // TODO Offset accelerometer einstellen
maetugr 34:3aa1cbcde59d 29 angle[2] += d_Gyro_angle[2]; // gyro only here TODO: Compass + 3D
maetugr 34:3aa1cbcde59d 30 #endif
maetugr 34:3aa1cbcde59d 31
maetugr 34:3aa1cbcde59d 32 #if 0 // alte berechnung, vielleicht Accelerometer zu stark gewichtet
maetugr 34:3aa1cbcde59d 33 angle[0] += (Acc.angle[0] - angle[0])/50 + d_Gyro_angle[0];
maetugr 34:3aa1cbcde59d 34 angle[1] += (Acc.angle[1] - angle[1])/50 + d_Gyro_angle[1];// TODO Offset accelerometer einstellen
maetugr 34:3aa1cbcde59d 35 //tempangle += (Comp.get_angle() - tempangle)/50 + Gyro.data[2] *dt/15000000.0;
maetugr 34:3aa1cbcde59d 36 angle[2] = Gyro_angle[2]; // gyro only here
maetugr 34:3aa1cbcde59d 37 #endif
maetugr 34:3aa1cbcde59d 38
maetugr 34:3aa1cbcde59d 39 #if 0 // neuer Test 2 (funktioniert wahrscheinlich nicht, denkfehler)
maetugr 34:3aa1cbcde59d 40 angle[0] += Gyro_angle[0] * 0.98 + Acc.angle[0] * 0.02;
maetugr 34:3aa1cbcde59d 41 angle[1] += Gyro_angle[1] * 0.98 + (Acc.angle[1] + 3) * 0.02; // TODO: Calibrierung Acc
maetugr 34:3aa1cbcde59d 42 angle[2] = Gyro_angle[2]; // gyro only here
maetugr 34:3aa1cbcde59d 43 #endif
maetugr 34:3aa1cbcde59d 44
maetugr 34:3aa1cbcde59d 45 #if 0 // all gyro only
maetugr 34:3aa1cbcde59d 46 for(int i = 0; i < 3; i++)
maetugr 34:3aa1cbcde59d 47 angle[i] += d_Gyro_angle[i];
maetugr 34:3aa1cbcde59d 48 #endif
maetugr 34:3aa1cbcde59d 49
maetugr 34:3aa1cbcde59d 50 // MARG
maetugr 34:3aa1cbcde59d 51 #if 1 // (from http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/)
maetugr 34:3aa1cbcde59d 52 float radGyro[3];
maetugr 34:3aa1cbcde59d 53
maetugr 34:3aa1cbcde59d 54 for(int i=0; i<3; i++) // Radians per second
maetugr 34:3aa1cbcde59d 55 radGyro[i] = Gyro_data[i] * PI / 180;
maetugr 34:3aa1cbcde59d 56
maetugr 34:3aa1cbcde59d 57 IMUupdate(dt/2, radGyro[0], radGyro[1], radGyro[2], Acc_data[0], Acc_data[1], Acc_data[2]);
maetugr 34:3aa1cbcde59d 58
maetugr 34:3aa1cbcde59d 59 float rangle[3]; // calculate angles in radians from quternion output
maetugr 34:3aa1cbcde59d 60 rangle[0] = atan2(2*q0*q1 + 2*q2*q3, 1 - 2*(q1*q1 + q2*q2)); // from Wiki
maetugr 34:3aa1cbcde59d 61 rangle[1] = asin(2*q0*q2 - 2*q3*q1);
maetugr 34:3aa1cbcde59d 62 rangle[2] = atan2(2*q0*q3 + 2*q1*q2, 1 - 2*(q2*q2 + q3*q3));
maetugr 34:3aa1cbcde59d 63
maetugr 37:34917f7c10ae 64 // TODO
maetugr 37:34917f7c10ae 65 // Pitch should have a range of +/-90 degrees.
maetugr 37:34917f7c10ae 66 // After you pitch past vertical (90 degrees) your roll and yaw value should swing 180 degrees.
maetugr 37:34917f7c10ae 67 // A pitch value of 100 degrees is measured as a pitch of 80 degrees and inverted flight (roll = 180 degrees).
maetugr 37:34917f7c10ae 68 // Another example is a pitch of 180 degrees (upside down). This is measured as a level pitch (0 degrees) and a roll of 180 degrees.
maetugr 37:34917f7c10ae 69 // And I think this solves the upside down issue...
maetugr 37:34917f7c10ae 70 // Handle roll reversal when inverted
maetugr 37:34917f7c10ae 71 /*if (Acc_data[2] < 0) {
maetugr 37:34917f7c10ae 72 if (Acc_data[0] < 0) {
maetugr 37:34917f7c10ae 73 rangle[1] = (180 - rangle[1]);
maetugr 37:34917f7c10ae 74 } else {
maetugr 37:34917f7c10ae 75 rangle[1] = (-180 - rangle[1]);
maetugr 37:34917f7c10ae 76 }
maetugr 37:34917f7c10ae 77 }*/
maetugr 37:34917f7c10ae 78
maetugr 34:3aa1cbcde59d 79 for(int i=0; i<3; i++) // angle in degree
maetugr 34:3aa1cbcde59d 80 angle[i] = rangle[i] * 180 / PI;
maetugr 34:3aa1cbcde59d 81 #endif
maetugr 34:3aa1cbcde59d 82 }
maetugr 34:3aa1cbcde59d 83
maetugr 34:3aa1cbcde59d 84 void IMU_Filter::get_Acc_angle(const float * Acc_data)
maetugr 34:3aa1cbcde59d 85 {
maetugr 34:3aa1cbcde59d 86 // calculate the angles for roll and pitch (0,1)
maetugr 34:3aa1cbcde59d 87 float R = sqrt(pow((float)Acc_data[0],2) + pow((float)Acc_data[1],2) + pow((float)Acc_data[2],2));
maetugr 34:3aa1cbcde59d 88 float temp[3];
maetugr 34:3aa1cbcde59d 89
maetugr 34:3aa1cbcde59d 90 temp[0] = -(Rad2Deg * acos(Acc_data[1] / R)-90);
maetugr 34:3aa1cbcde59d 91 temp[1] = Rad2Deg * acos(Acc_data[0] / R)-90;
maetugr 34:3aa1cbcde59d 92 temp[2] = Rad2Deg * acos(Acc_data[2] / R);
maetugr 34:3aa1cbcde59d 93
maetugr 34:3aa1cbcde59d 94 for(int i = 0;i < 3; i++)
maetugr 34:3aa1cbcde59d 95 if (temp[i] > -360 && temp[i] < 360)
maetugr 34:3aa1cbcde59d 96 Acc_angle[i] = temp[i];
maetugr 34:3aa1cbcde59d 97 }
maetugr 34:3aa1cbcde59d 98
maetugr 34:3aa1cbcde59d 99 // MARG
maetugr 34:3aa1cbcde59d 100 void IMU_Filter::IMUupdate(float halfT, float gx, float gy, float gz, float ax, float ay, float az)
maetugr 34:3aa1cbcde59d 101 {
maetugr 34:3aa1cbcde59d 102 float norm;
maetugr 34:3aa1cbcde59d 103 float vx, vy, vz;
maetugr 34:3aa1cbcde59d 104 float ex, ey, ez;
maetugr 34:3aa1cbcde59d 105
maetugr 34:3aa1cbcde59d 106 // normalise the measurements
maetugr 34:3aa1cbcde59d 107 norm = sqrt(ax*ax + ay*ay + az*az);
maetugr 34:3aa1cbcde59d 108 if(norm == 0.0f) return;
maetugr 34:3aa1cbcde59d 109 ax = ax / norm;
maetugr 34:3aa1cbcde59d 110 ay = ay / norm;
maetugr 34:3aa1cbcde59d 111 az = az / norm;
maetugr 34:3aa1cbcde59d 112
maetugr 34:3aa1cbcde59d 113 // estimated direction of gravity
maetugr 34:3aa1cbcde59d 114 vx = 2*(q1*q3 - q0*q2);
maetugr 34:3aa1cbcde59d 115 vy = 2*(q0*q1 + q2*q3);
maetugr 34:3aa1cbcde59d 116 vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
maetugr 34:3aa1cbcde59d 117
maetugr 34:3aa1cbcde59d 118 // error is sum of cross product between reference direction of field and direction measured by sensor
maetugr 34:3aa1cbcde59d 119 ex = (ay*vz - az*vy);
maetugr 34:3aa1cbcde59d 120 ey = (az*vx - ax*vz);
maetugr 34:3aa1cbcde59d 121 ez = (ax*vy - ay*vx);
maetugr 34:3aa1cbcde59d 122
maetugr 34:3aa1cbcde59d 123 // integral error scaled integral gain
maetugr 34:3aa1cbcde59d 124 exInt = exInt + ex*Ki;
maetugr 34:3aa1cbcde59d 125 eyInt = eyInt + ey*Ki;
maetugr 34:3aa1cbcde59d 126 ezInt = ezInt + ez*Ki;
maetugr 34:3aa1cbcde59d 127
maetugr 34:3aa1cbcde59d 128 // adjusted gyroscope measurements
maetugr 34:3aa1cbcde59d 129 gx = gx + Kp*ex + exInt;
maetugr 34:3aa1cbcde59d 130 gy = gy + Kp*ey + eyInt;
maetugr 34:3aa1cbcde59d 131 gz = gz + Kp*ez + ezInt;
maetugr 34:3aa1cbcde59d 132
maetugr 34:3aa1cbcde59d 133 // integrate quaternion rate and normalise
maetugr 34:3aa1cbcde59d 134 q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
maetugr 34:3aa1cbcde59d 135 q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
maetugr 34:3aa1cbcde59d 136 q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
maetugr 34:3aa1cbcde59d 137 q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;
maetugr 34:3aa1cbcde59d 138
maetugr 34:3aa1cbcde59d 139 // normalise quaternion
maetugr 34:3aa1cbcde59d 140 norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
maetugr 34:3aa1cbcde59d 141 q0 = q0 / norm;
maetugr 34:3aa1cbcde59d 142 q1 = q1 / norm;
maetugr 34:3aa1cbcde59d 143 q2 = q2 / norm;
maetugr 34:3aa1cbcde59d 144 q3 = q3 / norm;
maetugr 34:3aa1cbcde59d 145 }