devbox
正经夜光杯
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

基于micropython编写的esp32 mpu6050驱动_micropython mpu6050

作者：正经夜光杯 | 2024-07-07 11:12:49

赞

踩

micropython mpu6050

前言

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

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

关键词：micropython，esp32，mpu6050

所有代码展示

  1. from machine import Pin,I2C
  2. import time
  3. import math
  4.  
  5. class MPU6050():
  6.     def __init__(self, i2c, addr=0x68):
  7.         self.iic = i2c
  8.         self.addr = addr
  9.         self.accelScale=16384
  10.         self.gyroScale=131
  11.         # 唤醒
  12.         self.iic.writeto(self.addr, bytearray([107, 0]))
  13.         self.iic.writeto(self.addr,bytearray([26,0]))
  14.         # 配置加速度计量程
  15.         self.iic.writeto(self.addr,bytearray([27,0]))
  16.         # 配置角速度计量程
  17.         self.iic.writeto(self.addr,bytearray([28,0]))
  18.         # 配置量程系数
  19.         AFS_SEL=(self.iic.readfrom_mem(self.addr,27,1))[0]&0x18
  20.         if AFS_SEL==0:
  21.             self.accelScale=16384
  22.         elif AFS_SEL==0x08:
  23.             self.accelScale=8192
  24.         elif AFS_SEL==0x10:
  25.             self.accelScale=4096
  26.         elif AFS_SEL==0x18:
  27.             self.accelScale=2048
  28.  
  29.         AFS_SEL=(self.iic.readfrom_mem(self.addr,28,1))[0]&0x18
  30.         if(AFS_SEL==0):
  31.             self.gyroScale=131
  32.         elif AFS_SEL==0x08:
  33.             self.gyroScale=65.5
  34.         elif AFS_SEL==0x10:
  35.             self.gyroScale=32.8
  36.         elif AFS_SEL==0x18:
  37.             self.gyroScale=16.4
  38.         self.calibraton=self.calibrate()
  39.     # 读取寄存器原生数据
  40.     def get_raw_values(self):
  41.         reg_data=self.iic.readfrom_mem(self.addr, 59, 14)
  42.         return reg_data[:6],reg_data[8:]
  43.     # 将原生数据转6轴数据
  44.     def raw_to_ints(self):
  45.         a,g=self.get_raw_values()
  46.         aX=self.bytes_toint(a[0],a[1])/self.accelScale
  47.         aY=self.bytes_toint(a[2],a[3])/self.accelScale
  48.         aZ=self.bytes_toint(a[4],a[5])/self.accelScale
  49.         gX=self.bytes_toint(g[0],g[1])/self.gyroScale
  50.         gY=self.bytes_toint(g[2],g[3])/self.gyroScale
  51.         gZ=self.bytes_toint(g[4],g[5])/self.gyroScale
  52.         return [aX,aY,aZ,gX,gY,gZ]
  53.     # 定义转换规则
  54.     def bytes_toint(self, firstbyte, secondbyte):
  55.         if not firstbyte & 0x80:
  56.             return firstbyte << 8 | secondbyte
  57.         return - (((firstbyte ^ 255) << 8) | (secondbyte ^ 255) + 1)
  58.     # 处理后的6轴参数
  59.     def get_values(self,calibration):
  60.         return [a+b for a, b in zip(self.raw_to_ints(), calibration)] 
  61.  
  62.  
  63.     # 获取修正数组
  64.     def calibrate(self):
  65.         calibrationArray = [0,0,0,0,0,0]
  66.         for i in range(100):
  67.             calibtation=self.raw_to_ints()
  68.             calibrationArray[0] += calibtation[0]
  69.             calibrationArray[1] += calibtation[1]
  70.             calibrationArray[2] += calibtation[2]
  71.             calibrationArray[3] += calibtation[3]
  72.             calibrationArray[4] += calibtation[4]
  73.             calibrationArray[5] += calibtation[5]
  74.         for i in range(6):
  75.             calibrationArray[i]/=100
  76.             if i==2:
  77.                 calibrationArray[i]=1-calibrationArray[i]
  78.             else:
  79.                 calibrationArray[i]=-calibrationArray[i]
  80.         return calibrationArray
  81.     # 计算欧拉角
  82.     def get_angles(self,angle_data,calibration,k=0.95):
  83.         data=self.get_values(calibration)
  84.         aPitch=math.atan(data[0]/data[2])*57.3
  85.         aRow=math.atan(data[1]/data[2])*57.3
  86.         gRow=data[3]/7500
  87.         gPitch=data[4]/7500
  88.         gYaw=data[5]/7500
  89.         pitch=(1-k)*angle_data['pitch']+k*(aPitch+gPitch)
  90.         row=(1-k)*angle_data['row']+k*(aRow+gRow)
  91.         yaw=(1-k)*angle_data['yaw']+k*(angle_data['yaw']+gYaw)
  92.         return {'pitch':pitch,'yaw':yaw,'row':row}
  93.  
  94. # 使用demo
  95. i2c=I2C(0)
  96. a=MPU6050(i2c)
  97. angle={'pitch':0,'row':0,'yaw':0}
  98. cali=a.calibrate()
  99. while True:
  100.     angle=a.get_angles(angle,cali)
  101.     print(angle)
  102.     time.sleep(0.5)

代码解释

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

导包

  1. from machine import Pin,I2C
  2. import time
  3. import math

定义MPU6050类

包含几个类函数：

类初始化

  1. def __init__(self, i2c, addr=0x68):
  2.         self.iic = i2c
  3.         self.addr = addr
  4.         # 初始化加速度计量程系数（默认+-2g）
  5.         self.accelScale=16384
  6.         # 初始化陀螺仪量程系数（默认+-250°）
  7.         self.gyroScale=131
  8.         # 唤醒
  9.         self.iic.writeto(self.addr, bytearray([107, 0]))
  10.         self.iic.writeto(self.addr,bytearray([26,0]))
  11.         # 配置加速度计量程
  12.         self.iic.writeto(self.addr,bytearray([27,0]))
  13.         # 配置角速度计量程
  14.         self.iic.writeto(self.addr,bytearray([28,0]))
  15.         # 根据寄存器值配置量程系数
  16.         AFS_SEL=(self.iic.readfrom_mem(self.addr,27,1))[0]&0x18
  17.         if AFS_SEL==0:
  18.             self.accelScale=16384
  19.         elif AFS_SEL==0x08:
  20.             self.accelScale=8192
  21.         elif AFS_SEL==0x10:
  22.             self.accelScale=4096
  23.         elif AFS_SEL==0x18:
  24.             self.accelScale=2048
  25.  
  26.         AFS_SEL=(self.iic.readfrom_mem(self.addr,28,1))[0]&0x18
  27.         if(AFS_SEL==0):
  28.             self.gyroScale=131
  29.         elif AFS_SEL==0x08:
  30.             self.gyroScale=65.5
  31.         elif AFS_SEL==0x10:
  32.             self.gyroScale=32.8
  33.         elif AFS_SEL==0x18:
  34.             self.gyroScale=16.4
  35.         # 调用矫正函数进行初始矫正
  36.         self.calibraton=self.calibrate()
'
运行

获取原生6轴数据

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

将原生6轴数据转化

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

原生数据转化规则

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

定义矫正函数

  1.     def calibrate(self):
  2.         # 矫正数组初始化
  3.         calibrationArray = [0,0,0,0,0,0]
  4.         # 取100次数据累加后取平均
  5.         for i in range(100):
  6.             calibtation=self.raw_to_ints()
  7.             calibrationArray[0] += calibtation[0]
  8.             calibrationArray[1] += calibtation[1]
  9.             calibrationArray[2] += calibtation[2]
  10.             calibrationArray[3] += calibtation[3]
  11.             calibrationArray[4] += calibtation[4]
  12.             calibrationArray[5] += calibtation[5]
  13.         # 当静止且水平时，理论输出的6轴数组[0,0,1,0,0,0]
  14.         # 只有z轴受1g的加速度
  15.         for i in range(6):
  16.             calibrationArray[i]/=100
  17.             if i==2:
  18.                 calibrationArray[i]=1-calibrationArray[i]
  19.             else:
  20.                 calibrationArray[i]=-calibrationArray[i]
  21.         return calibrationArray
'
运行

返回矫正后的6轴数据

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

计算欧拉角

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

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/正经夜光杯/article/detail/795453
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号