From c3336e6b5bf0c435af79ee8c6af77cb04d35b908 Mon Sep 17 00:00:00 2001 From: mhkabir Date: Mon, 6 Oct 2014 20:27:36 +0530 Subject: [PATCH 01/13] Initial sys_time plugin import --- mavros/src/plugins/gps.cpp | 27 ++------------------------- mavros/src/plugins/sys_status.cpp | 19 ------------------- 2 files changed, 2 insertions(+), 44 deletions(-) diff --git a/mavros/src/plugins/gps.cpp b/mavros/src/plugins/gps.cpp index c7fa2bb8b..6a6d043a6 100644 --- a/mavros/src/plugins/gps.cpp +++ b/mavros/src/plugins/gps.cpp @@ -127,9 +127,8 @@ class GPSPlugin : public MavRosPlugin { const message_map get_rx_handlers() { return { MESSAGE_HANDLER(MAVLINK_MSG_ID_GPS_RAW_INT, &GPSPlugin::handle_gps_raw_int), - MESSAGE_HANDLER(MAVLINK_MSG_ID_GPS_STATUS, &GPSPlugin::handle_gps_status), - MESSAGE_HANDLER(MAVLINK_MSG_ID_SYSTEM_TIME, &GPSPlugin::handle_system_time), - }; + MESSAGE_HANDLER(MAVLINK_MSG_ID_GPS_STATUS, &GPSPlugin::handle_gps_status); + }; } private: @@ -219,28 +218,6 @@ class GPSPlugin : public MavRosPlugin { ROS_INFO_THROTTLE_NAMED(30, "gps", "GPS stat sat visible: %d", gps_stat.satellites_visible); } - void handle_system_time(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) { - mavlink_system_time_t mtime; - mavlink_msg_system_time_decode(msg, &mtime); - - if (mtime.time_unix_usec == 0) { - ROS_WARN_THROTTLE_NAMED(60, "gps", "Wrong system time. Is GPS Ok? (boot_ms: %u)", - mtime.time_boot_ms); - return; - } - - sensor_msgs::TimeReferencePtr time = boost::make_shared(); - ros::Time time_ref( - mtime.time_unix_usec / 1000000, // t_sec - (mtime.time_unix_usec % 1000000) * 1000); // t_nsec - - time->source = time_ref_source; - time->time_ref = time_ref; - time->header.frame_id = time_ref_source; - time->header.stamp = ros::Time::now(); - - time_ref_pub.publish(time); - } }; }; // namespace mavplugin diff --git a/mavros/src/plugins/sys_status.cpp b/mavros/src/plugins/sys_status.cpp index e3d88e6b7..2bdf7ebc0 100644 --- a/mavros/src/plugins/sys_status.cpp +++ b/mavros/src/plugins/sys_status.cpp @@ -345,13 +345,11 @@ class SystemStatusPlugin : public MavRosPlugin double conn_timeout_d; double conn_heartbeat_d; - double conn_system_time_d; double min_voltage; bool disable_diag; nh.param("conn_timeout", conn_timeout_d, 30.0); nh.param("conn_heartbeat", conn_heartbeat_d, 0.0); - nh.param("conn_system_time", conn_system_time_d, 0.0); nh.param("sys/min_voltage", min_voltage, 6.0); nh.param("sys/disable_diag", disable_diag, false); @@ -382,12 +380,6 @@ class SystemStatusPlugin : public MavRosPlugin heartbeat_timer.start(); } - if (conn_system_time_d > 0.0) { - sys_time_timer = nh.createTimer(ros::Duration(conn_system_time_d), - &SystemStatusPlugin::sys_time_cb, this); - sys_time_timer.start(); - } - state_pub = nh.advertise("state", 10); batt_pub = nh.advertise("battery", 10); rate_srv = nh.advertiseService("set_stream_rate", &SystemStatusPlugin::set_rate_cb, this); @@ -421,7 +413,6 @@ class SystemStatusPlugin : public MavRosPlugin BatteryStatusDiag batt_diag; ros::Timer timeout_timer; ros::Timer heartbeat_timer; - ros::Timer sys_time_timer; ros::Publisher state_pub; ros::Publisher batt_pub; @@ -579,16 +570,6 @@ class SystemStatusPlugin : public MavRosPlugin UAS_FCU(uas)->send_message(&msg); } - void sys_time_cb(const ros::TimerEvent &event) { - mavlink_message_t msg; - auto stamp = ros::Time::now(); - mavlink_msg_system_time_pack_chan(UAS_PACK_CHAN(uas), &msg, - stamp.toNSec() / 1000, /* nano -> micro */ - 0 - ); - UAS_FCU(uas)->send_message(&msg); - } - /* -*- ros callbacks -*- */ bool set_rate_cb(mavros::StreamRate::Request &req, From be82df8d0ab06c83078ba50db63ccd0a0aae39cb Mon Sep 17 00:00:00 2001 From: mhkabir Date: Mon, 6 Oct 2014 20:28:46 +0530 Subject: [PATCH 02/13] sys_time import. Removed all time related stuff from gps and sys_status --- mavros/src/plugins/sys_time.cpp | 164 ++++++++++++++++++++++++++++++++ 1 file changed, 164 insertions(+) create mode 100644 mavros/src/plugins/sys_time.cpp diff --git a/mavros/src/plugins/sys_time.cpp b/mavros/src/plugins/sys_time.cpp new file mode 100644 index 000000000..e29fefd1f --- /dev/null +++ b/mavros/src/plugins/sys_time.cpp @@ -0,0 +1,164 @@ +/** + * @brief Dummy plugin + * @file dummy.cpp + * @author Vladimir Ermakov + * + * @example dummy.cpp + * @addtogroup plugin + * @{ + */ +/* + * Copyright 2013 Vladimir Ermakov. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include +#include + +namespace mavplugin { + +class SystemTimePlugin : public MavRosPlugin { +public: + SystemTimePlugin(): + uas(nullptr) + { + uas = &uas_; + + bool companion_reboot = true; + uint64_t time_offset; + bool fcu_unix_valid; + bool ros_unix_valid; + uint64_t time_unix_usec; + + //timer for sending time sync messages + if (conn_system_time_d > 0.0) { + sys_time_timer = nh.createTimer(ros::Duration(conn_system_time_d), + &SystemStatusPlugin::sys_time_cb, this); + sys_time_timer.start(); + } + + }; + + void initialize(UAS &uas, + ros::NodeHandle &nh, + diagnostic_updater::Updater &diag_updater) + { + nh.param("frame_id", frame_id, "fcu"); + nh.param("time_ref_source", time_ref_source, frame_id); + + nh.param("conn_system_time", conn_system_time_d, 0.0); + + time_ref_pub = nh.advertise("time_reference", 10); + time_offset_pub = nh.advertise("time_offset", 10); + }; + + + std::string const get_name() const { + return "SystemTime"; + }; + + const message_map get_rx_handlers() { + return { + MESSAGE_HANDLER(MAVLINK_MSG_ID_SYSTEM_TIME, &SystemTimePlugin::handle_system_time); + }; + } + +private: + ros::Timer sys_time_timer; + + ros::Publisher time_ref_pub; + ros::Publisher time_offset_pub; + + void handle_system_time(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) { + + mavlink_system_time_t mtime; + mavlink_msg_system_time_decode(msg, &mtime); + + time_unix_usec = ros::Time::now().toNSec()/1000; + + int64_t dt = ((time_unix_usec/1000) - mtime.time_boot_ms) - time_offset ; + + fcu_unix_valid = mtime.tv_sec > 1293840000; + ros_unix_valid = (time_unix_usec/1000) > 1293840000; + + if(dt > 2000 || dt < -2000) //2 sec + { + ROS_WARN_THROTTLE_NAMED(60, "time", "Companion reboot / clock skew"); + companion_reboot = true; + } + else + { + time_offset = (time_offset + ((time_unix_usec/1000) - mtime.time_boot_ms)))/2; + companion_reboot = false; + } + + // px4 incoming msgs ADD + if(companion_reboot) + { + ROS_WARN_THROTTLE_NAMED(60, "time", "Large clock skew detected. Resyncing clocks"); + time_offset = ((time_unix_usec/1000) - mtime.time_boot_ms)); + companion_reboot = false; + } + + if(fcu_unix_valid) //continious publish for ntpd + { + sensor_msgs::TimeReferencePtr time_unix = boost::make_shared(); + ros::Time time_ref( + mtime.time_unix_usec / 1000000, // t_sec + (mtime.time_unix_usec % 1000000) * 1000); // t_nsec + + time_unix->source = time_ref_source; + time_unix->time_ref = time_ref; + time_unix->header.frame_id = time_ref_source; + time_unix->header.stamp = ros::Time::now(); + + time_ref_pub.publish(time_unix); + } + + //offset publisher + sensor_msgs::TimeReferencePtr time_offset = boost::make_shared(); + ros::Time time_ref( + time_offset / 1000, // t_sec + time_offset * 1000); // t_nsec + + time_offset->source = time_ref_source; + time_offset->time_ref = time_ref; + time_offset->header.frame_id = time_ref_source; + time_offset->header.stamp = ros::Time::now(); + + time_offset_pub.publish(time_offset); + + } + + void sys_time_cb(const ros::TimerEvent &event) { + mavlink_message_t msg; + + time_unix_usec = ros::Time::now().toNSec() / 1000; + + mavlink_msg_system_time_pack_chan(UAS_PACK_CHAN(uas), &msg, + time_unix_usec, /* nano -> micro */ + 0 + ); + UAS_FCU(uas)->send_message(&msg); + + } + +}; + +}; // namespace mavplugin + +PLUGINLIB_EXPORT_CLASS(mavplugin::SystemTimePlugin, mavplugin::MavRosPlugin) + From 15c628643912352365fd96743ea148ba96571e8a Mon Sep 17 00:00:00 2001 From: mhkabir Date: Mon, 6 Oct 2014 20:46:27 +0530 Subject: [PATCH 03/13] Fix build --- mavros/src/plugins/gps.cpp | 4 ++-- mavros/src/plugins/sys_time.cpp | 7 +++---- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/mavros/src/plugins/gps.cpp b/mavros/src/plugins/gps.cpp index 6a6d043a6..9df1d85c0 100644 --- a/mavros/src/plugins/gps.cpp +++ b/mavros/src/plugins/gps.cpp @@ -127,8 +127,8 @@ class GPSPlugin : public MavRosPlugin { const message_map get_rx_handlers() { return { MESSAGE_HANDLER(MAVLINK_MSG_ID_GPS_RAW_INT, &GPSPlugin::handle_gps_raw_int), - MESSAGE_HANDLER(MAVLINK_MSG_ID_GPS_STATUS, &GPSPlugin::handle_gps_status); - }; + MESSAGE_HANDLER(MAVLINK_MSG_ID_GPS_STATUS, &GPSPlugin::handle_gps_status), + }; } private: diff --git a/mavros/src/plugins/sys_time.cpp b/mavros/src/plugins/sys_time.cpp index e29fefd1f..42b98d113 100644 --- a/mavros/src/plugins/sys_time.cpp +++ b/mavros/src/plugins/sys_time.cpp @@ -1,9 +1,8 @@ /** - * @brief Dummy plugin - * @file dummy.cpp - * @author Vladimir Ermakov + * @brief System Time plugin + * @file sys_time.cpp + * @author M.H.Kabir mhkabir98@gmail.com> * - * @example dummy.cpp * @addtogroup plugin * @{ */ From e4b09f62a89971ffcfd98c6dc163d1ec43b9b92e Mon Sep 17 00:00:00 2001 From: mhkabir Date: Mon, 6 Oct 2014 22:22:58 +0530 Subject: [PATCH 04/13] Update sys_time.cpp --- mavros/src/plugins/sys_time.cpp | 38 +++++++++++++-------------------- 1 file changed, 15 insertions(+), 23 deletions(-) diff --git a/mavros/src/plugins/sys_time.cpp b/mavros/src/plugins/sys_time.cpp index 42b98d113..c8ce750b3 100644 --- a/mavros/src/plugins/sys_time.cpp +++ b/mavros/src/plugins/sys_time.cpp @@ -33,10 +33,15 @@ class SystemTimePlugin : public MavRosPlugin { public: SystemTimePlugin(): uas(nullptr) - { + {}; + + void initialize(UAS &uas, + ros::NodeHandle &nh, + diagnostic_updater::Updater &diag_updater) + { + uas = &uas_; - - bool companion_reboot = true; + uint64_t time_offset; bool fcu_unix_valid; bool ros_unix_valid; @@ -48,13 +53,7 @@ class SystemTimePlugin : public MavRosPlugin { &SystemStatusPlugin::sys_time_cb, this); sys_time_timer.start(); } - - }; - - void initialize(UAS &uas, - ros::NodeHandle &nh, - diagnostic_updater::Updater &diag_updater) - { + nh.param("frame_id", frame_id, "fcu"); nh.param("time_ref_source", time_ref_source, frame_id); @@ -83,6 +82,8 @@ class SystemTimePlugin : public MavRosPlugin { void handle_system_time(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) { + // We use 1/1/2011 as check for validity of UNIX time = 1293840000 s + mavlink_system_time_t mtime; mavlink_msg_system_time_decode(msg, &mtime); @@ -95,21 +96,12 @@ class SystemTimePlugin : public MavRosPlugin { if(dt > 2000 || dt < -2000) //2 sec { - ROS_WARN_THROTTLE_NAMED(60, "time", "Companion reboot / clock skew"); - companion_reboot = true; + ROS_WARN_THROTTLE_NAMED(60, "time", "Large clock skew detected. Resyncing clocks"); + time_offset = ((time_unix_usec/1000) - mtime.time_boot_ms)); } else { time_offset = (time_offset + ((time_unix_usec/1000) - mtime.time_boot_ms)))/2; - companion_reboot = false; - } - - // px4 incoming msgs ADD - if(companion_reboot) - { - ROS_WARN_THROTTLE_NAMED(60, "time", "Large clock skew detected. Resyncing clocks"); - time_offset = ((time_unix_usec/1000) - mtime.time_boot_ms)); - companion_reboot = false; } if(fcu_unix_valid) //continious publish for ntpd @@ -145,10 +137,10 @@ class SystemTimePlugin : public MavRosPlugin { void sys_time_cb(const ros::TimerEvent &event) { mavlink_message_t msg; - time_unix_usec = ros::Time::now().toNSec() / 1000; + time_unix_usec = ros::Time::now().toNSec() / 1000; //nano -> micro mavlink_msg_system_time_pack_chan(UAS_PACK_CHAN(uas), &msg, - time_unix_usec, /* nano -> micro */ + time_unix_usec, 0 ); UAS_FCU(uas)->send_message(&msg); From 788611369657dc92326fe83a934f2a80b1ceed2a Mon Sep 17 00:00:00 2001 From: mhkabir Date: Mon, 6 Oct 2014 22:25:52 +0530 Subject: [PATCH 05/13] Update mavros_plugins.xml --- mavros/mavros_plugins.xml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/mavros/mavros_plugins.xml b/mavros/mavros_plugins.xml index 0b9c750aa..eafbe76a8 100644 --- a/mavros/mavros_plugins.xml +++ b/mavros/mavros_plugins.xml @@ -53,5 +53,8 @@ Access to files on FCU via MAVLink-FTP. + + Synchronise clocks with FCU. + From 18e9ab3132525c6b3d8cc479fb412c321a0e896e Mon Sep 17 00:00:00 2001 From: mhkabir Date: Mon, 6 Oct 2014 22:26:14 +0530 Subject: [PATCH 06/13] Update CMakeLists.txt --- mavros/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/mavros/CMakeLists.txt b/mavros/CMakeLists.txt index 8fb074aa6..e2374e803 100644 --- a/mavros/CMakeLists.txt +++ b/mavros/CMakeLists.txt @@ -185,6 +185,7 @@ target_link_libraries(mavros add_library(mavros_plugins src/plugins/dummy.cpp src/plugins/sys_status.cpp + src/plugins/sys_time.cpp src/plugins/imu_pub.cpp src/plugins/gps.cpp src/plugins/param.cpp From 44157b1d3fc3fc0354531e693ac835d87a3d3039 Mon Sep 17 00:00:00 2001 From: mhkabir Date: Mon, 6 Oct 2014 22:33:25 +0530 Subject: [PATCH 07/13] Update sys_time.cpp --- mavros/src/plugins/sys_time.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/mavros/src/plugins/sys_time.cpp b/mavros/src/plugins/sys_time.cpp index c8ce750b3..802bd38dc 100644 --- a/mavros/src/plugins/sys_time.cpp +++ b/mavros/src/plugins/sys_time.cpp @@ -42,11 +42,6 @@ class SystemTimePlugin : public MavRosPlugin { uas = &uas_; - uint64_t time_offset; - bool fcu_unix_valid; - bool ros_unix_valid; - uint64_t time_unix_usec; - //timer for sending time sync messages if (conn_system_time_d > 0.0) { sys_time_timer = nh.createTimer(ros::Duration(conn_system_time_d), @@ -75,7 +70,15 @@ class SystemTimePlugin : public MavRosPlugin { } private: - ros::Timer sys_time_timer; + UAS *uas; + + + uint64_t time_offset; + bool fcu_unix_valid; + bool ros_unix_valid; + uint64_t time_unix_usec; + + ros::Timer sys_time_timer; ros::Publisher time_ref_pub; ros::Publisher time_offset_pub; From 6a2d7f5179e8f0bd99ddb1c1d2eab37ed5435917 Mon Sep 17 00:00:00 2001 From: mhkabir Date: Mon, 6 Oct 2014 22:49:54 +0530 Subject: [PATCH 08/13] Update sys_time.cpp --- mavros/src/plugins/sys_time.cpp | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/mavros/src/plugins/sys_time.cpp b/mavros/src/plugins/sys_time.cpp index 802bd38dc..7b96db282 100644 --- a/mavros/src/plugins/sys_time.cpp +++ b/mavros/src/plugins/sys_time.cpp @@ -27,6 +27,8 @@ #include #include +#include + namespace mavplugin { class SystemTimePlugin : public MavRosPlugin { @@ -35,13 +37,15 @@ class SystemTimePlugin : public MavRosPlugin { uas(nullptr) {}; - void initialize(UAS &uas, + void initialize(UAS &uas_, ros::NodeHandle &nh, diagnostic_updater::Updater &diag_updater) { uas = &uas_; + nh.param("conn_system_time", conn_system_time_d, 0.0); + //timer for sending time sync messages if (conn_system_time_d > 0.0) { sys_time_timer = nh.createTimer(ros::Duration(conn_system_time_d), @@ -52,11 +56,9 @@ class SystemTimePlugin : public MavRosPlugin { nh.param("frame_id", frame_id, "fcu"); nh.param("time_ref_source", time_ref_source, frame_id); - nh.param("conn_system_time", conn_system_time_d, 0.0); - time_ref_pub = nh.advertise("time_reference", 10); time_offset_pub = nh.advertise("time_offset", 10); - }; + } std::string const get_name() const { @@ -65,14 +67,13 @@ class SystemTimePlugin : public MavRosPlugin { const message_map get_rx_handlers() { return { - MESSAGE_HANDLER(MAVLINK_MSG_ID_SYSTEM_TIME, &SystemTimePlugin::handle_system_time); + MESSAGE_HANDLER(MAVLINK_MSG_ID_SYSTEM_TIME, &SystemTimePlugin::handle_system_time), }; } private: UAS *uas; - uint64_t time_offset; bool fcu_unix_valid; bool ros_unix_valid; @@ -82,6 +83,9 @@ class SystemTimePlugin : public MavRosPlugin { ros::Publisher time_ref_pub; ros::Publisher time_offset_pub; + + std::string frame_id; + std::string time_ref_source; void handle_system_time(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) { @@ -94,7 +98,7 @@ class SystemTimePlugin : public MavRosPlugin { int64_t dt = ((time_unix_usec/1000) - mtime.time_boot_ms) - time_offset ; - fcu_unix_valid = mtime.tv_sec > 1293840000; + fcu_unix_valid = (mtime.time_unix_usec/1000) > 1293840000; ros_unix_valid = (time_unix_usec/1000) > 1293840000; if(dt > 2000 || dt < -2000) //2 sec From 7040d15b920991ebf2d4e1ec528f0c3d4c6bcd4e Mon Sep 17 00:00:00 2001 From: mhkabir Date: Mon, 6 Oct 2014 23:03:29 +0530 Subject: [PATCH 09/13] Update sys_time.cpp --- mavros/src/plugins/sys_time.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/mavros/src/plugins/sys_time.cpp b/mavros/src/plugins/sys_time.cpp index 7b96db282..39d98b729 100644 --- a/mavros/src/plugins/sys_time.cpp +++ b/mavros/src/plugins/sys_time.cpp @@ -44,6 +44,8 @@ class SystemTimePlugin : public MavRosPlugin { uas = &uas_; + double conn_system_time_d; + nh.param("conn_system_time", conn_system_time_d, 0.0); //timer for sending time sync messages @@ -104,11 +106,11 @@ class SystemTimePlugin : public MavRosPlugin { if(dt > 2000 || dt < -2000) //2 sec { ROS_WARN_THROTTLE_NAMED(60, "time", "Large clock skew detected. Resyncing clocks"); - time_offset = ((time_unix_usec/1000) - mtime.time_boot_ms)); + time_offset = (time_unix_usec/1000) - mtime.time_boot_ms; } else { - time_offset = (time_offset + ((time_unix_usec/1000) - mtime.time_boot_ms)))/2; + time_offset = (time_offset + ((time_unix_usec/1000) - mtime.time_boot_ms))/2; } if(fcu_unix_valid) //continious publish for ntpd From a6619a404e830aef9552daf6e83218e3a39f8cfb Mon Sep 17 00:00:00 2001 From: mhkabir Date: Mon, 6 Oct 2014 23:13:16 +0530 Subject: [PATCH 10/13] Update sys_time.cpp --- mavros/src/plugins/sys_time.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/mavros/src/plugins/sys_time.cpp b/mavros/src/plugins/sys_time.cpp index 39d98b729..aeda99d73 100644 --- a/mavros/src/plugins/sys_time.cpp +++ b/mavros/src/plugins/sys_time.cpp @@ -51,7 +51,7 @@ class SystemTimePlugin : public MavRosPlugin { //timer for sending time sync messages if (conn_system_time_d > 0.0) { sys_time_timer = nh.createTimer(ros::Duration(conn_system_time_d), - &SystemStatusPlugin::sys_time_cb, this); + &SystemTimePlugin::sys_time_cb, this); sys_time_timer.start(); } @@ -131,8 +131,8 @@ class SystemTimePlugin : public MavRosPlugin { //offset publisher sensor_msgs::TimeReferencePtr time_offset = boost::make_shared(); ros::Time time_ref( - time_offset / 1000, // t_sec - time_offset * 1000); // t_nsec + time_offset/1000, // t_sec + time_offset*1000); // t_nsec time_offset->source = time_ref_source; time_offset->time_ref = time_ref; From 5a039a9b0f90acac1039ecb2537f234544d8abb7 Mon Sep 17 00:00:00 2001 From: <> Date: Tue, 7 Oct 2014 08:29:46 +0530 Subject: [PATCH 11/13] Fixes --- mavros/src/plugins/sys_time.cpp | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/mavros/src/plugins/sys_time.cpp b/mavros/src/plugins/sys_time.cpp index aeda99d73..7bee8f667 100644 --- a/mavros/src/plugins/sys_time.cpp +++ b/mavros/src/plugins/sys_time.cpp @@ -28,6 +28,7 @@ #include #include +#include namespace mavplugin { @@ -59,7 +60,7 @@ class SystemTimePlugin : public MavRosPlugin { nh.param("time_ref_source", time_ref_source, frame_id); time_ref_pub = nh.advertise("time_reference", 10); - time_offset_pub = nh.advertise("time_offset", 10); + time_offset_pub = nh.advertise("time_offset", 10); } @@ -129,17 +130,14 @@ class SystemTimePlugin : public MavRosPlugin { } //offset publisher - sensor_msgs::TimeReferencePtr time_offset = boost::make_shared(); + std_msgs::DurationPtr offset = boost::make_shared(); ros::Time time_ref( time_offset/1000, // t_sec time_offset*1000); // t_nsec - time_offset->source = time_ref_source; - time_offset->time_ref = time_ref; - time_offset->header.frame_id = time_ref_source; - time_offset->header.stamp = ros::Time::now(); + offset->data = time_ref; - time_offset_pub.publish(time_offset); + time_offset_pub.publish(offset); } @@ -160,5 +158,4 @@ class SystemTimePlugin : public MavRosPlugin { }; // namespace mavplugin -PLUGINLIB_EXPORT_CLASS(mavplugin::SystemTimePlugin, mavplugin::MavRosPlugin) - +PLUGINLIB_EXPORT_CLASS(mavplugin::SystemTimePlugin, mavplugin::MavRosPlugin) \ No newline at end of file From 50d3e868c049c5ad6729f2368893091b89339558 Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Tue, 7 Oct 2014 08:39:55 +0530 Subject: [PATCH 12/13] Fix --- mavros/src/plugins/sys_time.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavros/src/plugins/sys_time.cpp b/mavros/src/plugins/sys_time.cpp index 7bee8f667..11040ecbc 100644 --- a/mavros/src/plugins/sys_time.cpp +++ b/mavros/src/plugins/sys_time.cpp @@ -131,7 +131,7 @@ class SystemTimePlugin : public MavRosPlugin { //offset publisher std_msgs::DurationPtr offset = boost::make_shared(); - ros::Time time_ref( + ros::Duration time_ref( time_offset/1000, // t_sec time_offset*1000); // t_nsec From fd566e121aa9c59c77d52697c1139e8beba14056 Mon Sep 17 00:00:00 2001 From: mhkabir Date: Tue, 7 Oct 2014 08:52:26 +0530 Subject: [PATCH 13/13] Nanosecond fix --- mavros/src/plugins/sys_time.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mavros/src/plugins/sys_time.cpp b/mavros/src/plugins/sys_time.cpp index 11040ecbc..5f1c15568 100644 --- a/mavros/src/plugins/sys_time.cpp +++ b/mavros/src/plugins/sys_time.cpp @@ -132,8 +132,8 @@ class SystemTimePlugin : public MavRosPlugin { //offset publisher std_msgs::DurationPtr offset = boost::make_shared(); ros::Duration time_ref( - time_offset/1000, // t_sec - time_offset*1000); // t_nsec + time_offset / 1000, // t_sec + (time_offset % 1000) * 1000000); // t_nsec offset->data = time_ref;