From a3e174626cb2a8847277264298ba9e4a90f54961 Mon Sep 17 00:00:00 2001
From: Thomas Timm Andersen <thomas.timm.dk@gmail.com>
Date: Thu, 3 Dec 2015 11:29:21 +0100
Subject: [PATCH] Added verification of RT message length for firmware versions
 1.6 to 3.2

---
 src/robot_state_RT.cpp | 36 +++++++++++++++++++++++-------------
 src/ur_driver.cpp      |  1 +
 2 files changed, 24 insertions(+), 13 deletions(-)

diff --git a/src/robot_state_RT.cpp b/src/robot_state_RT.cpp
index e3c1dec..2074165 100644
--- a/src/robot_state_RT.cpp
+++ b/src/robot_state_RT.cpp
@@ -60,7 +60,6 @@ RobotStateRT::~RobotStateRT() {
 	pMsg_cond_->notify_all();
 }
 
-
 void RobotStateRT::setDataPublished() {
 	data_published_ = false;
 }
@@ -75,7 +74,6 @@ bool RobotStateRT::getControllerUpdated() {
 	return controller_updated_;
 }
 
-
 double RobotStateRT::ntohd(uint64_t nf) {
 	double x;
 	nf = be64toh(nf);
@@ -321,20 +319,32 @@ void RobotStateRT::unpack(uint8_t * buf) {
 
 	offset += sizeof(len);
 	len = ntohl(len);
-	if (version_ > 3. & version_ < 3.1 & len != 1044) {
-		//In 3.0, every 4th? package is malformed...?
-		//printf("Len: %i\n", len);
+
+	//Check the correct message length is received
+	bool len_good = true;
+	if (version_ >= 1.6 && version_ < 1.7) { //v1.6
+		if (len != 756)
+			len_good = false;
+	} else if (version_ >= 1.7 && version_ < 1.8) { //v1.7
+		if (len != 764)
+			len_good = false;
+	} else if (version_ >= 1.8 && version_ < 1.9) { //v1.8
+		if (len != 812)
+			len_good = false;
+	} else if (version_ >= 3.0 && version_ < 3.2) { //v3.0 & v3.1
+		if (len != 1044)
+			len_good = false;
+	} else if (version_ >= 3.2 && version_ < 3.3) { //v3.2
+		if (len != 1060)
+			len_good = false;
+	}
+
+	if (!len_good) {
+		printf("Wrong length of message on RT interface: %i\n", len);
 		val_lock_.unlock();
 		return;
 	}
-	
-	if (version_ > 1.8 & version_ < 1.9 & len != 812) {
-		// In 1.8.14035, every 17th and 18th package is 560 and 9 bytes long/malformed.
-		//printf("Len: %i\n", len);
-		val_lock_.unlock();
-		return;
-	}
-	
+
 	memcpy(&unpack_to, &buf[offset], sizeof(unpack_to));
 	time_ = RobotStateRT::ntohd(unpack_to);
 	offset += sizeof(double);
diff --git a/src/ur_driver.cpp b/src/ur_driver.cpp
index 274da91..5572aa2 100644
--- a/src/ur_driver.cpp
+++ b/src/ur_driver.cpp
@@ -30,6 +30,7 @@ UrDriver::UrDriver(std::condition_variable& rt_msg_cond,
 	struct sockaddr_in serv_addr;
 	int n, flag;
 
+	firmware_version_ = 0;
 	reverse_connected_ = false;
 	executing_traj_ = false;
 	rt_interface_ = new UrRealtimeCommunication(rt_msg_cond, host,