In this video lesson I show you how you can improve the performance of your Arduino MPU6050 IMU Project by incorporating a complimentary filter. We will combine the roll and pitch calculations from the accelerometer and gyro into a fused result which allows us to enjoy the best of both worlds.
For your convenience, the code developed is presented below:
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 | #include <Adafruit_MPU6050.h> #include <Adafruit_Sensor.h> #include <Wire.h> float Ax; float Ay; float Az; float Gx; float Gy; float Gz; float rollG=0; float pitchG=0; float yawG=0; float gyroXOffset=0; float gyroYOffset=0; float gyroZOffset=0; int tStart = millis(); // If You want Calibrated results, you must rotate // your board to all possible orientations, and // record your velues of xMax, xMin, yMax, yMin // zMax, zMin. I show you how to do that in LESSON // 79 on my youtube channel. Put in your values and then // uncomment the code below. Don't use my values, you have // to measure your own for it to work. // float xMax= 1.03; // float xMin= -.97; // float yMax= .98; // float yMin= -1.01; // float zMax=.96 ; // float zMin= -1.08; // float xOffset = (xMax+xMin)/2; // float yOffset = (yMax+yMin)/2; // float zOffset = (zMax+zMin)/2; // float xScale = 2/(xMax -xMin); // float yScale = 2/(yMax - yMin); // float zScale = 2/(zMax -zMin); float rollLP = 0; float pitchLP = 0; float rollRAW; float pitchRAW; float rollComp=0; float pitchComp=0; float deltaRoll=0; float deltaPitch=0; Adafruit_MPU6050 mpu; void calibrateGyro(){ Serial.println("Calibrating the Gyro: Keep Completely Stationary"); delay(1000); int i; float sumX = 0; float sumY = 0; float sumZ = 0; int numPoints=100; for (i=0;i<numPoints;i=i+1){ sensors_event_t a, g, temp; mpu.getEvent(&a, &g, &temp); sumX = sumX + g.gyro.x; sumY = sumY + g.gyro.y; sumZ = sumZ + g.gyro.z; delay(10); } gyroXOffset = sumX/numPoints; gyroYOffset = sumY/numPoints; gyroZOffset = sumZ/numPoints; } void setup() { // put your setup code here, to run once: Serial.begin(115200); mpu.begin(); Serial.println("MPU6050 Started!"); mpu.setAccelerometerRange(MPU6050_RANGE_2_G); mpu.setGyroRange(MPU6050_RANGE_2000_DEG); mpu.setFilterBandwidth(MPU6050_BAND_21_HZ); calibrateGyro(); tStart = millis(); } void loop() { // put your main code here, to run repeatedly: sensors_event_t a, g, temp; mpu.getEvent(&a, &g, &temp); Ax = a.acceleration.x / 9.81; Ay = a.acceleration.y / 9.81; Az = a.acceleration.z / 9.81; Gx = g.gyro.x-gyroXOffset; Gy = -(g.gyro.y-gyroYOffset); Gz = g.gyro.z-gyroZOffset; rollG = rollG + (millis() - tStart)/1000. * Gy *360./2./3.14; pitchG = pitchG + (millis() - tStart)/1000. * Gx *360./2./3.14; yawG = yawG + (millis() - tStart)/1000. * Gz *360./2./3.14; deltaRoll=(millis() - tStart)/1000. * Gy *360./2./3.14; deltaPitch=(millis() - tStart)/1000. * Gx *360./2./3.14; tStart = millis(); //Uncomment these lines of code to do the callibration //This will only work if you have measured your max //and min values, and put them in at the top of the code // Ax = xScale*(Ax-xOffset); // Ay = yScale*(Ay-yOffset); // Az = zScale*(Az-zOffset); pitchRAW = atan2(Ay, sqrt(Az * Az + Ax * Ax)) * 360 / (2 * 3.14); rollRAW = atan2(Ax, sqrt(Az * Az + Ay * Ay)) * 360 / (2 * 3.14); pitchLP = .75 * pitchLP + .25 * pitchRAW; rollLP = .75 * rollLP + .25 * rollRAW; rollComp= .1*rollRAW + .9*(rollComp + deltaRoll); pitchComp= .1*pitchRAW +.9*(pitchComp +deltaPitch); Serial.print("rollComp:"); Serial.print(rollComp); Serial.print(','); Serial.print("pitchComp:"); Serial.print(pitchComp); Serial.print(','); Serial.print("yawG:"); Serial.print(yawG); Serial.print(','); // Serial.print("rollComp:"); // Serial.print(rollComp); // Serial.print(','); // Serial.print("pitchG:"); // Serial.print(pitchG); // Serial.print(','); // Serial.print("yawG:"); // Serial.print(yawG); // Serial.print(','); Serial.print("UL:"); Serial.print(90); Serial.print(','); Serial.print("LL:"); Serial.println(-90); // Serial.print("Ax:"); // Serial.print(Ax); // Serial.print(','); // Serial.print("Ay:"); // Serial.print(Ay); // Serial.print(','); // Serial.print("Az:"); // Serial.print(Az); // Serial.print(','); // Serial.print("UL:"); // Serial.print(1); // Serial.print(','); // Serial.print("LL:"); // Serial.println(-1); delay(1); } |