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

12939 ワード
mavlink Arduino Arduino テキストリンク

背景

  • とあるドローン（ドローン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）のコード

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}