From 6ff8926fc8ea6b5bd3e029ca04d7a6d494f56843 Mon Sep 17 00:00:00 2001 From: robo Date: Sun, 30 Apr 2023 18:05:26 +0300 Subject: [PATCH] control time reference source use time_ref_source to control: - time_unix_usec - time_boot_ms using "epoc" string to use time_unix_usec other strings: boot_time --- mavros/src/plugins/sys_time.cpp | 28 ++++++++++++++++++++-------- 1 file changed, 20 insertions(+), 8 deletions(-) diff --git a/mavros/src/plugins/sys_time.cpp b/mavros/src/plugins/sys_time.cpp index b76bc9690..3e876f502 100644 --- a/mavros/src/plugins/sys_time.cpp +++ b/mavros/src/plugins/sys_time.cpp @@ -345,15 +345,27 @@ class SystemTimePlugin : public plugin::Plugin if (fcu_time_valid) { // continious publish for ntpd auto time_unix = sensor_msgs::msg::TimeReference(); - rclcpp::Time time_ref( - mtime.time_unix_usec / 1000000, // t_sec - (mtime.time_unix_usec % 1000000) * 1000); // t_nsec - - time_unix.header.stamp = node->now(); - time_unix.time_ref = time_ref; - time_unix.source = time_ref_source; + if (time_ref_source == "epoc") + { + // time_unix_usec: us + rclcpp::Time time_ref( + mtime.time_unix_usec / 1000000, // t_sec + (mtime.time_unix_usec % 1000000) * 1000); // t_nsec + time_unix.time_ref = time_ref; + } + else + { + // time_unix_usec: ms + rclcpp::Time time_ref( + mtime.time_boot_ms / 1000, // t_sec + (mtime.time_boot_ms % 1000) * 1000000); // t_nsec + time_unix.time_ref = time_ref; + } + time_unix.header.stamp = node->now(); + time_unix.header.frame_id = ""; + time_unix.source = time_ref_source; - time_ref_pub->publish(time_unix); + time_ref_pub->publish(time_unix); } else { RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 60000, "TM: Wrong FCU time."); }