コレは何？
Linuxの標準的なスレッドをSCEHD_FIFOに設定することにより、リアルタイムスレッドとして動作するかどうか、テストを行った。 結果は良好であり、カーネルを一切いじることなく、通常の一般的なLinuxそのままで100μs周期のリアルタイム制御ができることを確認した。 これでRTAIが無くてもおｋ。(ただし、マルチコアでないとマトモに動作しない上，ソフトリアルタイムである。。。) 急いでる人は途中読み飛ばして，最後だけ読んでおｋ。
さらに改良したアップデートも実施。SCHED_FIFOによるリアルタイム制御 ～その２～ も見てネ。
前提条件
- 余計なデーモン(サービス)は可能な限り止めること。(lsmodしても何もでないくらいが良い。)
- Xwindowも切ったほうが良い。
- CPUのクロック周波数を固定したほうが良い。
- root権限を持っていること。
1. よくやる方法
1.1 コーディング
じゃ，どうやるのか？ まーフツーに考えれば，スレッドをSCEHD_FIFOに設定して優先度を最上位にし，(開始時刻＋制御周期)となる時刻までスリープすれば良いだろう。 とすると，下記のようなコードが思いつく。(重要な部分のみ抜粋，変数宣言は省略，全体のコードは後に示す)
スレッド呼び出し側
1 : // 実時間スレッドの生成と優先度の設定
2 : ThreadParam.sched_priority = sched_get_priority_max(SCHED_FIFO); // 可能な限り高いスレッド優先度を取得(0～99)
3 : pthread_create(&ThreadID, NULL, (void*(*)(void*))RealTimeThread, this); // スレッド生成
4 : pthread_setschedparam(ThreadID, SCHED_FIFO, &ThreadParam); // スレッド優先度を可能な限り高く設定
実時間スレッド側(呼び出される側)
1 :clock_gettime(CLOCK_MONOTONIC, &NextTime); // 初期開始時刻の取得
2 :
3 :// 実時間ループ
4 :while(p->StateFlag != SFID_STOP){ // 動作状態フラグが「停止」に設定されるまでループ
5 : // ここからリアルタイム空間
6 : gettimeofday(&StartTime, NULL); // 開始時刻の取得
7 : timersub(&StartTime, &StartTimePrev, &ActPeriodTime); // 実際の周期時間を計算(timeval構造体は単純に減算できないことに注意)
8 : p->PeriodicTime = ActPeriodTime.tv_usec; // 計算結果を格納
9 : StartTimePrev = StartTime; // 次回用に今回の開始時刻を格納
10 :
11 : p->pRTfunc(p->pRTarg); // 制御用関数の実行(関数ポインタにより、ここで実際の制御関数が呼ばれる)
12 : p->LoopCount++; // 制御ループカウンタをインクリメント
13 :
14 : // 次の時刻を計算
15 : NextTime.tv_nsec = NextTime.tv_nsec + PeriodTime.tv_nsec; // 開始時刻に制御周期を加算
16 : if(NextTime.tv_nsec >= ONE_SEC_IN_NANO){ // tv_nsecが1秒を超えたら，
17 : NextTime.tv_nsec -= ONE_SEC_IN_NANO; // tv_nsecから1秒分のナノ秒数を引いて，
18 : NextTime.tv_sec++; // その代わりにtv_secに1秒分追加
19 : }
20 :
21 : gettimeofday(&EndTime, NULL); // 終了時刻の取得
22 : timersub(&EndTime, &StartTime, &CompTime); // 消費時間を計算(timeval構造体は単純に減算できないことに注意)
23 : p->ComputationTime = CompTime.tv_usec; // 計算結果を格納
24 :
25 : // 次の時刻になるまで待機
26 : clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &NextTime, NULL);
27 :}
それぞれの関数の詳細説明はしない。ググるべし。 11行目でやりたい処理をやって，「今回の開始時刻＋サンプリング時間＝次の開始時刻」を計算して，clock_nanosleep()に放り込んで待機するだけ。 ものっそい単純。 timeval構造体は単純に加減算できないので注意。それ用の関数があるので使うこと。 timespec構造体も単純に加算すればいいというわけではなく，上位カウンタにあたるものがあるので，それもいじってあげる必要あり。
1.2 実 験
さて実験をしよう。 制御周期を 1ms にセットし，実際の周期を計算し，CSVファイルに吐くようにした。 実時間スレッドは２つ用意して，並列に同時に走るようにした。 その結果が以下の通り。 横軸が時刻[s]，縦軸が実際に計測した制御周期[μs]である。
う～ん。まぁ，1ms なら，この方法使えないこともないか？
(黒実線は実時間スレッド1，赤破線は実時間スレッド2)
しかし，我々の業界では 1ms など遅すぎて話にならない。100μs程度は欲しい。
制御周期は制御帯域に直結するからだ。
我々は玩具を作っているわけではない。
ということで制御周期 100μs にセットし，再実験。
全然ダメだこりゃ。 まぁ，だからRTAI等々のリアルタイムカーネルが必要なんだけどね。 でも負けない。ここで諦めるのは悔しいではないか。
2. clock_nanosleepを使わない方法
2.1 コーディング
スリープがイケナイわけだよ，どう考えてもね。 じゃあ，clock_nanosleep()を使わなければいいわけだ。 スリープせずに，独自に，勝手にタイマーをポーリングして待機すればいい。 clock_nanosleep()をwhileで以下のように置き換える。
1 : // 実時間ループ
2 : while(p->StateFlag != SFID_STOP){ // 動作状態フラグが「停止」に設定されるまでループ
3 : // ここからリアルタイム空間
4 : gettimeofday(&StartTime, NULL); // 開始時刻の取得
5 : timersub(&StartTime, &StartTimePrev, &ActPeriodTime); // 実際の周期時間を計算(timeval構造体は単純に減算できないことに注意)
6 : p->PeriodicTime = ActPeriodTime.tv_usec; // 計算結果を格納
7 : StartTimePrev = StartTime; // 次回用に今回の開始時刻を格納
8 :
9 : p->pRTfunc(p->pRTarg); // 制御用関数の実行(関数ポインタにより、ここで実際の制御関数が呼ばれる)
10 : p->LoopCount++; // 制御ループカウンタをインクリメント
11 :
12 : // 次の時刻を計算
13 : NextTime.tv_nsec = NextTime.tv_nsec + PeriodTime.tv_nsec; // 開始時刻に制御周期を加算
14 : if(NextTime.tv_nsec >= ONE_SEC_IN_NANO){ // tv_nsecが1秒を超えたら，
15 : NextTime.tv_nsec -= ONE_SEC_IN_NANO; // tv_nsecから1秒分のナノ秒数を引いて，
16 : NextTime.tv_sec++; // その代わりにtv_secに1秒分追加
17 : }
18 :
19 : gettimeofday(&EndTime, NULL); // 終了時刻の取得
20 : timersub(&EndTime, &StartTime, &CompTime); // 消費時間を計算(timeval構造体は単純に減算できないことに注意)
21 : p->ComputationTime = CompTime.tv_usec; // 計算結果を格納
22 :
23 : // 次の時刻になるまで待機
24 : // clock_nanosleep関数を使うとジッタが酷すぎてお話にならないので独自に実装
25 : // (スレッドを休ませないことが肝。ただし、“割り当てられたCPUコアにおいて” 実時間スレッド以外のタスクはほぼ実行できなくなる諸刃の剣。)
26 : while(p->StateFlag != SFID_STOP){
27 : clock_gettime(CLOCK_MONOTONIC, &TimeInWait); // 現在時刻の取得
28 : if(NextTime.tv_sec <= TimeInWait.tv_sec && NextTime.tv_nsec <= TimeInWait.tv_nsec){
29 : // 現在時刻と予め計算した次の時刻とを比較して，超えたら待機終了
30 : break;
31 : }
32 : pthread_testcancel(); // スレッドのキャンセルが送られているか念のため確認
33 : asm("nop"); // 気休めのNOP命令
34 : }
35 :
36 : // リアルタイム空間ここまで
37 : }
26～34行目がそれである。「スレッドを休ませない」ことが肝要である。 nop命令は入れたくなったので，気休めに入れておいた。無くても良い。 動作説明はコメントの通り。 マイコンぽい使い方である。というかマイコンならタイマ割り込み使えるから，むしろそれ以下か。。 この方法はおそらくマルチコアでないと，まともに動かないと思われる。
2.2 実 験
よし，それでは実験だ。条件はさっきと同じ，制御周期100μsに設定。
！？ いやいや，なんかの間違いでしょ。こんなに綺麗になるはずがない。 そうだ，無負荷だからだよきっとね。 じゃ，PCに負荷を掛けてみよう。 Linuxには負荷を掛ける便利なコマンドがある。
yes >> /dev/null &
これを打ち込んでから，再度実験。条件は同一。結果は以下の通り。
なんともないよ。 おっ，これイけるんでね？ 同じ図に見えるけど，ちゃんと違うことは確認済み。
3. そして問題発覚…
3.1 問題１
できたできた～と喜んでいたら，早速問題にぶち当たる。 しばらく実時間スレッドを動かしっぱなしにしていると，以下のメッセージが画面に出てきてしまった。
BUG: soft lockup - CPU#0 Stuck for 67s!
おそらく，スレッドからいつまでたっても返ってこないから，中の人がお怒りなのだろう。 動作自体に問題はなさそうに見える。でも気になるし邪魔だ。
3.2 問題１の解決
それならば，ちょっとだけ clock_nanosleep() を入れればよいのではないか？
(一部抜粋。全体のコードは最後の方に示す)
宣言部分
1 : struct timespec PreventStuck = {0}; // 「BUG: soft lockup - CPU#0 Stuck for 67s!」を回避するためのスリープ用
実時間ループの方のwhileの中身部分(2.1節のコードの22行目に追加)
1 : clock_nanosleep(CLOCK_MONOTONIC, 0, &PreventStuck, NULL); // 「BUG: soft lockup - CPU#0 Stuck for 67s!」を回避するためのスリープ
つまり，時間ゼロのスリープを入れるということである。 こうすることで，「BUG: soft lockup - CPU#0 Stuck for 67s!」のメッセージを回避することができた。 実験結果はまったく変わらず，リアルタイム性に影響がないことを確認した。 問題１，ソルブド。
3.3 問題２
さぁ，察しのよい諸君は，問題１があるなら問題２もあるのだろ？とお思いのことだろう。
まったくもってしてその通り。
今回考えたリアルタイムスレッドの構成方法を早速，ARCS5.0 に使ってみたところ，スレッドをすぐに停止できなくなった。
ARCS5.0 では何かキーを押すと，
制御が止まるようになっているのだが，すぐには止まらず，キーを押してしばらく時間が経過した後，止まる。
リソースが食われているのでこうなるのは当然である。
3.4 問題２の解決
では，どうすればよいか？ マルチコアなんだから，明示的にスレッドをCPUに割り当て，負荷分散すればいいだろう。 スレッド生成後に，下記のように各スレッドをCPUコアに割り当てる。
スレッド呼び出し側(1.1節のコードの4行目の下に追加)
1 : // CPUコアの割り当て
2 : cpu_set_t cpuset; // CPU設定用変数
3 : CPU_ZERO(&cpuset); // まずCPU設定用変数をクリアして，
4 : CPU_SET(CPUno,&cpuset); // 使用するCPUコア番号をCPU設定用変数にセットし，
5 : pthread_setaffinity_np(ThreadID, sizeof(cpu_set_t), &cpuset); // 実時間スレッドを指定のCPUコアに割り当てる
変数 CPUno が割り当てるCPU番号である。 実時間スレッド１つにつき，１コのCPUを明示的に割り当てる。 画面描画やキー入力待ちなどのユーザーインターフェースに関わる部分はまた別のスレッドに実装し，実時間スレッドを処理していない別のCPUに割り当てる。 この当たり前な方法によってすぐにスレッドを停止できるようになり，問題は完全に解決された。
4. 本当にモータ制御できるか確認
4.1 位置制御
本当にモータ制御に使えるの？という声が聞こえてきそうである。 自分もそう思う。 そこでまず，外乱オブザーバで加速度制御＋PD位置制御をしてみたところ，なんの問題もなく動いた。 RTAIのときと何も変わらない。
4.2 バイラテラル制御
次に，加速度制御型バイラテラル制御を実装し動作させたところ，なんの問題もなく動いた。 RTAIのときと何も変わらない。 とある国内最大級の展示会にて，今回の方法を使ったバイラテラル制御のデモンストレーションを実施。 いきなり実戦投入！ 作戦は完遂した。
5. 全体のコード
5.1 ダウンロード
全体のコードをC++のクラスにまとめたものを下記からダウンロードできるようにしておいた。 興味のある方は参照して欲しい。 実際の制御周期の計測や計算に消費した時間等々を取得する関数も用意した。 ヘッダを参照頂ければ使い方は自ずとわかると思う。
5.2 SFthreadクラスの使い方
クラスの使い方は至って簡単。 SFthreadクラスをnewして，処理したいコードが書かれた関数の関数ポインタをコンストラクタに放り込む。 その後，開始信号を送り，動かしたいだけ動かして，停止信号を送り，消去するだけ。 使い方分かりにくいよってことで，サンプルコードを用意。
サンプルコード
1 : #include <cstdio>
2 : #include <cstdlib>
3 : #include "SFthread.hh"
4 :
5 : using namespace ARCS;
6 : typedef void (*FuncPtr)(void*); // 関数ポインタ定義
7 : void PeriodicFunction(void*); // 周期実行関数 プロトタイプ宣言
8 :
9 : int main(void){
10 : // ここがすべての始まり(エントリポイント)
11 : printf("Test Code for SCHED_FIFO RealTime Thread\n");
12 :
13 : // main関数側のCPUコアの割り当て
14 : pthread_t main_func = pthread_self(); // main関数のスレッドIDを取得
15 : cpu_set_t cpuset; // CPU設定用変数
16 : CPU_ZERO(&cpuset); // まずCPU設定用変数をクリアして，
17 : CPU_SET(3,&cpuset); // 4個目のCPUコア番号をCPU設定用変数にセットし，
18 : pthread_setaffinity_np(main_func, sizeof(cpu_set_t), &cpuset); // main関数をCPUコアに割り当てる
19 :
20 : // リアルタイムスレッド側の設定 s m u n
21 : const unsigned long PeriodicTime = 1000000000; // [ns] 周期時間
22 : unsigned int CPUno = 2; // 3個目のコアをリアルタイムスレッドに割り当てる
23 : FuncPtr pPFunc = PeriodicFunction; // 周期実行関数への関数ポインタを代入
24 :
25 : SFthread* pSFthread; // リアルタイムスレッドへのポインタ
26 : pSFthread = new SFthread(PeriodicTime, pPFunc, NULL, CPUno); // スレッド生成
27 :
28 : pSFthread->SendState(SFthread::SFID_START); // リアルタイムスレッドに開始信号を送出
29 : sleep(10); // 回したい時間だけ回す
30 : pSFthread->SendState(SFthread::SFID_STOP); // リアルタイムスレッドに停止信号を送出
31 :
32 : // リアルタイムスレッドが完全に停止するまで待機
33 : while(pSFthread->ReadState() != SFthread::SFID_EXCMPL) usleep(100000);
34 : delete pSFthread; // スレッド破棄
35 :
36 : return EXIT_SUCCESS;// 終了
37 : // ここがすべての終わり
38 : }
39 :
40 : void PeriodicFunction(void*){
41 : // 周期実行関数(リアルタイムスレッド)
42 :
43 : const double Ts = 1; // [s] 周期時間
44 : static unsigned long count = 0; // ループカウンタ
45 : static double t = 0; // [s] 経過時刻
46 :
47 : t = count*Ts; // 時刻の計算
48 : count++; // ループカウンタを進める
49 :
50 : printf("Time = %lf\n",t);
51 : }
実行結果
[yuki@robotslider SchedFifoTest]$ make gcc -I. -I/usr/src/linux/include -Wall -Weffc++ -O3 -c SchedFifoTest.cc g++ -L. -lpthread -lrt -lm -ltinfo -o SchedFifoTest SFthread.o SchedFifoTest.o [yuki@robotslider SchedFifoTest]$ ./SchedFifoTest Test Code for SCHED_FIFO RealTime Thread Time = 0.000000 Time = 1.000000 Time = 2.000000 Time = 3.000000 Time = 4.000000 Time = 5.000000 Time = 6.000000 Time = 7.000000 Time = 8.000000 Time = 9.000000 Time = 10.000000 [yuki@robotslider SchedFifoTest]$
ちゃんと，うごいた。
5.3 リアルタイムマルチスレッド
リアルタイムなマルチスレッドを作りたい場合は，以下のようにすれば便利。
この場合は，関数ポインタの配列を放り込む。 ただし，コンピュータが持っているCPUコアの個数に注意。 SFmultthreadクラスは内部でSFthreadクラスを欲しいだけnew/deleteしているだけである。 動作可能なスレッドの数は「コアの最大数 - 1」がおそらく限界である。 ハイパースレッディングを使ったときにどうなるかはまだ試していない。 具体的な使用方法は ARCS5.1 の該当部分のコードを眺めて察して欲しい。
6. まとめ
近年の超高性能PCの有り余る計算機資源を活用することで，ソフトリアルタイムであるものの， RTAI等々のリアルタイムカーネルなしに，リアルタイム性を確保できた。 実際に位置制御とバイラテラル制御によりモータ制御を行い，正常に動作することを確認した。 ただし，PCに重負荷が掛かったときにどうなるかは未確認である。 また，リアルタイム性の保証は一切ないので，生死に関わるシステムには当然使うべきでないと言い訳をここに記しておく。
さらに改良したバージョン→SCHED_FIFOによるリアルタイム制御 ～その２～もあるのでそちらも参照のこと。
研究室の横の倉庫 - Side Warehouse of Laboratory
Copyright(C), Side Warehouse, All rights reserved.