Raspberry Pi Pico W のMicropython で I2CのLSM9DS1モジュールを制御する　その２

このページは、「AE-LSM9DS1-I2Cで実験した内容」の続きです。
使用しているSTマイクロ社の複合センサーLSM9DS1をI2Cで容易に制御可能したモジュールです。
この複合センサーLSM9DS1は、3軸の加速度センサーと3軸のジャイロセンサおよび3軸の磁力センサーの９軸を持っています。
そして、Raspberry Pi Pico and Pico W　の情報源 を参考に、ブレッドボードで次のように配線しました。



左の配置において、
Picoの
左上から下に1〜20 ピンと並んでいます。
そして、
右下から上へ21〜40と並んでいます。

配線は次の通りです。

• PicoW_RUN_30Pin−リセットスイッチ−PicoW_GND_28Pin
• PicoW_GP14_19Pin−390Ω抵抗−LED−PicoW_GND_28Pin
• PicoW_RUN_30Pin−リセットスイッチ−PicoW_GND_28Pin
• LSM9DS1-I2C赤_1Pin−PicoW_3.3V_36Pin
• LSM9DS1-I2C黒_2Pin−PicoW_GND_28Pin
• LSM9DS1-I2C黄_3Pin−PicoW_0_SDA_21Pin
• LSM9DS1-I2C緑_4Pin−PicoW_0_SCL_22Pin

この接続イメージで、磁気方位のX軸方向が↓で、Y軸方向が→です。
ジャイロ（角速度）センサーの回転軸で、X軸が↑で、Y軸が→です。
加速度センサーで、X軸方向が↑で、Y軸方向が→です。

「AE-LSM9DS1-I2Cで実験した内容」を検証するため、 Micropythonで、I2Cインターフェイスを 介して得た９軸情報を、TCPサーバーへ送信し続ける作品が、最初の目標です。
この作成のために必要となるRaspberry Pi Pico W用クライアントモジュール（picowtcp.py)を次のように作りました。
これを利用して、
次のような7byteのバイト例をサーバに送るsend_data関数を定義しています。

Accelerometerの'A'、またはgyroscopeの'G'、またはmagneticの'M'の1byte

Xの2byte情報

Yの2byte情報

Zの2byte情報

# picowtcp.py
import socket #参考：https://micropython-docs-ja.readthedocs.io/ja/latest/library/socket.html
import network
import utime

SSID = '〇〇' # Wifi用
PW = '〇〇〇'　# 上記に接続するためのパスワード

ip, portnumber="192.168.0.110",59000#接続先サーバのIPアドレスと、ポート番号

def wifi__connect():
    wlan = network.WLAN(network.STA_IF)
    wlan.active(True)
    wlan.connect(SSID, PW)
    while wlan.isconnected() == False:
        print('Connecting to Wi-Fi AP')
        utime.sleep(1)
    #
    wlan_status = wlan.ifconfig()
    return f'''Connected!
    Pico IP Address:{wlan_status[0]} Netmask:{wlan_status[1]}
    Default Gateway:{wlan_status[2]} Name Server: {wlan_status[3]}'''

def tcp_connect():
   client=socket.socket(socket.AF_INET, socket.SOCK_STREAM)
   print(f'Connecting to {ip}:{portnumber}')
   client.connect((ip, portnumber))
   client.setblocking(True)# client.settimeout(None)  # タイムアウトを秒数で指定
   #client.setblocking(False)
   return client

def send_data(sock:socket, prefix:str, d16s:list):
    bin=prefix.encode('utf-8') + int(d16s[0]).to_bytes(2,'big')
    bin+=int(d16s[1]).to_bytes(2,'big')+int(d16s[2]).to_bytes(2,'big')
    n=0
    while n < len(bin):#上記の(1+2*3)=7byteを送る。(非ブロッキングにも対応)
        n+=sock.write(bin[n:])
    #sock.write(bin)、 sock.send(bin)、sock.sendall(bin)# などで全てOSError: [Errno 103] ECONNABORTED
    return bin
    
if __name__ == '__main__':
    print( wifi__connect() )
    client = tcp_connect()
    bin=<b >send_data(client, 'M', [50, 127, -1]) # 7byteのバイト例で送信</b>
    print( f"送信データ:{bin}")
    while True:
        pass
    client.close()

上記の　のWifi情報と、　のTCP接続先情報を設定し、
予め対応するTCPサーバーを起動させて、 実行した場合の結果例を以下に示します。

Connected!
    Pico IP Address:192.168.0.34 Netmask:255.255.255.0
    Default Gateway:192.168.0.1 Name Server: 210.198.29.106
送信データ:b'M\x002\x00\x7f\xff\xff'

上記を実行する場合、192.168.0.110のIPアドレスを持つPCで、次Unityのサーバプログラムを予め起動しておく必要があります。
それはクライアントからの接続に対して、受け入れ後に7byteを受信して、表示するコードです。
なお受信した7byteは、先頭1byteの文字、続けて2byteのx、y、zのデータにデコードして表示しています。

using UnityEngine;
using System.Net.Sockets;
using System.Net;
using System;

public class SensorServer : MonoBehaviour
{
    public TcpListener tcpListener = null; //サーバー用オブジェクト(接続待機用)
    TcpClient tcpClient = null;// 
    NetworkStream networkStream = null;// クライアントと通信するストリーム
    // Start is called before the first frame update
    void Start()
    {
        try
        {
            IPAddress ipAdrs = IPAddress.Parse("192.168.0.110");
            tcpListener = new TcpListener(ipAdrs, 59000); //サーバー用オブジェクト生成
            tcpListener.Start();
            Debug.Log("-----Strat-----");
        }
        catch (Exception e)
        {
        }
    }

    void Update()
    {
        if (tcpListener.Pending())// 保留中の接続要求があるか（接続依頼が来た？）
        {
            this.tcpClient = tcpListener.AcceptTcpClient();// 接続を許可 
            Debug.Log("-----Accept-----");
            this.networkStream = this.tcpClient.GetStream();// クライアントと通信するストリーム取得
        }
        if( this.networkStream != null)// クライアントと通信できるか？
        {            
            byte []buf=new byte[7];
            while (this.networkStream.DataAvailable)// 受信データがあるか？
            {
                for(int idx = 0; idx < buf.Length; idx++)
                {
                    int c = this.networkStream.ReadByte();
                    buf[idx] = (byte)c;
                }
                byte[] buffer2 = new byte[sizeof(UInt16)];
                Buffer.BlockCopy(buf, 1, buffer2, 0, buffer2.Length);
                short x = System.Buffers.Binary.BinaryPrimitives.ReadInt16BigEndian(buffer2);
                Buffer.BlockCopy(buf, 1+2, buffer2, 0, buffer2.Length);
                short y = System.Buffers.Binary.BinaryPrimitives.ReadInt16BigEndian(buffer2);
                Buffer.BlockCopy(buf, 1+4, buffer2, 0, buffer2.Length);
                short z = System.Buffers.Binary.BinaryPrimitives.ReadInt16BigEndian(buffer2);
                Debug.Log($"{(char)buf[0]}, x={x:######}, y={y:######}, z={z:######}");//受信データの確認
            }
        }
    }
}

上記実行のConsoleの表示例を以下に示します。

System.Net.Sockets.TcpListener-----Strat-----

System.Net.Sockets.TcpListener-----Accept-----

M, x=50, y=127, z=-1

前述 MicroPython (v1.22.2 )のコードで、上記のTCPサーバに接続して、送信の繰り返しを行うと、、数秒後にsock.write(bin)の箇所で次のエラーが生じました。 (2024-5確認)
OSError: [Errno 103] ECONNABORTED サーバ側受信ループをスレッドにするなど、受信可能タイミングを任意にしても、送信周期をsleep付加することでエラー頻度が変わりますが、エラー発生を避けるコードが できていません。エラーは、MicroPythonの接続が切れて、Unityから得られるエラーは「既存の接続はリモート ホストに強制的に切断されました。」の内容でした。

前述のpicowtcp.pyモジュールを利用して、AE-LSM9DS1-I2Cに取得した9軸情報を送信し続けるプログラムを以下に示します。
なおこれは、クライアントの接続が勝手に切断されてエラーにたった場合、接続をし直して送信し続けるようにしたコードです。

from machine import Pin, I2C
import picowtcp
import utime
import _thread

target_pin = Pin(14,Pin.OUT)
onboard_led = Pin( "LED", Pin.OUT )

target_pin.high()
print( picowtcp.wifi__connect() ) # Wifi接続
target_pin.low()

client=None

def receive(sock): #TCP受信のスレッド用関数
    print( "start receiveData" )
    while True:
        bin = sock.recv(1)#1byte受信
        print(f"----{bin}")
    #
    print( "receiveData ended." )

from machine import Pin, I2C
import utime

I2C_SDA=16# GP16のジェネラル端子をSDA「Serial Data」に指定用変数
I2C_SCL=17# GP17のジェネラル端子をSCL「Serial Clock」に指定用変数
I2C_CH=0# I2Cのチャンネル指定用変数（Picoでは0と1のチャンネルがある）
I2C_M_ADDR=0x1C#磁力センサのアドレス
I2C_AG_ADDR=0x6a#加速センサとジャイロセンサのアドレス

# SCL はスタンダードモードの100KHzの通信モードで、I2Cを初期化
i2c=I2C( I2C_CH, scl=Pin(I2C_SCL), sda=I2C_SDA, freq=100000)
for addr in i2c.scan(): print( f"enabled address:{addr:02x}" )

# 磁気センサー関連のレジスタ番号と設定値(lsm9ds1.pdf参照)
CTRL_REG1_M_N=0x20# レジスタ番号
CTRL_REG1_M=0b10010000# 温度補正 Low-power mode ODR:10Hz デフォルト:00010000
CTRL_REG2_M_N=0x21
CTRL_REG2_M=0b00000000# Full scale:±4gauss デフォルト:00000000
CTRL_REG3_M_N=0x22
CTRL_REG3_M=0b00000000# I2Cのenable Continuous conversion mode デフォルト:00000011
CTRL_REG4_M_N=0x23
CTRL_REG4_M=0b00000000# Z-axis:low power, data LSb at lower address, デフォルト: 00000000
CTRL_REG5_M_N=0x24
CTRL_REG5_M=0b00000000# continuous update デフォルト: 00000000
STATUS_REG_M=0x27# 状態
OUT_X_L_M=0x28 # このアドレスより、OUT_X_H_M、OUT_Y_L_M、OUT_Y_H_M、OUT_Z_L_M、OUT_Z_H_Mレジスタが並ぶ
OFFSET_X_REG_L_M=0x05

# ジャイロのセンサー関連のレジスタ番号と設定値
CTRL_REG1_G_N=0x10# レジスタ番号
CTRL_REG1_G=0b01000000# 角速度センサ制御レジスタ デフォルト:00000000
#CTRL_REG1_G[7〜5bit]のODR_G:010 測定周期59.5Hz
#CTRL_REG1_G[4〜3bit]のFS_G:00 Gyroscope full-scaleを245 dps
#CTRL_REG1_G[1〜0bit]のBW_G:00 帯域幅
CTRL_REG2_G_N=0x11
CTRL_REG2_G=0b00000000# デフォルト:00000000
CTRL_REG3_G_N=0x12
CTRL_REG3_G=0b00000000# デフォルト:00000000
#CTRL_REG3_G でLow-power disabled、High-pass filter　desable,Gyroscope high-pass filter cutoff frequency set
STATUS_REG=0x17# 状態
OUT_X_G=0x18 # このアドレスより、OOUT_Y_G (1Ah - 1Bh)、OUT_Z_G (1Ch - 1Dh)のレジスタが並ぶ

# 加速度のセンサー関連のレジスタ番号と設定値
CTRL_REG4_N=0x1E
CTRL_REG4=0b00111000#0,0,Zen_G,Yen_G,Xen_G,0,LIR_XL1,4D_XL1 #デフォルト値:00111000
CTRL_REG5_N=0x1F
CTRL_REG5=0b00111000#0,0,Zen_G,Yen_G,Xen_G,0,LIR_XL1,4D_XL1 #デフォルト値:00111000
CTRL_REG6_XL_N=0x20
CTRL_REG6_XL=0b00000000#デフォルト値:00000000
#CTRL_REG6_XL[7〜5bit]はジャイロスコープ利用時無効?で、ジャイロスコープと同じ測定周期
#CTRL_REG6_XL[4〜3bit]はAccelerometer full-scale：±2g
#CTRL_REG6_XL[4〜3bit]
OUT_X_L_XL=0x28#OUT_X_H_XL,OUT_Y_L_XL,OUT_Y_H_XL,OUT_Z_L_XL,OUT_Z_H_XL

#磁力センサのアドレスを指定して、各制御レジスタを設定
i2c.writeto_mem( I2C_M_ADDR, CTRL_REG1_M_N, bytes([CTRL_REG1_M]))
i2c.writeto_mem( I2C_M_ADDR, CTRL_REG2_M_N, bytes([CTRL_REG2_M]))
i2c.writeto_mem( I2C_M_ADDR, CTRL_REG3_M_N, bytes([CTRL_REG3_M]))
i2c.writeto_mem( I2C_M_ADDR, CTRL_REG4_M_N, bytes([CTRL_REG4_M]))
i2c.writeto_mem( I2C_M_ADDR, CTRL_REG5_M_N, bytes([CTRL_REG5_M]))

#各ジャイロセンサレジスタをを上記の通り設定
i2c.writeto_mem( I2C_AG_ADDR, CTRL_REG1_G_N, bytes([CTRL_REG1_G]))
i2c.writeto_mem( I2C_AG_ADDR, CTRL_REG2_G_N, bytes([CTRL_REG2_G]))
i2c.writeto_mem( I2C_AG_ADDR, CTRL_REG3_G_N, bytes([CTRL_REG3_G]))

#各加速度のセンサーレジスタを上記の通り設定
i2c.writeto_mem( I2C_AG_ADDR, CTRL_REG4_N, bytes([CTRL_REG4]))
i2c.writeto_mem( I2C_AG_ADDR, CTRL_REG5_N, bytes([CTRL_REG5]))
i2c.writeto_mem( I2C_AG_ADDR, CTRL_REG6_XL_N, bytes([CTRL_REG6_XL]))

#import math
target_pin.high()
while True:
    try:
        while client == None: # サーバが受け入れるまで、TCP接続要求を繰り返す
            client=picowtcp.tcp_connect()
            if client != None: t_id = _thread.start_new_thread(receive, (client,) )
        #
        statu=i2c.readfrom_mem( I2C_M_ADDR, STATUS_REG_M,1)
        #print(f"statu:{statu[0]:08b}", end=" ")
        if statu[0] & 0x80 :
            bin=i2c.readfrom_mem( I2C_M_ADDR, OUT_X_L_M, 6) # X,Y,Z軸の磁気センサー情報取得
            #print( [v for v in bin] )
            magbin=[bin[n] + (bin[n+1]<<8) for n in range(0,len(bin),2)] # 2byteからuint16へ変換
            magbin=[v if v <= 32767 else v - 65536 for v in magbin] # uint16からint16へ変換
            #deg = math.atan(magbin[1]/magbin[0]) * 360 / math.pi / 2 # 角度算出
            #print( f"M:{magbin[0]:6}, Y:{magbin[1]:6}, Z:{magbin[2]:6} , deg={deg:5.2f}" )
            picowtcp.send_data(client, 'M', magbin)
        if statu[0] & 0x02 : # ジャイロセンサー情報あり?
            bin=i2c.readfrom_mem( I2C_AG_ADDR, OUT_X_G, 6) # X,Y,Z軸のジャイロセンサー情報取得
            #print( [v for v in bin] )
            gyro=[bin[n] + (bin[n+1]<<8) for n in range(0,len(bin),2)]
            gyro=[v if v <= 32767 else v - 65536 for v in gyro]
            picowtcp.send_data(client, 'G', gyro)
        if statu[0] & 0x01: # 加速度情報情報あり?
            bin=i2c.readfrom_mem( I2C_AG_ADDR, OUT_X_L_XL, 6) # X,Y,Z軸の加速度情報取得
            #print( [v for v in bin] )
            accel=[bin[n] + (bin[n+1]<<8) for n in range(0,len(bin),2)]
            accel=[v if v <= 32767 else v - 65536 for v in accel]
            #print( f"Accelerometer:{accel}" )
            picowtcp.send_data(client, 'A', accel)
        if (statu[0] & 0x80) or (statu[0] & 0x02) or (statu[0] & 0x01):
            if onboard_led.value(): onboard_led.low()
            else: onboard_led.high()
            #utime.sleep(0.5)
    except Exception as e:
        print( e )
        client = None
        utime.sleep(0.5)

client.close()

"192.168.0.110"のIPアドレス、59000のポート番号で、TCPサーバーとして起動し、 接続してきた上記の「Raspberry Pi Pico W」から送られる次の情報を、receiveLoopのスレッドで受信するコードです。
なお、スレッド受信した磁気センサーのデータを、OnGUI()で表示しています。
（このコードは、適当なGameObjectにアタッチするだけで使えます。）

Accelerometerの'A'、またはgyroscopeの'G'、またはmagneticの'M'の1byte

Xの2byte情報

Yの2byte情報

Zの2byte情報

using UnityEngine;
using System.Net.Sockets;
using System.Net;
using System;

public class SensorServer : MonoBehaviour
{
    public TcpListener tcpListener = null; //サーバー用オブジェクト(接続待機用)
    TcpClient tcpClient = null;// 
    NetworkStream networkStream = null;// クライアントと通信するストリーム
    short mx, my, mz;
    short gx, gy, gz;
    short ax, ay, az;

    System.Threading.Thread receiveThread;////受信処理スレッド

    // Start is called before the first frame update
    void Start()
    {
        try
        {
            IPAddress ipAdrs = IPAddress.Parse("192.168.0.110");
            tcpListener = new TcpListener(ipAdrs, 59000); //サーバー用オブジェクト生成
            tcpListener.Start();
            Debug.Log($"{ipAdrs}:-----Sever Strat-----" );
        }
        catch (Exception e)
        {
        }
        //this.gameObject.
    }

    void FixedUpdate()
    {
        if ( tcpListener.Pending() )// 保留中の接続要求があるか（接続依頼が来た？）
        {
            this.tcpClient = tcpListener.AcceptTcpClient();// 接続を許可 
            Debug.Log("-----Accept-----");
            this.networkStream = this.tcpClient.GetStream();// クライアントと通信するストリーム取得
            receiveThread = new System.Threading.Thread(new System.Threading.ThreadStart(receiveLoop));
            receiveThread.Start();
        }
    }

    private void receiveLoop()
    {
        byte[] buf = new byte[7]; // 受信用バッファ 
        int idx = 0;
        int c;
        byte[] buffer2 = new byte[sizeof(UInt16)];// バイナリから整数に変換用バッファ
        while (this.networkStream != null)
        {
            try
            {
                c = this.networkStream.ReadByte();
            }
            catch (Exception e)
            {
                Debug.Log(e);
                this.networkStream = null;
                break;
            }
            if (c == -1) break;
            buf[idx++] = (byte)c;
            if (idx < buf.Length) continue;
            Buffer.BlockCopy(buf, 1, buffer2, 0, buffer2.Length);
            short x = System.Buffers.Binary.BinaryPrimitives.ReadInt16BigEndian(buffer2);
            Buffer.BlockCopy(buf, 1 + 2, buffer2, 0, buffer2.Length);
            short y = System.Buffers.Binary.BinaryPrimitives.ReadInt16BigEndian(buffer2);
            Buffer.BlockCopy(buf, 1 + 4, buffer2, 0, buffer2.Length);
            short z = System.Buffers.Binary.BinaryPrimitives.ReadInt16BigEndian(buffer2);
            //Debug.Log($"{buf[0]}:x={mx,6:D}, y={my,6:D}, z={mz,6:D}");
            if(buf[0] == 'M')
            {
                mx = x; my = y; mz = z;
            }
            else if(buf[0] == 'G')
            {
                gx = x; gy = y; gz = z;
            }
            else if(buf[0] == 'A')
            {
                ax = x; ay = y; az = z;
            }
            idx = 0;
        }
        Debug.Log("break receiveLoop");
    }

    private void OnGUI()
    {
        string msg = $"x={mx,6:D}, y={my,6:D}, z={mz,6:D}";//磁気センサーの受信データを確認する文字列生成

        float xPos = 150;
        float yPos = 50;
        float whidth = 600;
        float height = 200;
        GUIStyle lableStyle = GUI.skin.label;// ---Labelスタイル設定
        lableStyle.fontSize = 42;
        GUI.color = new Color(0.9f, 0.0f, 0.9f); //文字色設定
        GUI.Label(new Rect(xPos, yPos, whidth, height), msg);
    }
}