【K210+micropython】驱动MPU6050读取欧拉角_mpu6050计算角度micropython

【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)
1

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

• 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)模式
• scl： SCL 引脚，直接传引脚编号即可，取值范围： [0,47]。 可以不设置，而是使用 fm 统一管理引脚映射。
• sda： SDA 引脚，直接传引脚编号即可，取值范围： [0,47]。 可以不设置，而是使用 fm 统一管理引脚映射。
• freq： I2C通信频率， 支持标准100Kb/s, 快速400Kb/s， 以及更高速率（硬件支持超快速模式1000Kb/s，以及高速模式3.4Mb/s）

一、MPU6050

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

三、硬件连接

四、代码

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203

这个文件主要存放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]]

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66

这个文件是实现互补滤波算法的，我们从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)

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346

上面代码便是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_deadzone 和 default_calibration_gyro_deadzone两个阈值设置过小，导致所需校准次数过大，导致超出最大的passno。所有可以将这两个全局变量设置得大一点，但是代价是牺牲了精准度。

• 校准后还是不够精准

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

• 航偏角（yaw角）问题

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