MAVLink を利用して Raspberry Pi と Mission Planner を接続する（２）
おはようございます。ユービス株式会社の佃煮です。
今回は、MAVLinkの最初の接続手続きについてわかったことを記録していきます。
「阿若！」（アンニャーシ：完全に理解した！）とはいきませんが、理解できたことはひたすら書いておこうかと。
最初の接続を確立する
前回は pymablink のインポートについて解説していました。
今回は実際にライブラリを利用して、WindowsPC の地上ステーション（Mission Planner）と 制御PCに見立てた Raspberry Pi とを接続してみましょう。
Mission Planner には接続機能が備わっていますので、この Mission Planner からの接続をRaspberry Pi 側がきちんと理解して確立できるようになるのが今回の目標です。
MAVLinkの通信開始手続き：コネクションを作る
mavutil を利用すると、簡単にコネクションオブジェクトを用意することができます。
オブジェクトはライブラリ内の mavlink_connection というメソッドで作成できます。
書式は以下の通りです：
connection_object = mavutil.mavlink_connection('protocol:device or address:port')以下に一例を挙げます：
conn = mavutil.mavlink_connection('udp:localhost:14540')例えばこれだけで、UDP接続の準備が完了します。
UDP/TCPプロトコルのほかにも、WindowsのCOMポートやLinuxの/dev/にも対応しています（詳細は公式ページへ）
地上ステーションやコントロールプログラム等、接続して制御しに行く側は udp や tcp のように打ちっ放しでOKです。（自作のプログラムでドローンを操縦する等）
しかし今回は地上ステーションからの接続を待って制御される側ですので、udpin または tcpin を利用します。つまり：
conn = mavutil.mavlink_connection('tcpin:192.168.xxx.xxx:5760')となります。
udpin/tcpinと入力側であることを明示することで、接続の待ち受けを開始することができます。（受信側なのに udp/udpout や tcp/tcpout を利用すると、接続できずにプログラムが落ちます）
ポート開放
ネットワーク通信ですので、当然利用するポートを開放しておく必要があります。
ドローン界隈で一般的なのは UDP:14540, 14550, 14560 TCP:5760 あたりのようです。
（よくサンプルプログラムに出てくるので、このあたりを指定しておけば間違いないでしょう）
接続に利用する地上ステーションは準備（前回）のとおり「Mission Planner」を利用しますが、これがIPアドレス越しの接続時になぜかUDP接続ではうまくいかないため、TCP接続を利用するといった感じになっております。（多分、自身の知識が不足してるからと思います…）
ということで、TCP接続ポート5760番を開放します。
ファイアウォールを設定している場合、予めここに穴をあけておかなければいけません。
他にも通信ポートが増える場合は、都度ポート開放をしておく必要が出てきます。
この設定をうっかり失念していて、接続拒否をくらいまくって半日消費しました…orz
ハートビートの送受信
上記オブジェクトが無事作成されれば、以降はそのオブジェクトを利用して MAVLink メッセージの送受信ができます。メッセージは様々用意されていますが、最重要なのは接続を維持するための「ハートビート」です。
ハートビートは「自身はこういう者で、現在応答できます」ということを示すメッセージです。このメッセージを相互に確認することで、お互いに応答できる＝通信が可能であると認識します。
ハートビートは送信周期が決められており、稼働しているシステムは常にこのメッセージを1Hz、つまり1秒ごとに1回発信するようにします。
それでは、実際にハートビートの送受信の方法を見てみましょう。
ハートビートの送信
ハートビートもそうなのですが、実は MAVLink のメッセージは基本的に打ちっ放しです。
mavutil は基本メッセージを簡単に送信できるように設計されており、その中でも最重要のハートビートは送受信ともに専用メソッドが用意されていますので、送信するだけならばさほど難しいことはありません。
ハートビートの送信には、 mav.heartbeat_send というメソッドを利用します。
ハートビートメッセージを送るメソッドは以下の通りです：
connection_object.mav.heartbeat_send(機種タイプ, 自動運転方法・クラス, ベースモード, カスタムモード, システムステータス, mavlinkバージョン, mavlink1メッセージ利用) mavlinkバージョン情報とmavlink1メッセージの利用は省略可能です。
mavlink1メッセージ利用は、mavlink2メッセージが利用できない無線機等で利用するフラグと思われます。
例文としては以下になります：
conn.mav.heartbeat_send(mavutil.mavlink.MAV_TYPE_GROUND_ROVER, mavutil.mavlink.MAV_AUTOPILOT_GENERIC, 0, 0, mavutil.mavlink.MAV_STATE_STANDBY)意味としては、
「私は地上を走る車両で、一般的な自動運転メッセージに対応していて、応答をする準備ができています」
という「ハートビート」メッセージを接続済のネットワークに送信します。
このように、ハートビートは車種、制御方法、車両の状態などを発信するメッセージとなっています。これを定期的に送信することで、相手側は「この車両は応答があるので、通信ができる」ことを確認できます。
試験中はタイマーをインポートして一定間隔で送信する程度で良いと思いますが、実際にはセンサー類の動作や駆動部の制御との兼ね合いもありますので、できれば割り込み処理か何かで定期的に流せるようにしておくとよいでしょう。
定数それぞれの意味合いについては、以下のリファレンスを参照してください。
Messages (common) · MAVLink Developer Guide
※情報量が非常に多いので、検索をうまく活用してください。
ハートビートの受信
一方で、接続相手のハートビートを受信できなければ、プログラムによっては「接続できてない」とみなされる可能性もあります。また、自分からも相手の情報がわからなければ、何をどうやり取りすればよいのかわかりません。
そこで今度は、相手の「ハートビート」メッセージの受信を試みます。これには wait_heartbeat というメソッドが用意されています。
1秒ごとに「ハートビート」が送られてくるので通常は1秒以内に複数回確認すべきでしょうが（別の処理も動いているはずです）、初期状態ではメッセージを受信するまでスレッドを止めることもできます。戻り値は、相手の「ハートビート」メッセージです。相手方の情報がしっかり入ってきます。
以下のようになります：
message = connection_object.wait_heartbeat(blocking=True, timeout=None)特に設定がなければ、普通に呼び出すだけで相手の「ハートビート」メッセージが来るまでスレッドを停止します。
スレッドを停止されると困る場合は、引数に blocking=False を指定するか、timeout=1 でタイムアウトを設定すると完全フリーズを回避できます。
ですので、送受信は以下のような感じで組み合わせてみましょう。
import time
from pymavlink import mavutil
b = False
conn = mavutil.mavlink_connection('tcpin:192.168.xxx.xxx:5760', source_system=255)
while !b:
conn.mav.heartbeat_send(mavutil.mavlink.MAV_TYPE_GROUND_ROVER, mavutil.mavlink.MAV_AUTOPILOT_GENERIC, 0, 0, mavutil.mavlink.MAV_STATE_STANDBY)
beat = conn.wait_heartbeat(blocking=False)
if not beat:
print("waiting")
else:
b = True
time.sleep(1)
print("HeartBeat recv system: %u component: %u" % (conn.target_system, conn.target_component)「ハートビート」を受信するまでは約1秒ごとに「waiting」を表示しつつループします。
「ハートビート」を受信したらループから抜けて、接続相手のシステムとコンポーネントの情報を取得してプログラムを終了します。
では実際に上記の python プログラムをラズパイで走らせて、別PCから MissionPlanner を立ち上げて接続してみましょう。
Mission Planner での接続は、メイン画面の右上にコード型の「接続」ボタンがあります。その左にプロトコルとボーレートの表示があります。
TCP接続の場合はボーレートはさほど気にしなくてもよさそうですので、ボーレートは特に触らずプロトコルをTCPに設定して接続ボタンを押してみましょう。
するとまず、接続先のアドレスを聞かれますので、プログラム内で設定した Raspberry Pi のIPアドレスを入力しましょう。（事前に Raspberry Pi のローカルIPを固定しておいてください）
次にポート番号を聞かれますので、こちらもプログラム内で設定したポートを入力します。両方入力をすると、接続を試行します。
成功すれば、ラズパイのコンソールに「HeartBeat recv system: 0 component: 0」みたいな感じの表示が出て終了します。
とりあえず接続できたように見えます
これで、「ハートビート」をやり取りする準備ができました。