In this video lesson we show how to create a complimentary filter such we get pitch and roll data from the MPU6050 which is quick and responsive, accurate, and low noise. We are using the following schematic:
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 | 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 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) pitchComp= pitchA*.005 + .995*(pitchComp+xGyro*tLoop) cnt=cnt+1 if cnt==10: cnt=0 print('RA: ',rollA,'PA: ',pitchA,'RC: ',rollComp,'PC: ',pitchComp) tStop=time.ticks_ms() tLoop=(tStop-tStart)*.001 |