19 #include <sensor_msgs/TimeReference.h> 20 #include <std_msgs/Duration.h> 21 #include <mavros_msgs/TimesyncStatus.h> 22 #include <rosgraph_msgs/Clock.h> 25 namespace std_plugins {
52 std::lock_guard<std::mutex> lock(
mutex);
67 void tick(int64_t rtt_ns, uint64_t remote_timestamp_ns, int64_t time_offset_ns)
69 std::lock_guard<std::mutex> lock(
mutex);
80 std::lock_guard<std::mutex> lock(
mutex);
86 std::lock_guard<std::mutex> lock(
mutex);
92 double freq = events / window;
98 stat.
summary(2,
"No events recorded.");
101 stat.
summary(1,
"Frequency too low.");
104 stat.
summary(1,
"Frequency too high.");
110 stat.
addf(
"Timesyncs since startup",
"%d",
count_);
111 stat.
addf(
"Frequency (Hz)",
"%f", freq);
115 stat.
addf(
"Estimated time offset (s)",
"%0.9f",
offset / 1e9);
142 dt_diag(
"Time Sync", 10),
149 high_deviation_count(0)
156 PluginBase::initialize(uas_);
158 double conn_system_time_d;
159 double conn_timesync_d;
160 std::string ts_mode_str;
165 if (nh.getParam(
"conn/system_time_rate", conn_system_time_d) && conn_system_time_d != 0.0) {
169 if (nh.getParam(
"conn/timesync_rate", conn_timesync_d) && conn_timesync_d != 0.0) {
173 nh.param<std::string>(
"time/time_ref_source", time_ref_source,
"fcu");
174 nh.param<std::string>(
"time/timesync_mode", ts_mode_str,
"MAVLINK");
186 nh.param(
"time/timesync_alpha_initial", filter_alpha_initial, 0.05
f);
187 nh.param(
"time/timesync_beta_initial", filter_beta_initial, 0.05
f);
188 nh.param(
"time/timesync_alpha_final", filter_alpha_final, 0.003
f);
189 nh.param(
"time/timesync_beta_final", filter_beta_final, 0.003
f);
190 filter_alpha = filter_alpha_initial;
191 filter_beta = filter_beta_initial;
199 nh.param(
"time/convergence_window", convergence_window, 500);
210 nh.param(
"time/max_rtt_sample", max_rtt_sample, 10);
211 nh.param(
"time/max_deviation_sample", max_deviation_sample, 100);
212 nh.param(
"time/max_consecutive_high_rtt", max_cons_high_rtt, 5);
213 nh.param(
"time/max_consecutive_high_deviation", max_cons_high_deviation, 5);
217 m_uas->set_timesync_mode(ts_mode);
220 nh.param(
"time/publish_sim_time", publish_sim_time,
false);
221 if (publish_sim_time) {
222 sim_time_pub = nh.advertise<rosgraph_msgs::Clock>(
"/clock", 10);
227 time_ref_pub = nh.advertise<sensor_msgs::TimeReference>(
"time_reference", 10);
229 timesync_status_pub = nh.advertise<mavros_msgs::TimesyncStatus>(
"timesync_status", 10);
232 if (!conn_system_time.
isZero()) {
233 sys_time_timer = nh.createWallTimer(conn_system_time,
235 sys_time_timer.start();
239 if (!conn_timesync.
isZero() && !(ts_mode == TSM::NONE || ts_mode == TSM::PASSTHROUGH)) {
243 timesync_timer = nh.createWallTimer(conn_timesync,
245 timesync_timer.start();
296 void handle_system_time(
const mavlink::mavlink_message_t *msg, mavlink::common::msg::SYSTEM_TIME &mtime)
299 const bool fcu_time_valid = mtime.time_unix_usec > 1234567890ULL * 1000000;
301 if (fcu_time_valid) {
303 auto time_unix = boost::make_shared<sensor_msgs::TimeReference>();
305 mtime.time_unix_usec / 1000000,
306 (mtime.time_unix_usec % 1000000) * 1000);
309 time_unix->time_ref = time_ref;
310 time_unix->source = time_ref_source;
312 time_ref_pub.
publish(time_unix);
313 if(publish_sim_time) {
314 auto clock = boost::make_shared<rosgraph_msgs::Clock>();
315 clock->clock = time_ref;
324 void handle_timesync(
const mavlink::mavlink_message_t *msg, mavlink::common::msg::TIMESYNC &tsync)
328 if (tsync.tc1 == 0) {
329 send_timesync_msg(now_ns, tsync.ts1);
332 else if (tsync.tc1 > 0) {
335 add_timesync_observation((tsync.ts1 + now_ns - tsync.tc1 * 2) / 2, tsync.ts1, tsync.tc1);
344 mavlink::common::msg::SYSTEM_TIME
mtime {};
345 mtime.time_unix_usec = time_unix_usec;
352 auto ts_mode = m_uas->get_timesync_mode();
353 if (ts_mode == TSM::MAVLINK) {
355 }
else if (ts_mode == TSM::ONBOARD) {
358 uint64_t monotonic_now_ns = get_monotonic_now();
360 add_timesync_observation(realtime_now_ns - monotonic_now_ns, realtime_now_ns, monotonic_now_ns);
366 mavlink::common::msg::TIMESYNC tsync {};
370 UAS_FCU(m_uas)->send_message_ignore_drop(tsync);
378 uint64_t rtt_ns = now_ns - local_time_ns;
381 uint64_t deviation = llabs(int64_t(time_offset) - offset_ns);
383 if (rtt_ns < max_rtt_sample * 1000000ULL) {
384 if (sync_converged() && (deviation > max_deviation_sample * 1000000ULL)) {
386 high_deviation_count++;
390 if (high_deviation_count > max_cons_high_deviation) {
391 ROS_ERROR_NAMED(
"time",
"TM : Time jump detected. Resetting time synchroniser.");
402 if (!sync_converged()) {
404 float progress =
float(sequence) / convergence_window;
405 float p = 1.0f - expf(0.5
f * (1.0
f - 1.0
f / (1.0
f - progress)));
406 filter_alpha = p * filter_alpha_final + (1.0f - p) * filter_alpha_initial;
407 filter_beta = p * filter_beta_final + (1.0f - p) * filter_beta_initial;
409 filter_alpha = filter_alpha_final;
410 filter_beta = filter_beta_final;
414 add_sample(offset_ns);
417 m_uas->set_time_offset(sync_converged() ? time_offset : 0);
423 high_deviation_count = 0;
432 if (high_rtt_count > max_cons_high_rtt) {
434 ROS_WARN_NAMED(
"time",
"TM : RTT too high for timesync: %0.2f ms.", rtt_ns / 1000000.0);
442 auto timesync_status = boost::make_shared<mavros_msgs::TimesyncStatus>();
445 timesync_status->remote_timestamp_ns = remote_time_ns;
446 timesync_status->observed_offset_ns = offset_ns;
447 timesync_status->estimated_offset_ns = time_offset;
448 timesync_status->round_trip_time_ms =
float(rtt_ns / 1000000.0);
450 timesync_status_pub.
publish(timesync_status);
453 dt_diag.
tick(rtt_ns, remote_time_ns, time_offset);
463 double time_offset_prev = time_offset;
466 time_offset = offset_ns;
469 time_offset = filter_alpha * offset_ns + (1.0 - filter_alpha) * (time_offset + time_skew);
472 time_skew = filter_beta * (time_offset - time_offset_prev) + (1.0 - filter_beta) * time_skew;
482 filter_alpha = filter_alpha_initial;
483 filter_beta = filter_beta_initial;
484 high_deviation_count = 0;
490 return sequence >= convergence_window;
495 struct timespec spec;
496 clock_gettime(CLOCK_MONOTONIC, &spec);
498 return spec.tv_sec * 1000000000ULL + spec.tv_nsec;
MAVROS Plugin base class.
uint64_t get_monotonic_now(void)
int max_cons_high_deviation
#define ROS_WARN_THROTTLE_NAMED(period, name,...)
#define ROS_WARN_NAMED(name,...)
utils::timesync_mode timesync_mode
void summary(unsigned char lvl, const std::string s)
std::vector< ros::Time > times_
float filter_beta_initial
void add_sample(int64_t offset_ns)
ros::Publisher time_ref_pub
std::string time_ref_source
#define UAS_DIAG(uasobjptr)
helper accessor to diagnostic updater
#define ROS_INFO_STREAM_NAMED(name, args)
void set_timestamp(uint64_t remote_timestamp_ns)
void addf(const std::string &key, const char *format,...)
void send_timesync_msg(uint64_t tc1, uint64_t ts1)
void publish(const boost::shared_ptr< M > &message) const
std::vector< int > seq_nums_
Subscriptions get_subscriptions() override
Return vector of MAVLink message subscriptions (handlers)
#define UAS_FCU(uasobjptr)
helper accessor to FCU link interface
void initialize(UAS &uas_) override
Plugin initializer.
std::string to_string(timesync_mode e)
const size_t window_size_
ros::WallTimer sys_time_timer
void run(diagnostic_updater::DiagnosticStatusWrapper &stat)
DiagnosticTask(const std::string name)
ros::Publisher sim_time_pub
timesync_mode timesync_mode_from_str(const std::string &mode)
Retrieve timesync mode from name.
std::vector< HandlerInfo > Subscriptions
Subscriptions vector.
void sys_time_cb(const ros::WallTimerEvent &event)
void add_timesync_observation(int64_t offset_ns, uint64_t local_time_ns, uint64_t remote_time_ns)
void tick(int64_t rtt_ns, uint64_t remote_timestamp_ns, int64_t time_offset_ns)
ros::WallTimer timesync_timer
void timesync_cb(const ros::WallTimerEvent &event)
#define ROS_ERROR_NAMED(name,...)
ros::Publisher timesync_status_pub
float filter_alpha_initial
void handle_timesync(const mavlink::mavlink_message_t *msg, mavlink::common::msg::TIMESYNC &tsync)
void handle_system_time(const mavlink::mavlink_message_t *msg, mavlink::common::msg::SYSTEM_TIME &mtime)
TimeSyncStatus(const std::string &name, size_t win_size)
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
std::recursive_mutex mutex