中国ネット通販で￥2,259（時価）で買える3次元センサーを動かしてみる

#中国ネット通販で￥2,259（時価）で買える3次元センサーを動かしてみる
###物体までの距離Dと動きX,Yが取れるセンサーがなんとこのお値段
筆者はQiita投稿者としてはたぶん珍しく、ドローンを作ったり飛ばしたりしています。ところで「ドローン大国」の中国では信じられないぐらいの安値で高度な機能を持ったセンサーが売っています。AliExpress で"PMW3901"を検索してみましょう。
https://github.com/r9kawai/3DSensorExperiment

※検索結果の一例です。特定の製品を勧めるものではありません。互換性を持った製品は多く存在するようです。

安いですね（金銭感覚によります）。LIDARといえば自動運転車などで知られているハイテク感ある単語。オプティカルフローもOpenCVとかGPUでやる高度な画像処理（動き検出）です。それがこんなちっぽけで、こんな値段のボードでできるんだろうか？疑うのも無理はありませんが、これは本来はドローンの底部に装着して離着陸の自動制御を行うための「ランディングセンサー」。チップが大量に使用されているため安価なのだと思われます。
　ではこれを、Pythonのコードから使うことができたら楽しいのではないでしょうか？これに挑戦してみました。
以下の記事中ではWin10 PC上のVisualStudioのPython3のコードでセンサーのデータを受信しています。UbuntuやRaspberryPi上で同じことができるはずです。シリアルポート周りの設定だけ変更が必要かと思います。

###開封の儀（静電防止袋を）

ポチってから待つこと３週間。中国から送られてきたセンサーの実物です。小さい。小指より小さい。うっかりすると失くします。中国ネット通販を使うことの敷居は各自でなんとかしてください。

###ジャンパピンをはんだ付けする
このセンサーモジュールはそのままでは使えません。なので、どこのご家庭にも転がっているジャンパーピンをはんだ付けしてDIPメスが刺せるようにします。
こんな風にする。

###例のケーブル￥379（時価）もポチる
ラズパイ使いの皆さんならポチらなくても既に持っているでしょう。USBから3.3V-2線式シリアルと5V,GNDを取り出すあのケーブル（USB－TTLシリアルコンソール用ケーブル）が必要です。これは同等品が日本のAmazonで各種買えます。そしてこのように接続します。
赤－5V　黒－GND　緑－RX　白－TX

RaspberryPiやArduinoのピンヘッダのシリアル(3.3V系)に直接つなぐなどの方法を取れば変換ケーブルは必要なくなります。ここではWindows10 PCから制御します。センサーモジュールの電源は5Vです。消費電力はわずか(10mA)のためこれもRaspberryPiから取ることができるでしょう。

###くくりつける箱も用意する（なくてもいい）
このままだとケーブルの先に落ち着かない小さな基盤がくっついていて実験がしにくいですね。くくりつける箱を3Dプリンターで作成しました。STLファイルを投稿してあるのでよければ利用してください。プラ板でも段ボールでもなんでもいいと思いますが、モジュール上のセンサーの邪魔をしないようにしましょう。カメラレンズが光学センサーで、ひじょうに小さな穴が2個開いている部品がLIDARです。そこを避ければ覆っても良いでしょう。

こういう見た目になった

###ひとまず様子見で動かしてみる
　このセンサーは電源をつなぐと瞬時に起動し、シリアル上にひたすらセンシングデータを送りつづけます。それだけの一途なやつです。ファームの書き換え手段などは無いようです。まずは動いているかどうかの様子見にTeraTermでCOMポートのデータを覗いてみました。
　まずはWindowsのデバイスマネージャからUSBシリアル変換君のCOMポートの番号＊を調べます。次にTeraTermをCOM＊に接続し、シリアルの設定を115200bpsにします。

文字化けでぐちゃっているが、どばーっとデータが送られてきているのは確認できた

　TeraTermのログ機能を用い、バイナリ指定でデータを*.BINファイルに数秒分ほど落とします。バイナリエディタで中身を見るとこんな感じでした。
パケットらしき構造が見える。正しいデータが送られていると思われる

###本筋じゃないところでハマった
　実は、USBシリアル変換ケーブルのドライバーがWin10で動作せず。ちょっとハマりました。どうかするとコードを書くよりハマりました。
上手くドライバが認識されている場合

ダメな場合こうなる

"Prolific USB Serial ドライバ"で検索すると同じようにハマる人が多いことが判ります。何故だかドライバのバージョンを切り替えたりしないと動かなかったりするようです。

###MSPってどんなプロトコル？
　ようやくコードに近づきました。このセンサーモジュールはMSP(Multiwii Serial Protocol)でデータを喋り続けます。聞いたことが無い言葉ですね・・・。これはフライトコントローラと呼ばれるドローンの中枢部と、これに接続される様々なデバイス群（センサー、モータコントローラ、etc）との間で交わされる簡素な取り決めです。（筆者の主な取り組みはこちらです）
https://github.com/iNavFlight/inav/wiki/MSP-V2

　しかし、ひとまずプロトコルの正式な実装にはなるべく触れず、とにかくセンサーから送られてくるデータをPython上の変数に入れることを考えます。

###たぶんこれで正しく読めている
ようやくコードが登場します。

3DSensorExperiment_test.py

import sys
import time
import struct
import threading
import serial
from serial.tools import list_ports

ports = list_ports.comports()
devices = [info.device for info in ports]
if len(devices) == 0:
        print("error: device not found")
else:
        print("found: device %s" % devices[0])
comdev = serial.Serial(devices[0], baudrate=115200, parity=serial.PARITY_NONE)
#comdev = serial.Serial('COM1', baudrate=115200, parity=serial.PARITY_NONE)

def rev_thread_func():
    while not rcv_thread_stop.is_set():
        bytesA = comdev.read(1)
        mark = int.from_bytes(bytesA, 'little')
        if mark == 36:
            bytesB = comdev.read(5)
            bytesC = comdev.read(2)
            psize = int.from_bytes(bytesC, 'little')
            bytesD = comdev.read(psize)
            bytesE = comdev.read(1)
            if psize == 5:
                val1 = int.from_bytes(bytesD[0:1], 'little')
                val2 = int.from_bytes(bytesD[1:5], 'little', signed=True)
                val3 = 0
                tabs = '\t\t'
            elif psize == 9:
                val1 = int.from_bytes(bytesD[0:1], 'little')
                val2 = int.from_bytes(bytesD[1:5], 'little', signed=True)
                val3 = int.from_bytes(bytesD[5:8], 'little', signed=True)
                tabs = '\t'
            else:
                continue
            viewtext1 = str(bytesA.hex()) + ' : ' + str(bytesB.hex()) + ' : ' + str(bytesC.hex()) + ' : ' + str(bytesD.hex()) + ' : ' + str(bytesE.hex())
            viewtext2 = '(' + str(val1) + ',' + str(val2) + ',' + str(val3) + ')'
            print(viewtext1, tabs, viewtext2)
    comdev.close()
    return

rcv_thread_stop = threading.Event()
rcv_thread = threading.Thread(target = rev_thread_func)
rcv_thread.daemon = True
rcv_thread.start()

try:
    while True:
        time.sleep(1)
except KeyboardInterrupt:
    rcv_thread_stop.is_set()
    rcv_thread.join(1)
    print("done")

このコードではCOMポートの一番最初の番号を開き、受信したシリアルデータをMSPパケットとして処理、val1,val2,val3の3つの変数を取り出します。生データと変数にデコードした値を標準出力に流し続けます。CTRL+Cで停止します。COMポート番号を変えたい場合、Linux系で動かす場合は、15行のコメントを参考に変更してください。

###読める！読めるぞ！
実は、MSPのプロトコルであることは分かっていますが、そのペイロードの中身までは明確な仕様を見つけられませんでした、上記のコードは「9バイトと5バイトのパケットを送ってくるらしい」「奇数サイズだし、先頭は1byteの変数で、後は4byteの変数1個か2個だろう」「つまり、"8bit, 32bit" or "8bit, 32bit, 32bit"」を送ってきてるらしいぞ。「0xFFF... だったり 0x000... だったりするから符号付整数で＋／－がバタついてるのであろう」という推測に戻づいて書きました。センサーが距離、Ｘ、Ｙを送ってきていることは分かっているので、まあ間違いないでしょう。本当は正式な仕様を見つけられるといいのですが、このセンサー、どうも最初に作った製品のクローン製品の互換製品の・・・ということを繰り返しているらしく、正式な開発元というのがよくわからない。中国の安価な電子部品とはそういうものです。
実行結果

###UIを付けて見やすくする

これじゃあんまりなので、Tkinterでセンサーが返してくる値をリアルタイムで表示するUIを付けました。

3DSensorExperiment_test.py

import sys
import time
import struct
import tkinter as tki
from scrolledtext import ScrolledText
import threading
import serial
from serial.tools import list_ports

title = 'Start 3DSensorExperiment'
print(title)
win = tki.Tk()
win.title(title)

def winClose():
    rcv_thread_stop.is_set()
    rcv_thread.join(1)
    win.quit()
    return

win.wm_protocol("WM_DELETE_WINDOW", winClose)

distance_val = 0
distance_str = tki.StringVar()
distance_str.set('Distance : No Sense')
distance_indicate = tki.Label(textvariable=distance_str, width=15, anchor=tki.W, justify='left',
                                         foreground='#ffffff', background='#00007f', font=("",32))
distance_indicate.pack(fill="both", anchor=tki.W)

illuminance_val = 0
illuminance_str = tki.StringVar()
illuminance_str.set('Illuminance : No Sense')
illuminance_indicate = tki.Label(textvariable=illuminance_str, width=15, anchor=tki.W, justify='left',
                                         foreground='#ffffff', background='#000000', font=("",32))
illuminance_indicate.pack(fill="both", anchor=tki.W)

xmove_val = 0
xmove_str = tki.StringVar()
xmove_str.set('  X move : No Sense')
xmove_indicate = tki.Label(textvariable=xmove_str, width=15, anchor=tki.W, justify='left',
                                         foreground='#ffffff', background='#7f0000', font=("",32))
xmove_indicate.pack(fill="both", anchor=tki.W)

ymove_val = 0
ymove_str = tki.StringVar()
ymove_str.set('  Y move : No Sense')
ymove_indicate = tki.Label(textvariable=ymove_str, width=15, anchor=tki.W, justify='left',
                                         foreground='#ffffff', background='#007f00', font=("",32))
ymove_indicate.pack(fill="both", anchor=tki.W)

txtbox_lines = 0
txtbox = ScrolledText()
txtbox.pack()

ports = list_ports.comports()
devices = [info.device for info in ports]
if len(devices) == 0:
        print("error: device not found")
else:
        print("found: device %s" % devices[0])
comdev = serial.Serial(devices[0], baudrate=115200, parity=serial.PARITY_NONE)
#comdev = serial.Serial('COM1', baudrate=115200, parity=serial.PARITY_NONE)

def set_distance(arg):
    if arg > 0 and arg < 100000:
        distance_val = arg
        str_val = 'Distance : ' + str(distance_val) + ' [mm]'
    else:
        str_val = 'Distance : No Sense'
    distance_str.set(str_val)
    return

def set_illuminance(arg):
    if arg != 0:
        illuminance_val = arg
        str_val = 'Illuminance : ' + str(illuminance_val)
        illuminance_str.set(str_val)
    return

def set_xmove(arg):
    if arg != 0:
        xmove_val = arg
        str_val = '  X move : ' + str(xmove_val) + ' [pix]'
        xmove_str.set(str_val)
    return

def set_ymove(arg):
    if arg != 0:
        ymove_val = arg
        str_val = '  Y move : ' + str(ymove_val) + ' [pix]'
        ymove_str.set(str_val)
    return

def rev_thread_func():
    while not rcv_thread_stop.is_set():
        bytesA = comdev.read(1)
        mark = int.from_bytes(bytesA, 'little')
        if mark == 36:
            bytesB = comdev.read(5)
            bytesC = comdev.read(2)
            psize = int.from_bytes(bytesC, 'little')
            bytesD = comdev.read(psize+1)
            if psize == 5:
                val1 = int.from_bytes(bytesD[0:1], 'little')
                val2 = int.from_bytes(bytesD[1:5], 'little', signed=True)
                val3 = 0
            elif psize == 9:
                val1 = int.from_bytes(bytesD[0:1], 'little')
                val2 = int.from_bytes(bytesD[1:5], 'little', signed=True)
                val3 = int.from_bytes(bytesD[5:8], 'little', signed=True)
            else:
                val1 = 0
                val2 = 0
                val3 = 0
            if psize == 5:
                set_distance(val2)
            if psize == 9:
                set_illuminance(val1)
                set_xmove(val2)
                set_ymove(val3)

            viewtext = str(bytesA.hex()) + ' : ' + str(bytesB.hex()) + ' : ' + str(bytesC.hex()) + ' : ' + str(bytesD.hex())
#           print(viewtext)
            global txtbox_lines
            if txtbox_lines >= 100:
                txtbox.delete('1.0', '2.0')
            txtbox.insert('end', viewtext + '\n')
            txtbox_lines += 1
            txtbox.see('end')
            txtbox.focus_set()
    comdev.close()
    return

rcv_thread_stop = threading.Event()
rcv_thread = threading.Thread(target = rev_thread_func)
rcv_thread.daemon = True
rcv_thread.start()

win.mainloop()

実行結果

センサーの前で手を動かしたりしてみてください。各表示値が変化するはずです。
・Distance：単位はmm、約2000mmまでは反応するが、それ以上は反応しない。実際には最下位桁はほぼ誤差。
・Illuminance：-127~128の値で、周囲の明るさ？動き検出のコンディションを示す。
・X move / Y move：単位はピクセル？で、オプティカルフローセンサーの検出値を示す。

###使い道はいろいろ
距離と、XYの動きが取得できるのでいろいろな使い道があると思います。
こうした安価なセンサーが中国のドローン部品市場には豊富にあるので使いこなしてみましょう。

以上