ArduPilotのシミュレーション（SITL + FlightGear）で6発機（ヘキサコプター）に対応してみる

ArduPilotのシミュレーション（SITL + FlightGear）で6発機（ヘキサコプター）に対応してみたら、闇が深かったのでメモです。

Macで、ArduPilotのシミュレーション（SITL + FlightGear）をするには、以下を見て下さい。
https://qiita.com/ikeyasu/items/9d244bcd0dd9b776ff84

上記で、

```sh
$ cd ardupilot/Tools/autotest
$ ./sim_vehicle.py -v ArduCopter -L KSFO --map --console
```

とすると、SITLのシミュレーションが実行できますが、`./sim_vehicle.py`に`--frame hexa` を加えると、モーターが6つのドローンになります。


```sh
$ cd ardupilot/Tools/autotest
$ ./sim_vehicle.py -v ArduCopter -L KSFO --frame hexa --map --console
```

ヘキサコプターのシミュレーション自体は、上記の変更だけでできるのですが、見た目が変わりません。
本記事は、単に見た目を変えるための話になります。

最初に、ヘキサコプターというのは以下のようにモーターが6つあるものです。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/38555/717c023f-532f-07c0-fb87-6dad6a8ad943.png)
※ 画像の作者：Alexander Glinz、[画像のライセンス](https://commons.wikimedia.org/wiki/File:Hexacopter_Multicopter_DJI-S800_on-air_credit_Alexander_Glinz.jpg?uselang=ja)

FlightGear で表示すると以下のように、4つのモーター（クアッドコプター）になります。これをヘキサコプターにしてみます。

[![Image from Gyazo](https://i.gyazo.com/5ddf68e3eb269238aacd1d031c2a675f.gif)](https://gyazo.com/5ddf68e3eb269238aacd1d031c2a675f)

まずは、FlightGear の見た目だけの話なので、FlightGearで読み込んでいるモデルを変更でできるのかと思いました。
[Tools/autotest/fg_quad_view.sh](https://github.com/ArduPilot/ardupilot/blob/master/Tools/autotest/fg_quad_view.sh)を見てみると、[Tools/autotest/aircraft](https://github.com/ArduPilot/ardupilot/tree/master/Tools/autotest/aircraft)の中の、[arducopter](https://github.com/ArduPilot/ardupilot/tree/master/Tools/autotest/aircraft/arducopter)というディレクトリの中のモデルを見ているようです。

```fg_quad_view.sh
#!/bin/sh

AUTOTESTDIR=$(dirname $0)

nice fgfs \
    --native-fdm=socket,in,10,,5503,udp \
    --fdm=external \
    --aircraft=arducopter \
    --fg-aircraft="$AUTOTESTDIR/aircraft" \
    --airport=YKRY \
    --geometry=650x550 \
    --bpp=32 \
    --disable-anti-alias-hud \
    --disable-hud-3d \
    --disable-horizon-effect \
    --timeofday=noon \
    --disable-sound \
    --disable-fullscreen \
    --disable-random-objects \
    --disable-ai-models \
    --fog-disable \
    --disable-specular-highlight \
    --disable-anti-alias-hud \
    --wind=0@0 \
    $*
```

また、[こちら](http://wiki.flightgear.org/Aircraft-set.xml)のドキュメントにあるとおり、`aircraft`の名前と`-set.xml`のファイル名が読み込まれるようです。今回は、[arducopter-set.xml](https://github.com/ArduPilot/ardupilot/blob/master/Tools/autotest/aircraft/arducopter/arducopter-set.xml)になります。
とりあえず、ここにある`engines`は6つにします。

つぎに、モデル自体は、[Aircraft/arducopter/Models/arducopter.xml](https://github.com/ArduPilot/ardupilot/blob/master/Tools/autotest/aircraft/arducopter/Models/arducopter.xml)を読み込んでいるようです。
このファイルには、以下のようなものが並んでいますが、これがプロペラを回しています

```xml
  <!--Propeller Front-->
  <animation>
    <type>noshadow</type>
    <object-name>propeller2</object-name>
  </animation>
  <animation>
    <type>spin</type>
    <object-name>propeller2</object-name>
    <property>engines/engine[2]/rpm</property>
    <factor>100</factor>
    <axis>
      <x1-m>0.288</x1-m>
      <y1-m>0.000</y1-m>
      <z1-m>0.046</z1-m>
      <x2-m>0.288</x2-m>
      <y2-m>0.000</y2-m>
      <z2-m>0.012</z2-m>
    </axis>
  </animation>
```

重要な点は3つで

* `object-name` は、[AC3D](https://www.inivis.com/)という3Dモデリングソフトで作成したファイル quadcopter.ac のオブジェクト名
* `property` は、[Flight Gear のProperty Tree](http://wiki.flightgear.org/Property_tree)。使えるプロパティは[こちらから見られる](https://github.com/FlightGear/flightgear/blob/next/docs-mini/README.properties)
* `axis`は、AC3Dのプロペラの中心座標

とりあえず、[AC3D](https://www.inivis.com/)をダウンロードして、インストールします。
プロペラを適等に2つ増やします。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/38555/6165f7a2-ff68-a1f8-a44a-76bdff3eaa58.png)

そして、aruducopter.xmlをコピーして、arducopter_hexa.xmlを作成し、プロペラを2つ加えました。

https://github.com/ikeyasu/ardupilot/commit/07182959bd1ad7a81be8699b25c02ea606a4e8b7#diff-0d4b8a43953dac4edf99b162f85041bf

さて、加えた`propeller4`と`propeller5`の`property`を何にするかですが、`engines/engine[4]/rpm`では動きません。これが、今回の闇です。

`engines/engine[4]/rpm`は、`nil`になり、つかえないようです。そもそも、書き込まれていないのではという事で、SITL側を調べます。

ArduPilotのSITLのアーキテクチャは以下です。（[こちら](https://ardupilot.org/dev/docs/sitl-simulator-software-in-the-loop.html)に掲載されています）

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/38555/3cb156de-e94d-0125-4c1c-7564f247b16e.png)
※ 画像のタイトル：SITL Simulator、[画像のライセンス](https://github.com/ArduPilot/ardupilot_wiki/blob/master/LICENSE)

困ったことに、この図は微妙に実際と違いました。おそらく、古いのかと思います。JSBSimを使う場合は、正しいのですが、上記のコマンドでは、JSBSimは使いません。`sim_multicopter.py`の方と思うのですが、そんなソースは今はありません。

調べてみると、[ここからflightgear](https://github.com/ArduPilot/ardupilot/blob/8b0fc1207d1d3e34e88fd1b7ec6288ac89d62504/libraries/AP_HAL_SITL/SITL_State.cpp#L433)に送っているようです。つまり、ArduCopterのバイナリー内に、SITLの仕組みが内包されてようです。

そこでコードを見てみると、以下のように4つしか送っていません。

```SITL_State.cpp
     if (_vehicle == ArduCopter) {
         fdm.num_engines = 4;
         for (uint8_t i=0; i<4; i++) {
             fdm.rpm[i] = constrain_float((pwm_output[i]-1000), 0, 1000);
         }
     }
```

では、これを6にしよう、と思ったのですが、それは動きませんでした。
FlightGear への送信は、[こちらの構造体](https://github.com/ArduPilot/ardupilot/blob/f7b47679bb0db22e7093d85c16a5ff9c791d87bd/libraries/SITL/SIM_JSBSim.h#L90
)のバイナリを送っているようなのですが、これが4つのモーターしか対応していません。

治すには、FlightGear側も修正が必要で面倒なので、[こちらから](https://github.com/FlightGear/flightgear/blob/next/docs-mini/README.properties)つかって無さそうな`fuel-flow-gph`プロパティを使うことにしました。
[こちら](https://github.com/ikeyasu/ardupilot/commit/45482ca70c8088fa11bb8bfc09c823f7e07dba6e#diff-847e10305948bf2c2943b443852776a9)の通り、モーターの数が、6つの場合に、のこり2つは、`fuel_flow`に入れています。

```SITL_State.cpp
        fdm.num_engines = 4;
        for (uint8_t i=0; i<4; i++) {
            fdm.rpm[i] = constrain_float((pwm_output[i]-1000), 0, 1000);
        }
        if (sitl_model->get_num_motors() == 6) {
            for (uint8_t i=0; i<2; i++) {
                fdm.fuel_flow[i] = constrain_float((pwm_output[i]-1000), 0, 1000);
            }
        }
```

その他、モーターの回転数表示も修正しています。
https://github.com/ikeyasu/ardupilot/commit/45482ca70c8088fa11bb8bfc09c823f7e07dba6e

というわけで、以下のようにすれば、6発のモーターが回るようになりました。

```sh
$ cd ardupilot
$ git remote add ikeyasu https://github.com/ikeyasu/ardupilot.git
$ git fetch ikeyasu
$ git checkout -b hexa ikeyasu/hexa
$ cd Tools/autotest
$ ./sim_vehicle.py -v ArduCopter -L KSFO --frame hexa --map --console
```

以下、別のTerminalで、

```
$ cd ardupilot/Tools/autotest
$ ./fg_hexa_view.sh
```

[![Image from Gyazo](https://i.gyazo.com/b98387b2e71e6df79a2548bfe6c89b35.gif)](https://gyazo.com/b98387b2e71e6df79a2548bfe6c89b35)

あまりに不格好なので、AC3Dで好みのデザインに修正してみて下さい。