お題
トイドローン Tello のサンプルプログラム Tello3.py を改良して、ドローンの状態を受信して表示します。
前回の記事の最後に「ドローンが突然動作 (離陸) しなくなった」と書いたのですが、単なるバッテリー切れでした💦
Telloはバッテリー残量が少なくなると離陸コマンドを受け付けなくなるようです。ということで今回はドローンの状態を受信してバッテリー残量を表示してみたいと思います。
Telloの状態
Telloからは以下の状態が取得できます。
| pitch | ピッチ (角度) |
|---|---|
| roll | ロール (角度) |
| yaw | ヨー (角度) |
| vgx | x速度 |
| vgy | y速度 |
| vgz | z速度 |
| templ | 最低温度 (℃) |
| temph | 最高温度 (℃) |
| tof | TOF距離 (cm) |
| h | 高さ(cm) |
| bat | バッテリー残量 (%) |
| baro | 気圧 (cm) |
| time | モーター起動時間 |
| agx | x加速度 |
| agy | y加速度 |
| agz | z加速度 |
Telloの状態取得方法
SDKドキュメント によると、Telloの状態を受信するには以下を行う必要があります。
- 事前に
commandコマンドを送信しておく - UDP 8890番ポートで待ち受けを行う
これに従って、前回のサンプルプログラムに「起動時にTelloの状態を表示する」という実装を追加してみます (差分)。
(...)
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock.bind(("", 8889))
# command コマンドを送信
sock.sendto("command".encode(), ("192.168.10.1", 8889))
sock.recvfrom(1024)
# 状態を受信して表示
state_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
state_sock.bind(("", 8890))
data, _ = state_sock.recvfrom(1024)
print()
print(data.decode())
print("Tello Python3 Demo.")
(...)実行すると以下のように出力されます。
pitch:0;roll:0;yaw:2;vgx:0;vgy:0;vgz:0;templ:87;temph:89;tof:10;h:0;bat:57;baro:59.87;time:0;agx:-1.00;agy:2.00;agz:-1000.00;バッテリー残量は bat:57 の部分が該当するので、現在のバッテリー残量は「57%」ということになります。
表示メッセージの改善
今の出力内容だとちょっと情報が多すぎて分かりにくいので「バッテリーの残量だけ」を取り出して表示するように表示内容を改善します。
具体的には以下の実装を追加します:
- state の文字列を分解してディクショナリに格納
- batの値だけを取得して表示
state の文字列を分解してディクショナリに格納する処理はこんな感じです:
def __get_drone_state(data):
s = data.decode(errors="replace")
# state の文字列を `;` で区切って
# `pitch:0`, `roll:0`, `yaw:2`... という形に分割する
values = s.split(";")
state = {}
for v in values:
# 分割された文字列をさらに `:` で区切り
# キー (`pitch`) と値 (`0`) に分割して、ディクショナリに格納する
kv = v.split(":")
if len(kv) > 1:
state[kv[0]] = float(kv[1])
return state表示部分は以下のように変更します:
print(f'battery: {__get_drone_state(data)["bat"]}%')実行結果:
% python main.py
battery: 39.0%
Tello Python3 Demo.
(...)起動時以外も状態取得する
起動直後だけでなく、それ以降も状態取得してバッテリーの減り具合が確認できるともっと便利そうです。
状態取得の部分をスレッドにすればこれも実現できそうなので実装してみます。
もともとのサンプルプログラム Tello3.py を参考にして以下のように実装してみました (差分):
import threading
(...)
def receive_state():
while True:
try:
data, _ = state_sock.recvfrom(1024)
print(f'battery: {__get_drone_state(data)["bat"]}%')
except Exception:
print("\nExit . . .\n")
break
state_receive_thread = threading.Thread(target=receive_state)
state_receive_thread.start()実行結果:
% python main.py
battery: 39.0%
Tello Python3 Demo.
Tello: takeoff land flip forward back left right
up down cw ccw speed speed?
end -- quit demo.
> battery: 39.0%
battery: 39.0%
battery: 39.0%
battery: 39.0%
battery: 39.0%
battery: 39.0%
battery: 39.0%
battery: 39.0%
battery: 39.0%
(...)想定通りといえば想定通りなのですが、延々とバッテリー表示が出続けてまともにコマンドが打てない状態になってしまいました💦
ということで、次回はターミナルでの入出力を卒業して GUI を導入してみたいと思います。
Tello 関連記事
- Python によるドローン操作プログラミング
- Low-Level Protocolの活用
- 複合現実ヘッドセットによるドローン操作