From 92479413b4cbf6414a99f41f7445a9f5fc152c47 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 16 Sep 2020 14:19:53 +0200 Subject: [PATCH 1/4] Re-referenced util to include PODI encoder/decoder --- util | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/util b/util index 5aa342c11..2b7c38275 160000 --- a/util +++ b/util @@ -1 +1 @@ -Subproject commit 5aa342c1147d954492a79a5eff1a412049c962d7 +Subproject commit 2b7c3827591269c39853d572f4fdb27593a2ba01 From 609cff5df085ad52d3797bd47c54eff215454b8d Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 16 Sep 2020 15:33:29 +0200 Subject: [PATCH 2/4] Implemented function for sending PODI to all configured objects --- core/src/objectcontrol.c | 77 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 77 insertions(+) diff --git a/core/src/objectcontrol.c b/core/src/objectcontrol.c index 2cce3f7e1..5c546de07 100644 --- a/core/src/objectcontrol.c +++ b/core/src/objectcontrol.c @@ -139,6 +139,11 @@ static int configureObjectDataInjection(DataInjectionMap injectionMaps[], const const unsigned int numberOfObjects); static int parseDataInjectionSetting(const char objectFilePath[MAX_FILE_PATH], DataInjectionMap injectionMaps[], const unsigned int numberOfMaps); +static int sendDataInjectionMessages(const ObjectDataType* monitorData, + const DataInjectionMap dataInjectionMaps[], + const ObjectConnection objectConnections[], + const uint32_t transmitterIDs[], + const unsigned int numberOfObjects); static void *objectConnectorThread(void *args); static int checkObjectConnections(ObjectConnection objectConnections[], const struct timeval monitorTimeout, const unsigned int numberOfObjects); @@ -571,6 +576,9 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { OP[iIndex].Speed = (float)sqrt(pow(monitorData.MonrData.speed.lateral_m_s, 2) + pow(monitorData.MonrData.speed.longitudinal_m_s, 2)); + + // Pass on the information to the configured injection targets + sendDataInjectionMessages(&monitorData, dataInjectionMaps, objectConnections, object_transmitter_ids, nbr_objects); } else if (receivedMONRData > 0) LogMessage(LOG_LEVEL_WARNING, "Received unhandled message on monitoring socket"); @@ -2102,6 +2110,75 @@ int parseDataInjectionSetting(const char objectFilePath[MAX_FILE_PATH], return retval; } +/*! + * \brief sendDataInjectionMessages Translates the passed monitor data to an injection message + * and sends it to all configured receivers. + * \param objectData Monitor data to be passed on. + * \param dataInjectionMaps Map showing which objects are the receivers of injection data. + * \param objectConnections Connections to objects. Assumed to be ordered in the same way + * as the transmitter ID array. + * \param transmitterIDs Transmitter IDs of objects. + * \param numberOfObjects Size of all three arrays. + * \return 0 if successful, -1 otherwise. + */ +int sendDataInjectionMessages( + const ObjectDataType* objectData, + const DataInjectionMap dataInjectionMaps[], + const ObjectConnection objectConnections[], + const uint32_t transmitterIDs[], + const unsigned int numberOfObjects) { + + if (objectData == NULL || dataInjectionMaps == NULL || objectConnections == NULL) { + LogMessage(LOG_LEVEL_ERROR, "Attempted to pass null pointer to %s", __FUNCTION__); + return -1; + } + + const ObjectMonitorType* monitorData = &objectData->MonrData; + char transmissionBuffer[1024]; + ssize_t messageSize; + const DataInjectionMap* relevantMap = NULL; + PeerObjectInjectionType injectionMessage; + + // Find the map for source of monitor data + for (unsigned int i = 0; i < numberOfObjects; ++i) { + if (dataInjectionMaps[i].sourceID == objectData->ClientID) { + relevantMap = &dataInjectionMaps[i]; + } + } + if (relevantMap == NULL) { + LogMessage(LOG_LEVEL_ERROR, "Found no injection settings for sender ID %u", objectData->ClientID); + return -1; + } + if (relevantMap->numberOfTargets == 0) { + return 0; + } + + // Create the message + injectionMessage.dataTimestamp = monitorData->timestamp; + injectionMessage.position = monitorData->position; + injectionMessage.state = monitorData->state; + injectionMessage.speed = monitorData->speed; + injectionMessage.foreignTransmitterID = objectData->ClientID; + injectionMessage.isRollValid = 0; + injectionMessage.isPitchValid = 0; + + messageSize = encodePODIMessage(&injectionMessage, transmissionBuffer, sizeof (transmissionBuffer), 0); + if (messageSize == -1) { + LogMessage(LOG_LEVEL_ERROR, "Failed to encode PODI message"); + return -1; + } + + // Send message to all configured receivers + for (unsigned int i = 0; i < relevantMap->numberOfTargets; ++i) { + for (unsigned int j = 0; j < numberOfObjects; j++) { + if (transmitterIDs[j] == relevantMap->targetIDs[i]) { + UtilSendTCPData(MODULE_NAME, transmissionBuffer, messageSize, &objectConnections[j].commandSocket, 0); + } + } + } + return 0; +} + /*! * \brief readMonitorDataTimeout Reads the maximum allowed missing monitor * messages from settings and calculates a timeout period. From 763744bcdf0108615b45b253a483722433efdc87 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 16 Sep 2020 15:55:07 +0200 Subject: [PATCH 3/4] Updated util reference --- util | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/util b/util index 2b7c38275..5ffc61e77 160000 --- a/util +++ b/util @@ -1 +1 @@ -Subproject commit 2b7c3827591269c39853d572f4fdb27593a2ba01 +Subproject commit 5ffc61e7733a133879fdd2f47b64897080a35502 From ede32df06b05104f89254a2cffa288f6812350f8 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 16 Sep 2020 16:13:36 +0200 Subject: [PATCH 4/4] Ran code formatter --- core/src/objectcontrol.c | 25 ++++++++++++------------- 1 file changed, 12 insertions(+), 13 deletions(-) diff --git a/core/src/objectcontrol.c b/core/src/objectcontrol.c index d01d2c8c5..ef32f3d06 100644 --- a/core/src/objectcontrol.c +++ b/core/src/objectcontrol.c @@ -139,11 +139,10 @@ static int configureObjectDataInjection(DataInjectionMap injectionMaps[], const const unsigned int numberOfObjects); static int parseDataInjectionSetting(const char objectFilePath[MAX_FILE_PATH], DataInjectionMap injectionMaps[], const unsigned int numberOfMaps); -static int sendDataInjectionMessages(const ObjectDataType* monitorData, +static int sendDataInjectionMessages(const ObjectDataType * monitorData, const DataInjectionMap dataInjectionMaps[], const ObjectConnection objectConnections[], - const uint32_t transmitterIDs[], - const unsigned int numberOfObjects); + const uint32_t transmitterIDs[], const unsigned int numberOfObjects); static void *objectConnectorThread(void *args); static int checkObjectConnections(ObjectConnection objectConnections[], const struct timeval monitorTimeout, const unsigned int numberOfObjects); @@ -578,7 +577,8 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { pow(monitorData.MonrData.speed.longitudinal_m_s, 2)); // Pass on the information to the configured injection targets - sendDataInjectionMessages(&monitorData, dataInjectionMaps, objectConnections, object_transmitter_ids, nbr_objects); + sendDataInjectionMessages(&monitorData, dataInjectionMaps, objectConnections, + object_transmitter_ids, nbr_objects); } else if (receivedMONRData > 0) LogMessage(LOG_LEVEL_WARNING, "Received unhandled message on monitoring socket"); @@ -2111,22 +2111,20 @@ int parseDataInjectionSetting(const char objectFilePath[MAX_FILE_PATH], * \param numberOfObjects Size of all three arrays. * \return 0 if successful, -1 otherwise. */ -int sendDataInjectionMessages( - const ObjectDataType* objectData, - const DataInjectionMap dataInjectionMaps[], - const ObjectConnection objectConnections[], - const uint32_t transmitterIDs[], - const unsigned int numberOfObjects) { +int sendDataInjectionMessages(const ObjectDataType * objectData, + const DataInjectionMap dataInjectionMaps[], + const ObjectConnection objectConnections[], + const uint32_t transmitterIDs[], const unsigned int numberOfObjects) { if (objectData == NULL || dataInjectionMaps == NULL || objectConnections == NULL) { LogMessage(LOG_LEVEL_ERROR, "Attempted to pass null pointer to %s", __FUNCTION__); return -1; } - const ObjectMonitorType* monitorData = &objectData->MonrData; + const ObjectMonitorType *monitorData = &objectData->MonrData; char transmissionBuffer[1024]; ssize_t messageSize; - const DataInjectionMap* relevantMap = NULL; + const DataInjectionMap *relevantMap = NULL; PeerObjectInjectionType injectionMessage; // Find the map for source of monitor data @@ -2162,7 +2160,8 @@ int sendDataInjectionMessages( for (unsigned int i = 0; i < relevantMap->numberOfTargets; ++i) { for (unsigned int j = 0; j < numberOfObjects; j++) { if (transmitterIDs[j] == relevantMap->targetIDs[i]) { - UtilSendTCPData(MODULE_NAME, transmissionBuffer, messageSize, &objectConnections[j].commandSocket, 0); + UtilSendTCPData(MODULE_NAME, transmissionBuffer, messageSize, + &objectConnections[j].commandSocket, 0); } } }