PX4のシステムに自前のタスクを追加する方法 (SITL版)

Dronecodeの2大プロジェクトの一つであるPX4(PX4/Firmware)は、マルチタスク方式を採用することで機能をタスク別に分割しています。ちゃんと保守していくコードをPX4に実装するのであれば、自前のタスクを新規に定義して、そこに機能を実装するべきです。ここではそのタスク追加の手順をまとめておきます。

この記事はSITL(Software In The Loop)向けの手順です。pixhawk等のハード上で動作するファームでは少し手順が異なるので注意しましょう。

### __動作確認済み環境__
__PX4/Firmware Tag:__ 1.6.3
__OS:__ Ubuntu 16.04 LTS (64bit)

実装手順
=========
手順を列挙すると次のようになります。

1. 追加したいタスクのコードを書く
2. CMakeLists.txtを書く
3. cmakeファイルを書き換える
4. スタートアップスクリプトを書き換える

### 1. 追加したいタスクのコードを書く

今回は、タスクをPX4のmoduleとして追加することにします。従って、各種moduleのソースが入っている`Firmware/src/modules`にファイルを加えます。新規ディレクトリ`mytask`を作成し、次のコードを`Firmware/src/modules/mytask/mytask.c`に実装しましょう。

```c:mytask.c
#include <stdio.h>

int mytask_main(int argc, char *argv[]);

int mytask_main(int argc, char *argv[])
{
    printf("Hello World!\n");
    return 0;
}
```

エントリポイントの関数名には最後に`_main`をつけます。ここではもちろん`mytask_main`がエントリポイントです。

### 2. CMakeLists.txtを書く

続いて`Firmware/src/modules/mytask`に`CMakeLists.txt`を追加します。次の内容のファイルを作成しましょう。

```text:CMakeLists.txt
px4_add_module(
    MODULE modules__mytask
    MAIN mytask
    STACK_MAIN 1200
    COMPILE_FLAGS
    SRCS
        mytask.c
    DEPENDS
        platforms__common
    )
```

### 3. cmakeファイルを書き換える

今回はビルドのターゲットを`posix_sitl_default`とするので、`Firmware/cmake/configs/posix_sitl_default.cmake`を書き換えます。次のように、`set(config_module_list`以下に`modules/mytask`を追加することで、先ほど作成した自前のコードがビルド対象になります。

```cmake:posix_sitl_default.cmake
set(config_module_list
	# 中略
	modules/mytask
	)
```

### 4. スタートアップスクリプトを書き換える

タスクは明示的に起動しなければ実行されません。スタートアップスクリプトにタスクを起動するコマンドを記述して、起動時にタスクが立ち上がるようにします。
`Firmware/posix-configs/SITL/init/ekf2/iris`がSITLのデフォルトのスタートアップスクリプトとなっています。タスクは`mytask`という名前で登録されているので、最終行に`mytask`を追加します。

実行
=========
`Firmware`ディレクトリにて、以下のコマンドでビルド＆実行してみましょう。

```
make posix_sitl_default jmavsim
```

うまくいけば次のように表示されます。"Hello World!"は、最後から9行目のところに確認できます。

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

px4 starting.

INFO  [dataman] Unkown restart, data manager file 'rootfs/fs/microsd/dataman' size is 11797680 bytes
INFO  [platforms__posix__drivers__ledsim] LED::init
INFO  [platforms__posix__drivers__ledsim] LED::init
INFO  [simulator] Waiting for initial data on UDP port 14560. Please start the flight simulator to proceed..
Buildfile: /home/iwakura/Documents/px4_160/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

libEGL warning: bad surface attribute 0x3080
Init MAVLink
INFO  [simulator] Got initial simulation data, running sim..
INFO  [pwm_out_sim] MODE_8PWM
INFO  [tone_alarm] startup
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)
Hello World!
pxh> INFO  [logger] logger started (mode=all)
INFO  [logger] Start file log
INFO  [logger] Opened log file: rootfs/fs/microsd/log/2017-06-18/15_59_35.ulg
INFO  [lib__ecl] EKF aligned, (pressure height, IMU buf: 17, OBS buf: 16)
INFO  [lib__ecl] EKF GPS checks passed (WGS-84 origin set)
INFO  [lib__ecl] EKF commencing GPS fusion
INFO  [commander] home: 47.3977420, 8.5455939, 488.00
INFO  [tone_alarm] home_set
```
うまくいかない場合は、以下のコマンドを実行して必要なパッケージをインストールしてみましょう。

```
sudo apt-get update
sudo apt-get install cmake ant openjdk-8-jdk openjdk-8-jre
```