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に実装しましょう。

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を追加します。次の内容のファイルを作成しましょう。

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を追加することで、先ほど作成した自前のコードがビルド対象になります。

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