お題

トイドローン Tello のサンプルプログラム Tello3.py を改良します。

サンプルプログラムの改善点

サンプルプログラムを動かしたりソースコードを眺めたりしているうちにこのサンプルプログラムにはいくつか改善できそうな点があることに気付きました。

エラー発生の抑止

何度かコマンドを実行するうちに気付いたことですが、送信してその結果(OK)が返ってくるまでに別のコマンドを送信すると必ずエラーになります。

例えば、takeoff を実行してその結果が返ってくる前に別のコマンド (up 20) を実行すると以下のようにエラーが発生します。

takeoff                <-- コマンド実行(1)
up 20            <-- コマンド実行(2)
error Not joystick     <-- コマンド実行(2)の結果 (エラー)
ok                     <-- コマンド実行(1)の結果

このように最初に実行したコマンドの結果が返ってくる前に別のコマンドを実行しても即座にエラーが発生します。 であれば「コマンド実行結果が返ってくるまで別のコマンドを実行できないようにする」とするのがよさそうです。

サンプルプログラムでは「コマンド実行処理 (コマンドをドローンへ送信する処理)」と「コマンド送信結果の受信処理」は以下のように実装されていました。

コマンド送信処理:

while True: 
    try:
        (...) 
        sent = sock.sendto(msg, tello_address) # コマンド送信処理

コマンド送信結果の受信処理:

def recv():
    while True: 
        try:
            data, server = sock.recvfrom(1518)  # コマンド結果受信処理
    (...) 

recvThread = threading.Thread(target=recv) # コマンド結果受信処理用スレッド作成
recvThread.start()                         # スレッド起動

「コマンドの送信」と「コマンド送信結果の受信」が別のスレッドで実行されています。

これを「コマンド実行結果が返ってくるまで別のコマンドを実行できないようにする」ためには、受信処理を専用のスレッドで実行するのをやめて以下のようにこの二つの処理を同一のスレッドで連続して行うように実装すればよさそうです。

while True: 
    try:
        (...) 
        sent = sock.sendto(msg, tello_address) # コマンド送信処理
        data, server = sock.recvfrom(1518)     # コマンド受信処理

完成したコード

コード全体は以下になります :

import socket
import time

sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock.bind(("", 8889))

print("Tello Python3 Demo.")
print("Tello: command takeoff land flip forward back left right")
print("       up down cw ccw speed speed?")
print("end -- quit demo.")

while True:
    try:
        msg = input("> ")

        if not msg:
            break

        if "end" in msg:
            print("...")
            sock.close()
            break

        sock.sendto(msg.encode(), ("192.168.10.1", 8889))
        start = time.time()

        data, _ = sock.recvfrom(1024)
        print(data.decode(), f"{time.time() - start:.1f}\n")
    except KeyboardInterrupt:
        print(" . . .")
        sock.close()
        break

以下のような対応もあわせて実施しました。

  • 前回の記事で触れた謎の9000ポートを使うのをやめる
  • 無駄な処理を削除する
  • コマンド実行にかかる時間 (コマンド送信から結果受信までの時間) を表示する

もともとのサンプルプログラムは70行程度ですが、こちらの改良版では半分の30行ちょっとになりました🙂

実行するとこんな感じ:

Tello Python3 Demo.
Tello: command takeoff land flip forward back left right
       up down cw ccw speed speed?
end -- quit demo.

> command
ok 0.0

> takeoff
ok 8.8

> up 90
ok 2.5

> down 20
ok 2.3

> land
ok 4.7

> end
...

トラブル発生

これに気をよくしてしばらく遊んでいたら、突然動作しなくなりました😭
(takeoff しない)

ということで、解決したら顛末を書きます。

(2022-06-04更新) 書きました 🙂

Tello 関連記事

かわかみしんいち。島根県津和野町在住のフリーランスエンジニア。複合現実(Mixed Reality)と3DUXでおもちゃを作るのが趣味。 https://github.com/ototadana