1 /*
2 * Copyright (C) 2018 The Android Open Source Project
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17 #include <atomic>
18 #include <fstream>
19 #include <functional>
20 #include <iostream>
21 #include <memory>
22 #include <thread>
23
24 #include <gtest/gtest.h>
25 #include <log/log.h>
26 #include <utils/SystemClock.h>
27
28 #include "goog_gyro_direct.h"
29 #include "goog_sensor_motion.h"
30 #include "sensor_test_utils.h"
31
32 namespace android {
33 namespace camera_sensor_listener {
34
35 class GoogGyroTest : public ::testing::Test {
36 protected:
CreateGyroDirectListener(int64_t gyro_sampling_period_us,size_t event_queue_size)37 void CreateGyroDirectListener(int64_t gyro_sampling_period_us,
38 size_t event_queue_size) {
39 gyro_direct_ptr_ =
40 GoogGyroDirect::Create(gyro_sampling_period_us, event_queue_size);
41 ASSERT_NE(gyro_direct_ptr_, nullptr);
42 ASSERT_TRUE(gyro_direct_ptr_->GetSensorEnablingStatus());
43 }
44
CreateGyroPollListener(int64_t gyro_sampling_period_us,size_t event_queue_size)45 void CreateGyroPollListener(int64_t gyro_sampling_period_us,
46 size_t event_queue_size) {
47 gyro_poll_ptr_ = GoogSensorMotion::Create(
48 MotionSensorType::GYROSCOPE, gyro_sampling_period_us, event_queue_size);
49 ASSERT_NE(gyro_poll_ptr_, nullptr);
50 ASSERT_TRUE(gyro_poll_ptr_->GetSensorEnablingStatus());
51 // Set up gyro_poll_ptr_'s event callback.
52 std::function<void(const ExtendedSensorEvent& event)> enqueue_gyro_poll =
53 [&](const ExtendedSensorEvent& event) {
54 gyro_poll_timestamp_ns_.push_back(event.sensor_event.timestamp);
55 gyro_poll_azimuth_.push_back(event.sensor_event.u.vec3.x);
56 gyro_poll_pitch_.push_back(event.sensor_event.u.vec3.y);
57 gyro_poll_roll_.push_back(event.sensor_event.u.vec3.z);
58 gyro_poll_arrival_time_ns_.push_back(event.event_arrival_time_ns);
59 };
60 gyro_poll_ptr_->SetEventProcessor(enqueue_gyro_poll);
61 }
62
QueryGyroDirectEventsForDuration(int time_duration_ms,int check_period_ms)63 void QueryGyroDirectEventsForDuration(int time_duration_ms,
64 int check_period_ms) {
65 const std::chrono::milliseconds kCheckPeriod(check_period_ms);
66 int64_t check_time_duration_ns =
67 static_cast<int64_t>(time_duration_ms) * static_cast<int64_t>(1e6);
68
69 int64_t start_measure_time = android::elapsedRealtimeNano();
70 int64_t gyro_start_time = start_measure_time;
71 while (1) {
72 std::this_thread::sleep_for(kCheckPeriod);
73
74 std::vector<int64_t> gyro_timestamp_ns;
75 std::vector<float> azimuth;
76 std::vector<float> pitch;
77 std::vector<float> roll;
78 std::vector<int64_t> gyro_arrival_time_ns;
79
80 int64_t gyro_end_time = LLONG_MAX;
81 gyro_direct_ptr_->QueryGyroEventsBetweenTimestamps(
82 gyro_start_time, gyro_end_time, &gyro_timestamp_ns, &azimuth, &pitch,
83 &roll, &gyro_arrival_time_ns);
84
85 if (!gyro_timestamp_ns.empty()) {
86 EXPECT_GT(gyro_timestamp_ns.front(), gyro_start_time);
87 EXPECT_LE(gyro_timestamp_ns.back(), gyro_end_time);
88
89 gyro_direct_timestamp_ns_.insert(gyro_direct_timestamp_ns_.end(),
90 gyro_timestamp_ns.begin(),
91 gyro_timestamp_ns.end());
92 gyro_direct_azimuth_.insert(gyro_direct_azimuth_.end(), azimuth.begin(),
93 azimuth.end());
94 gyro_direct_pitch_.insert(gyro_direct_pitch_.end(), pitch.begin(),
95 pitch.end());
96 gyro_direct_roll_.insert(gyro_direct_roll_.end(), roll.begin(),
97 roll.end());
98 gyro_direct_arrival_time_ns_.insert(gyro_direct_arrival_time_ns_.end(),
99 gyro_arrival_time_ns.begin(),
100 gyro_arrival_time_ns.end());
101
102 gyro_start_time = gyro_timestamp_ns.back();
103 }
104
105 int64_t current_time = android::elapsedRealtimeNano();
106 if (current_time - start_measure_time > check_time_duration_ns) {
107 break;
108 }
109 }
110 }
111
CheckTimestampInterval(const std::vector<int64_t> & gyro_timestamp_ns,int64_t gyro_sampling_period_us)112 void CheckTimestampInterval(const std::vector<int64_t>& gyro_timestamp_ns,
113 int64_t gyro_sampling_period_us) {
114 // gyro samples' timestamp intervals need to be between
115 // [sampling_period * 0.9, sampling_period * 1.1].
116 int64_t delta_upper_ns =
117 gyro_sampling_period_us * 1000 + gyro_sampling_period_us * 100;
118 int64_t delta_lower_ns =
119 gyro_sampling_period_us * 1000 - gyro_sampling_period_us * 100;
120 size_t length = gyro_timestamp_ns.size();
121 EXPECT_GT(length, 0) << "Get zero gyro events";
122 for (int i = 1; i < length; ++i) {
123 int64_t delta = gyro_timestamp_ns[i] - gyro_timestamp_ns[i - 1];
124 EXPECT_LE(delta, delta_upper_ns)
125 << " gyro timestamp i+1 " << gyro_timestamp_ns[i]
126 << " gyro timestamp i " << gyro_timestamp_ns[i - 1] << " delta "
127 << delta;
128 EXPECT_GE(delta, delta_lower_ns)
129 << " gyro timestamp i+1 " << gyro_timestamp_ns[i]
130 << " gyro timestamp i " << gyro_timestamp_ns[i - 1] << " delta "
131 << delta;
132 }
133 }
134
CheckGyroSampleLatency(const std::vector<int64_t> & gyro_timestamp_ns,const std::vector<int64_t> & gyro_arrival_time_ns)135 void CheckGyroSampleLatency(const std::vector<int64_t>& gyro_timestamp_ns,
136 const std::vector<int64_t>& gyro_arrival_time_ns) {
137 int num_latency_10ms = 0;
138 int num_latency_15ms = 0;
139 int num_sample = gyro_timestamp_ns.size();
140 EXPECT_GT(num_sample, 0) << "Get zero gyro events";
141 int64_t sum_latency = 0;
142 for (int i = 0; i < num_sample; ++i) {
143 int64_t arrival_latency = gyro_arrival_time_ns[i] - gyro_timestamp_ns[i];
144 sum_latency += arrival_latency;
145 if (arrival_latency >= 10e6) {
146 num_latency_10ms++;
147 if (arrival_latency >= 15e6) {
148 num_latency_15ms++;
149 }
150 }
151 }
152 float average_latency = static_cast<float>(sum_latency) /
153 static_cast<float>(num_sample) / 1000000.f;
154 std::cout << "average latency " << average_latency << "ms\n";
155 float latency_rate_10ms =
156 static_cast<float>(num_latency_10ms) / static_cast<float>(num_sample);
157 float latency_rate_15ms =
158 static_cast<float>(num_latency_15ms) / static_cast<float>(num_sample);
159 EXPECT_LE(average_latency, 7.f) << "gyro average latency should be <= 7ms";
160 EXPECT_LE(latency_rate_10ms, 0.01f)
161 << "gyro latency 99 percentile should be <= 10ms";
162 EXPECT_LE(latency_rate_15ms, 0.001f)
163 << "gyro latency 99.9 percentile should be <= 15ms";
164 }
165
CheckGyroTimestampOffset()166 void CheckGyroTimestampOffset() {
167 // find the first corresponding gyro sample.
168 int poll_index = 0;
169 int direct_index = 0;
170 EXPECT_GT(gyro_poll_timestamp_ns_.size(), 0) << "Get zero gyro poll events";
171 EXPECT_GT(gyro_direct_timestamp_ns_.size(), 0)
172 << "Get zero gyro direct events";
173 while (poll_index < gyro_poll_timestamp_ns_.size() &&
174 direct_index < gyro_direct_timestamp_ns_.size()) {
175 if (llabs(gyro_poll_timestamp_ns_[poll_index] -
176 gyro_direct_timestamp_ns_[direct_index]) < 1e6 &&
177 gyro_poll_azimuth_[poll_index] == gyro_direct_azimuth_[direct_index] &&
178 gyro_poll_pitch_[poll_index] == gyro_direct_pitch_[direct_index] &&
179 gyro_poll_roll_[poll_index] == gyro_direct_roll_[direct_index]) {
180 break;
181 } else {
182 if (gyro_poll_timestamp_ns_[poll_index] <
183 gyro_direct_timestamp_ns_[direct_index]) {
184 ++poll_index;
185 } else {
186 ++direct_index;
187 }
188 }
189 }
190 int64_t max_offset = 0;
191 int num_offset = 0;
192 int num_samples = 0;
193
194 for (int i = poll_index, j = direct_index;
195 i < gyro_poll_timestamp_ns_.size() &&
196 j < gyro_direct_timestamp_ns_.size();
197 ++i, ++j) {
198 int64_t offset = gyro_direct_timestamp_ns_[j] - gyro_poll_timestamp_ns_[i];
199 if (offset != 0) {
200 ++num_offset;
201 if (llabs(offset) > max_offset) {
202 max_offset = llabs(offset);
203 }
204 }
205 ++num_samples;
206 }
207 EXPECT_LE(max_offset, 0) << "max timestamp drift " << max_offset << "ns\n";
208 EXPECT_LE(num_offset, 0) << "drifted samples " << num_offset << " out of "
209 << num_samples << " samples\n";
210 }
211
LogGyroDataToFile(const std::string & filename,const std::vector<int64_t> & gyro_timestamp_ns,const std::vector<float> & azimuth,const std::vector<float> & roll,const std::vector<float> & pitch,const std::vector<int64_t> & gyro_arrival_time_ns)212 void LogGyroDataToFile(const std::string& filename,
213 const std::vector<int64_t>& gyro_timestamp_ns,
214 const std::vector<float>& azimuth,
215 const std::vector<float>& roll,
216 const std::vector<float>& pitch,
217 const std::vector<int64_t>& gyro_arrival_time_ns) {
218 std::string output_filename;
219 bool result = GenerateLogFilename(filename, &output_filename);
220 EXPECT_EQ(result, true) << "fail to create gyro test log file";
221 std::cout << "save gyro log to " << output_filename << "\n";
222 std::ofstream ofs(output_filename);
223 for (int i = 0; i < gyro_timestamp_ns.size(); ++i) {
224 ofs << gyro_timestamp_ns[i] << " " << azimuth[i] << " " << roll[i] << " "
225 << pitch[i] << " " << gyro_arrival_time_ns[i] << "\n";
226 }
227 }
228
229 sp<GoogSensorMotion> gyro_poll_ptr_ = nullptr;
230 std::vector<int64_t> gyro_poll_timestamp_ns_;
231 std::vector<float> gyro_poll_azimuth_;
232 std::vector<float> gyro_poll_pitch_;
233 std::vector<float> gyro_poll_roll_;
234 std::vector<int64_t> gyro_poll_arrival_time_ns_;
235
236 std::unique_ptr<GoogGyroDirect> gyro_direct_ptr_ = nullptr;
237 std::vector<int64_t> gyro_direct_timestamp_ns_;
238 std::vector<float> gyro_direct_azimuth_;
239 std::vector<float> gyro_direct_pitch_;
240 std::vector<float> gyro_direct_roll_;
241 std::vector<int64_t> gyro_direct_arrival_time_ns_;
242 };
243
TEST_F(GoogGyroTest,TestDirectGyroLatency)244 TEST_F(GoogGyroTest, TestDirectGyroLatency) {
245 CreateGyroDirectListener(/*gyro_sampling_period_us=*/5000,
246 /*event_queue_size=*/60);
247 // Test duration 5 minutes, check period 5ms.
248 // Direct gyro arrival time is the poking time,
249 // to effectively test latency, check period has to be
250 // the same as sampling period.
251 QueryGyroDirectEventsForDuration(/*gyro_check_duration_ms=*/300000,
252 /*gyro_check_period_ms=*/5);
253 CheckGyroSampleLatency(gyro_direct_timestamp_ns_,
254 gyro_direct_arrival_time_ns_);
255 }
256
TEST_F(GoogGyroTest,TestGyroDirectIntervalAndDrift)257 TEST_F(GoogGyroTest, TestGyroDirectIntervalAndDrift) {
258 CreateGyroDirectListener(/*gyro_sampling_period_us=*/5000,
259 /*event_queue_size=*/60);
260 CreateGyroPollListener(/*gyro_sampling_period_us=*/5000,
261 /*event_queue_size=*/20);
262 QueryGyroDirectEventsForDuration(/*time_duration_ms=*/300000,
263 /*check_period_ms=*/33);
264 gyro_poll_ptr_->Disable();
265 CheckTimestampInterval(gyro_direct_timestamp_ns_,
266 /*gyro_sampling_period_us=*/5000);
267 CheckTimestampInterval(gyro_poll_timestamp_ns_,
268 /*gyro_sampling_period_us=*/5000);
269 CheckGyroTimestampOffset();
270
271 // Save gyro poll data to txt file.
272 LogGyroDataToFile("gyro_poll_log", gyro_poll_timestamp_ns_,
273 gyro_poll_azimuth_, gyro_poll_roll_, gyro_poll_pitch_,
274 gyro_poll_arrival_time_ns_);
275 // Save gyro direct data to txt file.
276 LogGyroDataToFile("gyro_direct_log", gyro_direct_timestamp_ns_,
277 gyro_direct_azimuth_, gyro_direct_roll_, gyro_direct_pitch_,
278 gyro_direct_arrival_time_ns_);
279 }
280
TEST_F(GoogGyroTest,TestGyroEnableDisable)281 TEST_F(GoogGyroTest, TestGyroEnableDisable) {
282 CreateGyroDirectListener(/*gyro_sampling_period_us=*/5000,
283 /*event_queue_size=*/20);
284 QueryGyroDirectEventsForDuration(/*time_duration_ms=*/300,
285 /*check_period_ms=*/33);
286 size_t num_gyro_direct_samples = gyro_direct_timestamp_ns_.size();
287 EXPECT_GT(gyro_direct_timestamp_ns_.size(), 0);
288 gyro_direct_ptr_->DisableDirectChannel();
289 EXPECT_FALSE(gyro_direct_ptr_->GetSensorEnablingStatus());
290 const std::chrono::milliseconds kWaitPeriod(/*wait_period_ms=*/300);
291 std::this_thread::sleep_for(kWaitPeriod);
292 gyro_direct_ptr_->EnableDirectChannel();
293 EXPECT_TRUE(gyro_direct_ptr_->GetSensorEnablingStatus());
294 QueryGyroDirectEventsForDuration(/*time_duration_ms=*/300,
295 /*check_period_ms=*/33);
296 EXPECT_GT(gyro_direct_timestamp_ns_.size(), num_gyro_direct_samples);
297
298 CreateGyroPollListener(/*gyro_sampling_period_us=*/5000,
299 /*event_queue_size=*/20);
300 std::this_thread::sleep_for(kWaitPeriod);
301 size_t num_gyro_poll_samples = gyro_poll_timestamp_ns_.size();
302 EXPECT_GT(num_gyro_poll_samples, 0);
303 gyro_poll_ptr_->Disable();
304 EXPECT_FALSE(gyro_poll_ptr_->GetSensorEnablingStatus());
305 std::this_thread::sleep_for(kWaitPeriod);
306 EXPECT_EQ(gyro_poll_timestamp_ns_.size(), num_gyro_poll_samples);
307 gyro_poll_ptr_->Enable();
308 EXPECT_TRUE(gyro_poll_ptr_->GetSensorEnablingStatus());
309 std::this_thread::sleep_for(kWaitPeriod);
310 EXPECT_GT(gyro_poll_timestamp_ns_.size(), num_gyro_poll_samples);
311 }
312
313 } // namespace camera_sensor_listener
314 } // namespace android
315