Posted at

ArduPilot with Linux その3

More than 3 years have passed since last update.


夏休み

いかがお過ごしでしょうか。コミケ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環境であれば、こうしたノウハウを活かせるのではないでしょうか。

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