｜ラズパイ｜C++｜での実行高速化について

いつもお世話になっています。

前提・実現したいこと

最終目標はループ内処理の高速化です。
（現在は1ループ65[ms]です。これを、40[ms]ぐらいにまで速めたいです。達成できなくても、極力早くできれば良いと思っています。）

Raspberry-PiとNAVIO2で9軸センサ・GPS・温度・高度を取得しています。
コーディングはC++11で行っています。

ジャイロセンサは温度によってドリフトしてしまうらしいので、
温度を取得し2次処理でドリフト補正をしたいのですが、
温度（と高度）を取得するために、usleep(10000);
つまり、10ミリ秒（×2）待機しなければなりません。
（試しに、値を下げてみたところ確からしくない値を吐きました。）

1ループに占める処理のうち、温度（と高度）が一番ボトルネックになっていますが、
待機時間を変えるわけにもいきません。

そこで、処理の高速化のためにコンパイルオプションを変えて（-o0→-o2）みたりしたのですが、ループ内処理時間は変わりませんでした。

他にも何かできることはあるでしょうか。
もう腹をくくってマルチスレッドの処理をすべきでしょうか。

該当のソースコード

公式が出している例題コードを殆ど切り貼りしただけなので、
C++のコードは余り高速化検討の参考にならないかもしれませんが、
MakeFileとともに以下に示します。

（追記）
ヘッダファイルのURLを明記します。
https://github.com/emlid/Navio2/tree/master/C%2B%2B/Navio

C++
1#include "Common/MPU9250.h"
2#include "Navio2/LSM9DS1.h"
3#include <Common/MS5611.h>
4#include <Common/Ublox.h>
5#include "Common/Util.h"
6#include <unistd.h>
7#include <string>
8#include <memory>
9#include <stdio.h>
10#include <Navio2/Led_Navio2.h>
11#include <Navio+/Led_Navio.h>
12#include <time.h>
13#include <chrono>
14#include <sys/types.h>
15#include <sys/socket.h>
16#include <netinet/in.h>
17#include <arpa/inet.h>
18
19using namespace std;
20
21unsigned char getStrLength(char *Str)
22{
23	unsigned char i = 0;
24	while(Str[i] != 0x00)
25	{
26		i++;
27	}
28	return i-1;
29}
30
31//=============================================================================
32int main(int argc, char *argv[])
33{
34    double sendData[34];
35
36    #pragma region IMU
37
38    // case mpu
39	auto sensor_mpu = unique_ptr<InertialSensor>{ new MPU9250() };
40
41	// case lsm
42	auto sensor_lsm = unique_ptr <InertialSensor>{ new LSM9DS1() };
43
44	sensor_mpu->initialize();
45	sensor_lsm->initialize();
46
47	float ax_mpu, ay_mpu, az_mpu;
48	float gx_mpu, gy_mpu, gz_mpu;
49	float mx_mpu, my_mpu, mz_mpu;
50	float ax_lsm, ay_lsm, az_lsm;
51	float gx_lsm, gy_lsm, gz_lsm;
52	float mx_lsm, my_lsm, mz_lsm;
53
54    #pragma endregion IMU
55
56    #pragma region GPS
57
58    // This vector is used to store location data, decoded from ubx messages.
59	// After you decode at least one message successfully, the information is stored in vector
60	// in a way described in function decodeMessage(vector<double>& data) of class UBXParser(see ublox.h)
61	vector<double> pos_data;
62
63	// create ublox class instance
64	Ublox gps;
65
66    #pragma endregion GPS
67
68    #pragma region Barometer
69
70    MS5611 barometer;
71
72    barometer.initialize();
73
74    #pragma endregion Barometer
75
76    #pragma region LED
77
78    auto led = unique_ptr <Led>{ new Led_Navio2()};
79    led->initialize();
80
81    #pragma endregion LED
82
83    #pragma region Time
84    chrono::system_clock::time_point start, end;
85    float smpu = 0.0;
86    float old_mpu = 0.0;
87    float slsm = 0.0;
88    float old_lsm = 0.0;
89    float dt_mpu, dt_lsm;
90    start = chrono::system_clock::now();
91    #pragma endregion Time
92
93    #pragma region UDP
94
95    int sock;
96    struct sockaddr_in addr;
97    sock = socket(AF_INET, SOCK_DGRAM, 0);
98    addr.sin_family = AF_INET;
99    addr.sin_port = htons(60000);
100    addr.sin_addr.s_addr = inet_addr("192.168.24.13");
101
102    #pragma endregion
103
104
105//-------------------------------------------------------------------------
106
107    while(1) {
108        #pragma region IMU
109
110        sensor_mpu->update();
111        sensor_lsm->update();
112
113        sensor_mpu->read_accelerometer(&ax_mpu, &ay_mpu, &az_mpu);
114        sensor_lsm->read_accelerometer(&ax_lsm, &ay_lsm, &az_lsm);
115
116        sensor_mpu->read_gyroscope(&gx_mpu, &gy_mpu, &gz_mpu);
117        sensor_lsm->read_gyroscope(&gx_lsm, &gy_lsm, &gz_lsm);
118
119        sensor_mpu->read_magnetometer(&mx_mpu, &my_mpu, &mz_mpu);
120        sensor_lsm->read_magnetometer(&mx_lsm, &my_lsm, &mz_lsm);
121
122        sendData[0] = ax_mpu;
123        sendData[1] = ay_mpu;
124        sendData[2] = az_mpu;
125        sendData[3] = gx_mpu;
126        sendData[4] = gy_mpu;
127        sendData[5] = gz_mpu;
128        sendData[6] = mx_mpu;
129        sendData[7] = my_mpu;
130        sendData[8] = mz_mpu;
131        sendData[9] = ax_lsm;
132        sendData[10] = ay_lsm;
133        sendData[11] = az_lsm;
134        sendData[12] = gx_lsm;
135        sendData[13] = gy_lsm;
136        sendData[14] = gz_lsm;
137        sendData[15] = mx_lsm;
138        sendData[16] = my_lsm;
139        sendData[17] = mz_lsm;
140
141        #pragma endregion IMU
142
143        #pragma region Time
144        end = chrono::system_clock::now();
145
146        dt_mpu = chrono::duration_cast <chrono::milliseconds> (end - start).count();
147        dt_lsm = chrono::duration_cast <chrono::milliseconds> (end - start).count();
148        
149        start = chrono::system_clock::now();
150
151        sendData[18] = dt_mpu;
152        sendData[19] = dt_lsm;
153        sendData[20] = 0;//year
154        sendData[21] = 0;//month
155        sendData[22] = 0;//day
156        sendData[23] = 0;//hour
157        sendData[24] = 0;//min
158        sendData[25] = 0;//sec
159
160        #pragma endregion
161
162        #pragma region GPS
163
164        if (gps.decodeSingleMessage(Ublox::NAV_POSLLH, pos_data) == 1)
165        {
166            sendData[26] = pos_data[0];//fix
167            sendData[27] = pos_data[1];//Lon
168            sendData[28] = pos_data[2];//Lat
169            sendData[29] = pos_data[3];//height
170            sendData[30] = pos_data[4];//hMSL
171            sendData[31] = pos_data[5];//hAcc
172            sendData[32] = pos_data[6];//vAcc
173            sendData[33] = pos_data[7];//gSpeed
174
175            switch ((int)pos_data[0]) {
176            case 0x00:
177                led -> setColor(Colors::Black);
178                break;
179
180            case 0x01:
181                led -> setColor(Colors::Red);
182                break;
183
184            case 0x02:
185                led -> setColor(Colors::Yellow);
186                break;
187
188            case 0x03:
189                led -> setColor(Colors::Green);
190                break;
191
192            case 0x04:
193                led -> setColor(Colors::Blue);
194                break;
195
196            case 0x05:
197                led -> setColor(Colors::Magenta);
198                break;
199
200            default:
201                led -> setColor(Colors::White);
202                break;
203            }
204        }
205        else
206        {
207            sendData[26] = 0;//fix
208            sendData[27] = 0;//Lon
209            sendData[28] = 0;//Lat
210            sendData[29] = 0;//height
211            sendData[30] = 0;//hMSL
212            sendData[31] = 0;//hAcc
213            sendData[32] = 0;//vAcc
214            sendData[33] = 0;//gSpeed
215
216            led -> setColor(Colors::Red);
217
218        }
219
220        #pragma endregion GPS
221
222        #pragma region Barometer
223
224        barometer.refreshPressure();
225        usleep(10000); // Waiting for pressure data ready
226        // usleep(9000); // Waiting for pressure data ready
227        barometer.readPressure();
228
229        barometer.refreshTemperature();
230        usleep(10000); // Waiting for temperature data ready
231        // usleep(9000); // Waiting for temperature data ready
232        barometer.readTemperature();
233
234        barometer.calculatePressureAndTemperature();
235
236        sendData[34] = (double)barometer.getTemperature();
237        sendData[35] = (double)barometer.getPressure();
238
239        #pragma endregion Barometer
240
241        #pragma region UDP
242
243        char send_num_str[400];
244        sprintf(send_num_str
245        , "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n"
246        , sendData[0]
247        , sendData[1]
248        , sendData[2]
249        , sendData[3]
250        , sendData[4]
251        , sendData[5]
252        , sendData[6]
253        , sendData[7]
254        , sendData[8]
255        , sendData[9]
256        , sendData[10]
257        , sendData[11]
258        , sendData[12]
259        , sendData[13]
260        , sendData[14]
261        , sendData[15]
262        , sendData[16]
263        , sendData[17]
264        , sendData[18]
265        , sendData[19]
266        , sendData[20]
267        , sendData[21]
268        , sendData[22]
269        , sendData[23]
270        , sendData[24]
271        , sendData[25]
272        , sendData[26]
273        , sendData[27]
274        , sendData[28]
275        , sendData[29]
276        , sendData[30]
277        , sendData[31]
278        , sendData[32]
279        , sendData[33]
280        , sendData[34]
281        , sendData[35]);
282
283        sendto(sock, send_num_str, strlen(send_num_str), 0, (struct sockaddr *)&addr, sizeof(addr));
284
285        #pragma endregion
286    }
287    return 0;
288}
289

MakeFile
1CXX ?= g++
2CFLAGS=-c -g -o2 -ggdb -Wall
3NAVIO = ../../Navio
4INCLUDES = -I ../../Navio
5
6all:
7	$(MAKE) -C ../../Navio all
8	$(CXX) -std=c++11 $(INCLUDES) AGMGPSBaroUDPdt.cpp -L$(NAVIO) -lnavio -o AGMGPSBaroUDPdt
9
10clean:
11	rm -f AGMGPSBaroUDPdt
12

補足情報（FW/ツールのバージョンなど）

C++11
Raspberry-Pi3 Model B

行動規範の内容に同意します

回答4件

ベストアンサー

こんにちは。

そのような時の原則は、処理時間の長い並列処理してよい処理を並列処理することです。
例えば、sendtoを非同期送信にするなどですね。
もし、refreshPressure()とrefreshTemperature()を同時処理してよいようなら（期待薄ですが可能性はあります）同時に要求して、同時に受け取ることです。（温度補正は１サイクル遅れますが、40～60mSecでの温度変化の影響が微細なら無視してもよいのでは？）
他にも、sensor_mpuとsensor_lsmの各処理で時間がかかるものがあるなら、それらを並列処理化することですね。
サブスレッドを使ったりmutexで排他制御したり等が必要になるでしょうから、かなり慎重な設計が必要ですが効果も高いです。

投稿2019/05/12 07:23

Chironian

総合スコア23272

P5_USER

2019/05/12 07:46

Chironianさん回答ありがとうございます。非同期通信なのですが、例えば、メインスレッド：通信担当｜sendto() サブスレッド１：9軸・GPS（処理の軽い方）サブスレッド２：温度・高度（処理の重い方）として、おいてサブスレッドが片方待機のときは零次ホールドして、もう片方のサブスレッドが処理を終了し次第、一緒にsendto()するという認識で大丈夫でしょうか。温度の補正は、確かに40~60 [ms]程度の遅延はネグっても良さそうですね。並列処理を行うとなると、4コア４スレッドなので最大4つのスレッドが立てられそうですね。

Chironian

2019/05/12 07:52

そのようなイメージで良さそうな感じです。ただ、処理の重い／軽いは認識を間違っています。温度・高度獲得処理も間に10mSecも待ちが入っているので処理は軽いです。CPUをぶん回すような処理が「重い」処理です。そのような重い処理については複数のコアで処理すると処理時間を短縮できます。ほとんど待ち時間ばかりのような処理はコア数はあまり気にする必要はありません。どうせCPUは遊んでいますから。

P5_USER

2019/05/12 08:05

待ち時間を意識してそれを「重い」処理だと思っていました。とすると、refreshPressure()とreedPressure()の間に、 9軸の取得を指示したりしておけば、usleep();の時間も減り、タスクの処理も行えて一石二鳥となるのでしょうか。今後、カルマンフィルタなどで姿勢推定をするのですが、行列（配列）演算を多用するだろうと思っています。恐らく最もCPUを使うと思うので、そのような時に分散させれば良いのですね。

Chironian

2019/05/12 08:15

> とすると、refreshPressure()とreedPressure()の間に、 > 9軸の取得を指示したりしておけば、usleep();の時間も減り、 > タスクの処理も行えて一石二鳥となるのでしょうか。その通りです。 > 恐らく最もCPUを使うと思うので、そのような時に分散させれば良いのですね。概ねそのようなイメージです。軽い処理の場合でも処理がそこそこ複雑（ループや条件判断が複雑で、かつ、関数呼び出しの深いところで待ちが入るような）な時はサブスレッドを使うと開発期間短縮に有効です。

P5_USER

2019/05/12 08:42

よく理解できました。ありがとうございます。あまり関係ない話で申し訳ないのですが、先程、非同期で調べたところROSがヒットしました。 (https://docs.emlid.com/navio2/common/dev/ros/) NAVIO2がこれをサポートしていて、素人目には非同期に見えるのですがこれも非同期と言えるのでしょうか。

Chironian

2019/05/12 09:27

そこそこ英文を読まないと分からないのでノーコメントとさせて下さい。

P5_USER

2019/05/12 09:37

わかりました。色々ありがとうございました。

yumetodo

2019/05/12 09:48

やっぱりrefreshPressure()とrefreshTemperature()の並列は無理っぽいですね、見た感じ。やるなら気圧と温度を交互に取り続けるスレッドを作ってあげるべきかなと(排他処理大変そう)。通信の非同期化はC++erだとboost.asioを真っ先に浮かべてしまうけれど使えるのかな？知らんけど。

行動規範の内容に同意します

そもそもなんですけど、なんかぐぐって適当に引っかかったMS5611クラスのソースコードと合わないんですが

どこでお使いのものは手にはいりますかね？

やるとしたら温度・高度取得、通信の非同期化でしょうが、どの程度効果が出るかはわからないですね・・・。

投稿2019/05/12 07:11

yumetodo

総合スコア5850

P5_USER

2019/05/12 07:20

yumetodoさんインクルードファイルの説明が抜けておりました。失礼しました。以下のURLにあるMS5611.cppとMS5611.hがラズパイで使っているコードです。 https://github.com/emlid/Navio2/tree/master/C%2B%2B/Navio/Common

行動規範の内容に同意します

温度と高度はそれほど時間変動がないと仮定すると、取得頻度をループ毎から数十ループ毎に１回に減らすことができそうです。すると待ち時間も10ミリ秒→サブミリ秒に抑えることができ、高速化できるのではないでしょうか。

投稿2019/05/12 06:40

can110

総合スコア38262

P5_USER

2019/05/12 07:21

can110さんありがとうございます。 int i = 0; if(i % 100 == 0){ センサ値取得 } で40[ms]前後まで短縮出来ました。

行動規範の内容に同意します

まずはどこで時間をくっているかを計測してみるとよいです、

google 検索して、 "gcc profile" などで検索すると計測方法がみつけることができると思います。

投稿2019/05/12 06:34

katoy

総合スコア22324

P5_USER

2019/05/12 07:26

katoyさん int i = 0; if(i == 100){ break; } i++; をwhileの中に挟み、MakeFile内に-gpを記述しましたが（-o2は消しました）、 gmon.outが生成されませんでした。参考にしたのは以下URLです。 http://nenya.cis.ibaraki.ac.jp/TIPS/gprof.html

行動規範の内容に同意します

あなたの回答