micropython编程 esp32+mpu6050

青右

已于 2023-04-02 16:11:48 修改

阅读量1.4k

点赞数 3

分类专栏： esp32 文章标签： python 单片机

于 2023-04-02 11:56:10 首次发布

本文链接：https://blog.csdn.net/weixin_46256482/article/details/129908217

版权

esp32 专栏收录该内容

2 篇文章 2 订阅

订阅专栏

模块代码

模块内使用了esp32的定时器timer0，未使用滤波算法，响应比较及时

from machine import *
import math

class accel():
    def __init__(self, i2c, addr=0x68):
        self.iic = i2c
        self.addr = addr
        self.iic.start()
        self.iic.writeto(self.addr, bytearray([107, 0]))
        self.iic.stop()
        
        self.tim = Timer(0)
        self.tim.init(period=5, callback=self.timHandler)
        self.flag = False
        
        self.axo = 0
        self.ayo = 0
        self.azo = 0
        self.gxo = 0
        self.gyo = 0
        self.gzo = 0
        
        self.aax = 0
        self.aay = 0
        self.aaz = 0
        self.agx = 0
        self.agy = 0
        self.agz = 0
        
    def timHandler(self, t):
        if self.flag:
            try:
                self.kfilter()
            except:
                pass

    def get_raw_values(self):
        self.iic.start()
        a = self.iic.readfrom_mem(self.addr, 0x3B, 14)
        self.iic.stop()
        return a

    def get_ints(self):
        b = self.get_raw_values()
        c = []
        for i in b:
            c.append(i)
        return c

    def bytes_toint(self, firstbyte, secondbyte):
        if not firstbyte & 0x80:
            return firstbyte << 8 | secondbyte
        return - (((firstbyte ^ 255) << 8) | (secondbyte ^ 255) + 1)

    def get_values(self):
        raw_ints = self.get_raw_values()
        vals = {}
        vals["AcX"] = self.bytes_toint(raw_ints[0], raw_ints[1])
        vals["AcY"] = self.bytes_toint(raw_ints[2], raw_ints[3])
        vals["AcZ"] = self.bytes_toint(raw_ints[4], raw_ints[5])
        vals["Tmp"] = self.bytes_toint(raw_ints[6], raw_ints[7]) / 340.00 + 36.53
        vals["GyX"] = self.bytes_toint(raw_ints[8], raw_ints[9])
        vals["GyY"] = self.bytes_toint(raw_ints[10], raw_ints[11])
        vals["GyZ"] = self.bytes_toint(raw_ints[12], raw_ints[13])
        return vals  # returned in range of Int16
        # -32768 to 32767

    def val_test(self):  # ONLY FOR TESTING! Also, fast reading sometimes crashes IIC
        from time import sleep
        while 1:
            print(self.get_values())
            sleep(0.05)
            
           
    def mean(self, times):
        i = 0
        while i < times:
            ad = self.get_values()
            self.axo += ad["AcX"]
            self.ayo += ad["AcY"]
            self.azo += ad["AcZ"]
            self.gxo += ad["GyX"]
            self.gyo += ad["GyY"]
            self.gzo += ad["GyZ"]
            i += 1
        self.axo /= times
        self.ayo /= times
        self.azo /= times
        self.gxo /= times
        self.gyo /= times
        self.gzo /= times
        self.flag = True
           
           
    def kfilter(self):
        ad = self.get_values()
        # 加速度计
        ax = 2*9.8*ad["AcX"]/32768
        ay = 2*9.8*ad["AcY"]/32768
        az = 2*9.8*ad["AcZ"]/32768
        # 弧度制角度
        if az == 0 and ay >= 0:
            self.aax = 90
        elif az == 0 and ay < 0:
            self.aax = -90
        else:
            self.aax = math.atan(ay/az)*180/math.pi
        
        if az == 0 and ax >= 0:
            self.aay = -90
        elif az == 0 and ax < 0:
            self.aay = 90
        else:
            self.aay = -math.atan(ax/az)*180/math.pi
        
        if ay == 0 and az >= 0:
            self.aaz = -90
        elif ay == 0 and az < 0:
            self.aaz = 90
        else:
            self.aaz = -math.atan(az/ay)*180/math.pi
        # 角速度计
        self.agx = 1000*(ad["GyX"]-self.gxo)/32768
        self.agy = 1000*(ad["GyY"]-self.gyo)/32768
        self.agz = 1000*(ad["GyZ"]-self.gzo)/32768

添加卡尔曼滤波算法

根据网上的C语言版本改写的python代码，滞后问题严重，这里贴出对X轴使用卡尔曼滤波的代码

from machine import *
import math

class accel():
    def __init__(self, i2c, addr=0x68):
        self.iic = i2c
        self.addr = addr
        self.iic.start()
        self.iic.writeto(self.addr, bytearray([107, 0]))
        self.iic.stop()
        
        self.tim = Timer(0)
        self.tim.init(period=1, callback=self.timHandler)
        self.dt = 0.001
        self.flag = False
        
        self.axo = 0
        self.ayo = 0
        self.azo = 0
        self.gxo = 0
        self.gyo = 0
        self.gzo = 0
        
        self.aax = 0
        self.aay = 0
        self.aaz = 0    
        self.agx = 0
        self.agy = 0
        self.agz = 0
        
        self.x = 0
        self.y = 0
        self.z = 0
        self.gx = 0
        self.gy = 0
        self.gz = 0
        
        self.Q_angle = 0.001
        self.Q_gyro = 0.005
        self.R_angle = 0.5
        self.C_0 = 1
        self.q_bias = 0
        self.angle_err = 0
        self.PCt_0 = 0
        self.PCt_1 = 0
        self.E = 0
        self.K_0 = 0
        self.K_1 = 0
        self.t_0 = 0
        self.t_1 = 0
        self.Pdot = [0]*4
        self.P = [[1,0],[0,1]]
        
    def timHandler(self, t):
        if self.flag:
            self.kfilter()

    def get_raw_values(self):
        self.iic.start()
        a = self.iic.readfrom_mem(self.addr, 0x3B, 14)
        self.iic.stop()
        return a

    def get_ints(self):
        b = self.get_raw_values()
        c = []
        for i in b:
            c.append(i)
        return c

    def bytes_toint(self, firstbyte, secondbyte):
        if not firstbyte & 0x80:
            return firstbyte << 8 | secondbyte
        return - (((firstbyte ^ 255) << 8) | (secondbyte ^ 255) + 1)

    def get_values(self):
        raw_ints = self.get_raw_values()
        vals = {}
        vals["AcX"] = self.bytes_toint(raw_ints[0], raw_ints[1])
        vals["AcY"] = self.bytes_toint(raw_ints[2], raw_ints[3])
        vals["AcZ"] = self.bytes_toint(raw_ints[4], raw_ints[5])
        vals["Tmp"] = self.bytes_toint(raw_ints[6], raw_ints[7]) / 340.00 + 36.53
        vals["GyX"] = self.bytes_toint(raw_ints[8], raw_ints[9])
        vals["GyY"] = self.bytes_toint(raw_ints[10], raw_ints[11])
        vals["GyZ"] = self.bytes_toint(raw_ints[12], raw_ints[13])
        return vals  # returned in range of Int16
        # -32768 to 32767

    def val_test(self):  # ONLY FOR TESTING! Also, fast reading sometimes crashes IIC
        from time import sleep
        while 1:
            print(self.get_values())
            sleep(0.05)
            
           
    def mean(self, times):
        i = 0
        while i < times:
            ad = self.get_values()
            self.axo += ad["AcX"]
            self.ayo += ad["AcY"]
            self.azo += ad["AcZ"]
            self.gxo += ad["GyX"]
            self.gyo += ad["GyY"]
            self.gzo += ad["GyZ"]
            i += 1
        self.axo /= times
        self.ayo /= times
        self.azo /= times
        self.gxo /= times
        self.gyo /= times
        self.gzo /= times
        self.flag = True
           
           
    def kfilter(self):
        ad = self.get_values()

        # 加速度计
        ax = 2*9.8*ad["AcX"]/32768
        ay = 2*9.8*ad["AcY"]/32768
        az = 2*9.8*ad["AcZ"]/32768
        # 弧度制角度
        if az == 0 and ay >= 0:
            self.aax = -90
        elif az == 0 and ay < 0:
            self.aax = 90
        else:
            self.aax = math.atan(ay/az)*(-180)/math.pi
        
        if az == 0 and ax >= 0:
            self.aay = 90
        elif az == 0 and ax < 0:
            self.aay = -90
        else:
            self.aay = math.atan(ax/az)*180/math.pi
        
        if ay == 0 and az >= 0:
            self.aaz = 90
        elif ay == 0 and az < 0:
            self.aaz = -90
        else:
            self.aaz = math.atan(az/ay)*180/math.pi
        # 角速度计
        self.agx = -1000*(ad["GyX"]-self.gxo)/32768
        self.agy = -1000*(ad["GyY"]-self.gyo)/32768
        self.agz = -1000*(ad["GyZ"]-self.gzo)/32768
        
        self.x,self.gx = self.k(self.x, self.aax, self.agy)
        
    def k(self, angle, a, g):
        angle += (g - self.q_bias)*self.dt
        self.angle_err = a - angle
        
        self.Pdot[0] = self.Q_angle - self.P[0][1] - self.P[1][0]
        self.Pdot[1] = - self.P[1][1]
        self.Pdot[2] = - self.P[1][1]
        self.Pdot[3] = self.Q_gyro
        
        self.P[0][0] += self.Pdot[0]*self.dt
        self.P[0][1] += self.Pdot[1]*self.dt
        self.P[1][0] += self.Pdot[2]*self.dt
        self.P[1][1] += self.Pdot[3]*self.dt
        
        self.PCt_0 = self.C_0 * self.P[0][0]
        self.PCt_1 = self.C_0 * self.P[1][0]
        
        self.E  = self.R_angle + self.C_0 * self.PCt_0
        self.K_0 = self.PCt_0 / self.E
        self.K_1 = self.PCt_1 / self.E
        
        self.t_0 = self.PCt_0
        self.t_1 = self.C_0 * self.P[0][1]
        
        self.P[0][0] -= self.K_0 * self.t_0
        self.P[0][1] -= self.K_0 * self.t_1
        self.P[1][0] -= self.K_1 * self.t_0
        self.P[1][1] -= self.K_1 * self.t_1
        
        angle += self.K_0 * self.angle_err
        self.q_bias += self.K_1 * self.angle_err
        gx = g - self.q_bias
        return angle,gx

使用方法

from machine import I2C,Pin
import mpu6050
import time


# 初始化
i2c = I2C(scl=Pin(26), sda=Pin(25))
accel = mpu6050.accel(i2c)
accel_dict = accel.get_values()
print(accel_dict)

# 计算误差（测量200次取平均值）
accel.mean(200)
print(accel.axo,accel.ayo,accel.azo,accel.gxo,accel.gyo,accel.gzo)

# accel.aax x轴角度
# accel.agx x轴角速度
# accel.aay y轴角度
# accel.agy y轴角速度
# accel.aaz z轴角度
# accel.agz z轴角速度
while True:
    print(accel.aax,accel.agx,accel.aay,accel.agy,accel.aaz,accel.agz)
    time.sleep(0.1)

参考

Micropython——六轴MPU6050模块的使用

青右

关注

3
点赞
踩
22

收藏

觉得还不错? 一键收藏
打赏
1
评论
micropython编程 esp32+mpu6050

根据网上的C语言版本改写的python代码，滞后问题严重，这里贴出对X轴使用卡尔曼滤波的代码。模块内使用了esp32的定时器timer0，未使用滤波算法，响应比较及时。
复制链接

扫一扫