Dronecode - MAVLinkの入門

ドローンの自律飛行を実現するには、DronecodeとMAVSDKを試してみます。

目次

1. PX4環境構築
2. uORB Messaging
3. QGroundControlの使い方（ユーザーガイド）
4. MAVLinkの入門
5. QGroundControlのカスタマイズ開発

動作環境

• Ubuntu 18.04
• ROS Melodic

MAVLink

MAVLinkとは、ドローンと地上ステーションの間に通信用データの送信と受信する機能、チェックサム機能のプロトコルです。

MAVLink Versions

Versions	普及時期	備考
1.0	2013
2.0	2017	現在推奨、V1.0と下位互換性有り

MAVLinkのデータ構造（v1.0）

項目	名前	index	長さ	値
STX	開始標識	0	1	254(0xFE)
LEN	PAYLOADの長さ	1	1	N(0-255)
SEQ	シーケンス番号	2	1	0-255
SYS	システムID	3	1	1-255
COMP	コンポーネントID	4	1	1-255
MSG	メッセージ種類ID	5	1	0-255
PAYLOAD	メッセージデータ	6	N(0-255)
CRC	CRCチェックサム	N+7	2

MAVLinkのデータ構造（v2.0）

項目	名前	index	長さ	値
STX	開始標識	0	1	253(0xFD)
LEN	PAYLOADの長さ	1	1	N(0-255)
INC FLAGS	Incompatibility Flags	2	1
CMP FLAGS	Compatibility Flags	3	1
SEQ	シーケンス番号	4	1	0-255
SYS	システムID	5	1	1-255
COMP	コンポーネントID	6	1	1-255
MSG	メッセージ種類ID	7-9	3	0 - 16777215
PAYLOAD	メッセージデータ	10	N(0-255)
CRC	CRCチェックサム	N+11	2
Signature	Signature	N+12	13

多バイトのデータのバイトオーダー(リトルエンディアン)

Message ID: uint32_t msgid:24(low, middle, high bytes)
CRC: uint16_t (low byte, high byte)

MAVLinkデータのサンプル（v2.0）

MISSION_COUNT (#44)のバイナリデータ（16進数で記述）

fd 04 00 00 37 ff 00 2c 00 00 [ 09 00 01 01 ] 29 24

項目	名前	index	長さ	値
STX	開始標識	0	1	253(0xfd)
LEN	PAYLOADの長さ	1	1	0x04
INC FLAGS	Incompatibility Flags	2	1	0x00
CMP FLAGS	Compatibility Flags	3	1	0x00
SEQ	シーケンス番号	4	1	0x37
SYS	システムID	5	1	255(0xff)
COMP	コンポーネントID	6	1	0x00
MSG	メッセージ種類ID	7-9	3	44(0x00002c)[ 2c 00 00 ]
PAYLOAD	メッセージデータ	10	N(0-255)	[ 09 00 01 01 ]
CRC	CRCチェックサム	N+11	2	[29 24]
Signature	Signature	N+12	13

MISSION_COUNT ( #44 )PAYLOADのバイナリデータ（16進数で記述）

[ 09 00 01 01 ]

項目	Description	index	Type	値
target_system	System ID	0	uint8_t	0x01
target_component	Component ID	1	uint8_t	0x01
count	Number of mission items	2	uint16_t	0x0009 [ 09 00 ]

MAVLink Versionsのハンドシェーク

autopilotとGCSの通信には、MAVLink 2をサポートできるかを判定する。 GCSからCOMMAND_LONG、或は COMMAND_INT メッセージを送信する。

送信：
command ID：MAV_CMD_REQUEST_PROTOCOL_VERSION

受信：
PROTOCOL_VERSION：MAVLink 2をサポート
NACK：MAVLink 2をサポートしない

Heartbeat/Connection プロトコル

Heartbeatプロトコルは、システムの存在をMAVLink ネットワークに通知する。

役割：接続と切断の検知、GCSレイアウトの表示、メッセージのルーティング

頻度：通常 1 Hz、４〜５のHeartbeatが受信できない場合切断とする

common/mavlink_msg_heartbeat.h

mavlink_msg_heartbeat.h

typedef struct __mavlink_heartbeat_t {
 uint32_t custom_mode; /*<  A bitfield for use for autopilot-specific flags*/
 uint8_t type; /*<  Vehicle or component type. For a flight controller component the vehicle type (quadrotor, helicopter, etc.). For other components the component type (e.g. camera, gimbal, etc.). This should be used in preference to component id for identifying the component type.*/
 uint8_t autopilot; /*<  Autopilot type / class. Use MAV_AUTOPILOT_INVALID for components that are not flight controllers.*/
 uint8_t base_mode; /*<  System mode bitmap.*/
 uint8_t system_status; /*<  System status flag.*/
 uint8_t mavlink_version; /*<  MAVLink version, not writable by user, gets added by protocol because of magic data type: uint8_t_mavlink_version*/
} mavlink_heartbeat_t;

heartbeatの送信

mavlink_msg_heartbeat.h

static inline void mavlink_msg_heartbeat_send
(
  mavlink_channel_t chan, 
  uint8_t type, 
  uint8_t autopilot, 
  uint8_t base_mode, 
  uint32_t custom_mode, 
  uint8_t system_status
)

SEQシーケンス番号の設定、CRCチェックサムの計算処理関数

mavlink_helper.h

MAVLINK_HELPER void _mav_finalize_message_chan_send
(
  mavlink_channel_t chan, 
  uint32_t msgid,
  const char *packet, 
  uint8_t min_length, 
  uint8_t length, 
  uint8_t crc_extra
)

heartbeatメッセージをheartbeatフォーマットに設定する関数

mavlink_msg_heartbeat.h

static inline uint16_t mavlink_msg_heartbeat_pack
(
  uint8_t system_id, 
  uint8_t component_id, 
  mavlink_message_t* msg, 
  uint8_t type, 
  uint8_t autopilot, 
  uint8_t base_mode, 
  uint32_t custom_mode, 
  uint8_t system_status
)

受信したmavlinkデータを解析する関数

mavlink_helper.h

MAVLINK_HELPER uint8_t mavlink_parse_char
(
  uint8_t chan, 
  uint8_t c, 
  mavlink_message_t* r_message, 
  mavlink_status_t* r_mavlink_status
)

解析したmavlinkデータタイプ

mavlink_types.h

MAVPACKED(
typedef struct __mavlink_message {
    uint16_t checksum;      ///< sent at end of packet
    uint8_t magic;          ///< protocol magic marker
    uint8_t len;            ///< Length of payload
    uint8_t incompat_flags; ///< flags that must be understood
    uint8_t compat_flags;   ///< flags that can be ignored if not understood
    uint8_t seq;            ///< Sequence of packet
    uint8_t sysid;          ///< ID of message sender system/aircraft
    uint8_t compid;         ///< ID of the message sender component
    uint32_t msgid:24;      ///< ID of message in payload
    uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8];
    uint8_t ck[2];          ///< incoming checksum bytes
    uint8_t signature[MAVLINK_SIGNATURE_BLOCK_LEN];
}) mavlink_message_t;

Upload Mission

Download Mission