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 を参考に開発環境を準備します。

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

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 のインストールを行い起動できることを確認しておきます。

# 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で動かしてみます。

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 画面

[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が接続された状態になります。

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

動作確認

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 で説明されています。

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

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

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

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