Arduino（ESP8266）でドローンのダミーをつくる

# 背景

- とあるドローン（ドローンXとする）とmavlink on UARTで通信するコンパニオンコンピュータをつくっている
- ドローンXの実機は最高機密らしいので貸してもらえない
- なのでプログラムの開発がめんどくさい
- 今回の要件的にはCommon Dialectで通信できればよいようなので、ArduinoでドローンのUARTをシミュレートするダミーをつくることにした
- 最初、Arduino UNOを使っていたが、UNOのRAM（2KB）ではmavlinkのライブラリを動かすには十分でなかったので、RAMが潤沢なESP8266（ユーザは約50KBくらい使えるらしい）に切り替えた

## 余談

- Dialect（方言）というのがめんどくさい概念なんですが、mavlinkではドローンの機能をXMLで定義して、それらにmavlinkプロトコルでアクセスするライブラリを生成するという仕組みになっているのだが、この定義のことをDialectというらしい
- Common Dialectというのはmavlinkプロトコルであらかじめ規定されているDialectで、いわば標準語。[MAVLINK Common Message Set](https://mavlink.io/en/messages/common.html)にメッセージの一覧が載っている
- 標準語を定めてるのはBLEのGATTに近い考え方かもしれない
- いまならESP8266じゃなくてESP32じゃない？って思ったけど、手元にESPr DeveloperがあったのでESP8266にした。（ESP32だとRAMが500KBくらいあってすごいので、これから買う人はESP32がおすすめ）


# ハードウェア

- スイッチサイエンスの[ESPr Developer](https://www.switch-science.com/catalog/2500/)
- スイッチサイエンスの[FTDI USBシリアル変換アダプター Rev.2](https://www.switch-science.com/catalog/2782/)
- ESPr Developer の`4`をUSBシリアル変換アダプターの`TX`につなぐ
- ESPr Developer の`5`をUSBシリアル変換アダプターの`RX`につなぐ
- ESPr Developer側のUSBシリアルでスケッチの書込とデバッグ出力の確認を行う
- USBシリアル変換アダプター経由でコンパニオンコンピュータ側のコード（pymavlink）と通信を行う

![esp8266_dummy_drone.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/26038/ecea2046-ac67-f23e-ee62-674687d197d7.png)
↑たった今からこいつは自分のことをクアッドローター機だと思っているかわいそうなESP8266です


# プロトコル

最初、[Read Single Parameter](https://mavlink.io/en/services/parameter.html#read_single)に示されている手順

<img width="428" alt="スクリーンショット 2019-05-04 21.30.19.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/26038/4988a27a-60aa-c7bc-41e6-bc77abc3b87e.png">

を用いるのかと思ってた。
これによると、GCS（あるいはコンパニオンコンピュータ）は`PARAM_REQUEST_READ`メッセージを送って、`PARAM_VALUE`メッセージが返ってくるのを待てばよいようだ。
しかし、`PARAM_VALUE`で送るのはパラメータひとつだけの模様。

ドローンXや3DR Solo（こちらはUARTじゃなくてUDPだが）で試したときは、コンパニオンコンピュータ側から`PARAM_REQUEST_READ`メッセージを送って、`VFR_HUD`メッセージがかえってきていた。
（このメッセージには https://mavlink.io/en/messages/common.html#VFR_HUD に記されている6個のパラメータが全て詰まっていた）

このあたりのことが公式ドキュメントのどこに書いてあるのかよくわからなかったが、結局のところ`PARAM_VALUE`メッセージを送出するのではなく、`VFR_HUD`メッセージを送出すればよかった。（実際のやりかたは後述）

- [PARAM_REQUEST_READメッセージの仕様](https://mavlink.io/en/messages/common.html#PARAM_REQUEST_READ)
- [PARAM_VALUEメッセージの仕様](https://mavlink.io/en/messages/common.html#PARAM_VALUE)
- [VFR_HUDメッセージの仕様](https://mavlink.io/en/messages/common.html#VFR_HUD)

# ダミードローン（Arduino）のコード

- https://github.com/tmaxxdd/arduino-with-mavlink/blob/master/arduino/full_code.ino を参考に実装してみる。
- heartbeatを送る -> PARAM_REQUEST_READが来るのを待つ -> VFR_HUD を送出する を繰り返す
- スケッチ全体は https://gist.github.com/miminashi/7dcf215c73ff898c54ddd2502d3898f5 にあげておいた

## mavlink_parse_char でパケットを読む

`mavlink_helpers.h`に説明が書いてあるのだが、`mavlink_parse_char`というのは便利な関数っぽくて、いちどに1バイトずつパケットの断片（char）を読んで、パース結果を`msg`に詰めて、パースが完了したら1を返してくれるっぽい。
つまり、次のようなコードでシリアル（UART）からmavlinkパケットを読み込める。

```c
mavlink_message_t msg;

while (Serial.available()) {
  uint8_t byte = Serial.read();
  if (mavlink_parse_char(chan, byte, &msg)) {
      printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid);
  }
}
```

## パラメータ VFR_HUD の送出

VFR_HUDを送出するには、`mavlink_msg_vfr_hud.h`の`mavlink_msg_vfr_hud_pack`関数でパケットをつくれば良い模様。

```c:VFR_HUDを送出する関数
void send_vfr_hud() {
  float airspeed = 0.0;
  float groundspeed = 0.0;
  int16_t heading = 184;
  uint16_t throttle = 0;
  float alt = 21.58;
  float climb = -0.0;

  mavlink_message_t msg;
  uint8_t buf[MAVLINK_MAX_PACKET_LEN];
  uint16_t len;
  int send_bytes = 0;

  mavlink_msg_vfr_hud_pack(system_id, component_id, &msg, airspeed, groundspeed, heading, throttle, alt, climb);
  len = mavlink_msg_to_send_buffer(buf, &msg);
  send_bytes = mavSerial.write(buf, len);
  Serial.print("mavlink_msg_vfr_hud_pack send_bytes: ");
  Serial.println(send_bytes, DEC);
}
```

# コンパニオンコンピュータ（Python）のコード

```python:get_altitude.py
import time
from pymavlink import mavutil

the_connection = mavutil.mavlink_connection('path/to/serial', baud=9600)

while True:
    print("waiting heartbeat...")
    the_connection.wait_heartbeat()
    print("Heartbeat from system (system %u component %u)" % (the_connection.target_system, the_connection.target_system))

    param_name = 'VFR_HUD'
    the_connection.param_fetch_one(param_name)
    msg = the_connection.recv_match(type=param_name, blocking=True, timeout=3)
    if msg:
        print(msg)
    else:
        print("no response")

    time.sleep(2)
```

起動:

```shell-session
$ pipenv install pyserial pymavlink
$ pipenv run python get_altitude.py
```

以下のような出力が繰り返し出てくれば成功。

```shell-session
waiting heartbeat...
Heartbeat from system (system 20 component 20)
send
VFR_HUD {airspeed : 0.0, groundspeed : 0.0, heading : 184, throttle : 0, alt : 21.579999923706055, climb : -0.0}
```