diff --git a/components/common/cpp/include/ftl/timer.hpp b/components/common/cpp/include/ftl/timer.hpp index 6af48cd1cd4b053e69586af88e90be3c4ded0c35..ecb93ecf3410fd60c4655767a2678e2034202b97 100644 --- a/components/common/cpp/include/ftl/timer.hpp +++ b/components/common/cpp/include/ftl/timer.hpp @@ -55,6 +55,8 @@ struct TimerHandle { TimerHandle &operator=(const TimerHandle &h) { const_cast<int&>(id) = h.id; return *this; } }; +int64_t get_time(); + /** * Milliseconds between calls. */ diff --git a/components/common/cpp/src/timer.cpp b/components/common/cpp/src/timer.cpp index 5f96f9045aac2cf506f28e9440f787b5d3f6a8d9..c068af25d8dec9e449a4b662de8c527c962214ae 100644 --- a/components/common/cpp/src/timer.cpp +++ b/components/common/cpp/src/timer.cpp @@ -38,7 +38,7 @@ struct TimerJob { static list<TimerJob> jobs[kTimerMAXLEVEL]; -static inline int64_t get_time() { +int64_t ftl::timer::get_time() { return time_point_cast<milliseconds>(high_resolution_clock::now()).time_since_epoch().count()+clock_adjust; } @@ -60,6 +60,15 @@ static void waitTimePoint() { msdelay = mspf - (now % mspf); } + // Still lots of time so do some idle jobs + while (msdelay >= 10 && sincelast != mspf) { + for (auto &j : jobs[kTimerIdle10]) { + j.job(now); + } + now = get_time(); + msdelay = mspf - (now % mspf); + } + // Spin loop until exact grab time //LOG(INFO) << "Spin Delay: " << (now / 40) << " = " << (40 - (now%40)); @@ -82,7 +91,7 @@ int ftl::timer::getInterval() { } void ftl::timer::setClockAdjustment(int64_t ms) { - clock_adjust = ms; + clock_adjust += ms; } const TimerHandle ftl::timer::add(timerlevel_t l, const std::function<void(int64_t ts)> &f) { diff --git a/components/rgbd-sources/src/streamer.cpp b/components/rgbd-sources/src/streamer.cpp index 7a47a207f7e9d3b3f5d0617b39818adeceed2c42..4758c58218bd33b1731b69a8f678b89f52700b80 100644 --- a/components/rgbd-sources/src/streamer.cpp +++ b/components/rgbd-sources/src/streamer.cpp @@ -172,16 +172,21 @@ void Streamer::_addClient(const string &source, int N, int rate, const ftl::UUID if (time_peer_ == ftl::UUID(0)) { time_peer_ = peer; - // Also do a time sync (but should be repeated periodically) - auto start = std::chrono::high_resolution_clock::now(); - int64_t mastertime = net_->call<int64_t>(peer, "__ping__"); - auto elapsed = std::chrono::high_resolution_clock::now() - start; - int64_t latency = std::chrono::duration_cast<std::chrono::milliseconds>(elapsed).count(); - clock_adjust_ = mastertime - (std::chrono::time_point_cast<std::chrono::milliseconds>(start).time_since_epoch().count() + (latency/2)); - LOG(INFO) << "Clock adjustment: " << clock_adjust_; - LOG(INFO) << "Latency: " << (latency / 2); - LOG(INFO) << "Local: " << std::chrono::time_point_cast<std::chrono::milliseconds>(start).time_since_epoch().count() << ", master: " << mastertime; - ftl::timer::setClockAdjustment(clock_adjust_); + ftl::timer::add(ftl::timer::kTimerIdle10, [peer,this](int id) { + // Also do a time sync (but should be repeated periodically) + auto start = std::chrono::high_resolution_clock::now(); + int64_t mastertime = net_->call<int64_t>(peer, "__ping__"); + auto elapsed = std::chrono::high_resolution_clock::now() - start; + int64_t latency = std::chrono::duration_cast<std::chrono::milliseconds>(elapsed).count(); + auto clock_adjust = mastertime - (ftl::timer::get_time() + (latency/2)); + + //if (clock_adjust > 0) { + LOG(INFO) << "Clock adjustment: " << clock_adjust; + //LOG(INFO) << "Latency: " << (latency / 2); + //LOG(INFO) << "Local: " << std::chrono::time_point_cast<std::chrono::milliseconds>(start).time_since_epoch().count() << ", master: " << mastertime; + ftl::timer::setClockAdjustment(clock_adjust); + //} + }); } }