tami/MYNT-EYE-S-SDK
6
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fix imu track period

Browse Source
This commit is contained in:
John Zhao 2018-05-16 16:37:12 +08:00
parent 3a49eec9c0
commit 25d3526086
1 changed files with 14 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
src/internal/channels.cc
View File

@ -286,22 +286,32 @@ void Channels::StartImuTracking(imu_callback_t callback) {
imu_sn_ = 0;
ImuReqPacket req_packet{imu_sn_};
ImuResPacket res_packet;
auto sleep_ms = [](std::intmax_t n) {
std::this_thread::sleep_for(std::chrono::milliseconds(n));
auto sleep = [](const times::system_clock::time_point &time_beg) {
auto &&time_elapsed_ms =
times::count<times::milliseconds>(times::now() - time_beg);
if (time_elapsed_ms < IMU_TRACK_PERIOD) {
std::this_thread::sleep_for(
std::chrono::milliseconds(IMU_TRACK_PERIOD - time_elapsed_ms));
VLOG(2) << "Imu track cost " << time_elapsed_ms << " ms"
<< ", sleep " << (IMU_TRACK_PERIOD - time_elapsed_ms) << " ms";
}
};
while (!imu_track_stop_) {
auto &&time_beg = times::now();
req_packet.serial_number = imu_sn_;
if (!XuImuWrite(req_packet)) {
sleep(time_beg);
continue;
}
if (!XuImuRead(&res_packet)) {
sleep(time_beg);
continue;
}
if (res_packet.packets.size() == 0) {
sleep(time_beg);
continue;
}
@ -317,6 +327,7 @@ void Channels::StartImuTracking(imu_callback_t callback) {
auto &&sn = res_packet.packets.back().serial_number;
if (imu_sn_ == sn) {
VLOG(2) << "New imu not ready, dropped";
sleep(time_beg);
continue;
}
imu_sn_ = sn;
@ -329,13 +340,7 @@ void Channels::StartImuTracking(imu_callback_t callback) {
res_packet.packets.clear();
auto &&time_elapsed_ms =
times::count<times::milliseconds>(times::now() - time_beg);
if (time_elapsed_ms < IMU_TRACK_PERIOD) {
sleep_ms(IMU_TRACK_PERIOD - time_elapsed_ms);
VLOG(2) << "Imu track cost " << time_elapsed_ms << " ms"
<< ", sleep " << (IMU_TRACK_PERIOD - time_elapsed_ms) << " ms";
}
sleep(time_beg);
}
});
}