From 191c6b62c8f6068a41c9213c95823a04a383c1e7 Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Thu, 31 May 2018 19:15:45 +0530 Subject: [PATCH 1/2] mavros_msgs : add timesync status message --- mavros_msgs/CMakeLists.txt | 1 + mavros_msgs/msg/TimesyncStatus.msg | 7 +++++++ 2 files changed, 8 insertions(+) create mode 100644 mavros_msgs/msg/TimesyncStatus.msg diff --git a/mavros_msgs/CMakeLists.txt b/mavros_msgs/CMakeLists.txt index 0dce5bb55..2564564c6 100644 --- a/mavros_msgs/CMakeLists.txt +++ b/mavros_msgs/CMakeLists.txt @@ -38,6 +38,7 @@ add_message_files( StatusText.msg Thrust.msg Trajectory.msg + TimesyncStatus.msg VFR_HUD.msg Vibration.msg Waypoint.msg diff --git a/mavros_msgs/msg/TimesyncStatus.msg b/mavros_msgs/msg/TimesyncStatus.msg new file mode 100644 index 000000000..dbfbf30eb --- /dev/null +++ b/mavros_msgs/msg/TimesyncStatus.msg @@ -0,0 +1,7 @@ +# Status of the MAVLink time synchroniser + +std_msgs/Header header +uint64 remote_timestamp_ns # remote system timestamp (nanoseconds) +int64 observed_offset_ns # raw time offset directly observed from this timesync packet (nanoseconds) +int64 estimated_offset_ns # smoothed time offset between companion system and Mavros (nanoseconds) +float32 round_trip_time_ms # round trip time of this timesync packet (milliseconds) \ No newline at end of file From f95707c1a3330372a20ac80394240fc218cf7ebd Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Thu, 31 May 2018 19:16:12 +0530 Subject: [PATCH 2/2] sys_time : add advanced timesync algorithm --- mavros/src/plugins/sys_time.cpp | 251 ++++++++++++++++++++++++++------ 1 file changed, 205 insertions(+), 46 deletions(-) diff --git a/mavros/src/plugins/sys_time.cpp b/mavros/src/plugins/sys_time.cpp index 3cb4c03bf..7b75103bf 100644 --- a/mavros/src/plugins/sys_time.cpp +++ b/mavros/src/plugins/sys_time.cpp @@ -18,6 +18,7 @@ #include #include +#include namespace mavros { namespace std_plugins { @@ -37,9 +38,9 @@ class TimeSyncStatus : public diagnostic_updater::DiagnosticTask tolerance_(0.1), times_(win_size), seq_nums_(win_size), - last_dt(0), - dt_sum(0), - last_ts(0), + last_rtt(0), + rtt_sum(0), + last_remote_ts(0), offset(0) { clear(); @@ -51,7 +52,7 @@ class TimeSyncStatus : public diagnostic_updater::DiagnosticTask ros::Time curtime = ros::Time::now(); count_ = 0; - dt_sum = 0; + rtt_sum = 0; for (int i = 0; i < window_size_; i++) { @@ -62,21 +63,21 @@ class TimeSyncStatus : public diagnostic_updater::DiagnosticTask hist_indx_ = 0; } - void tick(int64_t dt, uint64_t timestamp_ns, int64_t time_offset_ns) + void tick(int64_t rtt_ns, uint64_t remote_timestamp_ns, int64_t time_offset_ns) { std::lock_guard lock(mutex); count_++; - last_dt = dt; - dt_sum += dt; - last_ts = timestamp_ns; + last_rtt = rtt_ns; + rtt_sum += rtt_ns; + last_remote_ts = remote_timestamp_ns; offset = time_offset_ns; } - void set_timestamp(uint64_t timestamp_ns) + void set_timestamp(uint64_t remote_timestamp_ns) { std::lock_guard lock(mutex); - last_ts = timestamp_ns; + last_remote_ts = remote_timestamp_ns; } void run(diagnostic_updater::DiagnosticStatusWrapper &stat) @@ -107,10 +108,10 @@ class TimeSyncStatus : public diagnostic_updater::DiagnosticTask stat.addf("Timesyncs since startup", "%d", count_); stat.addf("Frequency (Hz)", "%f", freq); - stat.addf("Last dt (ms)", "%0.6f", last_dt / 1e6); - stat.addf("Mean dt (ms)", "%0.6f", (count_) ? dt_sum / count_ / 1e6 : 0.0); - stat.addf("Last system time (s)", "%0.9f", last_ts / 1e9); - stat.addf("Time offset (s)", "%0.9f", offset / 1e9); + stat.addf("Last RTT (ms)", "%0.6f", last_rtt / 1e6); + stat.addf("Mean RTT (ms)", "%0.6f", (count_) ? rtt_sum / count_ / 1e6 : 0.0); + stat.addf("Last remote time (s)", "%0.9f", last_remote_ts / 1e9); + stat.addf("Estimated time offset (s)", "%0.9f", offset / 1e9); } private: @@ -123,9 +124,9 @@ class TimeSyncStatus : public diagnostic_updater::DiagnosticTask const double min_freq_; const double max_freq_; const double tolerance_; - int64_t last_dt; - int64_t dt_sum; - uint64_t last_ts; + int64_t last_rtt; + int64_t rtt_sum; + uint64_t last_remote_ts; int64_t offset; }; @@ -138,8 +139,13 @@ class SystemTimePlugin : public plugin::PluginBase { SystemTimePlugin() : PluginBase(), nh("~"), dt_diag("Time Sync", 10), - time_offset_ns(0), - offset_avg_alpha(0) + time_offset(0.0), + time_skew(0.0), + sequence(0), + filter_alpha(0), + filter_beta(0), + high_rtt_count(0), + high_deviation_count(0) { } using TSM = UAS::timesync_mode; @@ -165,12 +171,45 @@ class SystemTimePlugin : public plugin::PluginBase { nh.param("time/time_ref_source", time_ref_source, "fcu"); nh.param("time/timesync_mode", ts_mode_str, "MAVLINK"); - nh.param("time/timesync_avg_alpha", offset_avg_alpha, 0.6); - /* - * alpha for exponential moving average. The closer alpha is to 1.0, - * the faster the moving average updates in response to new offset samples (more jitter) - * We need a significant amount of smoothing , more so for lower message rates like 1Hz - */ + + // Filter gains + // + // Alpha : Used to smooth the overall clock offset estimate. Smaller values will lead + // to a smoother estimate, but track time drift more slowly, introducing a bias + // in the estimate. Larger values will cause low-amplitude oscillations. + // + // Beta : Used to smooth the clock skew estimate. Smaller values will lead to a + // tighter estimation of the skew (derivative), but will negatively affect how fast the + // filter reacts to clock skewing (e.g cause by temperature changes to the oscillator). + // Larger values will cause large-amplitude oscillations. + nh.param("time/timesync_alpha_initial", filter_alpha_initial, 0.05f); + nh.param("time/timesync_beta_initial", filter_beta_initial, 0.05f); + nh.param("time/timesync_alpha_final", filter_alpha_final, 0.003f); + nh.param("time/timesync_beta_final", filter_beta_final, 0.003f); + filter_alpha = filter_alpha_initial; + filter_beta = filter_beta_initial; + + // Filter gain scheduling + // + // The filter interpolates between the initial and final gains while the number of + // exhanged timesync packets is less than convergence_window. A lower value will + // allow the timesync to converge faster, but with potentially less accurate initial + // offset and skew estimates. + nh.param("time/convergence_window", convergence_window, 500); + + // Outlier rejection and filter reset + // + // Samples with round-trip time higher than max_rtt_sample are not used to update the filter. + // More than max_consecutive_high_rtt number of such events in a row will throw a warning + // but not reset the filter. + // Samples whose calculated clock offset is more than max_deviation_sample off from the current + // estimate are not used to update the filter. More than max_consecutive_high_deviation number + // of such events in a row will reset the filter. This usually happens only due to a time jump + // on the remote system. + nh.param("time/max_rtt_sample", max_rtt_sample, 10); // in ms + nh.param("time/max_deviation_sample", max_deviation_sample, 100); // in ms + nh.param("time/max_consecutive_high_rtt", max_cons_high_rtt, 5); + nh.param("time/max_consecutive_high_deviation", max_cons_high_deviation, 5); // Set timesync mode auto ts_mode = utils::timesync_mode_from_str(ts_mode_str); @@ -179,6 +218,8 @@ class SystemTimePlugin : public plugin::PluginBase { time_ref_pub = nh.advertise("time_reference", 10); + timesync_status_pub = nh.advertise("timesync_status", 10); + // timer for sending system time messages if (!conn_system_time.isZero()) { sys_time_timer = nh.createTimer(conn_system_time, @@ -208,6 +249,7 @@ class SystemTimePlugin : public plugin::PluginBase { private: ros::NodeHandle nh; ros::Publisher time_ref_pub; + ros::Publisher timesync_status_pub; ros::Timer sys_time_timer; ros::Timer timesync_timer; @@ -215,8 +257,30 @@ class SystemTimePlugin : public plugin::PluginBase { TimeSyncStatus dt_diag; std::string time_ref_source; - int64_t time_offset_ns; - double offset_avg_alpha; + + // Estimated statistics + double time_offset; + double time_skew; + + // Filter parameters + uint32_t sequence; + double filter_alpha; + double filter_beta; + + // Filter settings + float filter_alpha_initial; + float filter_beta_initial; + float filter_alpha_final; + float filter_beta_final; + int convergence_window; + + // Outlier rejection + int max_rtt_sample; + int max_deviation_sample; + int max_cons_high_rtt; + int max_cons_high_deviation; + int high_rtt_count; + int high_deviation_count; void handle_system_time(const mavlink::mavlink_message_t *msg, mavlink::common::msg::SYSTEM_TIME &mtime) { @@ -250,8 +314,9 @@ class SystemTimePlugin : public plugin::PluginBase { return; } else if (tsync.tc1 > 0) { - // observation is the offset in nanoseconds - add_timesync_observation((tsync.ts1 + now_ns - tsync.tc1 * 2) / 2, tsync.tc1); + // Time offset between this system and the remote system is calculated assuming RTT for + // the timesync packet is roughly equal both ways. + add_timesync_observation((tsync.ts1 + now_ns - tsync.tc1 * 2) / 2, tsync.ts1, tsync.tc1); } } @@ -276,7 +341,7 @@ class SystemTimePlugin : public plugin::PluginBase { uint64_t realtime_now_ns = ros::Time::now().toNSec(); uint64_t monotonic_now_ns = get_monotonic_now(); - add_timesync_observation(realtime_now_ns - monotonic_now_ns, realtime_now_ns); + add_timesync_observation(realtime_now_ns - monotonic_now_ns, realtime_now_ns, monotonic_now_ns); } } @@ -289,30 +354,124 @@ class SystemTimePlugin : public plugin::PluginBase { UAS_FCU(m_uas)->send_message_ignore_drop(tsync); } - void add_timesync_observation(int64_t offset_ns, uint64_t observation_time) + void add_timesync_observation(int64_t offset_ns, uint64_t local_time_ns, uint64_t remote_time_ns) { - int64_t dt = time_offset_ns - offset_ns; + uint64_t now_ns = ros::Time::now().toNSec(); - if (std::abs(dt) > 10000000) { // 10 millisecond skew - time_offset_ns = offset_ns; // hard-set it. - m_uas->set_time_offset(time_offset_ns); + // Calculate the round trip time (RTT) it took the timesync packet to bounce back to us from remote system + uint64_t rtt_ns = now_ns - local_time_ns; + + // Calculate the difference of this sample from the current estimate + uint64_t deviation = llabs(int64_t(time_offset) - offset_ns); + + if (rtt_ns < max_rtt_sample * 1000000ULL) { // Only use samples with low RTT + if (sync_converged() && (deviation > max_deviation_sample * 1000000ULL)) { + // Increment the counter if we have a good estimate and are getting samples far from the estimate + high_deviation_count++; + + // We reset the filter if we received consecutive samples which violate our present estimate. + // This is most likely due to a time jump on the offboard system. + if (high_deviation_count > max_cons_high_deviation) { + ROS_ERROR_NAMED("time", "TM : Time jump detected. Resetting time synchroniser."); + + // Reset the filter + reset_filter(); + + // Reset diagnostics + dt_diag.clear(); + dt_diag.set_timestamp(remote_time_ns); + } + } else { + // Filter gain scheduling + if (!sync_converged()) { + // Interpolate with a sigmoid function + float progress = float(sequence) / convergence_window; + float p = 1.0f - expf(0.5f * (1.0f - 1.0f / (1.0f - progress))); + filter_alpha = p * filter_alpha_final + (1.0f - p) * filter_alpha_initial; + filter_beta = p * filter_beta_final + (1.0f - p) * filter_beta_initial; + } else { + filter_alpha = filter_alpha_final; + filter_beta = filter_beta_final; + } + + // Perform filter update + add_sample(offset_ns); + + // Save time offset for other components to use + m_uas->set_time_offset(sync_converged() ? time_offset : 0); + + // Increment sequence counter after filter update + sequence++; + + // Reset high deviation count after filter update + high_deviation_count = 0; + + // Reset high RTT count after filter update + high_rtt_count = 0; + } + } else { + // Increment counter if round trip time is too high for accurate timesync + high_rtt_count++; + + if (high_rtt_count > max_cons_high_rtt) { + // Issue a warning to the user if the RTT is constantly high + ROS_WARN_NAMED("time", "TM : RTT too high for timesync: %0.2f ms.", rtt_ns / 1000000.0); + + // Reset counter + high_rtt_count = 0; + } + } - dt_diag.clear(); - dt_diag.set_timestamp(observation_time); + // Publish timesync status + auto timesync_status = boost::make_shared(); - ROS_WARN_THROTTLE_NAMED(10, "time", "TM: Clock skew detected (%0.9f s). " - "Hard syncing clocks.", dt / 1e9); - } - else { - average_offset(offset_ns); - dt_diag.tick(dt, observation_time, time_offset_ns); - m_uas->set_time_offset(time_offset_ns); + timesync_status->header.stamp = ros::Time::now(); + timesync_status->remote_timestamp_ns = remote_time_ns; + timesync_status->observed_offset_ns = offset_ns; + timesync_status->estimated_offset_ns = time_offset; + timesync_status->round_trip_time_ms = float(rtt_ns / 1000000.0); + + timesync_status_pub.publish(timesync_status); + + // Update diagnostics + dt_diag.tick(rtt_ns, remote_time_ns, time_offset); + } + + void add_sample(int64_t offset_ns) + { + /* Online exponential smoothing filter. The derivative of the estimate is also + * estimated in order to produce an estimate without steady state lag: + * https://en.wikipedia.org/wiki/Exponential_smoothing#Double_exponential_smoothing + */ + + double time_offset_prev = time_offset; + + if (sequence == 0) { // First offset sample + time_offset = offset_ns; + } else { + // Update the clock offset estimate + time_offset = filter_alpha * offset_ns + (1.0 - filter_alpha) * (time_offset + time_skew); + + // Update the clock skew estimate + time_skew = filter_beta * (time_offset - time_offset_prev) + (1.0 - filter_beta) * time_skew; } } - inline void average_offset(int64_t offset_ns) + void reset_filter() + { + // Do a full reset of all statistics and parameters + sequence = 0; + time_offset = 0.0; + time_skew = 0.0; + filter_alpha = filter_alpha_initial; + filter_beta = filter_beta_initial; + high_deviation_count = 0; + high_rtt_count = 0; + } + + inline bool sync_converged() { - time_offset_ns = (offset_avg_alpha * offset_ns) + (1.0 - offset_avg_alpha) * time_offset_ns; + return sequence >= convergence_window; } uint64_t get_monotonic_now(void)