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

# 普通の状態でインストールして実行

ArduPilotのシミュレーション（SITL + Gazebo）インストールは以下の通りです。VM上でも良いので、Ubuntuを強く推奨。
Ubuntuならば、はまるところはありません。

https://ardupilot.org/dev/docs/using-gazebo-simulator-with-sitl.html

一応、Mac でもGazeboさえ入れれば動作可能ですが、Gazeboがとても重く、実用に耐えませんでした。
（ちゃんと調べていので、原因が分かる方は教えて頂きたいです）
以下の方法でMacにもGazeboは入れられます。

http://gazebosim.org/tutorials?tut=install_on_mac&cat=install

あとは、 https://qiita.com/ikeyasu/items/9d244bcd0dd9b776ff84 とほぼ同じ手順です。

QGroundControllerを入れて、以下のコマンドを実行します。

https://ardupilot.org/dev/docs/using-gazebo-simulator-with-sitl.html

Gazeboを起動し

```sh
cd ardupilot_gazebo
gazebo --verbose worlds/iris_arducopter_runway.world
```

別のターミナルで ardupilotを実行します。

```sh
cd ardupilot/ArduCopter
../Tools/autotest/sim_vehicle.py -f gazebo-iris --console --map
```

そして、QGroundController でMission Startすれば飛びます。

# Hexaへの対応

さて、[FlightGearのときと同じように](https://qiita.com/ikeyasu/items/7ec00ee0b0881490a06c) Hexaへ対応してみます。

今回はソースコードの変更はほとんど必要無く、モデルとパラメータの変更だけですみます。
以下変更したコミットです。

https://github.com/ikeyasu/ardupilot_gazebo/commit/38376ec5ade3ce0698f2060bfb0f76617b01056e
https://github.com/ikeyasu/ardupilot/commit/5183ebf8bc0c7ffb6adf26218be0901bdb5ced0f

修正後の動画は以下のような感じです。

https://youtu.be/s_lIOHutUFw

## ardupilot_gazeboの修正

https://github.com/ikeyasu/ardupilot_gazebo/commit/38376ec5ade3ce0698f2060bfb0f76617b01056e
の説明をします

### models/iris_with_ardupilot/model.sdf

models/iris_with_ardupilot/model.sdf に以下のような記述があります。これは、それぞれプロペラ（ローター）の効力・揚力の設定です。ブレードが2つ組み合わさって、1つのプロペラになっています。

```
    <plugin name="rotor_4_blade_1" filename="libLiftDragPlugin.so">
      <a0>0.3</a0>
      <alpha_stall>1.4</alpha_stall>
      <cla>4.2500</cla>
      <cda>0.10</cda>
      <cma>0.00</cma>
      <cla_stall>-0.025</cla_stall>
      <cda_stall>0.0</cda_stall>
      <cma_stall>0.0</cma_stall>
      <area>0.002</area>
      <air_density>1.2041</air_density>
      <cp>0.084 0 0</cp>
      <forward>0 1 0</forward>
      <upward>0 0 1</upward>
      <link_name>iris::rotor_4</link_name>
    </plugin>
    <plugin name="rotor_4_blade_2" filename="libLiftDragPlugin.so">
      <a0>0.3</a0>
      <alpha_stall>1.4</alpha_stall>
      <cla>4.2500</cla>
      <cda>0.10</cda>
      <cma>0.00</cma>
      <cla_stall>-0.025</cla_stall>
      <cda_stall>0.0</cda_stall>
      <cma_stall>0.0</cma_stall>
      <area>0.002</area>
      <air_density>1.2041</air_density>
      <cp>-0.084 0 0</cp>
      <forward>0 -1 0</forward>
      <upward>0 0 1</upward>
      <link_name>iris::rotor_4</link_name>
    </plugin>
```

models/iris_with_ardupilot/model.sdf にローターを2つ追加しました。
また、ローターの回転方向が、[CWかCCW](https://ja.wikipedia.org/wiki/%E6%99%82%E8%A8%88%E5%9B%9E%E3%82%8A%E3%83%BB%E5%8F%8D%E6%99%82%E8%A8%88%E5%9B%9E%E3%82%8A)かで`<forward>`のパラメーターが違います。CWの1番ローター（models/iris_with_ardupilot/model.sdf の記述は`rotor_0`）では、`rotor_0_blade_1`の`<forward>`は`0 -1 0`、`rotor_0_blade_2`の`<forward>`は`0 1 0`です。CCWの2番ローター（models/iris_with_ardupilot/model.sdf の記述は`rotor_1`）では、`rotor_1_blade_1`の`<forward>`は`0 1 0`、`rotor_1_blade_2`の`<forward>`は`0 -1 0`です。
https://ardupilot.org/copter/docs/connect-escs-and-motors.html にしたがって、CW・CCWの設定をしました。今回は、Hexacopterの”HEXA X”で試しています。

また、libLiftDragPlugin.soの各パラメーターについては、こちらに説明があります。
http://gazebosim.org/tutorials?tut=aerodynamics&cat=physics


あと、models/iris_with_ardupilot/model.sdf ではもう一箇所、`<control>`を2つ足しています。これはパラメータは特に気にせず、`channel=“3”`のをコピーしました。

```
      <control channel="4">
        <type>VELOCITY</type>
        <offset>0</offset>
        <p_gain>0.20</p_gain>
        <i_gain>0</i_gain>
        <d_gain>0</d_gain>
        <i_max>0</i_max>
        <i_min>0</i_min>
        <cmd_max>2.5</cmd_max>
        <cmd_min>-2.5</cmd_min>
        <jointName>iris::rotor_4_joint</jointName>
        <multiplier>838</multiplier>
        <controlVelocitySlowdownSim>1</controlVelocitySlowdownSim>
      </control>
```

### models/iris_with_standoffs/model.sdf

次に、models/iris_with_standoffs/model.sdf では、物理モデルと見た目の設定をします。

`<link name='rotor_0'>`のような要素は、各プロペラの設定になります。ここの`<pose>`で、プロペラの位置をHexaの位置にします。座標は適当な3Dソフトて当たりをつけてください。

```
    <link name='rotor_0'>
      <pose>0 -0.288 0.023 0 -0 0</pose>
```

以下の、`<mesh>`は、プロペラの3Dモデルです。先ほどの、CW・CCWの設定に合うように変更してください。


```
      <visual name='rotor_2_visual'>
        <pose>0 0 0 0 -0 0</pose>
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>model://iris_with_standoffs/meshes/iris_prop_cw.dae</uri>
          </mesh>
        </geometry>
        <material>
          <script>
            <name>Gazebo/Blue</name>
            <uri>__default__</uri>
          </script>
        </material>
      </visual>
```

さらに、models/iris_with_standoffs/model.sdf には、2つ、プロペラ、`rotor_4`と`rotor_5`を追加しています。


## ardupilotの修正

https://github.com/ikeyasu/ardupilot/commit/5183ebf8bc0c7ffb6adf26218be0901bdb5ced0f

Tools/autotest/default_params/gazebo-iris.parm を変えているだけです。

### Tools/autotest/default_params/gazebo-iris.parm

ここでは、`FRAME_CLASS`を2にしています。

Ardupilotには、FRAME_CLASSとFRAME_TYPEがあり以下に解説があります。

https://ardupilot.org/copter/docs/frame-type-configuration.html

今回、FRAME_TYPEはX、FRAME_CLASSはHEXAです。
それぞれの値は以下で分かります。

https://ardupilot.org/copter/docs/parameters-Copter-latest-V4.0.1-dev.html#frame-class
https://ardupilot.org/copter/docs/parameters-Copter-latest-V4.0.1-dev.html#frame-type

## ドローの形の変更

このままだと、ドローンの見た目ががirisのままです。
これは、models/iris_with_standoffs/meshes/iris.dae を修正すれば変わります。
ただし、Gazeboは、レポジトリ内のファイルを読み込まず、以下のファイルを読み込みます。
Iris.daeを変更後、以下にroot権限でコピーすれば、見た目が変わります。

/usr/share/gazebo-9/models/iris_with_standoffs/meshes/iris.dae


# hexaで飛ばしてみる

動画はこちら
https://youtu.be/s_lIOHutUFw


https://ardupilot.org/dev/docs/using-gazebo-simulator-with-sitl.html
のインストールが済んでいると、ardupilotと、ardupilot_gazebo のレポジトリの2つがある状態かと思います。
以下の通りRepositoryを足して、それぞれ、hexa_gazebo_qiitaのブランチをCheckoutすると動きます。


# おまけ：風をおこしてみる

ardupilot_gazebo の方の、worlds/iris_arducopter_runway.world に風のプラグインを追加します。

https://github.com/ikeyasu/ardupilot_gazebo/commit/64b3c4b66b651b6399cf2e83db1d402be0815f54

そして、gazebo上で、風を設定すると、ドローンの挙動が変わるのが分かります。
以下の動画では、iris_demo の `enable_wind`の設定を確認していますが、上記のコミットで`true`にしているので、通常は変更不要です。

https://youtu.be/s_lIOHutUFw?t=212