plugin: sys_time: Fix code style.

Also reduce class variables count (most not used outside the method).

Issue #156.

mavros/src/plugins/sys_time.cpp

@@ -1,7 +1,7 @@
 /**
  * @brief System Time plugin
  * @file sys_time.cpp
- * @author M.H.Kabir mhkabir98@gmail.com>
+ * @author M.H.Kabir <mhkabir98@gmail.com>
  *
  * @addtogroup plugin
  * @{
@@ -35,38 +35,36 @@ namespace mavplugin {
 class SystemTimePlugin : public MavRosPlugin {
 public:
 	SystemTimePlugin():
-	uas(nullptr)	
-	 {};
+		uas(nullptr)
+	{};
 
 	void initialize(UAS &uas_,
 			ros::NodeHandle &nh,
 			diagnostic_updater::Updater &diag_updater)
 	{
-
-		uas = &uas_;		
-
 		double conn_system_time_d;
-
+
+		uas = &uas_;
+
 		nh.param("conn_system_time", conn_system_time_d, 0.0);
-
-		//timer for sending time sync messages
+		nh.param<std::string>("frame_id", frame_id, "fcu");
+		nh.param<std::string>("time_ref_source", time_ref_source, frame_id);
+
+		time_ref_pub = nh.advertise<sensor_msgs::TimeReference>("time_reference", 10);
+		time_offset_pub = nh.advertise<std_msgs::Duration>("time_offset", 10);
+
+		// timer for sending time sync messages
 		if (conn_system_time_d > 0.0) {
 			sys_time_timer = nh.createTimer(ros::Duration(conn_system_time_d),
 					&SystemTimePlugin::sys_time_cb, this);
 			sys_time_timer.start();
 		}
-
-		nh.param<std::string>("frame_id", frame_id, "fcu"); 
-		nh.param<std::string>("time_ref_source", time_ref_source, frame_id);		
-
-		time_ref_pub = nh.advertise<sensor_msgs::TimeReference>("time_reference", 10);
-		time_offset_pub = nh.advertise<std_msgs::Duration>("time_offset", 10);
 	}
 
-	
+
 	std::string const get_name() const {
 		return "SystemTime";
-	};
+	}
 
 	const message_map get_rx_handlers() {
 		return {
@@ -76,86 +74,72 @@ class SystemTimePlugin : public MavRosPlugin {
 
 private:
 	UAS *uas;
-
-	uint64_t time_offset;
-	bool fcu_unix_valid;
-	bool ros_unix_valid;
-	uint64_t time_unix_usec;
-
-	ros::Timer sys_time_timer;		
-
 	ros::Publisher time_ref_pub;
 	ros::Publisher time_offset_pub;
-
+	ros::Timer sys_time_timer;
+
 	std::string frame_id;
 	std::string time_ref_source;
+	uint64_t time_offset;
 
 	void handle_system_time(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
-
-		// We use 1/1/2011 as check for validity of UNIX time = 1293840000 s
-
 		mavlink_system_time_t mtime;
 		mavlink_msg_system_time_decode(msg, &mtime);
 
-		time_unix_usec = ros::Time::now().toNSec()/1000; 
+		uint64_t time_unix_ms = ros::Time::now().toNSec() / 1000000;
+		int64_t dt = (time_unix_ms - mtime.time_boot_ms) - time_offset;
 
-		int64_t dt = ((time_unix_usec/1000) - mtime.time_boot_ms) - time_offset ;
-
-		fcu_unix_valid = (mtime.time_unix_usec/1000) > 1293840000; 
-		ros_unix_valid = (time_unix_usec/1000) > 1293840000;
+		// We use 1/1/2011 as check for validity of UNIX time = 1293840000 s
+		bool fcu_unix_valid = (mtime.time_unix_usec / 1000) > 1293840000;
+		bool ros_unix_valid = time_unix_ms > 1293840000;
 
-		if(dt > 2000 || dt < -2000) //2 sec
-		{
-		ROS_WARN_THROTTLE_NAMED(60, "time", "Large clock skew detected. Resyncing clocks");
-		time_offset = (time_unix_usec/1000) - mtime.time_boot_ms;
+		if (std::abs(dt) > 2000 /* milliseconds */) {
+			ROS_WARN_THROTTLE_NAMED(10, "time", "Large clock skew detected. Resyncing clocks.");
+			time_offset = time_unix_ms - mtime.time_boot_ms;
 		}
-		else
-		{
-		time_offset = (time_offset + ((time_unix_usec/1000) - mtime.time_boot_ms))/2; 
+		else {
+			time_offset = (time_offset + (time_unix_ms - mtime.time_boot_ms)) / 2;
 		}
 
-		if(fcu_unix_valid) //continious publish for ntpd
-		{
-		sensor_msgs::TimeReferencePtr 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
+		if (fcu_unix_valid) {
+			// continious publish for ntpd
+			sensor_msgs::TimeReferencePtr 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->source = time_ref_source;
-		time_unix->time_ref = time_ref;
-		time_unix->header.frame_id = time_ref_source;
-		time_unix->header.stamp = ros::Time::now();
+			time_unix->source = time_ref_source;
+			time_unix->time_ref = time_ref;
+			time_unix->header.frame_id = time_ref_source;
+			time_unix->header.stamp = ros::Time::now();
 
-		time_ref_pub.publish(time_unix);
+			time_ref_pub.publish(time_unix);
 		}
 
-		//offset publisher
+		// offset publisher
 		std_msgs::DurationPtr offset = boost::make_shared<std_msgs::Duration>();
 		ros::Duration time_ref(
-				time_offset / 1000,	// t_sec
+				time_offset / 1000,			// t_sec
 				(time_offset % 1000) * 1000000);	// t_nsec
 
 		offset->data = time_ref;
 
 		time_offset_pub.publish(offset);
-
 	}
 
 	void sys_time_cb(const ros::TimerEvent &event) {
 		mavlink_message_t msg;
 
-		time_unix_usec = ros::Time::now().toNSec() / 1000;  //nano -> micro 
-		
+		uint64_t time_unix_usec = ros::Time::now().toNSec() / 1000;  //nano -> micro
+
 		mavlink_msg_system_time_pack_chan(UAS_PACK_CHAN(uas), &msg,
-			time_unix_usec, 
-			0
-			);
+				time_unix_usec,
+				0
+				);
 		UAS_FCU(uas)->send_message(&msg);
-
 	}
-
 };
 
 }; // namespace mavplugin
 
-PLUGINLIB_EXPORT_CLASS(mavplugin::SystemTimePlugin, mavplugin::MavRosPlugin)
+PLUGINLIB_EXPORT_CLASS(mavplugin::SystemTimePlugin, mavplugin::MavRosPlugin)