【K210+micropython】驱动MPU6050读取欧拉角_mpu6050计算角度micropython-程序员宅基地

技术标签: microPython+K210  python  嵌入式  

【K210+micropython】驱动MPU6050读取欧拉角



相关知识准备


一、I2C

I2C 总线协议,简单地使用两条线(SCL,SDA)可以控制多个从机(主机模式)。
构造函数如下:

class machine.I2C(id, mode=I2C.MODE_MASTER, scl=None, sda=None, gscl=None, gsda=None, freq=400000, timeout=1000, addr=0, addr_size=7, on_recieve=None, on_transmit=None, on_event=None)

我们主要使用主机模式,所以要了解并能够修改的参数如下:

  • id: I2C ID, [0~2] (I2C.I2C0~I2C.I2C2) [3~5] (I2C.I2C3~I2C.I2C5, I2C_SOFT) 是软模拟 I2C 的编号
  • mode: 模式, 主机(I2C.MODE_MASTER)和从机(I2C.MODE_SLAVE)模式
  • sclSCL 引脚,直接传引脚编号即可,取值范围: [0,47]。 可以不设置,而是使用 fm 统一管理引脚映射。
  • sdaSDA 引脚,直接传引脚编号即可,取值范围: [0,47]。 可以不设置,而是使用 fm 统一管理引脚映射。
  • freq: I2C通信频率, 支持标准100Kb/s, 快速400Kb/s, 以及更高速率(硬件支持超快速模式1000Kb/s,以及高速模式3.4Mb/s)

一、MPU6050

MPU6050是一种非常流行的空间运动传感器芯片,可以获取器件当前的三个加速度分量和三个旋转角速度。MPU6050芯片内自带了一个数据处理子模块DMP,已经内置了滤波算法,在许多应用中使用DMP输出的数据已经能够很好的满足要求。在使用时只需要将偏移量减去即可使用,如果对精度有更高的要求可以使用上位机或这里使用卡尔曼滤波再次对数据进行优化。
在这里插入图片描述

三、硬件连接

K210 MPU6050
VCC VCC
GND GND
SCL(引脚25,i2c) SCL
SDA(引脚24,i2c) SDA

四、代码

1.mpu6050驱动相关库文件

我所使用的mpu6050库是修改的github上larsks大佬的esp8266+mpu6050代码。
源代码github链接如下:https://github.com/larsks/py-mpu6050

constants.py:

# constants extracted from
# https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/MPU6050/MPU6050.h

MPU6050_ADDRESS_AD0_LOW               = 0x68
MPU6050_ADDRESS_AD0_HIGH              = 0x69
MPU6050_DEFAULT_ADDRESS               = MPU6050_ADDRESS_AD0_LOW
MPU6050_RA_XG_OFFS_TC                 = 0x00
MPU6050_RA_YG_OFFS_TC                 = 0x01
MPU6050_RA_ZG_OFFS_TC                 = 0x02
MPU6050_RA_X_FINE_GAIN                = 0x03
MPU6050_RA_Y_FINE_GAIN                = 0x04
MPU6050_RA_Z_FINE_GAIN                = 0x05
MPU6050_RA_XA_OFFS_H                  = 0x06
MPU6050_RA_XA_OFFS_L_TC               = 0x07
MPU6050_RA_YA_OFFS_H                  = 0x08
MPU6050_RA_YA_OFFS_L_TC               = 0x09
MPU6050_RA_ZA_OFFS_H                  = 0x0A
MPU6050_RA_ZA_OFFS_L_TC               = 0x0B
MPU6050_RA_SELF_TEST_X                = 0x0D
MPU6050_RA_SELF_TEST_Y                = 0x0E
MPU6050_RA_SELF_TEST_Z                = 0x0F
MPU6050_RA_SELF_TEST_A                = 0x10
MPU6050_RA_XG_OFFS_USRH               = 0x13
MPU6050_RA_XG_OFFS_USRL               = 0x14
MPU6050_RA_YG_OFFS_USRH               = 0x15
MPU6050_RA_YG_OFFS_USRL               = 0x16
MPU6050_RA_ZG_OFFS_USRH               = 0x17
MPU6050_RA_ZG_OFFS_USRL               = 0x18
MPU6050_RA_SMPLRT_DIV                 = 0x19
MPU6050_RA_CONFIG                     = 0x1A
MPU6050_RA_GYRO_CONFIG                = 0x1B
MPU6050_RA_ACCEL_CONFIG               = 0x1C
MPU6050_RA_FF_THR                     = 0x1D
MPU6050_RA_FF_DUR                     = 0x1E
MPU6050_RA_MOT_THR                    = 0x1F
MPU6050_RA_MOT_DUR                    = 0x20
MPU6050_RA_ZRMOT_THR                  = 0x21
MPU6050_RA_ZRMOT_DUR                  = 0x22
MPU6050_RA_FIFO_EN                    = 0x23
MPU6050_RA_INT_PIN_CFG                = 0x37
MPU6050_RA_INT_ENABLE                 = 0x38
MPU6050_RA_DMP_INT_STATUS             = 0x39
MPU6050_RA_INT_STATUS                 = 0x3A
MPU6050_RA_ACCEL_XOUT_H               = 0x3B
MPU6050_RA_ACCEL_XOUT_L               = 0x3C
MPU6050_RA_ACCEL_YOUT_H               = 0x3D
MPU6050_RA_ACCEL_YOUT_L               = 0x3E
MPU6050_RA_ACCEL_ZOUT_H               = 0x3F
MPU6050_RA_ACCEL_ZOUT_L               = 0x40
MPU6050_RA_TEMP_OUT_H                 = 0x41
MPU6050_RA_TEMP_OUT_L                 = 0x42
MPU6050_RA_GYRO_XOUT_H                = 0x43
MPU6050_RA_GYRO_XOUT_L                = 0x44
MPU6050_RA_GYRO_YOUT_H                = 0x45
MPU6050_RA_GYRO_YOUT_L                = 0x46
MPU6050_RA_GYRO_ZOUT_H                = 0x47
MPU6050_RA_GYRO_ZOUT_L                = 0x48
MPU6050_RA_MOT_DETECT_STATUS          = 0x61
MPU6050_RA_SIGNAL_PATH_RESET          = 0x68
MPU6050_RA_MOT_DETECT_CTRL            = 0x69
MPU6050_RA_USER_CTRL                  = 0x6A
MPU6050_RA_PWR_MGMT_1                 = 0x6B
MPU6050_RA_PWR_MGMT_2                 = 0x6C
MPU6050_RA_BANK_SEL                   = 0x6D
MPU6050_RA_MEM_START_ADDR             = 0x6E
MPU6050_RA_MEM_R_W                    = 0x6F
MPU6050_RA_DMP_CFG_1                  = 0x70
MPU6050_RA_DMP_CFG_2                  = 0x71
MPU6050_RA_FIFO_COUNTH                = 0x72
MPU6050_RA_FIFO_COUNTL                = 0x73
MPU6050_RA_FIFO_R_W                   = 0x74
MPU6050_RA_WHO_AM_I                   = 0x75
MPU6050_TC_PWR_MODE_BIT               = 7
MPU6050_TC_OFFSET_BIT                 = 6
MPU6050_TC_OFFSET_LENGTH              = 6
MPU6050_TC_OTP_BNK_VLD_BIT            = 0
MPU6050_VDDIO_LEVEL_VLOGIC            = 0
MPU6050_VDDIO_LEVEL_VDD               = 1
MPU6050_CFG_EXT_SYNC_SET_BIT          = 5
MPU6050_CFG_EXT_SYNC_SET_LENGTH       = 3
MPU6050_CFG_DLPF_CFG_BIT              = 2
MPU6050_CFG_DLPF_CFG_LENGTH           = 3
MPU6050_DLPF_BW_256                   = 0x00
MPU6050_DLPF_BW_188                   = 0x01
MPU6050_DLPF_BW_98                    = 0x02
MPU6050_DLPF_BW_42                    = 0x03
MPU6050_DLPF_BW_20                    = 0x04
MPU6050_DLPF_BW_10                    = 0x05
MPU6050_DLPF_BW_5                     = 0x06
MPU6050_GCONFIG_FS_SEL_BIT            = 4
MPU6050_GCONFIG_FS_SEL_LENGTH         = 2
MPU6050_GYRO_FS_250                   = 0x00
MPU6050_GYRO_FS_500                   = 0x01
MPU6050_GYRO_FS_1000                  = 0x02
MPU6050_GYRO_FS_2000                  = 0x03
MPU6050_ACONFIG_XA_ST_BIT             = 7
MPU6050_ACONFIG_YA_ST_BIT             = 6
MPU6050_ACONFIG_ZA_ST_BIT             = 5
MPU6050_ACONFIG_AFS_SEL_BIT           = 4
MPU6050_ACONFIG_AFS_SEL_LENGTH        = 2
MPU6050_ACONFIG_ACCEL_HPF_BIT         = 2
MPU6050_ACONFIG_ACCEL_HPF_LENGTH      = 3
MPU6050_ACCEL_FS_2                    = 0x00
MPU6050_ACCEL_FS_4                    = 0x01
MPU6050_ACCEL_FS_8                    = 0x02
MPU6050_ACCEL_FS_16                   = 0x03
MPU6050_DHPF_RESET                    = 0x00
MPU6050_DHPF_5                        = 0x01
MPU6050_DHPF_2P5                      = 0x02
MPU6050_DHPF_1P25                     = 0x03
MPU6050_DHPF_0P63                     = 0x04
MPU6050_DHPF_HOLD                     = 0x07
MPU6050_TEMP_FIFO_EN_BIT              = 7
MPU6050_XG_FIFO_EN_BIT                = 6
MPU6050_YG_FIFO_EN_BIT                = 5
MPU6050_ZG_FIFO_EN_BIT                = 4
MPU6050_ACCEL_FIFO_EN_BIT             = 3
MPU6050_INTCFG_INT_LEVEL_BIT          = 7
MPU6050_INTCFG_INT_OPEN_BIT           = 6
MPU6050_INTCFG_LATCH_INT_EN_BIT       = 5
MPU6050_INTCFG_INT_RD_CLEAR_BIT       = 4
MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT    = 3
MPU6050_INTCFG_FSYNC_INT_EN_BIT       = 2
MPU6050_INTCFG_CLKOUT_EN_BIT          = 0
MPU6050_INTMODE_ACTIVEHIGH            = 0x00
MPU6050_INTMODE_ACTIVELOW             = 0x01
MPU6050_INTDRV_PUSHPULL               = 0x00
MPU6050_INTDRV_OPENDRAIN              = 0x01
MPU6050_INTLATCH_50USPULSE            = 0x00
MPU6050_INTLATCH_WAITCLEAR            = 0x01
MPU6050_INTCLEAR_STATUSREAD           = 0x00
MPU6050_INTCLEAR_ANYREAD              = 0x01
MPU6050_INTERRUPT_FF_BIT              = 7
MPU6050_INTERRUPT_MOT_BIT             = 6
MPU6050_INTERRUPT_ZMOT_BIT            = 5
MPU6050_INTERRUPT_FIFO_OFLOW_BIT      = 4
MPU6050_INTERRUPT_PLL_RDY_INT_BIT     = 2
MPU6050_INTERRUPT_DMP_INT_BIT         = 1
MPU6050_INTERRUPT_DATA_RDY_BIT        = 0
MPU6050_DMPINT_5_BIT                  = 5
MPU6050_DMPINT_4_BIT                  = 4
MPU6050_DMPINT_3_BIT                  = 3
MPU6050_DMPINT_2_BIT                  = 2
MPU6050_DMPINT_1_BIT                  = 1
MPU6050_DMPINT_0_BIT                  = 0
MPU6050_MOTION_MOT_XNEG_BIT           = 7
MPU6050_MOTION_MOT_XPOS_BIT           = 6
MPU6050_MOTION_MOT_YNEG_BIT           = 5
MPU6050_MOTION_MOT_YPOS_BIT           = 4
MPU6050_MOTION_MOT_ZNEG_BIT           = 3
MPU6050_MOTION_MOT_ZPOS_BIT           = 2
MPU6050_MOTION_MOT_ZRMOT_BIT          = 0
MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT = 7
MPU6050_PATHRESET_GYRO_RESET_BIT      = 2
MPU6050_PATHRESET_ACCEL_RESET_BIT     = 1
MPU6050_PATHRESET_TEMP_RESET_BIT      = 0
MPU6050_DETECT_ACCEL_ON_DELAY_BIT     = 5
MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH  = 2
MPU6050_DETECT_FF_COUNT_BIT           = 3
MPU6050_DETECT_FF_COUNT_LENGTH        = 2
MPU6050_DETECT_MOT_COUNT_BIT          = 1
MPU6050_DETECT_MOT_COUNT_LENGTH       = 2
MPU6050_DETECT_DECREMENT_RESET        = 0x0
MPU6050_DETECT_DECREMENT_1            = 0x1
MPU6050_DETECT_DECREMENT_2            = 0x2
MPU6050_DETECT_DECREMENT_4            = 0x3
MPU6050_USERCTRL_DMP_EN_BIT           = 7
MPU6050_USERCTRL_FIFO_EN_BIT          = 6
MPU6050_USERCTRL_DMP_RESET_BIT        = 3
MPU6050_USERCTRL_FIFO_RESET_BIT       = 2
MPU6050_USERCTRL_SIG_COND_RESET_BIT   = 0
MPU6050_PWR1_DEVICE_RESET_BIT         = 7
MPU6050_PWR1_SLEEP_BIT                = 6
MPU6050_PWR1_CYCLE_BIT                = 5
MPU6050_PWR1_TEMP_DIS_BIT             = 3
MPU6050_PWR1_CLKSEL_BIT               = 2
MPU6050_PWR1_CLKSEL_LENGTH            = 3
MPU6050_CLOCK_INTERNAL                = 0x00
MPU6050_CLOCK_PLL_XGYRO               = 0x01
MPU6050_CLOCK_PLL_YGYRO               = 0x02
MPU6050_CLOCK_PLL_ZGYRO               = 0x03
MPU6050_CLOCK_PLL_EXT32K              = 0x04
MPU6050_CLOCK_PLL_EXT19M              = 0x05
MPU6050_CLOCK_KEEP_RESET              = 0x07
MPU6050_PWR2_LP_WAKE_CTRL_BIT         = 7
MPU6050_PWR2_LP_WAKE_CTRL_LENGTH      = 2
MPU6050_PWR2_STBY_XA_BIT              = 5
MPU6050_PWR2_STBY_YA_BIT              = 4
MPU6050_PWR2_STBY_ZA_BIT              = 3
MPU6050_PWR2_STBY_XG_BIT              = 2
MPU6050_PWR2_STBY_YG_BIT              = 1
MPU6050_PWR2_STBY_ZG_BIT              = 0
MPU6050_WAKE_FREQ_1P25                = 0x0
MPU6050_WAKE_FREQ_2P5                 = 0x1
MPU6050_WAKE_FREQ_5                   = 0x2
MPU6050_WAKE_FREQ_10                  = 0x3
MPU6050_WHO_AM_I_BIT                  = 6
MPU6050_WHO_AM_I_LENGTH               = 6
MPU_SCL_PIN                           = 13
MPU_SDA_PIN                           = 12
MPU_DATA_RDY_PIN                      = 14
MPU_ADDR                              = MPU6050_DEFAULT_ADDRESS

这个文件主要存放mpu6050中寄存器值和地址的对应关系,方便之后通过i2c的读写操作mpu6050

cfilter.py:

import micropython
import math
import time

class ComplementaryFilter(object):
    def __init__(self, gyro_weight=0.95):
        self.gyro_weight = gyro_weight
        self.reset()

    def reset(self):
        self.last = 0

        self.accel_pos = [0, 0, 0]
        self.gyro_pos = [0, 0, 0]
        self.filter_pos = [0, 0, 0]

    def reset_gyro(self):
        self.gyro_pos = self.filter_pos

    def input(self, vals):
        now = time.ticks_ms()

        # unpack sensor readings
        accel_data = vals[0:3]
        gyro_data = vals[4:7]

        # convert accelerometer reading to degrees
        self.accel_pos = self.calculate_accel_pos(*accel_data)

        # if this is our first chunk of data, simply accept
        # the accelerometer reads and move on.
        if self.last == 0:
            self.filter_pos = self.gyro_pos = self.accel_pos
            self.last = now
            return

        # calculate the elapsed time (in seconds) since last data.
        # we need this because the gyroscope measures movement in
        # degrees/second.
        dt = time.ticks_diff(now, self.last)/1000
        self.last = now

        # calculate change in position from gyroscope readings
        gyro_delta = [i * dt for i in gyro_data]
        self.gyro_pos = [i + j for i, j in zip(self.gyro_pos, gyro_delta)]

        # pitch
        self.filter_pos[0] = (
            self.gyro_weight * (self.filter_pos[0] + gyro_delta[0])
            + (1-self.gyro_weight) * self.accel_pos[0])

        # roll
        self.filter_pos[1] = (
            self.gyro_weight * (self.filter_pos[1] + gyro_delta[1])
            + (1-self.gyro_weight) * self.accel_pos[1])

    def calculate_accel_pos(self, x, y, z):
        x2 = (x*x);
        y2 = (y*y);
        z2 = (z*z);

        adx = math.atan2(y, z)
        ady = math.atan2(-x, math.sqrt(y2 + z2))

        return [math.degrees(x) for x in [adx, ady, 0]]

这个文件是实现互补滤波算法的,我们从mpu6050中读出的数据主要有六个:三个轴的加速度和三个轴的角速度。如果要计算mpu6050的欧拉角,就要对这些数据进行处理,一般常用方法有:四元法、一阶互补滤波、卡尔曼滤波算法。所以这个库实现的便是三个欧拉角(俯仰角【pitch】,偏航角【yaw】,滚动角【roll】)的计算。之于何为欧拉角,上图
在这里插入图片描述

mpu6050.py:

import gc
from machine import I2C, PWM ,Timer
import time
import micropython
from ustruct import unpack
from Maix import GPIO
from board import board_info
from fpioa_manager import fm

from constants import *
import cfilter

micropython.alloc_emergency_exception_buf(100)

default_pin_scl = 25
default_pin_sda = 24
default_pin_intr = board_info.BOOT_KEY
default_pin_led = board_info.LED_B
default_sample_rate = 0x20

default_calibration_numsamples = 200
default_calibration_accel_deadzone = 15
default_calibration_gyro_deadzone = 5

accel_range = [2, 4, 8, 16]
gyro_range = [250, 500, 1000, 2000]

# These are what the sensors ought to read at rest
# on a level surface
expected = [0, 0, 16384, None, 0, 0, 0]

class CalibrationFailure(Exception):
    pass

class MPU(object):
    def __init__(self, scl=None, sda=None,
                 intr=None, led=None, rate=None,
                 address=None):

        self.scl = scl if scl is not None else default_pin_scl
        self.sda = sda if sda is not None else default_pin_sda
        self.intr = intr if intr is not None else default_pin_intr
        self.led = led if led is not None else default_pin_led
        self.rate = rate if rate is not None else default_sample_rate

        self.address = address if address else MPU6050_DEFAULT_ADDRESS

        self.buffer = bytearray(16)
        self.bytebuf = memoryview(self.buffer[0:1])
        self.wordbuf = memoryview(self.buffer[0:2])
        self.sensors = bytearray(14)

        self.calibration = [0] * 7

        self.filter = cfilter.ComplementaryFilter()

        self.init_pins()
        self.init_led()
        self.init_i2c()
        self.init_device()

    def write_byte(self, reg, val):
        self.bytebuf[0] = val
        self.bus.writeto_mem(self.address, reg, self.bytebuf)

    def read_byte(self, reg):
        self.bus.readfrom_mem_into(self.address, reg, self.bytebuf)
        return self.bytebuf[0]

    def set_bitfield(self, reg, pos, length, val):
        old = self.read_byte(reg)
        shift = pos - length + 1
        mask = (2**length - 1) << shift
        new = (old & ~mask) | (val << shift)
        self.write_byte(reg, new)

    def read_word(self, reg):
        self.bus.readfrom_mem_into(self.address, reg, self.wordbuf)
        return unpack('>H', self.wordbuf)[0]

    def read_word2(self, reg):
        self.bus.readfrom_mem_into(self.address, reg, self.wordbuf)
        return unpack('>h', self.wordbuf)[0]

    def init_i2c(self):
        print('* initializing i2c')
        self.bus = i2c = I2C(I2C.I2C0, mode=I2C.MODE_MASTER, freq=3400000, scl=self.pin_scl, sda=self.pin_sda, addr_size=7)

    def init_pins(self):
        print('* initializing pins')
        self.pin_sda = self.sda
        self.pin_scl = self.scl
        fm.register(board_info.BOOT_KEY, fm.fpioa.GPIOHS0)
        self.pin_intr = GPIO(GPIO.GPIOHS0, GPIO.IN, GPIO.PULL_NONE)
        tim = Timer(Timer.TIMER0, Timer.CHANNEL0, mode=Timer.MODE_PWM)
        self.pin_led = PWM(tim, freq=500000, duty=50, pin=self.led)

    def set_state_uncalibrated(self):
        self.pin_led.freq(1)
        self.pin_led.duty(50)

    def set_state_calibrating(self):
        self.pin_led.freq(10)
        self.pin_led.duty(50)

    def set_state_calibrated(self):
        self.pin_led.freq(1000)
        self.pin_led.duty(50)

    def set_state_disabled(self):
        self.pin_led.duty(0)

    def init_led(self):
        self.set_state_uncalibrated()

    def identify(self):
        print('* identifying i2c device')
        val = self.read_byte(MPU6050_RA_WHO_AM_I)
        if val != MPU6050_ADDRESS_AD0_LOW:
            raise OSError("No mpu6050 at address {}".format(self.address))

    def reset(self):
        print('* reset')
        self.write_byte(MPU6050_RA_PWR_MGMT_1, (
            (1 << MPU6050_PWR1_DEVICE_RESET_BIT)
        ))
        time.sleep_ms(100)

        self.write_byte(MPU6050_RA_SIGNAL_PATH_RESET, (
            (1 << MPU6050_PATHRESET_GYRO_RESET_BIT) |
            (1 << MPU6050_PATHRESET_ACCEL_RESET_BIT) |
            (1 << MPU6050_PATHRESET_TEMP_RESET_BIT)
        ))
        time.sleep_ms(100)

    def init_device(self):
        print('* initializing mpu')

        self.identify()

        # disable sleep mode and select clock source
        self.write_byte(MPU6050_RA_PWR_MGMT_1, MPU6050_CLOCK_PLL_XGYRO)

        # enable all sensors
        self.write_byte(MPU6050_RA_PWR_MGMT_2, 0)

        # set sampling rate
        self.write_byte(MPU6050_RA_SMPLRT_DIV, self.rate)

        # enable dlpf
        self.write_byte(MPU6050_RA_CONFIG, 1)

        # explicitly set accel/gyro range
        self.set_accel_range(MPU6050_ACCEL_FS_2)
        self.set_gyro_range(MPU6050_GYRO_FS_250)

    def set_gyro_range(self, fsr):
        self.gyro_range = gyro_range[fsr]
        self.set_bitfield(MPU6050_RA_GYRO_CONFIG,
                          MPU6050_GCONFIG_FS_SEL_BIT,
                          MPU6050_GCONFIG_FS_SEL_LENGTH,
                          fsr)

    def set_accel_range(self, fsr):
        self.accel_range = accel_range[fsr]
        self.set_bitfield(MPU6050_RA_ACCEL_CONFIG,
                          MPU6050_ACONFIG_AFS_SEL_BIT,
                          MPU6050_ACONFIG_AFS_SEL_LENGTH,
                          fsr)

    def read_sensors(self):
        self.bus.readfrom_mem_into(self.address,
                                   MPU6050_RA_ACCEL_XOUT_H,
                                   self.sensors)

        data = unpack('>hhhhhhh', self.sensors)

        # apply calibration values
        return [data[i] + self.calibration[i] for i in range(7)]

    def read_sensors_scaled(self):
        data = self.read_sensors()
        data[0:3] = [x/(65536//self.accel_range//2) for x in data[0:3]]
        data[4:7] = [x/(65536//self.gyro_range//2) for x in data[4:7]]
        return data

    def read_position(self):
        self.filter.input(self.read_sensors_scaled())
        return [
            self.filter.filter_pos,
            self.filter.accel_pos,
            self.filter.gyro_pos,
        ]

    def set_dhpf_mode(self, bandwidth):
        self.set_bitfield(MPU6050_RA_ACCEL_CONFIG,
                          MPU6050_ACONFIG_ACCEL_HPF_BIT,
                          MPU6050_ACONFIG_ACCEL_HPF_LENGTH,
                          bandwidth)

    def set_motion_detection_threshold(self, threshold):
        self.write_byte(MPU6050_RA_MOT_THR, threshold)

    def set_motion_detection_duration(self, duration):
        self.write_byte(MPU6050_RA_MOT_DUR, duration)

    def set_int_motion_enabled(self, enabled):
        self.set_bitfield(MPU6050_RA_INT_ENABLE,
                          MPU6050_INTERRUPT_MOT_BIT,
                          1,
                          enabled)

    def get_sensor_avg(self, samples, softstart=100):
        '''Return the average readings from the sensors over the
        given number of samples.  Discard the first softstart
        samples to give things time to settle.'''
        sample = self.read_sensors()
        counters = [0] * 7

        for i in range(samples + softstart):
            # the sleep here is to ensure we read a new sample
            # each time
            time.sleep_ms(50)

            sample = self.read_sensors()
            if i < softstart:
                continue

            for j, val in enumerate(sample):
                counters[j] += val

        return [x//samples for x in counters]

    stable_reading_timeout = 10
    max_gyro_variance = 5

    def wait_for_stable(self, numsamples=10):
        print('* waiting for gyros to stabilize')

        gc.collect()
        time_start = time.time()
        samples = []

        while True:
            now = time.time()
            if now - time_start > self.stable_reading_timeout:
                raise CalibrationFailure()

            # the sleep here is to ensure we read a new sample
            # each time
            time.sleep_ms(2)

            sample = self.read_sensors()
            samples.append(sample[4:7])
            if len(samples) < numsamples:
                continue

            samples = samples[-numsamples:]

            totals = [0] * 3
            for cola, colb in zip(samples, samples[1:]):
                deltas = [abs(a-b) for a,b in zip(cola, colb)]
                totals = [a+b for a,b in zip(deltas, totals)]

            avg = [a/numsamples for a in totals]

            if all(x < self.max_gyro_variance for x in avg):
                break

        now = time.time()
        print('* gyros stable after {:0.2f} seconds'.format(now-time_start))

    def calibrate(self,
                  numsamples=None,
                  accel_deadzone=None,
                  gyro_deadzone=None):

        old_calibration = self.calibration
        self.calibration = [0] * 7

        numsamples = (numsamples if numsamples is not None
                   else default_calibration_numsamples)
        accel_deadzone = (accel_deadzone if accel_deadzone is not None
                          else default_calibration_accel_deadzone)
        gyro_deadzone = (gyro_deadzone if gyro_deadzone is not None
                         else default_calibration_gyro_deadzone)

        print('* start calibration')
        self.set_state_calibrating()

        try:
            self.wait_for_stable()
            gc.collect()

            # calculate offsets between the expected values and
            # the average value for each sensor reading
            avg = self.get_sensor_avg(numsamples)
            off = [0 if expected[i] is None else expected[i] - avg[i]
                   for i in range(7)]

            accel_ready = False
            gyro_read = False
            for passno in range(20):
                self.calibration = off
                avg = self.get_sensor_avg(numsamples)

                check = [0 if expected[i] is None else expected[i] - avg[i]
                       for i in range(7)]
                print('- pass {}: {}'.format(passno, check))

                # check if current values are within acceptable offsets
                # from the expected values
                accel_ready = all(abs(x) < accel_deadzone
                                  for x in check[0:3])
                gyro_ready = all(abs(x) < gyro_deadzone
                                 for x in check[4:7])

                if accel_ready and gyro_ready:
                    break

                if not accel_ready:
                    off[0:3] = [off[i] + check[i]//accel_deadzone
                                for i in range(3)]

                if not gyro_ready:
                    off[4:7] = [off[i] + check[i]//gyro_deadzone
                                for i in range(4, 7)]
            else:
                raise CalibrationFailure()
        except CalibrationFailure:
            self.calibration = old_calibration
            print('! calibration failed')
            self.set_state_uncalibrated()
            return

        print('* calibrated!')
        self.set_state_calibrated()


    def demo():
        mpu = MPU()
        mpu.calibrate()
        while True:
            print(mpu.read_position())
            time.sleep_ms(100)

上面代码便是mpu6050的驱动库,其中包含了对其的校准操作calibration,一般使用流程为:先初始化MPU对象,再调用calibrate方法进行校准陀螺仪,校准成功后,连续输出陀螺仪的位置(欧拉角)。
这里需要注意的是,如果需要校准陀螺仪,务必在运行程序前将陀螺仪水平放置,也就是陀螺仪的Z轴竖直向上,接下来终端会打印出陀螺仪运行状态。

遇到的问题和总结

  • 打印输出数据时意外中断,报错为IO类错误

    这个问题比较玄学。解决方法为:把mpu6050的读取频率降低,也就是每次读取的时间间隔增大。我初步猜测应该是i2c的通信频率达不到所导致。反正这类问题就是与mpu6050之间的通信间隔过短所致。

  • 校准失败

    终端打印! calibration failed。出现这个问题的原因主要有两种:
    1.calibrate方法中 for passno in range(20):这一行,其中passno为校准次数,一般越大,校准成功率也就越大,但是相应时间也花费得更多。如果校准次数过少,可能达不到预期设置的deadzone阈值,抛出CalibrationFailure异常,导致校准失败。
    2. default_calibration_accel_deadzonedefault_calibration_gyro_deadzone两个阈值设置过小,导致所需校准次数过大,导致超出最大的passno。所有可以将这两个全局变量设置得大一点,但是代价是牺牲了精准度。

  • 校准后还是不够精准

    可以尝试加大default_calibration_numsamples,以及检查一下mpu6050在校准时是否严格水平放置。

  • 航偏角(yaw角)问题

    为什么校准后yaw角数据不为0呢?其实航偏角在这个实验中是没有参考价值的,一般不使用,因为其需要地标。

版权声明:本文为博主原创文章,遵循 CC 4.0 BY-SA 版权协议,转载请附上原文出处链接和本声明。
本文链接:https://blog.csdn.net/qq_39784672/article/details/119270854

智能推荐

Oracle EBS R12 - Clone EBS R12.1.1 on Oracle Linux 64 5.7 to Oracle Linux 64 5.7_ebs clone 无法访问-程序员宅基地

文章浏览阅读3k次。Clone EBS R12.1.1 on Oracle Linux 64 5.7 to Oracle Linux 64 5.7基于 Oracle Linux 64 5.7 的 EBS R12.1.1 详细克隆步骤Section 0: VersionSource: R12.1.1 on Oracle Linux 64 5.7, SID=r12dev, host=r12dev.example.comT_ebs clone 无法访问

安装配置Kerberos_krb5.conf-程序员宅基地

文章浏览阅读2.8k次。kerberos简介 Kerberos是一种计算机网络认证协议,此协议可以保护网络实体免受窃听和重复攻击,它允许某实体在非安全网络环境下向另一个实体以一种安全的方式证明自己的身份。Kerberos由麻省理工实验室实现此协议并发布的一套免费软件。其设计主要是针对客户-服务器模型,且提供了一系列交互认证——用户和服务器都能高安全性的验证对方的身份。链接kerber..._krb5.conf

玩机:解决小米手机锁屏忘记密码无法解锁_miuiroot后如何清楚锁屏密码-程序员宅基地

文章浏览阅读1.4w次,点赞2次,收藏5次。本文分享了如何在忘记密码的情况下解锁小米5手机,理论上适用于小米5后的更新机器。_miuiroot后如何清楚锁屏密码

【Java】Spring的AOP及事务-程序员宅基地

文章浏览阅读918次,点赞20次,收藏27次。问题1:AOP的作用是什么?问题2:连接点和切入点有什么区别,二者谁的范围大?问题3:请描述什么是切面?AOP(Aspect Oriented Programming)面向切面编程,一种编程范式,指导开发者如何组织程序结构OOP(Object Oriented Programming)面向对象编程作用:在不惊动原始设计的基础上为其进行功能增强。简单的说就是在不改变方法源代码的基础上对方法进行功能增强。Spring理念:无入侵式/无侵入式。

java -cp 和 java -jar 区别-程序员宅基地

文章浏览阅读1.1k次。java -cp 和 -classpath 一样,是指定类运行所依赖其他类的路径,通常是类库,jar包之类,需要全路径到jar包,window上分号“;”格式:java -cp .;myClass.jar packname.mainclassname表达式支持通配符,例如:java -cp .;c:\classes01\myClass.jar;c:\classes02\*.jar..._java -cp

数据结构 树与二叉树 笔记合集(C语言)完结_树与二叉树笔记-程序员宅基地

文章浏览阅读1w次,点赞2次,收藏10次。树与二叉树树常考考点二叉树二叉树的顺序存储二叉树的链式存储二叉树的遍历二叉树的层次遍历线索二叉树找中序前驱中序线索化先序线索化中序线索二叉树找中序后继先序线索二叉树找先序后继先序线索二叉树找先序前驱树的存储结构双亲表示法(顺序存储)孩子兄弟表示法(链式存储) C树的层次遍历(广度优先遍历)树的先根、后根遍历(深度优先遍历)树·结点之间的路径只能从上往下·结点的度:结点的分支数·树的度:树中..._树与二叉树笔记

随便推点

使用LeNet对于旋转数字进行识别:合并数字集合_如何识别旋转数字-程序员宅基地

文章浏览阅读788次。简 介: 将所有机械旋转字符合成一个大的训练集合(3415个样本),使用其中80%作为训练样本集合,利用LeNet网络进行训练。最终在测试集合上获得95%的识别率。对于误差超过1的样本只要0.7%。关键词: LeNet,旋转数字#mermaid-svg-ad2d0Nt4ij3gL1Zu .label{font-family:'trebuchet ms', verdana, arial;font-family:var(--mermaid-font-family);fill:#333;color:#3.._如何识别旋转数字

第一个车载以太网应用-BMW 环视项目背景及历史 BMW Automotive Ethernet application background (1)_宝马以太网-程序员宅基地

文章浏览阅读1k次。本文主要介绍,BMW当初在汽车上应用以太网技术的背景及历史;此篇主要介绍BroadR-Reach/100BASE-T1技术的简要历史及背景;The Breakthrough: UTSP Ethernet for AutomotiveBMW decided to synchronize all hitherto knowledge with the future requirements..._宝马以太网

学习总结重载和重写的区别与联系_重载与重写的区别与联系-程序员宅基地

文章浏览阅读1k次。重载(Overloading)方法重载是让类以统一的方式处理不同类型数据的一种手段。多个同名函数同时存在,具有不同的参数个数/类型。重载Overloading是一个类中多态性的一种表现。Java的方法重载,就是在类中可以创建多个方法,它们具有相同的名字,但具有不同的参数和不同的定义。调用方法时通过传递给它们的不同参数个数和参数类型来决定具体使用哪个方法, 这就是多态性。..._重载与重写的区别与联系

python用pandas读取excel_Python使用pandas读取Excel文件数据和预处理小案例-程序员宅基地

文章浏览阅读589次。假设有Excel文件data.xlsx,其中内容为现在需要将这个Excel文件中的数据读入pandas,并且在后续的处理中不关心ID列,还需要把sex列的female替换为1,把sex列的male替换为0。本文演示有关的几个操作。(1)导入pandas模块>>> import pandas as pd(2)把Excel文件中的数据读入pandas>>> df = pd.read_excel('da..._如何将一个excel表格读取为pd.dataframe变量并进行数据预处理

C语言基础学习笔记——分支语句(if/if else/else if/switch)_c语言条件分支语句if else语法规则-程序员宅基地

文章浏览阅读650次。语句:一条完整的计算机指令。语句与表达式的区别:语句在结束处有分号。(如:a = 1 + 2是表达式。a = 1+ 2;是表达式语句)语句的分类:  ①表达式语句:在表达式后面加分号。    ②函数调用语句:函数名(实际参数列表);(如scanf( ); printf( ))    ③空语句:只有一个分号,在程序中尽量避免使用。    ④复合语句:花括号中的一条或多条语句。如 ..._c语言条件分支语句if else语法规则

推荐文章

热门文章

相关标签