开发环境:thonny、pycharm
开发语言:python、microPython
开发设备:mpu6050、esp32、PC端
开发步骤:数据采集、数据处理、数据传输、数据绘图
目录
一、数据采集
使用microPython开发语言在thonny开发环境中编写mpu6050库文件和采集数据的主程序,然后烧录到esp32微处理器中。在thonny中运行主程序,驱动esp32控制mpu6050采集数据。
代码实现
1.mpu6050库文件
import machine
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
def val_test(self): # ONLY FOR TESTING! Also, fast reading sometimes crashes IIC
from time import sleep
while 1:
print(self.get_values())
sleep(0.05)
2.采集数据主程序
import mpu6050
from machine import Pin,SoftI2C
import time
i2c = SoftI2C(scl=Pin(22), sda=Pin(21))
Mpu = mpu6050.accel(i2c)
while True:
#获取原始数据(字典类型)
s = Mpu.get_values()
print(s)
time.sleep_ms(500)
采集结果(原始数据)
二、数据处理
使用microPython开发语言在thonny开发环境中编写数据处理的程序,然后烧录到esp32微处理器中,对采集到的原始数据进行处理,从而得到实用且有效的实验数据。
1.加速度、角速度的计算
代码实现
import mpu6050
from machine import Pin,SoftI2C
import time
i2c = SoftI2C(scl=Pin(22), sda=Pin(21))
Mpu = mpu6050.accel(i2c)
while True:
#1
#获取原始数据(字典类型)
s = Mpu.get_values()
#处理加速度数据
#2
#从字典中提取加速度原始数据
ax=s.get('AcX')
ay=s.get('AcY')
az=s.get('AcZ')
#3
#选择2g倍率,计算数据分辨率32768/2=16384,计算加速度=原始数据/数据分辨率
acx=ax/16384.0
acy=ay/16384.0
acz=az/16384.0
#4
#将加速度数据类型转换成字符串,并创建新对象,目的是适应后期绘图接收数据的格式要求
r = 'acx:'+str(acx)
o = 'acy:'+str(acy)
t = 'acz:'+str(acz)
m = r+', '+o+', '+t
#处理角速度数据(选择250度/秒倍率,32768/250=131(数据分辨率),倍率越低精度越好)
gx=s.get('GyX')#原始数据
gyx = gx/131.0#处理后的数据
gy=s.get('GyY')
gyy = gy/131.0
gz=s.get('GyZ')
gyz = gz/131.0
#将角速度数据类型转换成字符串,并创建新对象
u = 'gyx:'+str(gyx)
v = 'gyy:'+str(gyy)
w = 'gyz:'+str(gyz)
n = u+', '+v+', '+w
#gyx = 1000*gx/32768
print(m)
print(n)
time.sleep_ms(500)
数据处理结果
2.欧拉角的计算
代码实现
import mpu6050
from machine import Pin,SoftI2C
import time
i2c = SoftI2C(scl=Pin(22), sda=Pin(21))
Mpu = mpu6050.accel(i2c)
#1
#构建欧拉角函数
def IMUupdate(ax,ay,az,gx,gy,gz):
global q0
global q1
global q2
global q3
global exInt
global eyInt
global ezInt
# print(q0)
#测量正常化
norm = math.sqrt(ax*ax+ay*ay+az*az)
#单元化
ax = ax/norm
ay = ay/norm
az = az/norm
#估计方向的重力
vx = 2*(q1*q3 - q0*q2)
vy = 2*(q0*q1 + q2*q3)
vz = q0*q0 - q1*q1 - q2*q2 + q3*q3
#错误的领域和方向传感器测量参考方向之间的交叉乘积的总和
ex = (ay*vz - az*vy)
ey = (az*vx - ax*vz)
ez = (ax*vy - ay*vx)
#积分误差比例积分增益
exInt += ex*Ki
eyInt += ey*Ki
ezInt += ez*Ki
#调整后的陀螺仪测量
gx += Kp*ex + exInt
gy += Kp*ey + eyInt
gz += Kp*ez + ezInt
#整合四元数
q0 += (-q1*gx - q2*gy - q3*gz)*halfT
q1 += (q0*gx + q2*gz - q3*gy)*halfT
q2 += (q