Merge pull request #890 from irsdkv/pr-timesync

Add simple time synchronization with Autopilot

src/core/CMakeLists.txt

@@ -29,6 +29,7 @@ add_library(mavsdk
     cli_arg.cpp
     thread_pool.cpp
     geometry.cpp
+    timesync.cpp
 )
 
 target_link_libraries(mavsdk

src/core/global_include.cpp

@@ -4,10 +4,12 @@
 #include <cstdint>
 #include <limits>
 #include <cmath>
+#include <chrono>
 
 namespace mavsdk {
 
 using std::chrono::steady_clock;
+using std::chrono::system_clock;
 
 Time::Time() {}
 Time::~Time() {}
@@ -17,6 +19,11 @@ dl_time_t Time::steady_time()
     return steady_clock::now();
 }
 
+dl_system_time_t Time::system_time()
+{
+    return system_clock::now();
+}
+
 double Time::elapsed_s()
 {
     auto now = steady_time().time_since_epoch();
@@ -158,4 +165,32 @@ bool are_equal(double one, double two)
     return (std::fabs(one - two) < std::numeric_limits<double>::epsilon());
 }
 
+AutopilotTime::AutopilotTime() {}
+AutopilotTime::~AutopilotTime() {}
+
+dl_system_time_t AutopilotTime::system_time()
+{
+    return system_clock::now();
+}
+
+dl_autopilot_time_t AutopilotTime::now()
+{
+    std::lock_guard<std::mutex> lock(_autopilot_system_time_offset_mutex);
+    return dl_autopilot_time_t(std::chrono::duration_cast<std::chrono::microseconds>(
+        system_time().time_since_epoch() + _autopilot_time_offset));
+}
+
+void AutopilotTime::shift_time_by(std::chrono::nanoseconds offset)
+{
+    std::lock_guard<std::mutex> lock(_autopilot_system_time_offset_mutex);
+    _autopilot_time_offset = offset;
+};
+
+dl_autopilot_time_t AutopilotTime::time_in(dl_system_time_t local_system_time_point)
+{
+    std::lock_guard<std::mutex> lock(_autopilot_system_time_offset_mutex);
+    return dl_autopilot_time_t(std::chrono::duration_cast<std::chrono::microseconds>(
+        local_system_time_point.time_since_epoch() + _autopilot_time_offset));
+};
+
 } // namespace mavsdk

src/core/global_include.h

@@ -4,6 +4,7 @@
 
 #include <chrono>
 #include <thread>
+#include <mutex>
 
 // Instead of using the constant from math.h or cmath we define it ourselves. This way
 // we don't import all the other C math functions and make sure to use the C++ functions
@@ -25,13 +26,16 @@ constexpr float M_PI_F = float(M_PI);
 namespace mavsdk {
 
 typedef std::chrono::time_point<std::chrono::steady_clock> dl_time_t;
+typedef std::chrono::time_point<std::chrono::system_clock> dl_system_time_t;
+typedef std::chrono::time_point<std::chrono::system_clock> dl_autopilot_time_t;
 
 class Time {
 public:
     Time();
     virtual ~Time();
 
     virtual dl_time_t steady_time();
+    virtual dl_system_time_t system_time();
     double elapsed_s();
     double elapsed_since_s(const dl_time_t& since);
     dl_time_t steady_time_in_future(double duration_s);
@@ -63,6 +67,24 @@ class FakeTime : public Time {
     void add_overhead();
 };
 
+class AutopilotTime {
+public:
+    AutopilotTime();
+    virtual ~AutopilotTime();
+
+    dl_autopilot_time_t now();
+
+    void shift_time_by(std::chrono::nanoseconds offset);
+
+    dl_autopilot_time_t time_in(dl_system_time_t local_system_time_point);
+
+private:
+    mutable std::mutex _autopilot_system_time_offset_mutex{};
+    std::chrono::nanoseconds _autopilot_time_offset{};
+
+    virtual dl_system_time_t system_time();
+};
+
 double to_rad_from_deg(double deg);
 double to_deg_from_rad(double rad);
 

src/core/system_impl.cpp

@@ -21,6 +21,7 @@ SystemImpl::SystemImpl(MavsdkImpl& parent, uint8_t system_id, uint8_t comp_id, b
     _parent(parent),
     _params(*this),
     _commands(*this),
+    _timesync(*this),
     _timeout_handler(_time),
     _call_every_handler(_time)
 {
@@ -330,6 +331,7 @@ void SystemImpl::system_thread()
         _timeout_handler.run_once();
         _params.do_work();
         _commands.do_work();
+        _timesync.do_work();
 
         if (_connected) {
             // Work fairly fast if we're connected.

src/core/system_impl.h

@@ -7,6 +7,7 @@
 #include "timeout_handler.h"
 #include "call_every_handler.h"
 #include "thread_pool.h"
+#include "timesync.h"
 #include "system.h"
 #include <cstdint>
 #include <functional>
@@ -192,6 +193,7 @@ class SystemImpl {
     bool is_connected() const;
 
     Time& get_time() { return _time; };
+    AutopilotTime& get_autopilot_time() { return _autopilot_time; };
 
     void register_plugin(PluginImplBase* plugin_impl);
     void unregister_plugin(PluginImplBase* plugin_impl);
@@ -293,10 +295,13 @@ class SystemImpl {
 
     MAVLinkCommands _commands;
 
+    Timesync _timesync;
+
     TimeoutHandler _timeout_handler;
     CallEveryHandler _call_every_handler;
 
     Time _time{};
+    AutopilotTime _autopilot_time{};
 
     std::mutex _plugin_impls_mutex{};
     std::vector<PluginImplBase*> _plugin_impls{};

src/core/timesync.cpp

@@ -0,0 +1,94 @@
+#include "timesync.h"
+#include "log.h"
+#include "system_impl.h"
+
+// Partially based on: https://github.com/mavlink/mavros/blob/master/mavros/src/plugins/sys_time.cpp
+
+namespace mavsdk {
+
+Timesync::Timesync(SystemImpl& parent) : _parent(parent)
+{
+    using namespace std::placeholders; // for `_1`
+
+    _parent.register_mavlink_message_handler(
+        MAVLINK_MSG_ID_TIMESYNC, std::bind(&Timesync::process_timesync, this, _1), this);
+}
+
+Timesync::~Timesync()
+{
+    _parent.unregister_all_mavlink_message_handlers(this);
+}
+
+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<std::chrono::nanoseconds>(
+                                  _parent.get_time().system_time().time_since_epoch())
+                                  .count();
+            send_timesync(0, now_ns);
+        }
+        _last_time = _parent.get_time().steady_time();
+    }
+}
+
+void Timesync::process_timesync(const mavlink_message_t& message)
+{
+    mavlink_timesync_t timesync;
+
+    mavlink_msg_timesync_decode(&message, &timesync);
+
+    int64_t now_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
+                         _parent.get_time().system_time().time_since_epoch())
+                         .count();
+
+    if (timesync.tc1 == 0) {
+        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
+        // the timesync packet is roughly equal both ways.
+        set_timesync_offset((timesync.tc1 * 2 - (timesync.ts1 + now_ns)) / 2, timesync.ts1);
+    }
+}
+
+void Timesync::send_timesync(uint64_t tc1, uint64_t ts1)
+{
+    mavlink_message_t message;
+
+    mavlink_msg_timesync_pack(
+        _parent.get_own_system_id(), _parent.get_own_component_id(), &message, tc1, ts1);
+    _parent.send_message(message);
+}
+
+void Timesync::set_timesync_offset(int64_t offset_ns, uint64_t start_transfer_local_time_ns)
+{
+    uint64_t now_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
+                          _parent.get_time().system_time().time_since_epoch())
+                          .count();
+
+    // Calculate the round trip time (RTT) it took the timesync packet to bounce back to us from
+    // remote system
+    uint64_t rtt_ns = now_ns - start_transfer_local_time_ns;
+
+    if (rtt_ns < _MAX_RTT_SAMPLE_MS * 1000000ULL) { // Only use samples with low RTT
+
+        // Save time offset for other components to use
+        _parent.get_autopilot_time().shift_time_by(std::chrono::nanoseconds(offset_ns));
+
+        // Reset high RTT count after filter update
+        _high_rtt_count = 0;
+    } else {
+        // Increment counter if round trip time is too high for accurate timesync
+        _high_rtt_count++;
+
+        if (_high_rtt_count > _MAX_CONS_HIGH_RTT) {
+            // Issue a warning to the user if the RTT is constantly high
+            LogWarn() << "RTT too high for timesync: " << rtt_ns / 1000000.0 << " ms.";
+
+            // Reset counter
+            _high_rtt_count = 0;
+        }
+    }
+}
+
+} // namespace mavsdk

src/core/timesync.h

@@ -0,0 +1,34 @@
+#pragma once
+
+#include "global_include.h"
+#include "mavlink_include.h"
+
+namespace mavsdk {
+
+class SystemImpl;
+
+class Timesync {
+public:
+    Timesync(SystemImpl& parent);
+    ~Timesync();
+
+    void do_work();
+
+    Timesync(const Timesync&) = delete;
+    Timesync& operator=(const Timesync&) = delete;
+
+private:
+    SystemImpl& _parent;
+
+    void process_timesync(const mavlink_message_t& message);
+    void send_timesync(uint64_t tc1, uint64_t ts1);
+    void set_timesync_offset(int64_t offset_ns, uint64_t start_transfer_local_time_ns);
+
+    static constexpr double _TIMESYNC_SEND_INTERVAL_S = 5.0;
+    dl_time_t _last_time{};
+
+    static constexpr uint64_t _MAX_CONS_HIGH_RTT = 5;
+    static constexpr uint64_t _MAX_RTT_SAMPLE_MS = 10;
+    uint64_t _high_rtt_count{};
+};
+} // namespace mavsdk

src/plugins/mocap/mocap_impl.cpp

@@ -3,6 +3,7 @@
 #include "global_include.h"
 #include "px4_custom_mode.h"
 #include <array>
+#include <ctime>
 
 namespace mavsdk {
 
@@ -77,18 +78,25 @@ bool MocapImpl::send_vision_position_estimate()
     auto vision_position_estimate = _vision_position_estimate;
     _visual_position_estimate_mutex.unlock();
 
-    if (!vision_position_estimate.time_usec) {
-        vision_position_estimate.time_usec =
-            static_cast<uint64_t>(_parent->get_time().elapsed_s() * 1e3);
-    }
+    const uint64_t autopilot_time_usec =
+        (!vision_position_estimate.time_usec) ?
+            std::chrono::duration_cast<std::chrono::microseconds>(
+                _parent->get_autopilot_time().now().time_since_epoch())
+                .count() :
+            std::chrono::duration_cast<std::chrono::microseconds>(
+                _parent->get_autopilot_time()
+                    .time_in(dl_system_time_t(
+                        std::chrono::microseconds(vision_position_estimate.time_usec)))
+                    .time_since_epoch())
+                .count();
 
     mavlink_message_t message;
 
     mavlink_msg_vision_position_estimate_pack(
         _parent->get_own_system_id(),
         _parent->get_own_component_id(),
         &message,
-        vision_position_estimate.time_usec,
+        autopilot_time_usec,
         vision_position_estimate.position_body.x_m,
         vision_position_estimate.position_body.y_m,
         vision_position_estimate.position_body.z_m,
@@ -107,10 +115,18 @@ bool MocapImpl::send_attitude_position_mocap()
     auto attitude_position_mocap = _attitude_position_mocap;
     _attitude_position_mocap_mutex.unlock();
 
-    if (!attitude_position_mocap.time_usec) {
-        attitude_position_mocap.time_usec =
-            static_cast<uint64_t>(_parent->get_time().elapsed_s() * 1e3);
-    }
+    const uint64_t autopilot_time_usec =
+        (!attitude_position_mocap.time_usec) ?
+            std::chrono::duration_cast<std::chrono::microseconds>(
+                _parent->get_autopilot_time().now().time_since_epoch())
+                .count() :
+            std::chrono::duration_cast<std::chrono::microseconds>(
+                _parent->get_autopilot_time()
+                    .time_in(dl_system_time_t(
+                        std::chrono::microseconds(attitude_position_mocap.time_usec)))
+                    .time_since_epoch())
+                .count();
+
     mavlink_message_t message;
 
     std::array<float, 4> q{};
@@ -123,7 +139,7 @@ bool MocapImpl::send_attitude_position_mocap()
         _parent->get_own_system_id(),
         _parent->get_own_component_id(),
         &message,
-        attitude_position_mocap.time_usec,
+        autopilot_time_usec,
         q.data(),
         attitude_position_mocap.position_body.x_m,
         attitude_position_mocap.position_body.y_m,
@@ -139,9 +155,17 @@ bool MocapImpl::send_odometry()
     auto odometry = _odometry;
     _odometry_mutex.unlock();
 
-    if (!odometry.time_usec) {
-        odometry.time_usec = static_cast<uint64_t>(_parent->get_time().elapsed_s() * 1e3);
-    }
+    const uint64_t autopilot_time_usec =
+        (!odometry.time_usec) ?
+            std::chrono::duration_cast<std::chrono::microseconds>(
+                _parent->get_autopilot_time().now().time_since_epoch())
+                .count() :
+            std::chrono::duration_cast<std::chrono::microseconds>(
+                _parent->get_autopilot_time()
+                    .time_in(dl_system_time_t(std::chrono::microseconds(odometry.time_usec)))
+                    .time_since_epoch())
+                .count();
+
     mavlink_message_t message;
 
     std::array<float, 4> q{};
@@ -154,7 +178,7 @@ bool MocapImpl::send_odometry()
         _parent->get_own_system_id(),
         _parent->get_own_component_id(),
         &message,
-        odometry.time_usec,
+        autopilot_time_usec,
         static_cast<uint8_t>(odometry.frame_id),
         static_cast<uint8_t>(MAV_FRAME_BODY_FRD),
         odometry.position_body.x_m,