mpu6050使用(2)—(滤波)

已经不知道写的是啥了

boot.py

from machine import I2C, Pin
import mpu6050a
import utime
import gc
import avg

pin = Pin(2, Pin.OUT)
pin.value(0)
utime.sleep(10)
pin.value(1)

i2c = I2C(scl=Pin(5), sda=Pin(4))
accelerometer = mpu6050a.accel(i2c)
G = 9.7936  # wuhan_G = 9.7936 acc_z_ori = 16384 mean= sum(list)/len(list)

cal_AcX, cal_AcY, cal_AcZ, cal_GyX, cal_GyY, cal_GyZ = avg.calib(accelerometer)  # get calibrate_veu
# AcX-cal_AcX AcY-cal_AcY AcZ-cal_AcZ GyX-cal_GyX GyY-cal_GyY GyZ-cal_GyZ

while 1:
    gc.collect()
    filter_data = avg.avg_filiter(cal_AcX, cal_AcY, cal_AcZ, cal_GyX, cal_GyY, cal_GyZ)
    print(filter_data)
    utime.sleep(0.05)

avg.py

import utime
import gc
from machine import I2C, Pin
import mpu6050a

i2c = I2C(scl=Pin(5), sda=Pin(4))
accelerometer = mpu6050a.accel(i2c)

fAcX = [0 for i in range(0, 50)]
fAcY = [0 for ii in range(0, 50)]
fAcZ = [0 for j in range(0, 50)]
fGyX = [0 for jj in range(0, 50)]
fGyY = [0 for k in range(0, 50)]
fGyZ = [0 for kk in range(0, 50)]

gc.collect()

def calib(accelerometer):
i = 0
GyZ = []
GyY = []
GyX = []
AcZ = []
AcY = []
AcX = []
utime.sleep(5)
while i <= 500:
mpu = accelerometer.get_values()
GyZ.append(mpu['GyZ'])
GyY.append(mpu['GyY'])
GyX.append(mpu['GyX'])
AcZ.append(mpu['AcZ'])
AcY.append(mpu['AcY'])
AcX.append(mpu['AcX'])
utime.sleep_ms(10)
i += 1
cal_GyZ = int(sum(GyZ) / len(GyZ))
cal_GyY = int(sum(GyY) / len(GyY))
cal_GyX = int(sum(GyX) / len(GyX))
cal_AcZ = int(sum(AcZ) / len(AcZ))- 16384
cal_AcY = int(sum(AcY) / len(AcY))
cal_AcX = int(sum(AcX) / len(AcX))
gc.collect()
return cal_AcX, cal_AcY, cal_AcZ, cal_GyX, cal_GyY, cal_GyZ

def calied_mpu(cal_AcX, cal_AcY, cal_AcZ, cal_GyX, cal_GyY, cal_GyZ):
calied_mpu={}
ori_mpu = accelerometer.get_values() # {'GyZ':2,'GyY':57,'GyX':368,'Tmp':28.78,'AcZ':-1068,'AcY':-66,'AcX':-16026}
calied_mpu["AcX"] = ori_mpu["AcX"]-cal_AcX
calied_mpu["AcY"] = ori_mpu["AcY"]-cal_AcY
calied_mpu["AcZ"] = ori_mpu["AcZ"]-cal_AcZ
calied_mpu["Tmp"] = ori_mpu["Tmp"]-10
calied_mpu["GyX"] = ori_mpu["GyX"]-cal_GyX
calied_mpu["GyY"] = ori_mpu["GyY"]-cal_GyY
calied_mpu["GyZ"] = ori_mpu["GyZ"]-cal_GyZ
return calied_mpu

def avg_filiter(cal_AcX, cal_AcY, cal_AcZ, cal_GyX, cal_GyY, cal_GyZ):
filter_data = {}
calied_data = calied_mpu(cal_AcX, cal_AcY, cal_AcZ, cal_GyX, cal_GyY, cal_GyZ)
i = 0
while i < 50:
del fAcX[0]
del fAcY[0]
del fAcZ[0]
del fGyX[0]
del fGyY[0]
del fGyZ[0]
fAcX.append(calied_data["AcX"])
fAcY.append(calied_data["AcY"])
fAcZ.append(calied_data["AcZ"])
fGyX.append(calied_data["GyX"])
fGyY.append(calied_data["GyY"])
fGyZ.append(calied_data["GyZ"])
i += 1
filter_data["AcX"] = int(sum(fAcX) / 50)
filter_data["AcY"] = int(sum(fAcY) / 50)
filter_data["AcZ"] = int(sum(fAcZ) / 50)
filter_data["Tmp"] = calied_data["Tmp"]
filter_data["GyX"] = int(sum(fGyX) / 50)
filter_data["GyY"] = int(sum(fGyY) / 50)
filter_data["GyZ"] = int(sum(fGyZ) / 50)
gc.collect()
return filter_data

mpu6050a.py

class accel():
def __init__(self, i2c, addr=0x68):
self.iic = i2c
self.addr = addr
self.iic.start()
self.iic.writeto(self.addr, bytearray([107, 0]))
self.iic.stop()

def get_raw_values(self):
self.iic.start()
a = self.iic.readfrom_mem(self.addr, 0x3B, 14)
self.iic.stop()
return a

def get_ints(self):
b = self.get_raw_values()
c = []
for i in b:
c.append(i)
return c

def bytes_toint(self, firstbyte, secondbyte):
if not firstbyte & 0x80:
return firstbyte << 8 | secondbyte
return - (((firstbyte ^ 255) << 8) | (secondbyte ^ 255) + 1)

def get_values(self):
raw_ints = self.get_raw_values()
vals = {}
vals["AcX"] = self.bytes_toint(raw_ints[0], raw_ints[1])
vals["AcY"] = self.bytes_toint(raw_ints[2], raw_ints[3])
vals["AcZ"] = self.bytes_toint(raw_ints[4], raw_ints[5])
vals["Tmp"] = self.bytes_toint(raw_ints[6], raw_ints[7]) / 340.00 + 36.53
vals["GyX"] = self.bytes_toint(raw_ints[8], raw_ints[9])
vals["GyY"] = self.bytes_toint(raw_ints[10], raw_ints[11])
vals["GyZ"] = self.bytes_toint(raw_ints[12], raw_ints[13])
return vals # returned in range of Int16
# -32768 to 32767 mean = 16384

def val_test(self): # ONLY FOR TESTING! Also, fast reading sometimes crashes IIC
import utime
while 1:
print(self.get_values())
utime.sleep_ms(100)