In today’s video lesson we show how you can remove the vibration noise from your roll and pitch values using a low pass filter. The filter removes the high frequency noise resulting from vibrations.
Below is the code we developed in this lesson.
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 | #include <Adafruit_MPU6050.h> #include <Adafruit_Sensor.h> #include <Wire.h> float Ax; float Ay; float Az; // 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 roll=0; float pitch=0; float rollRAW; float pitchRAW; Adafruit_MPU6050 mpu; 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.setFilterBandwidth(MPU6050_BAND_21_HZ); } 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; //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); pitch = .75*pitch + .25*pitchRAW; roll = .75*roll + .25*rollRAW; Serial.print("RollRAW:"); Serial.print(rollRAW); Serial.print(','); Serial.print("PitchRAW:"); Serial.print(pitchRAW); Serial.print(','); Serial.print("Roll:"); Serial.print(roll); Serial.print(','); Serial.print("Pitch:"); Serial.print(pitch); 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(50); } |