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