From 01e8148211e801d3e33acffd683256444f9b6436 Mon Sep 17 00:00:00 2001 From: Ildar Sadykov Date: Tue, 17 Dec 2019 14:05:11 +0300 Subject: [PATCH] Send synced time on TIMESYNC --- src/core/global_include.cpp | 2 +- src/core/timesync.cpp | 9 +++++---- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/core/global_include.cpp b/src/core/global_include.cpp index ce4cac37ec..572903a9e2 100644 --- a/src/core/global_include.cpp +++ b/src/core/global_include.cpp @@ -183,7 +183,7 @@ dl_autopilot_time_t AutopilotTime::now() void AutopilotTime::shift_time_by(std::chrono::nanoseconds offset) { std::lock_guard lock(_autopilot_system_time_offset_mutex); - _autopilot_time_offset = offset; + _autopilot_time_offset+= offset; }; dl_autopilot_time_t AutopilotTime::time_in(dl_system_time_t local_system_time_point) diff --git a/src/core/timesync.cpp b/src/core/timesync.cpp index d6b59c68d6..07b2d301e2 100644 --- a/src/core/timesync.cpp +++ b/src/core/timesync.cpp @@ -24,7 +24,7 @@ void Timesync::do_work() if (_parent.get_time().elapsed_since_s(_last_time) >= _TIMESYNC_SEND_INTERVAL_S) { if (_parent.is_connected()) { uint64_t now_ns = std::chrono::duration_cast( - _parent.get_time().system_time().time_since_epoch()) + _parent.get_autopilot_time().now().time_since_epoch()) .count(); send_timesync(0, now_ns); } @@ -34,15 +34,16 @@ void Timesync::do_work() void Timesync::process_timesync(const mavlink_message_t& message) { - mavlink_timesync_t timesync; + mavlink_timesync_t timesync{}; mavlink_msg_timesync_decode(&message, ×ync); int64_t now_ns = std::chrono::duration_cast( - _parent.get_time().system_time().time_since_epoch()) + _parent.get_autopilot_time().now().time_since_epoch()) .count(); if (timesync.tc1 == 0) { + // Send synced time to remote system send_timesync(now_ns, timesync.ts1); } else if (timesync.tc1 > 0) { // Time offset between this system and the remote system is calculated assuming RTT for @@ -63,7 +64,7 @@ void Timesync::send_timesync(uint64_t tc1, uint64_t ts1) void Timesync::set_timesync_offset(int64_t offset_ns, uint64_t start_transfer_local_time_ns) { uint64_t now_ns = std::chrono::duration_cast( - _parent.get_time().system_time().time_since_epoch()) + _parent.get_autopilot_time().now().time_since_epoch()) .count(); // Calculate the round trip time (RTT) it took the timesync packet to bounce back to us from