Skip to content

Commit

Permalink
Merge pull request #40 from RI-SE/feature_onlinePlannedTraj
Browse files Browse the repository at this point in the history 
Feature online planned traj
  • Loading branch information
@Robert108
Robert108 committed Jun 14, 2023
2 parents 0253c26 + 1750791 commit 92b2c1a
Show file tree
Hide file tree
Showing 8 changed files with 428 additions and 216 deletions.
48 changes: 48 additions & 0 deletions include/grem.h
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,48 @@
#pragma once

#include <stdint.h>

#include "iso22133.h"
#include "header.h"
#include "footer.h"

#pragma pack(push, 1)
typedef struct{
HeaderType header;
uint16_t ReceivedHeaderTransmitterIDValueID;
uint16_t ReceivedHeaderTransmitterIDContentLength;
uint32_t ReceivedHeaderTransmitterID;

uint16_t ReceivedHeaderMessageCounterValueID;
uint16_t ReceivedHeaderMessageCounterContentLength;
uint8_t ReceivedHeaderMessageCounter;

uint16_t ReceivedHeaderMessageIDValueID;
uint16_t ReceivedHeaderMessageIDContentLength;
uint16_t ReceivedHeaderMessageID;

uint16_t ResponseCodeValueID;
uint16_t ResponseCodeContentLength;
uint8_t ResponseCode;

uint16_t PayloadLengthValueID;
uint16_t PayloadLengthContentLength;
uint16_t PayloadLength;

uint16_t PayloadDataValueID;
uint16_t PayloadDataContentLength;
FooterType footer;
} GREMType;
#pragma pack(pop)

//! GREM value IDs

#define VALUE_ID_GREM_RECEIVED_HEADER_TRANSMITTER_ID 0x0200
#define VALUE_ID_GREM_RECEIVED_HEADER_MESSAGE_COUNTER 0x0201
#define VALUE_ID_GREM_RECEIVED_HEADER_MESSAGE_ID 0x0202
#define VALUE_ID_GREM_RESPONSE_CODE 0x0203
#define VALUE_ID_GREM_PAYLOAD_LENGTH 0x0204
#define VALUE_ID_GREM_PAYLOAD_DATA 0x0205

enum ISOMessageReturnValue convertGREMoHostRepresentation(GREMType* GREMdata,
GeneralResponseMessageType* gremData);
8 changes: 8 additions & 0 deletions include/header.h
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,8 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif

#include <stdlib.h>
#include <stdint.h>
#include "iso22133.h"
Expand All @@ -20,3 +24,7 @@ enum ISOMessageReturnValue decodeISOHeader(const char *MessageBuffer, const size
HeaderType * HeaderData, const char debug);

HeaderType buildISOHeader(enum ISOMessageID id, uint32_t messageLength, const char debug);

#ifdef __cplusplus
}
#endif
203 changes: 1 addition & 202 deletions iso22133.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -373,19 +373,7 @@ static DebugStrings_t PODIRollDescription = {"Object roll", "[cdeg]", &pr
static DebugStrings_t PODILongitudinalSpeedDescription = {"Longitudinal speed", "[cm/s]", &printI16};
static DebugStrings_t PODILateralSpeedDescription = {"Lateral speed", "[cm/s]", &printI16};

typedef struct{
HeaderType header;
uint16_t statusValueID;
uint16_t statusContentLength;
uint8_t status;
FooterType footer;
} GREMType;

//! GREM value IDs
#define VALUE_ID_GREM_STATUS 0x00FF

//! GREM field descriptions
static DebugStrings_t GREMStatusDescription = {"Status", "", &printU32};

/*! RCMM message */
typedef struct {
Expand Down Expand Up @@ -541,8 +529,6 @@ static enum ISOMessageReturnValue convertFOPRToHostRepresentation(
ForeignObjectPropertiesType* foreignObjectProperties);
static enum ISOMessageReturnValue convertRCMMToHostRepresentation(RCMMType * RCMMData,
RemoteControlManoeuvreMessageType* rcmmData);
enum ISOMessageReturnValue convertGREMoHostRepresentation(GREMType* GREMdata,
GeneralResponseMessageType* gremData);
static enum ISOMessageReturnValue convertSTRTToHostRepresentation(const STRTType * STRTData, const struct timeval* currentTime,
StartMessageType * startdata);

Expand Down Expand Up @@ -581,7 +567,7 @@ char isValidMessageID(const uint16_t id) {
|| id == MESSAGE_ID_INFO || id == MESSAGE_ID_RCMM || id == MESSAGE_ID_SYPM || id == MESSAGE_ID_MTSP
|| id == MESSAGE_ID_TRCM || id == MESSAGE_ID_ACCM || id == MESSAGE_ID_TREO || id == MESSAGE_ID_EXAC
|| id == MESSAGE_ID_CATA || id == MESSAGE_ID_RCCM || id == MESSAGE_ID_RCRT || id == MESSAGE_ID_PIME
|| id == MESSAGE_ID_COSE || id == MESSAGE_ID_MOMA
|| id == MESSAGE_ID_COSE || id == MESSAGE_ID_MOMA || id == MESSAGE_ID_GREM
|| (id >= MESSAGE_ID_VENDOR_SPECIFIC_LOWER_LIMIT && id <= MESSAGE_ID_VENDOR_SPECIFIC_UPPER_LIMIT);
}

Expand Down Expand Up @@ -3670,193 +3656,6 @@ enum ISOMessageReturnValue convertRDCAToHostRepresentation(RDCAType* RDCAData,
}


/*!
* \brief decodeGREMMessage Fills GREM data elements from a buffer of raw data
* \param gremDataBuffer Raw data to be decoded
* \param bufferLength Number of bytes in buffer of raw data to be decoded
* \param gremData Struct to be filled
* \param debug Flag for enabling of debugging
* \return value according to ::ISOMessageReturnValue
*/
ssize_t decodeGREMMessage(
const char *gremDataBuffer,
const size_t bufferLength,
GeneralResponseMessageType* gremData,
const char debug) {

GREMType GREMdata;
const char *p = gremDataBuffer;
ssize_t retval = MESSAGE_OK;
uint16_t valueID = 0;
uint16_t contentLength = 0;
ssize_t expectedContentLength = 0;

if (gremDataBuffer == NULL || gremData == NULL) {
errno = EINVAL;
fprintf(stderr, "Input pointers to GREM parsing function cannot be null\n");
return ISO_FUNCTION_ERROR;
}

memset(&GREMdata, 0, sizeof (GREMdata));
memset(gremData, 0, sizeof (*gremData));

// Decode ISO header
if ((retval = decodeISOHeader(p, bufferLength, &GREMdata.header, debug)) != MESSAGE_OK) {
return retval;
}
p += sizeof (GREMdata.header);

// If message is not a GREM message, generate an error
if (GREMdata.header.messageID != MESSAGE_ID_VENDOR_SPECIFIC_ASTAZERO_GREM) {
fprintf(stderr, "Attempted to pass non-GREM message into GREM parsing function\n");
return MESSAGE_TYPE_ERROR;
}

if (GREMdata.header.messageLength > sizeof (GREMType) - sizeof (HeaderType) - sizeof (FooterType)) {
fprintf(stderr, "GREM message exceeds expected message length\n");
return MESSAGE_LENGTH_ERROR;
}

while (p - gremDataBuffer < GREMdata.header.messageLength + sizeof (HeaderType)) {
memcpy(&valueID, p, sizeof (valueID));
p += sizeof (valueID);
memcpy(&contentLength, p, sizeof (contentLength));
p += sizeof (contentLength);
valueID = le16toh(valueID);
contentLength = le16toh(contentLength);

switch (valueID) {
case VALUE_ID_GREM_STATUS:
memcpy(&GREMdata.status, p, sizeof (GREMdata.status));
GREMdata.statusValueID = valueID;
GREMdata.statusContentLength = contentLength;
expectedContentLength = sizeof (GREMdata.statusValueID);
break;
default:
fprintf(stderr, "Value ID 0x%x does not match any known GREM value IDs", valueID);
return MESSAGE_VALUE_ID_ERROR;
}

p += contentLength;
if (contentLength != expectedContentLength) {
fprintf(stderr, "Content length %u for value ID 0x%x does not match the expected %ld",
contentLength, valueID, expectedContentLength);
return MESSAGE_LENGTH_ERROR;
}
}

// Decode footer
if ((retval =
decodeISOFooter(p, bufferLength - (size_t) (p - gremDataBuffer), &GREMdata.footer,
debug)) != MESSAGE_OK) {
fprintf(stderr, "Error decoding GREM footer\n");
return retval;
}
p += sizeof (GREMdata.footer);

if (debug) {
printf("GREM message:\n");
printf("\tStatus transmitter ID value ID: 0x%x\n", GREMdata.statusValueID);
printf("\tStatus transmitter ID content length: %u\n", GREMdata.statusContentLength);
printf("\tStatus: %u\n", GREMdata.status);
}

// Fill output struct with parsed data
retval = convertGREMoHostRepresentation(&GREMdata, gremData);
}

/*!
* \brief convertPODIToHostRepresentation Converts a PODI message to SI representation
* \param PODIData Data struct containing ISO formatted data
* \param currentTime Current system time, used for determining the GPS week
* \param peerData Output data struct, for SI representation data
* \return Value according to ::ISOMessageReturnValue
*/
enum ISOMessageReturnValue convertGREMoHostRepresentation(GREMType* GREMdata,
GeneralResponseMessageType* gremData) {

if (GREMdata == NULL || gremData == NULL) {
errno = EINVAL;
fprintf(stderr, "GREM input pointer error");
return ISO_FUNCTION_ERROR;
}

if (!GREMdata->statusValueID ) {
fprintf(stderr, "Status Value ID not supplied in GREM message\n");
return MESSAGE_VALUE_ID_ERROR;
}

gremData->status = GREMdata->status;

return MESSAGE_OK;
}


/*!
* \brief encodeGREMMessage Fills an ISO vendor specific (AstaZero) GREM struct with relevant data fields,
* and corresponding value IDs and content lengths
* \param gremObjectData Struct containing relevant GREM data
* \param gremDataBuffer Data buffer to which message is to be printed
* \param bufferLength Available memory in data buffer
* \param debug Flag for enabling debugging
* \return Number of bytes written to buffer, or -1 in case of error
*/
ssize_t encodeGREMMessage(const GeneralResponseMessageType* gremObjectData,
char* gremDataBuffer,
const size_t bufferLength,
const char debug) {

GREMType GREMData;

memset(gremDataBuffer, 0, bufferLength);
char* p = gremDataBuffer;
size_t remainingBytes = bufferLength;
int retval = 0;

if (gremObjectData == NULL) {
fprintf(stderr, "GREM data input pointer error\n");
return -1;
}

// If buffer too small to hold GREM data, generate an error
if (bufferLength < sizeof (GREMType)) {
fprintf(stderr, "Buffer too small to hold necessary GREM data\n");
return -1;
}

// Construct header
GREMData.header = buildISOHeader(MESSAGE_ID_VENDOR_SPECIFIC_ASTAZERO_GREM, sizeof (GREMData), debug);
memcpy(p, &GREMData.header, sizeof (GREMData.header));
p += sizeof (GREMData.header);
remainingBytes -= sizeof (GREMData.header);

if (debug) {
printf("GREM message:\n");
}
// Fill contents
retval |= encodeContent(VALUE_ID_GREM_STATUS, &gremObjectData->status, &p,
sizeof (gremObjectData->status), &remainingBytes, &GREMStatusDescription, debug);

if (retval != 0 || remainingBytes < sizeof (FooterType)) {
fprintf(stderr, "Buffer too small to hold necessary GREM data\n");
return -1;
}

if (debug) {
printf("GREM message:\n\t<<debug printout not implemented>>\n");
}

// Construct footer
GREMData.footer = buildISOFooter(&GREMData, sizeof (GREMData), debug);
memcpy(p, &GREMData.footer, sizeof (GREMData.footer));
p += sizeof (GREMData.footer);
remainingBytes -= sizeof (GREMData.footer);


return p - gremDataBuffer;
}


/*!
* \brief mapISOHeadingToHostHeading Converts between ISO NED heading to internal heading measured from the test x axis
* \param isoHeading_rad Heading measured according to ISO specification, in radians
Expand Down
29 changes: 18 additions & 11 deletions iso22133.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -251,6 +251,7 @@ enum ISOMessageID {
MESSAGE_ID_TREO = 0x0013,
MESSAGE_ID_EXAC = 0x0014,
MESSAGE_ID_CATA = 0x0015,
MESSAGE_ID_GREM = 0x0018,
MESSAGE_ID_RCCM = 0x0020,
MESSAGE_ID_TRCM = 0x0021,
MESSAGE_ID_RCRT = 0x0033, //This is a temporary value, it was conflicting with TRCM
Expand All @@ -270,7 +271,6 @@ enum ISOMessageID {
MESSAGE_ID_VENDOR_SPECIFIC_ASTAZERO_DCTI = 0xA121,
MESSAGE_ID_VENDOR_SPECIFIC_ASTAZERO_GDRM = 0xA120,
MESSAGE_ID_VENDOR_SPECIFIC_ASTAZERO_RDCA = 0xA122,
MESSAGE_ID_VENDOR_SPECIFIC_ASTAZERO_GREM = 0xA123,
MESSAGE_ID_VENDOR_SPECIFIC_ASTAZERO_DCMM = 0xA124
};

Expand Down Expand Up @@ -316,12 +316,14 @@ enum ISOUnitType {

/*! GREM status */
enum GeneralResponseStatus {
GREM_STATUS_OK = 1,
GREM_DATA_MISSING = 2,
GREM_STATE_ERROR = 3,
GREM_TIME_SLIPPAGE = 4,
GREM_WRONG_ID = 5,
GREM_OUT_OF_RANGE = 6
UNDEFIEND_0 = 0,
GREM_OK = 1,
GREM_GENERAL_ERROR = 2,
GREM_NOT_SUPPORTED = 3,
GREM_NO_MEMORY = 4,
GREM_INVALID_DATA = 5,
GREM_CHUNK_RECEIVED = 6,
UNDEFIEND_1 = 7
};

enum RemoteControlManoeuvreCommandType {
Expand Down Expand Up @@ -462,7 +464,12 @@ typedef struct {
} StartMessageType;

typedef struct{
enum GeneralResponseStatus status;
uint32_t receivedHeaderTransmitterID;
uint8_t receivedHeaderMessageCounter;
uint16_t receivedHeaderMessageID;
uint8_t responseCode;
uint16_t payloadLength;
uint8_t payload;
} GeneralResponseMessageType;


Expand Down Expand Up @@ -515,12 +522,14 @@ ssize_t encodeMTSPMessage(const struct timeval * estSyncPointTime, char * mtspDa
ssize_t encodeTRCMMessage(const uint16_t* triggerID, const enum TriggerType_t* triggerType, const enum TriggerTypeParameter_t* param1, const enum TriggerTypeParameter_t* param2, const enum TriggerTypeParameter_t* param3, char * trcmDataBuffer, const size_t bufferLength, const char debug);
ssize_t encodeACCMMessage(const uint16_t* actionID, const enum ActionType_t* actionType, const enum ActionTypeParameter_t* param1, const enum ActionTypeParameter_t* param2, const enum ActionTypeParameter_t* param3, char * accmDataBuffer, const size_t bufferLength, const char debug);
ssize_t encodeEXACMessage(const uint16_t* actionID, const struct timeval * executionTime, char * exacDataBuffer, const size_t bufferLength, const char debug);
ssize_t decodeRCMMMessage(const char *rcmmDataBuffer, const size_t bufferLength, RemoteControlManoeuvreMessageType* rcmmData, const char debug);
ssize_t encodeRCMMMessage(const RemoteControlManoeuvreMessageType* rcmmObjectData, char* rcmmDataBuffer, const size_t bufferLength, const char debug);
ssize_t decodeGREMMessage(const char *gremDataBuffer, const size_t bufferLength, GeneralResponseMessageType* gremData, const char debug);
ssize_t encodeGREMMessage(const GeneralResponseMessageType* gremObjectData, char* gremDataBuffer, const size_t bufferLength, const char debug);
ssize_t encodeDRESMessage(const TestObjectDiscoveryType *testObjectDiscoveryData, char *dresDataBuffer, const size_t bufferLength, const char debug);
ssize_t decodeDRESMessage(const char* dresDataBuffer, const size_t bufferLength, TestObjectDiscoveryType *testObjectDiscoveryData, const char debug);
ssize_t encodeDREQMessage(char *dreqDataBuffer, const size_t bufferLength, const char debug);
ssize_t decodeDREQMessage(const char* dreqDataBuffer, const size_t bufferLength, TestObjectDiscoveryRequestType *testObjectDiscoveryRequestData, const char debug);
ssize_t decodeRCMMMessage(const char *rcmmDataBuffer, const size_t bufferLength, RemoteControlManoeuvreMessageType* rcmmData, const char debug);
ssize_t encodeINSUPMessage(const enum SupervisorCommandType, char * insupDataBuffer, const size_t bufferLength, const char debug);
ssize_t encodeDCTIMessage(const DctiMessageDataType *dctiData, char *dctiDataBuffer, const size_t bufferLength, const char debug);
enum ISOMessageReturnValue decodeDCTIMessage(const char *dctiDataBuffer, const size_t bufferLength, DctiMessageDataType* dctiData, const char debug);
Expand All @@ -540,8 +549,6 @@ ssize_t encodeRDCAMessage(const RequestControlActionType* requestControlActionDa
ssize_t decodeRDCAMessage(const char *rdcaDataBuffer, RequestControlActionType* requestControlActionData, const size_t bufferLength, const struct timeval currentTime, const char debug);
ssize_t encodeGDRMMessage(const GdrmMessageDataType *gdrmData, char *gdrmDataBuffer, const size_t bufferLength, const char debug);
enum ISOMessageReturnValue decodeGDRMMessage(const char *gdrmDataBuffer, const size_t bufferLength, GdrmMessageDataType* gdrmData, const char debug);
ssize_t decodeGREMMessage(const char *gremDataBuffer, const size_t bufferLength, GeneralResponseMessageType* gremData, const char debug);
ssize_t encodeGREMMessage(const GeneralResponseMessageType* gremObjectData, char* gremDataBuffer, const size_t bufferLength, const char debug);
ssize_t encodeDCMMMessage(const RemoteControlManoeuvreMessageType* command, char* dcmmDataBuffer, const size_t bufferLength, const char debug);
ssize_t decodeDCMMMessage(const char * dcmmDataBuffer, const size_t bufferLenght, RemoteControlManoeuvreMessageType* dcmmData, const char debug);
#ifdef __cplusplus
Expand Down
Loading

0 comments on commit 92b2c1a

Please sign in to comment.