sys_time.cpp
Go to the documentation of this file.
1 
9 /*
10  * Copyright 2014,2015,2016,2017 Vladimir Ermakov, M.H.Kabir.
11  *
12  * This file is part of the mavros package and subject to the license terms
13  * in the top-level LICENSE file of the mavros repository.
14  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
15  */
16 
17 #include <mavros/mavros_plugin.h>
18 
19 #include <sensor_msgs/TimeReference.h>
20 #include <std_msgs/Duration.h>
21 #include <mavros_msgs/TimesyncStatus.h>
22 
23 namespace mavros {
24 namespace std_plugins {
31 {
32 public:
33  TimeSyncStatus(const std::string &name, size_t win_size) :
35  window_size_(win_size),
36  min_freq_(0.01),
37  max_freq_(10),
38  tolerance_(0.1),
39  times_(win_size),
40  seq_nums_(win_size),
41  last_rtt(0),
42  rtt_sum(0),
43  last_remote_ts(0),
44  offset(0)
45  {
46  clear();
47  }
48 
49  void clear()
50  {
51  std::lock_guard<std::mutex> lock(mutex);
52 
53  ros::Time curtime = ros::Time::now();
54  count_ = 0;
55  rtt_sum = 0;
56 
57  for (int i = 0; i < window_size_; i++)
58  {
59  times_[i] = curtime;
60  seq_nums_[i] = count_;
61  }
62 
63  hist_indx_ = 0;
64  }
65 
66  void tick(int64_t rtt_ns, uint64_t remote_timestamp_ns, int64_t time_offset_ns)
67  {
68  std::lock_guard<std::mutex> lock(mutex);
69 
70  count_++;
71  last_rtt = rtt_ns;
72  rtt_sum += rtt_ns;
73  last_remote_ts = remote_timestamp_ns;
74  offset = time_offset_ns;
75  }
76 
77  void set_timestamp(uint64_t remote_timestamp_ns)
78  {
79  std::lock_guard<std::mutex> lock(mutex);
80  last_remote_ts = remote_timestamp_ns;
81  }
82 
84  {
85  std::lock_guard<std::mutex> lock(mutex);
86 
87  ros::Time curtime = ros::Time::now();
88  int curseq = count_;
89  int events = curseq - seq_nums_[hist_indx_];
90  double window = (curtime - times_[hist_indx_]).toSec();
91  double freq = events / window;
92  seq_nums_[hist_indx_] = curseq;
93  times_[hist_indx_] = curtime;
95 
96  if (events == 0) {
97  stat.summary(2, "No events recorded.");
98  }
99  else if (freq < min_freq_ * (1 - tolerance_)) {
100  stat.summary(1, "Frequency too low.");
101  }
102  else if (freq > max_freq_ * (1 + tolerance_)) {
103  stat.summary(1, "Frequency too high.");
104  }
105  else {
106  stat.summary(0, "Normal");
107  }
108 
109  stat.addf("Timesyncs since startup", "%d", count_);
110  stat.addf("Frequency (Hz)", "%f", freq);
111  stat.addf("Last RTT (ms)", "%0.6f", last_rtt / 1e6);
112  stat.addf("Mean RTT (ms)", "%0.6f", (count_) ? rtt_sum / count_ / 1e6 : 0.0);
113  stat.addf("Last remote time (s)", "%0.9f", last_remote_ts / 1e9);
114  stat.addf("Estimated time offset (s)", "%0.9f", offset / 1e9);
115  }
116 
117 private:
118  int count_;
119  std::vector<ros::Time> times_;
120  std::vector<int> seq_nums_;
123  const size_t window_size_;
124  const double min_freq_;
125  const double max_freq_;
126  const double tolerance_;
127  int64_t last_rtt;
128  int64_t rtt_sum;
129  uint64_t last_remote_ts;
130  int64_t offset;
131 };
132 
133 
138 public:
139  SystemTimePlugin() : PluginBase(),
140  nh("~"),
141  dt_diag("Time Sync", 10),
142  time_offset(0.0),
143  time_skew(0.0),
144  sequence(0),
145  filter_alpha(0),
146  filter_beta(0),
147  high_rtt_count(0),
148  high_deviation_count(0)
149  { }
150 
152 
153  void initialize(UAS &uas_) override
154  {
155  PluginBase::initialize(uas_);
156 
157  double conn_system_time_d;
158  double conn_timesync_d;
159  std::string ts_mode_str;
160 
161  ros::Duration conn_system_time;
162  ros::Duration conn_timesync;
163 
164  if (nh.getParam("conn/system_time_rate", conn_system_time_d) && conn_system_time_d != 0.0) {
165  conn_system_time = ros::Duration(ros::Rate(conn_system_time_d));
166  }
167 
168  if (nh.getParam("conn/timesync_rate", conn_timesync_d) && conn_timesync_d != 0.0) {
169  conn_timesync = ros::Duration(ros::Rate(conn_timesync_d));
170  }
171 
172  nh.param<std::string>("time/time_ref_source", time_ref_source, "fcu");
173  nh.param<std::string>("time/timesync_mode", ts_mode_str, "MAVLINK");
174 
175  // Filter gains
176  //
177  // Alpha : Used to smooth the overall clock offset estimate. Smaller values will lead
178  // to a smoother estimate, but track time drift more slowly, introducing a bias
179  // in the estimate. Larger values will cause low-amplitude oscillations.
180  //
181  // Beta : Used to smooth the clock skew estimate. Smaller values will lead to a
182  // tighter estimation of the skew (derivative), but will negatively affect how fast the
183  // filter reacts to clock skewing (e.g cause by temperature changes to the oscillator).
184  // Larger values will cause large-amplitude oscillations.
185  nh.param("time/timesync_alpha_initial", filter_alpha_initial, 0.05f);
186  nh.param("time/timesync_beta_initial", filter_beta_initial, 0.05f);
187  nh.param("time/timesync_alpha_final", filter_alpha_final, 0.003f);
188  nh.param("time/timesync_beta_final", filter_beta_final, 0.003f);
189  filter_alpha = filter_alpha_initial;
190  filter_beta = filter_beta_initial;
191 
192  // Filter gain scheduling
193  //
194  // The filter interpolates between the initial and final gains while the number of
195  // exhanged timesync packets is less than convergence_window. A lower value will
196  // allow the timesync to converge faster, but with potentially less accurate initial
197  // offset and skew estimates.
198  nh.param("time/convergence_window", convergence_window, 500);
199 
200  // Outlier rejection and filter reset
201  //
202  // Samples with round-trip time higher than max_rtt_sample are not used to update the filter.
203  // More than max_consecutive_high_rtt number of such events in a row will throw a warning
204  // but not reset the filter.
205  // Samples whose calculated clock offset is more than max_deviation_sample off from the current
206  // estimate are not used to update the filter. More than max_consecutive_high_deviation number
207  // of such events in a row will reset the filter. This usually happens only due to a time jump
208  // on the remote system.
209  nh.param("time/max_rtt_sample", max_rtt_sample, 10); // in ms
210  nh.param("time/max_deviation_sample", max_deviation_sample, 100); // in ms
211  nh.param("time/max_consecutive_high_rtt", max_cons_high_rtt, 5);
212  nh.param("time/max_consecutive_high_deviation", max_cons_high_deviation, 5);
213 
214  // Set timesync mode
215  auto ts_mode = utils::timesync_mode_from_str(ts_mode_str);
216  m_uas->set_timesync_mode(ts_mode);
217  ROS_INFO_STREAM_NAMED("time", "TM: Timesync mode: " << utils::to_string(ts_mode));
218 
219  time_ref_pub = nh.advertise<sensor_msgs::TimeReference>("time_reference", 10);
220 
221  timesync_status_pub = nh.advertise<mavros_msgs::TimesyncStatus>("timesync_status", 10);
222 
223  // timer for sending system time messages
224  if (!conn_system_time.isZero()) {
225  sys_time_timer = nh.createTimer(conn_system_time,
227  sys_time_timer.start();
228  }
229 
230  // timer for sending timesync messages
231  if (!conn_timesync.isZero() && !(ts_mode == TSM::NONE || ts_mode == TSM::PASSTHROUGH)) {
232  // enable timesync diag only if that feature enabled
233  UAS_DIAG(m_uas).add(dt_diag);
234 
235  timesync_timer = nh.createTimer(conn_timesync,
237  timesync_timer.start();
238  }
239  }
240 
242  {
243  return {
245  make_handler(&SystemTimePlugin::handle_timesync),
246  };
247  }
248 
249 private:
253 
256 
258 
259  std::string time_ref_source;
260 
261  // Estimated statistics
262  double time_offset;
263  double time_skew;
264 
265  // Filter parameters
266  uint32_t sequence;
267  double filter_alpha;
268  double filter_beta;
269 
270  // Filter settings
276 
277  // Outlier rejection
284 
285  void handle_system_time(const mavlink::mavlink_message_t *msg, mavlink::common::msg::SYSTEM_TIME &mtime)
286  {
287  // date -d @1234567890: Sat Feb 14 02:31:30 MSK 2009
288  const bool fcu_time_valid = mtime.time_unix_usec > 1234567890ULL * 1000000;
289 
290  if (fcu_time_valid) {
291  // continious publish for ntpd
292  auto time_unix = boost::make_shared<sensor_msgs::TimeReference>();
293  ros::Time time_ref(
294  mtime.time_unix_usec / 1000000, // t_sec
295  (mtime.time_unix_usec % 1000000) * 1000); // t_nsec
296 
297  time_unix->header.stamp = ros::Time::now();
298  time_unix->time_ref = time_ref;
299  time_unix->source = time_ref_source;
300 
301  time_ref_pub.publish(time_unix);
302  }
303  else {
304  ROS_WARN_THROTTLE_NAMED(60, "time", "TM: Wrong FCU time.");
305  }
306  }
307 
308  void handle_timesync(const mavlink::mavlink_message_t *msg, mavlink::common::msg::TIMESYNC &tsync)
309  {
310  uint64_t now_ns = ros::Time::now().toNSec();
311 
312  if (tsync.tc1 == 0) {
313  send_timesync_msg(now_ns, tsync.ts1);
314  return;
315  }
316  else if (tsync.tc1 > 0) {
317  // Time offset between this system and the remote system is calculated assuming RTT for
318  // the timesync packet is roughly equal both ways.
319  add_timesync_observation((tsync.ts1 + now_ns - tsync.tc1 * 2) / 2, tsync.ts1, tsync.tc1);
320  }
321  }
322 
323  void sys_time_cb(const ros::TimerEvent &event)
324  {
325  // For filesystem only
326  uint64_t time_unix_usec = ros::Time::now().toNSec() / 1000; // nano -> micro
327 
328  mavlink::common::msg::SYSTEM_TIME mtime {};
329  mtime.time_unix_usec = time_unix_usec;
330 
331  UAS_FCU(m_uas)->send_message_ignore_drop(mtime);
332  }
333 
334  void timesync_cb(const ros::TimerEvent &event)
335  {
336  auto ts_mode = m_uas->get_timesync_mode();
337  if (ts_mode == TSM::MAVLINK) {
338  send_timesync_msg(0, ros::Time::now().toNSec());
339  } else if (ts_mode == TSM::ONBOARD) {
340  // Calculate offset between CLOCK_REALTIME (ros::WallTime) and CLOCK_MONOTONIC
341  uint64_t realtime_now_ns = ros::Time::now().toNSec();
342  uint64_t monotonic_now_ns = get_monotonic_now();
343 
344  add_timesync_observation(realtime_now_ns - monotonic_now_ns, realtime_now_ns, monotonic_now_ns);
345  }
346  }
347 
348  void send_timesync_msg(uint64_t tc1, uint64_t ts1)
349  {
350  mavlink::common::msg::TIMESYNC tsync {};
351  tsync.tc1 = tc1;
352  tsync.ts1 = ts1;
353 
354  UAS_FCU(m_uas)->send_message_ignore_drop(tsync);
355  }
356 
357  void add_timesync_observation(int64_t offset_ns, uint64_t local_time_ns, uint64_t remote_time_ns)
358  {
359  uint64_t now_ns = ros::Time::now().toNSec();
360 
361  // Calculate the round trip time (RTT) it took the timesync packet to bounce back to us from remote system
362  uint64_t rtt_ns = now_ns - local_time_ns;
363 
364  // Calculate the difference of this sample from the current estimate
365  uint64_t deviation = llabs(int64_t(time_offset) - offset_ns);
366 
367  if (rtt_ns < max_rtt_sample * 1000000ULL) { // Only use samples with low RTT
368  if (sync_converged() && (deviation > max_deviation_sample * 1000000ULL)) {
369  // Increment the counter if we have a good estimate and are getting samples far from the estimate
370  high_deviation_count++;
371 
372  // We reset the filter if we received consecutive samples which violate our present estimate.
373  // This is most likely due to a time jump on the offboard system.
374  if (high_deviation_count > max_cons_high_deviation) {
375  ROS_ERROR_NAMED("time", "TM : Time jump detected. Resetting time synchroniser.");
376 
377  // Reset the filter
378  reset_filter();
379 
380  // Reset diagnostics
381  dt_diag.clear();
382  dt_diag.set_timestamp(remote_time_ns);
383  }
384  } else {
385  // Filter gain scheduling
386  if (!sync_converged()) {
387  // Interpolate with a sigmoid function
388  float progress = float(sequence) / convergence_window;
389  float p = 1.0f - expf(0.5f * (1.0f - 1.0f / (1.0f - progress)));
390  filter_alpha = p * filter_alpha_final + (1.0f - p) * filter_alpha_initial;
391  filter_beta = p * filter_beta_final + (1.0f - p) * filter_beta_initial;
392  } else {
393  filter_alpha = filter_alpha_final;
394  filter_beta = filter_beta_final;
395  }
396 
397  // Perform filter update
398  add_sample(offset_ns);
399 
400  // Save time offset for other components to use
401  m_uas->set_time_offset(sync_converged() ? time_offset : 0);
402 
403  // Increment sequence counter after filter update
404  sequence++;
405 
406  // Reset high deviation count after filter update
407  high_deviation_count = 0;
408 
409  // Reset high RTT count after filter update
410  high_rtt_count = 0;
411  }
412  } else {
413  // Increment counter if round trip time is too high for accurate timesync
414  high_rtt_count++;
415 
416  if (high_rtt_count > max_cons_high_rtt) {
417  // Issue a warning to the user if the RTT is constantly high
418  ROS_WARN_NAMED("time", "TM : RTT too high for timesync: %0.2f ms.", rtt_ns / 1000000.0);
419 
420  // Reset counter
421  high_rtt_count = 0;
422  }
423  }
424 
425  // Publish timesync status
426  auto timesync_status = boost::make_shared<mavros_msgs::TimesyncStatus>();
427 
428  timesync_status->header.stamp = ros::Time::now();
429  timesync_status->remote_timestamp_ns = remote_time_ns;
430  timesync_status->observed_offset_ns = offset_ns;
431  timesync_status->estimated_offset_ns = time_offset;
432  timesync_status->round_trip_time_ms = float(rtt_ns / 1000000.0);
433 
434  timesync_status_pub.publish(timesync_status);
435 
436  // Update diagnostics
437  dt_diag.tick(rtt_ns, remote_time_ns, time_offset);
438  }
439 
440  void add_sample(int64_t offset_ns)
441  {
442  /* Online exponential smoothing filter. The derivative of the estimate is also
443  * estimated in order to produce an estimate without steady state lag:
444  * https://en.wikipedia.org/wiki/Exponential_smoothing#Double_exponential_smoothing
445  */
446 
447  double time_offset_prev = time_offset;
448 
449  if (sequence == 0) { // First offset sample
450  time_offset = offset_ns;
451  } else {
452  // Update the clock offset estimate
453  time_offset = filter_alpha * offset_ns + (1.0 - filter_alpha) * (time_offset + time_skew);
454 
455  // Update the clock skew estimate
456  time_skew = filter_beta * (time_offset - time_offset_prev) + (1.0 - filter_beta) * time_skew;
457  }
458  }
459 
461  {
462  // Do a full reset of all statistics and parameters
463  sequence = 0;
464  time_offset = 0.0;
465  time_skew = 0.0;
466  filter_alpha = filter_alpha_initial;
467  filter_beta = filter_beta_initial;
468  high_deviation_count = 0;
469  high_rtt_count = 0;
470  }
471 
472  inline bool sync_converged()
473  {
474  return sequence >= convergence_window;
475  }
476 
477  uint64_t get_monotonic_now(void)
478  {
479  struct timespec spec;
480  clock_gettime(CLOCK_MONOTONIC, &spec);
481 
482  return spec.tv_sec * 1000000000ULL + spec.tv_nsec;
483  }
484 };
485 } // namespace std_plugins
486 } // namespace mavros
487 
void timesync_cb(const ros::TimerEvent &event)
Definition: sys_time.cpp:334
MAVROS Plugin base class.
Definition: mavros_plugin.h:36
#define ROS_WARN_THROTTLE_NAMED(rate, name,...)
void publish(const boost::shared_ptr< M > &message) const
#define ROS_WARN_NAMED(name,...)
utils::timesync_mode timesync_mode
Definition: mavros_uas.h:75
void summary(unsigned char lvl, const std::string s)
MAVROS Plugin base.
std::vector< ros::Time > times_
Definition: sys_time.cpp:119
void add_sample(int64_t offset_ns)
Definition: sys_time.cpp:440
#define UAS_DIAG(uasobjptr)
helper accessor to diagnostic updater
Definition: mavros_uas.h:48
#define ROS_INFO_STREAM_NAMED(name, args)
void set_timestamp(uint64_t remote_timestamp_ns)
Definition: sys_time.cpp:77
void addf(const std::string &key, const char *format,...)
void send_timesync_msg(uint64_t tc1, uint64_t ts1)
Definition: sys_time.cpp:348
void sys_time_cb(const ros::TimerEvent &event)
Definition: sys_time.cpp:323
UAS for plugins.
Definition: mavros_uas.h:67
Subscriptions get_subscriptions() override
Return vector of MAVLink message subscriptions (handlers)
Definition: sys_time.cpp:241
#define UAS_FCU(uasobjptr)
helper accessor to FCU link interface
Definition: mavros_uas.h:42
void initialize(UAS &uas_) override
Plugin initializer.
Definition: sys_time.cpp:153
std::string to_string(timesync_mode e)
uint64_t toNSec() const
void run(diagnostic_updater::DiagnosticStatusWrapper &stat)
Definition: sys_time.cpp:83
DiagnosticTask(const std::string name)
timesync_mode timesync_mode_from_str(const std::string &mode)
Retrieve timesync mode from name.
std::vector< HandlerInfo > Subscriptions
Subscriptions vector.
Definition: mavros_plugin.h:47
void add_timesync_observation(int64_t offset_ns, uint64_t local_time_ns, uint64_t remote_time_ns)
Definition: sys_time.cpp:357
void tick(int64_t rtt_ns, uint64_t remote_timestamp_ns, int64_t time_offset_ns)
Definition: sys_time.cpp:66
#define ROS_ERROR_NAMED(name,...)
static Time now()
void handle_timesync(const mavlink::mavlink_message_t *msg, mavlink::common::msg::TIMESYNC &tsync)
Definition: sys_time.cpp:308
void handle_system_time(const mavlink::mavlink_message_t *msg, mavlink::common::msg::SYSTEM_TIME &mtime)
Definition: sys_time.cpp:285
TimeSyncStatus(const std::string &name, size_t win_size)
Definition: sys_time.cpp:33
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
std::recursive_mutex mutex


mavros
Author(s): Vladimir Ermakov
autogenerated on Tue Jun 1 2021 02:36:26