Telloから送られてくるデータをMacbook側で受信して、Terminal表示してみた


過去に作成した次の記事では、TelloにWifi接続したMacbookのTerminalに、Telloの操縦コマンドを打ち込み、Telloを遠隔操作しました。

今回は、TelloからMacbookに向けて送られてくる、Telloの状態に関する信号を(Macbook側で)しっかりと受け止めて、MacbookのTerminalに出力する機能を追加してみました。

今後はさらに、Telloに搭載されている単眼カメラから送られてくる画像データも、Macbook側で受信して、ウィンドウに出力表示する機能も盛り込んでいきたいと思います。その際、Tello-PythonやTelloPyといったライブラリを使わずに、Macbook側でポート 11111のUDPサーバを立てて、OpenCV2のimshowメソッドで画像フレームをリアルタイムに表示する部分を、フルスクラッチで書いてみたいと思います。

実行画面

Terminal
electron@diynoMacBook-Pro Tello % python3 Tello_python_rev.py
Telloに送る指示コマンドを入力してください。:  takeoff
Telloに送る指示コマンドを入力してください。:  sdk?
この機体のSDK Ver.: 4
Telloに送る指示コマンドを入力してください。:  sn?
この機体のシリアル番号: 3
Telloに送る指示コマンドを入力してください。:  speed?
現在の速度: 6
Telloに送る指示コマンドを入力してください。:  forward 100
Telloに送る指示コマンドを入力してください。:  right 100
Telloに送る指示コマンドを入力してください。:  back 100 
Telloに送る指示コマンドを入力してください。:  time?
累積飛行時間: 5
Telloに送る指示コマンドを入力してください。:  battery?
バッテリ残量(%): 8
Telloに送る指示コマンドを入力してください。:  back 50
Telloに送る指示コマンドを入力してください。:  back 30
Telloに送る指示コマンドを入力してください。:  left 30
Telloに送る指示コマンドを入力してください。:  right 100
Telloに送る指示コマンドを入力してください。:  forward 20
Telloに送る指示コマンドを入力してください。:  time?
累積飛行時間: 5
Telloに送る指示コマンドを入力してください。:  speed?
現在の速度: 6
Telloに送る指示コマンドを入力してください。:  time?
累積飛行時間: 5
Telloに送る指示コマンドを入力してください。:  battery?
バッテリ残量(%): 8
Telloに送る指示コマンドを入力してください。:  flip
Telloに送る指示コマンドを入力してください。:  down 20
Telloに送る指示コマンドを入力してください。:  cw 360
Telloに送る指示コマンドを入力してください。:  ccw 720
Telloに送る指示コマンドを入力してください。:  battery?
バッテリ残量(%): 8
Telloに送る指示コマンドを入力してください。:  time?
累積飛行時間: 5
Telloに送る指示コマンドを入力してください。:  land
着陸します。
累積飛行時間: 5 
バッテリ残量(%): 8

Exit . . .

electron@diynoMacBook-Pro Tello % 

実行したスクリプトファイル

Tello_python_rev.py
import threading 
import socket
import sys
import time

host = ''
port = 9000
locaddr = (host, port) 

# Create a UDP socket
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
tello_address = ('192.168.10.1', 8889)

sock.bind(locaddr)

def recv():
    count = 0
    while True: 
        try:
            data, server = sock.recvfrom(1518)
        except Exception:
            print ('\nExit . . .\n')
            break

#recvThread create
recvThread = threading.Thread(target=recv)
recvThread.start()

while True: 
    # 最初に入力が必要な"command"命令は、ユーザが打ち込まなくて良いようにスクリプト側でTelloに送出する
    msg = str("command").encode(encoding="utf-8") 
    sent = sock.sendto(msg, tello_address)
    # "takeoff"以降の命令をユーザ入力から受け取る
    try:
        msg = input("Telloに送る指示コマンドを入力してください。:  ")
        if not msg:
            break  

        # Send data
        # Tello SDK v2.0 https://dl-cdn.ryzerobotics.com/downloads/Tello/Tello%20SDK%202.0%20User%20Guide.pdf
        if msg == "land":
            order_command = "battery?"
            battery_volume = sock.sendto(order_command.encode(encoding="utf-8") , tello_address)
            order_command = "time?"
            flight_time = sock.sendto(order_command.encode(encoding="utf-8") , tello_address)            
            print("着陸します。")
            message = "累積飛行時間: {0} \nバッテリ残量(%): {1}".format(str(flight_time), str(battery_volume))
            print(message)
            sock.close()  
            break
        elif msg == "battery?":
            msg = msg.encode(encoding="utf-8") 
            sent = sock.sendto(msg, tello_address)
            print("バッテリ残量(%): " + str(sent))
        elif msg == "speed?":
            msg = msg.encode(encoding="utf-8") 
            sent = sock.sendto(msg, tello_address)
            print("現在の速度: " + str(sent))
        elif msg == "time?":
            msg = msg.encode(encoding="utf-8") 
            sent = sock.sendto(msg, tello_address)
            print("累積飛行時間: " + str(sent))
        elif msg == "sdk?":
            msg = msg.encode(encoding="utf-8") 
            sent = sock.sendto(msg, tello_address)
            print("この機体のSDK Ver.: " + str(sent))
        elif msg == "sn?":
            msg = msg.encode(encoding="utf-8") 
            sent = sock.sendto(msg, tello_address)
            print("この機体のシリアル番号: " + str(sent))
        else:
            msg = msg.encode(encoding="utf-8") 
            sent = sock.sendto(msg, tello_address)

    except KeyboardInterrupt:
        sock.close()  
        break