MAVLinkまたはMicro Air Vehicle Linkは、無人偵察機または小型の無人航空機（飛行、浮上、クロールなど）との情報相互作用のプロトコルであり、以下MAV（Micro Air Vehicle）と呼ばれます。 MAVLinkは、LGPLライセンスの下で、Python用モジュール（便利なDroneKitラッパーがあります）およびヘッダー専用C / C ++ライブラリを含むさまざまな言語用のライブラリジェネレーターの形で配布されます。 MAVLink バージョンv1 （これから使用します）およびバージョンv2用に既に生成されたライブラリのリポジトリもあります。
このプロトコルは、MAVとGCS（地上管制局）-地上管制局、およびその構成要素-コンポーネントなどのシステム間の情報の相互作用を記述しています。 MAVLinkの基本は、次の形式のパッケージです。
パケットの最初のバイト（ STX ）は開始文字です。バージョンv2.0では0xFD、バージョンv1.0では0xFE、バージョンv0.9では0x55です。 LEN-ペイロードの長さ（メッセージ）。 SEQ-パケットカウンター（0〜255）が含まれています。これは、メッセージの損失を識別するのに役立ちます。 SYS （システムID）は送信側システムの識別子であり、 COMP （コンポーネントID）は送信側コンポーネントの識別子です。 MSG （メッセージID）-メッセージのタイプ。パケットのペイロードに含まれるデータを決定します。 PAYLOAD-パケットペイロード、メッセージ、0〜255バイトのサイズ。 パケットの最後の2バイト-CKAおよびCKB （それぞれ下位および上位バイト）には、パケットのチェックサムが含まれます。
MAVLinkライブラリを使用すると、プロトコルに従ってパケットをエンコードおよびデコードできますが、データの送信先となるハードウェアおよびソフトウェアを規制しません。TCP/ UDPメッセージ、シリアル通信、または双方向通信を提供するものであれば何でもかまいません。 ライブラリは入力データをバイト単位で処理し、それをバッファに追加し、それ自体からパケットを収集します。 各システムまたはコンポーネントは、異なるソースで同時にデータを交換できます。その後、各ソースにchannelと呼ばれる特別な識別子が割り当てられます 。 MAVLinkには、各チャネルにバッファーが含まれています。
ボードからハートビートを取得します
理論から実践に移り、MAVLinkの上にOOPラッパーを作成してみましょう。 以下に、Qtを使用したC ++コードの例を示します。 私はこのツールを選択しました。最初に、将来Qt QuickとQt Locationを使用してMAVLinkパラメーターを視覚化する予定であり、次にQtで記述されたqgroundcontrolプロジェクトの多くのソリューションを見ました。
まず、通信チャネルに抽象化を導入します。これをAbstractLinkクラスとし、そのインターフェイスで、QByteArrayの形式でデータを送受信できる機能を定義します。 このクラスの相続人であるUdpLinkおよびSerialLinkは、ネットワークおよびシリアルポートを介したデータ転送を提供します。
クラスインターフェイスAbstractLink
class AbstractLink: public QObject { Q_OBJECT public: explicit AbstractLink(QObject* parent = nullptr); virtual bool isUp() const = 0; public slots: virtual void up() = 0; virtual void down() = 0; virtual void sendData(const QByteArray& data) = 0; signals: void upChanged(bool isUp); void dataReceived(const QByteArray& data); };
MavLinkCommunicatorクラスのMAVLinkプロトコルで直接作業をカプセル化します。 彼の職務には、通信チャネルでデータを受信し、それらをmavlink_message_tパケットにデコードすること、および通信チャネルでメッセージを送信することが含まれます。 各通信チャネルに対して、MAVLinkには独自のバッファが含まれているため、通信チャネルへのポインタの辞書をチャネル識別子に導入します。
MavLinkCommunicatorクラスインターフェイス
class MavLinkCommunicator: public QObject { Q_OBJECT public: MavLinkCommunicator(QObject* parent = nullptr); public slots: void addLink(AbstractLink* link, uint8_t channel); void removeLink(AbstractLink* link); void sendMessage(mavlink_message_t& message, AbstractLink* link); void sendMessageOnLastReceivedLink(mavlink_message_t& message); void sendMessageOnAllLinks(mavlink_message_t& message); signals: void messageReceived(const mavlink_message_t& message); private slots: void onDataReceived(const QByteArray& data); private: QMap<AbstractLink*, uint8_t> m_linkChannels; AbstractLink* m_lastReceivedLink; };
データストリームからパッケージを組み立てる方法を検討してください。 上記のように、MAVLinkは受信データストリームをバイト単位で読み取ります。このため、mavlink_parse_char関数が使用され、メッセージデータを受信できない場合はNULLを返し、受信文字を内部バッファーに保存します。 MAVLinkには、各チャネルのバッファーが含まれています。 このアプローチにより、データをシリアルポートからMAVLink解析関数に直接転送することができ、ユーザーがプレジャーライブラリを使用してストリームからメッセージを手動で収集できなくなります。
MavLinkCommunicatorクラスパッケージビルドメソッド
void MavLinkCommunicator::onDataReceived(const QByteArray& data) { mavlink_message_t message; mavlink_status_t status; // onDataReceived , AbstractLink m_lastReceivedLink = qobject_cast<AbstractLink*>(this->sender()); if (!m_lastReceivedLink) return; // uint8_t channel = m_linkChannels.value(m_lastReceivedLink); for (int pos = 0; pos < data.length(); ++pos) { if (!mavlink_parse_char(channel, (uint8_t)data[pos], &message, &status)) continue; emit messageReceived(message); // } // }
有用なデータを取得するには、パッケージアセンブリだけでは不十分です。 パッケージからメッセージを受信し、msgid識別子に従ってペイロードを抽出する必要があります 。 MAVLinkには、各msgid（メッセージタイプ）の組み込みタイプと、パッケージからこれらのメッセージを受信するための関数のセットがあります。 別の抽象タイプ-AbstractHandlerを導入します。このクラスのインターフェースでは、MavLinkCommunicatorから受信したメッセージを処理するための純粋な仮想スロットprocessMessageを定義します。 AbstractHandlerクラスの子孫は、特定のメッセージを処理できるかどうかを決定し、可能であればそれを処理します。 たとえば、 heartbeatなどのメッセージを処理します。 これは最も基本的なパッケージであり、システムはそれが存在し、それが何であるかを示します。 MAVLinkが使用する必要があるパッケージのタイプはハートビートだけであることに注意してください。 このタイプのメッセージハンドラ、HeartbeatHandlerを紹介しましょう。
HeartbeatHandlerクラスのprocessMessageメソッドの実装
void HeartbeatHandler::processMessage(const mavlink_message_t& message) { // , if (message.msgid != MAVLINK_MSG_ID_HEARTBEAT) return; mavlink_heartbeat_t heartbeat; // heartbeat mavlink_msg_heartbeat_decode(&message, &heartbeat); // // , qDebug() << "Heartbeat received, system type:" << heartbeat.type; }
ここで、クラスを構成して正しい接続を確立すると、フライトコントローラーからハートビートメッセージを受信できます。 いくつかの無線モデムと、APMオートパイロットが実行されているNAVIO2シールド付きのRaspberry Piを使用します。 理論的には、これは現在のバージョンのMAVLinkをサポートするすべての自動操縦で動作するはずですが、手元に何もない場合は、もう少し自動操縦シミュレータの例になります。
機能コードメイン
int main(int argc, char* argv[]) { QCoreApplication app(argc, argv); domain::MavLinkCommunicator communicator; domain::HeartbeatHandler heartbeatHandler; // heartbeat QObject::connect(&communicator, &domain::MavLinkCommunicator::messageReceived, &heartbeatHandler, &domain::HeartbeatHandler::processMessage); domain::SerialLink link("/dev/ttyUSB0", 57600); // communicator.addLink(&link, MAVLINK_COMM_0); link.up(); return app.exec(); }
プログラムを起動し、自動操縦をオンにして、数秒後に実行する必要があります。
Heartbeat received, system type: 1 System status: 2 Heartbeat received, system type: 1 System status: 2 Heartbeat received, system type: 1 System status: 2 Heartbeat received, system type: 1 System status: 5 Heartbeat received, system type: 1 System status: 5
ハートビートを送る
計画どおり、各システムはハートビートを送信する必要があります。 MavLinkCommunicatorクラスのパッケージを送信する機能を実装することから始めましょう。 mavlink_msg_to_send_buffer関数は、送信のためにメッセージパケットをバッファに書き込みます。 この段階では、長さとチェックサムを含むパケットのすべてのフィールドが正しく入力されていると想定されます。
MavLinkCommunicatorクラスのパッケージを送信する方法
void MavLinkCommunicator::sendMessage(mavlink_message_t& message, AbstractLink* link) { if (!link || !link->isUp()) return; uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; int lenght = mavlink_msg_to_send_buffer(buffer, &message); if (!lenght) return; link->sendData(QByteArray((const char*)buffer, lenght)); }
パケットを送信する機能ができたので、メッセージを作成してパケットに書き込むだけです。 このタスクを既存のHeartbeatHandlerクラスに委任し、AbstractHandlerインターフェイスにメッセージ送信信号を追加します。 mavlink_msg_heartbeat_encode関数は、ハートビートメッセージをパケットに書き込みます。すべての組み込みメッセージに同様の関数が存在します。 mavlinkは追加の機能を提供するという事実に注目します。たとえば、mavlink_msg_heartbeat_packを使用すると、mavlink_heartbeat_t型のオブジェクトを明示的に作成せずにハートビートメッセージをmavlink_message_tに書き込むことができます。 これらの機能の使用方法の詳細については、 こちらをご覧ください 。 オプションの_chan末尾（たとえば、mavlink_msg_heartbeat_pack_chan）は、メッセージが送信されるチャネルを示します。
HeartbeatHandler timerEventイベントコード
void HeartbeatHandler::timerEvent(QTimerEvent* event) { Q_UNUSED(event) mavlink_message_t message; mavlink_heartbeat_t heartbeat; heartbeat.type = m_type; mavlink_msg_heartbeat_encode(m_systemId, m_componentId, &message, &heartbeat); emit sendMessage(message); }
周波数1 Hzのタイマーでハートビートを送信します。 通信チャネルdata.toHex（）のデータを送信するメソッドにデバッグ出力を配置すると、記事の冒頭の図にあるように、メッセージが表示されます。 各クロックカウンタが増加し、それに応じてチェックサムが変化するはずです。
"fe09000100000821ee85017f0000023f08" "fe09010100000821ee85017f000002d576" "fe09020100000821ee85017f000002ebf5"
ハートビートが機能しているかどうかを確認するために、2つのアセンブリターゲットを作成します。gcs-地上管制局のシミュレーターとuav-ドローンのシミュレーターです。
機能コードmain gcs
int main(int argc, char* argv[]) { QCoreApplication app(argc, argv); // GCS 255 sysid domain::MavLinkCommunicator communicator(255, 0); // - domain::HeartbeatHandler heartbeatHandler(MAV_TYPE_GCS); QObject::connect(&communicator, &domain::MavLinkCommunicator::messageReceived, &heartbeatHandler, &domain::HeartbeatHandler::processMessage); // heartbeat QObject::connect(&heartbeatHandler, &domain::HeartbeatHandler::sendMessage, &communicator, &domain::MavLinkCommunicator::sendMessageOnAllLinks); // UDP localhost domain::UdpLink link(14550, QString("127.0.0.1"), 14551); communicator.addLink(&link, MAVLINK_COMM_0); link.up(); return app.exec(); }
機能コードメインUAV
int main(int argc, char* argv[]) { QCoreApplication app(argc, argv); // - sysid=1 domain::MavLinkCommunicator communicator(1, 0); // - domain::HeartbeatHandler heartbeatHandler(MAV_TYPE_FIXED_WING); QObject::connect(&communicator, &domain::MavLinkCommunicator::messageReceived, &heartbeatHandler, &domain::HeartbeatHandler::processMessage); // heartbeat QObject::connect(&heartbeatHandler, &domain::HeartbeatHandler::sendMessage, &communicator, &domain::MavLinkCommunicator::sendMessageOnAllLinks); // UDP localhost domain::UdpLink link(14551, QString("127.0.0.1"), 14550); communicator.addLink(&link, MAVLINK_COMM_0); link.up(); return app.exec(); }
その結果、ハートビートパケットの双方向の交換が行われます。 必要に応じて、さらに実験することができます。別のシステムまたは通信チャネルを追加します。 この例の完全なソースコードはgithubにあります。 最初の部分が非常にシンプルになったにもかかわらず、それが面白かったと思います。 次回の記事では、他のタイプのメッセージと、それらでどのような面白いことができるかについてお話します。 ご清聴ありがとうございました！
興味深いリンク：
MAVLink公式ウェブサイト
Dronecodeプロジェクトサイト
DIYドローンの英語チュートリアル