In this video lesson I show you how to remove long term steady state error from the tilt values calculated from the MPU6050 IMU. We are using the following schematic for our prototype.
![MPU 6050](https://toptechboy.com/wp-content/uploads/2023/09/mpu6050-1-1024x357.jpg)
For your convenience, this is the code we developed in the video.
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 | from imu import MPU6050 from machine import I2C,Pin import math import time i2c=I2C(0, sda=Pin(16), scl=Pin(17), freq=400000) mpu = MPU6050(i2c) rollG=0 pitchG=0 rollComp=0 pitchComp=0 errorR=0 errorP=0 yaw=0 tLoop=0 cnt=0 while True: tStart=time.ticks_ms() xGyro=mpu.gyro.x yGyro=-mpu.gyro.y zGyro=mpu.gyro.z xAccel=mpu.accel.x yAccel=mpu.accel.y zAccel=mpu.accel.z rollG=rollG+yGyro*tLoop pitchG=pitchG+xGyro*tLoop rollA=math.atan(xAccel/zAccel)/2/math.pi*360 pitchA=math.atan(yAccel/zAccel)/2/math.pi*360 rollComp= rollA*.005 + .995*(rollComp+yGyro*tLoop)+errorR*.005 pitchComp= pitchA*.005 + .995*(pitchComp+xGyro*tLoop)+errorP*.005 errorP=errorP + (pitchA-pitchComp)*tLoop errorR=errorR + (rollA-rollComp)*tLoop cnt=cnt+1 if cnt==10: cnt=0 print('RA: ',rollA,'PA: ',pitchA,'RC: ',rollComp,'PC: ',pitchComp) #print('RA: ',rollA,'RC: ',rollComp) tStop=time.ticks_ms() tLoop=(tStop-tStart)*.001 |