From a48845f6a7fd80aa65baac921b1dd12c7a392e35 Mon Sep 17 00:00:00 2001 From: Ildar Sadykov Date: Fri, 25 Oct 2019 15:59:32 +0300 Subject: [PATCH 1/2] Added simple time synchronization with FCU --- src/core/CMakeLists.txt | 1 + src/core/global_include.cpp | 20 +++++++ src/core/global_include.h | 30 ++++++++++ src/core/system_impl.cpp | 2 + src/core/system_impl.h | 5 ++ src/core/timesync.cpp | 100 +++++++++++++++++++++++++++++++ src/core/timesync.h | 41 +++++++++++++ src/plugins/mocap/mocap_impl.cpp | 49 ++++++++++++--- 8 files changed, 240 insertions(+), 8 deletions(-) create mode 100644 src/core/timesync.cpp create mode 100644 src/core/timesync.h diff --git a/src/core/CMakeLists.txt b/src/core/CMakeLists.txt index a4272110e0..aa7a50f29b 100644 --- a/src/core/CMakeLists.txt +++ b/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 diff --git a/src/core/global_include.cpp b/src/core/global_include.cpp index 71f2d2985a..c5fcb129a3 100644 --- a/src/core/global_include.cpp +++ b/src/core/global_include.cpp @@ -8,6 +8,7 @@ namespace mavsdk { using std::chrono::steady_clock; +using std::chrono::system_clock; Time::Time() {} Time::~Time() {} @@ -17,6 +18,11 @@ dl_time_t Time::steady_time() return steady_clock::now(); } +dl_stime_t Time::system_time() +{ + return system_clock::now(); +} + double Time::elapsed_s() { auto now = steady_time().time_since_epoch(); @@ -158,4 +164,18 @@ bool are_equal(double one, double two) return (std::fabs(one - two) < std::numeric_limits::epsilon()); } +FCUTime::FCUTime() {} +FCUTime::~FCUTime() {} + +dl_stime_t FCUTime::system_time() +{ + return system_clock::now(); +} + +dl_fcu_time_t FCUTime::now() +{ + std::lock_guard lock(_fcu_system_time_offset_mutex); + return dl_fcu_time_t(system_time() + _fcu_time_offset); +} + } // namespace mavsdk diff --git a/src/core/global_include.h b/src/core/global_include.h index 271b107882..56240dfb32 100644 --- a/src/core/global_include.h +++ b/src/core/global_include.h @@ -4,6 +4,7 @@ #include #include +#include // 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,6 +26,8 @@ constexpr float M_PI_F = float(M_PI); namespace mavsdk { typedef std::chrono::time_point dl_time_t; +typedef std::chrono::time_point dl_stime_t; +typedef std::chrono::time_point dl_fcu_time_t; class Time { public: @@ -32,6 +35,7 @@ class Time { virtual ~Time(); virtual dl_time_t steady_time(); + virtual dl_stime_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,32 @@ class FakeTime : public Time { void add_overhead(); }; +class FCUTime { +public: + FCUTime(); + virtual ~FCUTime(); + + dl_fcu_time_t now(); + + template void shift_fcu_time_by(T offset) + { + std::lock_guard lock(_fcu_system_time_offset_mutex); + _fcu_time_offset = std::chrono::duration_cast(offset); + }; + + template inline dl_fcu_time_t time_in(T local_system_time_point) + { + std::lock_guard lock(_fcu_system_time_offset_mutex); + return dl_fcu_time_t(local_system_time_point + std::chrono::duration_cast(_fcu_time_offset)); + }; + +private: + mutable std::mutex _fcu_system_time_offset_mutex{}; + std::chrono::nanoseconds _fcu_time_offset{}; + + virtual dl_stime_t system_time(); +}; + double to_rad_from_deg(double deg); double to_deg_from_rad(double rad); diff --git a/src/core/system_impl.cpp b/src/core/system_impl.cpp index fc2bd02b84..c96c79e4f9 100644 --- a/src/core/system_impl.cpp +++ b/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. diff --git a/src/core/system_impl.h b/src/core/system_impl.h index 2cfa4145ba..46f969a6e9 100644 --- a/src/core/system_impl.h +++ b/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 #include @@ -192,6 +193,7 @@ class SystemImpl { bool is_connected() const; Time& get_time() { return _time; }; + FCUTime& get_fcu_time() { return _fcu_time; }; void register_plugin(PluginImplBase* plugin_impl); void unregister_plugin(PluginImplBase* plugin_impl); @@ -292,10 +294,13 @@ class SystemImpl { MAVLinkCommands _commands; + Timesync _timesync; + TimeoutHandler _timeout_handler; CallEveryHandler _call_every_handler; Time _time{}; + FCUTime _fcu_time{}; std::mutex _plugin_impls_mutex{}; std::vector _plugin_impls{}; diff --git a/src/core/timesync.cpp b/src/core/timesync.cpp new file mode 100644 index 0000000000..ecdb659f17 --- /dev/null +++ b/src/core/timesync.cpp @@ -0,0 +1,100 @@ +#include "timesync.h" +#include "system_impl.h" +#include "log.h" +#include +#include +#include +#include + +// 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( + _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, ×ync); + + uint64_t now_ns = std::chrono::duration_cast( + _parent.get_time().system_time().time_since_epoch()) + .count(); + + if (timesync.tc1 == 0) { + send_timesync(now_ns, timesync.ts1); + return; + } 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 local_time_ns) +{ + uint64_t now_ns = std::chrono::duration_cast( + _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 - 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_fcu_time().shift_fcu_time_by( + std::chrono::nanoseconds(static_cast(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 diff --git a/src/core/timesync.h b/src/core/timesync.h new file mode 100644 index 0000000000..08d192295a --- /dev/null +++ b/src/core/timesync.h @@ -0,0 +1,41 @@ +#pragma once + +#include +#include + +#include "global_include.h" +#include "mavlink_include.h" + +// Since not all vehicles support/require level calibration, this +// is disabled for now. +//#define LEVEL_CALIBRATION + +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 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 diff --git a/src/plugins/mocap/mocap_impl.cpp b/src/plugins/mocap/mocap_impl.cpp index d363820206..b8edaa3a76 100644 --- a/src/plugins/mocap/mocap_impl.cpp +++ b/src/plugins/mocap/mocap_impl.cpp @@ -3,6 +3,7 @@ #include "global_include.h" #include "px4_custom_mode.h" #include +#include namespace mavsdk { @@ -73,13 +74,23 @@ Mocap::Result MocapImpl::set_odometry(const Mocap::Odometry& odometry) bool MocapImpl::send_vision_position_estimate() { + uint64_t fcu_time_usec{}; + _visual_position_estimate_mutex.lock(); 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(_parent->get_time().elapsed_s() * 1e3); + fcu_time_usec = std::chrono::duration_cast( + _parent->get_fcu_time().now().time_since_epoch()) + .count(); + } else { + fcu_time_usec = + std::chrono::duration_cast( + _parent->get_fcu_time() + .time_in(std::chrono::microseconds(vision_position_estimate.time_usec)) + .time_since_epoch()) + .count(); } mavlink_message_t message; @@ -88,7 +99,7 @@ bool MocapImpl::send_vision_position_estimate() _parent->get_own_system_id(), _parent->get_own_component_id(), &message, - vision_position_estimate.time_usec, + fcu_time_usec, vision_position_estimate.position_body.x_m, vision_position_estimate.position_body.y_m, vision_position_estimate.position_body.z_m, @@ -103,14 +114,25 @@ bool MocapImpl::send_vision_position_estimate() bool MocapImpl::send_attitude_position_mocap() { + uint64_t fcu_time_usec{}; + _attitude_position_mocap_mutex.lock(); 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(_parent->get_time().elapsed_s() * 1e3); + fcu_time_usec = std::chrono::duration_cast( + _parent->get_fcu_time().now().time_since_epoch()) + .count(); + } else { + fcu_time_usec = + std::chrono::duration_cast( + _parent->get_fcu_time() + .time_in(std::chrono::microseconds(attitude_position_mocap.time_usec)) + .time_since_epoch()) + .count(); } + mavlink_message_t message; std::array q{}; @@ -123,7 +145,7 @@ bool MocapImpl::send_attitude_position_mocap() _parent->get_own_system_id(), _parent->get_own_component_id(), &message, - attitude_position_mocap.time_usec, + fcu_time_usec, q.data(), attitude_position_mocap.position_body.x_m, attitude_position_mocap.position_body.y_m, @@ -135,13 +157,24 @@ bool MocapImpl::send_attitude_position_mocap() bool MocapImpl::send_odometry() { + uint64_t fcu_time_usec{}; + _odometry_mutex.lock(); auto odometry = _odometry; _odometry_mutex.unlock(); if (!odometry.time_usec) { - odometry.time_usec = static_cast(_parent->get_time().elapsed_s() * 1e3); + fcu_time_usec = std::chrono::duration_cast( + _parent->get_fcu_time().now().time_since_epoch()) + .count(); + } else { + fcu_time_usec = std::chrono::duration_cast( + _parent->get_fcu_time() + .time_in(std::chrono::microseconds(odometry.time_usec)) + .time_since_epoch()) + .count(); } + mavlink_message_t message; std::array q{}; @@ -154,7 +187,7 @@ bool MocapImpl::send_odometry() _parent->get_own_system_id(), _parent->get_own_component_id(), &message, - odometry.time_usec, + fcu_time_usec, static_cast(odometry.frame_id), static_cast(MAV_FRAME_BODY_FRD), odometry.position_body.x_m, From c4e3da215fba390f89d1ac8492ceb7f6ca6fd6ac Mon Sep 17 00:00:00 2001 From: Ildar Sadykov Date: Fri, 25 Oct 2019 17:13:06 +0300 Subject: [PATCH 2/2] Mavros artifact removed, FCUTime->AutopilotTime And cast fixes, code suggestions --- src/core/global_include.cpp | 29 ++++++++++---- src/core/global_include.h | 32 ++++++--------- src/core/system_impl.h | 4 +- src/core/timesync.cpp | 20 ++++------ src/core/timesync.h | 9 +---- src/plugins/mocap/mocap_impl.cpp | 67 ++++++++++++++------------------ 6 files changed, 73 insertions(+), 88 deletions(-) diff --git a/src/core/global_include.cpp b/src/core/global_include.cpp index c5fcb129a3..ce4cac37ec 100644 --- a/src/core/global_include.cpp +++ b/src/core/global_include.cpp @@ -4,6 +4,7 @@ #include #include #include +#include namespace mavsdk { @@ -18,7 +19,7 @@ dl_time_t Time::steady_time() return steady_clock::now(); } -dl_stime_t Time::system_time() +dl_system_time_t Time::system_time() { return system_clock::now(); } @@ -164,18 +165,32 @@ bool are_equal(double one, double two) return (std::fabs(one - two) < std::numeric_limits::epsilon()); } -FCUTime::FCUTime() {} -FCUTime::~FCUTime() {} +AutopilotTime::AutopilotTime() {} +AutopilotTime::~AutopilotTime() {} -dl_stime_t FCUTime::system_time() +dl_system_time_t AutopilotTime::system_time() { return system_clock::now(); } -dl_fcu_time_t FCUTime::now() +dl_autopilot_time_t AutopilotTime::now() { - std::lock_guard lock(_fcu_system_time_offset_mutex); - return dl_fcu_time_t(system_time() + _fcu_time_offset); + std::lock_guard lock(_autopilot_system_time_offset_mutex); + return dl_autopilot_time_t(std::chrono::duration_cast( + system_time().time_since_epoch() + _autopilot_time_offset)); } +void AutopilotTime::shift_time_by(std::chrono::nanoseconds offset) +{ + std::lock_guard 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 lock(_autopilot_system_time_offset_mutex); + return dl_autopilot_time_t(std::chrono::duration_cast( + local_system_time_point.time_since_epoch() + _autopilot_time_offset)); +}; + } // namespace mavsdk diff --git a/src/core/global_include.h b/src/core/global_include.h index 56240dfb32..9a94676eba 100644 --- a/src/core/global_include.h +++ b/src/core/global_include.h @@ -26,8 +26,8 @@ constexpr float M_PI_F = float(M_PI); namespace mavsdk { typedef std::chrono::time_point dl_time_t; -typedef std::chrono::time_point dl_stime_t; -typedef std::chrono::time_point dl_fcu_time_t; +typedef std::chrono::time_point dl_system_time_t; +typedef std::chrono::time_point dl_autopilot_time_t; class Time { public: @@ -35,7 +35,7 @@ class Time { virtual ~Time(); virtual dl_time_t steady_time(); - virtual dl_stime_t system_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); @@ -67,30 +67,22 @@ class FakeTime : public Time { void add_overhead(); }; -class FCUTime { +class AutopilotTime { public: - FCUTime(); - virtual ~FCUTime(); + AutopilotTime(); + virtual ~AutopilotTime(); - dl_fcu_time_t now(); + dl_autopilot_time_t now(); - template void shift_fcu_time_by(T offset) - { - std::lock_guard lock(_fcu_system_time_offset_mutex); - _fcu_time_offset = std::chrono::duration_cast(offset); - }; + void shift_time_by(std::chrono::nanoseconds offset); - template inline dl_fcu_time_t time_in(T local_system_time_point) - { - std::lock_guard lock(_fcu_system_time_offset_mutex); - return dl_fcu_time_t(local_system_time_point + std::chrono::duration_cast(_fcu_time_offset)); - }; + dl_autopilot_time_t time_in(dl_system_time_t local_system_time_point); private: - mutable std::mutex _fcu_system_time_offset_mutex{}; - std::chrono::nanoseconds _fcu_time_offset{}; + mutable std::mutex _autopilot_system_time_offset_mutex{}; + std::chrono::nanoseconds _autopilot_time_offset{}; - virtual dl_stime_t system_time(); + virtual dl_system_time_t system_time(); }; double to_rad_from_deg(double deg); diff --git a/src/core/system_impl.h b/src/core/system_impl.h index 46f969a6e9..c9437c314d 100644 --- a/src/core/system_impl.h +++ b/src/core/system_impl.h @@ -193,7 +193,7 @@ class SystemImpl { bool is_connected() const; Time& get_time() { return _time; }; - FCUTime& get_fcu_time() { return _fcu_time; }; + AutopilotTime& get_autopilot_time() { return _autopilot_time; }; void register_plugin(PluginImplBase* plugin_impl); void unregister_plugin(PluginImplBase* plugin_impl); @@ -300,7 +300,7 @@ class SystemImpl { CallEveryHandler _call_every_handler; Time _time{}; - FCUTime _fcu_time{}; + AutopilotTime _autopilot_time{}; std::mutex _plugin_impls_mutex{}; std::vector _plugin_impls{}; diff --git a/src/core/timesync.cpp b/src/core/timesync.cpp index ecdb659f17..d6b59c68d6 100644 --- a/src/core/timesync.cpp +++ b/src/core/timesync.cpp @@ -1,10 +1,6 @@ #include "timesync.h" -#include "system_impl.h" #include "log.h" -#include -#include -#include -#include +#include "system_impl.h" // Partially based on: https://github.com/mavlink/mavros/blob/master/mavros/src/plugins/sys_time.cpp @@ -42,13 +38,12 @@ void Timesync::process_timesync(const mavlink_message_t& message) mavlink_msg_timesync_decode(&message, ×ync); - uint64_t now_ns = std::chrono::duration_cast( - _parent.get_time().system_time().time_since_epoch()) - .count(); + int64_t now_ns = std::chrono::duration_cast( + _parent.get_time().system_time().time_since_epoch()) + .count(); if (timesync.tc1 == 0) { send_timesync(now_ns, timesync.ts1); - return; } 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. @@ -65,7 +60,7 @@ void Timesync::send_timesync(uint64_t tc1, uint64_t ts1) _parent.send_message(message); } -void Timesync::set_timesync_offset(int64_t offset_ns, uint64_t local_time_ns) +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()) @@ -73,13 +68,12 @@ void Timesync::set_timesync_offset(int64_t offset_ns, uint64_t local_time_ns) // 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 - local_time_ns; + 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_fcu_time().shift_fcu_time_by( - std::chrono::nanoseconds(static_cast(offset_ns))); + _parent.get_autopilot_time().shift_time_by(std::chrono::nanoseconds(offset_ns)); // Reset high RTT count after filter update _high_rtt_count = 0; diff --git a/src/core/timesync.h b/src/core/timesync.h index 08d192295a..ce27349afd 100644 --- a/src/core/timesync.h +++ b/src/core/timesync.h @@ -1,15 +1,8 @@ #pragma once -#include -#include - #include "global_include.h" #include "mavlink_include.h" -// Since not all vehicles support/require level calibration, this -// is disabled for now. -//#define LEVEL_CALIBRATION - namespace mavsdk { class SystemImpl; @@ -29,7 +22,7 @@ class Timesync { 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 local_time_ns); + 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{}; diff --git a/src/plugins/mocap/mocap_impl.cpp b/src/plugins/mocap/mocap_impl.cpp index b8edaa3a76..c0989e5938 100644 --- a/src/plugins/mocap/mocap_impl.cpp +++ b/src/plugins/mocap/mocap_impl.cpp @@ -74,24 +74,21 @@ Mocap::Result MocapImpl::set_odometry(const Mocap::Odometry& odometry) bool MocapImpl::send_vision_position_estimate() { - uint64_t fcu_time_usec{}; - _visual_position_estimate_mutex.lock(); auto vision_position_estimate = _vision_position_estimate; _visual_position_estimate_mutex.unlock(); - if (!vision_position_estimate.time_usec) { - fcu_time_usec = std::chrono::duration_cast( - _parent->get_fcu_time().now().time_since_epoch()) - .count(); - } else { - fcu_time_usec = + const uint64_t autopilot_time_usec = + (!vision_position_estimate.time_usec) ? + std::chrono::duration_cast( + _parent->get_autopilot_time().now().time_since_epoch()) + .count() : std::chrono::duration_cast( - _parent->get_fcu_time() - .time_in(std::chrono::microseconds(vision_position_estimate.time_usec)) + _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; @@ -99,7 +96,7 @@ bool MocapImpl::send_vision_position_estimate() _parent->get_own_system_id(), _parent->get_own_component_id(), &message, - fcu_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, @@ -114,24 +111,21 @@ bool MocapImpl::send_vision_position_estimate() bool MocapImpl::send_attitude_position_mocap() { - uint64_t fcu_time_usec{}; - _attitude_position_mocap_mutex.lock(); auto attitude_position_mocap = _attitude_position_mocap; _attitude_position_mocap_mutex.unlock(); - if (!attitude_position_mocap.time_usec) { - fcu_time_usec = std::chrono::duration_cast( - _parent->get_fcu_time().now().time_since_epoch()) - .count(); - } else { - fcu_time_usec = + const uint64_t autopilot_time_usec = + (!attitude_position_mocap.time_usec) ? std::chrono::duration_cast( - _parent->get_fcu_time() - .time_in(std::chrono::microseconds(attitude_position_mocap.time_usec)) + _parent->get_autopilot_time().now().time_since_epoch()) + .count() : + std::chrono::duration_cast( + _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; @@ -145,7 +139,7 @@ bool MocapImpl::send_attitude_position_mocap() _parent->get_own_system_id(), _parent->get_own_component_id(), &message, - fcu_time_usec, + autopilot_time_usec, q.data(), attitude_position_mocap.position_body.x_m, attitude_position_mocap.position_body.y_m, @@ -157,23 +151,20 @@ bool MocapImpl::send_attitude_position_mocap() bool MocapImpl::send_odometry() { - uint64_t fcu_time_usec{}; - _odometry_mutex.lock(); auto odometry = _odometry; _odometry_mutex.unlock(); - if (!odometry.time_usec) { - fcu_time_usec = std::chrono::duration_cast( - _parent->get_fcu_time().now().time_since_epoch()) - .count(); - } else { - fcu_time_usec = std::chrono::duration_cast( - _parent->get_fcu_time() - .time_in(std::chrono::microseconds(odometry.time_usec)) - .time_since_epoch()) - .count(); - } + const uint64_t autopilot_time_usec = + (!odometry.time_usec) ? + std::chrono::duration_cast( + _parent->get_autopilot_time().now().time_since_epoch()) + .count() : + std::chrono::duration_cast( + _parent->get_autopilot_time() + .time_in(dl_system_time_t(std::chrono::microseconds(odometry.time_usec))) + .time_since_epoch()) + .count(); mavlink_message_t message; @@ -187,7 +178,7 @@ bool MocapImpl::send_odometry() _parent->get_own_system_id(), _parent->get_own_component_id(), &message, - fcu_time_usec, + autopilot_time_usec, static_cast(odometry.frame_id), static_cast(MAV_FRAME_BODY_FRD), odometry.position_body.x_m,