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
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.
+
diff --git a/mavros/src/plugins/gps.cpp b/mavros/src/plugins/gps.cpp
index c7fa2bb8b..9df1d85c0 100644
--- a/mavros/src/plugins/gps.cpp
+++ b/mavros/src/plugins/gps.cpp
@@ -128,7 +128,6 @@ class GPSPlugin : public MavRosPlugin {
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),
};
}
@@ -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,
diff --git a/mavros/src/plugins/sys_time.cpp b/mavros/src/plugins/sys_time.cpp
new file mode 100644
index 000000000..5f1c15568
--- /dev/null
+++ b/mavros/src/plugins/sys_time.cpp
@@ -0,0 +1,161 @@
+/**
+ * @brief System Time plugin
+ * @file sys_time.cpp
+ * @author M.H.Kabir mhkabir98@gmail.com>
+ *
+ * @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
+
+#include
+#include
+
+namespace mavplugin {
+
+class SystemTimePlugin : public MavRosPlugin {
+public:
+ SystemTimePlugin():
+ uas(nullptr)
+ {};
+
+ void initialize(UAS &uas_,
+ ros::NodeHandle &nh,
+ diagnostic_updater::Updater &diag_updater)
+ {
+
+ uas = &uas_;
+
+ double conn_system_time_d;
+
+ 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),
+ &SystemTimePlugin::sys_time_cb, this);
+ sys_time_timer.start();
+ }
+
+ nh.param("frame_id", frame_id, "fcu");
+ 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);
+ }
+
+
+ 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:
+ 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;
+
+ std::string frame_id;
+ std::string time_ref_source;
+
+ 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);
+
+ 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.time_unix_usec/1000) > 1293840000;
+ ros_unix_valid = (time_unix_usec/1000) > 1293840000;
+
+ 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;
+ }
+ else
+ {
+ time_offset = (time_offset + ((time_unix_usec/1000) - mtime.time_boot_ms))/2;
+ }
+
+ 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
+ std_msgs::DurationPtr offset = boost::make_shared();
+ ros::Duration time_ref(
+ time_offset / 1000, // t_sec
+ (time_offset % 1000) * 1000000); // t_nsec
+
+ offset->data = time_ref;
+
+ time_offset_pub.publish(offset);
+
+ }
+
+ void sys_time_cb(const ros::TimerEvent &event) {
+ mavlink_message_t msg;
+
+ time_unix_usec = ros::Time::now().toNSec() / 1000; //nano -> micro
+
+ mavlink_msg_system_time_pack_chan(UAS_PACK_CHAN(uas), &msg,
+ time_unix_usec,
+ 0
+ );
+ UAS_FCU(uas)->send_message(&msg);
+
+ }
+
+};
+
+}; // namespace mavplugin
+
+PLUGINLIB_EXPORT_CLASS(mavplugin::SystemTimePlugin, mavplugin::MavRosPlugin)
\ No newline at end of file