PX4シミュレーション環境(jMAVSim + SITL)

PX4の開発方法には、以下３つの方法があります。

  * 実機体を使った開発
  * シミュレーターと実フライトコントローラを使った開発(HITL: Hardware In the Loop)
  * シミュレーターとPC環境を使った開発(SITL: Software In the Loop)

この記事では、SITL環境の構築と操作方法について解説します。

## 開発環境準備

### PX4開発環境準備
PX4 Developer Guide の [Development Environment on Ubuntu LTS / Debian Linux](https://dev.px4.io/en/setup/dev_env_linux_ubuntu.html) を参考に開発環境を準備します。

ubuntu 16.04 の場合、インストールスクリプトが用意されているのでこれを利用します。インストールスクリプトを実行すると、ビルドに必要なツールやライブラリのインストールと、```~/src/Firmware``` にPX4リポジトリの clone とが行われます。

```sh:ubuntu_sim.sh

# 開発アカウントをdialout グループに追加
# 実行後、設定を反映するために再ログインすること
sudo usermod -a -G dialout $USER


# インストールスクリプト取得
wget https://raw.githubusercontent.com/PX4/Devguide/master/build_scripts/ubuntu_sim.sh

# インストールスクリプト実行
./ubuntu_sim.sh
```

### シミュレーション環境準備
シミレーション環境では、キーボードを使ってドローンを動かすことが出来ますが、せっかくなのでPS4コントローラを使ってドローンを操作できるようにしておきます。

#### QGroundControlインストール
PS4コントローラの入力をシミュレーション環境に流すために、フライトコントロールツール[QGroundControl](http://qgroundcontrol.com/) のインストールを行い起動できることを確認しておきます。

```sh
# ubuntu用のtarを取得
https://s3-us-west-2.amazonaws.com/qgroundcontrol/latest/QGroundControl.tar.bz2

# インストールはtar展開するだけ
tar jxf QGroundControl.tar.bz2

# qgroundcontrolの起動
cd qgroundcontrol
./qgroundcontrol-start.sh
``` 

#### PS4コントローラ準備

PS4コントローラを使えるかテストするためのツールとしてjoytest-gtk をインストールします。

```
sudo apt-get install jstest-gtk
``` 

#### usbケーブルで接続する場合
パソコンとPS4コントローラをUSBケーブルでつないだ後、jstest-gtk を起動して、PS4コントローラを認識していることを確認します。

#### bluetooth で接続する場合

https://github.com/chrippa/ds4drv のREADME.md を元にblutooth元にblutooth接続環境を構築します。
環境構築後、PS4コントローラのPSボタンとSHAREボタンを同時に押して、blutooth接続を行います。成功すると、PS4コントローラのLEDが青く点灯します。PS4ボタンを10秒ほど押したままにすると、青く点灯していたLEDが消えてコントローラが切断されます。

## シミュレーター実行
PX4と連携して動かせるシミュレーターには、jMAVSim、Gazebo、AirSim などがありますが、とりあえずjMAVSimで動かしてみます。

```sh
cd src/Firmware
make posix_sitl_default jmavsim
```

起動するとコンソールに以下のようなメッセージが表示され、コマンドの入力待ち状態になるとともに、jMAVSimのシミュレーター画面が起動します。

```
______  __   __    ___ 
| ___ \ \ \ / /   /   |
| |_/ /  \ V /   / /| |
|  __/   /   \  / /_| |
| |     / /^\ \ \___  |
\_|     \/   \/     |_/

px4 starting.

INFO  [dataman] Unknown restart, data manager file 'rootfs/fs/microsd/dataman' size is 11405132 bytes
INFO  [simulator] Waiting for initial data on UDP port 14560. Please start the flight simulator to proceed..
Buildfile: ~/src/Firmware/Tools/jMAVSim/build.xml

make_dirs:

compile:

create_run_jar:

copy_res:

BUILD SUCCESSFUL
Total time: 0 seconds
Options parsed, starting Sim.
Starting GUI...
3D [dev] 1.6.0-pre12-daily-experimental daily

Init MAVLink
INFO  [simulator] Got initial simulation data, running sim..
INFO  [pwm_out_sim] MODE_16PWM
ERROR [sensors] no ADC found: /dev/adc0 (9)
INFO  [mavlink] mode: Normal, data rate: 4000000 B/s on udp port 14556 remote port 14550
INFO  [mavlink] mode: Onboard, data rate: 4000000 B/s on udp port 14557 remote port 14540
INFO  [mavlink] MAVLink only on localhost (set param MAV_BROADCAST = 1 to enable network)
pxh> INFO  [logger] logger started (mode=all)
INFO  [logger] Start file log
INFO  [logger] Opened log file: rootfs/fs/microsd/log/2018-05-15/22_22_06.ulg
INFO  [Unknown] EKF aligned, (pressure height, IMU buf: 22, OBS buf: 14)
INFO  [Unknown] EKF GPS checks passed (WGS-84 origin set)
INFO  [Unknown] EKF commencing GPS fusion

pxh> WARN  [Unknown] EKF GPS data stopped
WARN  [Unknown] EKF stopping navigation
INFO  [Unknown] EKF commencing GPS fusion
pxh> 
```
jMAVSim 画面
![jmavsim.png](https://qiita-image-store.s3.amazonaws.com/0/247417/c8233711-5f50-3180-9df0-f52769a834e0.png)

[Unknown] EKF commencing GPS fusion と表示されたら、commander takeoff コマンドで飛行開始を指示してみます。

```
pxh> commander takeoff
```

コマンド実行後、画面上のドローンが飛び上がるのですが、しばらくするとそのまま地面に着地してしまいます。この時、コンソールには、

```
pxh> commander takeoff
pxh> INFO  [commander] Takeoff detected
WARN  [commander_tests] Failsafe enabled: no datalink
INFO  [navigator] RTL: climb to 493 m (5 m above home)
INFO  [navigator] RTL: return at 493 m (5 m above home)
INFO  [navigator] RTL: descend to 493 m (5 m above home)
INFO  [navigator] RTL: loiter 5.0s
WARN  [navigator] RTL: land at home
INFO  [commander] Landing detected
INFO  [logger] closed logfile, bytes written: 7017656
```

のように表示されます。datalinkが無いためにFalesafeが働いたようです。そこで、datalinkとしてQGroundControlを設定します。

### QGroundControl設定

#### PX4とQGroundControlの接続
シミュレータが動作した状態で、QGroundControlを起動します。QGroundControlが起動したら、画面上部の一番左のボタンを押して、Application Setting画面に切り替えて、Comm Linksボタンで接続設定画面を表示、ADDボタンで新しい接続の作成画面を表示します。

接続作成画面では、

  * name:  適当な名前を設定
  * Type:  UDP
  * Automatically Connect Start: チェック
  * Listening Port: 14550

と設定してOKボタンで登録を行います。登録が完了すると以下のような画面が表示されるので、[UDP Link on Port 14550]ボタンをクリックしたと、[connect]ボタンをクリックすると、QGroundControlと、PX4が接続された状態になります。

![qground2.png](https://qiita-image-store.s3.amazonaws.com/0/247417/d5ec83bc-0bac-0f08-2bd5-76eab8b8223b.png)

#### QGroundControlにおけるPS4コントローラ設定
PS4コントローラを接続した状態で、QGroundControlを起動すると、[Vehicle Setup]画面(画面上部のボタンの左から２番め)に、[Joystick]ボタンが表示されます。ここで、PS4コントローラの各ボタン割当を設定します。joystickのボタンを押すと、対応している値が変化するので、確認しながらボタンに機能を割り当てていきます。

**※ ボタンを押しても何も反応がない場合、Active Joystickで選択しているjoystickを切り替えてください。**

今回は、以下のように設定してみました。

* Tx mode: 2
  * 左スティック: yaw(向き), throttle(上下)
  * 右スティック: roll(左右), pitch(前後)
* Enable joystick input: チェック
* Full down stick is zero throttle: チェック
* Button Actions:
    * ボタン0(☓): Arm
    * ボタン1(◯): Stabilized
    * ボタン2(□): Position
    * ボタン3(△): Disarm
 
![joystick.png](https://qiita-image-store.s3.amazonaws.com/0/247417/49fb5514-4341-8c75-a5be-6a82c60103ea.png)

### 動作確認
PS4コントローラを接続した状態で、「commander takeoff」コマンドを実行すると、シミュレーション画面上でドローンが上昇し続ける状態になります。コントローラの左右スティックを操作すると、ドローンを自在に動かすことが出来ます。

PS4コントローラのボタン1-ボタン2 の順に押すと、stabilized モード の positionモードが有効になり、画面上のドローンがスティックを動かさない限り静止し、操作が簡単になります。

「commander land」コマンドを実行すると、ドローンが着地します。

```
pxh> INFO  [commander] Landing at current position
INFO  [commander] Landing detected
INFO  [logger] closed logfile, bytes written: 14409048
```

コントローラだけで、操作することも可能です。

ドローンが着地してる状態で、左スティックを一番下にして、ボタン0を押すとarmが実行（モータon）されます。この状態で、左スティックを徐々に上げて行くと画面上のドローンが上昇していきます。**stabilizeモードがonの状態では上昇しないので、上昇しない場合は、ボタン1を１度押してstabilizeモードを解除します**。

左スティックを一番下に下げた状態で、ボタン3を押すとdisarm(モータoff)になります。

## シミレーション詳細
シミュレーション詳細については、PX4 Developer Guide の [simulator](https://dev.px4.io/en/simulation/) で説明されています。

実機との違いは、以下３点になります。

* センサーデータはシミュレータ(jMAVSim)からudpポートを介して取得
* モーターコントロール情報を同じudpポートを介してシミュレータに送信
* プログラムがPC上で動作

ドローンを操作したりドローンからデータを取得するアプリケーション開発だけであれば、実機、実フライトコントローラを使わなくても、この環境だけで、かなりのテストを行うことが可能です。

また、実機ではテストしにくい障害発生時の動作チェックも、```src/Firmware/Tools/jMAVSim/src/me/drton/jmavsim``` 以下のコードをカスタムすることである程度可能になります。