sys_time.cpp

Go to the documentation of this file.

 
/*
 * Copyright 2014,2015,2016,2017 Vladimir Ermakov, M.H.Kabir.
 *
 * This file is part of the mavros package and subject to the license terms
 * in the top-level LICENSE file of the mavros repository.
 * https://github.com/mavlink/mavros/tree/master/LICENSE.md
 */
 
#include <mavros/mavros_plugin.h>
 
#include <sensor_msgs/TimeReference.h>
#include <std_msgs/Duration.h>
#include <mavros_msgs/TimesyncStatus.h>
#include <rosgraph_msgs/Clock.h>
 
namespace mavros {
namespace std_plugins {
class TimeSyncStatus : public diagnostic_updater::DiagnosticTask
{
public:
        TimeSyncStatus(const std::string &name, size_t win_size) :
                diagnostic_updater::DiagnosticTask(name),
                times_(win_size),
                seq_nums_(win_size),
                window_size_(win_size),
                min_freq_(0.01),
                max_freq_(10),
                tolerance_(0.1),
                last_rtt(0),
                rtt_sum(0),
                last_remote_ts(0),
                offset(0)
        {
                clear();
        }
 
        void clear()
        {
                std::lock_guard<std::mutex> lock(mutex);
 
                ros::Time curtime = ros::Time::now();
                count_ = 0;
                rtt_sum = 0;
 
                for (size_t i = 0; i < window_size_; i++)
                {
                        times_[i] = curtime;
                        seq_nums_[i] = count_;
                }
 
                hist_indx_ = 0;
        }
 
        void tick(int64_t rtt_ns, uint64_t remote_timestamp_ns, int64_t time_offset_ns)
        {
                std::lock_guard<std::mutex> lock(mutex);
 
                count_++;
                last_rtt = rtt_ns;
                rtt_sum += rtt_ns;
                last_remote_ts = remote_timestamp_ns;
                offset = time_offset_ns;
        }
 
        void set_timestamp(uint64_t remote_timestamp_ns)
        {
                std::lock_guard<std::mutex> lock(mutex);
                last_remote_ts = remote_timestamp_ns;
        }
 
        void run(diagnostic_updater::DiagnosticStatusWrapper &stat)
        {
                std::lock_guard<std::mutex> lock(mutex);
 
                ros::Time curtime = ros::Time::now();
                int curseq = count_;
                int events = curseq - seq_nums_[hist_indx_];
                double window = (curtime - times_[hist_indx_]).toSec();
                double freq = events / window;
                seq_nums_[hist_indx_] = curseq;
                times_[hist_indx_] = curtime;
                hist_indx_ = (hist_indx_ + 1) % window_size_;
 
                if (events == 0) {
                        stat.summary(2, "No events recorded.");
                }
                else if (freq < min_freq_ * (1 - tolerance_)) {
                        stat.summary(1, "Frequency too low.");
                }
                else if (freq > max_freq_ * (1 + tolerance_)) {
                        stat.summary(1, "Frequency too high.");
                }
                else {
                        stat.summary(0, "Normal");
                }
 
                stat.addf("Timesyncs since startup", "%d", count_);
                stat.addf("Frequency (Hz)", "%f", freq);
                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:
        int count_;
        std::vector<ros::Time> times_;
        std::vector<int> seq_nums_;
        int hist_indx_;
        std::mutex mutex;
        const size_t window_size_;
        const double min_freq_;
        const double max_freq_;
        const double tolerance_;
        int64_t last_rtt;
        int64_t rtt_sum;
        uint64_t last_remote_ts;
        int64_t offset;
};
 
 
class SystemTimePlugin : public plugin::PluginBase {
public:
        SystemTimePlugin() : PluginBase(),
                nh("~"),
                dt_diag("Time Sync", 10),
                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;
 
        void initialize(UAS &uas_) override
        {
                PluginBase::initialize(uas_);
 
                double conn_system_time_d;
                double conn_timesync_d;
                std::string ts_mode_str;
 
                ros::WallDuration conn_system_time;
                ros::WallDuration conn_timesync;
 
                if (nh.getParam("conn/system_time_rate", conn_system_time_d) && conn_system_time_d != 0.0) {
                        conn_system_time = ros::WallDuration(ros::Rate(conn_system_time_d));
                }
 
                if (nh.getParam("conn/timesync_rate", conn_timesync_d) && conn_timesync_d != 0.0) {
                        conn_timesync = ros::WallDuration(ros::Rate(conn_timesync_d));
                }
 
                nh.param<std::string>("time/time_ref_source", time_ref_source, "fcu");
                nh.param<std::string>("time/timesync_mode", ts_mode_str, "MAVLINK");
 
                // 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);
                m_uas->set_timesync_mode(ts_mode);
                ROS_INFO_STREAM_NAMED("time", "TM: Timesync mode: " << utils::to_string(ts_mode));
 
                nh.param("time/publish_sim_time", publish_sim_time, false);
                if (publish_sim_time) {
                        sim_time_pub = nh.advertise<rosgraph_msgs::Clock>("/clock", 10);
                        ROS_INFO_STREAM_NAMED("time", "TM: Publishing sim time");
                } else {
                        ROS_INFO_STREAM_NAMED("time", "TM: Not publishing sim time");
                }
                time_ref_pub = nh.advertise<sensor_msgs::TimeReference>("time_reference", 10);
 
                timesync_status_pub = nh.advertise<mavros_msgs::TimesyncStatus>("timesync_status", 10);
 
                // timer for sending system time messages
                if (!conn_system_time.isZero()) {
                        sys_time_timer = nh.createWallTimer(conn_system_time,
                                                &SystemTimePlugin::sys_time_cb, this);
                        sys_time_timer.start();
                }
 
                // timer for sending timesync messages
                if (!conn_timesync.isZero() && !(ts_mode == TSM::NONE || ts_mode == TSM::PASSTHROUGH)) {
                        // enable timesync diag only if that feature enabled
                        UAS_DIAG(m_uas).add(dt_diag);
 
                        timesync_timer = nh.createWallTimer(conn_timesync,
                                                &SystemTimePlugin::timesync_cb, this);
                        timesync_timer.start();
                }
        }
 
        Subscriptions get_subscriptions() override
        {
                return {
                               make_handler(&SystemTimePlugin::handle_system_time),
                               make_handler(&SystemTimePlugin::handle_timesync),
                };
        }
 
private:
        ros::NodeHandle nh;
        ros::Publisher sim_time_pub;
        ros::Publisher time_ref_pub;
        ros::Publisher timesync_status_pub;
 
        ros::WallTimer sys_time_timer;
        ros::WallTimer timesync_timer;
 
        TimeSyncStatus dt_diag;
 
        std::string time_ref_source;
 
        // 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;
 
        bool publish_sim_time;
 
        void handle_system_time(const mavlink::mavlink_message_t *msg, mavlink::common::msg::SYSTEM_TIME &mtime)
        {
                // date -d @1234567890: Sat Feb 14 02:31:30 MSK 2009
                const bool fcu_time_valid = mtime.time_unix_usec > 1234567890ULL * 1000000;
 
                if (fcu_time_valid) {
                        // continious publish for ntpd
                        auto 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->header.stamp = ros::Time::now();
                        time_unix->time_ref = time_ref;
                        time_unix->source = time_ref_source;
 
                        time_ref_pub.publish(time_unix);
                        if(publish_sim_time) {
                                auto clock = boost::make_shared<rosgraph_msgs::Clock>();
                                clock->clock = time_ref;
                                sim_time_pub.publish(clock);
                        }
                }
                else {
                        ROS_WARN_THROTTLE_NAMED(60, "time", "TM: Wrong FCU time.");
                }
        }
 
        void handle_timesync(const mavlink::mavlink_message_t *msg, mavlink::common::msg::TIMESYNC &tsync)
        {
                uint64_t now_ns = ros::Time::now().toNSec();
 
                if (tsync.tc1 == 0) {
                        send_timesync_msg(now_ns, tsync.ts1);
                        return;
                }
                else if (tsync.tc1 > 0) {
                        // 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);
                }
        }
 
        void sys_time_cb(const ros::WallTimerEvent &event)
        {
                // For filesystem only
                uint64_t time_unix_usec = ros::Time::now().toNSec() / 1000;     // nano -> micro
 
                mavlink::common::msg::SYSTEM_TIME mtime {};
                mtime.time_unix_usec = time_unix_usec;
 
                UAS_FCU(m_uas)->send_message_ignore_drop(mtime);
        }
 
        void timesync_cb(const ros::WallTimerEvent &event)
        {
                auto ts_mode = m_uas->get_timesync_mode();
                if (ts_mode == TSM::MAVLINK) {
                        send_timesync_msg(0, ros::Time::now().toNSec());
                } else if (ts_mode == TSM::ONBOARD) {
                        // Calculate offset between CLOCK_REALTIME (ros::WallTime) and CLOCK_MONOTONIC
                        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, monotonic_now_ns);
                }
        }
 
        void send_timesync_msg(uint64_t tc1, uint64_t ts1)
        {
                mavlink::common::msg::TIMESYNC tsync {};
                tsync.tc1 = tc1;
                tsync.ts1 = ts1;
 
                UAS_FCU(m_uas)->send_message_ignore_drop(tsync);
        }
 
        void add_timesync_observation(int64_t offset_ns, uint64_t local_time_ns, uint64_t remote_time_ns)
        {
                uint64_t now_ns = ros::Time::now().toNSec();
 
                // 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;
                        }
                }
 
                // Publish timesync status
                auto timesync_status = boost::make_shared<mavros_msgs::TimesyncStatus>();
 
                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;
                }
        }
 
        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()
        {
                return sequence >= convergence_window;
        }
 
        uint64_t get_monotonic_now(void)
        {
                struct timespec spec;
                clock_gettime(CLOCK_MONOTONIC, &spec);
 
                return spec.tv_sec * 1000000000ULL + spec.tv_nsec;
        }
};
}       // namespace std_plugins
}       // namespace mavros
 
#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(mavros::std_plugins::SystemTimePlugin, mavros::plugin::PluginBase)