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.
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 52 53 54 55 56 57 58 59 60 61 |
# ==================================================================== # DISCLAIMER: # This code is provided as-is for educational and experimental # purposes only. The author makes no representations or warranties of # any kind concerning the safety, suitability, or accuracy of this # code. Use at your own risk. The author assumes no liability for any # damages, system failures, security breaches, or network issues # resulting from the use or implementation of this script. # ==================================================================== 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 |