Go to Qiita Advent Calendar 2024 Top
LoginSignup
0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 3 years have passed since last update.

@sakaeda11(榮枝 純一)in株式会社アプトポッド

Dronecode Devマニュアル抜粋&翻訳 - First Application Tutorial (Hello Sky)

Posted at

#この記事は
Dronecode本家サイトのFirst Application Tutorial (Hello Sky)ページの抜粋・翻訳です。

プログラミングの最初の一歩、Hello WorldのPX4版、といった感じの内容ですね。

First Application Tutorial (Hello Sky)

このトピックでは、最初のオンボードアプリケーションを作成して実行するための手順を説明します。その中で、PX4の開発に必要な基本概念やAPIなども一通りカバーします。

なお、単純化のため、start/stop機能やコマンドライン引数といった発展的な特徴については省略しています。これらはApplication/Module Templateで取り扱っています。

Prerequisites|前提条件

まず先に以下の準備が必要です。

Firmware/src/examples/px4_simple_appのディレクトリにあるソースコードにはこのチュートリアルの完全版が含まれているので、もし詰まったら参照してください。

  • なお、px4_simple_appのディレクトリ名は変更するか削除するかしておいてください。

Minimal Application|最小限のアプリケーション

このセクションでは、"Hello Sky!"と表示する最小限のアプリケーションを作成します。これは単一のC言語ファイルと、cmake定義ファイルで構成されます。(cmake定義ファイルはツールチェーンにどのようにアプリケーションをビルドするかを伝えます。)

1.Firmware/src/examples/px4_simple_appに新しいディレクトリを作成します
2.上記ディレクトリにpx4_simple_app.cというファイル名で以下のようなC言語ファイルを作成します。

/****************************************************************************
 *
 *   Copyright (c) 2012-2019 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

(ここはコピーライトだけですね。)

  • 上記のデフォルトのヘッダー部分に続いて以下をコピーします。これは提供されたファイル全てに存在するはずです。
/**
 * @file px4_simple_app.c
 * Minimal application example for PX4 autopilot
 *
 * @author Example User <mail@example.com>
 */

#include <px4_platform_common/log.h>

__EXPORT int px4_simple_app_main(int argc, char *argv[]);

int px4_simple_app_main(int argc, char *argv[])
{
    PX4_INFO("Hello Sky!");
    return OK;
}

note1:

このメイン関数の名前は必ず<module_name>_mainであり、上記で示されているようにexportされている必要があります。

note2:

PX4_INFO というのはPX4シェルでのprintfのようなものです。(px4_platform_common/log.hをincludeする際に読み込まれています。)ちなみに、PX4_INFO, PX4_WARN, PX4_ERR, PX4_DEBUGといった幾つかのログレベルが存在します。警告(WARN)とエラー(ERR)はULogに書き込まれるのとフライトレビューに表示されます。

4.CMakeLists.txtというファイル名のcmake定義ファイルを新規作成し、以下の内容をコピーします：

############################################################################
#
#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
#    notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
#    notice, this list of conditions and the following disclaimer in
#    the documentation and/or other materials provided with the
#    distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
#    used to endorse or promote products derived from this software
#    without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_module(
    MODULE examples__px4_simple_app
    MAIN px4_simple_app
    STACK_MAIN 2000
    SRCS
        px4_simple_app.c
    DEPENDS
    )

px4_add_module()メソッドはモジュール記述を元に静的ライブラリをビルドします。MAINブロックにはモジュールの名前が列挙され、コマンドがNuttXに登録され、PX4のシェルやSITLコンソールから呼び出せるようになります。

note1:

px4_add_module()フォーマットについての説明はFirmware/cmake/px4_add_module.cmakeにあります。

note2:

px4_add_moduleのオプションとしてDYNAMICを指定すると、POSIXプラットフォーム上で静的ライブラリの代わりに共有ライブラリが作成されます。(これらはPX4を再コンパイルせずにロードでき、ソースコードではなくバイナリとして他のユーザーと共有できます。)アプリは組み込みコマンドにはなりませんが、examples_px4_simple_app.px4modという名前の別のファイルになります。dynというコマンドを使って実行中にファイルをロードし、コマンドを使えるように出来ます。コマンド例：dyn ./examples__px4_simple_app.px4mod

Build the Application/Firmware|アプリケーション/ファームウェアのビルド

ここまででアプリケーションは完成です。これを実行するためにはまずPX4の一部としてビルトする必要があります。アプリケーションは適切なボードレベルの以下のターゲット毎のcmakeファイルによってbuild/firmwareに追加されます。

アプリケーションのファームウェアへのコンパイルを有効にするために、cmakeファイルの中にそのアプリケーション用の行を以下のように追加します。

examples/px4_simple_app

サンプルはファームウェアにデフォルトで含まれているため、この行はほとんどのファイルに既に存在しています。

以下のようなボード毎のコマンドでサンプルアプリケーションをビルドします。

  • jMAVSim Simulator: make px4_sitl_default jmavsim
  • Pixhawk v1/2: make px4_fmu-v2_default (または単に make px4_fmu-v2)
  • Pixhawk v3: make px4_fmu-v4_default
  • 他のボードは: Building the Codeのページを参照

Test App (Hardware)| ハードウェアを用いたテスト

Upload the firmware to your board|ボードへのファームウェアのアップロード

以下のコマンドでアップローダーを有効化し、ボードをリセットします。

  • Pixhawk v1/2: make px4_fmu-v2_default upload
  • Pixhawk v3: make px4_fmu-v4_default upload

このコマンドでは、ボードをリセットする前に多数のコンパイルメッセージが表示され、最後に以下が表示されます：

Loaded firmware for X,X, waiting for the bootloader...

ボードがリセットされ、アップロードされると以下のように表示されます：

Erase  : [====================] 100.0%
Program: [====================] 100.0%
Verify : [====================] 100.0%
Rebooting.

[100%] Built target upload

Connect the Console|コンソールへの接続

次に、シリアルまたはUSBでシステムコンソールに接続します。Enterキーを押すと、シェルプロンプトが表示されます。

nsh>

helpとタイプして、ENTERを押します。

nsh> help
  help usage:  help [-v] [<cmd>]

  [           df          kill        mkfifo      ps          sleep       
  ?           echo        losetup     mkrd        pwd         test        
  cat         exec        ls          mh          rm          umount      
  cd          exit        mb          mount       rmdir       unset       
  cp          free        mkdir       mv          set         usleep      
  dd          help        mkfatfs     mw          sh          xd          

Builtin Apps:
  reboot
  perf
  top
  ..
  px4_simple_app
  ..
  sercon
  serdis

px4_simple_appが利用可能コマンドの一つになっていることが分かります。
px4_simple_appとタイプしたあとENTERで以下のように実行されます。

nsh> px4_simple_app
Hello Sky!

これで、アプリケーションはシステムに正しく登録され、実際に役立つタスクを拡張できるようになりました。

Test App (SITL)|SITL(シミュレーター)でのテスト

もしSITLを利用している場合、PX4コンソールが自動的に開始します。(Building the Code > First Build(Using the jMAVSim Simulator)を参照してください。)nshコンソール(前のセクションを見てください。)の場合、helpと打つことでビルトインアプリケーションのリストを確認出来ます。

px4_simple_app と打つことでサンプルの最小アプリを実行出来ます。

pxh> px4_simple_app
INFO  [px4_simple_app] Hello Sky!

これで、アプリケーションを拡張して、実際に役立つタスクを実行できるようになりました。

Subscribing to Sensor Data| センサーデータのサブスクライブ(購読)

何か有意義な事をするためには、アプリケーションは入力のサブスクライブと出力のパブリッシュが必要になってきます。(例えばモーターやサーボのコマンドなど)

PX4ハードウェアの抽象化のメリットはここで発揮されます。センサードライバーを操作する必要はなく、ボードやセンサーが更新された場合にアプリを更新する必要もありません。

アプリケーション間の個別のメッセージチャネルはtopicと呼ばれます。
このチュートリアルでは、sensor_combinedというシステム全体の同期化されたセンサーデータを保持するトピックに注目します。

topicのサブスクリプションを行うのは以下のように簡単です：

#include <uORB/topics/sensor_combined.h>
..
int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));

sensor_sub_fd はtopic handleで、新しいデータのブロッキング待機を非常に効率的に実行するために使用できます。
現在のスレッドはスリープ状態になり、新しいデータが利用可能になるとスケジューラによって自動的に起動される、という機構で、待機中にCPUサイクルを消費しません。これを実現するためにPOSIXシステムコールのpoll() を利用しています。

poll()を追加すると以下のようになります。(これは擬似的なコードなので、実行するには下にある完全版を利用してください。)

#include <poll.h>
#include <uORB/topics/sensor_combined.h>
..
int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));

/* この手法で複数のtopicを待つことができます。ここでは1つだけ使用しています。 */
px4_pollfd_struct_t fds[] = {
    { .fd = sensor_sub_fd,   .events = POLLIN },
};

while (true) {
    /* 1つのファイル記述子のセンサーの更新を1000ms(1秒)待ちます */
    int poll_ret = px4_poll(fds, 1, 1000);
    ..
    if (fds[0].revents & POLLIN) {
        /* 最初のファイル記述子のデータを取得します */
        struct sensor_combined_s raw;
        /* センサーのrawデータをローカルバッファにコピーします。 */
        orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw);
        PX4_INFO("Accelerometer:\t%8.4f\t%8.4f\t%8.4f",
                    (double)raw.accelerometer_m_s2[0],
                    (double)raw.accelerometer_m_s2[1],
                    (double)raw.accelerometer_m_s2[2]);
    }
}

以下のコマンドでアプリケーションを再度コンパイルします。

make

Testing the uORB Subscription|uORBのサブスクライブテスト

アプリケーションをバックグラウンドのプロセスまたはタスクとして実行するための最後のステップは、nsh上で以下のようにタイプすることです。

px4_simple_app &

アプリケーションは5つのセンサーの値をコンソールに表示し終了します。

[px4_simple_app] Accelerometer:   0.0483          0.0821          0.0332
[px4_simple_app] Accelerometer:   0.0486          0.0820          0.0336
[px4_simple_app] Accelerometer:   0.0487          0.0819          0.0327
[px4_simple_app] Accelerometer:   0.0482          0.0818          0.0323
[px4_simple_app] Accelerometer:   0.0482          0.0827          0.0331
[px4_simple_app] Accelerometer:   0.0489          0.0804          0.0328

コマンドラインから制御できるバックグラウンドプロセスを記述するためにThe Module Template for Full Applicationsを利用出来ます。

Publishing Data|データのパブリッシュ(発行)

次のステップは、計算された出力を使用するために結果をパブリッシュすることです。以下ではattitude topicを発行する方法を示します。

ここで例としてattitudeを選択したのは、mavlinkアプリがattituleをGCSに送信しているので、結果を簡単に確認できるためです。

インターフェースはとてもシンプルです。以下のようにパブリッシュするtopicのstruct(構造体)を初期化し、advertiseします。

#include <uORB/topics/vehicle_attitude.h>
..
/* attitude topicのadvertise */
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
orb_advert_t att_pub_fd = orb_advertise(ORB_ID(vehicle_attitude), &att);

メインループの中で、準備ができたら情報をパブリッシュします。

orb_publish(ORB_ID(vehicle_attitude), att_pub_fd, &att);

Full Example Code|サンプルコード全体

完全なソースコードは以下のようになります。

/****************************************************************************
 *
 *   Copyright (c) 2012-2019 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file px4_simple_app.c
 * Minimal application example for PX4 autopilot
 *
 * @author Example User <mail@example.com>
 */

#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/tasks.h>
#include <px4_platform_common/posix.h>
#include <unistd.h>
#include <stdio.h>
#include <poll.h>
#include <string.h>
#include <math.h>

#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>

__EXPORT int px4_simple_app_main(int argc, char *argv[]);

int px4_simple_app_main(int argc, char *argv[])
{
    PX4_INFO("Hello Sky!");

    /* sensor_combined topicのサブスクライブ */
    int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));
    /* 更新頻度を5Hzに制限 */
    orb_set_interval(sensor_sub_fd, 200);

    /* attitude topicのadvertise */
    struct vehicle_attitude_s att;
    memset(&att, 0, sizeof(att));
    orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);

    /* この手法で複数のtopicを待つことができます。ここでは1つだけ使用しています。 */
    px4_pollfd_struct_t fds[] = {
        { .fd = sensor_sub_fd,   .events = POLLIN },
        /* 以下の様な形式で、もっとファイル記述子を追加出来ます。：
         * { .fd = other_sub_fd,   .events = POLLIN },
         */
    };

    int error_counter = 0;

    for (int i = 0; i < 5; i++) {
        /* 1つのファイル記述子のセンサーの更新を1000ms(1秒)待ちます */
        int poll_ret = px4_poll(fds, 1, 1000);

        /* pollの結果を処理します。 */
        if (poll_ret == 0) {
            /* これはプロバイダーが何もデータを出さなかったことを意味しています */
            PX4_ERR("Got no data within a second");

        } else if (poll_ret < 0) {
            /* このケースはかなり悪い状態で、緊急事態の可能性があります */
            if (error_counter < 10 || error_counter % 50 == 0) {
                /* 以下のエラーが大量表示されるのを防ぐためにcounterを利用します */
                PX4_ERR("ERROR return value from poll(): %d", poll_ret);
            }

            error_counter++;

        } else {

            if (fds[0].revents & POLLIN) {
                /* 最初のファイル記述子のデータを取得します */
                struct sensor_combined_s raw;
                /* センサーのrawデータをローカルバッファにコピーします。 */
                orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw);
                PX4_INFO("Accelerometer:\t%8.4f\t%8.4f\t%8.4f",
                     (double)raw.accelerometer_m_s2[0],
                     (double)raw.accelerometer_m_s2[1],
                     (double)raw.accelerometer_m_s2[2]);

                /* set att and publish this information for other apps
                 the following does not have any meaning, it's just an example
                */
                /* 他のアプリケーションでサブスクライブするためにattに値を設定し、パブリッシュしています。
                 　なお、以下は単なるサンプルで、特に意味のあるデータではないです。
                */
                att.q[0] = raw.accelerometer_m_s2[0];
                att.q[1] = raw.accelerometer_m_s2[1];
                att.q[2] = raw.accelerometer_m_s2[2];

                orb_publish(ORB_ID(vehicle_attitude), att_pub, &att);
            }

            /* there could be more file descriptors here, in the form like:
             * if (fds[1..n].revents & POLLIN) {}
             */
            /* 以下の例のように、ここに他のファイル記述子を記述出来ます。
             * if (fds[1..n].revents & POLLIN) {}
             */
        }
    }

    PX4_INFO("exiting");

    return 0;
}

Running the Complete Example|完成例の実行

最後に以下のコマンドでアプリケーションを実行します。

px4_simple_app

QGCを利用していれば、リアルタイムな描画でセンサー値を確認出来ます。(詳細はこちら)

Wrap-Up|まとめ

このチュートリアルでは、基礎的なPX4 autopilotアプリケーションの開発に必要なすべてをカバーしています。
uORBのメッセージやtopicの全リストはこちらで確認出来ます。各メッセージやtopicの説明は各ファイルヘッダーに説明が詳しくありますのでそちらを参照してください。

さらなる情報やトラブルシューティング、よくあるハマりどころなどはuORBで見つかるかと思います。

次のページでは、開始および停止機能を備えた完全なアプリケーションを作成するためのテンプレートについて説明します。

0
0
1

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?