From f2de9c15f72ec38c137a5d5e165f5548f4e14534 Mon Sep 17 00:00:00 2001
From: Jonas Vautherin <jonas.vautherin@gmail.com>
Date: Wed, 19 Aug 2020 11:27:49 +0200
Subject: [PATCH] Disable timesync by default

---
 src/core/system.cpp      |  5 +++++
 src/core/system.h        |  5 +++++
 src/core/system_impl.cpp |  5 +++++
 src/core/system_impl.h   |  3 ++-
 src/core/timesync.cpp    | 21 ++++++++++++++-------
 src/core/timesync.h      |  4 +++-
 6 files changed, 34 insertions(+), 9 deletions(-)

diff --git a/src/core/system.cpp b/src/core/system.cpp
index 97e3af9157..40c0eff250 100644
--- a/src/core/system.cpp
+++ b/src/core/system.cpp
@@ -57,4 +57,9 @@ void System::register_component_discovered_callback(discover_callback_t callback
     return _system_impl->register_component_discovered_callback(callback);
 }
 
+void System::enable_timesync()
+{
+    _system_impl->enable_timesync();
+}
+
 } // namespace mavsdk
diff --git a/src/core/system.h b/src/core/system.h
index d485e941c5..0f45f35c28 100644
--- a/src/core/system.h
+++ b/src/core/system.h
@@ -97,6 +97,11 @@ class System {
      */
     void register_component_discovered_callback(discover_callback_t callback) const;
 
+    /**
+     * @brief Enable time synchronization using the TIMESYNC messages.
+     */
+    void enable_timesync();
+
     /**
      * @brief Copy constructor (object is not copyable).
      */
diff --git a/src/core/system_impl.cpp b/src/core/system_impl.cpp
index d94dd32140..c7563766b3 100644
--- a/src/core/system_impl.cpp
+++ b/src/core/system_impl.cpp
@@ -108,6 +108,11 @@ void SystemImpl::unregister_timeout_handler(const void* cookie)
     _parent.timeout_handler.remove(cookie);
 }
 
+void SystemImpl::enable_timesync()
+{
+    _timesync.enable();
+}
+
 void SystemImpl::process_mavlink_message(mavlink_message_t& message)
 {
     // This is a low level interface where incoming messages can be tampered
diff --git a/src/core/system_impl.h b/src/core/system_impl.h
index 363c493a20..9c015e8d1c 100644
--- a/src/core/system_impl.h
+++ b/src/core/system_impl.h
@@ -53,6 +53,8 @@ class SystemImpl : public Sender {
         MavsdkImpl& parent, uint8_t system_id, uint8_t component_id, bool connected);
     ~SystemImpl();
 
+    void enable_timesync();
+
     void process_mavlink_message(mavlink_message_t& message);
 
     typedef std::function<void(const mavlink_message_t&)> mavlink_message_handler_t;
@@ -231,7 +233,6 @@ class SystemImpl : public Sender {
     const SystemImpl& operator=(const SystemImpl&) = delete;
 
 private:
-    // Helper methods added to increase readablity
     static bool is_autopilot(uint8_t comp_id);
     static bool is_camera(uint8_t comp_id);
 
diff --git a/src/core/timesync.cpp b/src/core/timesync.cpp
index ef4bcbcd57..2865e81bc4 100644
--- a/src/core/timesync.cpp
+++ b/src/core/timesync.cpp
@@ -6,21 +6,28 @@
 
 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(SystemImpl& parent) : _parent(parent) {}
 
 Timesync::~Timesync()
 {
     _parent.unregister_all_mavlink_message_handlers(this);
 }
 
+void Timesync::enable()
+{
+    _is_enabled = true;
+    _parent.register_mavlink_message_handler(
+        MAVLINK_MSG_ID_TIMESYNC,
+        std::bind(&Timesync::process_timesync, this, std::placeholders::_1),
+        this);
+}
+
 void Timesync::do_work()
 {
+    if (!_is_enabled) {
+        return;
+    }
+
     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>(
diff --git a/src/core/timesync.h b/src/core/timesync.h
index c719b5c921..6c134babae 100644
--- a/src/core/timesync.h
+++ b/src/core/timesync.h
@@ -12,6 +12,7 @@ class Timesync {
     Timesync(SystemImpl& parent);
     ~Timesync();
 
+    void enable();
     void do_work();
 
     Timesync(const Timesync&) = delete;
@@ -30,6 +31,7 @@ class Timesync {
     static constexpr uint64_t _MAX_CONS_HIGH_RTT = 5;
     static constexpr uint64_t _MAX_RTT_SAMPLE_MS = 10;
     uint64_t _high_rtt_count{};
-    bool _autopilot_timesync_acquired = false;
+    bool _autopilot_timesync_acquired{false};
+    bool _is_enabled{false};
 };
 } // namespace mavsdk