An fully working IMU-Filter and Sensor drivers for the 10DOF-Board over I2C. All in one simple class. Include, calibrate sensors, call read, get angles. (3D Visualisation code for Python also included) Sensors: L3G4200D, ADXL345, HMC5883, BMP085
IMU/IMU_Filter/IMU_Filter.cpp@4:f62337b907e5, 2013-08-29 (annotated)
- Committer:
- maetugr
- Date:
- Thu Aug 29 13:52:30 2013 +0000
- Revision:
- 4:f62337b907e5
- Parent:
- 0:3e7450f1a938
The Altitude Sensor is now implemented, it's really 10DOF now ;); TODO: Autocalibration
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
maetugr | 0:3e7450f1a938 | 1 | #include "IMU_Filter.h" |
maetugr | 0:3e7450f1a938 | 2 | |
maetugr | 0:3e7450f1a938 | 3 | // IMU/AHRS |
maetugr | 0:3e7450f1a938 | 4 | #define PI 3.1415926535897932384626433832795 |
maetugr | 0:3e7450f1a938 | 5 | #define Kp 2.0f // proportional gain governs rate of convergence to accelerometer/magnetometer |
maetugr | 0:3e7450f1a938 | 6 | #define Ki 0.005f // integral gain governs rate of convergence of gyroscope biases |
maetugr | 0:3e7450f1a938 | 7 | |
maetugr | 0:3e7450f1a938 | 8 | IMU_Filter::IMU_Filter() |
maetugr | 0:3e7450f1a938 | 9 | { |
maetugr | 0:3e7450f1a938 | 10 | for(int i=0; i<3; i++) |
maetugr | 0:3e7450f1a938 | 11 | angle[i]=0; |
maetugr | 0:3e7450f1a938 | 12 | |
maetugr | 0:3e7450f1a938 | 13 | // IMU/AHRS |
maetugr | 0:3e7450f1a938 | 14 | q0 = 1; q1 = 0; q2 = 0; q3 = 0; |
maetugr | 0:3e7450f1a938 | 15 | exInt = 0; eyInt = 0; ezInt = 0; |
maetugr | 0:3e7450f1a938 | 16 | } |
maetugr | 0:3e7450f1a938 | 17 | |
maetugr | 0:3e7450f1a938 | 18 | void IMU_Filter::compute(float dt, const float * Gyro_data, const float * Acc_data, const float * Comp_data) |
maetugr | 0:3e7450f1a938 | 19 | { |
maetugr | 0:3e7450f1a938 | 20 | // IMU/AHRS (from http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/) |
maetugr | 0:3e7450f1a938 | 21 | |
maetugr | 0:3e7450f1a938 | 22 | float radGyro[3]; // Gyro in radians per second |
maetugr | 0:3e7450f1a938 | 23 | for(int i=0; i<3; i++) |
maetugr | 0:3e7450f1a938 | 24 | radGyro[i] = Gyro_data[i] * PI / 180; |
maetugr | 0:3e7450f1a938 | 25 | |
maetugr | 0:3e7450f1a938 | 26 | //IMUupdate(dt/2, radGyro[0], radGyro[1], radGyro[2], Acc_data[0], Acc_data[1], Acc_data[2]); |
maetugr | 0:3e7450f1a938 | 27 | AHRSupdate(dt/2, radGyro[0], radGyro[1], radGyro[2], Acc_data[0], Acc_data[1], Acc_data[2], Comp_data[0], Comp_data[1], Comp_data[2]); |
maetugr | 0:3e7450f1a938 | 28 | |
maetugr | 0:3e7450f1a938 | 29 | float rangle[3]; // calculate angles in radians from quternion output, formula from Wiki (http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles) |
maetugr | 0:3e7450f1a938 | 30 | rangle[0] = atan2(2*q0*q1 + 2*q2*q3, 1 - 2*(q1*q1 + q2*q2)); |
maetugr | 0:3e7450f1a938 | 31 | rangle[1] = asin(2*q0*q2 - 2*q3*q1); |
maetugr | 0:3e7450f1a938 | 32 | rangle[2] = atan2(2*q0*q3 + 2*q1*q2, 1 - 2*(q2*q2 + q3*q3)); |
maetugr | 0:3e7450f1a938 | 33 | |
maetugr | 0:3e7450f1a938 | 34 | // TODO |
maetugr | 0:3e7450f1a938 | 35 | // Pitch should have a range of +/-90 degrees. |
maetugr | 0:3e7450f1a938 | 36 | // After you pitch past vertical (90 degrees) your roll and yaw value should swing 180 degrees. |
maetugr | 0:3e7450f1a938 | 37 | // A pitch value of 100 degrees is measured as a pitch of 80 degrees and inverted flight (roll = 180 degrees). |
maetugr | 0:3e7450f1a938 | 38 | // 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 | 0:3e7450f1a938 | 39 | // And I think this solves the upside down issue... |
maetugr | 0:3e7450f1a938 | 40 | // Handle roll reversal when inverted |
maetugr | 0:3e7450f1a938 | 41 | /*if (Acc_data[2] < 0) { |
maetugr | 0:3e7450f1a938 | 42 | if (Acc_data[0] < 0) { |
maetugr | 0:3e7450f1a938 | 43 | rangle[1] = (180 - rangle[1]); |
maetugr | 0:3e7450f1a938 | 44 | } else { |
maetugr | 0:3e7450f1a938 | 45 | rangle[1] = (-180 - rangle[1]); |
maetugr | 0:3e7450f1a938 | 46 | } |
maetugr | 0:3e7450f1a938 | 47 | }*/ |
maetugr | 0:3e7450f1a938 | 48 | |
maetugr | 0:3e7450f1a938 | 49 | for(int i=0; i<3; i++) // angle in degree |
maetugr | 0:3e7450f1a938 | 50 | angle[i] = rangle[i] * 180 / PI; |
maetugr | 0:3e7450f1a938 | 51 | } |
maetugr | 0:3e7450f1a938 | 52 | |
maetugr | 0:3e7450f1a938 | 53 | //------------------------------------------------------------------------------------------------------------------ |
maetugr | 4:f62337b907e5 | 54 | // Code copied from S.O.H. Madgwick (File AHRS.c from https://code.google.com/p/imumargalgorithm30042010sohm/) |
maetugr | 0:3e7450f1a938 | 55 | //------------------------------------------------------------------------------------------------------------------ |
maetugr | 0:3e7450f1a938 | 56 | |
maetugr | 0:3e7450f1a938 | 57 | // IMU |
maetugr | 0:3e7450f1a938 | 58 | void IMU_Filter::IMUupdate(float halfT, float gx, float gy, float gz, float ax, float ay, float az) { |
maetugr | 0:3e7450f1a938 | 59 | float norm; |
maetugr | 0:3e7450f1a938 | 60 | float vx, vy, vz; |
maetugr | 0:3e7450f1a938 | 61 | float ex, ey, ez; |
maetugr | 0:3e7450f1a938 | 62 | |
maetugr | 0:3e7450f1a938 | 63 | // normalise the measurements |
maetugr | 0:3e7450f1a938 | 64 | norm = sqrt(ax*ax + ay*ay + az*az); |
maetugr | 0:3e7450f1a938 | 65 | if(norm == 0.0f) return; |
maetugr | 0:3e7450f1a938 | 66 | ax = ax / norm; |
maetugr | 0:3e7450f1a938 | 67 | ay = ay / norm; |
maetugr | 0:3e7450f1a938 | 68 | az = az / norm; |
maetugr | 0:3e7450f1a938 | 69 | |
maetugr | 0:3e7450f1a938 | 70 | // estimated direction of gravity |
maetugr | 0:3e7450f1a938 | 71 | vx = 2*(q1*q3 - q0*q2); |
maetugr | 0:3e7450f1a938 | 72 | vy = 2*(q0*q1 + q2*q3); |
maetugr | 0:3e7450f1a938 | 73 | vz = q0*q0 - q1*q1 - q2*q2 + q3*q3; |
maetugr | 0:3e7450f1a938 | 74 | |
maetugr | 0:3e7450f1a938 | 75 | // error is sum of cross product between reference direction of field and direction measured by sensor |
maetugr | 0:3e7450f1a938 | 76 | ex = (ay*vz - az*vy); |
maetugr | 0:3e7450f1a938 | 77 | ey = (az*vx - ax*vz); |
maetugr | 0:3e7450f1a938 | 78 | ez = (ax*vy - ay*vx); |
maetugr | 0:3e7450f1a938 | 79 | |
maetugr | 0:3e7450f1a938 | 80 | // integral error scaled integral gain |
maetugr | 0:3e7450f1a938 | 81 | exInt = exInt + ex*Ki; |
maetugr | 0:3e7450f1a938 | 82 | eyInt = eyInt + ey*Ki; |
maetugr | 0:3e7450f1a938 | 83 | ezInt = ezInt + ez*Ki; |
maetugr | 0:3e7450f1a938 | 84 | |
maetugr | 0:3e7450f1a938 | 85 | // adjusted gyroscope measurements |
maetugr | 0:3e7450f1a938 | 86 | gx = gx + Kp*ex + exInt; |
maetugr | 0:3e7450f1a938 | 87 | gy = gy + Kp*ey + eyInt; |
maetugr | 0:3e7450f1a938 | 88 | gz = gz + Kp*ez + ezInt; |
maetugr | 0:3e7450f1a938 | 89 | |
maetugr | 0:3e7450f1a938 | 90 | // integrate quaternion rate and normalise |
maetugr | 0:3e7450f1a938 | 91 | q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT; |
maetugr | 0:3e7450f1a938 | 92 | q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT; |
maetugr | 0:3e7450f1a938 | 93 | q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT; |
maetugr | 0:3e7450f1a938 | 94 | q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT; |
maetugr | 0:3e7450f1a938 | 95 | |
maetugr | 0:3e7450f1a938 | 96 | // normalise quaternion |
maetugr | 0:3e7450f1a938 | 97 | norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3); |
maetugr | 0:3e7450f1a938 | 98 | q0 = q0 / norm; |
maetugr | 0:3e7450f1a938 | 99 | q1 = q1 / norm; |
maetugr | 0:3e7450f1a938 | 100 | q2 = q2 / norm; |
maetugr | 0:3e7450f1a938 | 101 | q3 = q3 / norm; |
maetugr | 0:3e7450f1a938 | 102 | } |
maetugr | 0:3e7450f1a938 | 103 | |
maetugr | 0:3e7450f1a938 | 104 | // AHRS |
maetugr | 0:3e7450f1a938 | 105 | void IMU_Filter::AHRSupdate(float halfT, float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) { |
maetugr | 0:3e7450f1a938 | 106 | float norm; |
maetugr | 0:3e7450f1a938 | 107 | float hx, hy, hz, bx, bz; |
maetugr | 0:3e7450f1a938 | 108 | float vx, vy, vz, wx, wy, wz; |
maetugr | 0:3e7450f1a938 | 109 | float ex, ey, ez; |
maetugr | 0:3e7450f1a938 | 110 | |
maetugr | 0:3e7450f1a938 | 111 | // auxiliary variables to reduce number of repeated operations |
maetugr | 0:3e7450f1a938 | 112 | float q0q0 = q0*q0; |
maetugr | 0:3e7450f1a938 | 113 | float q0q1 = q0*q1; |
maetugr | 0:3e7450f1a938 | 114 | float q0q2 = q0*q2; |
maetugr | 0:3e7450f1a938 | 115 | float q0q3 = q0*q3; |
maetugr | 0:3e7450f1a938 | 116 | float q1q1 = q1*q1; |
maetugr | 0:3e7450f1a938 | 117 | float q1q2 = q1*q2; |
maetugr | 0:3e7450f1a938 | 118 | float q1q3 = q1*q3; |
maetugr | 0:3e7450f1a938 | 119 | float q2q2 = q2*q2; |
maetugr | 0:3e7450f1a938 | 120 | float q2q3 = q2*q3; |
maetugr | 0:3e7450f1a938 | 121 | float q3q3 = q3*q3; |
maetugr | 0:3e7450f1a938 | 122 | |
maetugr | 0:3e7450f1a938 | 123 | // normalise the measurements |
maetugr | 0:3e7450f1a938 | 124 | norm = sqrt(ax*ax + ay*ay + az*az); |
maetugr | 0:3e7450f1a938 | 125 | if(norm == 0.0f) return; |
maetugr | 0:3e7450f1a938 | 126 | ax = ax / norm; |
maetugr | 0:3e7450f1a938 | 127 | ay = ay / norm; |
maetugr | 0:3e7450f1a938 | 128 | az = az / norm; |
maetugr | 0:3e7450f1a938 | 129 | norm = sqrt(mx*mx + my*my + mz*mz); |
maetugr | 0:3e7450f1a938 | 130 | if(norm == 0.0f) return; |
maetugr | 0:3e7450f1a938 | 131 | mx = mx / norm; |
maetugr | 0:3e7450f1a938 | 132 | my = my / norm; |
maetugr | 0:3e7450f1a938 | 133 | mz = mz / norm; |
maetugr | 0:3e7450f1a938 | 134 | |
maetugr | 0:3e7450f1a938 | 135 | // compute reference direction of flux |
maetugr | 0:3e7450f1a938 | 136 | hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2); |
maetugr | 0:3e7450f1a938 | 137 | hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1); |
maetugr | 0:3e7450f1a938 | 138 | hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2); |
maetugr | 0:3e7450f1a938 | 139 | bx = sqrt((hx*hx) + (hy*hy)); |
maetugr | 0:3e7450f1a938 | 140 | bz = hz; |
maetugr | 0:3e7450f1a938 | 141 | |
maetugr | 0:3e7450f1a938 | 142 | // estimated direction of gravity and flux (v and w) |
maetugr | 0:3e7450f1a938 | 143 | vx = 2*(q1q3 - q0q2); |
maetugr | 0:3e7450f1a938 | 144 | vy = 2*(q0q1 + q2q3); |
maetugr | 0:3e7450f1a938 | 145 | vz = q0q0 - q1q1 - q2q2 + q3q3; |
maetugr | 0:3e7450f1a938 | 146 | wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2); |
maetugr | 0:3e7450f1a938 | 147 | wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3); |
maetugr | 0:3e7450f1a938 | 148 | wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2); |
maetugr | 0:3e7450f1a938 | 149 | |
maetugr | 0:3e7450f1a938 | 150 | // error is sum of cross product between reference direction of fields and direction measured by sensors |
maetugr | 0:3e7450f1a938 | 151 | ex = (ay*vz - az*vy) + (my*wz - mz*wy); |
maetugr | 0:3e7450f1a938 | 152 | ey = (az*vx - ax*vz) + (mz*wx - mx*wz); |
maetugr | 0:3e7450f1a938 | 153 | ez = (ax*vy - ay*vx) + (mx*wy - my*wx); |
maetugr | 0:3e7450f1a938 | 154 | |
maetugr | 0:3e7450f1a938 | 155 | // integral error scaled integral gain |
maetugr | 0:3e7450f1a938 | 156 | exInt = exInt + ex*Ki; |
maetugr | 0:3e7450f1a938 | 157 | eyInt = eyInt + ey*Ki; |
maetugr | 0:3e7450f1a938 | 158 | ezInt = ezInt + ez*Ki; |
maetugr | 0:3e7450f1a938 | 159 | |
maetugr | 0:3e7450f1a938 | 160 | // adjusted gyroscope measurements |
maetugr | 0:3e7450f1a938 | 161 | gx = gx + Kp*ex + exInt; |
maetugr | 0:3e7450f1a938 | 162 | gy = gy + Kp*ey + eyInt; |
maetugr | 0:3e7450f1a938 | 163 | gz = gz + Kp*ez + ezInt; |
maetugr | 0:3e7450f1a938 | 164 | |
maetugr | 0:3e7450f1a938 | 165 | // integrate quaternion rate and normalise |
maetugr | 0:3e7450f1a938 | 166 | q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT; |
maetugr | 0:3e7450f1a938 | 167 | q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT; |
maetugr | 0:3e7450f1a938 | 168 | q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT; |
maetugr | 0:3e7450f1a938 | 169 | q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT; |
maetugr | 0:3e7450f1a938 | 170 | |
maetugr | 0:3e7450f1a938 | 171 | // normalise quaternion |
maetugr | 0:3e7450f1a938 | 172 | norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3); |
maetugr | 0:3e7450f1a938 | 173 | q0 = q0 / norm; |
maetugr | 0:3e7450f1a938 | 174 | q1 = q1 / norm; |
maetugr | 0:3e7450f1a938 | 175 | q2 = q2 / norm; |
maetugr | 0:3e7450f1a938 | 176 | q3 = q3 / norm; |
maetugr | 0:3e7450f1a938 | 177 | } |