1# -*- coding: UTF-8 -*-
2
3"""
4The driver for mpu6050 chip, it is a temperature and humidity sensor.
5"""
6from micropython import const
7from driver import I2C
8from utime import sleep_ms
9import math
10
11MPU_SELF_TESTX_REG = const(0X0D)   #自检寄存器X
12MPU_SELF_TESTY_REG = const(0X0E)   #自检寄存器Y
13MPU_SELF_TESTZ_REG = const(0X0F)   #自检寄存器Z
14MPU_SELF_TESTA_REG = const(0X10)   #自检寄存器A
15MPU_SAMPLE_RATE_REG = const(0X19)  #采样频率分频器
16MPU_CFG_REG         = const(0X1A)  #配置寄存器
17MPU_GYRO_CFG_REG    = const(0X1B)  #陀螺仪配置寄存器
18MPU_ACCEL_CFG_REG   = const(0X1C)  #加速度计配置寄存器
19MPU_MOTION_DET_REG  = const(0X1F)  #运动检测阀值设置寄存器
20MPU_FIFO_EN_REG     = const(0X23)  #FIFO使能寄存器
21MPU_I2CMST_CTRL_REG = const(0X24)  #IIC主机控制寄存器
22MPU_I2CSLV0_ADDR_REG = const(0X25) #IIC从机0器件地址寄存器
23MPU_I2CSLV0_REG      = const(0X26) #IIC从机0数据地址寄存器
24MPU_I2CSLV0_CTRL_REG = const(0X27) #IIC从机0控制寄存器
25MPU_I2CSLV1_ADDR_REG = const(0X28) #IIC从机1器件地址寄存器
26MPU_I2CSLV1_REG      = const(0X29) #IIC从机1数据地址寄存器
27MPU_I2CSLV1_CTRL_REG = const(0X2A) #IIC从机1控制寄存器
28MPU_I2CSLV2_ADDR_REG = const(0X2B) #IIC从机2器件地址寄存器
29MPU_I2CSLV2_REG      = const(0X2C) #IIC从机2数据地址寄存器
30MPU_I2CSLV2_CTRL_REG = const(0X2D) #IIC从机2控制寄存器
31MPU_I2CSLV3_ADDR_REG = const(0X2E) #IIC从机3器件地址寄存器
32MPU_I2CSLV3_REG      = const(0X2F) #IC从机3数据地址寄存器
33MPU_I2CSLV3_CTRL_REG = const(0X30) #IIC从机3控制寄存器
34MPU_I2CSLV4_ADDR_REG = const(0X31) #IIC从机4器件地址寄存器
35MPU_I2CSLV4_REG      = const(0X32) #IIC从机4数据地址寄存器
36MPU_I2CSLV4_DO_REG   = const(0X33) #IIC从机4写数据寄存器
37MPU_I2CSLV4_CTRL_REG = const(0X34) #IIC从机4控制寄存器
38MPU_I2CSLV4_DI_REG   = const(0X35) #IIC从机4读数据寄存器
39
40MPU_I2CMST_STA_REG = const(0X36) #IIC主机状态寄存器
41MPU_INTBP_CFG_REG  = const(0X37) #中断/旁路设置寄存器
42MPU_INT_EN_REG     = const(0X38) #中断使能寄存器
43MPU_INT_STA_REG    = const(0X3A) #中断状态寄存器
44
45MPU_ACCEL_XOUTH_REG = const(0X3B) #加速度值,X轴高8位寄存器
46MPU_ACCEL_XOUTL_REG = const(0X3C) #速度值,X轴低8位寄存器
47MPU_ACCEL_YOUTH_REG = const(0X3D) #加速度值,Y轴高8位寄存器
48MPU_ACCEL_YOUTL_REG = const(0X3E) #加速度值,Y轴低8位寄存器
49MPU_ACCEL_ZOUTH_REG = const(0X3F) #加速度值,Z轴高8位寄存器
50MPU_ACCEL_ZOUTL_REG = const(0X40) #加速度值,Z轴低8位寄存器
51
52MPU_TEMP_OUTH_REG = const(0X41) #温度值高八位寄存器
53MPU_TEMP_OUTL_REG = const(0X42) #温度值低8位寄存器
54
55MPU_GYRO_XOUTH_REG = const(0X43) #陀螺仪值,X轴高8位寄存器
56MPU_GYRO_XOUTL_REG = const(0X44) #陀螺仪值,X轴低8位寄存器
57MPU_GYRO_YOUTH_REG = const(0X45) #陀螺仪值,Y轴高8位寄存器
58MPU_GYRO_YOUTL_REG = const(0X46) #陀螺仪值,Y轴低8位寄存器
59MPU_GYRO_ZOUTH_REG = const(0X47) #陀螺仪值,Z轴高8位寄存器
60MPU_GYRO_ZOUTL_REG = const(0X48) #陀螺仪值,Z轴低8位寄存器
61
62MPU_I2CSLV0_DO_REG = const(0X63) #IIC从机0数据寄存器
63MPU_I2CSLV1_DO_REG = const(0X64) #IIC从机1数据寄存器
64MPU_I2CSLV2_DO_REG = const(0X65) #IIC从机2数据寄存器
65MPU_I2CSLV3_DO_REG = const(0X66) #IIC从机3数据寄存器
66
67MPU_I2CMST_DELAY_REG = const(0X67) #IIC主机延时管理寄存器
68MPU_SIGPATH_RST_REG  = const(0X68) #信号通道复位寄存器
69MPU_MDETECT_CTRL_REG = const(0X69) #运动检测控制寄存器
70MPU_USER_CTRL_REG    = const(0X6A) #用户控制寄存器
71MPU_PWR_MGMT1_REG    = const(0X6B) #电源管理寄存器1
72MPU_PWR_MGMT2_REG    = const(0X6C) #电源管理寄存器2
73MPU_FIFO_CNTH_REG    = const(0X72) #FIFO计数寄存器高八位
74MPU_FIFO_CNTL_REG    = const(0X73) #FIFO计数寄存器低八位
75MPU_FIFO_RW_REG      = const(0X74) #FIFO读写寄存器
76MPU_DEVICE_ID_REG    = const(0X75) #器件ID寄存器
77
78# 如果AD0脚(9脚)接地,IIC地址为0X68(不包含最低位).
79# 如果接V3.3,则IIC地址为0X69(不包含最低位).
80MPU_ADDR     = const(0X69)
81MPU_DEV_ID   = const(0x68)
82
83class MPU6050Error(Exception):
84    def __init__(self, value=0, msg="mpu6050 common error"):
85        self.value = value
86        self.msg = msg
87
88    def __str__(self):
89        return "Error code:%d, Error message: %s" % (self.value, str(self.msg))
90
91    __repr__ = __str__
92
93class MPU6050(object):
94    """
95    This class implements mpu6050 chip's defs.
96    """
97    def __init__(self):
98        self.i2cDev = None
99
100    def open(self, devid):
101        self.i2cDev = I2C()
102        self.i2cDev.open(devid)
103
104    def i2c_write_byte(self, addr, value):
105            Reg = bytearray([addr, value])
106            self.i2cDev.write(Reg)
107            print("--> write addr " + str(addr) + ", value = " + str(value))
108
109    def i2c_read_byte(self, addr):
110            Reg = bytearray([addr])
111            self.i2cDev.write(Reg)
112            tmp = bytearray(1)
113            self.i2cDev.read(tmp)
114            print("<-- read addr " + str(addr) + ", value = " + str(tmp[0]))
115            return tmp[0]
116
117    def i2c_read_len(self, addr, len):
118        reg = bytearray([addr])
119        data = bytearray(len)
120        self.i2cDev.write(reg)
121        sleep_ms(20)
122        self.i2cDev.read(data)
123        # print("--> read " + str(len) + " bytes from addr " + str(addr) + ", " + str(len) + " bytes value = " + str(data))
124        return data
125
126    # 设置MPU6050陀螺仪传感器满量程范围
127    # fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
128    # 返回值:0,设置成功
129    # 其他,设置失败
130    def MPU_Set_Gyro_Fsr(self, fsr):
131        return self.i2c_write_byte(MPU_GYRO_CFG_REG, fsr << 3) # 设置陀螺仪满量程范围
132
133    # 设置MPU6050加速度传感器满量程范围
134    # fsr:0,±2g;1,±4g;2,±8g;3,±16g
135    # 返回值:0,设置成功
136    # 其他,设置失败
137    def MPU_Set_Accel_Fsr(self, fsr):
138        return self.i2c_write_byte(MPU_ACCEL_CFG_REG, fsr << 3) # 设置加速度传感器满量程范围
139
140    # 设置MPU6050的数字低通滤波器
141    # lpf:数字低通滤波频率(Hz)
142    # 返回值:0,设置成功
143    # 其他,设置失败
144    def MPU_Set_LPF(self, lpf):
145        if (lpf >= 188):
146            data = 1
147        elif (lpf >= 98):
148            data = 2
149        elif (lpf >= 42):
150            data = 3
151        elif (lpf >= 20):
152            data = 4
153        elif (lpf >= 10):
154            data = 5
155        else:
156            data = 6
157
158        return self.i2c_write_byte(MPU_CFG_REG, data) # 设置数字低通滤波器
159
160    # 设置MPU6050的采样率(假定Fs=1KHz)
161    # rate:4~1000(Hz)
162    # 返回值:0,设置成功
163    # 其他,设置失败
164    def MPU_Set_Rate(self, rate):
165        if (rate > 1000):
166            rate = 1000
167        if (rate < 4):
168            rate = 4
169        data = 1000 // rate - 1
170        self.i2c_write_byte(MPU_SAMPLE_RATE_REG, data) # 设置数字低通滤波器
171        return self.MPU_Set_LPF(rate / 2) # 自动设置LPF为采样率的一半
172
173    # 得到温度值
174    # 返回值:温度值(扩大了100倍)
175    def get_Temperature(self):
176        buf = bytearray(2)
177        buf = self.i2c_read_len(MPU_TEMP_OUTH_REG, 2)
178        raw  = (buf[0] << 8) | buf[1]
179        print("get_Temperature:",buf[0], buf[1], raw)
180        if (raw > (1 << 15)):
181            raw = raw - (1<<16)
182
183        temp = 36.53 + (raw) / 340
184        return temp * 100
185
186    # 得到陀螺仪值(原始值)
187    # gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
188    # 返回值:0,成功
189    # 其他,错误代码
190    def get_Gyroscope(self):
191        arr = [1, 2, 3]
192        buf = bytearray(6)
193
194        buf = self.i2c_read_len(MPU_GYRO_XOUTH_REG, 6)
195        gx = (buf[0] << 8) | buf[1]
196        gy = (buf[2] << 8) | buf[3]
197        gz = (buf[4] << 8) | buf[5]
198
199        if (gx > (1 << 15)):
200            gx = gx - (1<<16)
201
202        if (gy > (1 << 15)):
203            gy = gy - (1<<16)
204
205        if (gz > (1 << 15)):
206            gz = gz - (1<<16)
207
208        arr[0] = gx
209        arr[1] = gy
210        arr[2] = gz
211
212        return arr
213
214    # 得到加速度值(原始值)
215    # gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
216    # 返回值:0,成功
217    # 其他,错误代码
218    def get_Accelerometer(self):
219        arr = [1, 2, 3]
220        buf = bytearray(6)
221
222        buf = self.i2c_read_len(MPU_ACCEL_XOUTH_REG, 6)
223        ax = (buf[0] << 8) | buf[1]
224        ay = (buf[2] << 8) | buf[3]
225        az = (buf[4] << 8) | buf[5]
226
227        if (ax > (1 << 15)):
228            ax = ax - (1<<16)
229
230        if (ay > (1 << 15)):
231            ay = ay - (1<<16)
232
233        if (az > (1 << 15)) :
234            az = az - (1<<16)
235
236        arr[0] = ax
237        arr[1] = ay
238        arr[2] = az
239
240        return arr
241
242    # 初始化MPU6050
243    # 返回值:0,成功
244    # 其他,错误代码
245    def init(self):
246        device_id = 0
247
248        self.i2c_write_byte(MPU_PWR_MGMT1_REG, 0X80)  # 复位MPU6050
249        sleep_ms(200)
250        self.i2c_write_byte(MPU_PWR_MGMT1_REG, 0X00)  # 唤醒MPU6050
251        self.MPU_Set_Gyro_Fsr(3)                      # 陀螺仪传感器,±2000dps
252        self.MPU_Set_Accel_Fsr(0)                     # 加速度传感器,±2g
253        self.MPU_Set_Rate(50)                         # 设置采样率50Hz
254        self.i2c_write_byte(MPU_INT_EN_REG, 0X00)     # 关闭所有中断
255        self.i2c_write_byte(MPU_USER_CTRL_REG, 0X00)  # I2C主模式关闭
256        self.i2c_write_byte(MPU_FIFO_EN_REG, 0X00)    # 关闭FIFO
257        self.i2c_write_byte(MPU_INTBP_CFG_REG, 0X80)  # INT引脚低电平有效
258        device_id = self.i2c_read_byte(MPU_DEVICE_ID_REG)
259        if (device_id == MPU_DEV_ID):
260            # 器件ID正确
261            self.i2c_write_byte(MPU_PWR_MGMT1_REG, 0X01) # 设置CLKSEL,PLL X轴为参考
262            self.i2c_write_byte(MPU_PWR_MGMT2_REG, 0X00) # 加速度与陀螺仪都工作
263            self.MPU_Set_Rate(50)                        # 设置采样率为50Hz
264            return 0
265        else:
266            return 1
267
268    def close(self):
269        self.i2cDev.close()
270