Merge pull request #44 from RI-SE/fix_missing_traj_point

Fix missing traj point
RI-SE · Oct 23, 2023 · 789df73 · 789df73
2 parents 323bcd5 + a49f38f
commit 789df73
Show file tree

Hide file tree

Showing 5 changed files with 18 additions and 19 deletions.
diff --git a/iso22133.h b/iso22133.h
@@ -63,12 +63,6 @@ typedef struct {
 	} timeServer;
 } ObjectSettingsType;
 
-/*! ISO message constants */
-enum ISOConstantsType {
-	ISO_TRAJ_HEADER_SIZE = 91,
-	ISO_TRAJ_WAYPOINT_SIZE = 70
-};
-
 typedef enum {
 	TRAJECTORY_INFO_RELATIVE_TO_OBJECT = 1,
 	TRAJECTORY_INFO_RELATIVE_TO_ORIGIN = 2,

diff --git a/src/header.c b/src/header.c
@@ -130,12 +130,12 @@ enum ISOMessageReturnValue decodeISOHeader(
 
 	if (debug) {
 		printf("syncWord = 0x%x\n", HeaderData->syncWord);
-		printf("messageLength = 0x%x\n", HeaderData->messageLength);
-		printf("ackReqProtVer = 0x%x\n", HeaderData->ackReqProtVer);
-		printf("transmitterID = 0x%x\n", HeaderData->transmitterID);
-		printf("receiverID = 0x%x\n", HeaderData->receiverID);
-		printf("messageCounter = 0x%x\n", HeaderData->messageCounter);
-		printf("messageID = 0x%x\n", HeaderData->messageID);
+		printf("messageLength = %d bytes, [0x%x]\n", HeaderData->messageLength, HeaderData->messageLength);
+		printf("ackReqProtVer = %d, [0x%x]\n", HeaderData->ackReqProtVer, HeaderData->ackReqProtVer);
+		printf("transmitterID = %d, [0x%x]\n", HeaderData->transmitterID, HeaderData->transmitterID);
+		printf("receiverID = %d, [0x%x]\n", HeaderData->receiverID, HeaderData->receiverID);
+		printf("messageCounter = %d, [0x%x]\n", HeaderData->messageCounter, HeaderData->messageCounter);
+		printf("messageID = %d, [0x%x]\n", HeaderData->messageID, HeaderData->messageID);
 	}
 
 	return retval;

diff --git a/src/traj.c b/src/traj.c
@@ -219,7 +219,7 @@ ssize_t decodeTRAJMessageHeader(
 		printf("\tTrajectory name: %s\n", TRAJHeaderData.trajectoryName);
 		printf("\tTrajectory info: %u\n", TRAJHeaderData.trajectoryInfo);
 		printf("\tTRAJ length: %ld bytes\n", TRAJHeaderData.header.messageLength
-			- sizeof(TRAJHeaderType) - sizeof(TRAJFooterType) + sizeof(FooterType));
+			- sizeof(TRAJHeaderType)  + sizeof(HeaderType) - sizeof(TRAJFooterType) + sizeof(FooterType));
 	}
 
 	// Fill output struct with parsed data
@@ -244,7 +244,7 @@ enum ISOMessageReturnValue convertTRAJHeaderToHostRepresentation(TRAJHeaderType*
 	trajectoryHeaderData->trajectoryID = TRAJHeaderData->trajectoryID;
 	memcpy(trajectoryHeaderData->trajectoryName, TRAJHeaderData->trajectoryName, sizeof(TRAJHeaderData->trajectoryName));
 	trajectoryHeaderData->trajectoryInfo = TRAJHeaderData->trajectoryInfo;
-	trajectoryHeaderData->trajectoryLength = trajectoryLength - sizeof(TRAJHeaderType) - sizeof(TRAJFooterType) + sizeof(FooterType);
+	trajectoryHeaderData->trajectoryLength = trajectoryLength - sizeof(TRAJHeaderType) + sizeof(HeaderType) - sizeof(TRAJFooterType) + sizeof(FooterType);
 	trajectoryHeaderData->nWaypoints = trajectoryHeaderData->trajectoryLength/sizeof(TRAJPointType);
 
 	return MESSAGE_OK;

diff --git a/tests/osem.cpp b/tests/osem.cpp
@@ -50,7 +50,7 @@ class EncodeOSEM : public ::testing::Test
 	}
 	void TearDown() override
 	{
-		setTransmitterID(0xFFFF);
+		setTransmitterID(0xFFFFFFFF);
 	}
 
 	ObjectSettingsType settings;

diff --git a/tests/traj.cpp b/tests/traj.cpp
@@ -82,10 +82,10 @@ class DecodeTRAJHeader : public ::testing::Test
 	DecodeTRAJHeader() {
 		decodeBuffer[0] = 0x7F;
 		decodeBuffer[1] = 0x7E;	// preamble
-		decodeBuffer[2] = 0x52;
-		decodeBuffer[3] = 0x00;
+		decodeBuffer[2] = 0x1E;
+		decodeBuffer[3] = 0x03;
 		decodeBuffer[4] = 0x00;
-		decodeBuffer[5] = 0x00;	 // TODO Message length
+		decodeBuffer[5] = 0x00;	 // Message length 0x31E (sizeof(TRAJHeaderType) + sizeof(TRAJPointType) * 21 + sizeof(TRAJFooterType)) - (sizeof(TRAJHeaderType) + sizeof(HeaderType)) - (sizeof(TRAJFooterType) + sizeof(FooterType));
 		decodeBuffer[6] = 0x02;	 // Acknowledge protocol version
 		decodeBuffer[7] = 0x34;
 		decodeBuffer[8] = 0x12;
@@ -145,7 +145,7 @@ class DecodeTRAJHeader : public ::testing::Test
 			&header,
 			decodeBuffer,
 			sizeof(decodeBuffer),
-			false);
+			true);
 		ASSERT_GT(res, 0);
 	}
 
@@ -171,6 +171,11 @@ TEST_F(DecodeTRAJHeader, Name)
 	}
 }
 
+TEST_F(DecodeTRAJHeader, nWaypoints)
+{
+	EXPECT_EQ(header.nWaypoints, 21);
+}
+
 class EncodeTRAJPoint : public ::testing::Test
 {
 protected: