Ardupilot mavgenが生成するモジュールとmavutil

ロボット

Ardupilotのプログラミング環境において、多くはPythonで間に合う。
PyMAVLinkという低レベルな操作、Dronekitとという高水準なものまであり、MAVProxyなどもPythonで書かれている。
ただし、FirmwareやGCSはC++やC#で書かれている。

mavgenとは

PyMAVLinkの範疇に、mavgenという一風変わったツールがあり、C, C++, C#, Python, Javascript,TypeScrpt, Lua, WLUa, Objective-C, Swift, Javaといったプログラミング言語それぞれに対応したライブラリーを出力してくれる。
じゃ、中身がなにかというと、概要いわく

「メッセージ(MAVLink)のエンコード、デコード、及び署名とチェックを行う(MAVLink2）機能を提供します。
一般的には開発者はmavutilモジュールで間に合うはずですが、このモジュールはリンクの設定、メッセージの送受信、現在アクティブなモードなど自動操縦の状態を問い合わせる方法を提供します。
mavutilを使う時には注意すべき点があります。

  • リンクは同じポートで実行されている複数システムは適切に処理しません。
  • モジュールはArdupilot用です。
  • mavutilは低レベルMAVLink APIです。MAVLink マイクロサービスはより高水準です。

PyMAVLink (mavgenで生成されるモジュール）では

  • XMLファイルで定義された定数
  • メッセージ識別子の定数
  • MAVLInkメッセージのクラス（XMLフォーマット）
  • mavutilは通信リンクのセットアップ、メッセージの受信とデコード、MAVLinkユーティリティ関数がそろっています。
  • mavwpはウェイポイント、ジオフェンス、ラリーポイントをロード・保存するためのものです。
  • mavparm: MAVLinkパラメーターんのセットをロード・保存します
  • mavextra: 値やメッセージを変換するための関数（例：m/sec をkm/hへ）

以下、mavgen wikiの抄訳 mavutilを知りたかったため、低レベルapiはスキップしています。

 mavutil

接続の設定

mavutilはシリアル、tcp, udpチャネルでMAVL印k接続ができます。
次の例はmavlinkでUDPポートに接続し、HEARTBEATを待ちます。

from pymavlink import mavutil

# UDPポートで接続をリッスンする
the_connection = mavutil.mavlink_connection('udpin:localhost:14540')

# ハートビートを待つ 
the_connection.wait_heartbeat()

#   これはリンクに必要なリモートシステムシステムやコンポーネントIDを設定する
print("Heartbeat from system (system %u component %u)" % (the_connection.target_system, the_connection.target_component))

# コネクションが成立したら'the_connection'はメッセージの送受信に使うことができる

ここではシミュレーターを想定してudpinですが、udpoutで開始することもできます。

接続文字列の書式は

[プロトコル:ipアドレス:ポート]

です。

  • プロトコルは次のようなものがありますが、指定していないとシリアルか、ファイルか判断しようとし、いずれでもなければudpとします。
  • tcp:address:port TCP接続を開始します
  • tcpin:address:port 指定されたIPでTCP接続をリッスンします
  • udpin:address:port 指定されたIPでUDP接続をリッスンします
  • udpout:address:port 指定されたIPでudp接続を開始します
  • udp デフォルトではudpinと同じです。
  • udpcast: UDPアドレスとポートをブロードキャストします

メッセージの送信

たとえばsystem_time_send()関数はSYSTEM TIMEを送信するために使われます。

def system_time_send(self, time_unix_usec, time_boot_ms, force_mavlink1=False):
    '''
    The system time is the time of the master clock, typically the
    computer clock of the main onboard computer.

    time_unix_usec    : Timestamp (UNIX epoch time). (uint64_t)
    time_boot_ms      : Timestamp (time since system boot). (uint32_t)
    '''

リンク管理にmavutilを仕様している場合、同様のオブジェクトを使うことができます。
たとえばthe_connection SYSTEM_TIMEという名前でメッセージを送信するには、

the_connection.mav.system_time_send(time_unix_usec, time_boot_ms)

メッセージの受信

mavutil recv_match()メソッドを使用して、メッセージが到着するのを待って受け取ることができます。

the_connectionをセットアップしておいて、次のようなメッセージを待つことができます。

msg = the_connection.recv_match(blocking=True)

特定の属性値を持つ特定のメッセージを取得したい場合は、代わりに次のようにします。

# Wait for a 'SYS_STATUS' message with the specified values.
msg = the_connection.recv_match(type='SYS_STATUS', condition='SYS_STATUS.mode==2 and SYS_STATUS.nav_mode==4', blocking=True)

メッセージを使用する前に、メッセージが有効であることも確認する必要があります。

msg = m.recv_match(type='SYS_STATUS',blocking=True)
if not msg:
    return
if msg.get_type() == "BAD_DATA":
    if mavutil.all_printable(msg.data):
        sys.stdout.write(msg.data)
        sys.stdout.flush()
else:
    #Message is valid
    # Use the attribute
    print('Mode: %s' % msg.mode)

ハートビートの送信

the_connectionという名前のmavutilリンクを使用していると仮定すると、次のようにハートビートを送信できます。mavutil.mavlink_connection()

# Send heartbeat from a GCS (types are define as enum in the dialect file). 
the_connection.mav.heartbeat_send(mavutil.mavlink.MAV_TYPE_GCS,
                                                mavutil.mavlink.MAV_AUTOPILOT_INVALID, 0, 0, 0)

# Send heartbeat from a MAVLink application. 
the_connection.mav.heartbeat_send(mavutil.mavlink.MAV_TYPE_ONBOARD_CONTROLLER,
                                                mavutil.mavlink.MAV_AUTOPILOT_INVALID, 0, 0, 0)