LoginSignup
0
0

More than 5 years have passed since last update.

@akachochin

ArduPilot with Linux その3

Posted at

夏休み

いかがお過ごしでしょうか。コミケ1日目にあたる本日も仕事でした。
さて、前回halの概要などを見ました。
今回は、初期化時のhal.init()を掘り下げてみたいと思います。

halによる初期化

前回書いたように、以下の実装がAP_HAL_MAIN()に含まれています。

AP_HAL_MAIN()実装より抜粋
    hal.init(argc, argv);           \
        hal.scheduler->system_initialized(); \

今回、hal.init()の実装に踏み込んでみましょう。
hal.init()は以下の実装のとおり、scheduler->init()呼び出し後、各種ハードウェアの初期化をしています。

ardupilot/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp
void HAL_Linux::init(int argc,char* const argv[]) const 
{
    int opt;

    /* 起動時に指定されたオプションの処理。長いし、本質的なところでないため、今は省略 */

    scheduler->init(NULL);
    gpio->init();
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
    i2c->begin();
    i2c1->begin();
    i2c2->begin();
#else
    i2c->begin();
#endif
    rcout->init(NULL);
    rcin->init(NULL);
    uartA->begin(115200);
    uartE->begin(115200);
    spi->init(NULL);
    analogin->init(NULL);
    utilInstance.init(argc+gopt.optind-1, &argv[gopt.optind-1]);
}

さて、ここでは、scheduler->init()の詳細に踏み込みましょう。schedulerが指す実体は、以下のschedulerInstanceです。
(え？となった人は、前回引用したHAL_LinuxのコンストラクタとHAL.hの中にあるAP_HAL::HALの定義を読んでみましょう。)

ardupilot/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp
static LinuxScheduler schedulerInstance;

LinuxSchedulerは、以下のヘッダで定義されています。publicなメソッドだけ引用します。

ardupilot/libraries/AP_HAL_Linux/Scheduler.h
class Linux::LinuxScheduler : public AP_HAL::Scheduler {

typedef void *(*pthread_startroutine_t)(void *);

public:
    LinuxScheduler();
    void     init(void* machtnichts);
    void     delay(uint16_t ms);
    uint32_t millis();
    uint32_t micros();
    uint64_t millis64();
    uint64_t micros64();
    void     delay_microseconds(uint16_t us);
    void     register_delay_callback(AP_HAL::Proc,
                uint16_t min_time_ms);

    void     register_timer_process(AP_HAL::MemberProc);
    void     register_io_process(AP_HAL::MemberProc);
    void     suspend_timer_procs();
    void     resume_timer_procs();

    bool     in_timerprocess();

    void     register_timer_failsafe(AP_HAL::Proc, uint32_t period_us);

    void     begin_atomic();
    void     end_atomic();

    bool     system_initializing();
    void     system_initialized();

    void     panic(const prog_char_t *errormsg) NORETURN;
    void     reboot(bool hold_in_bootloader);

    void     stop_clock(uint64_t time_usec);

schedulerにありがちな時間に関するメソッドだけでなく、reboot()やpanic()、atomicなんたらのような排他を連想させるものまで手広いメソッドがあることがわかります。

Scheduler::init()の実装

scheduler::init()を読みます。これはなかなか重要そうなので、詳しく見ましょう。

ardupilot/libraries/AP_HAL_Linux/Scheduler.cpp
void LinuxScheduler::init(void* machtnichts)
{
    mlockall(MCL_CURRENT|MCL_FUTURE);

    clock_gettime(CLOCK_MONOTONIC, &_sketch_start_time);

    struct sched_param param = { .sched_priority = APM_LINUX_MAIN_PRIORITY };
    sched_setscheduler(0, SCHED_FIFO, &param);

    struct {
        pthread_t *ctx;
        int rtprio;
        const char *name;
        pthread_startroutine_t start_routine;
    } *iter, table[] = {
    // スレッドに関するパラメータ。記述が長くかつ後述するため、省略。
    };

    if (geteuid() != 0) {
        printf("WARNING: running as non-root. Will not use realtime scheduling\n");
    }

    for (iter = table; iter->ctx; iter++)
        _create_realtime_thread(iter->ctx, iter->rtprio, iter->name,
                                iter->start_routine);
}

mlockall()について

LinuxScheduler
    mlockall(MCL_CURRENT|MCL_FUTURE);

mlockall()とは、この関数を呼び出したプロセスのアドレス空間内にマップされたページ全てがレジデントでかつページアウト or スワップアウトされないようにするためのlibc関数です。

そして、引数でMCL_FUTUREも指定されているので、mlockall()呼び出し以後に割り当てられるメモリに対しても同様となります。
(例えばmlockall()呼び出し後のmalloc()による動的メモリ割り当てのようなケースを想定しています。)
なお、mlock_all()の詳細は、Linuxのmanページが詳しいです。

レジデント

また、「レジデント」というのは、「ある仮想アドレスに対応する物理メモリが存在すること」です。
ユーザ空間の仮想アドレス空間は原則としてCOW(コピーオンライト)です。
COW属性のアドレス空間の場合、仮想アドレス空間割当時でなく、実際のアクセスが行われたときに物理メモリの割当が行われ、レジデントな状態となります。
もう少し具体的に書くと、実際のアクセス発生時にページフォルトが発生し、ページフォルトハンドラが物理メモリの割当をする仕組みとなっています。
この仕組みによって、「実際のアクセスが行われていないアドレス空間には物理メモリを割り当てないので、無駄に物理メモリを使わない」メリットがあります。
しかし、その反面、「実際のメモリアクセス時にページフォルトハンドラが動作することがあり、メモリアクセス時に余分な時間がかかるケースがある」とも言えます。

mlock_all()呼び出し理由について

組み込みでは、このような「たまに遅くなり、処理時間に不確実さが加わる」ことをしばしば嫌います。
PCに載っているCore i7とかでなく、Cortex-A9程度(※1)のCPUではページフォルトハンドラ実行にもそれなりのコストがありそうです(現実に計測していませんので推測です)。
設計者が把握できる起動プロセス(スレッド)数で大体のRAM消費量が設計段階で分かるのであれば、メモリの節約よりも処理時間の確実さを選択したほうが良いと判断するケースもあります。
こうした理由から、mlockall()によって、マップされたアドレス空間に対して物理メモリがレジデントになるようにしていると推定できます。

※1.このWebサイトに紹介されているフライトコントローラを見る限り、Cortex-A9レベルのCPU採用例が多いようです。

clock_gettime()

LinuxScheduler
    clock_gettime(CLOCK_MONOTONIC, &_sketch_start_time);

clock_gettime()については、やはりLinux manが詳しい。
これは、ソフト起動開始時点での時間を取得しています。開始時点での「クロック値」を取得しています。これは単調増加するだけで、例えsettimeofday()で時刻を過去に変更しても減少することはありません。

スレッド生成

最後に以下の関数を呼び出しています。名称から、スレッド生成の関数であると想定できます。

LinuxScheduler
    for (iter = table; iter->ctx; iter++)
        _create_realtime_thread(iter->ctx, iter->rtprio, iter->name,
                                iter->start_routine);

続きを読むと、やはりpthread_create()を呼び出してスレッド生成しています。

ardupilot/libraries/AP_HAL_Linux/Scheduler.cpp
void LinuxScheduler::_create_realtime_thread(pthread_t *ctx, int rtprio,
                                             const char *name,
                                             pthread_startroutine_t start_routine)
{
    struct sched_param param = { .sched_priority = rtprio };
    pthread_attr_t attr;
    int r;

    pthread_attr_init(&attr);
    /*
      we need to run as root to get realtime scheduling. Allow it to
      run as non-root for debugging purposes, plus to allow the Replay
      tool to run
     */
    if (geteuid() == 0) {
        pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
        pthread_attr_setschedpolicy(&attr, SCHED_FIFO);
        pthread_attr_setschedparam(&attr, &param);
    }
    r = pthread_create(ctx, &attr, start_routine, this);
    if (r != 0) {
        hal.console->printf("Error creating thread '%s': %s\n",
                            name, strerror(r));
        panic(PSTR("Failed to create thread"));
    }
    pthread_attr_destroy(&attr);

    if (name) {
        pthread_setname_np(*ctx, name);
    }
}

ここで見るべきは以下の箇所です。

ardupilot/libraries/AP_HAL_Linux/Scheduler.cpp
    struct sched_param param = { .sched_priority = rtprio };
    // 略
        pthread_attr_setschedpolicy(&attr, SCHED_FIFO);
        pthread_attr_setschedparam(&attr, &param);

pthread_attr_setschedpolicy()でスケジューリングポリシをSCHED_FIFOとしています。
これは、POSIXで定められたリアルタイム向けのスケジューリングポリシの1つで、ポリシは以下の3種類あります。

スケジューリングポリシ 概要
SCHED_FIFO yield()で自発的に実行権を手放すか、ブロックするまで優先度の高いスレッドが実行権を獲得し動き続ける。
SCHED_RR SCHED_FIFOと基本的に同じだが、タイムスライス(一定の時間間隔)を使いきった場合にも実行権を手放す
SCHED_OTHER "普通の" Unixのスレッド優先度。リアルタイムではない。

要するに、LinuxをリアルタイムOS的に扱おうとする工夫です。
小規模なシステムであれば、設計者がシステムの全容を把握できるので、事前にスレッド(or プロセス or タスク)の優先度をかっちりと決めることが多々あります。
なお、POSIXのリアルタイム規格の詳細はこの論文に書いてありますので、一読すると面白いですよ。いつもと違った顔を垣間見ることができます。

生成しようとする各スレッドの確認

LinuxScheduler::init()に定義されているtableから設定値を抜き出したものが以下の表になります。
以下の通り、生成するスレッドは5つです。

スレッド優先度(rtprio) スレッド名 実体(start_routine)
APM_LINUX_TIMER_PRIORITY sched-timer &Linux::LinuxScheduler::_timer_thread
APM_LINUX_UART_PRIORITY sched-uart &Linux::LinuxScheduler::_uart_thread
APM_LINUX_RCIN_PRIORITY sched-rcin &Linux::LinuxScheduler::_rcin_thread
APM_LINUX_TONEALARM_PRIORITY sched-tonealarm &Linux::LinuxScheduler::_tonealarm_thread
APM_LINUX_IO_PRIORITY sched-io Linux::LinuxScheduler::_io_thread

また、スレッド優先度は以下のとおり定義されています。なお、pthreadでは優先度の数値が大きいほど優先度は高くなります。

ardupilot/libraries/AP_HAL_Linux/Scheduler.cpp
#define APM_LINUX_TIMER_PRIORITY        15
#define APM_LINUX_UART_PRIORITY         14
#define APM_LINUX_RCIN_PRIORITY         13
#define APM_LINUX_MAIN_PRIORITY         12
#define APM_LINUX_TONEALARM_PRIORITY    11
#define APM_LINUX_IO_PRIORITY           10

やはり、タイマに絡む処理が一番優先度が高いですね。

次回

今回見たコードはおそらく100行程度ですが、この短いコードの中に多くの工夫がなされていることがわかります。
比較的小規模な組み込みLinux環境であれば、こうしたノウハウを活かせるのではないでしょうか。

次回は各スレッドの実装を見ましょう。

0
0
0

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