最近需要简单测量人体运动姿态,主要使用pitch和roll,故选用了最常见的MPU6050惯导单元。
一、MPU6050
MPU6050是一个6轴姿态传感器,包含3 轴陀螺仪和 3 轴加速度传感器,可以测量芯片自身X、Y、Z轴的加速度等参数,通过数据融合,可以得到姿态角(欧拉角)。本文通过micropython、esp32s3解析欧拉角。
二、硬件连接
ESP | MPU |
3.3v | VCC |
GND | GND |
1 | SCL |
2 | SDA |
三、主要程序
程序来源(转自),做了一些简单修改:(演示)mpu6050+esp32 mpy,dmp姿态角运算。三行代码实现调用_哔哩哔哩_bilibili
驱动程序请见该Up的视频。
但是时间久了,会出现温漂现象,数据很奇怪,准备从软硬件两方面尝试解决这个问题。
3.1 软件
思路是通过定时器,在软件层面上实现硬件重启。(突发奇想的)
import mpu6050, time, math
from machine import SoftI2C, Pin, Timer, reset
class Compute:
# 测试偏差 yaw pitch roll 用于归零
def __init__(self, i2c, bias=(-0, -0, 1.5)):
self.mpu = mpu6050.MPU6050(i2c)
self.mpu.dmpInitialize()
self.mpu.setDMPEnabled(True)
self.packetSize = self.mpu.dmpGetFIFOPacketSize()
self.bias = bias
def Wait4Ypr(self):
while self.mpu.getIntStatus() < 2:
# check for DMP data ready interrupt (this should happen frequently)
# get current FIFO count
time.sleep_ms(10)
fifoCount = self.mpu.getFIFOCount()
# check for overflow (this should never happen unless our code is too inefficient)
if fifoCount == 1024:
# reset so we can continue cleanly
self.mpu.resetFIFO()
print('FIFO overflow!')
# wait for correct available data length, should be a VERY short wait
fifoCount = self.mpu.getFIFOCount()
while fifoCount < self.packetSize:
fifoCount = self.mpu.getFIFOCount()
result = self.mpu.getFIFOBytes(self.packetSize)
q = self.mpu.dmpGetQuaternion(result)
g = self.mpu.dmpGetGravity(q)
ypr = self.mpu.dmpGetYawPitchRoll(q, g)
ypr = (ypr['yaw'] * 180 / math.pi + self.bias[0],
ypr['pitch'] * 180 / math.pi + self.bias[1],
ypr['roll'] * 180 / math.pi + self.bias[2])
print('pitch {:.2f}'.format(ypr[1]), end=' ')
print('roll {:.2f}'.format(ypr[2]))
return ypr
def restart(timer):
print('Restarting...')
reset()
# Main code
i2c = SoftI2C(scl=Pin(1), sda=Pin(2), freq=400000)
mpu = Compute(i2c, bias=(-0, -0, 1.4))
main_timer = Timer(-1)
main_timer.init(period=20000, mode=Timer.ONE_SHOT, callback=restart)
while True:
mpu.Wait4Ypr()
3.2 硬件
准备购买一款高级一点的IMU,具体效果待续,硬件还没回来。