背景

とあるドローン（ドローン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にメッセージの一覧が載っている
標準語を定めてるのはBLEのGATTに近い考え方かもしれない
いまならESP8266じゃなくてESP32じゃない？って思ったけど、手元にESPr DeveloperがあったのでESP8266にした。（ESP32だとRAMが500KBくらいあってすごいので、これから買う人はESP32がおすすめ）

ハードウェア

スイッチサイエンスのESPr Developer
スイッチサイエンスのFTDI USBシリアル変換アダプター Rev.2
ESPr Developer の4をUSBシリアル変換アダプターのTXにつなぐ
ESPr Developer の5をUSBシリアル変換アダプターのRXにつなぐ
ESPr Developer側のUSBシリアルでスケッチの書込とデバッグ出力の確認を行う
USBシリアル変換アダプター経由でコンパニオンコンピュータ側のコード（pymavlink）と通信を行う

↑たった今からこいつは自分のことをクアッドローター機だと思っているかわいそうなESP8266です

プロトコル

最初、Read Single Parameterに示されている手順

を用いるのかと思ってた。
これによると、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メッセージを送出すればよかった。（実際のやりかたは後述）

ダミードローン（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パケットを読み込める。

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関数でパケットをつくれば良い模様。

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）のコード

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)

起動:

$ pipenv install pyserial pymavlink
$ pipenv run python get_altitude.py

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

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}

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

背景

余談