haas506 2.0开发教程-example-mpu6050
- 1.mpu6050
- 2.程序代码
- 3.日志输出
- 4.总结
1.mpu6050
案例说明
- 获取mpu6050六轴数据
- MPU6050 是 InvenSense 公司推出的全球首款整合性 6 轴运动处理组件,内部整合了 3 轴陀螺仪和 3 轴加速度传感器,相较于多组件方案,免除了组合陀螺仪与加速器时之轴间差的问题,减少了安装空间。
- 使用I2C进行连接
2.程序代码
读取 MPU6050 的加速度和角度传感器数据前需要进行一些初始化操作
- 复位MPU6050
- 设置角速度传感器(陀螺仪)和加速度传感器的满量程范围
- 设置陀螺仪采样率和设置数字低通滤波器(DLPF)等
- 配置系统时钟源并使能角速度传感器和加速度传感器
mpu6050.py
import math
class mpu6050:
# 重力加速度值
GRAVITIY_MS2 = 9.80665
#比例增益支配率收敛到加速度计/磁强计
Kp=100.0
#积分增益支配率的陀螺仪偏见的衔接
Ki= 0.002
#采样周期的一半
halfT= 0.001
#俯仰角 横滚角 航向角
Pitch=0
Roll=0
Yaw=0
#四元数的元素,代表估计方向
q0 = 1
q1 = 0
q2 = 0
q3 = 0
#按比例缩小积分误差
exInt = 0
eyInt = 0
ezInt = 0
# Scale Modifiers
ACCEL_SCALE_MODIFIER_2G = 16384.0
ACCEL_SCALE_MODIFIER_4G = 8192.0
ACCEL_SCALE_MODIFIER_8G = 4096.0
ACCEL_SCALE_MODIFIER_16G = 2048.0
GYRO_SCALE_MODIFIER_250DEG = 131.0
GYRO_SCALE_MODIFIER_500DEG = 65.5
GYRO_SCALE_MODIFIER_1000DEG = 32.8
GYRO_SCALE_MODIFIER_2000DEG = 16.4
# Pre-defined ranges
ACCEL_RANGE_2G = 0x00
ACCEL_RANGE_4G = 0x08
ACCEL_RANGE_8G = 0x10
ACCEL_RANGE_16G = 0x18
GYRO_RANGE_250DEG = 0x00
GYRO_RANGE_500DEG = 0x08
GYRO_RANGE_1000DEG = 0x10
GYRO_RANGE_2000DEG = 0x18
# MPU-6050 Registers
PWR_MGMT_1 = 0x6B
PWR_MGMT_2 = 0x6C
ACCEL_XOUT0 = 0x3B
ACCEL_YOUT0 = 0x3D
ACCEL_ZOUT0 = 0x3F
TEMP_OUT0 = 0x41
GYRO_XOUT0 = 0x43
GYRO_YOUT0 = 0x45
GYRO_ZOUT0 = 0x47
ACCEL_CONFIG = 0x1C
GYRO_CONFIG = 0x1B
def __init__(self):
from driver import I2C
self.i2c=I2C()
self.i2c.open('mpu6050')
writeBuf=bytearray(2)
writeBuf[0]=self.PWR_MGMT_1
writeBuf[1]=0x00
# Wake up the MPU-6050 since it starts in sleep mode
self.i2c.write(writeBuf,2)
def get_dev_id(self):
readBuf=bytearray([0x75])
self.i2c.read(readBuf,1)
return readBuf
def read_i2c_word(self, register):
# Read the data from the registers
readBuf=bytearray(2)
readBuf[0]=register
readBuf[1]=register+1
self.i2c.read(readBuf,2)
high=readBuf[0]
low=readBuf[1]
value = (high << 8) + low
if (value >= 0x8000):
return -((65535 - value) + 1)
else:
return value
# 得到mpu6050的温度值
def get_temp(self):
readBuf=bytearray(1)
readBuf[0]=self.TEMP_OUT0
self.i2c.read(readBuf,1)
actual_temp=(ord(readBuf.decode())/340)+36.53
return actual_temp
#设置加速度量程
def set_accel_range(self, accel_range):
# First change it to 0x00 to make sure we write the correct value
writeBuf1=bytearray(2)
writeBuf1[0]=self.ACCEL_CONFIG
writeBuf1[1]=0x00
self.i2c.write(writeBuf1,2)
# Write the new range to the ACCEL_CONFIG register
writeBuf2=bytearray(2)
writeBuf2[0]=self.ACCEL_CONFIG
writeBuf2[1]=accel_range
self.i2c.write(writeBuf2,2)
#读取加速度量程
def read_accel_range(self, raw = False):
readBuf=bytearray(1)
readBuf[0]=self.ACCEL_CONFIG
self.i2c.read(readBuf,1)
if raw is True:
return readBuf
elif raw is False:
if readBuf == self.ACCEL_RANGE_2G:
return 2
elif readBuf == self.ACCEL_RANGE_4G:
return 4
elif readBuf == self.ACCEL_RANGE_8G:
return 8
elif readBuf == self.ACCEL_RANGE_16G:
return 16
else:
return -1
#得到加速度数据
def get_accel_data(self, g = False):
x = self.read_i2c_word(self.ACCEL_XOUT0)
y = self.read_i2c_word(self.ACCEL_YOUT0)
z = self.read_i2c_word(self.ACCEL_ZOUT0)
accel_scale_modifier = None
accel_range = self.read_accel_range(True)
if accel_range == self.ACCEL_RANGE_2G:
accel_scale_modifier = self.ACCEL_SCALE_MODIFIER_2G
elif accel_range == self.ACCEL_RANGE_4G:
accel_scale_modifier = self.ACCEL_SCALE_MODIFIER_4G
elif accel_range == self.ACCEL_RANGE_8G:
accel_scale_modifier = self.ACCEL_SCALE_MODIFIER_8G
elif accel_range == self.ACCEL_RANGE_16G:
accel_scale_modifier = self.ACCEL_SCALE_MODIFIER_16G
else:
#如果不设置量程,会默认使用ACCEL_SCALE_MODIFIER_2G
print("Unkown range - accel_scale_modifier set to self.ACCEL_SCALE_MODIFIER_2G")
accel_scale_modifier = self.ACCEL_SCALE_MODIFIER_2G
x = x / accel_scale_modifier
y = y / accel_scale_modifier
z = z / accel_scale_modifier
if g is True:
return {'x': x, 'y': y, 'z': z}
elif g is False:
x = x * self.GRAVITIY_MS2
y = y * self.GRAVITIY_MS2
z = z * self.GRAVITIY_MS2
return {'x': x, 'y': y, 'z': z}
#设置角速度量程
def set_gyro_range(self, gyro_range):
writeBuf1=bytearray(2)
writeBuf1[0]=self.GYRO_CONFIG
writeBuf1[1]=0x00
self.i2c.write(writeBuf1,2)
writeBuf2=bytearray(2)
writeBuf2[0]=self.GYRO_CONFIG
writeBuf2[1]=gyro_range
self.i2c.write(writeBuf2,2)
#读取角速度量程
def read_gyro_range(self, raw = False):
readBuf=bytearray(1)
readBuf[0]=self.GYRO_CONFIG
self.i2c.read(readBuf,1)
if raw is True:
return readBuf
elif raw is False:
if readBuf == self.GYRO_RANGE_250DEG:
return 250
elif readBuf == self.GYRO_RANGE_500DEG:
return 500
elif readBuf == self.GYRO_RANGE_1000DEG:
return 1000
elif readBuf == self.GYRO_RANGE_2000DEG:
return 2000
else:
return -1
#得到角速度数据
def get_gyro_data(self):
x = self.read_i2c_word(self.GYRO_XOUT0)
y = self.read_i2c_word(self.GYRO_YOUT0)
z = self.read_i2c_word(self.GYRO_ZOUT0)
gyro_scale_modifier = None
gyro_range = self.read_gyro_range(True)
if gyro_range == self.GYRO_RANGE_250DEG:
gyro_scale_modifier = self.GYRO_SCALE_MODIFIER_250DEG
elif gyro_range == self.GYRO_RANGE_500DEG:
gyro_scale_modifier = self.GYRO_SCALE_MODIFIER_500DEG
elif gyro_range == self.GYRO_RANGE_1000DEG:
gyro_scale_modifier = self.GYRO_SCALE_MODIFIER_1000DEG
elif gyro_range == self.GYRO_RANGE_2000DEG:
gyro_scale_modifier = self.GYRO_SCALE_MODIFIER_2000DEG
else:
#如果不设置量程,会默认使用GYRO_SCALE_MODIFIER_250DEG
print("Unkown range - gyro_scale_modifier set to self.GYRO_SCALE_MODIFIER_250DEG")
gyro_scale_modifier = self.GYRO_SCALE_MODIFIER_250DEG
x = x / gyro_scale_modifier
y = y / gyro_scale_modifier
z = z / gyro_scale_modifier
return {'x': x, 'y': y, 'z': z}
#得到 加速度和角速度数据,和温度值
def get_all_data(self):
temp = self.get_temp()
accel = self.get_accel_data()
gyro = self.get_gyro_data()
return [accel, gyro, temp]
def get_imu_angle(self,ax,ay,az,gx,gy,gz):
#测量正常化
norm1 = math.sqrt(ax*ax + ay*ay + az*az);
if norm1!=0:
#单位化
ax = ax / norm1
ay = ay / norm1
az = az / norm1
# 估计方向的重力
vx = 2*(self.q1*self.q3 - self.q0*self.q2)
vy = 2*(self.q0*self.q1 + self.q2*self.q3)
vz = self.q0*self.q0 - self.q1*self.q1 - self.q2*self.q2 + self.q3*self.q3
#错误的领域和方向传感器测量参考方向之间的交叉乘积的总和
ex = (ay*vz - az*vy)
ey = (az*vx - ax*vz)
ez = (ax*vy - ay*vx)
#积分误差比例积分增益
exInt = self.exInt + ex*self.Ki
eyInt = self.eyInt + ey*self.Ki
ezInt = self.ezInt + ez*self.Ki
# 调整后的陀螺仪测量
gx = gx + self.Kp*ex + exInt
gy = gy + self.Kp*ey + eyInt
gz = gz + self.Kp*ez + ezInt
else:
print("norm1 is 0")
#整合四元数率和正常化
q0 = self.q0 + (-self.q1*gx - self.q2*gy - self.q3*gz)*self.halfT
q1 = self.q1 + (self.q0*gx + self.q2*gz - self.q3*gy)*self.halfT
q2 = self.q2 + (self.q0*gy - self.q1*gz + self.q3*gx)*self.halfT
q3 = self.q3 + (self.q0*gz + self.q1*gy - self.q2*gx)*self.halfT
# 正常化四元
norm2 = math.sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3)
if norm2!=0:
q0 = q0 / norm2
q1 = q1 / norm2
q2 = q2 / norm2
q3 = q3 / norm2
Pitch = math.asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3
Roll = math.atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3
#Yaw可以注释掉,因为六轴传感器没有磁力计,所以得到的航偏角不准
Yaw = math.atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;
return {'Pitch': Pitch, 'Roll': Roll, 'Yaw': Yaw}
else:
print("norm2 is 0")
mian.py
import utime as time
import usys
usys.path.append('/data/pyamp')
from mpu6050 import mpu6050
mpu=mpu6050()
mpu_id=mpu.get_dev_id()
#mpu6050 chip id = bytearray(b'h')
print('mpu6050 chip id is ',hex(ord(mpu_id.decode())))
#mpu6050 chip id is 0x68
while True:
print('--------------------------------mpu6050test----------------------------------')
print('Temperature:',mpu.get_temp())
accel_data = mpu.get_accel_data()
print('accel_data = {},{},{}'.format(accel_data['x'],accel_data['y'],accel_data['z']))
gyro_data = mpu.get_gyro_data()
print('gyro_data = {},{},{}'.format(gyro_data['x'],gyro_data['y'],gyro_data['z']))
angle=mpu.get_imu_angle(accel_data['x'],accel_data['y'],accel_data['z'],gyro_data['x'],gyro_data['y'],gyro_data['z'])
print("Roll={},pitch={},Yaw={}".format(angle['Pitch'],angle['Roll'],angle['Yaw']))
print('-------------------------------------------------------------------------------')
time.sleep(1)
board.json
{
"version": "2.0.0",
"io": {
"mpu6050": {
"type": "I2C",
"port": 1,
"addrWidth": 7,
"freq": 100000,
"mode": "master",
"devAddr": 104
}
},
"debugLevel": "ERROR"
}
连接串口工具开启程序后,每隔一秒会实时更新mpu6050的数据信息。
--------------------------------mpu6050test----------------------------------
mpu6050 chip id = bytearray(b'h')
mpu6050 chip id is 0x68
Temperature: 37.23588235294118
Unkown range - accel_scale_modifier set to self.ACCEL_SCALE_MODIFIER_2G
accel_data = 9.428366137695312,3.924096520996093,-0.9289502441406249
Unkown range - gyro_scale_modifier set to self.GYRO_SCALE_MODIFIER_250DEG
gyro_data = 3.702290076335878,0.7404580152671755,-0.8473282442748092
Roll=-10.40073482164199,pitch=4.856018492034298,Yaw=-0.5392972556225775
-------------------------------------------------------------------------------
本节介绍了如何使用mpu6050获取六轴数据,以及将六轴数据融合成三个角度值。其中航偏角不准,因为六轴传感器没有磁力计。代码中需要注意的:可以设置加速度和角速度的量程,如果不设置,默认使用最小的量程。