Dronecode - MAVLinkの入門

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

# 目次
1. [PX4環境構築](https://qiita.com/k-koh/items/77af5422b3857e68ad9a)
2. [uORB Messaging](https://qiita.com/k-koh/items/c57c02df772785816c14)
3. [QGroundControlの使い方（ユーザーガイド）](https://qiita.com/k-koh/items/eedc303cb0b72d1272ee)
4. [MAVLinkの入門](https://qiita.com/k-koh/items/9e9c1694dbb5a2a4117a)
5. [QGroundControlのカスタマイズ開発](https://qiita.com/k-koh/items/36c34f0e3e18b2e055ac)

## 動作環境
- Ubuntu 18.04
- ROS Melodic

## MAVLink

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

### MAVLink Versions

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


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

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/386259/76ef1e76-c3c9-a8ab-05ed-500b55ef887e.png)

| 項目     |   名前  | 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）

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/386259/57a11155-8160-14f8-6eba-fb5b7d488218.png)

| 項目     |   名前  | 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をサポートしない

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/386259/cdec56cf-4ce0-90b2-33de-baddd870bfec.png)


### Heartbeat/Connection プロトコル

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

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

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

common/mavlink_msg_heartbeat.h

```c++: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の送信

```c++: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チェックサムの計算処理関数

```c++: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フォーマットに設定する関数

```c++: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データを解析する関数

```c++: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データタイプ

```c++: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

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/386259/8b9b3423-ca5b-b2b3-a184-b4fd55073f61.png)

### Download Mission

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/386259/24a9ec05-2f27-28b5-6dc9-9fe53ff3ae06.png)