基于micropython编写的esp32 mpu6050驱动

最新推荐文章于 2024-05-15 17:46:04 发布

supermodule

最新推荐文章于 2024-05-15 17:46:04 发布

阅读量377

点赞数 8

分类专栏： esp32 文章标签： python 开发语言

本文链接：https://blog.csdn.net/supermodule/article/details/137654789

版权

esp32 专栏收录该内容

19 篇文章 2 订阅

订阅专栏

前言

文章分两部分，第一部分是全部代码展示，复制即用；第二部分是代码分段解释。

用的是esp32的I2C(0)，默认scl接GPIO18，sda接GPIO19

关键词：micropython，esp32，mpu6050

所有代码展示

from machine import Pin,I2C
import time
import math

class MPU6050():
    def __init__(self, i2c, addr=0x68):
        self.iic = i2c
        self.addr = addr
        self.accelScale=16384
        self.gyroScale=131
        # 唤醒
        self.iic.writeto(self.addr, bytearray([107, 0]))
        self.iic.writeto(self.addr,bytearray([26,0]))
        # 配置加速度计量程
        self.iic.writeto(self.addr,bytearray([27,0]))
        # 配置角速度计量程
        self.iic.writeto(self.addr,bytearray([28,0]))
        # 配置量程系数
        AFS_SEL=(self.iic.readfrom_mem(self.addr,27,1))[0]&0x18
        if AFS_SEL==0:
            self.accelScale=16384
        elif AFS_SEL==0x08:
            self.accelScale=8192
        elif AFS_SEL==0x10:
            self.accelScale=4096
        elif AFS_SEL==0x18:
            self.accelScale=2048

        AFS_SEL=(self.iic.readfrom_mem(self.addr,28,1))[0]&0x18
        if(AFS_SEL==0):
            self.gyroScale=131
        elif AFS_SEL==0x08:
            self.gyroScale=65.5
        elif AFS_SEL==0x10:
            self.gyroScale=32.8
        elif AFS_SEL==0x18:
            self.gyroScale=16.4
        self.calibraton=self.calibrate()
    # 读取寄存器原生数据
    def get_raw_values(self):
        reg_data=self.iic.readfrom_mem(self.addr, 59, 14)
        return reg_data[:6],reg_data[8:]
    # 将原生数据转6轴数据
    def raw_to_ints(self):
        a,g=self.get_raw_values()
        aX=self.bytes_toint(a[0],a[1])/self.accelScale
        aY=self.bytes_toint(a[2],a[3])/self.accelScale
        aZ=self.bytes_toint(a[4],a[5])/self.accelScale
        gX=self.bytes_toint(g[0],g[1])/self.gyroScale
        gY=self.bytes_toint(g[2],g[3])/self.gyroScale
        gZ=self.bytes_toint(g[4],g[5])/self.gyroScale
        return [aX,aY,aZ,gX,gY,gZ]
    # 定义转换规则
    def bytes_toint(self, firstbyte, secondbyte):
        if not firstbyte & 0x80:
            return firstbyte << 8 | secondbyte
        return - (((firstbyte ^ 255) << 8) | (secondbyte ^ 255) + 1)
    # 处理后的6轴参数
    def get_values(self,calibration):
        return [a+b for a, b in zip(self.raw_to_ints(), calibration)] 


    # 获取修正数组
    def calibrate(self):
        calibrationArray = [0,0,0,0,0,0]
        for i in range(100):
            calibtation=self.raw_to_ints()
            calibrationArray[0] += calibtation[0]
            calibrationArray[1] += calibtation[1]
            calibrationArray[2] += calibtation[2]
            calibrationArray[3] += calibtation[3]
            calibrationArray[4] += calibtation[4]
            calibrationArray[5] += calibtation[5]
        for i in range(6):
            calibrationArray[i]/=100
            if i==2:
                calibrationArray[i]=1-calibrationArray[i]
            else:
                calibrationArray[i]=-calibrationArray[i]
        return calibrationArray
    # 计算欧拉角
    def get_angles(self,angle_data,calibration,k=0.95):
        data=self.get_values(calibration)
        aPitch=math.atan(data[0]/data[2])*57.3
        aRow=math.atan(data[1]/data[2])*57.3
        gRow=data[3]/7500
        gPitch=data[4]/7500
        gYaw=data[5]/7500
        pitch=(1-k)*angle_data['pitch']+k*(aPitch+gPitch)
        row=(1-k)*angle_data['row']+k*(aRow+gRow)
        yaw=(1-k)*angle_data['yaw']+k*(angle_data['yaw']+gYaw)
        return {'pitch':pitch,'yaw':yaw,'row':row}

# 使用demo
i2c=I2C(0)
a=MPU6050(i2c)
angle={'pitch':0,'row':0,'yaw':0}
cali=a.calibrate()
while True:
    angle=a.get_angles(angle,cali)
    print(angle)
    time.sleep(0.5)

代码解释

总体分为两部分，导包和MPU6050类的定义

导包

from machine import Pin,I2C
import time
import math

定义MPU6050类

包含几个类函数：

类初始化

def __init__(self, i2c, addr=0x68):
        self.iic = i2c
        self.addr = addr
        # 初始化加速度计量程系数（默认+-2g）
        self.accelScale=16384
        # 初始化陀螺仪量程系数（默认+-250°）
        self.gyroScale=131
        # 唤醒
        self.iic.writeto(self.addr, bytearray([107, 0]))
        self.iic.writeto(self.addr,bytearray([26,0]))
        # 配置加速度计量程
        self.iic.writeto(self.addr,bytearray([27,0]))
        # 配置角速度计量程
        self.iic.writeto(self.addr,bytearray([28,0]))
        # 根据寄存器值配置量程系数
        AFS_SEL=(self.iic.readfrom_mem(self.addr,27,1))[0]&0x18
        if AFS_SEL==0:
            self.accelScale=16384
        elif AFS_SEL==0x08:
            self.accelScale=8192
        elif AFS_SEL==0x10:
            self.accelScale=4096
        elif AFS_SEL==0x18:
            self.accelScale=2048

        AFS_SEL=(self.iic.readfrom_mem(self.addr,28,1))[0]&0x18
        if(AFS_SEL==0):
            self.gyroScale=131
        elif AFS_SEL==0x08:
            self.gyroScale=65.5
        elif AFS_SEL==0x10:
            self.gyroScale=32.8
        elif AFS_SEL==0x18:
            self.gyroScale=16.4
        # 调用矫正函数进行初始矫正
        self.calibraton=self.calibrate()

获取原生6轴数据

# 读取寄存器原生数据，返回加速度和陀螺仪的12个数据
    def get_raw_values(self):
        reg_data=self.iic.readfrom_mem(self.addr, 59, 14)
        return reg_data[:6],reg_data[8:]

将原生6轴数据转化

# 将原生数据转6轴数据
    def raw_to_ints(self):
        # 获取原生6轴数据
        a,g=self.get_raw_values()
        # 调用bytes_toint方法进行转化，除以量程系数将数据映射到范围内
        aX=self.bytes_toint(a[0],a[1])/self.accelScale
        aY=self.bytes_toint(a[2],a[3])/self.accelScale
        aZ=self.bytes_toint(a[4],a[5])/self.accelScale
        gX=self.bytes_toint(g[0],g[1])/self.gyroScale
        gY=self.bytes_toint(g[2],g[3])/self.gyroScale
        gZ=self.bytes_toint(g[4],g[5])/self.gyroScale
        # 返回处理后的6轴数据
        return [aX,aY,aZ,gX,gY,gZ]

原生数据转化规则

# 定义转换规则
    def bytes_toint(self, firstbyte, secondbyte):
        if not firstbyte & 0x80:
            return firstbyte << 8 | secondbyte
        return - (((firstbyte ^ 255) << 8) | (secondbyte ^ 255) + 1)

定义矫正函数

    def calibrate(self):
        # 矫正数组初始化
        calibrationArray = [0,0,0,0,0,0]
        # 取100次数据累加后取平均
        for i in range(100):
            calibtation=self.raw_to_ints()
            calibrationArray[0] += calibtation[0]
            calibrationArray[1] += calibtation[1]
            calibrationArray[2] += calibtation[2]
            calibrationArray[3] += calibtation[3]
            calibrationArray[4] += calibtation[4]
            calibrationArray[5] += calibtation[5]
        # 当静止且水平时，理论输出的6轴数组[0,0,1,0,0,0]
        # 只有z轴受1g的加速度
        for i in range(6):
            calibrationArray[i]/=100
            if i==2:
                calibrationArray[i]=1-calibrationArray[i]
            else:
                calibrationArray[i]=-calibrationArray[i]
        return calibrationArray

返回矫正后的6轴数据

def get_values(self,calibration):
        # 处理后的6轴数据与矫正数组相加得到矫正的数据
        return [a+b for a, b in zip(self.raw_to_ints(), calibration)]

计算欧拉角

"""
参数说明：
angle_data 上一时刻欧拉角数据
calibration 矫正数组
k 权重系数
"""
    def get_angles(self,angle_data,calibration,k=0.95):
        # 获取矫正后的6轴数据
        data=self.get_values(calibration)
        aPitch=math.atan(data[0]/data[2])*57.3
        aRow=math.atan(data[1]/data[2])*57.3
        
        gRow=data[3]/7500
        gPitch=data[4]/7500
        gYaw=data[5]/7500
        # 本次数据与上一时刻数据加权求和，减少扰动
        pitch=(1-k)*angle_data['pitch']+k*(aPitch+gPitch)
        row=(1-k)*angle_data['row']+k*(aRow+gRow)
        yaw=(1-k)*angle_data['yaw']+k*(angle_data['yaw']+gYaw)
        # 返回欧拉角字典
        return {'pitch':pitch,'yaw':yaw,'row':row}