diff --git a/include/grem.h b/include/grem.h index 23a15de..cd09d05 100644 --- a/include/grem.h +++ b/include/grem.h @@ -5,6 +5,7 @@ extern "C" { #include #include "iso22133.h" +#include "defines.h" #include "header.h" #include "footer.h" diff --git a/iso22133.h b/iso22133.h index 6c2bafb..81fff35 100644 --- a/iso22133.h +++ b/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, diff --git a/src/header.c b/src/header.c index 993e349..a2524d7 100644 --- 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 index b99bd67..5f73185 100644 --- a/src/traj.c +++ b/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; diff --git a/tests/osem.cpp b/tests/osem.cpp index 2c51145..d5c039d 100644 --- a/tests/osem.cpp +++ b/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 diff --git a/tests/traj.cpp b/tests/traj.cpp index 9e2776c..9f3e982 100644 --- a/tests/traj.cpp +++ b/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'); }