plugin: sys_time: Add time syncronization diag.

Issue #156.

mavros/src/plugins/sys_time.cpp

@@ -32,11 +32,112 @@
 
 namespace mavplugin {
 
+/**
+ * Time syncronization status publisher
+ *
+ * Based on diagnistic_updater::FrequencyStatus
+ */
+class TimeSyncStatus : public diagnostic_updater::DiagnosticTask
+{
+public:
+	TimeSyncStatus(const std::string name, size_t win_size) :
+		diagnostic_updater::DiagnosticTask(name),
+		window_size_(win_size),
+		min_freq_(0.01),
+		max_freq_(10),
+		tolerance_(0.1),
+		times_(win_size),
+		seq_nums_(win_size),
+		last_dt(0),
+		dt_sum(0)
+	{
+		clear();
+	}
+
+	void clear() {
+		lock_guard lock(mutex);
+		ros::Time curtime = ros::Time::now();
+		count_ = 0;
+		dt_sum = 0;
+
+		for (int i = 0; i < window_size_; i++)
+		{
+			times_[i] = curtime;
+			seq_nums_[i] = count_;
+		}
+
+		hist_indx_ = 0;
+	}
+
+	void tick(int64_t dt, uint64_t timestamp_us) {
+		lock_guard lock(mutex);
+		count_++;
+		last_dt = dt;
+		dt_sum += dt;
+		last_ts = timestamp_us;
+	}
+
+	void set_timestamp(uint64_t timestamp_us) {
+		lock_guard lock(mutex);
+		last_ts = timestamp_us;
+	}
+
+	void run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
+		lock_guard 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("Events in window", "%d", events);
+		stat.addf("Events since startup", "%d", count_);
+		stat.addf("Duration of window (s)", "%f", window);
+		stat.addf("Actual frequency (Hz)", "%f", freq);
+		stat.addf("Last dt (ms)", "%0.3f", last_dt / 1000.0);
+		stat.addf("Mean dt (ms)", "%0.3f", (count_)? dt_sum / count_ / 1000.0 : 0.0);
+		stat.addf("Last system time (s)", "%0.6f", last_ts / 1e6);
+	}
+
+private:
+	int count_;
+	std::vector<ros::Time> times_;
+	std::vector<int> seq_nums_;
+	int hist_indx_;
+	std::recursive_mutex mutex;
+	const size_t window_size_;
+	const double min_freq_;
+	const double max_freq_;
+	const double tolerance_;
+	int64_t last_dt;
+	int64_t dt_sum;
+	uint64_t last_ts;
+};
+
+
+
 class SystemTimePlugin : public MavRosPlugin {
 public:
 	SystemTimePlugin():
 		uas(nullptr),
-		time_offset_ms(0)
+		dt_diag("Time Sync", 10),
+		time_offset_us(0)
 	{};
 
 	void initialize(UAS &uas_,
@@ -51,6 +152,8 @@ class SystemTimePlugin : public MavRosPlugin {
 		nh.param<std::string>("frame_id", frame_id, "fcu");
 		nh.param<std::string>("time_ref_source", time_ref_source, frame_id);
 
+		diag_updater.add(dt_diag);
+
 		time_ref_pub = nh.advertise<sensor_msgs::TimeReference>("time_reference", 10);
 		time_offset_pub = nh.advertise<std_msgs::Duration>("time_offset", 10);
 
@@ -78,10 +181,11 @@ class SystemTimePlugin : public MavRosPlugin {
 	ros::Publisher time_ref_pub;
 	ros::Publisher time_offset_pub;
 	ros::Timer sys_time_timer;
+	TimeSyncStatus dt_diag;
 
 	std::string frame_id;
 	std::string time_ref_source;
-	uint64_t time_offset_ms;
+	uint64_t time_offset_us;
 
 	void handle_system_time(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
 		mavlink_system_time_t mtime;
@@ -93,15 +197,18 @@ class SystemTimePlugin : public MavRosPlugin {
 		const bool fcu_time_valid = mtime.time_unix_usec > 1234567890L * 1000000;
 		const bool ros_time_valid = now_ms > 1234567890L * 1000;
 
-		int64_t offset_ms = now_ms - mtime.time_boot_ms;
-		int64_t dt = offset_ms - time_offset_ms;
-		if (std::abs(dt) > 2000 /* milliseconds */) {
-			ROS_WARN_THROTTLE_NAMED(10, "time", "TM: Large clock skew detected (%0.3f s). "
-					"Resyncing clocks.", dt / 1000.0);
-			time_offset_ms = offset_ms;
+		int64_t offset_us = (now_ms - mtime.time_boot_ms) * 1000;
+		int64_t dt = offset_us - time_offset_us;
+		if (std::abs(dt) > 2000000 /* microseconds */) {
+			ROS_WARN_THROTTLE_NAMED(10, "time", "TM: Large clock skew detected (%0.6f s). "
+					"Resyncing clocks.", dt / 1e6);
+			time_offset_us = offset_us;
+			dt_diag.clear();
+			dt_diag.set_timestamp(mtime.time_unix_usec);
 		}
 		else {
-			time_offset_ms = (time_offset_ms + offset_ms) / 2;
+			time_offset_us = (time_offset_us + offset_us) / 2;
+			dt_diag.tick(dt, mtime.time_unix_usec);
 		}
 
 		if (fcu_time_valid) {
@@ -124,12 +231,12 @@ class SystemTimePlugin : public MavRosPlugin {
 		// offset publisher
 		std_msgs::DurationPtr offset = boost::make_shared<std_msgs::Duration>();
 		ros::Duration time_ref(
-				time_offset_ms / 1000,			// t_sec
-				(time_offset_ms % 1000) * 1000000);	// t_nsec
+				time_offset_us / 1000000,		// t_sec
+				(time_offset_us % 1000000) * 1000);	// t_nsec
 
 		offset->data = time_ref;
 
-		uas->set_time_offset(time_offset_ms);
+		uas->set_time_offset(time_offset_us);
 		time_offset_pub.publish(offset);
 	}