ESP32のMicroPythonで10DOF IMU Sensor(GY-91)に搭載されているMPU9250を使ってみる

Share on:

MicroPythonを入れたESP32でGY-91に搭載されているMPU9250から値を取得して、PC上のPyQtGraphに表示してみました。

接続

接続は下記の通りで、ESP32とGY-91をI2Cで接続するだけです。他にはGY-91には電源(3.3V)とGNDを接続します。

センサから値の取得

MicroPythonのi2c.readfrom_mem()を使って、デバイスのアドレス、読み込むメモリのアドレス、読み込むデータのバイト数を指定してデータを読み込みます。加速度の場合は、0x3B(下記コードではself.ACCEL_OUT)のメモリから6バイトのデータを読み込み、8ビット×2のデータを16ビットに変換して値が得られます。

 1data = self.i2c.readfrom_mem(self.address, self.ACCEL_OUT, 6)
 2x = self.data_convert(data[1], data[0])
 3y = self.data_convert(data[3], data[2])
 4z = self.data_convert(data[5], data[4])
 5
 6def data_convert(self, data1, data2):
 7    value = data1 | (data2 << 8)
 8    if(value & (1 << 16 - 1)):
 9        value -= (1<<16)
10    return value

センサの値をESP32からPCへ送信

データを送信するためにESP32でprint()を実行します。これによってシリアル通信経由でデータを送信することができます。送信内容はax:0.0,az:0.0…といった内容です。項目を「,」で区切り、軸を「:」で区切るようにしています。

 1while True:
 2    
 3    accel = mpu9250.read_accel()
 4    gyro = mpu9250.read_gyro()
 5    mag = mpu9250.read_magnet()
 6
 7    text  = ','.join( [ 'a'+key+':'+str(accel[key]) for key in accel ] ) + ','
 8    text += ','.join( [ 'g'+key+':'+str( gyro[key]) for key in accel ] ) + ','
 9    text += ','.join( [ 'm'+key+':'+str(  mag[key]) for key in accel ] )
10
11    print(text)
12
13    utime.sleep(0.1)

送信内容は下記のような感じです。

1ax:0.143,az:1.027,ay:0.079,gx:0.641,gz:0.336,gy:0.153,mx:15.951,mz:5.028,my:-39.635
2ax:0.143,az:1.03,ay:0.079,gx:0.702,gz:0.397,gy:0.214,mx:15.772,mz:5.548,my:-40.178
3ax:0.144,az:1.028,ay:0.078,gx:0.641,gz:0.336,gy:0.153,mx:15.234,mz:3.987,my:-40.359

PCでPyQtGraphを使って表示

ESP32から送られてきたデータを処理してPyQtGraphに表示しました。加速度、ジャイロ、地磁気の3つ分のPlotWidgetを作成して、1つのPlotWidgetにx、y、zの3つのplotItemを追加して表示しています。

50msec間隔でグラフの描画を実行するためにQtCore.QTimer()を使用しました。self.timer.timeout.connect()に渡したself.update_dataが50msec間隔で実行されます。そのタイミングでシリアル通信からのデータをself.serial.readline()で取得して、文字列の処理を行い、値を取得しています。処理された値をPlotDataItemに渡すことでグラフに描画されます。

 1    # timer
 2    self.timer = QtCore.QTimer()
 3    self.timer.timeout.connect(self.update_data)
 4    self.timer.start(50)
 5
 6def update_data(self):
 7    # stop timer
 8    self.timer.stop()
 9
10    # clear
11    self.plotwidget['a'].clear()
12    self.plotwidget['g'].clear()
13    self.plotwidget['m'].clear()
14
15    # get serial
16    try:
17        line = self.serial.readline().decode()
18        data = { data.split(':')[0]:data.split(':')[1] for data in line.split(',') }
19
20        # increase time
21        self.plot_data['t'] = np.append( self.plot_data['t'][1:], self.plot_data['t'][-1]+1 )
22
23        # add data
24        for key in self.plot_data:
25            if key == 't':
26                continue
27            self.plot_data[key] = np.append( self.plot_data[key][1:], float(data[key]) )
28
29            # set data
30            self.plotwidget[key[0]].addItem(
31                pyqtgraph.PlotDataItem(
32                    x=self.plot_data['t'], y=self.plot_data[key], 
33                    pen=pyqtgraph.mkPen(color=self.plot_data_color[key], width=3), antialias=True
34                )
35            )
36    except:
37        pass
38    
39    # start timer
40    self.timer.start(50)

動作の様子

ソースコード

ESP32用 main.py

 1from machine import Pin, I2C
 2import utime
 3from mpu9250 import MPU9250
 4
 5def main():
 6
 7    print('hello')
 8
 9    i2c = I2C(scl=Pin(21), sda=Pin(22), freq=100000)
10
11    print(i2c.scan())
12
13    mpu9250 = MPU9250(i2c)
14    mpu9250.setting(mpu9250.GFS_1000, mpu9250.AFS_16G)
15
16    while True:
17        
18        accel = mpu9250.read_accel()
19        gyro = mpu9250.read_gyro()
20        mag = mpu9250.read_magnet()
21
22        text  = ','.join( [ 'a'+key+':'+str(accel[key]) for key in accel ] ) + ','
23        text += ','.join( [ 'g'+key+':'+str( gyro[key]) for key in accel ] ) + ','
24        text += ','.join( [ 'm'+key+':'+str(  mag[key]) for key in accel ] )
25
26        print(text)
27
28        utime.sleep(0.1)
29
30if __name__ == "__main__":
31    main()

ESP32用 mpu9250.py

  1from micropython import const
  2import utime
  3
  4class MPU9250():
  5    MPU9250_ADDRESS = const(0x68)
  6    DEVICE_ID       = const(0x71)
  7
  8    SMPLRT_DIV     = const(0x19)
  9    CONFIG         = const(0x1A)
 10    GYRO_CONFIG    = const(0x1B)
 11    ACCEL_CONFIG   = const(0x1C)
 12    ACCEL_CONFIG_2 = const(0x1D)
 13    LP_ACCEL_ODR   = const(0x1E)
 14    WOM_THR        = const(0x1F)
 15    FIFO_EN        = const(0x23)
 16    I2C_MST_CTRL   = const(0x24)
 17    I2C_MST_STATUS = const(0x36)
 18    INT_PIN_CFG    = const(0x37)
 19    INT_ENABLE     = const(0x38)
 20    INT_STATUS     = const(0x3A)
 21    ACCEL_OUT      = const(0x3B)
 22    TEMP_OUT       = const(0x41)
 23    GYRO_OUT       = const(0x43)
 24
 25    I2C_MST_DELAY_CTRL = const(0x67)
 26    SIGNAL_PATH_RESET  = const(0x68)
 27    MOT_DETECT_CTRL    = const(0x69)
 28    USER_CTRL          = const(0x6A)
 29    PWR_MGMT_1         = const(0x6B)
 30    PWR_MGMT_2         = const(0x6C)
 31    FIFO_R_W           = const(0x74)
 32    WHO_AM_I           = const(0x75)
 33
 34    GFS_250  = const(0x00)
 35    GFS_500  = const(0x01)
 36    GFS_1000 = const(0x02)
 37    GFS_2000 = const(0x03)
 38    AFS_2G   = const(0x00)
 39    AFS_4G   = const(0x01)
 40    AFS_8G   = const(0x02)
 41    AFS_16G  = const(0x03)
 42
 43    def __init__(self, i2c, address = MPU9250_ADDRESS):
 44        self.i2c = i2c
 45        self.address = address
 46        self.setting(self.GFS_250, self.AFS_2G)
 47        self.ak8963 = AK8963(self.i2c)
 48
 49    def searchDevice(self):
 50        who_am_i = self.i2c.readfrom(self.address, self.WHO_AM_I)
 51        if(who_am_i == self.DEVICE_ID):
 52            return True
 53        else:
 54            return False
 55
 56    def setting(self, gfs, afs):
 57        if gfs == self.GFS_250:
 58            self.gres = 250.0/32768.0
 59        elif gfs == self.GFS_500:
 60            self.gres = 500.0/32768.0
 61        elif gfs == self.GFS_1000:
 62            self.gres = 1000.0/32768.0
 63        else:  # gfs == GFS_2000
 64            self.gres = 2000.0/32768.0
 65
 66        if afs == self.AFS_2G:
 67            self.ares = 2.0/32768.0
 68        elif afs == self.AFS_4G:
 69            self.ares = 4.0/32768.0
 70        elif afs == self.AFS_8G:
 71            self.ares = 8.0/32768.0
 72        else: # afs == AFS_16G:
 73            self.ares = 16.0/32768.0
 74
 75        buffer = bytearray(1)
 76        self.i2c.writeto_mem(self.address, self.PWR_MGMT_1, b'\x00') # sleep off
 77        utime.sleep_ms(100)
 78        self.i2c.writeto_mem(self.address, self.PWR_MGMT_1, b'\x01') # auto select clock source
 79        utime.sleep_ms(100)
 80        self.i2c.writeto_mem(self.address, self.CONFIG, b'\x03') # DLPF_CFG
 81        self.i2c.writeto_mem(self.address, self.SMPLRT_DIV, b'\x04') # sample rate divider
 82        buffer[0] = gfs << 3
 83        self.i2c.writeto_mem(self.address, self.GYRO_CONFIG, buffer) # gyro full scale select
 84        buffer[0] = afs << 3
 85        self.i2c.writeto_mem(self.address, self.ACCEL_CONFIG, buffer) # accel full scale select
 86        self.i2c.writeto_mem(self.address, self.ACCEL_CONFIG_2, b'\x03') # A_DLPFCFG
 87        self.i2c.writeto_mem(self.address, self.INT_PIN_CFG, b'\x02') # BYPASS_EN
 88        utime.sleep_ms(100)
 89
 90    def check_data_ready(self):
 91        drdy = self.i2c.readfrom(self.address, self.INT_STATUS)
 92        if drdy & 0x01:
 93            return True
 94        else:
 95            return False
 96            
 97    def read_accel(self):
 98        data = self.i2c.readfrom_mem(self.address, self.ACCEL_OUT, 6)
 99        x = self.data_convert(data[1], data[0])
100        y = self.data_convert(data[3], data[2])
101        z = self.data_convert(data[5], data[4])
102
103        x = round(x*self.ares, 3)
104        y = round(y*self.ares, 3)
105        z = round(z*self.ares, 3)
106
107        return {"x":x, "y":y, "z":z}
108
109    def read_gyro(self):
110        data = self.i2c.readfrom_mem(self.address, self.GYRO_OUT, 6)
111
112        x = self.data_convert(data[1], data[0])
113        y = self.data_convert(data[3], data[2])
114        z = self.data_convert(data[5], data[4])
115
116        x = round(x*self.gres, 3)
117        y = round(y*self.gres, 3)
118        z = round(z*self.gres, 3)
119
120        return {"x":x, "y":y, "z":z}
121
122    def read_magnet(self):
123        return self.ak8963.read_magnet()
124
125    def data_convert(self, data1, data2):
126        value = data1 | (data2 << 8)
127        if(value & (1 << 16 - 1)):
128            value -= (1<<16)
129        return value
130
131class AK8963():
132    AK8963_SLAVE_ADDRESS = const(0x0C)
133    AK8963_ST1           = const(0x02)
134    AK8963_MAGNET_OUT    = const(0x03)
135    AK8963_CNTL1         = const(0x0A)
136    AK8963_CNTL2         = const(0x0B)
137    AK8963_ASAX          = const(0x10)
138    AK8963_MODE_DOWN     = const(0x00)
139    AK8963_MODE_ONE      = const(0x01)
140    AK8963_MODE_C8HZ     = const(0x02)
141    AK8963_MODE_C100HZ   = const(0x06)
142    AK8963_BIT_14        = const(0x00)
143    AK8963_BIT_16        = const(0x01)
144
145    def __init__(self, i2c, address=0x76):
146        self.i2c = i2c
147        self.address = address
148        self.setting(self.AK8963_MODE_C8HZ, self.AK8963_BIT_16)
149
150    def setting(self, mode, mfs):
151        if mfs == self.AK8963_BIT_14:
152            self.mres = 4912.0/8190.0
153        else: #  mfs == AK8963_BIT_16:
154            self.mres = 4912.0/32760.0
155
156        self.i2c.writeto_mem(self.AK8963_SLAVE_ADDRESS, self.AK8963_CNTL1, b'\x00')
157        utime.sleep_ms(10)
158        self.i2c.writeto_mem(self.AK8963_SLAVE_ADDRESS, self.AK8963_CNTL1, b'\x0F') # set read FuseROM mode
159        utime.sleep_ms(10)
160        data = self.i2c.readfrom_mem(self.AK8963_SLAVE_ADDRESS, self.AK8963_ASAX, 3) # read coef data
161
162        self.magXcoef = (data[0] - 128) / 256.0 + 1.0
163        self.magYcoef = (data[1] - 128) / 256.0 + 1.0
164        self.magZcoef = (data[2] - 128) / 256.0 + 1.0
165
166        self.i2c.writeto_mem(self.AK8963_SLAVE_ADDRESS, self.AK8963_CNTL1, b'\x00') # set power down mode
167        utime.sleep_ms(10)
168        buffer = bytearray(1)
169        buffer[0] = (mfs << 4 | mode)
170        self.i2c.writeto_mem(self.AK8963_SLAVE_ADDRESS, self.AK8963_CNTL1, buffer) # set scale&continous mode
171        utime.sleep_ms(10)
172
173    def read_magnet(self):
174        x, y, z=0, 0, 0
175        
176        data = self.i2c.readfrom_mem(self.AK8963_SLAVE_ADDRESS, self.AK8963_MAGNET_OUT, 7)
177
178        # check overflow
179        if (data[6] & 0x08)!=0x08:
180            x = self.data_convert(data[0], data[1])
181            y = self.data_convert(data[2], data[3])
182            z = self.data_convert(data[4], data[5])
183
184            x = round(x * self.mres * self.magXcoef, 3)
185            y = round(y * self.mres * self.magYcoef, 3)
186            z = round(z * self.mres * self.magZcoef, 3)
187
188        return {"x":x, "y":y, "z":z}
189
190    def data_convert(self, data1, data2):
191        value = data1 | (data2 << 8)
192        if(value & (1 << 16 - 1)):
193            value -= (1<<16)
194        return value

PC用 main.py

  1# -*- coding: utf-8 -*-
  2import sys
  3from PyQt5 import QtCore, QtGui, QtWidgets
  4from pyqtgraph import PlotWidget
  5import pyqtgraph
  6import numpy as np
  7import serial
  8
  9class MainWindow(QtWidgets.QMainWindow):
 10    def __init__(self, parent=None):
 11        super(MainWindow, self).__init__(parent)
 12
 13        self.plot_data = {
 14            'ax':np.full(100, 0),
 15            'ay':np.full(100, 0),
 16            'az':np.full(100, 0),
 17            'gx':np.full(100, 0),
 18            'gy':np.full(100, 0),
 19            'gz':np.full(100, 0),
 20            'mx':np.full(100, 0),
 21            'my':np.full(100, 0),
 22            'mz':np.full(100, 0),
 23            't' :np.arange(100)
 24        }
 25
 26        self.plot_data_color = {
 27            'ax':'#FF0000',
 28            'ay':'#00FF00',
 29            'az':'#0000FF',
 30            'gx':'#DDDD00',
 31            'gy':'#00DDDD',
 32            'gz':'#DD00DD',
 33            'mx':'#808020',
 34            'my':'#208080',
 35            'mz':'#802080'
 36        }
 37
 38        self.resize(600, 600)
 39        self.setStyleSheet("QMainWindow {background: 'white';}")
 40        self.serial = serial.Serial('COM3', 115200, timeout=None)
 41
 42        # leyout
 43        self.centralwidget = QtWidgets.QWidget(self)
 44        self.setCentralWidget(self.centralwidget)
 45        self.verticalLayout = QtWidgets.QVBoxLayout(self.centralwidget)
 46        
 47        # pot widget
 48        self.plotwidget = { axis:PlotWidget(self) for axis in ['a','g','m'] }
 49        titles = { 'a':'Acceleration','g':'Gyro','m':'Geomagnetism' }
 50        for key in self.plotwidget:
 51            self.plotwidget[key].setBackground("#FFFFFFFF")
 52            plotitem = self.plotwidget[key].plotItem
 53            plotitem.setLabels(bottom='time', left=titles[key])
 54            plotitem.getAxis('bottom').setPen( pyqtgraph.mkPen(color='#000000') )
 55            plotitem.getAxis('left').setPen( pyqtgraph.mkPen(color='#000000') )
 56        
 57        # leyout
 58        self.verticalLayout.addWidget(self.plotwidget['a'])
 59        self.verticalLayout.addWidget(self.plotwidget['g'])
 60        self.verticalLayout.addWidget(self.plotwidget['m'])
 61
 62        # timer
 63        self.timer = QtCore.QTimer()
 64        self.timer.timeout.connect(self.update_data)
 65        self.timer.start(50)
 66
 67    def update_data(self):
 68        # stop timer
 69        self.timer.stop()
 70
 71        # clear
 72        self.plotwidget['a'].clear()
 73        self.plotwidget['g'].clear()
 74        self.plotwidget['m'].clear()
 75
 76        # get serial
 77        try:
 78            line = self.serial.readline().decode()
 79            data = { data.split(':')[0]:data.split(':')[1] for data in line.split(',') }
 80
 81            # increase time
 82            self.plot_data['t'] = np.append( self.plot_data['t'][1:], self.plot_data['t'][-1]+1 )
 83
 84            # add data
 85            for key in self.plot_data:
 86                if key == 't':
 87                    continue
 88                self.plot_data[key] = np.append( self.plot_data[key][1:], float(data[key]) )
 89
 90                # set data
 91                self.plotwidget[key[0]].addItem(
 92                    pyqtgraph.PlotDataItem(
 93                        x=self.plot_data['t'], y=self.plot_data[key], 
 94                        pen=pyqtgraph.mkPen(color=self.plot_data_color[key], width=3), antialias=True
 95                    )
 96                )
 97        except:
 98            pass
 99        
100        # start timer
101        self.timer.start(50)
102
103def main():
104    app = QtWidgets.QApplication(sys.argv)
105    mainwindow = MainWindow(None)
106    mainwindow.show()
107    app.exec()
108
109if __name__ == '__main__':
110    main()

関連記事