-
Notifications
You must be signed in to change notification settings - Fork 990
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #159 from mhkabir/master
Add support for time synchronisation with FCU.
- Loading branch information
Showing
5 changed files
with
165 additions
and
42 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 <mavros/mavros_plugin.h> | ||
| #include <pluginlib/class_list_macros.h> | ||
|
|
||
| #include <sensor_msgs/TimeReference.h> | ||
| #include <std_msgs/Duration.h> | ||
|
|
||
| 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<std::string>("frame_id", frame_id, "fcu"); | ||
| nh.param<std::string>("time_ref_source", time_ref_source, frame_id); | ||
|
|
||
| time_ref_pub = nh.advertise<sensor_msgs::TimeReference>("time_reference", 10); | ||
| time_offset_pub = nh.advertise<std_msgs::Duration>("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<sensor_msgs::TimeReference>(); | ||
| 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<std_msgs::Duration>(); | ||
| 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) |