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

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

上記で、

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

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

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

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

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

※ 画像の作者：Alexander Glinz、画像のライセンス

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

まずは、FlightGear の見た目だけの話なので、FlightGearで読み込んでいるモデルを変更でできるのかと思いました。
Tools/autotest/fg_quad_view.shを見てみると、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 \
    $*

また、こちらのドキュメントにあるとおり、aircraftの名前と-set.xmlのファイル名が読み込まれるようです。今回は、arducopter-set.xmlになります。
とりあえず、ここにあるenginesは6つにします。

つぎに、モデル自体は、Aircraft/arducopter/Models/arducopter.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という3Dモデリングソフトで作成したファイル quadcopter.ac のオブジェクト名
property は、Flight Gear のProperty Tree。使えるプロパティはこちらから見られる
axisは、AC3Dのプロペラの中心座標

とりあえず、AC3Dをダウンロードして、インストールします。
プロペラを適等に2つ増やします。

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

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

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

ArduPilotのSITLのアーキテクチャは以下です。（こちらに掲載されています）

※ 画像のタイトル：SITL Simulator、画像のライセンス

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

調べてみると、ここからflightgearに送っているようです。つまり、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 への送信は、こちらの構造体のバイナリを送っているようなのですが、これが4つのモーターしか対応していません。

治すには、FlightGear側も修正が必要で面倒なので、こちらからつかって無さそうなfuel-flow-gphプロパティを使うことにしました。
こちらの通り、モーターの数が、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発のモーターが回るようになりました。

$ 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

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