merge with master

include/grem.h

@@ -5,6 +5,7 @@ extern "C" {
 #include <stdint.h>
 
 #include "iso22133.h"
+#include "defines.h"
 #include "header.h"
 #include "footer.h"
 

iso22133.h

@@ -75,12 +75,6 @@ typedef struct {
 	OSEMTimeServer 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,

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;

src/traj.c

@@ -68,17 +68,20 @@ ssize_t encodeTRAJMessageHeader(
 		printf("Trajectory name <%s> too long for TRAJ message\n", trajectoryName);
 		return -1;
 	}
-
+	
 	// Construct ISO header
-	uint32_t messageLength = sizeof (TRAJHeaderType)
-		+ numberOfPointsInTraj * sizeof (TRAJPointType)
-		+ sizeof (TRAJFooterType);
-	TRAJData.header = buildISOHeader(MESSAGE_ID_TRAJ, inputHeader, messageLength, debug);
+	TRAJData.header = buildISOHeader(
+		MESSAGE_ID_TRAJ,
+		inputHeader,
+		sizeof (TRAJHeaderType)
+			+ numberOfPointsInTraj * sizeof (TRAJPointType)
+			+ sizeof (TRAJFooterType),
+		debug);
 	memcpy(p, &TRAJData.header, sizeof(TRAJData.header));
 	p += sizeof (HeaderType);
 
 	if (debug) {
-			printf("TRAJ message header:\n");
+		printf("TRAJ message header:\n");
 	}
 
 	// Fill contents
@@ -220,7 +223,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
@@ -245,7 +248,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;

tests/osem.cpp

@@ -54,6 +54,7 @@ class EncodeOSEM : public ::testing::Test
 			sizeof(encodeBuffer), false);
 		ASSERT_GT(res, 0);
 	}
+
 	ObjectSettingsType settings;
 	char encodeBuffer[1024];
 	char* id = encodeBuffer + 18; // skip header

tests/traj.cpp

@@ -87,10 +87,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,11 +145,12 @@ class DecodeTRAJHeader : public ::testing::Test
 
 	void SetUp() override
 	{
+		memset(&header, 0, sizeof(header));
 		auto res = decodeTRAJMessageHeader(
 			&header,
 			decodeBuffer,
 			sizeof(decodeBuffer),
-			false);
+			true);
 		ASSERT_GT(res, 0);
 	}
 
@@ -175,6 +176,11 @@ TEST_F(DecodeTRAJHeader, Name)
 	}
 }
 
+TEST_F(DecodeTRAJHeader, nWaypoints)
+{
+	EXPECT_EQ(header.nWaypoints, 21);
+}
+
 class EncodeTRAJPoint : public ::testing::Test
 {
 protected:
@@ -457,53 +463,62 @@ class EncodeTRAJFooter : public ::testing::Test
 	}
 	void SetUp() override
 	{
+        auto trajectoryID = 0x123;
+		auto trajectoryVersion = TRAJECTORY_INFO_RELATIVE_TO_OBJECT;
+		auto trajectoryName = "some description";
+		auto nameLength = strlen(trajectoryName)-1;
+		auto numberOfPointsInTraj = 3;
 		memset(encodeBuffer, 0, sizeof(encodeBuffer));
-		auto p = encodeBuffer;
+		auto points = encodeBuffer;
+		auto bufferLength = sizeof(encodeBuffer);
+		bool debug = false;
+
 		MessageHeaderType inputHeader;
-		inputHeader.receiverID = 0;
-		inputHeader.messageCounter = 0;
-		inputHeader.transmitterID = 0x000000FF;
+		memset(&inputHeader, 0, sizeof(inputHeader));
+		inputHeader.transmitterID = 0xFFFFFFFF;
+
 		auto offset = encodeTRAJMessageHeader(
 			&inputHeader,
-			0x123,
+			trajectoryID,
 			TRAJECTORY_INFO_RELATIVE_TO_OBJECT,
-			"some description",
-			17,
-			3,
-			p,
+			trajectoryName,
+			nameLength,
+			numberOfPointsInTraj,
+			points,
 			sizeof(encodeBuffer),
 			false);
 		ASSERT_GT(offset, 0);
-		p += offset;
+		points += offset;
 		for (int i = 0; i < 3; i++) {
 			struct timeval tv = {1,2};
 			CartesianPosition pos = {1,2,3,4,true,true,true,true,true};
 			SpeedType spd = {1,2,true,true};
 			AccelerationType acc = {1,2,true,true};
+			float curvature = 12.34;
 			offset = encodeTRAJMessagePoint(
 				&tv,
 				pos,
 				spd,
 				acc,
-				12.34,
-				p,
-				sizeof(encodeBuffer) - (p-encodeBuffer),
+				curvature,
+				points,
+				sizeof(encodeBuffer) - (points-encodeBuffer),
 				false);
 			ASSERT_GT(offset, 0);
-			p += offset;
+			points += offset;
 		}
 		offset = encodeTRAJMessageFooter(
-			p,
-			sizeof(encodeBuffer) - (p - encodeBuffer),
+			points,
+			sizeof(encodeBuffer) - (points - encodeBuffer),
 			false
 		);
 		ASSERT_GT(offset, 0);
-		p += offset;
-		// Run this to view raw data for which to generate CRC
-		// for (int i = 0; i < p - encodeBuffer; ++i) {
-		// 	printf("%02x ",(uint8_t)encodeBuffer[i]);
-		// }
-		// printf("\n");
+		points += offset;
+		printf("Raw data for CRC:\n");
+		for (int i = 0; i < points - encodeBuffer - 2; ++i) {
+			printf("%02x ",(uint8_t)encodeBuffer[i]);
+		}
+		printf("\n");
 	}
 
 	char encodeBuffer[1024];
@@ -519,8 +534,8 @@ TEST_F(EncodeTRAJFooter, EndOfTransmission) {
 	EXPECT_EQ(lineInfo[4], '\x04');
 }
 
-// https://crccalc.com/?crc=7f 7e ba 00 00 00 02 ff 00 00 00 00 00 00 00 00 01 00 01 01 02 00 23 01 04 01 01 00 01 02 01 40 00 73 6f 6d 65 20 64 65 73 63 72 69 70 74 69 6f 6e 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 01 00 1e 00 e8 03 00 00 e8 03 00 00 d0 07 00 00 b8 0b 00 00 86 59 64 00 c8 00 e8 03 d0 07 a4 70 45 41 01 00 1e 00 e8 03 00 00 e8 03 00 00 d0 07 00 00 b8 0b 00 00 86 59 64 00 c8 00 e8 03 d0 07 a4 70 45 41 01 00 1e 00 e8 03 00 00 e8 03 00 00 d0 07 00 00 b8 0b 00 00 86 59 64 00 c8 00 e8 03 d0 07 a4 70 45 41 53 00 01 00 04&method=crc16&datatype=hex&outtype=0
+// https://crccalc.com/?crc=7f 7e ba 00 00 00 02 ff ff ff ff 00 00 00 00 00 01 00 01 01 02 00 23 01 04 01 01 00 01 02 01 40 00 73 6f 6d 65 20 64 65 73 63 72 69 70 74 69 6f 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 01 00 1e 00 e8 03 00 00 e8 03 00 00 d0 07 00 00 b8 0b 00 00 86 59 64 00 c8 00 e8 03 d0 07 a4 70 45 41 01 00 1e 00 e8 03 00 00 e8 03 00 00 d0 07 00 00 b8 0b 00 00 86 59 64 00 c8 00 e8 03 d0 07 a4 70 45 41 01 00 1e 00 e8 03 00 00 e8 03 00 00 d0 07 00 00 b8 0b 00 00 86 59 64 00 c8 00 e8 03 d0 07 a4 70 45 41 53 00 01 00 04&method=crc16&datatype=hex&outtype=0// CRC-16/XMODEM
 TEST_F(EncodeTRAJFooter, Crc) {
-	EXPECT_EQ(crc[0], '\x34');
-	EXPECT_EQ(crc[1], '\x21');
+	EXPECT_EQ(crc[0], '\x3D');
+	EXPECT_EQ(crc[1], '\xB5');
 }