3.3 haas506开发教程-example-mpu6050

1.mpu6050

• 案例说明
  使用mpu6050获取六轴数据
• 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)

• 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")

• board.json

  {
  "version": "1.0.0",
  "io": {
    "mpu6050": {
      "type": "I2C",
      "port": 1,
      "addrWidth": 7,
      "freq": 100000,
      "mode": "master",
      "devAddr": 104
    }
  },
  "debugLevel": "ERROR"
}

• 日志输出

--------------------------------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
-------------------------------------------------------------------------------

2.总结

 本节介绍了如何使用mpu6050获取六轴数据，以及将六轴数据融合成三个角度值。其中航偏角不准，因为六轴传感器没有磁力计。代码中需要注意的：可以设置加速度和角速度的量程，如果不设置，默认使用最小的量程。