From 364f36a363bf23363045caa4defc1ea5fc15d77b Mon Sep 17 00:00:00 2001 From: hyle-VM Date: Tue, 7 Jan 2020 02:44:43 +0100 Subject: [PATCH 001/523] Added vizualisation to the buildscript --- buildMaestro.sh | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/buildMaestro.sh b/buildMaestro.sh index bad494e87..37e0277ac 100755 --- a/buildMaestro.sh +++ b/buildMaestro.sh @@ -29,6 +29,12 @@ cd $MAESTRODIR/modules/ScenarioControl/build echo "Building ScenarioControl" cmake .. && make || exit 1 +# Build Visualization module +mkdir $MAESTRODIR/modules/Visualization/build +cd $MAESTRODIR/modules/Visualization/build +echo "Building Visualization" +cmake .. && make || exit 1 + # Build Supervision module mkdir $MAESTRODIR/modules/Supervision/build cd $MAESTRODIR/modules/Supervision/build From 542cb0b09cdd18f45535551ec198a26476c74b26 Mon Sep 17 00:00:00 2001 From: hyle-VM Date: Tue, 7 Jan 2020 02:45:22 +0100 Subject: [PATCH 002/523] save --- modules/Visualization/src/main.c | 11 +++++++++++ runServer.sh | 2 +- 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/modules/Visualization/src/main.c b/modules/Visualization/src/main.c index 9bb2f52c5..1c7e12cd5 100644 --- a/modules/Visualization/src/main.c +++ b/modules/Visualization/src/main.c @@ -84,6 +84,17 @@ int main() { // ((TODO define this protocol clearly - leave this for now)) UtilSendUDPData("Visualization", &visual_server, &visual_server_addr, busReceiveBuffer, sizeof (busReceiveBuffer), 0); + + MonitorDataType monitorData; + int busReceiveBufferLen = sizeof (busReceiveBuffer); + struct timeval monrTime, systemTime; + + + //TimeSetToCurrentSystemTime(&systemTime); + //UtilPopulateMonitorDataStruct(busReceiveBuffer, (size_t) (busReceiveBufferLen), &monitorData, 0); + //TimeSetToGPStime(&monrTime, TimeGetAsGPSweek(&systemTime), monitorData.MONR.GPSQmsOfWeekU32); + printf("HELLO!"); + break; case COMM_LOG: break; diff --git a/runServer.sh b/runServer.sh index e00e986c9..d43a0c019 100755 --- a/runServer.sh +++ b/runServer.sh @@ -2,7 +2,7 @@ #### User settings # Modify this array by adding more modules to include them in the execution -MODULES=(Supervision ScenarioControl) +MODULES=(Visualization) #### # Save top directory From 4d4599aabeac97c630e68d6009e17edcad523968 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 7 Jan 2020 10:57:55 +0100 Subject: [PATCH 003/523] Initial commit --- modules/ObjectMonitoring/CMakeLists.txt | 38 +++++++++++++++++++ modules/ObjectMonitoring/README.md | 18 +++++++++ modules/ObjectMonitoring/src/main.cpp | 50 +++++++++++++++++++++++++ 3 files changed, 106 insertions(+) create mode 100644 modules/ObjectMonitoring/CMakeLists.txt create mode 100644 modules/ObjectMonitoring/README.md create mode 100644 modules/ObjectMonitoring/src/main.cpp diff --git a/modules/ObjectMonitoring/CMakeLists.txt b/modules/ObjectMonitoring/CMakeLists.txt new file mode 100644 index 000000000..f46019f70 --- /dev/null +++ b/modules/ObjectMonitoring/CMakeLists.txt @@ -0,0 +1,38 @@ +cmake_minimum_required(VERSION 2.8) + +project(ObjectMonitoring) +# This module is an example of how to set up a new module external to the Maestro executable + + +include_directories(inc) +include_directories(../../util/C/logging) +include_directories(../../util/C/time) +include_directories(../../util/C/MQBus) +include(GNUInstallDirs) + + +# Create library +add_library(MaestroLogging + ../../util/C/logging/logging.h + ../../util/C/logging/logging.c +) + +add_library(MaestroTime + ../../util/C/time/maestroTime.h + ../../util/C/time/maestroTime.c +) + +add_library(MQBus + ../../util/C/MQBus/mqbus.h + ../../util/C/MQBus/mqbus.c +) + +add_executable(ObjectMonitoring src/main.cpp) + +install(TARGETS ObjectMonitoring DESTINATION bin) + +target_link_libraries(ObjectMonitoring MaestroTime MaestroLogging) +target_link_libraries(MQBus rt m) + + + diff --git a/modules/ObjectMonitoring/README.md b/modules/ObjectMonitoring/README.md new file mode 100644 index 000000000..412eedc66 --- /dev/null +++ b/modules/ObjectMonitoring/README.md @@ -0,0 +1,18 @@ +## Dummy module +This module is an example for how to build a Maestro-external module and connect it via message bus. It is also written in C++ so can serve as an example for how to import Maestro C code and connect to message bus using another code language. + +### Build process +1) Ensure your util repo is up to date +2) Navigate to this README.md file +3) Create the build directory: ```mkdir build``` +4) Enter the build directory: ```cd build``` +5) Generate necessary cmake files: ```cmake ..``` +6) Build the module: ```make``` + +### Run the module +1) Ensure you have built the module +2) Navigate to the build directory +3) Run the module: ```./dummy``` +4) Run Maestro + +Note: steps 3 and 4 can be replaced with running the runServer.sh script in the top directory of this repository diff --git a/modules/ObjectMonitoring/src/main.cpp b/modules/ObjectMonitoring/src/main.cpp new file mode 100644 index 000000000..30ddbbb2b --- /dev/null +++ b/modules/ObjectMonitoring/src/main.cpp @@ -0,0 +1,50 @@ +#include +#include + +#include "logging.h" +#include "util.h" + +#define MODULE_NAME "Dummy" + +int main() +{ + COMMAND command = COMM_INV; + char mqRecvData[MQ_MSG_SIZE]; + const struct timespec sleepTimePeriod = {0,10000000}; + const struct timespec abortWaitTime = {1,0}; + struct timespec remTime; + + LogInit(MODULE_NAME,LOG_LEVEL_DEBUG); + LogMessage(LOG_LEVEL_INFO, "Task running with PID: %u",getpid()); + + // Initialize message bus connection + while(iCommInit()) + { + nanosleep(&sleepTimePeriod,&remTime); + } + + while(true) + { + if (iCommRecv(&command,mqRecvData,MQ_MSG_SIZE,nullptr) < 0) + { + util_error("Message bus receive error"); + } + + switch (command) { + case COMM_INV: + nanosleep(&sleepTimePeriod,&remTime); + break; + case COMM_OBC_STATE: + break; + case COMM_STRT: + nanosleep(&abortWaitTime,&remTime); + LogMessage(LOG_LEVEL_WARNING,"Sending ABORT"); + iCommSend(COMM_ABORT,nullptr,0); + break; + default: + LogMessage(LOG_LEVEL_INFO,"Received command %u",command); + } + } + + return 0; +} From 62f860b11f36f36840b13230af2fd14afc9f12e6 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 7 Jan 2020 10:58:52 +0100 Subject: [PATCH 004/523] Modified README.md to remove dummy stuff --- modules/ObjectMonitoring/README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/modules/ObjectMonitoring/README.md b/modules/ObjectMonitoring/README.md index 412eedc66..e8e7fafc7 100644 --- a/modules/ObjectMonitoring/README.md +++ b/modules/ObjectMonitoring/README.md @@ -1,5 +1,5 @@ -## Dummy module -This module is an example for how to build a Maestro-external module and connect it via message bus. It is also written in C++ so can serve as an example for how to import Maestro C code and connect to message bus using another code language. +## Object monitoring module +TODO ### Build process 1) Ensure your util repo is up to date @@ -12,7 +12,7 @@ This module is an example for how to build a Maestro-external module and connect ### Run the module 1) Ensure you have built the module 2) Navigate to the build directory -3) Run the module: ```./dummy``` +3) Run the module: ```./ObjectMonitoring``` 4) Run Maestro Note: steps 3 and 4 can be replaced with running the runServer.sh script in the top directory of this repository From badc6156aeac47664aa4031cd546abe34629627e Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 7 Jan 2020 12:35:56 +0100 Subject: [PATCH 005/523] Empty connection handler class --- modules/ObjectMonitoring/connectionhandler.cpp | 6 ++++++ modules/ObjectMonitoring/connectionhandler.h | 11 +++++++++++ modules/ObjectMonitoring/inc/connectionhandler.h | 11 +++++++++++ modules/ObjectMonitoring/src/connectionhandler.cpp | 6 ++++++ 4 files changed, 34 insertions(+) create mode 100644 modules/ObjectMonitoring/connectionhandler.cpp create mode 100644 modules/ObjectMonitoring/connectionhandler.h create mode 100644 modules/ObjectMonitoring/inc/connectionhandler.h create mode 100644 modules/ObjectMonitoring/src/connectionhandler.cpp diff --git a/modules/ObjectMonitoring/connectionhandler.cpp b/modules/ObjectMonitoring/connectionhandler.cpp new file mode 100644 index 000000000..642a31be4 --- /dev/null +++ b/modules/ObjectMonitoring/connectionhandler.cpp @@ -0,0 +1,6 @@ +#include "connectionhandler.h" + +ConnectionHandler::ConnectionHandler() +{ + +} diff --git a/modules/ObjectMonitoring/connectionhandler.h b/modules/ObjectMonitoring/connectionhandler.h new file mode 100644 index 000000000..b21d11ce1 --- /dev/null +++ b/modules/ObjectMonitoring/connectionhandler.h @@ -0,0 +1,11 @@ +#ifndef CONNECTIONHANDLER_H +#define CONNECTIONHANDLER_H + + +class ConnectionHandler +{ +public: + ConnectionHandler(); +}; + +#endif // CONNECTIONHANDLER_H \ No newline at end of file diff --git a/modules/ObjectMonitoring/inc/connectionhandler.h b/modules/ObjectMonitoring/inc/connectionhandler.h new file mode 100644 index 000000000..b21d11ce1 --- /dev/null +++ b/modules/ObjectMonitoring/inc/connectionhandler.h @@ -0,0 +1,11 @@ +#ifndef CONNECTIONHANDLER_H +#define CONNECTIONHANDLER_H + + +class ConnectionHandler +{ +public: + ConnectionHandler(); +}; + +#endif // CONNECTIONHANDLER_H \ No newline at end of file diff --git a/modules/ObjectMonitoring/src/connectionhandler.cpp b/modules/ObjectMonitoring/src/connectionhandler.cpp new file mode 100644 index 000000000..642a31be4 --- /dev/null +++ b/modules/ObjectMonitoring/src/connectionhandler.cpp @@ -0,0 +1,6 @@ +#include "connectionhandler.h" + +ConnectionHandler::ConnectionHandler() +{ + +} From f2ed2bf7d729623a1616a7d0701094d34b122971 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 7 Jan 2020 12:36:35 +0100 Subject: [PATCH 006/523] Removed old, updated cmakelists --- modules/ObjectMonitoring/CMakeLists.txt | 9 ++++++++- modules/ObjectMonitoring/connectionhandler.cpp | 6 ------ modules/ObjectMonitoring/connectionhandler.h | 11 ----------- 3 files changed, 8 insertions(+), 18 deletions(-) delete mode 100644 modules/ObjectMonitoring/connectionhandler.cpp delete mode 100644 modules/ObjectMonitoring/connectionhandler.h diff --git a/modules/ObjectMonitoring/CMakeLists.txt b/modules/ObjectMonitoring/CMakeLists.txt index f46019f70..b084f84cc 100644 --- a/modules/ObjectMonitoring/CMakeLists.txt +++ b/modules/ObjectMonitoring/CMakeLists.txt @@ -8,6 +8,7 @@ include_directories(inc) include_directories(../../util/C/logging) include_directories(../../util/C/time) include_directories(../../util/C/MQBus) +include_directories(../../server/inc) include(GNUInstallDirs) @@ -27,11 +28,17 @@ add_library(MQBus ../../util/C/MQBus/mqbus.c ) +add_library(util + ../../server/inc/util.h + ../../server/src/util.c +) + add_executable(ObjectMonitoring src/main.cpp) install(TARGETS ObjectMonitoring DESTINATION bin) -target_link_libraries(ObjectMonitoring MaestroTime MaestroLogging) +target_link_libraries(ObjectMonitoring MaestroTime MaestroLogging util) +target_link_libraries(util MQBus MaestroLogging MaestroTime) target_link_libraries(MQBus rt m) diff --git a/modules/ObjectMonitoring/connectionhandler.cpp b/modules/ObjectMonitoring/connectionhandler.cpp deleted file mode 100644 index 642a31be4..000000000 --- a/modules/ObjectMonitoring/connectionhandler.cpp +++ /dev/null @@ -1,6 +0,0 @@ -#include "connectionhandler.h" - -ConnectionHandler::ConnectionHandler() -{ - -} diff --git a/modules/ObjectMonitoring/connectionhandler.h b/modules/ObjectMonitoring/connectionhandler.h deleted file mode 100644 index b21d11ce1..000000000 --- a/modules/ObjectMonitoring/connectionhandler.h +++ /dev/null @@ -1,11 +0,0 @@ -#ifndef CONNECTIONHANDLER_H -#define CONNECTIONHANDLER_H - - -class ConnectionHandler -{ -public: - ConnectionHandler(); -}; - -#endif // CONNECTIONHANDLER_H \ No newline at end of file From 23d18ea02decc01cf60ad6c347761edf2f95726c Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 7 Jan 2020 17:04:04 +0100 Subject: [PATCH 007/523] Added class for holding decoded protocol data --- modules/ObjectMonitoring/CMakeLists.txt | 17 ++++- .../ObjectMonitoring/inc/connectionhandler.h | 19 +++++- modules/ObjectMonitoring/inc/protocoldata.h | 24 +++++++ .../src/connectionhandler.cpp | 63 ++++++++++++++++++- modules/ObjectMonitoring/src/protocoldata.cpp | 3 + 5 files changed, 120 insertions(+), 6 deletions(-) create mode 100644 modules/ObjectMonitoring/inc/protocoldata.h create mode 100644 modules/ObjectMonitoring/src/protocoldata.cpp diff --git a/modules/ObjectMonitoring/CMakeLists.txt b/modules/ObjectMonitoring/CMakeLists.txt index b084f84cc..8f63d0c13 100644 --- a/modules/ObjectMonitoring/CMakeLists.txt +++ b/modules/ObjectMonitoring/CMakeLists.txt @@ -33,11 +33,24 @@ add_library(util ../../server/src/util.c ) -add_executable(ObjectMonitoring src/main.cpp) +add_executable(ObjectMonitoring + src/main.cpp +) + +add_library(ConnectionHandler + src/connectionhandler.cpp + inc/connectionhandler.h +) + +add_library(ProtocolData + src/protocoldata.cpp + inc/protocoldata.h +) install(TARGETS ObjectMonitoring DESTINATION bin) -target_link_libraries(ObjectMonitoring MaestroTime MaestroLogging util) +target_link_libraries(ObjectMonitoring MaestroTime MaestroLogging util ConnectionHandler) +target_link_libraries(ConnectionHandler ProtocolData) target_link_libraries(util MQBus MaestroLogging MaestroTime) target_link_libraries(MQBus rt m) diff --git a/modules/ObjectMonitoring/inc/connectionhandler.h b/modules/ObjectMonitoring/inc/connectionhandler.h index b21d11ce1..aaa5e4cd1 100644 --- a/modules/ObjectMonitoring/inc/connectionhandler.h +++ b/modules/ObjectMonitoring/inc/connectionhandler.h @@ -1,11 +1,26 @@ #ifndef CONNECTIONHANDLER_H #define CONNECTIONHANDLER_H +#include +#include "protocoldata.h" class ConnectionHandler { public: - ConnectionHandler(); + ConnectionHandler(int openSocketDescriptor); + ConnectionHandler(int openSocketDescriptor, unsigned long readBufferSize); + +private: + int socketDescriptor = 0; + unsigned long readBufferSize; + pthread_t readThread; + constexpr static unsigned long DefaultReadBufferSize = 4096; + ProtocolData data; + + void* threadRoutine(void*); + static void* routineWrapper(void* context) { + return static_cast(context)->threadRoutine(nullptr); + } }; -#endif // CONNECTIONHANDLER_H \ No newline at end of file +#endif // CONNECTIONHANDLER_H diff --git a/modules/ObjectMonitoring/inc/protocoldata.h b/modules/ObjectMonitoring/inc/protocoldata.h new file mode 100644 index 000000000..01f706e1a --- /dev/null +++ b/modules/ObjectMonitoring/inc/protocoldata.h @@ -0,0 +1,24 @@ +#ifndef PROTOCOLDATA_H +#define PROTOCOLDATA_H + +#include + +class ProtocolData +{ +public: + ProtocolData(); + + typedef enum { + OK, + DECODE_INCOMPLETE, + DECODE_SUCCESSFUL, + DECODE_ERROR + } ReturnCode; + + virtual ReturnCode decodeFromData(); + +private: + +}; + +#endif // PROTOCOLDATA_H diff --git a/modules/ObjectMonitoring/src/connectionhandler.cpp b/modules/ObjectMonitoring/src/connectionhandler.cpp index 642a31be4..16d52948a 100644 --- a/modules/ObjectMonitoring/src/connectionhandler.cpp +++ b/modules/ObjectMonitoring/src/connectionhandler.cpp @@ -1,6 +1,65 @@ #include "connectionhandler.h" +#include "logging.h" +#include +#include +#include +#include +#include +#include +#include +#include -ConnectionHandler::ConnectionHandler() -{ +typedef void* (*ThreadFunctionPointerType)(void *); +ConnectionHandler::ConnectionHandler(int openSocketDescriptor) + : ConnectionHandler::ConnectionHandler(openSocketDescriptor, DefaultReadBufferSize) {} + +ConnectionHandler::ConnectionHandler(int openSocketDescriptor, unsigned long readBufferSize) { + int returnCode; + if (openSocketDescriptor < 0) { + LogMessage(LOG_LEVEL_ERROR, "Attempted to create connection handler with invalid socket descriptor"); + return; + } + socketDescriptor = openSocketDescriptor; + + if (readBufferSize == 0) { + LogMessage(LOG_LEVEL_ERROR, "Attempted to create connection handler with invalid socket descriptor"); + return; + } + this->readBufferSize = readBufferSize; + + LogMessage(LOG_LEVEL_INFO, "Creating thread to handle connection"); + returnCode = pthread_create(&readThread, nullptr, &ConnectionHandler::routineWrapper, this); + if (returnCode) { + throw std::system_error(); + } +} + + +void* ConnectionHandler::threadRoutine(void*) { + struct sockaddr_in address; + socklen_t addrlen = sizeof (address); + std::vector readBuffer(readBufferSize); + std::vector messageBuffer; + ssize_t readBytes; + + socketDescriptor = accept(socketDescriptor, (struct sockaddr *)&address, static_cast(&addrlen)); + if (socketDescriptor < 0) { + pthread_exit(static_cast(&socketDescriptor)); + } + + while ( (readBytes = read(socketDescriptor, readBuffer.data(), readBuffer.size()) ) > 0) { + messageBuffer.insert(messageBuffer.end(), readBuffer.begin(), readBuffer.end()); + // TODO: Decode raw data into MQ friendly data + + } + + if (readBytes < 0) { + LogMessage(LOG_LEVEL_ERROR, "Socket read error"); + pthread_exit(static_cast(&readBytes)); + } + else { + LogMessage(LOG_LEVEL_INFO, "Remote closed connection"); + pthread_exit(nullptr); + } } diff --git a/modules/ObjectMonitoring/src/protocoldata.cpp b/modules/ObjectMonitoring/src/protocoldata.cpp new file mode 100644 index 000000000..a755ab10a --- /dev/null +++ b/modules/ObjectMonitoring/src/protocoldata.cpp @@ -0,0 +1,3 @@ +#include "protocoldata.h" +#include "logging.h" + From 4f01d3e59ed8c036d70f4e4407999ba29c81a89c Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 7 Jan 2020 17:04:34 +0100 Subject: [PATCH 008/523] Minor update to main function --- modules/ObjectMonitoring/src/main.cpp | 30 +++++---------------------- 1 file changed, 5 insertions(+), 25 deletions(-) diff --git a/modules/ObjectMonitoring/src/main.cpp b/modules/ObjectMonitoring/src/main.cpp index 30ddbbb2b..906985bd8 100644 --- a/modules/ObjectMonitoring/src/main.cpp +++ b/modules/ObjectMonitoring/src/main.cpp @@ -4,7 +4,7 @@ #include "logging.h" #include "util.h" -#define MODULE_NAME "Dummy" +#define MODULE_NAME "ObjectMonitoring" int main() { @@ -15,35 +15,15 @@ int main() struct timespec remTime; LogInit(MODULE_NAME,LOG_LEVEL_DEBUG); - LogMessage(LOG_LEVEL_INFO, "Task running with PID: %u",getpid()); + LogMessage(LOG_LEVEL_INFO, "Task running with PID: %u", getpid()); // Initialize message bus connection - while(iCommInit()) - { + while(iCommInit()) { nanosleep(&sleepTimePeriod,&remTime); } - while(true) - { - if (iCommRecv(&command,mqRecvData,MQ_MSG_SIZE,nullptr) < 0) - { - util_error("Message bus receive error"); - } - - switch (command) { - case COMM_INV: - nanosleep(&sleepTimePeriod,&remTime); - break; - case COMM_OBC_STATE: - break; - case COMM_STRT: - nanosleep(&abortWaitTime,&remTime); - LogMessage(LOG_LEVEL_WARNING,"Sending ABORT"); - iCommSend(COMM_ABORT,nullptr,0); - break; - default: - LogMessage(LOG_LEVEL_INFO,"Received command %u",command); - } + while(true) { + } return 0; From c2f6050644e5a34349f156a87c6434432fb86e3e Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 9 Jan 2020 10:15:02 +0100 Subject: [PATCH 009/523] Made a class for ISO22133 communication data --- modules/ObjectMonitoring/inc/ISO22133ProtocolData.h | 13 +++++++++++++ .../ObjectMonitoring/src/ISO22133ProtocolData.cpp | 9 +++++++++ 2 files changed, 22 insertions(+) create mode 100644 modules/ObjectMonitoring/inc/ISO22133ProtocolData.h create mode 100644 modules/ObjectMonitoring/src/ISO22133ProtocolData.cpp diff --git a/modules/ObjectMonitoring/inc/ISO22133ProtocolData.h b/modules/ObjectMonitoring/inc/ISO22133ProtocolData.h new file mode 100644 index 000000000..49d90194f --- /dev/null +++ b/modules/ObjectMonitoring/inc/ISO22133ProtocolData.h @@ -0,0 +1,13 @@ +#ifndef ISO22133PROTOCOLDATA_H +#define ISO22133PROTOCOLDATA_H + +#include "protocoldata.h" + +class ISO22133ProtocolData : public ProtocolData { +public: + ISO22133ProtocolData() {} + ~ISO22133ProtocolData() override; + ReturnCode decodeFrom(const std::vector& rawData) override; +}; + +#endif // PROTOCOLDATA_H diff --git a/modules/ObjectMonitoring/src/ISO22133ProtocolData.cpp b/modules/ObjectMonitoring/src/ISO22133ProtocolData.cpp new file mode 100644 index 000000000..90436f4ba --- /dev/null +++ b/modules/ObjectMonitoring/src/ISO22133ProtocolData.cpp @@ -0,0 +1,9 @@ +#include "ISO22133ProtocolData.h" + +ISO22133ProtocolData::~ISO22133ProtocolData() { + +} + +ProtocolData::ReturnCode ISO22133ProtocolData::decodeFrom(const std::vector &rawData) { + return DECODE_ERROR; +} From 58d2856d312dba174780d07da504cbac0a9222e9 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 9 Jan 2020 10:16:01 +0100 Subject: [PATCH 010/523] Implemented use of connection handler and protocol data objects --- modules/ObjectMonitoring/CMakeLists.txt | 11 +++- .../ObjectMonitoring/inc/connectionhandler.h | 10 ++-- modules/ObjectMonitoring/inc/protocoldata.h | 20 +++++--- .../src/connectionhandler.cpp | 51 +++++++++++-------- modules/ObjectMonitoring/src/main.cpp | 37 +++++++++++++- modules/ObjectMonitoring/src/protocoldata.cpp | 3 +- 6 files changed, 97 insertions(+), 35 deletions(-) diff --git a/modules/ObjectMonitoring/CMakeLists.txt b/modules/ObjectMonitoring/CMakeLists.txt index 8f63d0c13..171cb1933 100644 --- a/modules/ObjectMonitoring/CMakeLists.txt +++ b/modules/ObjectMonitoring/CMakeLists.txt @@ -1,4 +1,8 @@ -cmake_minimum_required(VERSION 2.8) +cmake_minimum_required(VERSION 3.1) +SET(CMAKE_CXX_STANDARD 11) +SET(CMAKE_CXX_STANDARD_REQUIRED ON) +SET(CMAKE_C_STANDARD 11) +SET(CMAKE_C_STANDARD_REQUIRED ON) project(ObjectMonitoring) # This module is an example of how to set up a new module external to the Maestro executable @@ -11,6 +15,7 @@ include_directories(../../util/C/MQBus) include_directories(../../server/inc) include(GNUInstallDirs) +find_package(Threads) # Create library add_library(MaestroLogging @@ -45,12 +50,14 @@ add_library(ConnectionHandler add_library(ProtocolData src/protocoldata.cpp inc/protocoldata.h + src/ISO22133ProtocolData.cpp + inc/ISO22133ProtocolData.h ) install(TARGETS ObjectMonitoring DESTINATION bin) target_link_libraries(ObjectMonitoring MaestroTime MaestroLogging util ConnectionHandler) -target_link_libraries(ConnectionHandler ProtocolData) +target_link_libraries(ConnectionHandler ProtocolData ${CMAKE_THREAD_LIBS_INIT}) target_link_libraries(util MQBus MaestroLogging MaestroTime) target_link_libraries(MQBus rt m) diff --git a/modules/ObjectMonitoring/inc/connectionhandler.h b/modules/ObjectMonitoring/inc/connectionhandler.h index aaa5e4cd1..7e18a97fe 100644 --- a/modules/ObjectMonitoring/inc/connectionhandler.h +++ b/modules/ObjectMonitoring/inc/connectionhandler.h @@ -7,20 +7,24 @@ class ConnectionHandler { public: - ConnectionHandler(int openSocketDescriptor); - ConnectionHandler(int openSocketDescriptor, unsigned long readBufferSize); + ConnectionHandler(int openSocketDescriptor, ProtocolData& data); + ConnectionHandler(int openSocketDescriptor, ProtocolData& data, unsigned long readBufferSize); + ~ConnectionHandler(); + bool isTerminated() { return terminated; } private: int socketDescriptor = 0; + ProtocolData &data; unsigned long readBufferSize; pthread_t readThread; + bool terminated = false; constexpr static unsigned long DefaultReadBufferSize = 4096; - ProtocolData data; void* threadRoutine(void*); static void* routineWrapper(void* context) { return static_cast(context)->threadRoutine(nullptr); } + [[ noreturn ]] void terminate(void* retval); }; #endif // CONNECTIONHANDLER_H diff --git a/modules/ObjectMonitoring/inc/protocoldata.h b/modules/ObjectMonitoring/inc/protocoldata.h index 01f706e1a..4d4e6fe02 100644 --- a/modules/ObjectMonitoring/inc/protocoldata.h +++ b/modules/ObjectMonitoring/inc/protocoldata.h @@ -2,20 +2,28 @@ #define PROTOCOLDATA_H #include +#include +#include +#include +#include +#include -class ProtocolData -{ +class ProtocolData { public: - ProtocolData(); + ProtocolData() = default; + ProtocolData(const ProtocolData&) = default; + ProtocolData(ProtocolData&&) = default; + ProtocolData& operator=(const ProtocolData&) = default; + ProtocolData& operator=(ProtocolData&&) = default; + virtual ~ProtocolData(); typedef enum { - OK, - DECODE_INCOMPLETE, + DECODE_PARTIAL, DECODE_SUCCESSFUL, DECODE_ERROR } ReturnCode; - virtual ReturnCode decodeFromData(); + virtual ReturnCode decodeFrom(const std::vector &rawData) = 0; private: diff --git a/modules/ObjectMonitoring/src/connectionhandler.cpp b/modules/ObjectMonitoring/src/connectionhandler.cpp index 16d52948a..7a6dc0e0e 100644 --- a/modules/ObjectMonitoring/src/connectionhandler.cpp +++ b/modules/ObjectMonitoring/src/connectionhandler.cpp @@ -1,21 +1,17 @@ #include "connectionhandler.h" #include "logging.h" + #include #include -#include #include -#include -#include -#include #include -typedef void* (*ThreadFunctionPointerType)(void *); - -ConnectionHandler::ConnectionHandler(int openSocketDescriptor) - : ConnectionHandler::ConnectionHandler(openSocketDescriptor, DefaultReadBufferSize) {} +ConnectionHandler::ConnectionHandler(int openSocketDescriptor, ProtocolData& data) + : ConnectionHandler::ConnectionHandler(openSocketDescriptor, data, DefaultReadBufferSize) {} -ConnectionHandler::ConnectionHandler(int openSocketDescriptor, unsigned long readBufferSize) { +ConnectionHandler::ConnectionHandler(int openSocketDescriptor, ProtocolData& data, unsigned long readBufferSize) : data(data) { int returnCode; + if (openSocketDescriptor < 0) { LogMessage(LOG_LEVEL_ERROR, "Attempted to create connection handler with invalid socket descriptor"); return; @@ -23,7 +19,7 @@ ConnectionHandler::ConnectionHandler(int openSocketDescriptor, unsigned long rea socketDescriptor = openSocketDescriptor; if (readBufferSize == 0) { - LogMessage(LOG_LEVEL_ERROR, "Attempted to create connection handler with invalid socket descriptor"); + LogMessage(LOG_LEVEL_ERROR, "Attempted to create connection handler with zero buffer size"); return; } this->readBufferSize = readBufferSize; @@ -31,35 +27,48 @@ ConnectionHandler::ConnectionHandler(int openSocketDescriptor, unsigned long rea LogMessage(LOG_LEVEL_INFO, "Creating thread to handle connection"); returnCode = pthread_create(&readThread, nullptr, &ConnectionHandler::routineWrapper, this); if (returnCode) { + LogMessage(LOG_LEVEL_ERROR, "Error creating thread"); + close(socketDescriptor); + terminated = true; throw std::system_error(); } } +ConnectionHandler::~ConnectionHandler() { + close(socketDescriptor); +} + void* ConnectionHandler::threadRoutine(void*) { - struct sockaddr_in address; - socklen_t addrlen = sizeof (address); std::vector readBuffer(readBufferSize); std::vector messageBuffer; ssize_t readBytes; - socketDescriptor = accept(socketDescriptor, (struct sockaddr *)&address, static_cast(&addrlen)); - if (socketDescriptor < 0) { - pthread_exit(static_cast(&socketDescriptor)); - } - while ( (readBytes = read(socketDescriptor, readBuffer.data(), readBuffer.size()) ) > 0) { messageBuffer.insert(messageBuffer.end(), readBuffer.begin(), readBuffer.end()); - // TODO: Decode raw data into MQ friendly data - + switch (data.decodeFrom(messageBuffer)) { + case ProtocolData::DECODE_PARTIAL: + break; + case ProtocolData::DECODE_SUCCESSFUL: + messageBuffer.clear(); + break; + case ProtocolData::DECODE_ERROR: + messageBuffer.clear(); + break; + } } if (readBytes < 0) { LogMessage(LOG_LEVEL_ERROR, "Socket read error"); - pthread_exit(static_cast(&readBytes)); + terminate(static_cast(&readBytes)); } else { LogMessage(LOG_LEVEL_INFO, "Remote closed connection"); - pthread_exit(nullptr); + terminate(nullptr); } } + +void ConnectionHandler::terminate(void *retval) { + terminated = true; + pthread_exit(retval); +} diff --git a/modules/ObjectMonitoring/src/main.cpp b/modules/ObjectMonitoring/src/main.cpp index 906985bd8..5048bdf44 100644 --- a/modules/ObjectMonitoring/src/main.cpp +++ b/modules/ObjectMonitoring/src/main.cpp @@ -1,10 +1,20 @@ #include #include +#include +#include #include "logging.h" +#include "connectionhandler.h" +#include "protocoldata.h" +#include "ISO22133ProtocolData.h" #include "util.h" #define MODULE_NAME "ObjectMonitoring" +#define ISO22133_PORT 53240 + +using namespace std; + +static void handleNewConnection(int socketDescriptor, vector &handlers); int main() { @@ -12,9 +22,10 @@ int main() char mqRecvData[MQ_MSG_SIZE]; const struct timespec sleepTimePeriod = {0,10000000}; const struct timespec abortWaitTime = {1,0}; - struct timespec remTime; + struct timespec remTime; + vector handlers; - LogInit(MODULE_NAME,LOG_LEVEL_DEBUG); + LogInit(MODULE_NAME, LOG_LEVEL_DEBUG); LogMessage(LOG_LEVEL_INFO, "Task running with PID: %u", getpid()); // Initialize message bus connection @@ -23,8 +34,30 @@ int main() } while(true) { + // TODO: listen + + handleNewConnection(0, handlers); } return 0; } + +void handleNewConnection(int socketDescriptor, vector &handlers) { + struct sockaddr_in socketAddress; + socklen_t addressLength = sizeof (socketAddress); + getsockname(socketDescriptor, (struct sockaddr *)&socketAddress, &addressLength); + + switch(socketAddress.sin_port) { + case ISO22133_PORT: + { + ISO22133ProtocolData protoData; + handlers.push_back(new ConnectionHandler(socketDescriptor, protoData)); + break; + } + default: + LogMessage(LOG_LEVEL_WARNING, "New connection made but no protocol specified for connected port: closing connection"); + close(socketDescriptor); + return; + } +} diff --git a/modules/ObjectMonitoring/src/protocoldata.cpp b/modules/ObjectMonitoring/src/protocoldata.cpp index a755ab10a..4f721bc34 100644 --- a/modules/ObjectMonitoring/src/protocoldata.cpp +++ b/modules/ObjectMonitoring/src/protocoldata.cpp @@ -1,3 +1,4 @@ #include "protocoldata.h" -#include "logging.h" +ProtocolData::~ProtocolData() { +} From 32af67e9ddbc83a0e5f8b91582e5f46c5950f8bd Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 10 Jan 2020 09:20:24 +0100 Subject: [PATCH 011/523] Moved port definition to class --- .../inc/ISO22133ProtocolData.h | 2 ++ modules/ObjectMonitoring/src/main.cpp | 22 +++++++++++++++++-- 2 files changed, 22 insertions(+), 2 deletions(-) diff --git a/modules/ObjectMonitoring/inc/ISO22133ProtocolData.h b/modules/ObjectMonitoring/inc/ISO22133ProtocolData.h index 49d90194f..d55ea9a50 100644 --- a/modules/ObjectMonitoring/inc/ISO22133ProtocolData.h +++ b/modules/ObjectMonitoring/inc/ISO22133ProtocolData.h @@ -5,6 +5,8 @@ class ISO22133ProtocolData : public ProtocolData { public: + static constexpr uint16_t TCP_PORT = 12345; + ISO22133ProtocolData() {} ~ISO22133ProtocolData() override; ReturnCode decodeFrom(const std::vector& rawData) override; diff --git a/modules/ObjectMonitoring/src/main.cpp b/modules/ObjectMonitoring/src/main.cpp index 5048bdf44..afb5cbf4e 100644 --- a/modules/ObjectMonitoring/src/main.cpp +++ b/modules/ObjectMonitoring/src/main.cpp @@ -2,6 +2,7 @@ #include #include #include +#include #include "logging.h" #include "connectionhandler.h" @@ -15,6 +16,8 @@ using namespace std; static void handleNewConnection(int socketDescriptor, vector &handlers); +static void pruneTerminatedConnectionHandlers(vector &handlers); +static void listenForNewConnection(void); int main() { @@ -34,10 +37,11 @@ int main() } while(true) { - // TODO: listen + listenForNewConnection(); handleNewConnection(0, handlers); + pruneTerminatedConnectionHandlers(handlers); } return 0; @@ -49,7 +53,7 @@ void handleNewConnection(int socketDescriptor, vector &handl getsockname(socketDescriptor, (struct sockaddr *)&socketAddress, &addressLength); switch(socketAddress.sin_port) { - case ISO22133_PORT: + case ISO22133ProtocolData::TCP_PORT: { ISO22133ProtocolData protoData; handlers.push_back(new ConnectionHandler(socketDescriptor, protoData)); @@ -61,3 +65,17 @@ void handleNewConnection(int socketDescriptor, vector &handl return; } } + +void pruneTerminatedConnectionHandlers(vector &handlers) { + // Remove any connection handlers which are null pointers + handlers.erase( std::remove(handlers.begin(), handlers.end(), nullptr), handlers.end() ); + // Remove any connection handlers which have finished their tasks + handlers.erase( std::remove_if(handlers.begin(), handlers.end(), + [](const ConnectionHandler* handler) { handler->isTerminated(); } + ), handlers.end() ); +} + + +void listenForNewConnection(void) { + // TODO +} From 255344e5f7d480ca3cb6f44c938ab07bcb44e5ff Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 10 Jan 2020 09:20:49 +0100 Subject: [PATCH 012/523] Made getter const --- modules/ObjectMonitoring/inc/connectionhandler.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/ObjectMonitoring/inc/connectionhandler.h b/modules/ObjectMonitoring/inc/connectionhandler.h index 7e18a97fe..150428c52 100644 --- a/modules/ObjectMonitoring/inc/connectionhandler.h +++ b/modules/ObjectMonitoring/inc/connectionhandler.h @@ -11,7 +11,7 @@ class ConnectionHandler ConnectionHandler(int openSocketDescriptor, ProtocolData& data, unsigned long readBufferSize); ~ConnectionHandler(); - bool isTerminated() { return terminated; } + bool isTerminated() const { return terminated; } private: int socketDescriptor = 0; ProtocolData &data; From 3d5666faad7763878996671029182161c63240e4 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 10 Jan 2020 16:11:52 +0100 Subject: [PATCH 013/523] Added function which handles partial or stacked messages from recv() --- server/src/systemcontrol.c | 34 +++++++++++++++++++++++++++++++++- 1 file changed, 33 insertions(+), 1 deletion(-) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index 68adcba69..c650219ef 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -187,6 +187,7 @@ I32 SystemControlBuildRVSSMaestroChannelMessage(C8 * RVSSData, U32 * RVSSDataLen I32 SystemControlBuildRVSSAspChannelMessage(C8 * RVSSData, U32 * RVSSDataLengthU32, U8 Debug); I32 SystemControlBuildRVSSMONRChannelMessage(C8 * RVSSData, U32 * RVSSDataLengthU32, MonitorDataType MonrData, U8 Debug); +static ssize_t SystemControlReceiveUserControlData(I32 socket, C8 * dataBuffer, size_t dataBufferLength); static C8 SystemControlVerifyHostAddress(char *ip); static void signalHandler(int signo); @@ -354,7 +355,7 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { PreviousSystemControlCommand = SystemControlCommand; bzero(pcBuffer, IPC_BUFFER_SIZE); - ClientResult = recv(ClientSocket, pcBuffer, IPC_BUFFER_SIZE, MSG_DONTWAIT); + ClientResult = SystemControlReceiveUserControlData(ClientSocket, pcBuffer, sizeof (pcBuffer)); if (ClientResult <= -1) { if (errno != EAGAIN && errno != EWOULDBLOCK) { @@ -1166,6 +1167,37 @@ SystemControlCommand_t SystemControlFindCommand(const char *CommandBuffer, return nocommand; } + +ssize_t SystemControlReceiveUserControlData(I32 socket, C8 * dataBuffer, size_t dataBufferLength) { + static char recvBuffer[TCP_RECV_BUFFER_SIZE]; + static size_t bytesInBuffer = 0; + ssize_t readResult; + uint32_t messageLength = 0; + + readResult = recv(socket, recvBuffer + bytesInBuffer, sizeof (recvBuffer) - bytesInBuffer, MSG_DONTWAIT); + + if (readResult > 0) { + bytesInBuffer += (size_t) readResult; + memcpy(&messageLength, recvBuffer, sizeof (messageLength)); + messageLength = ntohl(messageLength); + + if (bytesInBuffer >= messageLength + sizeof (messageLength)) { + if (dataBufferLength < messageLength + sizeof (messageLength)) { + LogMessage(LOG_LEVEL_WARNING, "Discarding message too large for data buffer"); + readResult = -1; + errno = ENOBUFS; + } + else { + memcpy(dataBuffer, recvBuffer, messageLength + sizeof (messageLength)); + } + bytesInBuffer -= messageLength + sizeof (messageLength); + memmove(recvBuffer, recvBuffer + messageLength + sizeof (messageLength), bytesInBuffer); + } + } + return readResult; +} + + void SystemControlSendMONR(C8 * MONRStr, I32 * Sockfd, U8 Debug) { int i, n, j, t; C8 Length[4]; From 1ae647e2ef73de36a4efbdfc9ef01ddf6c663f31 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 10 Jan 2020 17:51:19 +0100 Subject: [PATCH 014/523] Readjusted for missing message length --- server/src/systemcontrol.c | 27 ++++++++++++++++++--------- 1 file changed, 18 insertions(+), 9 deletions(-) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index c650219ef..8767d0757 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -1171,27 +1171,36 @@ SystemControlCommand_t SystemControlFindCommand(const char *CommandBuffer, ssize_t SystemControlReceiveUserControlData(I32 socket, C8 * dataBuffer, size_t dataBufferLength) { static char recvBuffer[TCP_RECV_BUFFER_SIZE]; static size_t bytesInBuffer = 0; + const char endOfMessagePattern[] = ";\r\n\r\n"; + char* endOfMessage = NULL; ssize_t readResult; - uint32_t messageLength = 0; + size_t messageLength = 0; readResult = recv(socket, recvBuffer + bytesInBuffer, sizeof (recvBuffer) - bytesInBuffer, MSG_DONTWAIT); - if (readResult > 0) { bytesInBuffer += (size_t) readResult; - memcpy(&messageLength, recvBuffer, sizeof (messageLength)); - messageLength = ntohl(messageLength); - if (bytesInBuffer >= messageLength + sizeof (messageLength)) { - if (dataBufferLength < messageLength + sizeof (messageLength)) { + if ((endOfMessage = strstr(recvBuffer, endOfMessagePattern)) != NULL) { + endOfMessage += sizeof (endOfMessagePattern) - 1; + messageLength = (size_t)(endOfMessage - recvBuffer); + } + else { + messageLength = 0; + readResult = -1; + errno = EAGAIN; + } + + if (bytesInBuffer >= messageLength) { + if (dataBufferLength < messageLength) { LogMessage(LOG_LEVEL_WARNING, "Discarding message too large for data buffer"); readResult = -1; errno = ENOBUFS; } else { - memcpy(dataBuffer, recvBuffer, messageLength + sizeof (messageLength)); + memcpy(dataBuffer, recvBuffer, messageLength); } - bytesInBuffer -= messageLength + sizeof (messageLength); - memmove(recvBuffer, recvBuffer + messageLength + sizeof (messageLength), bytesInBuffer); + bytesInBuffer -= messageLength; + memmove(recvBuffer, recvBuffer + messageLength, bytesInBuffer); } } return readResult; From a2e4355f84407023135cecf4885d0fb43fd2342b Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 10 Jan 2020 18:01:46 +0100 Subject: [PATCH 015/523] Added \r\n\r\n to end of MSCP messages in tests --- server/integration-tests/tools/MSCP.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/server/integration-tests/tools/MSCP.py b/server/integration-tests/tools/MSCP.py index 7a7315311..9e4913eb3 100644 --- a/server/integration-tests/tools/MSCP.py +++ b/server/integration-tests/tools/MSCP.py @@ -162,43 +162,43 @@ def listen(self): self.uploadReplyLock.release() def GetStatus(self): - message = "POST /maestro HTTP/1.1\r\nHost: " + self.host + "\r\n\r\nGetServerStatus();" + message = "POST /maestro HTTP/1.1\r\nHost: " + self.host + "\r\n\r\nGetServerStatus();\r\n\r\n" self.Send(message) print("=== GetServerStatus() sent") def Abort(self): - message = "POST /maestro HTTP/1.1\r\nHost:" + self.host + "\r\n\r\nAbortScenario();" + message = "POST /maestro HTTP/1.1\r\nHost:" + self.host + "\r\n\r\nAbortScenario();\r\n\r\n" self.Send(message) print("=== Abort() sent") def Arm(self): - message = "POST /maestro HTTP/1.1\r\nHost:" + self.host + "\r\n\r\nArmScenario();" + message = "POST /maestro HTTP/1.1\r\nHost:" + self.host + "\r\n\r\nArmScenario();\r\n\r\n" self.Send(message) print("=== ArmScenario() sent") def Init(self): - message = "POST /maestro HTTP/1.1\r\nHost:" + self.host + "\r\n\r\nInitializeScenario();" + message = "POST /maestro HTTP/1.1\r\nHost:" + self.host + "\r\n\r\nInitializeScenario();\r\n\r\n" self.Send(message) print("=== Init() sent") def Connect(self): - message = "POST /maestro HTTP/1.1\r\nHost:" + self.host + "\r\n\r\nConnectObject();" + message = "POST /maestro HTTP/1.1\r\nHost:" + self.host + "\r\n\r\nConnectObject();\r\n\r\n" self.Send(message) print("=== Connect() sent") def Disconnect(self): - message = "POST /maestro HTTP/1.1\r\nHost:" + self.host + "\r\n\r\nDisconnectObject();" + message = "POST /maestro HTTP/1.1\r\nHost:" + self.host + "\r\n\r\nDisconnectObject();\r\n\r\n" self.Send(message) print("=== Disconnect() sent") def Start(self,delayTime_ms): - message = "POST /maestro HTTP/1.1\r\nHost:" + self.host + "\r\n\r\nStartScenario(" + str(delayTime_ms) + ");" + message = "POST /maestro HTTP/1.1\r\nHost:" + self.host + "\r\n\r\nStartScenario(" + str(delayTime_ms) + ");\r\n\r\n" self.Send(message) print("=== StartScenario() sent") def UploadFile(self,targetPath,fileContents): packetSize = 1200 - message = "POST /maestro HTTP/1.1\r\nHost:" + self.host + "\r\n\r\nUploadFile(" + targetPath + "," + str(len(fileContents)) + "," + str(packetSize) + ");" + message = "POST /maestro HTTP/1.1\r\nHost:" + self.host + "\r\n\r\nUploadFile(" + targetPath + "," + str(len(fileContents)) + "," + str(packetSize) + ");\r\n\r\n" self.uploadReplyLock.acquire() self.lastUploadReply["status"] = "UNKNOWN" self.uploadReplyLock.release() From 73f986330eb36e417be5c5d2f70bbdb5459edd79 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 10 Jan 2020 19:31:01 +0100 Subject: [PATCH 016/523] Made systemcontrol not sleep when it has recently received usercontrol communication --- server/src/systemcontrol.c | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index 8767d0757..ff8ea85cf 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -381,6 +381,7 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { server_state == SERVER_STATE_UNDEFINED; // TODO: Should this be an assignment? } else if (ClientResult > 0 && ClientResult < TCP_RECV_BUFFER_SIZE) { + LogPrint("HERE: %s", pcBuffer); // TODO: Move this entire decoding process into a separate function for (i = 0; i < SYSTEM_CONTROL_ARG_MAX_COUNT; i++) bzero(SystemControlArgument[i], SYSTEM_CONTROL_ARGUMENT_MAX_LENGTH); @@ -1110,7 +1111,8 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { } sleep_time = (iCommand == COMM_INV - && server_state != SERVER_STATE_INWORK) ? mqEmptyPollPeriod : mqNonEmptyPollPeriod; + && server_state != SERVER_STATE_INWORK + && ClientResult < 0) ? mqEmptyPollPeriod : mqNonEmptyPollPeriod; nanosleep(&sleep_time, &ref_time); } @@ -1172,17 +1174,18 @@ ssize_t SystemControlReceiveUserControlData(I32 socket, C8 * dataBuffer, size_t static char recvBuffer[TCP_RECV_BUFFER_SIZE]; static size_t bytesInBuffer = 0; const char endOfMessagePattern[] = ";\r\n\r\n"; - char* endOfMessage = NULL; + char *endOfMessage = NULL; ssize_t readResult; size_t messageLength = 0; readResult = recv(socket, recvBuffer + bytesInBuffer, sizeof (recvBuffer) - bytesInBuffer, MSG_DONTWAIT); if (readResult > 0) { bytesInBuffer += (size_t) readResult; - + } + if (bytesInBuffer > 0) { if ((endOfMessage = strstr(recvBuffer, endOfMessagePattern)) != NULL) { endOfMessage += sizeof (endOfMessagePattern) - 1; - messageLength = (size_t)(endOfMessage - recvBuffer); + messageLength = (size_t) (endOfMessage - recvBuffer); } else { messageLength = 0; @@ -1203,6 +1206,7 @@ ssize_t SystemControlReceiveUserControlData(I32 socket, C8 * dataBuffer, size_t memmove(recvBuffer, recvBuffer + messageLength, bytesInBuffer); } } + return readResult; } From 08406820526467193b7336940527e63b15ad5c58 Mon Sep 17 00:00:00 2001 From: hyle-VM Date: Tue, 14 Jan 2020 04:00:30 +0100 Subject: [PATCH 017/523] save --- modules/Visualization/src/main.c | 49 ++++++++++++++++++++++++-------- 1 file changed, 37 insertions(+), 12 deletions(-) diff --git a/modules/Visualization/src/main.c b/modules/Visualization/src/main.c index 1c7e12cd5..ab60f11aa 100644 --- a/modules/Visualization/src/main.c +++ b/modules/Visualization/src/main.c @@ -29,7 +29,27 @@ ------------------------------------------------------------*/ static void vConnectVisualizationChannel(int *sockfd, struct sockaddr_in *addr); static void vDisconnectVisualizationChannel(int *sockfd); +static void vCreateMonitorData(char *commandData, ssize_t commandDatalen); +static void vCreateMonitorData(char *commandData, ssize_t commandDatalen) +{ + char ipStringBuffer[INET_ADDRSTRLEN]; + MonitorDataType monitorData; + struct timeval monrTime, systemTime; + const int debug = 0; + + if (commandDatalen < 0) + return; + + TimeSetToCurrentSystemTime(&systemTime); + + UtilPopulateMonitorDataStruct(commandData, (size_t) (commandDatalen), &monitorData, debug); + TimeSetToGPStime(&monrTime, TimeGetAsGPSweek(&systemTime), monitorData.MONR.GPSQmsOfWeekU32); + + + LogMessage(LOG_LEVEL_INFO, "HELLOTHEROBIWANKENOBI"); + +} int main() { enum COMMAND command = COMM_INV; @@ -79,21 +99,26 @@ int main() { // Ignore old style MONR data break; case COMM_MONR: - // TODO: Call util function to fill MonitorDataType struct - // TODO: Convert to temporary visualisation protocol - implement this function in this main.c - // ((TODO define this protocol clearly - leave this for now)) - UtilSendUDPData("Visualization", &visual_server, &visual_server_addr, busReceiveBuffer, - sizeof (busReceiveBuffer), 0); - MonitorDataType monitorData; - int busReceiveBufferLen = sizeof (busReceiveBuffer); - struct timeval monrTime, systemTime; + vCreateMonitorData(busReceiveBuffer, (sizeof (busReceiveBuffer))); - - //TimeSetToCurrentSystemTime(&systemTime); + // TODO: Call util function to fill MonitorDataType struct + // TODO: Convert to temporary visualisation protocol - implement this function in this main.c + // ((TODO define this protocol clearly - leave this for now)) + //UtilSendUDPData("Visualization", &visual_server, &visual_server_addr, "busReceiveBuffer", + // sizeof ("busReceiveBuffer"), 0); + // + //MonitorDataType monitorData; + //int busReceiveBufferLen = sizeof (busReceiveBuffer); + //struct timeval monrTime, systemTime; + // + // + ////TimeSetToCurrentSystemTime(&systemTime); //UtilPopulateMonitorDataStruct(busReceiveBuffer, (size_t) (busReceiveBufferLen), &monitorData, 0); - //TimeSetToGPStime(&monrTime, TimeGetAsGPSweek(&systemTime), monitorData.MONR.GPSQmsOfWeekU32); - printf("HELLO!"); + ////TimeSetToGPStime(&monrTime, TimeGetAsGPSweek(&systemTime), monitorData.MONR.GPSQmsOfWeekU32); + // + //LogMessage(LOG_LEVEL_INFO, "Visualization HELLOOOOOO"); + //LogMessage(LOG_LEVEL_INFO, monitorData.MONR.XPositionI32); break; case COMM_LOG: From d6f61b2a5336a29bbf0afc4bcc7942a05698a679 Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Tue, 14 Jan 2020 10:31:27 +0100 Subject: [PATCH 018/523] sitched from util_error to log error for connection problems --- server/src/objectcontrol.c | 19 +++++++++++++------ 1 file changed, 13 insertions(+), 6 deletions(-) diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 87542be21..f71eed152 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -810,6 +810,13 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { LogMessage(LOG_LEVEL_INFO, "CONNECT received"); LOG_SEND(LogBuffer, "[ObjectControl] CONNECT received."); + + if(nbr_objects < 1){ + LogMessage(LOG_LEVEL_ERROR, "No objects avaliable to connect."); + DisconnectU8 = 1; + } + + /* Connect and send drive files */ for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { @@ -841,22 +848,22 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { (void)sleep(3); // TODO: Move this to the rest of the sleep operations? Also, remove the hardcoded 3 break; case EADDRINUSE: - util_error("[ObjectControl] Local address/port already in use"); + LogMessage(LOG_LEVEL_ERROR, "[ObjectControl] Local address/port already in use"); break; case EALREADY: - util_error("[ObjectControl] Previous connection attempt still in progress"); + LogMessage(LOG_LEVEL_ERROR, "[ObjectControl] Previous connection attempt still in progress"); break; case EISCONN: - util_error("[ObjectControl] Socket is already connected"); + LogMessage(LOG_LEVEL_ERROR, "[ObjectControl] Socket is already connected"); break; case ENETUNREACH: - util_error("[ObjectControl] Network unreachable"); + LogMessage(LOG_LEVEL_ERROR, "[ObjectControl] Network unreachable"); break; case ETIMEDOUT: - util_error("[ObjectControl] Connection timed out"); + LogMessage(LOG_LEVEL_ERROR, "[ObjectControl] Connection timed out"); break; default: - util_error("ERR: Failed to connect to control socket"); + LogMessage(LOG_LEVEL_ERROR, "ERR: Failed to connect to control socket"); break; } From a1aa3889323e1b5585106b9b9d4b9e5d61471a74 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 14 Jan 2020 11:18:17 +0100 Subject: [PATCH 019/523] Moved sync word definition to iso22133.h --- server/inc/iso22133.h | 5 ++++- server/inc/util.h | 2 -- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/server/inc/iso22133.h b/server/inc/iso22133.h index 478b0bbcd..35218cef2 100644 --- a/server/inc/iso22133.h +++ b/server/inc/iso22133.h @@ -12,8 +12,10 @@ extern "C" { #include -#pragma pack(push,1) +const uint8_t SupportedProtocolVersions[] = {2}; +#define ISO_SYNC_WORD 0x7E7E +#pragma pack(push,1) typedef struct { uint16_t SyncWordU16; @@ -29,6 +31,7 @@ typedef struct uint16_t Crc; } FooterType; //2 bytes + //! *************************** TRCM #define COMMAND_TRCM_CODE 0x0011 typedef struct diff --git a/server/inc/util.h b/server/inc/util.h index f85df3b8a..c7a8a6963 100644 --- a/server/inc/util.h +++ b/server/inc/util.h @@ -176,8 +176,6 @@ extern "C"{ // Why do we need this memory efficiency? There is a risk that this breaks included code which isn't using pragma pack #pragma pack(1) // #pragma pack ( 1 ) directive can be used for arranging memory for structure members very next to the end of other structure members. -#define SYNC_WORD 0x7e7e - #define SERVER_PREPARED 0x01 #define SERVER_PREPARED_BIG_PACKET_SIZE 0x02 #define PATH_INVALID_MISSING 0x03 From a074624cff08737e5ff9aee153a536618d65efb4 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 14 Jan 2020 11:19:34 +0100 Subject: [PATCH 020/523] Updated MONR and ISO header parsing functions with some guards and checks (version among them) --- server/src/objectcontrol.c | 108 ++++++++++++++++++------------------- server/src/util.c | 73 +++++++++++++++---------- 2 files changed, 98 insertions(+), 83 deletions(-) diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 87542be21..06c5ef6d0 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -197,7 +197,7 @@ I32 ObjectControlMONRToASCII(MONRType * MONRData, GeoPosition * OriginPosition, C8 * LongitudinalSpeed, C8 * LateralSpeed, C8 * LongitudinalAcc, C8 * LateralAcc, C8 * Heading, C8 * DriveDirection, C8 * ObjectState, C8 * ReadyToArm, C8 * ErrorStatus, C8 debug); -I32 ObjectControlBuildMONRMessage(C8 * MonrData, MONRType * MONRData, U8 debug); +I32 ObjectControlBuildMONRMessage(C8 * MonrData, size_t length, MONRType * MONRData, U8 debug); I32 ObjectControlBuildVOILMessage(C8 * MessageBuffer, VOILType * VOILData, C8 * SimData, U8 debug); I32 ObjectControlSendDTMMessage(C8 * DTMData, I32 * Socket, I32 RowCount, C8 * IP, U32 Port, DOTMType * DOTMData, U8 debug); @@ -461,14 +461,20 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - bzero(buffer, RECV_MESSAGE_BUFFER); - receivedMONRData = uiRecvMonitor(&safety_socket_fd[iIndex], buffer, RECV_MESSAGE_BUFFER); + memset(buffer, 0, sizeof (buffer)); + receivedMONRData = uiRecvMonitor(&safety_socket_fd[iIndex], buffer, sizeof (buffer)); - if (receivedMONRData == sizeof (MONRType)) { + if (receivedMONRData > 0) { LogMessage(LOG_LEVEL_DEBUG, "Recieved new data from %s %d %d: %s", object_address_name[iIndex], object_udp_port[iIndex], receivedMONRData, buffer); + if ( ObjectControlBuildMONRMessage(buffer, receivedMONRData, &MONRData, 0) == -1 ) { + LogMessage(LOG_LEVEL_INFO, "Error decoding MONR from %s: disconnecting object", object_address_name[iIndex]); + vDisconnectObject(&safety_socket_fd[iIndex]); + continue; + } + if (ObjectcontrolExecutionMode == OBJECT_CONTROL_CONTROL_MODE) { // Append IP to buffer memcpy(&buffer[receivedMONRData], &safety_object_addr[iIndex].sin_addr.s_addr, @@ -484,7 +490,6 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { } } - ObjectControlBuildMONRMessage(buffer, &MONRData, 0); //Store MONR in GSD //UtilSendUDPData("ObjectControl", &ObjectControlUDPSocketfdI32, &simulator_addr, &MONRData, sizeof(MONRData), 0); @@ -1203,7 +1208,7 @@ I32 ObjectControlBuildVOILMessage(C8 * MessageBuffer, VOILType * VOILData, C8 * COMMAND_MESSAGE_HEADER_LENGTH); - VOILData->Header.SyncWordU16 = SYNC_WORD; + VOILData->Header.SyncWordU16 = ISO_SYNC_WORD; VOILData->Header.TransmitterIdU8 = 0; VOILData->Header.MessageCounterU8 = 0; VOILData->Header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; @@ -1254,52 +1259,43 @@ I32 ObjectControlBuildVOILMessage(C8 * MessageBuffer, VOILType * VOILData, C8 * } - -I32 ObjectControlBuildMONRMessage(C8 * MonrData, MONRType * MONRData, U8 debug) { - I32 MessageIndex = 0, i = 0; - dbl Data; - U16 Crc = 0, U16Data = 0; - I16 I16Data = 0; - U32 U32Data = 0; - I32 I32Data = 0; - U64 U64Data = 0; - U16 contentLength = 0; - U16 valueID = 0; +/*! + * \brief ObjectControlBuildMONRMessage Fills a MONRType struct from a buffer of raw data + * \param MonrData Raw data to be decoded + * \param MONRData Struct to be filled + * \param debug Flag for enabling of debugging + * \return 0 on success, -1 otherwise + */ +I32 ObjectControlBuildMONRMessage(C8 * MonrData, size_t length, MONRType * MONRData, U8 debug) { C8 *p = MonrData; - // Decode ISO header - memcpy(&MONRData->Header.SyncWordU16, p, sizeof (MONRData->Header.SyncWordU16)); - p += sizeof (MONRData->Header.SyncWordU16); - - memcpy(&MONRData->Header.TransmitterIdU8, p, sizeof (MONRData->Header.TransmitterIdU8)); - p += sizeof (MONRData->Header.TransmitterIdU8); - - memcpy(&MONRData->Header.MessageCounterU8, p, sizeof (MONRData->Header.MessageCounterU8)); - p += sizeof (MONRData->Header.MessageCounterU8); - - memcpy(&MONRData->Header.AckReqProtVerU8, p, sizeof (MONRData->Header.AckReqProtVerU8)); - p += sizeof (MONRData->Header.AckReqProtVerU8); - - memcpy(&MONRData->Header.MessageIdU16, p, sizeof (MONRData->Header.MessageIdU16)); - p += sizeof (MONRData->Header.MessageIdU16); - - memcpy(&MONRData->Header.MessageLengthU32, p, sizeof (MONRData->Header.MessageLengthU32)); - p += sizeof (MONRData->Header.MessageLengthU32); + if ( UtilISOBuildHeader(MonrData, &MONRData->Header, 0) == -1 ) { + memset(MONRData, 0, sizeof (*MONRData)); + return -1; + } + p += sizeof (MONRData->Header); // Decode content header - memcpy(&valueID, p, sizeof (valueID)); - if (valueID == VALUE_ID_MONR_STRUCT) { - memcpy(&MONRData->MonrStructValueIdU16, p, sizeof (MONRData->MonrStructValueIdU16)); - p += sizeof (MONRData->MonrStructValueIdU16); + memcpy(&MONRData->MonrStructValueIdU16, p, sizeof (MONRData->MonrStructValueIdU16)); + p += sizeof (MONRData->MonrStructValueIdU16); - //memcpy(&contentLength, p, sizeof (contentLength)); - memcpy(&MONRData->MonrStructContentLengthU16, p, sizeof (MONRData->MonrStructContentLengthU16)); - p += sizeof (MONRData->MonrStructContentLengthU16); - - // TODO: check on content length + if (MONRData->MonrStructValueIdU16 != VALUE_ID_MONR_STRUCT) { + errno = EINVAL; + LogMessage(LOG_LEVEL_ERROR, "Attempted to pass non-MONR struct into MONR parsing function"); + memset(MONRData, 0, sizeof (*MONRData)); + return -1; } - else if (debug) { - LogPrint("MONR message did not contain a content header"); + + memcpy(&MONRData->MonrStructContentLengthU16, p, sizeof (MONRData->MonrStructContentLengthU16)); + p += sizeof (MONRData->MonrStructContentLengthU16); + + if ( MONRData->MonrStructContentLengthU16 != (U16)(sizeof (MONRData) + - sizeof (MONRData->Header) - sizeof (MONRData->CRC) + - sizeof (MONRData->MonrStructValueIdU16) - sizeof (MONRData->MonrStructContentLengthU16)) ) { + errno = EINVAL; + LogMessage(LOG_LEVEL_ERROR, "MONR content length differs from the expected"); + memset(MONRData, 0, sizeof (*MONRData)); + return -1; } // Decode content @@ -1523,7 +1519,7 @@ I32 ObjectControlBuildOSEMMessage(C8 * MessageBuffer, OSEMType * OSEMData, TimeT bzero(MessageBuffer, COMMAND_OSEM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); - OSEMData->Header.SyncWordU16 = SYNC_WORD; + OSEMData->Header.SyncWordU16 = ISO_SYNC_WORD; OSEMData->Header.TransmitterIdU8 = 0; OSEMData->Header.MessageCounterU8 = 0; OSEMData->Header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; @@ -1631,7 +1627,7 @@ int ObjectControlBuildSTRTMessage(C8 * MessageBuffer, STRTType * STRTData, TimeT bzero(MessageBuffer, COMMAND_STRT_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); - STRTData->Header.SyncWordU16 = SYNC_WORD; + STRTData->Header.SyncWordU16 = ISO_SYNC_WORD; STRTData->Header.TransmitterIdU8 = 0; STRTData->Header.MessageCounterU8 = 0; STRTData->Header.AckReqProtVerU8 = 0; @@ -1691,7 +1687,7 @@ I32 ObjectControlBuildOSTMMessage(C8 * MessageBuffer, OSTMType * OSTMData, C8 Co bzero(MessageBuffer, COMMAND_OSTM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); - OSTMData->Header.SyncWordU16 = SYNC_WORD; + OSTMData->Header.SyncWordU16 = ISO_SYNC_WORD; OSTMData->Header.TransmitterIdU8 = 0; OSTMData->Header.MessageCounterU8 = 0; OSTMData->Header.AckReqProtVerU8 = 0; @@ -1738,7 +1734,7 @@ I32 ObjectControlBuildHEABMessage(C8 * MessageBuffer, HEABType * HEABData, TimeT bzero(MessageBuffer, COMMAND_HEAB_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); - HEABData->Header.SyncWordU16 = SYNC_WORD; + HEABData->Header.SyncWordU16 = ISO_SYNC_WORD; HEABData->Header.TransmitterIdU8 = 0; HEABData->Header.MessageCounterU8 = 0; HEABData->Header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; @@ -1824,7 +1820,7 @@ I32 ObjectControlBuildSYPMMessage(C8 * MessageBuffer, SYPMType * SYPMData, U32 S bzero(MessageBuffer, COMMAND_SYPM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); - SYPMData->Header.SyncWordU16 = SYNC_WORD; + SYPMData->Header.SyncWordU16 = ISO_SYNC_WORD; SYPMData->Header.TransmitterIdU8 = 0; SYPMData->Header.MessageCounterU8 = 0; SYPMData->Header.AckReqProtVerU8 = 0; @@ -1874,7 +1870,7 @@ I32 ObjectControlBuildMTSPMessage(C8 * MessageBuffer, MTSPType * MTSPData, U32 S bzero(MessageBuffer, COMMAND_MTSP_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); - MTSPData->Header.SyncWordU16 = SYNC_WORD; + MTSPData->Header.SyncWordU16 = ISO_SYNC_WORD; MTSPData->Header.TransmitterIdU8 = 0; MTSPData->Header.MessageCounterU8 = 0; MTSPData->Header.AckReqProtVerU8 = 0; @@ -1957,7 +1953,7 @@ I32 ObjectControlBuildTRAJMessageHeader(C8 * MessageBuffer, I32 * RowCount, Head bzero(MessageBuffer, COMMAND_MESSAGE_HEADER_LENGTH + COMMAND_TRAJ_INFO_ROW_MESSAGE_LENGTH); - HeaderData->SyncWordU16 = SYNC_WORD; + HeaderData->SyncWordU16 = ISO_SYNC_WORD; HeaderData->TransmitterIdU8 = 0; HeaderData->MessageCounterU8 = 0; HeaderData->AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; @@ -2556,7 +2552,7 @@ I32 ObjectControlSendEXACMessage(EXACData * EXAC, I32 * socket, U8 debug) { */ I32 ObjectControlBuildACCMMessage(ACCMData * mqACCMData, ACCMType * ACCM, U8 debug) { // Header - ACCM->header.SyncWordU16 = SYNC_WORD; + ACCM->header.SyncWordU16 = ISO_SYNC_WORD; ACCM->header.TransmitterIdU8 = 0; ACCM->header.MessageCounterU8 = 0; ACCM->header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; @@ -2634,7 +2630,7 @@ I32 ObjectControlBuildEXACMessage(EXACData * mqEXACData, EXACType * EXAC, U8 deb TimeSetToCurrentSystemTime(&systemTime); // Header - EXAC->header.SyncWordU16 = SYNC_WORD; + EXAC->header.SyncWordU16 = ISO_SYNC_WORD; EXAC->header.TransmitterIdU8 = 0; EXAC->header.MessageCounterU8 = 0; EXAC->header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; @@ -2686,7 +2682,7 @@ I32 ObjectControlBuildEXACMessage(EXACData * mqEXACData, EXACType * EXAC, U8 deb */ I32 ObjectControlBuildTRCMMessage(TRCMData * mqTRCMData, TRCMType * TRCM, U8 debug) { // Header - TRCM->header.SyncWordU16 = SYNC_WORD; + TRCM->header.SyncWordU16 = ISO_SYNC_WORD; TRCM->header.TransmitterIdU8 = 0; TRCM->header.MessageCounterU8 = 0; TRCM->header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; diff --git a/server/src/util.c b/server/src/util.c index ed2634a18..0365ed698 100644 --- a/server/src/util.c +++ b/server/src/util.c @@ -3256,7 +3256,7 @@ I32 UtilISOBuildINSUPMessage(C8 * MessageBuffer, INSUPType * INSUPData, C8 Comma bzero(MessageBuffer, ISO_INSUP_MESSAGE_LENGTH + ISO_MESSAGE_FOOTER_LENGTH); - INSUPData->Header.SyncWordU16 = SYNC_WORD; + INSUPData->Header.SyncWordU16 = ISO_SYNC_WORD; INSUPData->Header.TransmitterIdU8 = 0; INSUPData->Header.MessageCounterU8 = 0; INSUPData->Header.AckReqProtVerU8 = 0; @@ -3302,7 +3302,7 @@ I32 UtilISOBuildHEABMessage(C8 * MessageBuffer, HEABType * HEABData, TimeType * bzero(MessageBuffer, ISO_HEAB_MESSAGE_LENGTH + ISO_MESSAGE_FOOTER_LENGTH); - HEABData->Header.SyncWordU16 = SYNC_WORD; + HEABData->Header.SyncWordU16 = ISO_SYNC_WORD; HEABData->Header.TransmitterIdU8 = 0; HEABData->Header.MessageCounterU8 = 0; HEABData->Header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; @@ -3452,7 +3452,7 @@ I32 UtilISOBuildTRAJMessageHeader(C8 * MessageBuffer, I32 RowCount, HeaderType * bzero(MessageBuffer, ISO_MESSAGE_HEADER_LENGTH + ISO_TRAJ_INFO_ROW_MESSAGE_LENGTH); - HeaderData->SyncWordU16 = SYNC_WORD; + HeaderData->SyncWordU16 = ISO_SYNC_WORD; HeaderData->TransmitterIdU8 = 0; HeaderData->MessageCounterU8 = 0; HeaderData->AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; @@ -3649,33 +3649,52 @@ I32 UtilISOBuildTRAJMessage(C8 * MessageBuffer, C8 * DTMData, I32 RowCount, DOTM } I32 UtilISOBuildHeader(C8 * MessageBuffer, HeaderType * HeaderData, U8 Debug) { - I32 MessageIndex = 0, i = 0; - dbl Data; - U16 Crc = 0, U16Data = 0; - I16 I16Data = 0; - U32 U32Data = 0; - I32 I32Data = 0; - U64 U64Data = 0; - C8 *p; + C8 *p = MessageBuffer; + I32 retval = 0; + const U8 ProtocolVersionBitmask = 0x7F; + U8 messageProtocolVersion = 0; + U8 isProtocolVersionSupported = 0; + + // Decode ISO header + memcpy(&HeaderData->SyncWordU16, p, sizeof (HeaderData->SyncWordU16)); + p += sizeof (HeaderData->SyncWordU16); + + if ( HeaderData->SyncWordU16 != ISO_SYNC_WORD) { + errno = EINVAL; + LogMessage(LOG_LEVEL_ERROR, "Sync word error when decoding ISO header"); + memset(HeaderData, 0, sizeof (*HeaderData)); + return -1; + } - U16Data = (U16Data | *(MessageBuffer + 1)) << 8; - U16Data = U16Data | *(MessageBuffer + 0); + memcpy(&HeaderData->TransmitterIdU8, p, sizeof (HeaderData->TransmitterIdU8)); + p += sizeof (HeaderData->TransmitterIdU8); - HeaderData->SyncWordU16 = U16Data; - HeaderData->TransmitterIdU8 = *(MessageBuffer + 2); - HeaderData->MessageCounterU8 = *(MessageBuffer + 3); - HeaderData->AckReqProtVerU8 = *(MessageBuffer + 4); + memcpy(&HeaderData->MessageCounterU8, p, sizeof (HeaderData->MessageCounterU8)); + p += sizeof (HeaderData->MessageCounterU8); - U16Data = 0; - U16Data = (U16Data | *(MessageBuffer + 6)) << 8; - U16Data = U16Data | *(MessageBuffer + 5); - HeaderData->MessageIdU16 = U16Data; + memcpy(&HeaderData->AckReqProtVerU8, p, sizeof (HeaderData->AckReqProtVerU8)); + p += sizeof (HeaderData->AckReqProtVerU8); - U32Data = (U32Data | *(MessageBuffer + 10)) << 8; - U32Data = (U32Data | *(MessageBuffer + 9)) << 8; - U32Data = (U32Data | *(MessageBuffer + 8)) << 8; - U32Data = U32Data | *(MessageBuffer + 7); - HeaderData->MessageLengthU32 = U32Data; + // Loop over permitted protocol versions + messageProtocolVersion = HeaderData->AckReqProtVerU8 & ProtocolVersionBitmask; + for (size_t i = 0; i < sizeof (SupportedProtocolVersions) / sizeof (SupportedProtocolVersions[0]); ++i) { + if (SupportedProtocolVersions[i] == messageProtocolVersion) { + isProtocolVersionSupported = 1; + break; + } + } + + if ( !isProtocolVersionSupported ) { + errno = EPROTONOSUPPORT; + LogMessage(LOG_LEVEL_WARNING, "Protocol version %u not supported", messageProtocolVersion); + retval = -1; + } + + memcpy(&HeaderData->MessageIdU16, p, sizeof (HeaderData->MessageIdU16)); + p += sizeof (HeaderData->MessageIdU16); + + memcpy(&HeaderData->MessageLengthU32, p, sizeof (HeaderData->MessageLengthU32)); + p += sizeof (HeaderData->MessageLengthU32); if (Debug) { LogPrint("SyncWordU16 = 0x%x", HeaderData->SyncWordU16); @@ -3686,7 +3705,7 @@ I32 UtilISOBuildHeader(C8 * MessageBuffer, HeaderType * HeaderData, U8 Debug) { LogPrint("MessageLengthU32 = 0x%x", HeaderData->MessageLengthU32); } - return 0; + return retval; } From f9fbd11fbd839d44b2cb962610a6fcfb85c196cc Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 14 Jan 2020 12:37:33 +0100 Subject: [PATCH 021/523] Removed disconnect on MONR parse failure --- server/src/objectcontrol.c | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 06c5ef6d0..96f2e88ff 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -156,7 +156,6 @@ typedef enum { */ typedef StateTransitionResult(*StateTransition) (OBCState_t * currentState, OBCState_t requestedState); - C8 TrajBuffer[COMMAND_DOTM_ROWS_IN_TRANSMISSION * COMMAND_DOTM_ROW_MESSAGE_LENGTH + COMMAND_MESSAGE_HEADER_LENGTH + COMMAND_TRAJ_INFO_ROW_MESSAGE_LENGTH]; @@ -470,9 +469,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { buffer); if ( ObjectControlBuildMONRMessage(buffer, receivedMONRData, &MONRData, 0) == -1 ) { - LogMessage(LOG_LEVEL_INFO, "Error decoding MONR from %s: disconnecting object", object_address_name[iIndex]); - vDisconnectObject(&safety_socket_fd[iIndex]); - continue; + // TODO react on error } if (ObjectcontrolExecutionMode == OBJECT_CONTROL_CONTROL_MODE) { From 8b626af9fbddeb4551af44e2d03786085d84aae8 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 14 Jan 2020 12:38:12 +0100 Subject: [PATCH 022/523] Added continue to reaction when failing to parse MONR --- server/src/objectcontrol.c | 1 + 1 file changed, 1 insertion(+) diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 96f2e88ff..4388dee14 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -470,6 +470,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { if ( ObjectControlBuildMONRMessage(buffer, receivedMONRData, &MONRData, 0) == -1 ) { // TODO react on error + continue; } if (ObjectcontrolExecutionMode == OBJECT_CONTROL_CONTROL_MODE) { From 3047c7d75f4bf46fc8ddcc014e3669d45755331a Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 14 Jan 2020 12:41:23 +0100 Subject: [PATCH 023/523] Added simple disconnect if error when parsing MONR --- server/src/objectcontrol.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 4388dee14..d9c9de32c 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -469,7 +469,9 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { buffer); if ( ObjectControlBuildMONRMessage(buffer, receivedMONRData, &MONRData, 0) == -1 ) { - // TODO react on error + LogMessage(LOG_LEVEL_INFO, "Error decoding MONR from %s: disconnecting object", object_address_name[iIndex]); + vDisconnectObject(&safety_socket_fd[iIndex]); + // TODO smarter way of handling? continue; } From afa6ce2255ef28bb219e63df6d604dd06715d5db Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 14 Jan 2020 13:07:21 +0100 Subject: [PATCH 024/523] Made a getter function for supported versions instead of a global array --- server/inc/iso22133.h | 5 ++++- server/src/iso22133.c | 6 ++++++ server/src/util.c | 7 +++++-- 3 files changed, 15 insertions(+), 3 deletions(-) create mode 100644 server/src/iso22133.c diff --git a/server/inc/iso22133.h b/server/inc/iso22133.h index 35218cef2..7ed8789bc 100644 --- a/server/inc/iso22133.h +++ b/server/inc/iso22133.h @@ -11,8 +11,8 @@ extern "C" { */ #include +#include -const uint8_t SupportedProtocolVersions[] = {2}; #define ISO_SYNC_WORD 0x7E7E #pragma pack(push,1) @@ -223,6 +223,9 @@ typedef struct #pragma pack(pop) +void getSupportedISOProtocolVersions(const uint8_t** supportedProtocolVersions, size_t* nProtocols); + + #ifdef __cplusplus } #endif diff --git a/server/src/iso22133.c b/server/src/iso22133.c new file mode 100644 index 000000000..9f5dd1e61 --- /dev/null +++ b/server/src/iso22133.c @@ -0,0 +1,6 @@ +#include "iso22133.h" +static const uint8_t SupportedProtocolVersions[] = {2}; +void getSupportedISOProtocolVersions(const uint8_t** supportedProtocolVersions, size_t* nProtocols) { + *supportedProtocolVersions = SupportedProtocolVersions; + *nProtocols = sizeof (SupportedProtocolVersions) / sizeof (SupportedProtocolVersions[0]); +} diff --git a/server/src/util.c b/server/src/util.c index 0365ed698..e488017c7 100644 --- a/server/src/util.c +++ b/server/src/util.c @@ -3654,6 +3654,8 @@ I32 UtilISOBuildHeader(C8 * MessageBuffer, HeaderType * HeaderData, U8 Debug) { const U8 ProtocolVersionBitmask = 0x7F; U8 messageProtocolVersion = 0; U8 isProtocolVersionSupported = 0; + const uint8_t* supportedProtocolVersions; + size_t nSupportedProtocols = 0; // Decode ISO header memcpy(&HeaderData->SyncWordU16, p, sizeof (HeaderData->SyncWordU16)); @@ -3677,8 +3679,9 @@ I32 UtilISOBuildHeader(C8 * MessageBuffer, HeaderType * HeaderData, U8 Debug) { // Loop over permitted protocol versions messageProtocolVersion = HeaderData->AckReqProtVerU8 & ProtocolVersionBitmask; - for (size_t i = 0; i < sizeof (SupportedProtocolVersions) / sizeof (SupportedProtocolVersions[0]); ++i) { - if (SupportedProtocolVersions[i] == messageProtocolVersion) { + getSupportedISOProtocolVersions(&supportedProtocolVersions, &nSupportedProtocols); + for (size_t i = 0; i < nSupportedProtocols; ++i) { + if (supportedProtocolVersions[i] == messageProtocolVersion) { isProtocolVersionSupported = 1; break; } From 30001de2967b647f60e935bf82446cb627dd459f Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 14 Jan 2020 13:08:08 +0100 Subject: [PATCH 025/523] Added iso22133.c linking for all CMakeLists.txt --- modules/ScenarioControl/CMakeLists.txt | 2 ++ modules/Supervision/CMakeLists.txt | 2 ++ server/CMakeLists.txt | 1 + 3 files changed, 5 insertions(+) diff --git a/modules/ScenarioControl/CMakeLists.txt b/modules/ScenarioControl/CMakeLists.txt index ab7e2aec0..a0fa949a2 100644 --- a/modules/ScenarioControl/CMakeLists.txt +++ b/modules/ScenarioControl/CMakeLists.txt @@ -37,6 +37,8 @@ add_library(MQBus add_library(util ../../server/src/util.c ../../server/inc/util.h + ../../server/src/iso22133.c + ../../server/inc/iso22133.h ) add_library(Trigger diff --git a/modules/Supervision/CMakeLists.txt b/modules/Supervision/CMakeLists.txt index 17b6afcb5..a6af6ab81 100644 --- a/modules/Supervision/CMakeLists.txt +++ b/modules/Supervision/CMakeLists.txt @@ -36,6 +36,8 @@ add_library(MQBus add_library(util ../../server/src/util.c ../../server/inc/util.h + ../../server/src/iso22133.c + ../../server/inc/iso22133.h ) add_executable(Supervision diff --git a/server/CMakeLists.txt b/server/CMakeLists.txt index 9f2741feb..6eb0224aa 100644 --- a/server/CMakeLists.txt +++ b/server/CMakeLists.txt @@ -82,6 +82,7 @@ add_library(util src/util.c inc/util.h inc/iso22133.h + src/iso22133.c ) # Create library From 340957485e899fd643de80b90a16e2b3d56ebc0f Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 14 Jan 2020 13:33:42 +0100 Subject: [PATCH 026/523] Added check on raw data length in header parser function and corrected sizeof statement --- server/inc/util.h | 2 +- server/src/objectcontrol.c | 16 +++++++++------- server/src/util.c | 9 ++++++++- 3 files changed, 18 insertions(+), 9 deletions(-) diff --git a/server/inc/util.h b/server/inc/util.h index c7a8a6963..da29e95a6 100644 --- a/server/inc/util.h +++ b/server/inc/util.h @@ -895,7 +895,7 @@ U32 UtilHexTextToBinary(U32 DataLength, C8 *Text, C8 *Binary, U8 Debug); U32 UtilCreateDirContent(C8* DirPath, C8* TempPath); U16 UtilGetMillisecond(TimeType *GPSTime); -I32 UtilISOBuildHeader(C8 *MessageBuffer, HeaderType *HeaderData, U8 Debug); +I32 UtilISOBuildHeader(C8 *MessageBuffer, const size_t length, HeaderType *HeaderData, U8 Debug); I32 UtilISOBuildINSUPMessage(C8* MessageBuffer, INSUPType *INSUPData, C8 CommandOption, U8 Debug); I32 UtilISOBuildHEABMessage(C8* MessageBuffer, HEABType *HEABData, TimeType *GPSTime, U8 CCStatus, U8 Debug); I32 UtilISOBuildTRAJMessageHeader(C8* MessageBuffer, I32 RowCount, HeaderType *HeaderData, TRAJInfoType *TRAJInfoData, U8 Debug); diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 4388dee14..eb323e226 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -196,7 +196,7 @@ I32 ObjectControlMONRToASCII(MONRType * MONRData, GeoPosition * OriginPosition, C8 * LongitudinalSpeed, C8 * LateralSpeed, C8 * LongitudinalAcc, C8 * LateralAcc, C8 * Heading, C8 * DriveDirection, C8 * ObjectState, C8 * ReadyToArm, C8 * ErrorStatus, C8 debug); -I32 ObjectControlBuildMONRMessage(C8 * MonrData, size_t length, MONRType * MONRData, U8 debug); +I32 ObjectControlBuildMONRMessage(C8 * MonrData, const size_t length, MONRType * MONRData, U8 debug); I32 ObjectControlBuildVOILMessage(C8 * MessageBuffer, VOILType * VOILData, C8 * SimData, U8 debug); I32 ObjectControlSendDTMMessage(C8 * DTMData, I32 * Socket, I32 RowCount, C8 * IP, U32 Port, DOTMType * DOTMData, U8 debug); @@ -1264,10 +1264,13 @@ I32 ObjectControlBuildVOILMessage(C8 * MessageBuffer, VOILType * VOILData, C8 * * \param debug Flag for enabling of debugging * \return 0 on success, -1 otherwise */ -I32 ObjectControlBuildMONRMessage(C8 * MonrData, size_t length, MONRType * MONRData, U8 debug) { +I32 ObjectControlBuildMONRMessage(C8 * MonrData, const size_t length, MONRType * MONRData, U8 debug) { C8 *p = MonrData; + const U16 ExpectedMONRStructSize = (U16)(sizeof (*MONRData) - sizeof (MONRData->Header) + - sizeof (MONRData->CRC) - sizeof (MONRData->MonrStructValueIdU16) + - sizeof (MONRData->MonrStructContentLengthU16)); - if ( UtilISOBuildHeader(MonrData, &MONRData->Header, 0) == -1 ) { + if ( UtilISOBuildHeader(MonrData, length, &MONRData->Header, 0) == -1 ) { memset(MONRData, 0, sizeof (*MONRData)); return -1; } @@ -1287,11 +1290,10 @@ I32 ObjectControlBuildMONRMessage(C8 * MonrData, size_t length, MONRType * MONRD memcpy(&MONRData->MonrStructContentLengthU16, p, sizeof (MONRData->MonrStructContentLengthU16)); p += sizeof (MONRData->MonrStructContentLengthU16); - if ( MONRData->MonrStructContentLengthU16 != (U16)(sizeof (MONRData) - - sizeof (MONRData->Header) - sizeof (MONRData->CRC) - - sizeof (MONRData->MonrStructValueIdU16) - sizeof (MONRData->MonrStructContentLengthU16)) ) { + if ( MONRData->MonrStructContentLengthU16 != ExpectedMONRStructSize ) { errno = EINVAL; - LogMessage(LOG_LEVEL_ERROR, "MONR content length differs from the expected"); + LogMessage(LOG_LEVEL_ERROR, "MONR content length %u differs from the expected length %u", + MONRData->MonrStructContentLengthU16, ExpectedMONRStructSize); memset(MONRData, 0, sizeof (*MONRData)); return -1; } diff --git a/server/src/util.c b/server/src/util.c index e488017c7..d46768a43 100644 --- a/server/src/util.c +++ b/server/src/util.c @@ -3648,7 +3648,7 @@ I32 UtilISOBuildTRAJMessage(C8 * MessageBuffer, C8 * DTMData, I32 RowCount, DOTM return MessageIndex; //Total number of bytes } -I32 UtilISOBuildHeader(C8 * MessageBuffer, HeaderType * HeaderData, U8 Debug) { +I32 UtilISOBuildHeader(C8 * MessageBuffer, const size_t length, HeaderType * HeaderData, U8 Debug) { C8 *p = MessageBuffer; I32 retval = 0; const U8 ProtocolVersionBitmask = 0x7F; @@ -3657,6 +3657,13 @@ I32 UtilISOBuildHeader(C8 * MessageBuffer, HeaderType * HeaderData, U8 Debug) { const uint8_t* supportedProtocolVersions; size_t nSupportedProtocols = 0; + if ( length < sizeof (HeaderData) ) { + errno = EINVAL; + LogMessage(LOG_LEVEL_ERROR, "Too little raw data to fill ISO header"); + memset(HeaderData, 0, sizeof (*HeaderData)); + return -1; + } + // Decode ISO header memcpy(&HeaderData->SyncWordU16, p, sizeof (HeaderData->SyncWordU16)); p += sizeof (HeaderData->SyncWordU16); From e8e6e818a1ea5d5e0bb9c5bb63c109d7643f600f Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 14 Jan 2020 13:34:12 +0100 Subject: [PATCH 027/523] Ran code formatter --- server/src/iso22133.c | 4 ++-- server/src/objectcontrol.c | 13 +++++++------ server/src/util.c | 8 ++++---- 3 files changed, 13 insertions(+), 12 deletions(-) diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 9f5dd1e61..6311e1478 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -1,6 +1,6 @@ #include "iso22133.h" -static const uint8_t SupportedProtocolVersions[] = {2}; -void getSupportedISOProtocolVersions(const uint8_t** supportedProtocolVersions, size_t* nProtocols) { +static const uint8_t SupportedProtocolVersions[] = { 2 }; +void getSupportedISOProtocolVersions(const uint8_t ** supportedProtocolVersions, size_t * nProtocols) { *supportedProtocolVersions = SupportedProtocolVersions; *nProtocols = sizeof (SupportedProtocolVersions) / sizeof (SupportedProtocolVersions[0]); } diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index eb323e226..6e4f68378 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -468,7 +468,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { object_address_name[iIndex], object_udp_port[iIndex], receivedMONRData, buffer); - if ( ObjectControlBuildMONRMessage(buffer, receivedMONRData, &MONRData, 0) == -1 ) { + if (ObjectControlBuildMONRMessage(buffer, receivedMONRData, &MONRData, 0) == -1) { // TODO react on error continue; } @@ -1266,11 +1266,12 @@ I32 ObjectControlBuildVOILMessage(C8 * MessageBuffer, VOILType * VOILData, C8 * */ I32 ObjectControlBuildMONRMessage(C8 * MonrData, const size_t length, MONRType * MONRData, U8 debug) { C8 *p = MonrData; - const U16 ExpectedMONRStructSize = (U16)(sizeof (*MONRData) - sizeof (MONRData->Header) - - sizeof (MONRData->CRC) - sizeof (MONRData->MonrStructValueIdU16) - - sizeof (MONRData->MonrStructContentLengthU16)); + const U16 ExpectedMONRStructSize = (U16) (sizeof (*MONRData) - sizeof (MONRData->Header) + - sizeof (MONRData->CRC) - + sizeof (MONRData->MonrStructValueIdU16) + - sizeof (MONRData->MonrStructContentLengthU16)); - if ( UtilISOBuildHeader(MonrData, length, &MONRData->Header, 0) == -1 ) { + if (UtilISOBuildHeader(MonrData, length, &MONRData->Header, 0) == -1) { memset(MONRData, 0, sizeof (*MONRData)); return -1; } @@ -1290,7 +1291,7 @@ I32 ObjectControlBuildMONRMessage(C8 * MonrData, const size_t length, MONRType * memcpy(&MONRData->MonrStructContentLengthU16, p, sizeof (MONRData->MonrStructContentLengthU16)); p += sizeof (MONRData->MonrStructContentLengthU16); - if ( MONRData->MonrStructContentLengthU16 != ExpectedMONRStructSize ) { + if (MONRData->MonrStructContentLengthU16 != ExpectedMONRStructSize) { errno = EINVAL; LogMessage(LOG_LEVEL_ERROR, "MONR content length %u differs from the expected length %u", MONRData->MonrStructContentLengthU16, ExpectedMONRStructSize); diff --git a/server/src/util.c b/server/src/util.c index d46768a43..f450d523d 100644 --- a/server/src/util.c +++ b/server/src/util.c @@ -3654,10 +3654,10 @@ I32 UtilISOBuildHeader(C8 * MessageBuffer, const size_t length, HeaderType * Hea const U8 ProtocolVersionBitmask = 0x7F; U8 messageProtocolVersion = 0; U8 isProtocolVersionSupported = 0; - const uint8_t* supportedProtocolVersions; + const uint8_t *supportedProtocolVersions; size_t nSupportedProtocols = 0; - if ( length < sizeof (HeaderData) ) { + if (length < sizeof (HeaderData)) { errno = EINVAL; LogMessage(LOG_LEVEL_ERROR, "Too little raw data to fill ISO header"); memset(HeaderData, 0, sizeof (*HeaderData)); @@ -3668,7 +3668,7 @@ I32 UtilISOBuildHeader(C8 * MessageBuffer, const size_t length, HeaderType * Hea memcpy(&HeaderData->SyncWordU16, p, sizeof (HeaderData->SyncWordU16)); p += sizeof (HeaderData->SyncWordU16); - if ( HeaderData->SyncWordU16 != ISO_SYNC_WORD) { + if (HeaderData->SyncWordU16 != ISO_SYNC_WORD) { errno = EINVAL; LogMessage(LOG_LEVEL_ERROR, "Sync word error when decoding ISO header"); memset(HeaderData, 0, sizeof (*HeaderData)); @@ -3694,7 +3694,7 @@ I32 UtilISOBuildHeader(C8 * MessageBuffer, const size_t length, HeaderType * Hea } } - if ( !isProtocolVersionSupported ) { + if (!isProtocolVersionSupported) { errno = EPROTONOSUPPORT; LogMessage(LOG_LEVEL_WARNING, "Protocol version %u not supported", messageProtocolVersion); retval = -1; From dd545d612829833db1052d2bbff0ca202d0abcc1 Mon Sep 17 00:00:00 2001 From: alfaro01 Date: Tue, 14 Jan 2020 13:39:51 +0100 Subject: [PATCH 028/523] First version of working Visualization data. Only includes X, Y, Z, and Heading though --- modules/Visualization/src/main.c | 74 ++++++++++++++------------------ 1 file changed, 33 insertions(+), 41 deletions(-) diff --git a/modules/Visualization/src/main.c b/modules/Visualization/src/main.c index ab60f11aa..997325fd7 100644 --- a/modules/Visualization/src/main.c +++ b/modules/Visualization/src/main.c @@ -29,26 +29,21 @@ ------------------------------------------------------------*/ static void vConnectVisualizationChannel(int *sockfd, struct sockaddr_in *addr); static void vDisconnectVisualizationChannel(int *sockfd); -static void vCreateMonitorData(char *commandData, ssize_t commandDatalen); +void vCreateVisualizationMessage(MonitorDataType *_monitorData, char *_visualizationMessage, int _debug); -static void vCreateMonitorData(char *commandData, ssize_t commandDatalen) +void vCreateVisualizationMessage(MonitorDataType *_monitorData, char *_visualizationMessage, int _debug) { - char ipStringBuffer[INET_ADDRSTRLEN]; - MonitorDataType monitorData; - struct timeval monrTime, systemTime; - const int debug = 0; - - if (commandDatalen < 0) - return; - - TimeSetToCurrentSystemTime(&systemTime); - - UtilPopulateMonitorDataStruct(commandData, (size_t) (commandDatalen), &monitorData, debug); - TimeSetToGPStime(&monrTime, TimeGetAsGPSweek(&systemTime), monitorData.MONR.GPSQmsOfWeekU32); - - - LogMessage(LOG_LEVEL_INFO, "HELLOTHEROBIWANKENOBI"); - + sprintf(_visualizationMessage,"%d;%d;%d;%d", _monitorData->MONR.XPositionI32, _monitorData->MONR.YPositionI32, _monitorData->MONR.ZPositionI32, _monitorData->MONR.HeadingU16); + + if(_debug) + { + LogMessage(LOG_LEVEL_INFO, "X: %d", _monitorData->MONR.XPositionI32); + LogMessage(LOG_LEVEL_INFO, "Y: %d", _monitorData->MONR.YPositionI32); + LogMessage(LOG_LEVEL_INFO, "Z: %d", _monitorData->MONR.ZPositionI32); + LogMessage(LOG_LEVEL_INFO, "Heading: %d", _monitorData->MONR.HeadingU16); + LogMessage(LOG_LEVEL_INFO, "LatAcc: %d", _monitorData->MONR.LateralAccI16); + LogMessage(LOG_LEVEL_INFO, "LongAcc: %d", _monitorData->MONR.LongitudinalAccI16); + } } int main() { @@ -58,6 +53,12 @@ int main() { const struct timespec abortWaitTime = { 1, 0 }; struct timespec remTime; + MonitorDataType monitorData; + int sizeOfMessage = (sizeof (monitorData.MONR.XPositionI32) + + sizeof (monitorData.MONR.YPositionI32) + + sizeof (monitorData.MONR.ZPositionI32) + + sizeof (monitorData.MONR.HeadingU16)); + LogInit(MODULE_NAME, LOG_LEVEL_DEBUG); LogMessage(LOG_LEVEL_INFO, "Task running with PID: %u", getpid()); @@ -66,8 +67,7 @@ int main() { vConnectVisualizationChannel(&visual_server, &visual_server_addr); - I32 iExit = 0; - char busReceiveBuffer[MBUS_MAX_DATALEN]; //!< Buffer for receiving from message bus + I32 iExit = 0; // Initialize message bus connection @@ -98,28 +98,20 @@ int main() { case COMM_MONI: // Ignore old style MONR data break; - case COMM_MONR: - - vCreateMonitorData(busReceiveBuffer, (sizeof (busReceiveBuffer))); - - // TODO: Call util function to fill MonitorDataType struct - // TODO: Convert to temporary visualisation protocol - implement this function in this main.c - // ((TODO define this protocol clearly - leave this for now)) - //UtilSendUDPData("Visualization", &visual_server, &visual_server_addr, "busReceiveBuffer", - // sizeof ("busReceiveBuffer"), 0); - // - //MonitorDataType monitorData; - //int busReceiveBufferLen = sizeof (busReceiveBuffer); - //struct timeval monrTime, systemTime; - // - // - ////TimeSetToCurrentSystemTime(&systemTime); - //UtilPopulateMonitorDataStruct(busReceiveBuffer, (size_t) (busReceiveBufferLen), &monitorData, 0); - ////TimeSetToGPStime(&monrTime, TimeGetAsGPSweek(&systemTime), monitorData.MONR.GPSQmsOfWeekU32); - // - //LogMessage(LOG_LEVEL_INFO, "Visualization HELLOOOOOO"); - //LogMessage(LOG_LEVEL_INFO, monitorData.MONR.XPositionI32); + case COMM_MONR: + { + //Populate the monitorType + UtilPopulateMonitorDataStruct(mqRecvData, (size_t) (sizeof (mqRecvData)), &monitorData, 0); + + //Create visualization message and insert values from the monitor datastruct above + char visualizationMessage[42]; + vCreateVisualizationMessage(&monitorData, visualizationMessage, 0); + + //Send visualization message on the UDP socket + UtilSendUDPData("Visualization", &visual_server, &visual_server_addr, visualizationMessage, + sizeof (visualizationMessage), 0); + } break; case COMM_LOG: break; From 00a9044081e79e1b6091f64916b24075161440c9 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 14 Jan 2020 13:40:11 +0100 Subject: [PATCH 029/523] Removed annoying debug printout --- server/src/systemcontrol.c | 1 - 1 file changed, 1 deletion(-) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index ff8ea85cf..0ba038ea2 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -381,7 +381,6 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { server_state == SERVER_STATE_UNDEFINED; // TODO: Should this be an assignment? } else if (ClientResult > 0 && ClientResult < TCP_RECV_BUFFER_SIZE) { - LogPrint("HERE: %s", pcBuffer); // TODO: Move this entire decoding process into a separate function for (i = 0; i < SYSTEM_CONTROL_ARG_MAX_COUNT; i++) bzero(SystemControlArgument[i], SYSTEM_CONTROL_ARGUMENT_MAX_LENGTH); From 0df43df8e81a2f2e45ede71f0611164579fe2c66 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 14 Jan 2020 15:38:59 +0100 Subject: [PATCH 030/523] Changed the frequency of status polling in test --- server/integration-tests/tools/MSCP.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/server/integration-tests/tools/MSCP.py b/server/integration-tests/tools/MSCP.py index 9e4913eb3..7b55f3e19 100644 --- a/server/integration-tests/tools/MSCP.py +++ b/server/integration-tests/tools/MSCP.py @@ -240,7 +240,7 @@ def waitForObjectControlState(self,state,timeout=3.0): sr = self.lastStatusReply["objectControlState"] self.statusReplyLock.release() while sr != state and time.time() < timeoutTime: - time.sleep(0.005) + time.sleep(0.25) self.statusReplyLock.acquire() sr = self.lastStatusReply["objectControlState"] self.statusReplyLock.release() From 10e4bc06b0b1248d081e32b34e3f5d96f27bd76a Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 14 Jan 2020 15:55:07 +0100 Subject: [PATCH 031/523] Increased the frequency again --- server/integration-tests/tools/MSCP.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/server/integration-tests/tools/MSCP.py b/server/integration-tests/tools/MSCP.py index 7b55f3e19..9e4913eb3 100644 --- a/server/integration-tests/tools/MSCP.py +++ b/server/integration-tests/tools/MSCP.py @@ -240,7 +240,7 @@ def waitForObjectControlState(self,state,timeout=3.0): sr = self.lastStatusReply["objectControlState"] self.statusReplyLock.release() while sr != state and time.time() < timeoutTime: - time.sleep(0.25) + time.sleep(0.005) self.statusReplyLock.acquire() sr = self.lastStatusReply["objectControlState"] self.statusReplyLock.release() From cadc73632cba210ec0c55adbdb649bf567ab5dce Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 14 Jan 2020 15:55:21 +0100 Subject: [PATCH 032/523] Added a debug printout --- server/src/systemcontrol.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index 0ba038ea2..6a6bbf345 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -1190,6 +1190,7 @@ ssize_t SystemControlReceiveUserControlData(I32 socket, C8 * dataBuffer, size_t messageLength = 0; readResult = -1; errno = EAGAIN; + LogMessage(LOG_LEVEL_WARNING, "Part of message received"); } if (bytesInBuffer >= messageLength) { @@ -2148,7 +2149,9 @@ I32 SystemControlReceiveRxData(I32 * sockfd, C8 * Path, C8 * FileSize, C8 * Pack } bzero(RxBuffer, PacketSizeU16); + LogPrint("AAAA"); ClientStatus = recv(*sockfd, RxBuffer, PacketSizeU16, MSG_WAITALL); + LogPrint("BBBB"); if (ClientStatus > 0) { From ae0ad83c1e58842a2804b3007a4aa987e8eca18d Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 14 Jan 2020 16:06:59 +0100 Subject: [PATCH 033/523] Added a printout again --- server/integration-tests/tools/MSCP.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/server/integration-tests/tools/MSCP.py b/server/integration-tests/tools/MSCP.py index 9e4913eb3..5f8516c04 100644 --- a/server/integration-tests/tools/MSCP.py +++ b/server/integration-tests/tools/MSCP.py @@ -52,7 +52,8 @@ def listen(self): except ConnectionResetError as e: if not self.quit: raise e - + + print("Received: " + str(len(data))) for replyPattern in replyPatterns: match = re.search(replyPattern["regex"],data) if match is not None: From e1e183403e265042e3405028ca5601f25a44f141 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 14 Jan 2020 16:39:58 +0100 Subject: [PATCH 034/523] Added printout in the MSCP function file sender --- server/integration-tests/tools/MSCP.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/server/integration-tests/tools/MSCP.py b/server/integration-tests/tools/MSCP.py index 5f8516c04..2e5808a8e 100644 --- a/server/integration-tests/tools/MSCP.py +++ b/server/integration-tests/tools/MSCP.py @@ -161,6 +161,8 @@ def listen(self): else: self.lastUploadReply["status"] = "UNKNOWN" self.uploadReplyLock.release() + else: + print("=== Unable to match against data: " + str(data)) def GetStatus(self): message = "POST /maestro HTTP/1.1\r\nHost: " + self.host + "\r\n\r\nGetServerStatus();\r\n\r\n" @@ -206,8 +208,10 @@ def UploadFile(self,targetPath,fileContents): self.Send(message) print("=== UploadFile() sent") self.waitForUploadReply("SERVER_PREPARED") + print("=== Sending file contents") # Send file self.Send(fileContents) + print("=== Sent file contents") self.uploadReplyLock.acquire() self.lastUploadReply["status"] = "UNKNOWN" self.uploadReplyLock.release() From 05c5fdfc9fee2114986d4bb49df1c8f49f10294d Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 14 Jan 2020 16:54:12 +0100 Subject: [PATCH 035/523] Added more printouts --- server/integration-tests/tools/MSCP.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/server/integration-tests/tools/MSCP.py b/server/integration-tests/tools/MSCP.py index 2e5808a8e..5a1ba3b4f 100644 --- a/server/integration-tests/tools/MSCP.py +++ b/server/integration-tests/tools/MSCP.py @@ -230,10 +230,13 @@ def waitForUploadReply(self,status,timeout=3.0): self.uploadReplyLock.acquire() ur = self.lastUploadReply["status"] self.uploadReplyLock.release() + print("=== Entering waiting state") while ur == "UNKNOWN" and time.time() < timeoutTime: self.uploadReplyLock.acquire() ur = self.lastUploadReply["status"] self.uploadReplyLock.release() + + print("=== Exited waiting state") if ur != status and time.time() >= timeoutTime: raise TimeoutError("Timed out while waiting for reply to UploadFile") elif ur != status: From 18ee6eff22f7ced4d1a973e31f27e12416e279e9 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 14 Jan 2020 17:05:19 +0100 Subject: [PATCH 036/523] Added yet another printout --- server/integration-tests/tools/MSCP.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/server/integration-tests/tools/MSCP.py b/server/integration-tests/tools/MSCP.py index 5a1ba3b4f..3ad204498 100644 --- a/server/integration-tests/tools/MSCP.py +++ b/server/integration-tests/tools/MSCP.py @@ -236,7 +236,7 @@ def waitForUploadReply(self,status,timeout=3.0): ur = self.lastUploadReply["status"] self.uploadReplyLock.release() - print("=== Exited waiting state") + print("=== Exited waiting state: " + ur) if ur != status and time.time() >= timeoutTime: raise TimeoutError("Timed out while waiting for reply to UploadFile") elif ur != status: From a42e5293c50e46e2ef74af81cf031297e41ca05a Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 14 Jan 2020 17:06:45 +0100 Subject: [PATCH 037/523] Added yet another printout --- server/integration-tests/tools/MSCP.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/server/integration-tests/tools/MSCP.py b/server/integration-tests/tools/MSCP.py index 3ad204498..a90ddcacb 100644 --- a/server/integration-tests/tools/MSCP.py +++ b/server/integration-tests/tools/MSCP.py @@ -238,8 +238,10 @@ def waitForUploadReply(self,status,timeout=3.0): print("=== Exited waiting state: " + ur) if ur != status and time.time() >= timeoutTime: + print("Fall A") raise TimeoutError("Timed out while waiting for reply to UploadFile") elif ur != status: + print("Fall B") raise ValueError("Expected status " + status + " but received " + ur) def waitForObjectControlState(self,state,timeout=3.0): From 3a3e05378bccdde0751786f98a0fc3de35dbce15 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 14 Jan 2020 17:16:52 +0100 Subject: [PATCH 038/523] Added tjenixen to test procedure --- .../249-uploadRowMismatchingTrajectories.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/server/integration-tests/249-uploadRowMismatchingTrajectories.py b/server/integration-tests/249-uploadRowMismatchingTrajectories.py index 6b9d2f1e4..48a5a2f9f 100644 --- a/server/integration-tests/249-uploadRowMismatchingTrajectories.py +++ b/server/integration-tests/249-uploadRowMismatchingTrajectories.py @@ -43,7 +43,11 @@ def checkProgramStatus(failurePrintout): normalTraj = ReadTrajectoryFile("resources/trajectories") # 4: Upload short trajectory - userControl.UploadFile("traj/127.0.0.1", fewRowTraj) + try: + userControl.UploadFile("traj/127.0.0.1", fewRowTraj) + except TimeoutError as e: + print("tjenixen") + raise e # 5: Send init try: From 3fc0f05d1872bc834d6b94ea28c16e9ec1ba9c66 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 15 Jan 2020 09:27:13 +0100 Subject: [PATCH 039/523] Printout of all data received --- server/integration-tests/tools/MSCP.py | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/server/integration-tests/tools/MSCP.py b/server/integration-tests/tools/MSCP.py index a90ddcacb..cae125e01 100644 --- a/server/integration-tests/tools/MSCP.py +++ b/server/integration-tests/tools/MSCP.py @@ -48,12 +48,15 @@ def listen(self): print("=== Starting listener on " + str(self.host) + ":" + str(self.port)) while not self.quit: try: - data = self.socket.recv(1024) + data = self.socket.recv(2048) except ConnectionResetError as e: if not self.quit: raise e - print("Received: " + str(len(data))) + print("=== Received " + str(len(data)) + " bytes") + if len(data) > 0: + print("=== " + data) + for replyPattern in replyPatterns: match = re.search(replyPattern["regex"],data) if match is not None: @@ -161,7 +164,7 @@ def listen(self): else: self.lastUploadReply["status"] = "UNKNOWN" self.uploadReplyLock.release() - else: + elif len(data) > 0: print("=== Unable to match against data: " + str(data)) def GetStatus(self): @@ -238,10 +241,10 @@ def waitForUploadReply(self,status,timeout=3.0): print("=== Exited waiting state: " + ur) if ur != status and time.time() >= timeoutTime: - print("Fall A") + print("=== Timed out") raise TimeoutError("Timed out while waiting for reply to UploadFile") elif ur != status: - print("Fall B") + print("=== Error response") raise ValueError("Expected status " + status + " but received " + ur) def waitForObjectControlState(self,state,timeout=3.0): From ff6831f04ecd0a9717c88b98712c48df6d5c5ea5 Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Wed, 15 Jan 2020 09:28:04 +0100 Subject: [PATCH 040/523] Removed old monr to GSD --- server/src/datadictionary.c | 1 + server/src/objectcontrol.c | 11 ----------- 2 files changed, 1 insertion(+), 11 deletions(-) diff --git a/server/src/datadictionary.c b/server/src/datadictionary.c index 2b93a108d..6bd72ad0c 100644 --- a/server/src/datadictionary.c +++ b/server/src/datadictionary.c @@ -43,6 +43,7 @@ static pthread_mutex_t DataDictionaryRVSSRateMutex = PTHREAD_MUTEX_INITIALIZER; static pthread_mutex_t ASPDataMutex = PTHREAD_MUTEX_INITIALIZER; static pthread_mutex_t MiscDataMutex = PTHREAD_MUTEX_INITIALIZER; static pthread_mutex_t OBCStateMutex = PTHREAD_MUTEX_INITIALIZER; +static pthread_mutex_t MONRMutex = PTHREAD_MUTEX_INITIALIZER; diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 87542be21..c3507d6a8 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -486,17 +486,6 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { ObjectControlBuildMONRMessage(buffer, &MONRData, 0); - //Store MONR in GSD - //UtilSendUDPData("ObjectControl", &ObjectControlUDPSocketfdI32, &simulator_addr, &MONRData, sizeof(MONRData), 0); - for (i = 0; - i < - (MONRData.Header.MessageLengthU32 + COMMAND_MESSAGE_HEADER_LENGTH + - COMMAND_MESSAGE_FOOTER_LENGTH); i++) - GSD->MONRData[i] = buffer[i]; - GSD->MONRSizeU8 = - MONRData.Header.MessageLengthU32 + COMMAND_MESSAGE_HEADER_LENGTH + - COMMAND_MESSAGE_FOOTER_LENGTH; - ObjectControlMONRToASCII(&MONRData, &OriginPosition, iIndex, Id, Timestamp, XPosition, YPosition, ZPosition, LongitudinalSpeed, LateralSpeed, LongitudinalAcc, LateralAcc, Heading, DriveDirection, From c54573a904c4fcfad63bbb08f54657f9f9b39fb9 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 15 Jan 2020 09:31:18 +0100 Subject: [PATCH 041/523] Fixed printout --- server/integration-tests/tools/MSCP.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/server/integration-tests/tools/MSCP.py b/server/integration-tests/tools/MSCP.py index cae125e01..a3102c6a3 100644 --- a/server/integration-tests/tools/MSCP.py +++ b/server/integration-tests/tools/MSCP.py @@ -55,7 +55,7 @@ def listen(self): print("=== Received " + str(len(data)) + " bytes") if len(data) > 0: - print("=== " + data) + print("=== " + str(data)) for replyPattern in replyPatterns: match = re.search(replyPattern["regex"],data) From 340eafc54144bb0a062024463a6061fbd63352f2 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 15 Jan 2020 09:38:56 +0100 Subject: [PATCH 042/523] Added try/catch blocks to all file uploads --- .../249-uploadRowMismatchingTrajectories.py | 18 +++++++++++++++--- 1 file changed, 15 insertions(+), 3 deletions(-) diff --git a/server/integration-tests/249-uploadRowMismatchingTrajectories.py b/server/integration-tests/249-uploadRowMismatchingTrajectories.py index 48a5a2f9f..23559c947 100644 --- a/server/integration-tests/249-uploadRowMismatchingTrajectories.py +++ b/server/integration-tests/249-uploadRowMismatchingTrajectories.py @@ -62,7 +62,11 @@ def checkProgramStatus(failurePrintout): time.sleep(0.05) # 6: Upload normal trajectory, to verify we can still initialise - userControl.UploadFile("traj/127.0.0.1", normalTraj) + try: + userControl.UploadFile("traj/127.0.0.1", normalTraj) + except TimeoutError as e: + print("tjenixen") + raise e userControl.Init() userControl.waitForObjectControlState("INITIALIZED") @@ -73,7 +77,11 @@ def checkProgramStatus(failurePrintout): userControl.waitForObjectControlState("IDLE") # 7: Upload long trajectory - userControl.UploadFile("traj/127.0.0.1", manyRowTraj) + try: + userControl.UploadFile("traj/127.0.0.1", manyRowTraj) + except TimeoutError as e: + print("tjenixen") + raise e # 8: Send init try: @@ -88,7 +96,11 @@ def checkProgramStatus(failurePrintout): time.sleep(0.05) # 9: Upload normal trajectory, to verify we can still initialise - userControl.UploadFile("traj/127.0.0.1", normalTraj) + try: + userControl.UploadFile("traj/127.0.0.1", normalTraj) + except TimeoutError as e: + print("tjenixen") + raise e userControl.Init() userControl.waitForObjectControlState("INITIALIZED") From 4c770600ea31659757f2940625f2bd497ef37a42 Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Wed, 15 Jan 2020 09:42:40 +0100 Subject: [PATCH 043/523] Added skeleton for monr in DD --- server/src/datadictionary.c | 36 ++++++++++++++++++++++++++++++++++++ 1 file changed, 36 insertions(+) diff --git a/server/src/datadictionary.c b/server/src/datadictionary.c index 6bd72ad0c..d5de04b7d 100644 --- a/server/src/datadictionary.c +++ b/server/src/datadictionary.c @@ -1666,6 +1666,42 @@ OBCState_t DataDictionaryGetOBCStateU8(GSDType * GSD) { /*END OBCState*/ +/*! + * \brief DataDictionarySetMONR Parses input variable and sets variable to corresponding value + * \param GSD Pointer to shared allocated memory + * \param MONRdata Monitor data + * \param id object id + * \return Result according to ::ReadWriteAccess_t + */ +ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, MONRType * MONR, int id) { + ReadWriteAccess_t Res; + + Res = WRITE_OK; + pthread_mutex_lock(&MONRMutex); + //GSD->MONRdata[id] = MONR + pthread_mutex_unlock(&MONRMutex); + + return Res; +} + +/*! + * \brief DataDictionaryGetMONR Reads variable from shared memory + * \param GSD Pointer to shared allocated memory + * \param MONRdata Return variable pointer + * \param id requesed object id + * \return Result according to ::ReadWriteAccess_t + */ +ReadWriteAccess_t DataDictionaryGetMONR(GSDType * GSD, MONRType * MONR, int id) { + pthread_mutex_lock(&MONRMutex); + //*MONR = GSD->MONRdata[id]; + pthread_mutex_unlock(&MONRMutex); + return READ_OK; +} + +/*END of MONR*/ + + + /*! * \brief DataDictionarySearchParameter Searches for parameters in the configuration file and returns the parameter value. From 321c1036c67ba71cc6631d81401a8f44cca8fdb1 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 15 Jan 2020 11:30:24 +0100 Subject: [PATCH 044/523] Made sleep time to 10 ms instead of half a second --- server/src/timecontrol.c | 29 ++++++++++++++++------------- 1 file changed, 16 insertions(+), 13 deletions(-) diff --git a/server/src/timecontrol.c b/server/src/timecontrol.c index ebdb0abe5..de1aeb724 100644 --- a/server/src/timecontrol.c +++ b/server/src/timecontrol.c @@ -40,7 +40,7 @@ #define REPLY_TIMEOUT_S 3 #define SLEEP_TIME_GPS_CONNECTED_S 0 -#define SLEEP_TIME_GPS_CONNECTED_NS 500000000 +#define SLEEP_TIME_GPS_CONNECTED_NS 5000000//500000000 #define SLEEP_TIME_NO_GPS_CONNECTED_S 1 #define SLEEP_TIME_NO_GPS_CONNECTED_NS 0 @@ -52,6 +52,12 @@ #define FIX_QUALITY_BASIC 1 #define FIX_QUALITY_DIFFERENTIAL 2 +// Time intervals for sleeping when no message bus message was received and for when one was received +#define TC_SLEEP_TIME_EMPTY_MQ_S 0 +#define TC_SLEEP_TIME_EMPTY_MQ_NS 10000000 +#define TC_SLEEP_TIME_NONEMPTY_MQ_S 0 +#define TC_SLEEP_TIME_NONEMPTY_MQ_NS 0 + /*------------------------------------------------------------ -- Function declarations. ------------------------------------------------------------*/ @@ -83,6 +89,9 @@ void timecontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { U16 ServerPortU16; I32 SocketfdI32 = -1; struct sockaddr_in time_addr; + const struct timespec mqEmptyPollPeriod = { TC_SLEEP_TIME_EMPTY_MQ_S, TC_SLEEP_TIME_EMPTY_MQ_NS }; + const struct timespec mqNonEmptyPollPeriod = + { TC_SLEEP_TIME_NONEMPTY_MQ_S, TC_SLEEP_TIME_NONEMPTY_MQ_NS }; I32 result; C8 TimeBuffer[TIME_CONTROL_RECEIVE_BUFFER_SIZE]; @@ -164,7 +173,9 @@ void timecontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { while (!iExit) { // Ignore any commands received, just empty the bus - iCommRecv(&command, busReceiveBuffer, sizeof (busReceiveBuffer), NULL); + do { + iCommRecv(&command, busReceiveBuffer, sizeof (busReceiveBuffer), NULL); + } while (command != COMM_INV); gettimeofday(&ExecTime, NULL); CurrentMilliSecondU16 = (U16) (ExecTime.tv_usec / 1000); @@ -236,17 +247,9 @@ void timecontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { (void)iCommClose(); } - if (ReceivedNewData && GPSTime->isGPSenabled) { - /* Make call periodic */ - sleep_time.tv_sec = SLEEP_TIME_GPS_CONNECTED_S; - sleep_time.tv_nsec = SLEEP_TIME_GPS_CONNECTED_NS; - nanosleep(&sleep_time, &ref_time); - } - else if (!GPSTime->isGPSenabled) { - sleep_time.tv_sec = SLEEP_TIME_NO_GPS_CONNECTED_S; - sleep_time.tv_nsec = SLEEP_TIME_NO_GPS_CONNECTED_NS; - nanosleep(&sleep_time, &ref_time); - } + + sleep_time = command == COMM_INV ? mqEmptyPollPeriod : mqNonEmptyPollPeriod; + nanosleep(&sleep_time, &ref_time); } LogMessage(LOG_LEVEL_INFO, "Time control exiting"); From 60fc3739ecb0dbcffa2cac2cb2ac85b3a5f01cc0 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 15 Jan 2020 11:30:46 +0100 Subject: [PATCH 045/523] Ran code formatter --- server/src/timecontrol.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/server/src/timecontrol.c b/server/src/timecontrol.c index de1aeb724..93717e0df 100644 --- a/server/src/timecontrol.c +++ b/server/src/timecontrol.c @@ -40,7 +40,7 @@ #define REPLY_TIMEOUT_S 3 #define SLEEP_TIME_GPS_CONNECTED_S 0 -#define SLEEP_TIME_GPS_CONNECTED_NS 5000000//500000000 +#define SLEEP_TIME_GPS_CONNECTED_NS 5000000 //500000000 #define SLEEP_TIME_NO_GPS_CONNECTED_S 1 #define SLEEP_TIME_NO_GPS_CONNECTED_NS 0 From 38a3350ad63c47f67f89d1ed6e1e241889d6d891 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 15 Jan 2020 13:09:35 +0100 Subject: [PATCH 046/523] Added protocol version to messages that were missing it --- server/src/objectcontrol.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 6e4f68378..786918971 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -1631,7 +1631,7 @@ int ObjectControlBuildSTRTMessage(C8 * MessageBuffer, STRTType * STRTData, TimeT STRTData->Header.SyncWordU16 = ISO_SYNC_WORD; STRTData->Header.TransmitterIdU8 = 0; STRTData->Header.MessageCounterU8 = 0; - STRTData->Header.AckReqProtVerU8 = 0; + STRTData->Header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; STRTData->Header.MessageIdU16 = COMMAND_STRT_CODE; STRTData->Header.MessageLengthU32 = sizeof (STRTType) - sizeof (HeaderType); STRTData->StartTimeValueIdU16 = VALUE_ID_GPS_SECOND_OF_WEEK; @@ -1691,7 +1691,7 @@ I32 ObjectControlBuildOSTMMessage(C8 * MessageBuffer, OSTMType * OSTMData, C8 Co OSTMData->Header.SyncWordU16 = ISO_SYNC_WORD; OSTMData->Header.TransmitterIdU8 = 0; OSTMData->Header.MessageCounterU8 = 0; - OSTMData->Header.AckReqProtVerU8 = 0; + OSTMData->Header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; OSTMData->Header.MessageIdU16 = COMMAND_OSTM_CODE; OSTMData->Header.MessageLengthU32 = sizeof (OSTMType) - sizeof (HeaderType); OSTMData->StateValueIdU16 = VALUE_ID_STATE_CHANGE_REQUEST; @@ -1824,7 +1824,7 @@ I32 ObjectControlBuildSYPMMessage(C8 * MessageBuffer, SYPMType * SYPMData, U32 S SYPMData->Header.SyncWordU16 = ISO_SYNC_WORD; SYPMData->Header.TransmitterIdU8 = 0; SYPMData->Header.MessageCounterU8 = 0; - SYPMData->Header.AckReqProtVerU8 = 0; + SYPMData->Header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; SYPMData->Header.MessageIdU16 = COMMAND_SYPM_CODE; SYPMData->Header.MessageLengthU32 = sizeof (SYPMType) - sizeof (HeaderType); SYPMData->SyncPointTimeValueIdU16 = 1; @@ -1874,7 +1874,7 @@ I32 ObjectControlBuildMTSPMessage(C8 * MessageBuffer, MTSPType * MTSPData, U32 S MTSPData->Header.SyncWordU16 = ISO_SYNC_WORD; MTSPData->Header.TransmitterIdU8 = 0; MTSPData->Header.MessageCounterU8 = 0; - MTSPData->Header.AckReqProtVerU8 = 0; + MTSPData->Header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; MTSPData->Header.MessageIdU16 = COMMAND_MTSP_CODE; MTSPData->Header.MessageLengthU32 = sizeof (MTSPType) - sizeof (HeaderType); MTSPData->EstSyncPointTimeValueIdU16 = 1; From 89015fe96a87cea02e0d001e5b7afc0b8526416d Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Wed, 15 Jan 2020 13:10:29 +0100 Subject: [PATCH 047/523] Cleanup --- server/src/objectcontrol.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index f71eed152..d25dcd52a 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -810,12 +810,12 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { LogMessage(LOG_LEVEL_INFO, "CONNECT received"); LOG_SEND(LogBuffer, "[ObjectControl] CONNECT received."); - + /* if(nbr_objects < 1){ LogMessage(LOG_LEVEL_ERROR, "No objects avaliable to connect."); DisconnectU8 = 1; } - + */ /* Connect and send drive files */ for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { From 7db2d4b30607af414e6b45fc4676983f1e4f02f2 Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Wed, 15 Jan 2020 13:11:18 +0100 Subject: [PATCH 048/523] Cleanup --- server/src/objectcontrol.c | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index d25dcd52a..cc3666058 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -810,12 +810,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { LogMessage(LOG_LEVEL_INFO, "CONNECT received"); LOG_SEND(LogBuffer, "[ObjectControl] CONNECT received."); - /* - if(nbr_objects < 1){ - LogMessage(LOG_LEVEL_ERROR, "No objects avaliable to connect."); - DisconnectU8 = 1; - } - */ + /* Connect and send drive files */ for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { From cf9063349a8262c6acc8513d4ea384e7586e6efc Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 15 Jan 2020 13:43:43 +0100 Subject: [PATCH 049/523] Added a finally block which shuts down test components --- .../249-uploadRowMismatchingTrajectories.py | 134 ++++++++---------- 1 file changed, 61 insertions(+), 73 deletions(-) diff --git a/server/integration-tests/249-uploadRowMismatchingTrajectories.py b/server/integration-tests/249-uploadRowMismatchingTrajectories.py index 23559c947..31c4dd9be 100644 --- a/server/integration-tests/249-uploadRowMismatchingTrajectories.py +++ b/server/integration-tests/249-uploadRowMismatchingTrajectories.py @@ -25,93 +25,81 @@ def checkProgramStatus(failurePrintout): if __name__ == "__main__": - - # Note: server does not close sockets properly so this fails frequently (cross fingers for now): - #WaitForPortAvailable(54241,"TCP",timeout=0) - server = Executable("../build/TEServer",["-m","0"]) - time.sleep(0.05) - checkProgramStatus("=== Starting the server caused a problem") - - # 1: Connect to the server - userControl = MSCP("127.0.0.1") - time.sleep(0.25) - checkProgramStatus("=== Connecting to the server caused a problem") - - # 2: Load trajectory - fewRowTraj = ReadTrajectoryFile("resources/trajectories/faulty",fileName="GarageplanInnerring_lessRowsThanSpecified.traj") - manyRowTraj = ReadTrajectoryFile("resources/trajectories/faulty",fileName="GarageplanInnerring_moreRowsThanSpecified.traj") - normalTraj = ReadTrajectoryFile("resources/trajectories") - - # 4: Upload short trajectory try: + # Note: server does not close sockets properly so this fails frequently (cross fingers for now): + #WaitForPortAvailable(54241,"TCP",timeout=0) + server = Executable("../build/TEServer",["-m","0"]) + time.sleep(0.05) + checkProgramStatus("=== Starting the server caused a problem") + + # 1: Connect to the server + userControl = MSCP("127.0.0.1") + time.sleep(0.25) + checkProgramStatus("=== Connecting to the server caused a problem") + + # 2: Load trajectory + fewRowTraj = ReadTrajectoryFile("resources/trajectories/faulty",fileName="GarageplanInnerring_lessRowsThanSpecified.traj") + manyRowTraj = ReadTrajectoryFile("resources/trajectories/faulty",fileName="GarageplanInnerring_moreRowsThanSpecified.traj") + normalTraj = ReadTrajectoryFile("resources/trajectories") + + # 4: Upload short trajectory userControl.UploadFile("traj/127.0.0.1", fewRowTraj) - except TimeoutError as e: - print("tjenixen") - raise e - # 5: Send init - try: - userControl.Init() - time.sleep(0.05) - checkProgramStatus("=== Sending init to the server after uploading trajectory with less rows than specified caused a problem") - userControl.waitForObjectControlState("INITIALIZED", timeout=0.5) - raise AssertionError("Transitioned to initialized even though malformed trajectory was uploaded") - except TimeoutError as e: - # If there was a timeout while waiting for initialized that means everything went as intended - print("=== Timed out successfully while waiting for initialisation") - - time.sleep(0.05) - # 6: Upload normal trajectory, to verify we can still initialise - try: + # 5: Send init + try: + userControl.Init() + time.sleep(0.05) + checkProgramStatus("=== Sending init to the server after uploading trajectory with less rows than specified caused a problem") + userControl.waitForObjectControlState("INITIALIZED", timeout=0.5) + raise AssertionError("Transitioned to initialized even though malformed trajectory was uploaded") + except TimeoutError as e: + # If there was a timeout while waiting for initialized that means everything went as intended + print("=== Timed out successfully while waiting for initialisation") + + time.sleep(0.05) + + # 6: Upload normal trajectory, to verify we can still initialise userControl.UploadFile("traj/127.0.0.1", normalTraj) - except TimeoutError as e: - print("tjenixen") - raise e - userControl.Init() - userControl.waitForObjectControlState("INITIALIZED") - - time.sleep(0.05) + userControl.Init() + userControl.waitForObjectControlState("INITIALIZED") + + time.sleep(0.05) - userControl.Disconnect() - userControl.waitForObjectControlState("IDLE") + userControl.Disconnect() + userControl.waitForObjectControlState("IDLE") - # 7: Upload long trajectory - try: + # 7: Upload long trajectory userControl.UploadFile("traj/127.0.0.1", manyRowTraj) - except TimeoutError as e: - print("tjenixen") - raise e - # 8: Send init - try: - userControl.Init() + # 8: Send init + try: + userControl.Init() + time.sleep(0.05) + checkProgramStatus("=== Sending init to the server after uploading trajectory with more rows than specified caused a problem") + userControl.waitForObjectControlState("INITIALIZED", timeout=0.5) + raise AssertionError("Transitioned to initialized even though malformed trajectory was uploaded") + except TimeoutError as e: + # If there was a timeout while waiting for initialized that means everything went as intended + print("=== Timed out successfully while waiting for initialisation") + time.sleep(0.05) - checkProgramStatus("=== Sending init to the server after uploading trajectory with more rows than specified caused a problem") - userControl.waitForObjectControlState("INITIALIZED", timeout=0.5) - raise AssertionError("Transitioned to initialized even though malformed trajectory was uploaded") - except TimeoutError as e: - # If there was a timeout while waiting for initialized that means everything went as intended - print("=== Timed out successfully while waiting for initialisation") - - time.sleep(0.05) - # 9: Upload normal trajectory, to verify we can still initialise - try: + + # 9: Upload normal trajectory, to verify we can still initialise userControl.UploadFile("traj/127.0.0.1", normalTraj) - except TimeoutError as e: - print("tjenixen") - raise e - userControl.Init() - userControl.waitForObjectControlState("INITIALIZED") + userControl.Init() + userControl.waitForObjectControlState("INITIALIZED") - time.sleep(0.05) + time.sleep(0.05) - userControl.Disconnect() - userControl.waitForObjectControlState("IDLE") + userControl.Disconnect() + userControl.waitForObjectControlState("IDLE") - # 10: Done! - userControl.shutdown() - server.stop() + finally: + # 10: Done! + userControl.shutdown() + server.stop() + sys.exit(0) From 4676bc37e9fedc8e92ba03c4d3c8c7d1eef5ef7d Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 15 Jan 2020 17:16:14 +0100 Subject: [PATCH 050/523] Added printout when uploadfile command received --- server/src/systemcontrol.c | 1 + 1 file changed, 1 insertion(+) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index 6a6bbf345..642b3b0f7 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -748,6 +748,7 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { } break; case UploadFile_3: + LogMessage(LOG_LEVEL_INFO, "Received upload command"); if (CurrentInputArgCount == CommandArgCount) { SystemControlCommand = Idle_0; bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); From aab847bb26f5055434ce058ac581a5eb51ace53c Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 15 Jan 2020 17:16:52 +0100 Subject: [PATCH 051/523] Removed a printout --- server/src/systemcontrol.c | 3 --- 1 file changed, 3 deletions(-) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index 642b3b0f7..46142f816 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -2150,10 +2150,7 @@ I32 SystemControlReceiveRxData(I32 * sockfd, C8 * Path, C8 * FileSize, C8 * Pack } bzero(RxBuffer, PacketSizeU16); - LogPrint("AAAA"); ClientStatus = recv(*sockfd, RxBuffer, PacketSizeU16, MSG_WAITALL); - LogPrint("BBBB"); - if (ClientStatus > 0) { i++; From c0e9a4c7e525bb84efb0e192ccb712ac30f3f0ee Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 15 Jan 2020 17:23:09 +0100 Subject: [PATCH 052/523] Added a printout of the entire received data --- server/src/systemcontrol.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index 46142f816..00b6b4368 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -381,6 +381,7 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { server_state == SERVER_STATE_UNDEFINED; // TODO: Should this be an assignment? } else if (ClientResult > 0 && ClientResult < TCP_RECV_BUFFER_SIZE) { + LogPrint("Received: %s",pcBuffer); // TODO: Move this entire decoding process into a separate function for (i = 0; i < SYSTEM_CONTROL_ARG_MAX_COUNT; i++) bzero(SystemControlArgument[i], SYSTEM_CONTROL_ARGUMENT_MAX_LENGTH); @@ -748,7 +749,6 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { } break; case UploadFile_3: - LogMessage(LOG_LEVEL_INFO, "Received upload command"); if (CurrentInputArgCount == CommandArgCount) { SystemControlCommand = Idle_0; bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); From 04e243054c065ec6d3e7009de9493a3cf3cb3b0e Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 15 Jan 2020 17:30:15 +0100 Subject: [PATCH 053/523] Added printout of raw data --- server/src/systemcontrol.c | 1 + 1 file changed, 1 insertion(+) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index 00b6b4368..3bd4e284b 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -1181,6 +1181,7 @@ ssize_t SystemControlReceiveUserControlData(I32 socket, C8 * dataBuffer, size_t readResult = recv(socket, recvBuffer + bytesInBuffer, sizeof (recvBuffer) - bytesInBuffer, MSG_DONTWAIT); if (readResult > 0) { bytesInBuffer += (size_t) readResult; + LogPrint("Raw: %s",recvBuffer); } if (bytesInBuffer > 0) { if ((endOfMessage = strstr(recvBuffer, endOfMessagePattern)) != NULL) { From 66fb2415bfcdc82420d6ff7f3d797682920168b2 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 15 Jan 2020 17:35:40 +0100 Subject: [PATCH 054/523] Ran code formatter --- server/src/systemcontrol.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index 3bd4e284b..17b686f52 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -381,7 +381,7 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { server_state == SERVER_STATE_UNDEFINED; // TODO: Should this be an assignment? } else if (ClientResult > 0 && ClientResult < TCP_RECV_BUFFER_SIZE) { - LogPrint("Received: %s",pcBuffer); + LogPrint("Received: %s", pcBuffer); // TODO: Move this entire decoding process into a separate function for (i = 0; i < SYSTEM_CONTROL_ARG_MAX_COUNT; i++) bzero(SystemControlArgument[i], SYSTEM_CONTROL_ARGUMENT_MAX_LENGTH); @@ -1181,7 +1181,7 @@ ssize_t SystemControlReceiveUserControlData(I32 socket, C8 * dataBuffer, size_t readResult = recv(socket, recvBuffer + bytesInBuffer, sizeof (recvBuffer) - bytesInBuffer, MSG_DONTWAIT); if (readResult > 0) { bytesInBuffer += (size_t) readResult; - LogPrint("Raw: %s",recvBuffer); + LogPrint("Raw: %s", recvBuffer); } if (bytesInBuffer > 0) { if ((endOfMessage = strstr(recvBuffer, endOfMessagePattern)) != NULL) { From 2d2b549cd01566014680eca1acd4ef62e4a07750 Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Thu, 16 Jan 2020 10:11:13 +0100 Subject: [PATCH 055/523] Added basic allocation --- server/inc/datadictionary.h | 7 +++++ server/inc/util.h | 2 ++ server/src/datadictionary.c | 55 +++++++++++++++++++++++++++++++++++++ 3 files changed, 64 insertions(+) diff --git a/server/inc/datadictionary.h b/server/inc/datadictionary.h index 5c8287c91..b6765c395 100644 --- a/server/inc/datadictionary.h +++ b/server/inc/datadictionary.h @@ -119,6 +119,13 @@ ReadWriteAccess_t DataDictionaryGetMiscDataC8(GSDType *GSD, C8 *MiscData, U32 Bu ReadWriteAccess_t DataDictionarySetOBCStateU8(GSDType *GSD, OBCState_t OBCState); OBCState_t DataDictionaryGetOBCStateU8(GSDType *GSD); +ReadWriteAccess_t DataDictionaryFreeMONR(GSDType * GSD); +ReadWriteAccess_t DataDictionaryInitMONR(GSDType * GSD, int size); +ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, MONRType * MONR, int id); +ReadWriteAccess_t DataDictionaryGetMONR(GSDType * GSD, MONRType * MONR, int id); + +/*END of MONR*/ + ReadWriteAccess_t DataDictionaryConstructor(GSDType *GSD); #endif diff --git a/server/inc/util.h b/server/inc/util.h index f85df3b8a..e271d4914 100644 --- a/server/inc/util.h +++ b/server/inc/util.h @@ -532,6 +532,8 @@ typedef struct U8 MONRData[100]; U8 HEABSizeU8; U8 HEABData[100]; + + MONRType* MonrData; //U8 OSTMSizeU8; //U8 OSTMData[100]; //U8 STRTSizeU8; diff --git a/server/src/datadictionary.c b/server/src/datadictionary.c index d5de04b7d..dbc949f31 100644 --- a/server/src/datadictionary.c +++ b/server/src/datadictionary.c @@ -1666,6 +1666,61 @@ OBCState_t DataDictionaryGetOBCStateU8(GSDType * GSD) { /*END OBCState*/ +/*! + * \brief DataDictionaryInitMONR inits a data structure for saving object monr + * \param GSD Pointer to shared allocated memory + * \param objects number of objects that will transmitt monr + * \return Result according to ::ReadWriteAccess_t + */ +ReadWriteAccess_t DataDictionaryInitMONR(GSDType * GSD, int size){ + ReadWriteAccess_t Res; + + Res = WRITE_OK; + pthread_mutex_lock(&MONRMutex); + + GSD->MonrData = (MONRType*)malloc(size * sizeof(MONRType)); + + // Check if the memory has been successfully + if (GSD->MonrData == NULL) { + LogPrint(LOG_LEVEL_ERROR, "Coult not allocate memmory for MONR"); + } + else { + // Memory has been successfully allocated + LogPrint(LOG_LEVEL_INFO, "Allocated memory for %d objects.", size); + } + + pthread_mutex_unlock(&MONRMutex); + return Res; +} + +/*! + * \brief DataDictionaryInitMONR inits a data structure for saving object monr + * \param GSD Pointer to shared allocated memory + * \param objects number of objects that will transmitt monr + * \return Result according to ::ReadWriteAccess_t + */ +ReadWriteAccess_t DataDictionaryFreeMONR(GSDType * GSD){ + ReadWriteAccess_t Res; + + Res = WRITE_OK; + pthread_mutex_lock(&MONRMutex); + + free(GSD->MonrData); + + // Check if the memory has been freed successfully + if (GSD->MonrData == NULL) { + LogPrint(LOG_LEVEL_INFO, "MONR array freed"); + } + else { + // Memory has been successfully allocated + LogPrint(LOG_LEVEL_ERROR, "Could not free monr data struct"); + } + + pthread_mutex_unlock(&MONRMutex); + return Res; +} + + /*! * \brief DataDictionarySetMONR Parses input variable and sets variable to corresponding value * \param GSD Pointer to shared allocated memory From 4061cd56140b2c648a3b17bc15bea048c389657c Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 16 Jan 2020 12:40:56 +0100 Subject: [PATCH 056/523] Removed debug printouts from systemcontrol and fixed return value --- server/src/systemcontrol.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index 17b686f52..f6e79a324 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -381,7 +381,6 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { server_state == SERVER_STATE_UNDEFINED; // TODO: Should this be an assignment? } else if (ClientResult > 0 && ClientResult < TCP_RECV_BUFFER_SIZE) { - LogPrint("Received: %s", pcBuffer); // TODO: Move this entire decoding process into a separate function for (i = 0; i < SYSTEM_CONTROL_ARG_MAX_COUNT; i++) bzero(SystemControlArgument[i], SYSTEM_CONTROL_ARGUMENT_MAX_LENGTH); @@ -1181,8 +1180,8 @@ ssize_t SystemControlReceiveUserControlData(I32 socket, C8 * dataBuffer, size_t readResult = recv(socket, recvBuffer + bytesInBuffer, sizeof (recvBuffer) - bytesInBuffer, MSG_DONTWAIT); if (readResult > 0) { bytesInBuffer += (size_t) readResult; - LogPrint("Raw: %s", recvBuffer); } + if (bytesInBuffer > 0) { if ((endOfMessage = strstr(recvBuffer, endOfMessagePattern)) != NULL) { endOfMessage += sizeof (endOfMessagePattern) - 1; @@ -1203,6 +1202,7 @@ ssize_t SystemControlReceiveUserControlData(I32 socket, C8 * dataBuffer, size_t } else { memcpy(dataBuffer, recvBuffer, messageLength); + readResult = (ssize_t) messageLength; } bytesInBuffer -= messageLength; memmove(recvBuffer, recvBuffer + messageLength, bytesInBuffer); From 2a91ebcc6b09856d560537bf882576200680d0b8 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 16 Jan 2020 12:46:36 +0100 Subject: [PATCH 057/523] Made test printouts a bit cleaner --- server/integration-tests/tools/MSCP.py | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/server/integration-tests/tools/MSCP.py b/server/integration-tests/tools/MSCP.py index a3102c6a3..efd87d0fc 100644 --- a/server/integration-tests/tools/MSCP.py +++ b/server/integration-tests/tools/MSCP.py @@ -53,8 +53,8 @@ def listen(self): if not self.quit: raise e - print("=== Received " + str(len(data)) + " bytes") if len(data) > 0: + print("=== Received " + str(len(data)) + " bytes:") print("=== " + str(data)) for replyPattern in replyPatterns: @@ -219,7 +219,7 @@ def UploadFile(self,targetPath,fileContents): self.lastUploadReply["status"] = "UNKNOWN" self.uploadReplyLock.release() self.waitForUploadReply("UPLOAD_SUCCESS") - print("=== File uploaded") + print("=== File upload verified") def Send(self,message): self.socket.send(message.encode()) @@ -233,18 +233,16 @@ def waitForUploadReply(self,status,timeout=3.0): self.uploadReplyLock.acquire() ur = self.lastUploadReply["status"] self.uploadReplyLock.release() - print("=== Entering waiting state") while ur == "UNKNOWN" and time.time() < timeoutTime: self.uploadReplyLock.acquire() ur = self.lastUploadReply["status"] self.uploadReplyLock.release() - print("=== Exited waiting state: " + ur) if ur != status and time.time() >= timeoutTime: - print("=== Timed out") + print("=== Timed out while waiting for upload reply") raise TimeoutError("Timed out while waiting for reply to UploadFile") elif ur != status: - print("=== Error response") + print("=== File upload error response") raise ValueError("Expected status " + status + " but received " + ur) def waitForObjectControlState(self,state,timeout=3.0): @@ -259,7 +257,7 @@ def waitForObjectControlState(self,state,timeout=3.0): self.statusReplyLock.release() print("=== Expecting: " + state + ", Current: " + sr) self.GetStatus() - print("=== Expecting: " + state + ", Current: " + sr) + if sr != state: raise TimeoutError("Timed out while waiting for transition to " + state) From 8fbf1244d8de3380951d0e6288e2da2a8e599650 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 16 Jan 2020 14:52:09 +0100 Subject: [PATCH 058/523] Ran code formatter --- server/src/objectcontrol.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 65c565847..a6bf3bc4a 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -468,8 +468,9 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { object_address_name[iIndex], object_udp_port[iIndex], receivedMONRData, buffer); - if ( ObjectControlBuildMONRMessage(buffer, receivedMONRData, &MONRData, 0) == -1 ) { - LogMessage(LOG_LEVEL_INFO, "Error decoding MONR from %s: disconnecting object", object_address_name[iIndex]); + if (ObjectControlBuildMONRMessage(buffer, receivedMONRData, &MONRData, 0) == -1) { + LogMessage(LOG_LEVEL_INFO, "Error decoding MONR from %s: disconnecting object", + object_address_name[iIndex]); vDisconnectObject(&safety_socket_fd[iIndex]); // TODO smarter way of handling? continue; From 10ec5376451dc1f101a61adaa3cff2a8883b3e5e Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Thu, 16 Jan 2020 15:39:51 +0100 Subject: [PATCH 059/523] Basic allocation --- server/inc/datadictionary.h | 8 +- server/inc/util.h | 2 +- server/src/datadictionary.c | 28 +- server/src/objectcontrol.c | 5504 ++++++++++++++++++----------------- 4 files changed, 2780 insertions(+), 2762 deletions(-) diff --git a/server/inc/datadictionary.h b/server/inc/datadictionary.h index b6765c395..3e87eb198 100644 --- a/server/inc/datadictionary.h +++ b/server/inc/datadictionary.h @@ -119,10 +119,10 @@ ReadWriteAccess_t DataDictionaryGetMiscDataC8(GSDType *GSD, C8 *MiscData, U32 Bu ReadWriteAccess_t DataDictionarySetOBCStateU8(GSDType *GSD, OBCState_t OBCState); OBCState_t DataDictionaryGetOBCStateU8(GSDType *GSD); -ReadWriteAccess_t DataDictionaryFreeMONR(GSDType * GSD); -ReadWriteAccess_t DataDictionaryInitMONR(GSDType * GSD, int size); -ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, MONRType * MONR, int id); -ReadWriteAccess_t DataDictionaryGetMONR(GSDType * GSD, MONRType * MONR, int id); +ReadWriteAccess_t DataDictionaryFreeMONR(GSDType *GSD); +ReadWriteAccess_t DataDictionaryInitMONR(GSDType *GSD); +ReadWriteAccess_t DataDictionarySetMONR(GSDType *GSD, MONRType * MONR, U8 size); +ReadWriteAccess_t DataDictionaryGetMONR(GSDType *GSD, MONRType * MONR, U8 size); /*END of MONR*/ diff --git a/server/inc/util.h b/server/inc/util.h index e271d4914..e9f11898e 100644 --- a/server/inc/util.h +++ b/server/inc/util.h @@ -533,7 +533,7 @@ typedef struct U8 HEABSizeU8; U8 HEABData[100]; - MONRType* MonrData; + MONRType **test; //U8 OSTMSizeU8; //U8 OSTMData[100]; //U8 STRTSizeU8; diff --git a/server/src/datadictionary.c b/server/src/datadictionary.c index dbc949f31..ba7b9506e 100644 --- a/server/src/datadictionary.c +++ b/server/src/datadictionary.c @@ -82,7 +82,7 @@ ReadWriteAccess_t DataDictionaryConstructor(GSDType * GSD) { Res = Res == READ_OK ? DataDictionaryInitSimulatorTCPPortU16(GSD) : Res; Res = Res == READ_OK ? DataDictionaryInitSimulatorUDPPortU16(GSD) : Res; Res = Res == READ_OK ? DataDictionaryInitSimulatorModeU8(GSD) : Res; - Res = Res == READ_OK ? DataDictionaryInitVOILReceiversC8(GSD) : Res; + Res = Res == READ_OK ? DataDictionaryInitVOILReceiversC8(GSD) : Res; Res = Res == READ_OK ? DataDictionaryInitDTMReceiversC8(GSD) : Res; Res = Res == READ_OK ? DataDictionaryInitExternalSupervisorIPU32(GSD) : Res; Res = Res == READ_OK ? DataDictionaryInitRVSSConfigU32(GSD) : Res; @@ -90,9 +90,10 @@ ReadWriteAccess_t DataDictionaryConstructor(GSDType * GSD) { Res = Res == READ_OK ? DataDictionaryInitSupervisorTCPPortU16(GSD) : Res; Res = Res == READ_OK ? DataDictionaryInitMiscDataC8(GSD) : Res; - DataDictionarySetOBCStateU8(GSD, OBC_STATE_UNDEFINED); + DataDictionarySetOBCStateU8(GSD, OBC_STATE_UNDEFINED); + GSD->test = (MONRType**)malloc(sizeof(MONRType*)); - return Res; + return Res; } @@ -1672,21 +1673,21 @@ OBCState_t DataDictionaryGetOBCStateU8(GSDType * GSD) { * \param objects number of objects that will transmitt monr * \return Result according to ::ReadWriteAccess_t */ -ReadWriteAccess_t DataDictionaryInitMONR(GSDType * GSD, int size){ +ReadWriteAccess_t DataDictionaryInitMONR(GSDType * GSD){ ReadWriteAccess_t Res; Res = WRITE_OK; pthread_mutex_lock(&MONRMutex); - GSD->MonrData = (MONRType*)malloc(size * sizeof(MONRType)); - + GSD->test = (MONRType*)malloc(sizeof(MONRType)); + LogPrint("Size of MONRtype = %d", sizeof (MONRType)); // Check if the memory has been successfully - if (GSD->MonrData == NULL) { + if (GSD->test == NULL) { LogPrint(LOG_LEVEL_ERROR, "Coult not allocate memmory for MONR"); } else { // Memory has been successfully allocated - LogPrint(LOG_LEVEL_INFO, "Allocated memory for %d objects.", size); + LogPrint(LOG_LEVEL_INFO, "Allocated pointer for MONR."); } pthread_mutex_unlock(&MONRMutex); @@ -1705,10 +1706,11 @@ ReadWriteAccess_t DataDictionaryFreeMONR(GSDType * GSD){ Res = WRITE_OK; pthread_mutex_lock(&MONRMutex); - free(GSD->MonrData); + + free(GSD->test); // Check if the memory has been freed successfully - if (GSD->MonrData == NULL) { + if (GSD->test == NULL) { LogPrint(LOG_LEVEL_INFO, "MONR array freed"); } else { @@ -1728,12 +1730,12 @@ ReadWriteAccess_t DataDictionaryFreeMONR(GSDType * GSD){ * \param id object id * \return Result according to ::ReadWriteAccess_t */ -ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, MONRType * MONR, int id) { +ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, MONRType * MONR, U8 size) { ReadWriteAccess_t Res; Res = WRITE_OK; pthread_mutex_lock(&MONRMutex); - //GSD->MONRdata[id] = MONR + GSD->test = MONR; pthread_mutex_unlock(&MONRMutex); return Res; @@ -1746,7 +1748,7 @@ ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, MONRType * MONR, int id) * \param id requesed object id * \return Result according to ::ReadWriteAccess_t */ -ReadWriteAccess_t DataDictionaryGetMONR(GSDType * GSD, MONRType * MONR, int id) { +ReadWriteAccess_t DataDictionaryGetMONR(GSDType * GSD, MONRType * MONR, U8 id) { pthread_mutex_lock(&MONRMutex); //*MONR = GSD->MONRdata[id]; pthread_mutex_unlock(&MONRMutex); diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index c3507d6a8..80fe48f11 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -134,16 +134,16 @@ typedef enum { - COMMAND_HEARTBEAT_GO, - COMMAND_HEARTBEAT_ABORT + COMMAND_HEARTBEAT_GO, + COMMAND_HEARTBEAT_ABORT } hearbeatCommand_t; typedef enum { - TRANSITION_RESULT_UNDEFINED, - TRANSITION_OK, - TRANSITION_INVALID, - TRANSITION_MEMORY_ERROR + TRANSITION_RESULT_UNDEFINED, + TRANSITION_OK, + TRANSITION_INVALID, + TRANSITION_MEMORY_ERROR } StateTransitionResult; /* Small note: syntax for declaring a function pointer is (example for a function taking an int and a float, @@ -158,7 +158,7 @@ typedef StateTransitionResult(*StateTransition) (OBCState_t * currentState, OBCS C8 TrajBuffer[COMMAND_DOTM_ROWS_IN_TRANSMISSION * COMMAND_DOTM_ROW_MESSAGE_LENGTH + - COMMAND_MESSAGE_HEADER_LENGTH + COMMAND_TRAJ_INFO_ROW_MESSAGE_LENGTH]; + COMMAND_MESSAGE_HEADER_LENGTH + COMMAND_TRAJ_INFO_ROW_MESSAGE_LENGTH]; /*------------------------------------------------------------ @@ -169,40 +169,40 @@ static void vDisconnectObject(int *sockfd); static I32 vCheckRemoteDisconnected(int *sockfd); static void vCreateSafetyChannel(const char *name, const uint32_t port, int *sockfd, - struct sockaddr_in *addr); + struct sockaddr_in *addr); static void vCloseSafetyChannel(int *sockfd); I32 ObjectControlBuildOSEMMessage(C8 * MessageBuffer, OSEMType * OSEMData, TimeType * GPSTime, C8 * Latitude, - C8 * Longitude, C8 * Altitude, U8 debug); + C8 * Longitude, C8 * Altitude, U8 debug); static size_t uiRecvMonitor(int *sockfd, char *buffer, size_t length); static int iGetObjectIndexFromObjectIP(in_addr_t ipAddr, in_addr_t objectIPs[], unsigned int numberOfObjects); static void signalHandler(int signo); I32 ObjectControlBuildSTRTMessage(C8 * MessageBuffer, STRTType * STRTData, TimeType * GPSTime, - U32 ScenarioStartTime, U32 DelayStart, U32 * OutgoingStartTime, U8 debug); + U32 ScenarioStartTime, U32 DelayStart, U32 * OutgoingStartTime, U8 debug); I32 ObjectControlBuildOSTMMessage(C8 * MessageBuffer, OSTMType * OSTMData, C8 CommandOption, U8 debug); I32 ObjectControlBuildHEABMessage(C8 * MessageBuffer, HEABType * HEABData, TimeType * GPSTime, U8 CCStatus, - U8 debug); + U8 debug); int ObjectControlBuildLLCMMessage(char *MessageBuffer, unsigned short Speed, unsigned short Curvature, - unsigned char Mode, char debug); + unsigned char Mode, char debug); I32 ObjectControlBuildSYPMMessage(C8 * MessageBuffer, SYPMType * SYPMData, U32 SyncPoint, U32 StopTime, - U8 debug); + U8 debug); I32 ObjectControlBuildMTSPMessage(C8 * MessageBuffer, MTSPType * MTSPData, U32 SyncTimestamp, U8 debug); I32 ObjectControlBuildTRAJMessageHeader(C8 * MessageBuffer, I32 * RowCount, HeaderType * HeaderData, - TRAJInfoType * TRAJInfoData, C8 * TrajFileHeader, U8 debug); + TRAJInfoType * TRAJInfoData, C8 * TrajFileHeader, U8 debug); I32 ObjectControlBuildTRAJMessage(C8 * MessageBuffer, FILE * fd, I32 RowCount, DOTMType * DOTMData, U8 debug); I32 ObjectControlSendTRAJMessage(C8 * Filename, I32 * Socket, I32 RowCount, C8 * IP, U32 Port, - DOTMType * DOTMData, U8 debug); + DOTMType * DOTMData, U8 debug); int ObjectControlSendUDPData(int *sockfd, struct sockaddr_in *addr, char *SendData, int Length, char debug); I32 ObjectControlMONRToASCII(MONRType * MONRData, GeoPosition * OriginPosition, I32 Idn, C8 * Id, - C8 * Timestamp, C8 * XPosition, C8 * YPosition, C8 * ZPosition, - C8 * LongitudinalSpeed, C8 * LateralSpeed, C8 * LongitudinalAcc, C8 * LateralAcc, - C8 * Heading, C8 * DriveDirection, C8 * ObjectState, C8 * ReadyToArm, - C8 * ErrorStatus, C8 debug); + C8 * Timestamp, C8 * XPosition, C8 * YPosition, C8 * ZPosition, + C8 * LongitudinalSpeed, C8 * LateralSpeed, C8 * LongitudinalAcc, C8 * LateralAcc, + C8 * Heading, C8 * DriveDirection, C8 * ObjectState, C8 * ReadyToArm, + C8 * ErrorStatus, C8 debug); I32 ObjectControlBuildMONRMessage(C8 * MonrData, MONRType * MONRData, U8 debug); I32 ObjectControlBuildVOILMessage(C8 * MessageBuffer, VOILType * VOILData, C8 * SimData, U8 debug); I32 ObjectControlSendDTMMessage(C8 * DTMData, I32 * Socket, I32 RowCount, C8 * IP, U32 Port, - DOTMType * DOTMData, U8 debug); + DOTMType * DOTMData, U8 debug); I32 ObjectControlBuildDTMMessage(C8 * MessageBuffer, C8 * DTMData, I32 RowCount, DOTMType * DOTMData, - U8 debug); + U8 debug); I32 ObjectControlBuildASPMessage(C8 * MessageBuffer, ASPType * ASPData, U8 debug); I32 ObjectControlBuildACCMMessage(ACCMData * mqACCMData, ACCMType * ACCM, U8 debug); I32 ObjectControlBuildEXACMessage(EXACData * mqEXACData, EXACType * EXAC, U8 debug); @@ -212,8 +212,8 @@ I32 ObjectControlSendTRCMMessage(TRCMData * TRCM, I32 * socket, U8 debug); I32 ObjectControlSendEXACMessage(EXACData * EXAC, I32 * socket, U8 debug); static int iFindObjectsInfo(C8 object_traj_file[MAX_OBJECTS][MAX_FILE_PATH], - C8 object_address_name[MAX_OBJECTS][MAX_FILE_PATH], - in_addr_t objectIPs[MAX_OBJECTS], I32 * nbr_objects); + C8 object_address_name[MAX_OBJECTS][MAX_FILE_PATH], + in_addr_t objectIPs[MAX_OBJECTS], I32 * nbr_objects); OBCState_t vInitializeState(OBCState_t firstState, GSDType * GSD); inline OBCState_t vGetState(GSDType * GSD); @@ -239,856 +239,872 @@ static volatile int iExit = 0; ------------------------------------------------------------*/ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { - I32 safety_socket_fd[MAX_OBJECTS]; - struct sockaddr_in safety_object_addr[MAX_OBJECTS]; - I32 socket_fds[MAX_OBJECTS]; - I32 socket_fd = 0; - C8 object_traj_file[MAX_OBJECTS][MAX_FILE_PATH]; - C8 object_address_name[MAX_OBJECTS][MAX_FILE_PATH]; - in_addr_t objectIPs[MAX_OBJECTS]; - U32 object_udp_port[MAX_OBJECTS]; - U32 object_tcp_port[MAX_OBJECTS]; - I32 nbr_objects = 0; - enum COMMAND iCommand; - U8 pcRecvBuffer[RECV_MESSAGE_BUFFER]; - C8 pcTempBuffer[512]; - C8 MessageBuffer[BUFFER_SIZE_3100]; - I32 iIndex = 0, i = 0; - struct timespec sleep_time, ref_time; - - /*! Timers for reporting state over message bus */ - const struct timespec mqEmptyPollPeriod = { OC_SLEEP_TIME_EMPTY_MQ_S, OC_SLEEP_TIME_EMPTY_MQ_NS }; - const struct timespec mqNonEmptyPollPeriod = - { OC_SLEEP_TIME_NONEMPTY_MQ_S, OC_SLEEP_TIME_NONEMPTY_MQ_NS }; - const struct timeval stateReportPeriod = { OC_STATE_REPORT_PERIOD_S, OC_STATE_REPORT_PERIOD_US }; - struct timeval currentTime, nextStateReportTime; - U8 iForceObjectToLocalhostU8 = 0; - - FILE *fd; - C8 Id[SMALL_BUFFER_SIZE_0]; - C8 GPSWeek[SMALL_BUFFER_SIZE_0], Timestamp[SMALL_BUFFER_SIZE_0], XPosition[SMALL_BUFFER_SIZE_0], - YPosition[SMALL_BUFFER_SIZE_0], ZPosition[SMALL_BUFFER_SIZE_0]; - C8 Speed[SMALL_BUFFER_SIZE_0], LongitudinalSpeed[SMALL_BUFFER_SIZE_0], LateralSpeed[SMALL_BUFFER_SIZE_0], - LongitudinalAcc[SMALL_BUFFER_SIZE_0], LateralAcc[SMALL_BUFFER_SIZE_0]; - C8 Heading[SMALL_BUFFER_SIZE_0], DriveDirection[SMALL_BUFFER_SIZE_1], ObjectState[SMALL_BUFFER_SIZE_1], - ReadyToArm[SMALL_BUFFER_SIZE_1], MTSP[SMALL_BUFFER_SIZE_0], ErrorStatus[SMALL_BUFFER_SIZE_0]; - I32 MessageLength; - C8 *MiscPtr; - C8 MiscText[SMALL_BUFFER_SIZE_0]; - U32 StartTimeU32 = 0; - U32 OutgoingStartTimeU32 = 0; - U32 DelayedStartU32 = 0; - U32 CurrentTimeU32 = 0; - U32 OldTimeU32 = 0; - U64 TimeCap1, TimeCap2; - struct timeval CurrentTimeStruct; - I32 HeartbeatMessageCounter = 0; - - ObjectPosition OP[MAX_OBJECTS]; - flt SpaceArr[MAX_OBJECTS][TRAJECTORY_FILE_MAX_ROWS]; - flt TimeArr[MAX_OBJECTS][TRAJECTORY_FILE_MAX_ROWS]; - SpaceTime SpaceTimeArr[MAX_OBJECTS][TRAJECTORY_FILE_MAX_ROWS]; - C8 OriginLatitude[SMALL_BUFFER_SIZE_0], OriginLongitude[SMALL_BUFFER_SIZE_0], - OriginAltitude[SMALL_BUFFER_SIZE_0], OriginHeading[SMALL_BUFFER_SIZE_0]; - C8 TextBuffer[SMALL_BUFFER_SIZE_0]; - dbl OriginLatitudeDbl; - dbl OriginLongitudeDbl; - dbl OriginAltitudeDbl; - dbl OriginHeadingDbl; - C8 pcSendBuffer[MBUS_MAX_DATALEN]; - C8 ObjectPort[SMALL_BUFFER_SIZE_0]; - HeaderType HeaderData; - OSEMType OSEMData; - STRTType STRTData; - OSTMType OSTMData; - HEABType HEABData; - MONRType MONRData; - DOTMType DOTMData; - TRAJInfoType TRAJInfoData; - VOILType VOILData; - SYPMType SYPMData; - MTSPType MTSPData; - ACCMData mqACCMData; - EXACData mqEXACData; - GeoPosition OriginPosition; - ASPType ASPData; - - ASPData.MTSPU32 = 0; - ASPData.TimeToSyncPointDbl = 0; - ASPData.PrevTimeToSyncPointDbl = 0; - ASPData.CurrentTimeDbl = 0; - AdaptiveSyncPoint ASP[MAX_ADAPTIVE_SYNC_POINTS]; - I32 SyncPointCount = 0; - I32 SearchStartIndex = 0; - dbl ASPMaxTimeDiffDbl = 0; - dbl ASPMaxTrajDiffDbl = 0; - dbl ASPFilterLevelDbl = 0; - dbl ASPMaxDeltaTimeDbl = 0; - I32 ASPDebugRate = 1; - I32 ASPStepBackCount = 0; - char confDirectoryPath[MAX_FILE_PATH]; - - U8 ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_BOOTING; - - vInitializeState(OBC_STATE_IDLE, GSD); - U8 uiTimeCycle = 0; - I32 ObjectcontrolExecutionMode = OBJECT_CONTROL_CONTROL_MODE; - - C8 Buffer2[SMALL_BUFFER_SIZE_1]; - C8 LogBuffer[LOG_BUFFER_LENGTH]; - C8 VOILReceivers[SMALL_BUFFER_SIZE_254]; - C8 DTMReceivers[SMALL_BUFFER_SIZE_254]; - U32 RowCount; - U32 DTMIpU32; - U32 DTMLengthU32; - - U8 DisconnectU8 = 0; - I32 iResult; - - FILE *TempFd; - U16 MiscU16; - I32 j = 0; - - U8 STRTSentU8 = 0; - C8 FileHeaderBufferC8[TRAJ_FILE_HEADER_ROW]; - - - // Create log - LogInit(MODULE_NAME, logLevel); - LogMessage(LOG_LEVEL_INFO, "Object control task running with PID: %i", getpid()); - - - // Set up signal handlers - if (signal(SIGINT, signalHandler) == SIG_ERR) - util_error("Unable to initialize signal handler"); - - // Set up message bus connection - if (iCommInit()) - util_error("Unable to connect to message queue bus"); - - // Initialize timer for sending state - TimeSetToCurrentSystemTime(¤tTime); - nextStateReportTime = currentTime; - - // Initialize timer for sending state - TimeSetToCurrentSystemTime(¤tTime); - nextStateReportTime = currentTime; - - while (!iExit) { - - if (vGetState(GSD) == OBC_STATE_ERROR) { - ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_ABORT; - MessageLength = - ObjectControlBuildHEABMessage(MessageBuffer, &HEABData, GPSTime, ObjectControlServerStatus, - 0); - UtilSendUDPData("Object Control", &safety_socket_fd[iIndex], &safety_object_addr[iIndex], - MessageBuffer, MessageLength, 0); - } - - if (vGetState(GSD) == OBC_STATE_RUNNING || vGetState(GSD) == OBC_STATE_ARMED - || vGetState(GSD) == OBC_STATE_CONNECTED) { - /*HEAB*/ for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - if (uiTimeCycle == 0) { - //HeartbeatMessageCounter ++; - MessageLength = - ObjectControlBuildHEABMessage(MessageBuffer, &HEABData, GPSTime, - ObjectControlServerStatus, 0); - //ObjectControlSendUDPData(&safety_socket_fd[iIndex], &safety_object_addr[iIndex], MessageBuffer, MessageLength, 1); - UtilSendUDPData("Object Control", &safety_socket_fd[iIndex], &safety_object_addr[iIndex], - MessageBuffer, MessageLength, 0); - } - } - - - // Check if any object has disconnected - if so, disconnect all objects and return to idle - DisconnectU8 = 0; - for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - DisconnectU8 |= vCheckRemoteDisconnected(&socket_fds[iIndex]); - if (DisconnectU8) { - LogMessage(LOG_LEVEL_WARNING, "Lost connection to IP %s - returning to IDLE", - object_address_name[iIndex]); - - for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - vDisconnectObject(&socket_fds[iIndex]); - } - - /* Close safety socket */ - for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - vCloseSafetyChannel(&safety_socket_fd[iIndex]); - } - vSetState(OBC_STATE_IDLE, GSD); - break; - } - } - } - - if (vGetState(GSD) == OBC_STATE_RUNNING || vGetState(GSD) == OBC_STATE_CONNECTED - || vGetState(GSD) == OBC_STATE_ARMED) { - char buffer[RECV_MESSAGE_BUFFER]; - size_t receivedMONRData = 0; - - // this is etsi time lets remov it ans use utc instead - //gettimeofday(&CurrentTimeStruct, NULL); - - CurrentTimeU32 = - ((GPSTime->GPSSecondsOfWeekU32 * 1000 + (U32) TimeControlGetMillisecond(GPSTime)) << 2) + - GPSTime->MicroSecondU16; - - - /*MTSP*/ if (HeartbeatMessageCounter == 0) { - HeartbeatMessageCounter = 0; - for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - for (i = 0; i < SyncPointCount; i++) { - if (TEST_SYNC_POINTS == 0 - && strstr(object_address_name[iIndex], ASP[i].SlaveIP) != NULL - && ASPData.MTSPU32 > 0 && ASPData.TimeToSyncPointDbl > -1) { - /*Send Master time to adaptive sync point */ - MessageLength = - ObjectControlBuildMTSPMessage(MessageBuffer, &MTSPData, ASPData.MTSPU32, 0); - //ObjectControlSendUDPData(&safety_socket_fd[iIndex], &safety_object_addr[iIndex], MessageBuffer, MessageLength, 0); - UtilSendUDPData("Object Control", &safety_socket_fd[iIndex], - &safety_object_addr[iIndex], MessageBuffer, MessageLength, 0); - } - /*else if(TEST_SYNC_POINTS == 1 && iIndex == 1 && ASPData.MTSPU32 > 0 && ASPData.TimeToSyncPointDbl > -1 ) - { - Send Master time to adaptive sync point - MessageLength =ObjectControlBuildMTSPMessage(MessageBuffer, &MTSPData, (U32)ASPData.MTSPU32, 0); - ObjectControlSendUDPData(&safety_socket_fd[iIndex], &safety_object_addr[iIndex], MessageBuffer, MessageLength, 0); - } */ - } - } - } - - - for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - bzero(buffer, RECV_MESSAGE_BUFFER); - receivedMONRData = uiRecvMonitor(&safety_socket_fd[iIndex], buffer, RECV_MESSAGE_BUFFER); - - if (receivedMONRData == sizeof (MONRType)) { - LogMessage(LOG_LEVEL_DEBUG, "Recieved new data from %s %d %d: %s", - object_address_name[iIndex], object_udp_port[iIndex], receivedMONRData, - buffer); - - if (ObjectcontrolExecutionMode == OBJECT_CONTROL_CONTROL_MODE) { - // Append IP to buffer - memcpy(&buffer[receivedMONRData], &safety_object_addr[iIndex].sin_addr.s_addr, - sizeof (in_addr_t)); - // Send MONR message as bytes - - if (iCommSend(COMM_MONR, buffer, (size_t) (receivedMONRData) + sizeof (in_addr_t)) < - 0) { - LogMessage(LOG_LEVEL_ERROR, - "Fatal communication fault when sending MONR command - entering error state"); - vSetState(OBC_STATE_ERROR, GSD); - ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_ABORT; - } - } - - ObjectControlBuildMONRMessage(buffer, &MONRData, 0); - - ObjectControlMONRToASCII(&MONRData, &OriginPosition, iIndex, Id, Timestamp, XPosition, - YPosition, ZPosition, LongitudinalSpeed, LateralSpeed, - LongitudinalAcc, LateralAcc, Heading, DriveDirection, - ObjectState, ReadyToArm, ErrorStatus, 1); - bzero(buffer, OBJECT_MESS_BUFFER_SIZE); - strcat(buffer, object_address_name[iIndex]); - strcat(buffer, ";"); - strcat(buffer, "0"); - strcat(buffer, ";"); - strcat(buffer, Timestamp); - strcat(buffer, ";"); - strcat(buffer, XPosition); - strcat(buffer, ";"); - strcat(buffer, YPosition); - strcat(buffer, ";"); - strcat(buffer, ZPosition); - strcat(buffer, ";"); - strcat(buffer, Heading); - strcat(buffer, ";"); - strcat(buffer, LongitudinalSpeed); - strcat(buffer, ";"); - strcat(buffer, LateralSpeed); - strcat(buffer, ";"); - strcat(buffer, LongitudinalAcc); - strcat(buffer, ";"); - strcat(buffer, LateralAcc); - strcat(buffer, ";"); - strcat(buffer, DriveDirection); - strcat(buffer, ";"); - strcat(buffer, ObjectState); - strcat(buffer, ";"); - strcat(buffer, ReadyToArm); - strcat(buffer, ";"); - strcat(buffer, ErrorStatus); - strcat(buffer, ";"); - - - if (ASPData.MTSPU32 != 0) { - //Add MTSP to MONR if not 0 - bzero(MTSP, SMALL_BUFFER_SIZE_0); - sprintf(MTSP, "%" PRIu32, ASPData.MTSPU32); - strcat(buffer, MTSP); - strcat(buffer, ";"); - } - - //Ok, let's do the ASP - for (i = 0; i < SyncPointCount; i++) { - if (TEST_SYNC_POINTS == 0 - && strstr(object_address_name[iIndex], ASP[i].MasterIP) != NULL - && CurrentTimeU32 > StartTimeU32 && StartTimeU32 > 0 - && ASPData.TimeToSyncPointDbl > -1 - /*|| TEST_SYNC_POINTS == 1 && ASP[0].TestPort == object_udp_port[iIndex] && StartTimeU32 > 0 && iIndex == 0 && TimeToSyncPoint > -1 */ - ) { - // Use the util.c function for time here but it soent mather - gettimeofday(&CurrentTimeStruct, NULL); //Capture time - - TimeCap1 = (uint64_t) CurrentTimeStruct.tv_sec * 1000 + (uint64_t) CurrentTimeStruct.tv_usec / 1000; //Calculate initial timestamp - - OP[iIndex].x = ((dbl) atoi(XPosition)) / 1000; //Set x and y on OP (ObjectPosition) - OP[iIndex].y = ((dbl) atoi(YPosition)) / 1000; - - //OP[iIndex].OrigoDistance = sqrt(pow(OP[iIndex].x,2) + pow(OP[iIndex].y,2)); //Calculate hypotenuse - - UtilCalcPositionDelta(OriginLatitudeDbl, OriginLongitudeDbl, - atof(XPosition) / 1e7, atof(YPosition) / 1e7, &OP[iIndex]); - - if (OP[iIndex].BestFoundTrajectoryIndex <= OP[iIndex].SyncIndex) { - ASPData.CurrentTimeU32 = CurrentTimeU32; - ASPData.CurrentTimeDbl = - (((double)CurrentTimeU32 - (double)StartTimeU32) / 1000); - SearchStartIndex = OP[iIndex].BestFoundTrajectoryIndex - ASPStepBackCount; - UtilFindCurrentTrajectoryPosition(&OP[iIndex], SearchStartIndex, - ASPData.CurrentTimeDbl, ASPMaxTrajDiffDbl, - ASPMaxTimeDiffDbl, 0); - ASPData.BestFoundIndexI32 = OP[iIndex].BestFoundTrajectoryIndex; - - if (OP[iIndex].BestFoundTrajectoryIndex != TRAJ_POSITION_NOT_FOUND) { - ASPData.TimeToSyncPointDbl = UtilCalculateTimeToSync(&OP[iIndex]); - if (ASPData.TimeToSyncPointDbl > 0) { - if (ASPData.PrevTimeToSyncPointDbl != 0 && ASPFilterLevelDbl > 0) { - if (ASPData.TimeToSyncPointDbl / ASPData.PrevTimeToSyncPointDbl > - (1 + ASPFilterLevelDbl / 100)) - ASPData.TimeToSyncPointDbl = ASPData.PrevTimeToSyncPointDbl + ASPMaxDeltaTimeDbl; //TimeToSyncPoint*ASPFilterLevelDbl/500; - else if (ASPData.TimeToSyncPointDbl / - ASPData.PrevTimeToSyncPointDbl < - (1 - ASPFilterLevelDbl / 100)) - ASPData.TimeToSyncPointDbl = ASPData.PrevTimeToSyncPointDbl - ASPMaxDeltaTimeDbl; //TimeToSyncPoint*ASPFilterLevelDbl/500; - } - ASPData.MTSPU32 = - CurrentTimeU32 + (U32) (ASPData.TimeToSyncPointDbl * 4000); - - ASPData.PrevTimeToSyncPointDbl = ASPData.TimeToSyncPointDbl; - OldTimeU32 = CurrentTimeU32; - } - else { - CurrentTimeU32 = 0; - ASPData.TimeToSyncPointDbl = -1; - } - - } - - gettimeofday(&CurrentTimeStruct, NULL); - TimeCap2 = - (uint64_t) CurrentTimeStruct.tv_sec * 1000 + - (uint64_t) CurrentTimeStruct.tv_usec / 1000; - - ASPData.SyncPointIndexI32 = OP[iIndex].SyncIndex; - ASPData.IterationTimeU16 = (U16) (TimeCap2 - TimeCap1); - //Build ASP debug data and set to GSD - //bzero(buffer,OBJECT_MESS_BUFFER_SIZE); - //ObjectControlBuildASPMessage(buffer, &ASPData, 0); - DataDictionarySetRVSSAsp(GSD, &ASPData); - - if (atoi(Timestamp) % ASPDebugRate == 0) { - printf("%d, %d, %3.3f, %s, %s\n", CurrentTimeU32, StartTimeU32, - ASPData.TimeToSyncPointDbl, object_address_name[iIndex], - ASP[i].MasterIP); - printf - ("TtS=%3.3f, BestIndex=%d, MTSP=%d, iIndex=%d, IterationTime=%3.0f ms\n", - ASPData.TimeToSyncPointDbl, OP[iIndex].BestFoundTrajectoryIndex, - ASPData.MTSPU32, iIndex, ((double)(TimeCap2) - (double)TimeCap1)); - printf("CurrentTime=%3.3f, x=%3.3f mm, y=%3.3f\n\n", - ASPData.CurrentTimeDbl, OP[iIndex].x, OP[iIndex].y); - - //Build and send ASP on message queue - //(void)iCommSend(COMM_ASP,buffer); - } - } - } - } - OP[iIndex].Speed = atof(Speed); - } - else if (receivedMONRData > 0) - LogMessage(LOG_LEVEL_INFO, "MONR length error (should be %d but is %ld) from %s.", - sizeof (MONRType), object_address_name[iIndex]); - } - } - - - bzero(pcRecvBuffer, RECV_MESSAGE_BUFFER); - //Have we recieved a command? - if (iCommRecv(&iCommand, pcRecvBuffer, RECV_MESSAGE_BUFFER, NULL)) { - LogMessage(LOG_LEVEL_INFO, "Received command %d", iCommand); - - - if (iCommand == COMM_ARM && vGetState(GSD) == OBC_STATE_CONNECTED) { - - LogMessage(LOG_LEVEL_INFO, "Sending ARM"); - LOG_SEND(LogBuffer, "[ObjectControl] Sending ARM"); - vSetState(OBC_STATE_ARMED, GSD); - MessageLength = - ObjectControlBuildOSTMMessage(MessageBuffer, &OSTMData, COMMAND_OSTM_OPT_SET_ARMED_STATE, - 0); - for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - /*Send OSTM message */ - UtilSendTCPData("[Object Control]", MessageBuffer, MessageLength, &socket_fds[iIndex], 0); - } - - ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_OK; //Set server to READY - } - else if (iCommand == COMM_DISARM && vGetState(GSD) == OBC_STATE_ARMED) { - - LogMessage(LOG_LEVEL_INFO, "Sending DISARM"); - LOG_SEND(LogBuffer, "[ObjectControl] Sending DISARM"); - vSetState(OBC_STATE_CONNECTED, GSD); - - MessageLength = - ObjectControlBuildOSTMMessage(MessageBuffer, &OSTMData, - COMMAND_OSTM_OPT_SET_DISARMED_STATE, 0); - for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - /*Send OSTM message */ - UtilSendTCPData("[" MODULE_NAME "]", MessageBuffer, MessageLength, &socket_fds[iIndex], - 0); - } - - ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_OK; //Set server to READY - } - else if (iCommand == COMM_STRT && (vGetState(GSD) == OBC_STATE_ARMED) /*|| OBC_STATE_INITIALIZED) */ ) //OBC_STATE_INITIALIZED is temporary! - { - bzero(Timestamp, SMALL_BUFFER_SIZE_0); - MiscPtr = strchr(pcRecvBuffer, ';'); - strncpy(Timestamp, MiscPtr + 1, (uint64_t) strchr(MiscPtr + 1, ';') - (uint64_t) MiscPtr - 1); - StartTimeU32 = atol(Timestamp); - bzero(Timestamp, SMALL_BUFFER_SIZE_0); - MiscPtr += 1; - MiscPtr = strchr(pcRecvBuffer, ';'); - strncpy(Timestamp, MiscPtr + 1, (uint64_t) strchr(MiscPtr + 1, ';') - (uint64_t) MiscPtr - 1); - DelayedStartU32 = atoi(Timestamp); - ASPData.MTSPU32 = 0; - ASPData.TimeToSyncPointDbl = 0; - SearchStartIndex = -1; - ASPData.PrevTimeToSyncPointDbl = 0; - OldTimeU32 = CurrentTimeU32; - ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_OK; //Set server to READY - MessageLength = - ObjectControlBuildSTRTMessage(MessageBuffer, &STRTData, GPSTime, (U32) StartTimeU32, - DelayedStartU32, &OutgoingStartTimeU32, 0); - for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - UtilSendTCPData("Object Control", MessageBuffer, MessageLength, &socket_fds[iIndex], 0); - } - vSetState(OBC_STATE_RUNNING, GSD); - - //Store STRT in GSD - if (STRTSentU8 == 0) { - //for(i = 0; i < MessageLength; i++) GSD->STRTData[i] = MessageBuffer[i]; - //GSD->STRTSizeU8 = (U8)MessageLength; - STRTSentU8 = 1; - } - //OBCState = OBC_STATE_INITIALIZED; //This is temporary! - //printf("OutgoingStartTimeU32 = %d\n", OutgoingStartTimeU32); - GSD->ScenarioStartTimeU32 = OutgoingStartTimeU32; - bzero(MiscText, SMALL_BUFFER_SIZE_0); - sprintf(MiscText, "%" PRIu32, GSD->ScenarioStartTimeU32 << 2); - LOG_SEND(LogBuffer, "[ObjectControl] START received <%s>, GPS time <%s>", pcRecvBuffer, - MiscText); - } - else if (iCommand == COMM_REPLAY) { - ObjectcontrolExecutionMode = OBJECT_CONTROL_REPLAY_MODE; - LogMessage(LOG_LEVEL_INFO, "Entering REPLAY mode <%s>", pcRecvBuffer); - } - else if (iCommand == COMM_ABORT && vGetState(GSD) == OBC_STATE_RUNNING) { - vSetState(OBC_STATE_CONNECTED, GSD); - ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_ABORT; //Set server to ABORT - LogMessage(LOG_LEVEL_WARNING, "ABORT received"); - LOG_SEND(LogBuffer, "[ObjectControl] ABORT received."); - } - else if (iCommand == COMM_CONTROL) { - ObjectcontrolExecutionMode = OBJECT_CONTROL_CONTROL_MODE; - printf("[ObjectControl] Object control in CONTROL mode\n"); - } - else if (iCommand == COMM_INIT) { - LogMessage(LOG_LEVEL_INFO, "INIT received"); - LOG_SEND(LogBuffer, "[ObjectControl] INIT received."); - nbr_objects = 0; - if (iFindObjectsInfo(object_traj_file, object_address_name, objectIPs, &nbr_objects) == 0) { - // Get objects; name and drive file - DataDictionaryGetForceToLocalhostU8(GSD, &iForceObjectToLocalhostU8); - - for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - if (0 == iForceObjectToLocalhostU8) { - object_udp_port[iIndex] = SAFETY_CHANNEL_PORT; - object_tcp_port[iIndex] = CONTROL_CHANNEL_PORT; - } - else { - object_udp_port[iIndex] = SAFETY_CHANNEL_PORT + iIndex * 2; - object_tcp_port[iIndex] = CONTROL_CHANNEL_PORT + iIndex * 2; - } - } - - /*Setup Adaptive Sync Points (ASP) */ - UtilGetConfDirectoryPath(confDirectoryPath, sizeof (confDirectoryPath)); - strcat(confDirectoryPath, ADAPTIVE_SYNC_FILE_NAME); - fd = fopen(confDirectoryPath, "r"); - if (fd) { - SyncPointCount = UtilCountFileRows(fd) - 1; - fclose(fd); - fd = fopen(confDirectoryPath, "r"); - UtilReadLineCntSpecChars(fd, pcTempBuffer); //Read header - - for (i = 0; i < SyncPointCount; i++) { - UtilSetAdaptiveSyncPoint(&ASP[i], fd, 0); - if (TEST_SYNC_POINTS == 1) - ASP[i].TestPort = SAFETY_CHANNEL_PORT; - } - fclose(fd); - } - - vSetState(OBC_STATE_INITIALIZED, GSD); - LogMessage(LOG_LEVEL_INFO, "ObjectControl is initialized"); - LOG_SEND(LogBuffer, "[ObjectControl] ObjectControl is initialized."); - - //Remove temporary file - remove(TEMP_LOG_FILE); - if (USE_TEMP_LOGFILE) { - //Create temporary file - TempFd = fopen(TEMP_LOG_FILE, "w+"); - } - - //OSEMSentU8 = 0; - STRTSentU8 = 0; - } - else { - LogMessage(LOG_LEVEL_INFO, - "Could not initialize: object info was not processed successfully"); - pcSendBuffer[0] = (uint8_t) iCommand; - iCommSend(COMM_FAILURE, pcSendBuffer, sizeof (iCommand)); - } - } - else if (iCommand == COMM_ACCM && vGetState(GSD) == OBC_STATE_CONNECTED) { - UtilPopulateACCMDataStructFromMQ(pcRecvBuffer, sizeof (pcRecvBuffer), &mqACCMData); - iIndex = - iGetObjectIndexFromObjectIP(mqACCMData.ip, objectIPs, - sizeof (objectIPs) / sizeof (objectIPs[0])); - if (iIndex != -1) { - ObjectControlSendACCMMessage(&mqACCMData, &(socket_fds[iIndex]), 0); - } - else - LogMessage(LOG_LEVEL_WARNING, "Unable to send ACCM: no valid socket found"); - } - else if (iCommand == COMM_EXAC && vGetState(GSD) == OBC_STATE_RUNNING) { - UtilPopulateEXACDataStructFromMQ(pcRecvBuffer, sizeof (pcRecvBuffer), &mqEXACData); - iIndex = - iGetObjectIndexFromObjectIP(mqEXACData.ip, objectIPs, - sizeof (objectIPs) / sizeof (objectIPs[0])); - if (iIndex != -1) - ObjectControlSendEXACMessage(&mqEXACData, &(socket_fds[iIndex]), 0); - else - LogMessage(LOG_LEVEL_WARNING, "Unable to send EXAC: no valid socket found"); - } - else if (iCommand == COMM_CONNECT && vGetState(GSD) == OBC_STATE_INITIALIZED) { - LogMessage(LOG_LEVEL_INFO, "CONNECT received"); - LOG_SEND(LogBuffer, "[ObjectControl] CONNECT received."); - - /* Connect and send drive files */ - for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - - UtilSetObjectPositionIP(&OP[iIndex], object_address_name[iIndex]); - - MessageLength = ObjectControlBuildOSEMMessage(MessageBuffer, &OSEMData, GPSTime, - OriginLatitude, OriginLongitude, - OriginAltitude, 0); - - DisconnectU8 = 0; - - do { - - iResult = - vConnectObject(&socket_fds[iIndex], object_address_name[iIndex], - object_tcp_port[iIndex], &DisconnectU8); - - if (iResult < 0) { - switch (errno) { - case ECONNREFUSED: - LogMessage(LOG_LEVEL_INFO, - "Unable to connect to object %s:%d, retry in %d sec...", - object_address_name[iIndex], object_tcp_port[iIndex], - (!(1 & DisconnectU8)) * 3); - LOG_SEND(LogBuffer, - "[ObjectControl] Was not able to connect to object, [IP: %s] [PORT: %d], retry in %d sec...", - object_address_name[iIndex], object_tcp_port[iIndex], - (!(1 & DisconnectU8)) * 3); - (void)sleep(3); // TODO: Move this to the rest of the sleep operations? Also, remove the hardcoded 3 - break; - case EADDRINUSE: - util_error("[ObjectControl] Local address/port already in use"); - break; - case EALREADY: - util_error("[ObjectControl] Previous connection attempt still in progress"); - break; - case EISCONN: - util_error("[ObjectControl] Socket is already connected"); - break; - case ENETUNREACH: - util_error("[ObjectControl] Network unreachable"); - break; - case ETIMEDOUT: - util_error("[ObjectControl] Connection timed out"); - break; - default: - util_error("ERR: Failed to connect to control socket"); - break; - } - - } - - bzero(pcRecvBuffer, RECV_MESSAGE_BUFFER); - //Have we received a command? - if (iCommRecv(&iCommand, pcRecvBuffer, RECV_MESSAGE_BUFFER, NULL)) { - if (iCommand == COMM_DISCONNECT) { - DisconnectU8 = 1; - LOG_SEND(LogBuffer, "[ObjectControl] DISCONNECT received."); - } - - } - - } while (iResult < 0 && DisconnectU8 == 0); - - if (iResult >= 0) { - /* Send OSEM command in mq so that we get some information like GPSweek, origin (latitude,logitude,altitude in gps coordinates) */ - LogMessage(LOG_LEVEL_INFO, "Sending OSEM"); - LOG_SEND(LogBuffer, "[ObjectControl] Sending OSEM."); - - ObjectControlOSEMtoASCII(&OSEMData, GPSWeek, OriginLatitude, OriginLongitude, - OriginAltitude); - bzero(pcSendBuffer, sizeof (pcSendBuffer)); - strcat(pcSendBuffer, GPSWeek); - strcat(pcSendBuffer, ";"); - strcat(pcSendBuffer, OriginLatitude); - strcat(pcSendBuffer, ";"); - strcat(pcSendBuffer, OriginLongitude); - strcat(pcSendBuffer, ";"); - strcat(pcSendBuffer, OriginAltitude); - - //Restore the buffers - DataDictionaryGetOriginLatitudeC8(GSD, OriginLatitude, SMALL_BUFFER_SIZE_0); - DataDictionaryGetOriginLongitudeC8(GSD, OriginLongitude, SMALL_BUFFER_SIZE_0); - DataDictionaryGetOriginAltitudeC8(GSD, OriginAltitude, SMALL_BUFFER_SIZE_0); - - if (iCommSend(COMM_OSEM, pcSendBuffer, strlen(pcSendBuffer) + 1) < 0) { - LogMessage(LOG_LEVEL_ERROR, - "Fatal communication fault when sending OSEM command - entering error state"); - vSetState(OBC_STATE_ERROR, GSD); - ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_ABORT; - } - UtilSendTCPData("Object Control", MessageBuffer, MessageLength, &socket_fds[iIndex], - 0); - - /*Here we send TRAJ, if the IP-address not is found */ - if (strstr(DTMReceivers, object_address_name[iIndex]) == NULL) { - - fd = fopen(object_traj_file[iIndex], "r"); - - if (fd != NULL) { - //RowCount = UtilCountFileRows(fd); - //printf("RowCount: %d\n", RowCount); - //fclose(fd); - - //fd = fopen(object_traj_file[iIndex], "r"); - //printf("Open file: %s\n", object_traj_file[iIndex]); - UtilReadLineCntSpecChars(fd, FileHeaderBufferC8); - fclose(fd); - - /*TRAJ*/ - MessageLength = ObjectControlBuildTRAJMessageHeader(TrajBuffer, - &RowCount, - &HeaderData, - &TRAJInfoData, - FileHeaderBufferC8, - 0); - - //printf("RowCount: %d\n", RowCount); - - /*Send TRAJ header */ - UtilSendTCPData("Object Control", TrajBuffer, MessageLength, - &socket_fds[iIndex], 0); - - /*Send TRAJ data */ - ObjectControlSendTRAJMessage(object_traj_file[iIndex], &socket_fds[iIndex], - RowCount, - (char *)&object_address_name[iIndex], - object_tcp_port[iIndex], &DOTMData, 0); - - } - else - LogMessage(LOG_LEVEL_WARNING, "Could not open file <%s>", - object_traj_file[iIndex]); - } - - - /* Adaptive Sync Points object configuration start... */ - if (TEST_SYNC_POINTS == 1) - printf("Trajfile: %s\n", object_traj_file[iIndex]); - OP[iIndex].TrajectoryPositionCount = RowCount; - OP[iIndex].SpaceArr = SpaceArr[iIndex]; - OP[iIndex].TimeArr = TimeArr[iIndex]; - OP[iIndex].SpaceTimeArr = SpaceTimeArr[iIndex]; - UtilPopulateSpaceTimeArr(&OP[iIndex], object_traj_file[iIndex]); - - LogMessage(LOG_LEVEL_INFO, "Sync point counts: %d", SyncPointCount); - for (i = 0; i < SyncPointCount; i++) { - if (TEST_SYNC_POINTS == 1 && iIndex == 1) { - /*Send SYPM to slave */ - MessageLength = - ObjectControlBuildSYPMMessage(MessageBuffer, &SYPMData, - ASP[i].SlaveTrajSyncTime * 1000, - ASP[i].SlaveSyncStopTime * 1000, 1); - UtilSendTCPData("Object Control", MessageBuffer, MessageLength, - &socket_fds[iIndex], 0); - } - else if (TEST_SYNC_POINTS == 0 - && strstr(object_address_name[iIndex], ASP[i].SlaveIP) != NULL) { - /*Send SYPM to slave */ - MessageLength = - ObjectControlBuildSYPMMessage(MessageBuffer, &SYPMData, - ASP[i].SlaveTrajSyncTime * 1000, - ASP[i].SlaveSyncStopTime * 1000, 1); - UtilSendTCPData("Object Control", MessageBuffer, MessageLength, - &socket_fds[iIndex], 0); - } - } - - /*Set Sync point in OP */ - for (i = 0; i < SyncPointCount; i++) { - if (TEST_SYNC_POINTS == 1 && iIndex == 0) - UtilSetSyncPoint(&OP[iIndex], 0, 0, 0, ASP[i].MasterTrajSyncTime); - else if (TEST_SYNC_POINTS == 0 - && strstr(object_address_name[iIndex], ASP[i].MasterIP) != NULL) - UtilSetSyncPoint(&OP[iIndex], 0, 0, 0, ASP[i].MasterTrajSyncTime); - } - /* ...end */ - } - - } - for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - if (USE_TEST_HOST == 0) - vCreateSafetyChannel(object_address_name[iIndex], object_udp_port[iIndex], - &safety_socket_fd[iIndex], &safety_object_addr[iIndex]); - else if (USE_TEST_HOST == 1) - vCreateSafetyChannel(TESTSERVER_IP, object_udp_port[iIndex], - &safety_socket_fd[iIndex], &safety_object_addr[iIndex]); - } - - uiTimeCycle = 0; - - /* Execution mode */ - ObjectcontrolExecutionMode = OBJECT_CONTROL_CONTROL_MODE; - - /*Set server status */ - ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_OK; //Set server to READY - - if (DisconnectU8 == 0) { - vSetState(OBC_STATE_CONNECTED, GSD); - iCommSend(COMM_OBJECTS_CONNECTED, NULL, 0); - } - else if (DisconnectU8 == 1) - vSetState(OBC_STATE_IDLE, GSD); - } - else if (iCommand == COMM_DATA_DICT) { - - LogMessage(LOG_LEVEL_INFO, "Updating variables from DataDictionary."); - DataDictionaryGetOriginLatitudeC8(GSD, OriginLatitude, SMALL_BUFFER_SIZE_0); - DataDictionaryGetOriginLongitudeC8(GSD, OriginLongitude, SMALL_BUFFER_SIZE_0); - DataDictionaryGetOriginAltitudeC8(GSD, OriginAltitude, SMALL_BUFFER_SIZE_0); - - DataDictionaryGetOriginLatitudeDbl(GSD, &OriginLatitudeDbl); - DataDictionaryGetOriginLongitudeDbl(GSD, &OriginLongitudeDbl); - DataDictionaryGetOriginAltitudeDbl(GSD, &OriginAltitudeDbl); - - OriginLatitudeDbl = atof(OriginLatitude); - OriginLongitudeDbl = atof(OriginLongitude); - OriginAltitudeDbl = atof(OriginAltitude); - OriginHeadingDbl = atof(OriginHeading); - OriginPosition.Latitude = OriginLatitudeDbl; - OriginPosition.Longitude = OriginLongitudeDbl; - OriginPosition.Altitude = OriginAltitudeDbl; - OriginPosition.Heading = OriginHeadingDbl; - - DataDictionaryGetForceToLocalhostU8(GSD, &iForceObjectToLocalhostU8); - LogMessage(LOG_LEVEL_INFO, "ForceObjectToLocalhost = %d", iForceObjectToLocalhostU8); - LOG_SEND(LogBuffer, "[ObjectControl] ForceObjectToLocalhost = %d", iForceObjectToLocalhostU8); - - DataDictionaryGetASPMaxTimeDiffDbl(GSD, &ASPMaxTimeDiffDbl); - DataDictionaryGetASPMaxTrajDiffDbl(GSD, &ASPMaxTrajDiffDbl); - DataDictionaryGetASPStepBackCountU32(GSD, &ASPStepBackCount); - DataDictionaryGetASPFilterLevelDbl(GSD, &ASPFilterLevelDbl); - DataDictionaryGetASPMaxDeltaTimeDbl(GSD, &ASPMaxDeltaTimeDbl); - ASPDebugRate = 1; - DataDictionaryGetVOILReceiversC8(GSD, VOILReceivers, SMALL_BUFFER_SIZE_254); - DataDictionaryGetDTMReceiversC8(GSD, DTMReceivers, SMALL_BUFFER_SIZE_254); - } - else if (iCommand == COMM_DISCONNECT) { - //#ifndef NOTCP - for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - vDisconnectObject(&socket_fds[iIndex]); - } - //#endif //NOTCP - - LogMessage(LOG_LEVEL_INFO, "DISCONNECT received"); - LOG_SEND(LogBuffer, "[ObjectControl] DISCONNECT received."); - /* Close safety socket */ - for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - vCloseSafetyChannel(&safety_socket_fd[iIndex]); - } - vSetState(OBC_STATE_IDLE, GSD); - } - else if (iCommand == COMM_EXIT) { - iExit = 1; - iCommClose(); - } - else { - LogMessage(LOG_LEVEL_WARNING, "Unhandled command in object control: %d", iCommand); - } - } - - if (!iExit) { - /* Make call periodic */ - sleep_time = iCommand == COMM_INV ? mqEmptyPollPeriod : mqNonEmptyPollPeriod; - - ++uiTimeCycle; - if (uiTimeCycle >= HEARTBEAT_TIME_MS / TASK_PERIOD_MS) { - uiTimeCycle = 0; - } - - // Periodically send state to signal aliveness - TimeSetToCurrentSystemTime(¤tTime); - if (timercmp(¤tTime, &nextStateReportTime, >)) { - timeradd(&nextStateReportTime, &stateReportPeriod, &nextStateReportTime); - - bzero(Buffer2, sizeof (Buffer2)); - Buffer2[0] = (uint8_t) (DataDictionaryGetOBCStateU8(GSD)); - if (iCommSend(COMM_OBC_STATE, Buffer2, sizeof (Buffer2)) < 0) { - LogMessage(LOG_LEVEL_ERROR, - "Fatal communication fault when sending OBC_STATE command - entering error state"); - vSetState(OBC_STATE_ERROR, GSD); - ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_ABORT; - } - } - - (void)nanosleep(&sleep_time, &ref_time); - } - } - - LogMessage(LOG_LEVEL_INFO, "Object control exiting"); + I32 safety_socket_fd[MAX_OBJECTS]; + struct sockaddr_in safety_object_addr[MAX_OBJECTS]; + I32 socket_fds[MAX_OBJECTS]; + I32 socket_fd = 0; + C8 object_traj_file[MAX_OBJECTS][MAX_FILE_PATH]; + C8 object_address_name[MAX_OBJECTS][MAX_FILE_PATH]; + in_addr_t objectIPs[MAX_OBJECTS]; + U32 object_udp_port[MAX_OBJECTS]; + U32 object_tcp_port[MAX_OBJECTS]; + I32 nbr_objects = 0; + enum COMMAND iCommand; + U8 pcRecvBuffer[RECV_MESSAGE_BUFFER]; + C8 pcTempBuffer[512]; + C8 MessageBuffer[BUFFER_SIZE_3100]; + I32 iIndex = 0, i = 0; + struct timespec sleep_time, ref_time; + + /*! Timers for reporting state over message bus */ + const struct timespec mqEmptyPollPeriod = { OC_SLEEP_TIME_EMPTY_MQ_S, OC_SLEEP_TIME_EMPTY_MQ_NS }; + const struct timespec mqNonEmptyPollPeriod = + { OC_SLEEP_TIME_NONEMPTY_MQ_S, OC_SLEEP_TIME_NONEMPTY_MQ_NS }; + const struct timeval stateReportPeriod = { OC_STATE_REPORT_PERIOD_S, OC_STATE_REPORT_PERIOD_US }; + struct timeval currentTime, nextStateReportTime; + U8 iForceObjectToLocalhostU8 = 0; + + FILE *fd; + C8 Id[SMALL_BUFFER_SIZE_0]; + C8 GPSWeek[SMALL_BUFFER_SIZE_0], Timestamp[SMALL_BUFFER_SIZE_0], XPosition[SMALL_BUFFER_SIZE_0], + YPosition[SMALL_BUFFER_SIZE_0], ZPosition[SMALL_BUFFER_SIZE_0]; + C8 Speed[SMALL_BUFFER_SIZE_0], LongitudinalSpeed[SMALL_BUFFER_SIZE_0], LateralSpeed[SMALL_BUFFER_SIZE_0], + LongitudinalAcc[SMALL_BUFFER_SIZE_0], LateralAcc[SMALL_BUFFER_SIZE_0]; + C8 Heading[SMALL_BUFFER_SIZE_0], DriveDirection[SMALL_BUFFER_SIZE_1], ObjectState[SMALL_BUFFER_SIZE_1], + ReadyToArm[SMALL_BUFFER_SIZE_1], MTSP[SMALL_BUFFER_SIZE_0], ErrorStatus[SMALL_BUFFER_SIZE_0]; + I32 MessageLength; + C8 *MiscPtr; + C8 MiscText[SMALL_BUFFER_SIZE_0]; + U32 StartTimeU32 = 0; + U32 OutgoingStartTimeU32 = 0; + U32 DelayedStartU32 = 0; + U32 CurrentTimeU32 = 0; + U32 OldTimeU32 = 0; + U64 TimeCap1, TimeCap2; + struct timeval CurrentTimeStruct; + I32 HeartbeatMessageCounter = 0; + + ObjectPosition OP[MAX_OBJECTS]; + flt SpaceArr[MAX_OBJECTS][TRAJECTORY_FILE_MAX_ROWS]; + flt TimeArr[MAX_OBJECTS][TRAJECTORY_FILE_MAX_ROWS]; + SpaceTime SpaceTimeArr[MAX_OBJECTS][TRAJECTORY_FILE_MAX_ROWS]; + C8 OriginLatitude[SMALL_BUFFER_SIZE_0], OriginLongitude[SMALL_BUFFER_SIZE_0], + OriginAltitude[SMALL_BUFFER_SIZE_0], OriginHeading[SMALL_BUFFER_SIZE_0]; + C8 TextBuffer[SMALL_BUFFER_SIZE_0]; + dbl OriginLatitudeDbl; + dbl OriginLongitudeDbl; + dbl OriginAltitudeDbl; + dbl OriginHeadingDbl; + C8 pcSendBuffer[MBUS_MAX_DATALEN]; + C8 ObjectPort[SMALL_BUFFER_SIZE_0]; + HeaderType HeaderData; + OSEMType OSEMData; + STRTType STRTData; + OSTMType OSTMData; + HEABType HEABData; + MONRType MONRData; + DOTMType DOTMData; + TRAJInfoType TRAJInfoData; + VOILType VOILData; + SYPMType SYPMData; + MTSPType MTSPData; + ACCMData mqACCMData; + EXACData mqEXACData; + GeoPosition OriginPosition; + ASPType ASPData; + + ASPData.MTSPU32 = 0; + ASPData.TimeToSyncPointDbl = 0; + ASPData.PrevTimeToSyncPointDbl = 0; + ASPData.CurrentTimeDbl = 0; + AdaptiveSyncPoint ASP[MAX_ADAPTIVE_SYNC_POINTS]; + I32 SyncPointCount = 0; + I32 SearchStartIndex = 0; + dbl ASPMaxTimeDiffDbl = 0; + dbl ASPMaxTrajDiffDbl = 0; + dbl ASPFilterLevelDbl = 0; + dbl ASPMaxDeltaTimeDbl = 0; + I32 ASPDebugRate = 1; + I32 ASPStepBackCount = 0; + char confDirectoryPath[MAX_FILE_PATH]; + + U8 ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_BOOTING; + + vInitializeState(OBC_STATE_IDLE, GSD); + U8 uiTimeCycle = 0; + I32 ObjectcontrolExecutionMode = OBJECT_CONTROL_CONTROL_MODE; + + C8 Buffer2[SMALL_BUFFER_SIZE_1]; + C8 LogBuffer[LOG_BUFFER_LENGTH]; + C8 VOILReceivers[SMALL_BUFFER_SIZE_254]; + C8 DTMReceivers[SMALL_BUFFER_SIZE_254]; + U32 RowCount; + U32 DTMIpU32; + U32 DTMLengthU32; + + U8 DisconnectU8 = 0; + I32 iResult; + + FILE *TempFd; + U16 MiscU16; + I32 j = 0; + + U8 STRTSentU8 = 0; + C8 FileHeaderBufferC8[TRAJ_FILE_HEADER_ROW]; + + + // Create log + LogInit(MODULE_NAME, logLevel); + LogMessage(LOG_LEVEL_INFO, "Object control task running with PID: %i", getpid()); + + + // Set up signal handlers + if (signal(SIGINT, signalHandler) == SIG_ERR) + util_error("Unable to initialize signal handler"); + + // Set up message bus connection + if (iCommInit()) + util_error("Unable to connect to message queue bus"); + + // Initialize timer for sending state + TimeSetToCurrentSystemTime(¤tTime); + nextStateReportTime = currentTime; + + // Initialize timer for sending state + TimeSetToCurrentSystemTime(¤tTime); + nextStateReportTime = currentTime; + + while (!iExit) { + + if (vGetState(GSD) == OBC_STATE_ERROR) { + ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_ABORT; + MessageLength = + ObjectControlBuildHEABMessage(MessageBuffer, &HEABData, GPSTime, ObjectControlServerStatus, + 0); + UtilSendUDPData("Object Control", &safety_socket_fd[iIndex], &safety_object_addr[iIndex], + MessageBuffer, MessageLength, 0); + } + + if (vGetState(GSD) == OBC_STATE_RUNNING || vGetState(GSD) == OBC_STATE_ARMED + || vGetState(GSD) == OBC_STATE_CONNECTED) { + /*HEAB*/ for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { + if (uiTimeCycle == 0) { + //HeartbeatMessageCounter ++; + MessageLength = + ObjectControlBuildHEABMessage(MessageBuffer, &HEABData, GPSTime, + ObjectControlServerStatus, 0); + //ObjectControlSendUDPData(&safety_socket_fd[iIndex], &safety_object_addr[iIndex], MessageBuffer, MessageLength, 1); + UtilSendUDPData("Object Control", &safety_socket_fd[iIndex], &safety_object_addr[iIndex], + MessageBuffer, MessageLength, 0); + } + } + + + // Check if any object has disconnected - if so, disconnect all objects and return to idle + DisconnectU8 = 0; + for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { + DisconnectU8 |= vCheckRemoteDisconnected(&socket_fds[iIndex]); + if (DisconnectU8) { + LogMessage(LOG_LEVEL_WARNING, "Lost connection to IP %s - returning to IDLE", + object_address_name[iIndex]); + + for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { + vDisconnectObject(&socket_fds[iIndex]); + } + + /* Close safety socket */ + for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { + vCloseSafetyChannel(&safety_socket_fd[iIndex]); + } + vSetState(OBC_STATE_IDLE, GSD); + break; + } + } + } + + if (vGetState(GSD) == OBC_STATE_RUNNING || vGetState(GSD) == OBC_STATE_CONNECTED + || vGetState(GSD) == OBC_STATE_ARMED) { + char buffer[RECV_MESSAGE_BUFFER]; + size_t receivedMONRData = 0; + + // this is etsi time lets remov it ans use utc instead + //gettimeofday(&CurrentTimeStruct, NULL); + + CurrentTimeU32 = + ((GPSTime->GPSSecondsOfWeekU32 * 1000 + (U32) TimeControlGetMillisecond(GPSTime)) << 2) + + GPSTime->MicroSecondU16; + + + /*MTSP*/ if (HeartbeatMessageCounter == 0) { + HeartbeatMessageCounter = 0; + for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { + for (i = 0; i < SyncPointCount; i++) { + if (TEST_SYNC_POINTS == 0 + && strstr(object_address_name[iIndex], ASP[i].SlaveIP) != NULL + && ASPData.MTSPU32 > 0 && ASPData.TimeToSyncPointDbl > -1) { + /*Send Master time to adaptive sync point */ + MessageLength = + ObjectControlBuildMTSPMessage(MessageBuffer, &MTSPData, ASPData.MTSPU32, 0); + //ObjectControlSendUDPData(&safety_socket_fd[iIndex], &safety_object_addr[iIndex], MessageBuffer, MessageLength, 0); + UtilSendUDPData("Object Control", &safety_socket_fd[iIndex], + &safety_object_addr[iIndex], MessageBuffer, MessageLength, 0); + } + /*else if(TEST_SYNC_POINTS == 1 && iIndex == 1 && ASPData.MTSPU32 > 0 && ASPData.TimeToSyncPointDbl > -1 ) + { + Send Master time to adaptive sync point + MessageLength =ObjectControlBuildMTSPMessage(MessageBuffer, &MTSPData, (U32)ASPData.MTSPU32, 0); + ObjectControlSendUDPData(&safety_socket_fd[iIndex], &safety_object_addr[iIndex], MessageBuffer, MessageLength, 0); + } */ + } + } + } + + + for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { + bzero(buffer, RECV_MESSAGE_BUFFER); + receivedMONRData = uiRecvMonitor(&safety_socket_fd[iIndex], buffer, RECV_MESSAGE_BUFFER); + + if (receivedMONRData == sizeof (MONRType)) { + LogMessage(LOG_LEVEL_DEBUG, "Recieved new data from %s %d %d: %s", + object_address_name[iIndex], object_udp_port[iIndex], receivedMONRData, + buffer); + + if (ObjectcontrolExecutionMode == OBJECT_CONTROL_CONTROL_MODE) { + // Append IP to buffer + memcpy(&buffer[receivedMONRData], &safety_object_addr[iIndex].sin_addr.s_addr, + sizeof (in_addr_t)); + // Send MONR message as bytes + + if (iCommSend(COMM_MONR, buffer, (size_t) (receivedMONRData) + sizeof (in_addr_t)) < + 0) { + LogMessage(LOG_LEVEL_ERROR, + "Fatal communication fault when sending MONR command - entering error state"); + vSetState(OBC_STATE_ERROR, GSD); + ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_ABORT; + } + } + + ObjectControlBuildMONRMessage(buffer, &MONRData, 0); + + //Store MONR in GSD + //UtilSendUDPData("ObjectControl", &ObjectControlUDPSocketfdI32, &simulator_addr, &MONRData, sizeof(MONRData), 0); + /*for (i = 0; + i < + (MONRData.Header.MessageLengthU32 + COMMAND_MESSAGE_HEADER_LENGTH + + COMMAND_MESSAGE_FOOTER_LENGTH); i++) + GSD->MONRData[i] = buffer[i]; + GSD->MONRSizeU8 = + MONRData.Header.MessageLengthU32 + COMMAND_MESSAGE_HEADER_LENGTH + + COMMAND_MESSAGE_FOOTER_LENGTH; + */ + + DataDictionarySetMONR(GSD, &MONRData, 0); + + + ObjectControlMONRToASCII(&MONRData, &OriginPosition, iIndex, Id, Timestamp, XPosition, + YPosition, ZPosition, LongitudinalSpeed, LateralSpeed, + LongitudinalAcc, LateralAcc, Heading, DriveDirection, + ObjectState, ReadyToArm, ErrorStatus, 1); + bzero(buffer, OBJECT_MESS_BUFFER_SIZE); + strcat(buffer, object_address_name[iIndex]); + strcat(buffer, ";"); + strcat(buffer, "0"); + strcat(buffer, ";"); + strcat(buffer, Timestamp); + strcat(buffer, ";"); + strcat(buffer, XPosition); + strcat(buffer, ";"); + strcat(buffer, YPosition); + strcat(buffer, ";"); + strcat(buffer, ZPosition); + strcat(buffer, ";"); + strcat(buffer, Heading); + strcat(buffer, ";"); + strcat(buffer, LongitudinalSpeed); + strcat(buffer, ";"); + strcat(buffer, LateralSpeed); + strcat(buffer, ";"); + strcat(buffer, LongitudinalAcc); + strcat(buffer, ";"); + strcat(buffer, LateralAcc); + strcat(buffer, ";"); + strcat(buffer, DriveDirection); + strcat(buffer, ";"); + strcat(buffer, ObjectState); + strcat(buffer, ";"); + strcat(buffer, ReadyToArm); + strcat(buffer, ";"); + strcat(buffer, ErrorStatus); + strcat(buffer, ";"); + + + if (ASPData.MTSPU32 != 0) { + //Add MTSP to MONR if not 0 + bzero(MTSP, SMALL_BUFFER_SIZE_0); + sprintf(MTSP, "%" PRIu32, ASPData.MTSPU32); + strcat(buffer, MTSP); + strcat(buffer, ";"); + } + + //Ok, let's do the ASP + for (i = 0; i < SyncPointCount; i++) { + if (TEST_SYNC_POINTS == 0 + && strstr(object_address_name[iIndex], ASP[i].MasterIP) != NULL + && CurrentTimeU32 > StartTimeU32 && StartTimeU32 > 0 + && ASPData.TimeToSyncPointDbl > -1 + /*|| TEST_SYNC_POINTS == 1 && ASP[0].TestPort == object_udp_port[iIndex] && StartTimeU32 > 0 && iIndex == 0 && TimeToSyncPoint > -1 */ + ) { + // Use the util.c function for time here but it soent mather + gettimeofday(&CurrentTimeStruct, NULL); //Capture time + + TimeCap1 = (uint64_t) CurrentTimeStruct.tv_sec * 1000 + (uint64_t) CurrentTimeStruct.tv_usec / 1000; //Calculate initial timestamp + + OP[iIndex].x = ((dbl) atoi(XPosition)) / 1000; //Set x and y on OP (ObjectPosition) + OP[iIndex].y = ((dbl) atoi(YPosition)) / 1000; + + //OP[iIndex].OrigoDistance = sqrt(pow(OP[iIndex].x,2) + pow(OP[iIndex].y,2)); //Calculate hypotenuse + + UtilCalcPositionDelta(OriginLatitudeDbl, OriginLongitudeDbl, + atof(XPosition) / 1e7, atof(YPosition) / 1e7, &OP[iIndex]); + + if (OP[iIndex].BestFoundTrajectoryIndex <= OP[iIndex].SyncIndex) { + ASPData.CurrentTimeU32 = CurrentTimeU32; + ASPData.CurrentTimeDbl = + (((double)CurrentTimeU32 - (double)StartTimeU32) / 1000); + SearchStartIndex = OP[iIndex].BestFoundTrajectoryIndex - ASPStepBackCount; + UtilFindCurrentTrajectoryPosition(&OP[iIndex], SearchStartIndex, + ASPData.CurrentTimeDbl, ASPMaxTrajDiffDbl, + ASPMaxTimeDiffDbl, 0); + ASPData.BestFoundIndexI32 = OP[iIndex].BestFoundTrajectoryIndex; + + if (OP[iIndex].BestFoundTrajectoryIndex != TRAJ_POSITION_NOT_FOUND) { + ASPData.TimeToSyncPointDbl = UtilCalculateTimeToSync(&OP[iIndex]); + if (ASPData.TimeToSyncPointDbl > 0) { + if (ASPData.PrevTimeToSyncPointDbl != 0 && ASPFilterLevelDbl > 0) { + if (ASPData.TimeToSyncPointDbl / ASPData.PrevTimeToSyncPointDbl > + (1 + ASPFilterLevelDbl / 100)) + ASPData.TimeToSyncPointDbl = ASPData.PrevTimeToSyncPointDbl + ASPMaxDeltaTimeDbl; //TimeToSyncPoint*ASPFilterLevelDbl/500; + else if (ASPData.TimeToSyncPointDbl / + ASPData.PrevTimeToSyncPointDbl < + (1 - ASPFilterLevelDbl / 100)) + ASPData.TimeToSyncPointDbl = ASPData.PrevTimeToSyncPointDbl - ASPMaxDeltaTimeDbl; //TimeToSyncPoint*ASPFilterLevelDbl/500; + } + ASPData.MTSPU32 = + CurrentTimeU32 + (U32) (ASPData.TimeToSyncPointDbl * 4000); + + ASPData.PrevTimeToSyncPointDbl = ASPData.TimeToSyncPointDbl; + OldTimeU32 = CurrentTimeU32; + } + else { + CurrentTimeU32 = 0; + ASPData.TimeToSyncPointDbl = -1; + } + + } + + gettimeofday(&CurrentTimeStruct, NULL); + TimeCap2 = + (uint64_t) CurrentTimeStruct.tv_sec * 1000 + + (uint64_t) CurrentTimeStruct.tv_usec / 1000; + + ASPData.SyncPointIndexI32 = OP[iIndex].SyncIndex; + ASPData.IterationTimeU16 = (U16) (TimeCap2 - TimeCap1); + //Build ASP debug data and set to GSD + //bzero(buffer,OBJECT_MESS_BUFFER_SIZE); + //ObjectControlBuildASPMessage(buffer, &ASPData, 0); + DataDictionarySetRVSSAsp(GSD, &ASPData); + + if (atoi(Timestamp) % ASPDebugRate == 0) { + printf("%d, %d, %3.3f, %s, %s\n", CurrentTimeU32, StartTimeU32, + ASPData.TimeToSyncPointDbl, object_address_name[iIndex], + ASP[i].MasterIP); + printf + ("TtS=%3.3f, BestIndex=%d, MTSP=%d, iIndex=%d, IterationTime=%3.0f ms\n", + ASPData.TimeToSyncPointDbl, OP[iIndex].BestFoundTrajectoryIndex, + ASPData.MTSPU32, iIndex, ((double)(TimeCap2) - (double)TimeCap1)); + printf("CurrentTime=%3.3f, x=%3.3f mm, y=%3.3f\n\n", + ASPData.CurrentTimeDbl, OP[iIndex].x, OP[iIndex].y); + + //Build and send ASP on message queue + //(void)iCommSend(COMM_ASP,buffer); + } + } + } + } + OP[iIndex].Speed = atof(Speed); + } + else if (receivedMONRData > 0) + LogMessage(LOG_LEVEL_INFO, "MONR length error (should be %d but is %ld) from %s.", + sizeof (MONRType), object_address_name[iIndex]); + } + } + + + bzero(pcRecvBuffer, RECV_MESSAGE_BUFFER); + //Have we recieved a command? + if (iCommRecv(&iCommand, pcRecvBuffer, RECV_MESSAGE_BUFFER, NULL)) { + LogMessage(LOG_LEVEL_INFO, "Received command %d", iCommand); + + + if (iCommand == COMM_ARM && vGetState(GSD) == OBC_STATE_CONNECTED) { + + LogMessage(LOG_LEVEL_INFO, "Sending ARM"); + LOG_SEND(LogBuffer, "[ObjectControl] Sending ARM"); + vSetState(OBC_STATE_ARMED, GSD); + MessageLength = + ObjectControlBuildOSTMMessage(MessageBuffer, &OSTMData, COMMAND_OSTM_OPT_SET_ARMED_STATE, + 0); + for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { + /*Send OSTM message */ + UtilSendTCPData("[Object Control]", MessageBuffer, MessageLength, &socket_fds[iIndex], 0); + } + + ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_OK; //Set server to READY + } + else if (iCommand == COMM_DISARM && vGetState(GSD) == OBC_STATE_ARMED) { + + LogMessage(LOG_LEVEL_INFO, "Sending DISARM"); + LOG_SEND(LogBuffer, "[ObjectControl] Sending DISARM"); + vSetState(OBC_STATE_CONNECTED, GSD); + + MessageLength = + ObjectControlBuildOSTMMessage(MessageBuffer, &OSTMData, + COMMAND_OSTM_OPT_SET_DISARMED_STATE, 0); + for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { + /*Send OSTM message */ + UtilSendTCPData("[" MODULE_NAME "]", MessageBuffer, MessageLength, &socket_fds[iIndex], + 0); + } + + ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_OK; //Set server to READY + } + else if (iCommand == COMM_STRT && (vGetState(GSD) == OBC_STATE_ARMED) /*|| OBC_STATE_INITIALIZED) */ ) //OBC_STATE_INITIALIZED is temporary! + { + bzero(Timestamp, SMALL_BUFFER_SIZE_0); + MiscPtr = strchr(pcRecvBuffer, ';'); + strncpy(Timestamp, MiscPtr + 1, (uint64_t) strchr(MiscPtr + 1, ';') - (uint64_t) MiscPtr - 1); + StartTimeU32 = atol(Timestamp); + bzero(Timestamp, SMALL_BUFFER_SIZE_0); + MiscPtr += 1; + MiscPtr = strchr(pcRecvBuffer, ';'); + strncpy(Timestamp, MiscPtr + 1, (uint64_t) strchr(MiscPtr + 1, ';') - (uint64_t) MiscPtr - 1); + DelayedStartU32 = atoi(Timestamp); + ASPData.MTSPU32 = 0; + ASPData.TimeToSyncPointDbl = 0; + SearchStartIndex = -1; + ASPData.PrevTimeToSyncPointDbl = 0; + OldTimeU32 = CurrentTimeU32; + ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_OK; //Set server to READY + MessageLength = + ObjectControlBuildSTRTMessage(MessageBuffer, &STRTData, GPSTime, (U32) StartTimeU32, + DelayedStartU32, &OutgoingStartTimeU32, 0); + for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { + UtilSendTCPData("Object Control", MessageBuffer, MessageLength, &socket_fds[iIndex], 0); + } + vSetState(OBC_STATE_RUNNING, GSD); + + //Store STRT in GSD + if (STRTSentU8 == 0) { + //for(i = 0; i < MessageLength; i++) GSD->STRTData[i] = MessageBuffer[i]; + //GSD->STRTSizeU8 = (U8)MessageLength; + STRTSentU8 = 1; + } + //OBCState = OBC_STATE_INITIALIZED; //This is temporary! + //printf("OutgoingStartTimeU32 = %d\n", OutgoingStartTimeU32); + GSD->ScenarioStartTimeU32 = OutgoingStartTimeU32; + bzero(MiscText, SMALL_BUFFER_SIZE_0); + sprintf(MiscText, "%" PRIu32, GSD->ScenarioStartTimeU32 << 2); + LOG_SEND(LogBuffer, "[ObjectControl] START received <%s>, GPS time <%s>", pcRecvBuffer, + MiscText); + } + else if (iCommand == COMM_REPLAY) { + ObjectcontrolExecutionMode = OBJECT_CONTROL_REPLAY_MODE; + LogMessage(LOG_LEVEL_INFO, "Entering REPLAY mode <%s>", pcRecvBuffer); + } + else if (iCommand == COMM_ABORT && vGetState(GSD) == OBC_STATE_RUNNING) { + vSetState(OBC_STATE_CONNECTED, GSD); + ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_ABORT; //Set server to ABORT + LogMessage(LOG_LEVEL_WARNING, "ABORT received"); + LOG_SEND(LogBuffer, "[ObjectControl] ABORT received."); + } + else if (iCommand == COMM_CONTROL) { + ObjectcontrolExecutionMode = OBJECT_CONTROL_CONTROL_MODE; + printf("[ObjectControl] Object control in CONTROL mode\n"); + } + else if (iCommand == COMM_INIT) { + LogMessage(LOG_LEVEL_INFO, "INIT received"); + + LOG_SEND(LogBuffer, "[ObjectControl] INIT received."); + nbr_objects = 0; + if (iFindObjectsInfo(object_traj_file, object_address_name, objectIPs, &nbr_objects) == 0) { + // Get objects; name and drive file + DataDictionaryGetForceToLocalhostU8(GSD, &iForceObjectToLocalhostU8); + + for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { + if (0 == iForceObjectToLocalhostU8) { + object_udp_port[iIndex] = SAFETY_CHANNEL_PORT; + object_tcp_port[iIndex] = CONTROL_CHANNEL_PORT; + } + else { + object_udp_port[iIndex] = SAFETY_CHANNEL_PORT + iIndex * 2; + object_tcp_port[iIndex] = CONTROL_CHANNEL_PORT + iIndex * 2; + } + } + + /*Setup Adaptive Sync Points (ASP) */ + UtilGetConfDirectoryPath(confDirectoryPath, sizeof (confDirectoryPath)); + strcat(confDirectoryPath, ADAPTIVE_SYNC_FILE_NAME); + fd = fopen(confDirectoryPath, "r"); + if (fd) { + SyncPointCount = UtilCountFileRows(fd) - 1; + fclose(fd); + fd = fopen(confDirectoryPath, "r"); + UtilReadLineCntSpecChars(fd, pcTempBuffer); //Read header + + for (i = 0; i < SyncPointCount; i++) { + UtilSetAdaptiveSyncPoint(&ASP[i], fd, 0); + if (TEST_SYNC_POINTS == 1) + ASP[i].TestPort = SAFETY_CHANNEL_PORT; + } + fclose(fd); + } + + vSetState(OBC_STATE_INITIALIZED, GSD); + LogMessage(LOG_LEVEL_INFO, "ObjectControl is initialized"); + LOG_SEND(LogBuffer, "[ObjectControl] ObjectControl is initialized."); + + //Remove temporary file + remove(TEMP_LOG_FILE); + if (USE_TEMP_LOGFILE) { + //Create temporary file + TempFd = fopen(TEMP_LOG_FILE, "w+"); + } + + //OSEMSentU8 = 0; + STRTSentU8 = 0; + } + else { + LogMessage(LOG_LEVEL_INFO, + "Could not initialize: object info was not processed successfully"); + pcSendBuffer[0] = (uint8_t) iCommand; + iCommSend(COMM_FAILURE, pcSendBuffer, sizeof (iCommand)); + } + } + else if (iCommand == COMM_ACCM && vGetState(GSD) == OBC_STATE_CONNECTED) { + UtilPopulateACCMDataStructFromMQ(pcRecvBuffer, sizeof (pcRecvBuffer), &mqACCMData); + iIndex = + iGetObjectIndexFromObjectIP(mqACCMData.ip, objectIPs, + sizeof (objectIPs) / sizeof (objectIPs[0])); + if (iIndex != -1) { + ObjectControlSendACCMMessage(&mqACCMData, &(socket_fds[iIndex]), 0); + } + else + LogMessage(LOG_LEVEL_WARNING, "Unable to send ACCM: no valid socket found"); + } + else if (iCommand == COMM_EXAC && vGetState(GSD) == OBC_STATE_RUNNING) { + UtilPopulateEXACDataStructFromMQ(pcRecvBuffer, sizeof (pcRecvBuffer), &mqEXACData); + iIndex = + iGetObjectIndexFromObjectIP(mqEXACData.ip, objectIPs, + sizeof (objectIPs) / sizeof (objectIPs[0])); + if (iIndex != -1) + ObjectControlSendEXACMessage(&mqEXACData, &(socket_fds[iIndex]), 0); + else + LogMessage(LOG_LEVEL_WARNING, "Unable to send EXAC: no valid socket found"); + } + else if (iCommand == COMM_CONNECT && vGetState(GSD) == OBC_STATE_INITIALIZED) { + LogMessage(LOG_LEVEL_INFO, "CONNECT received"); + LOG_SEND(LogBuffer, "[ObjectControl] CONNECT received."); + + /* Connect and send drive files */ + for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { + + UtilSetObjectPositionIP(&OP[iIndex], object_address_name[iIndex]); + + MessageLength = ObjectControlBuildOSEMMessage(MessageBuffer, &OSEMData, GPSTime, + OriginLatitude, OriginLongitude, + OriginAltitude, 0); + + DisconnectU8 = 0; + + do { + + iResult = + vConnectObject(&socket_fds[iIndex], object_address_name[iIndex], + object_tcp_port[iIndex], &DisconnectU8); + + if (iResult < 0) { + switch (errno) { + case ECONNREFUSED: + LogMessage(LOG_LEVEL_INFO, + "Unable to connect to object %s:%d, retry in %d sec...", + object_address_name[iIndex], object_tcp_port[iIndex], + (!(1 & DisconnectU8)) * 3); + LOG_SEND(LogBuffer, + "[ObjectControl] Was not able to connect to object, [IP: %s] [PORT: %d], retry in %d sec...", + object_address_name[iIndex], object_tcp_port[iIndex], + (!(1 & DisconnectU8)) * 3); + (void)sleep(3); // TODO: Move this to the rest of the sleep operations? Also, remove the hardcoded 3 + break; + case EADDRINUSE: + LogMessage(LOG_LEVEL_ERROR, "[ObjectControl] Local address/port already in use"); + break; + case EALREADY: + LogMessage(LOG_LEVEL_ERROR, "[ObjectControl] Previous connection attempt still in progress"); + break; + case EISCONN: + LogMessage(LOG_LEVEL_ERROR, "[ObjectControl] Socket is already connected"); + break; + case ENETUNREACH: + LogMessage(LOG_LEVEL_ERROR, "[ObjectControl] Network unreachable"); + break; + case ETIMEDOUT: + LogMessage(LOG_LEVEL_ERROR, "[ObjectControl] Connection timed out"); + break; + default: + LogMessage(LOG_LEVEL_ERROR, "ERR: Failed to connect to control socket"); + break; + } + + } + + bzero(pcRecvBuffer, RECV_MESSAGE_BUFFER); + //Have we received a command? + if (iCommRecv(&iCommand, pcRecvBuffer, RECV_MESSAGE_BUFFER, NULL)) { + if (iCommand == COMM_DISCONNECT) { + DisconnectU8 = 1; + LOG_SEND(LogBuffer, "[ObjectControl] DISCONNECT received."); + } + + } + + } while (iResult < 0 && DisconnectU8 == 0); + + if (iResult >= 0) { + /* Send OSEM command in mq so that we get some information like GPSweek, origin (latitude,logitude,altitude in gps coordinates) */ + LogMessage(LOG_LEVEL_INFO, "Sending OSEM"); + LOG_SEND(LogBuffer, "[ObjectControl] Sending OSEM."); + + ObjectControlOSEMtoASCII(&OSEMData, GPSWeek, OriginLatitude, OriginLongitude, + OriginAltitude); + bzero(pcSendBuffer, sizeof (pcSendBuffer)); + strcat(pcSendBuffer, GPSWeek); + strcat(pcSendBuffer, ";"); + strcat(pcSendBuffer, OriginLatitude); + strcat(pcSendBuffer, ";"); + strcat(pcSendBuffer, OriginLongitude); + strcat(pcSendBuffer, ";"); + strcat(pcSendBuffer, OriginAltitude); + + //Restore the buffers + DataDictionaryGetOriginLatitudeC8(GSD, OriginLatitude, SMALL_BUFFER_SIZE_0); + DataDictionaryGetOriginLongitudeC8(GSD, OriginLongitude, SMALL_BUFFER_SIZE_0); + DataDictionaryGetOriginAltitudeC8(GSD, OriginAltitude, SMALL_BUFFER_SIZE_0); + + if (iCommSend(COMM_OSEM, pcSendBuffer, strlen(pcSendBuffer) + 1) < 0) { + LogMessage(LOG_LEVEL_ERROR, + "Fatal communication fault when sending OSEM command - entering error state"); + vSetState(OBC_STATE_ERROR, GSD); + ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_ABORT; + } + UtilSendTCPData("Object Control", MessageBuffer, MessageLength, &socket_fds[iIndex], + 0); + + /*Here we send TRAJ, if the IP-address not is found */ + if (strstr(DTMReceivers, object_address_name[iIndex]) == NULL) { + + fd = fopen(object_traj_file[iIndex], "r"); + + if (fd != NULL) { + //RowCount = UtilCountFileRows(fd); + //printf("RowCount: %d\n", RowCount); + //fclose(fd); + + //fd = fopen(object_traj_file[iIndex], "r"); + //printf("Open file: %s\n", object_traj_file[iIndex]); + UtilReadLineCntSpecChars(fd, FileHeaderBufferC8); + fclose(fd); + + /*TRAJ*/ + MessageLength = ObjectControlBuildTRAJMessageHeader(TrajBuffer, + &RowCount, + &HeaderData, + &TRAJInfoData, + FileHeaderBufferC8, + 0); + + //printf("RowCount: %d\n", RowCount); + + /*Send TRAJ header */ + UtilSendTCPData("Object Control", TrajBuffer, MessageLength, + &socket_fds[iIndex], 0); + + /*Send TRAJ data */ + ObjectControlSendTRAJMessage(object_traj_file[iIndex], &socket_fds[iIndex], + RowCount, + (char *)&object_address_name[iIndex], + object_tcp_port[iIndex], &DOTMData, 0); + + } + else + LogMessage(LOG_LEVEL_WARNING, "Could not open file <%s>", + object_traj_file[iIndex]); + } + + + /* Adaptive Sync Points object configuration start... */ + if (TEST_SYNC_POINTS == 1) + printf("Trajfile: %s\n", object_traj_file[iIndex]); + OP[iIndex].TrajectoryPositionCount = RowCount; + OP[iIndex].SpaceArr = SpaceArr[iIndex]; + OP[iIndex].TimeArr = TimeArr[iIndex]; + OP[iIndex].SpaceTimeArr = SpaceTimeArr[iIndex]; + UtilPopulateSpaceTimeArr(&OP[iIndex], object_traj_file[iIndex]); + + LogMessage(LOG_LEVEL_INFO, "Sync point counts: %d", SyncPointCount); + for (i = 0; i < SyncPointCount; i++) { + if (TEST_SYNC_POINTS == 1 && iIndex == 1) { + /*Send SYPM to slave */ + MessageLength = + ObjectControlBuildSYPMMessage(MessageBuffer, &SYPMData, + ASP[i].SlaveTrajSyncTime * 1000, + ASP[i].SlaveSyncStopTime * 1000, 1); + UtilSendTCPData("Object Control", MessageBuffer, MessageLength, + &socket_fds[iIndex], 0); + } + else if (TEST_SYNC_POINTS == 0 + && strstr(object_address_name[iIndex], ASP[i].SlaveIP) != NULL) { + /*Send SYPM to slave */ + MessageLength = + ObjectControlBuildSYPMMessage(MessageBuffer, &SYPMData, + ASP[i].SlaveTrajSyncTime * 1000, + ASP[i].SlaveSyncStopTime * 1000, 1); + UtilSendTCPData("Object Control", MessageBuffer, MessageLength, + &socket_fds[iIndex], 0); + } + } + + /*Set Sync point in OP */ + for (i = 0; i < SyncPointCount; i++) { + if (TEST_SYNC_POINTS == 1 && iIndex == 0) + UtilSetSyncPoint(&OP[iIndex], 0, 0, 0, ASP[i].MasterTrajSyncTime); + else if (TEST_SYNC_POINTS == 0 + && strstr(object_address_name[iIndex], ASP[i].MasterIP) != NULL) + UtilSetSyncPoint(&OP[iIndex], 0, 0, 0, ASP[i].MasterTrajSyncTime); + } + /* ...end */ + } + + } + for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { + if (USE_TEST_HOST == 0) + vCreateSafetyChannel(object_address_name[iIndex], object_udp_port[iIndex], + &safety_socket_fd[iIndex], &safety_object_addr[iIndex]); + else if (USE_TEST_HOST == 1) + vCreateSafetyChannel(TESTSERVER_IP, object_udp_port[iIndex], + &safety_socket_fd[iIndex], &safety_object_addr[iIndex]); + } + + uiTimeCycle = 0; + + /* Execution mode */ + ObjectcontrolExecutionMode = OBJECT_CONTROL_CONTROL_MODE; + + /*Set server status */ + ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_OK; //Set server to READY + + if (DisconnectU8 == 0) { + vSetState(OBC_STATE_CONNECTED, GSD); + iCommSend(COMM_OBJECTS_CONNECTED, NULL, 0); + } + else if (DisconnectU8 == 1) + vSetState(OBC_STATE_IDLE, GSD); + } + else if (iCommand == COMM_DATA_DICT) { + + LogMessage(LOG_LEVEL_INFO, "Updating variables from DataDictionary."); + DataDictionaryGetOriginLatitudeC8(GSD, OriginLatitude, SMALL_BUFFER_SIZE_0); + DataDictionaryGetOriginLongitudeC8(GSD, OriginLongitude, SMALL_BUFFER_SIZE_0); + DataDictionaryGetOriginAltitudeC8(GSD, OriginAltitude, SMALL_BUFFER_SIZE_0); + + DataDictionaryGetOriginLatitudeDbl(GSD, &OriginLatitudeDbl); + DataDictionaryGetOriginLongitudeDbl(GSD, &OriginLongitudeDbl); + DataDictionaryGetOriginAltitudeDbl(GSD, &OriginAltitudeDbl); + + OriginLatitudeDbl = atof(OriginLatitude); + OriginLongitudeDbl = atof(OriginLongitude); + OriginAltitudeDbl = atof(OriginAltitude); + OriginHeadingDbl = atof(OriginHeading); + OriginPosition.Latitude = OriginLatitudeDbl; + OriginPosition.Longitude = OriginLongitudeDbl; + OriginPosition.Altitude = OriginAltitudeDbl; + OriginPosition.Heading = OriginHeadingDbl; + + DataDictionaryGetForceToLocalhostU8(GSD, &iForceObjectToLocalhostU8); + LogMessage(LOG_LEVEL_INFO, "ForceObjectToLocalhost = %d", iForceObjectToLocalhostU8); + LOG_SEND(LogBuffer, "[ObjectControl] ForceObjectToLocalhost = %d", iForceObjectToLocalhostU8); + + DataDictionaryGetASPMaxTimeDiffDbl(GSD, &ASPMaxTimeDiffDbl); + DataDictionaryGetASPMaxTrajDiffDbl(GSD, &ASPMaxTrajDiffDbl); + DataDictionaryGetASPStepBackCountU32(GSD, &ASPStepBackCount); + DataDictionaryGetASPFilterLevelDbl(GSD, &ASPFilterLevelDbl); + DataDictionaryGetASPMaxDeltaTimeDbl(GSD, &ASPMaxDeltaTimeDbl); + ASPDebugRate = 1; + DataDictionaryGetVOILReceiversC8(GSD, VOILReceivers, SMALL_BUFFER_SIZE_254); + DataDictionaryGetDTMReceiversC8(GSD, DTMReceivers, SMALL_BUFFER_SIZE_254); + } + else if (iCommand == COMM_DISCONNECT) { + //#ifndef NOTCP + for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { + vDisconnectObject(&socket_fds[iIndex]); + } + //#endif //NOTCP + + LogMessage(LOG_LEVEL_INFO, "DISCONNECT received"); + LOG_SEND(LogBuffer, "[ObjectControl] DISCONNECT received."); + /* Close safety socket */ + for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { + vCloseSafetyChannel(&safety_socket_fd[iIndex]); + } + vSetState(OBC_STATE_IDLE, GSD); + } + else if (iCommand == COMM_EXIT) { + iExit = 1; + iCommClose(); + } + else { + LogMessage(LOG_LEVEL_WARNING, "Unhandled command in object control: %d", iCommand); + } + } + + if (!iExit) { + /* Make call periodic */ + sleep_time = iCommand == COMM_INV ? mqEmptyPollPeriod : mqNonEmptyPollPeriod; + + ++uiTimeCycle; + if (uiTimeCycle >= HEARTBEAT_TIME_MS / TASK_PERIOD_MS) { + uiTimeCycle = 0; + } + + // Periodically send state to signal aliveness + TimeSetToCurrentSystemTime(¤tTime); + if (timercmp(¤tTime, &nextStateReportTime, >)) { + timeradd(&nextStateReportTime, &stateReportPeriod, &nextStateReportTime); + + bzero(Buffer2, sizeof (Buffer2)); + Buffer2[0] = (uint8_t) (DataDictionaryGetOBCStateU8(GSD)); + if (iCommSend(COMM_OBC_STATE, Buffer2, sizeof (Buffer2)) < 0) { + LogMessage(LOG_LEVEL_ERROR, + "Fatal communication fault when sending OBC_STATE command - entering error state"); + vSetState(OBC_STATE_ERROR, GSD); + ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_ABORT; + } + } + + (void)nanosleep(&sleep_time, &ref_time); + } + } + + LogMessage(LOG_LEVEL_INFO, "Object control exiting"); } @@ -1097,1162 +1113,1162 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { ------------------------------------------------------------*/ void signalHandler(int signo) { - if (signo == SIGINT) { - LogMessage(LOG_LEVEL_WARNING, "Caught keyboard interrupt"); - iExit = 1; - } - else { - LogMessage(LOG_LEVEL_ERROR, "Caught unhandled signal"); - } + if (signo == SIGINT) { + LogMessage(LOG_LEVEL_WARNING, "Caught keyboard interrupt"); + iExit = 1; + } + else { + LogMessage(LOG_LEVEL_ERROR, "Caught unhandled signal"); + } } I32 ObjectControlBuildVOILMessage(C8 * MessageBuffer, VOILType * VOILData, C8 * SimData, U8 debug) { - I32 MessageIndex = 0, i; - U16 Crc = 0; - C8 *p; - U16 U16Data = 0; - I16 I16Data = 0; - U32 U32Data = 0; - I32 I32Data = 0; - - if (debug) { - printf("Length: %d\n", *(SimData + 3)); - for (i = 0; i < *(SimData + 3) + 4; i++) - printf("%x-", *(SimData + i)); - printf("\n"); - } - - U16Data = (U16Data | *(SimData + 5)) << 8; - U16Data = (U16Data | *(SimData + 4)); - U16 MessageId = U16Data; - - //printf("MessageId = %x\n", MessageId); - - U32Data = (U32Data | *(SimData + 6)) << 8; - U32Data = (U32Data | *(SimData + 7)) << 8; - U32Data = (U32Data | *(SimData + 8)) << 8; - U32Data = (U32Data | *(SimData + 9)); - U32 GPSSOW = U32Data; - - //printf("GPSSOW = %x\n", GPSSOW); - U8 DynamicWorldState = *(SimData + 10); - U8 ObjectCount = *(SimData + 11); - - //printf("ObjectCount = %d\n", ObjectCount); - - U8 ObjectId = *(SimData + 12); - U8 ObjectState = *(SimData + 13); - - I32Data = (I32Data | *(SimData + 14)) << 8; - I32Data = (I32Data | *(SimData + 15)) << 8; - I32Data = (I32Data | *(SimData + 16)) << 8; - I32Data = (I32Data | *(SimData + 17)); - I32 XPosition = I32Data; - - I32Data = 0; - I32Data = (I32Data | *(SimData + 18)) << 8; - I32Data = (I32Data | *(SimData + 19)) << 8; - I32Data = (I32Data | *(SimData + 20)) << 8; - I32Data = (I32Data | *(SimData + 21)); - I32 YPosition = I32Data; - - I32Data = 0; - I32Data = (I32Data | *(SimData + 22)) << 8; - I32Data = (I32Data | *(SimData + 23)) << 8; - I32Data = (I32Data | *(SimData + 24)) << 8; - I32Data = (I32Data | *(SimData + 25)); - I32 ZPosition = I32Data; - - U16Data = 0; - U16Data = (U16Data | *(SimData + 26)) << 8; - U16Data = (U16Data | *(SimData + 27)); - U16 Heading = U16Data; - - U16Data = 0; - U16Data = (U16Data | *(SimData + 28)) << 8; - U16Data = (U16Data | *(SimData + 29)); - U16 Pitch = U16Data; - - U16Data = 0; - U16Data = (U16Data | *(SimData + 30)) << 8; - U16Data = (U16Data | *(SimData + 31)); - U16 Roll = U16Data; - - //printf("Roll = %d\n", Roll); - I16Data = 0; - I16Data = (I16Data | *(SimData + 32)) << 8; - I16Data = (I16Data | *(SimData + 33)); - I16 Speed = I16Data; - - //printf("Speed = %d\n", Speed); - - - bzero(MessageBuffer, - ObjectCount * sizeof (Sim1Type) + 6 + COMMAND_MESSAGE_FOOTER_LENGTH + - COMMAND_MESSAGE_HEADER_LENGTH); - - - VOILData->Header.SyncWordU16 = SYNC_WORD; - VOILData->Header.TransmitterIdU8 = 0; - VOILData->Header.MessageCounterU8 = 0; - VOILData->Header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; - VOILData->Header.MessageIdU16 = COMMAND_VOIL_CODE; - VOILData->Header.MessageLengthU32 = ObjectCount * sizeof (Sim1Type) + 6 - COMMAND_MESSAGE_HEADER_LENGTH; - VOILData->GPSQmsOfWeekU32 = GPSSOW; - VOILData->WorldStateU8 = DynamicWorldState; - VOILData->ObjectCountU8 = ObjectCount; - VOILData->SimObjects[0].ObjectIdU8 = ObjectId; - VOILData->SimObjects[0].ObjectStateU8 = ObjectState; - VOILData->SimObjects[0].XPositionI32 = XPosition; - VOILData->SimObjects[0].YPositionI32 = YPosition; - VOILData->SimObjects[0].ZPositionI32 = ZPosition; - VOILData->SimObjects[0].HeadingU16 = Heading; - VOILData->SimObjects[0].PitchU16 = Pitch; - VOILData->SimObjects[0].RollU16 = Roll; - VOILData->SimObjects[0].SpeedI16 = Speed; - - - p = (C8 *) VOILData; - for (i = 0; i < ObjectCount * sizeof (Sim1Type) + 6 + COMMAND_MESSAGE_HEADER_LENGTH; i++) - *(MessageBuffer + i) = *p++; - //Crc = crc_16((const C8*)MessageBuffer, sizeof(VOILData)); - Crc = 0; - *(MessageBuffer + i++) = (U8) (Crc); - *(MessageBuffer + i++) = (U8) (Crc >> 8); - MessageIndex = i; - - if (debug) { - // TODO: use byte printout from logging when it has been implemented - printf("VOILData total length = %d bytes (header+message+footer)\n", - (int)(ObjectCount * sizeof (Sim1Type) + 6 + COMMAND_MESSAGE_FOOTER_LENGTH + - COMMAND_MESSAGE_HEADER_LENGTH)); - printf("----HEADER----\n"); - for (i = 0; i < sizeof (HeaderType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----MESSAGE----\n"); - for (; i < sizeof (Sim1Type) * ObjectCount + 6 + COMMAND_MESSAGE_HEADER_LENGTH; i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----FOOTER----\n"); - for (; i < MessageIndex; i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n"); - } - - - return ObjectCount * sizeof (Sim1Type) + 6 + COMMAND_MESSAGE_HEADER_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH; //Total number of bytes + I32 MessageIndex = 0, i; + U16 Crc = 0; + C8 *p; + U16 U16Data = 0; + I16 I16Data = 0; + U32 U32Data = 0; + I32 I32Data = 0; + + if (debug) { + printf("Length: %d\n", *(SimData + 3)); + for (i = 0; i < *(SimData + 3) + 4; i++) + printf("%x-", *(SimData + i)); + printf("\n"); + } + + U16Data = (U16Data | *(SimData + 5)) << 8; + U16Data = (U16Data | *(SimData + 4)); + U16 MessageId = U16Data; + + //printf("MessageId = %x\n", MessageId); + + U32Data = (U32Data | *(SimData + 6)) << 8; + U32Data = (U32Data | *(SimData + 7)) << 8; + U32Data = (U32Data | *(SimData + 8)) << 8; + U32Data = (U32Data | *(SimData + 9)); + U32 GPSSOW = U32Data; + + //printf("GPSSOW = %x\n", GPSSOW); + U8 DynamicWorldState = *(SimData + 10); + U8 ObjectCount = *(SimData + 11); + + //printf("ObjectCount = %d\n", ObjectCount); + + U8 ObjectId = *(SimData + 12); + U8 ObjectState = *(SimData + 13); + + I32Data = (I32Data | *(SimData + 14)) << 8; + I32Data = (I32Data | *(SimData + 15)) << 8; + I32Data = (I32Data | *(SimData + 16)) << 8; + I32Data = (I32Data | *(SimData + 17)); + I32 XPosition = I32Data; + + I32Data = 0; + I32Data = (I32Data | *(SimData + 18)) << 8; + I32Data = (I32Data | *(SimData + 19)) << 8; + I32Data = (I32Data | *(SimData + 20)) << 8; + I32Data = (I32Data | *(SimData + 21)); + I32 YPosition = I32Data; + + I32Data = 0; + I32Data = (I32Data | *(SimData + 22)) << 8; + I32Data = (I32Data | *(SimData + 23)) << 8; + I32Data = (I32Data | *(SimData + 24)) << 8; + I32Data = (I32Data | *(SimData + 25)); + I32 ZPosition = I32Data; + + U16Data = 0; + U16Data = (U16Data | *(SimData + 26)) << 8; + U16Data = (U16Data | *(SimData + 27)); + U16 Heading = U16Data; + + U16Data = 0; + U16Data = (U16Data | *(SimData + 28)) << 8; + U16Data = (U16Data | *(SimData + 29)); + U16 Pitch = U16Data; + + U16Data = 0; + U16Data = (U16Data | *(SimData + 30)) << 8; + U16Data = (U16Data | *(SimData + 31)); + U16 Roll = U16Data; + + //printf("Roll = %d\n", Roll); + I16Data = 0; + I16Data = (I16Data | *(SimData + 32)) << 8; + I16Data = (I16Data | *(SimData + 33)); + I16 Speed = I16Data; + + //printf("Speed = %d\n", Speed); + + + bzero(MessageBuffer, + ObjectCount * sizeof (Sim1Type) + 6 + COMMAND_MESSAGE_FOOTER_LENGTH + + COMMAND_MESSAGE_HEADER_LENGTH); + + + VOILData->Header.SyncWordU16 = SYNC_WORD; + VOILData->Header.TransmitterIdU8 = 0; + VOILData->Header.MessageCounterU8 = 0; + VOILData->Header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; + VOILData->Header.MessageIdU16 = COMMAND_VOIL_CODE; + VOILData->Header.MessageLengthU32 = ObjectCount * sizeof (Sim1Type) + 6 - COMMAND_MESSAGE_HEADER_LENGTH; + VOILData->GPSQmsOfWeekU32 = GPSSOW; + VOILData->WorldStateU8 = DynamicWorldState; + VOILData->ObjectCountU8 = ObjectCount; + VOILData->SimObjects[0].ObjectIdU8 = ObjectId; + VOILData->SimObjects[0].ObjectStateU8 = ObjectState; + VOILData->SimObjects[0].XPositionI32 = XPosition; + VOILData->SimObjects[0].YPositionI32 = YPosition; + VOILData->SimObjects[0].ZPositionI32 = ZPosition; + VOILData->SimObjects[0].HeadingU16 = Heading; + VOILData->SimObjects[0].PitchU16 = Pitch; + VOILData->SimObjects[0].RollU16 = Roll; + VOILData->SimObjects[0].SpeedI16 = Speed; + + + p = (C8 *) VOILData; + for (i = 0; i < ObjectCount * sizeof (Sim1Type) + 6 + COMMAND_MESSAGE_HEADER_LENGTH; i++) + *(MessageBuffer + i) = *p++; + //Crc = crc_16((const C8*)MessageBuffer, sizeof(VOILData)); + Crc = 0; + *(MessageBuffer + i++) = (U8) (Crc); + *(MessageBuffer + i++) = (U8) (Crc >> 8); + MessageIndex = i; + + if (debug) { + // TODO: use byte printout from logging when it has been implemented + printf("VOILData total length = %d bytes (header+message+footer)\n", + (int)(ObjectCount * sizeof (Sim1Type) + 6 + COMMAND_MESSAGE_FOOTER_LENGTH + + COMMAND_MESSAGE_HEADER_LENGTH)); + printf("----HEADER----\n"); + for (i = 0; i < sizeof (HeaderType); i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n----MESSAGE----\n"); + for (; i < sizeof (Sim1Type) * ObjectCount + 6 + COMMAND_MESSAGE_HEADER_LENGTH; i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n----FOOTER----\n"); + for (; i < MessageIndex; i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n"); + } + + + return ObjectCount * sizeof (Sim1Type) + 6 + COMMAND_MESSAGE_HEADER_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH; //Total number of bytes } I32 ObjectControlBuildMONRMessage(C8 * MonrData, MONRType * MONRData, U8 debug) { - I32 MessageIndex = 0, i = 0; - dbl Data; - U16 Crc = 0, U16Data = 0; - I16 I16Data = 0; - U32 U32Data = 0; - I32 I32Data = 0; - U64 U64Data = 0; - U16 contentLength = 0; - U16 valueID = 0; - C8 *p = MonrData; - - // Decode ISO header - memcpy(&MONRData->Header.SyncWordU16, p, sizeof (MONRData->Header.SyncWordU16)); - p += sizeof (MONRData->Header.SyncWordU16); - - memcpy(&MONRData->Header.TransmitterIdU8, p, sizeof (MONRData->Header.TransmitterIdU8)); - p += sizeof (MONRData->Header.TransmitterIdU8); - - memcpy(&MONRData->Header.MessageCounterU8, p, sizeof (MONRData->Header.MessageCounterU8)); - p += sizeof (MONRData->Header.MessageCounterU8); - - memcpy(&MONRData->Header.AckReqProtVerU8, p, sizeof (MONRData->Header.AckReqProtVerU8)); - p += sizeof (MONRData->Header.AckReqProtVerU8); - - memcpy(&MONRData->Header.MessageIdU16, p, sizeof (MONRData->Header.MessageIdU16)); - p += sizeof (MONRData->Header.MessageIdU16); - - memcpy(&MONRData->Header.MessageLengthU32, p, sizeof (MONRData->Header.MessageLengthU32)); - p += sizeof (MONRData->Header.MessageLengthU32); - - // Decode content header - memcpy(&valueID, p, sizeof (valueID)); - if (valueID == VALUE_ID_MONR_STRUCT) { - memcpy(&MONRData->MonrStructValueIdU16, p, sizeof (MONRData->MonrStructValueIdU16)); - p += sizeof (MONRData->MonrStructValueIdU16); - - //memcpy(&contentLength, p, sizeof (contentLength)); - memcpy(&MONRData->MonrStructContentLengthU16, p, sizeof (MONRData->MonrStructContentLengthU16)); - p += sizeof (MONRData->MonrStructContentLengthU16); - - // TODO: check on content length - } - else if (debug) { - LogPrint("MONR message did not contain a content header"); - } - - // Decode content - memcpy(&MONRData->GPSQmsOfWeekU32, p, sizeof (MONRData->GPSQmsOfWeekU32)); - p += sizeof (MONRData->GPSQmsOfWeekU32); - - memcpy(&MONRData->XPositionI32, p, sizeof (MONRData->XPositionI32)); - p += sizeof (MONRData->XPositionI32); - - memcpy(&MONRData->YPositionI32, p, sizeof (MONRData->YPositionI32)); - p += sizeof (MONRData->YPositionI32); - - memcpy(&MONRData->ZPositionI32, p, sizeof (MONRData->ZPositionI32)); - p += sizeof (MONRData->ZPositionI32); - - memcpy(&MONRData->HeadingU16, p, sizeof (MONRData->HeadingU16)); - p += sizeof (MONRData->HeadingU16); - - memcpy(&MONRData->LongitudinalSpeedI16, p, sizeof (MONRData->LongitudinalSpeedI16)); - p += sizeof (MONRData->LongitudinalSpeedI16); - - memcpy(&MONRData->LateralSpeedI16, p, sizeof (MONRData->LateralSpeedI16)); - p += sizeof (MONRData->LateralSpeedI16); - - memcpy(&MONRData->LongitudinalAccI16, p, sizeof (MONRData->LongitudinalAccI16)); - p += sizeof (MONRData->LongitudinalAccI16); - - memcpy(&MONRData->LateralAccI16, p, sizeof (MONRData->LateralAccI16)); - p += sizeof (MONRData->LateralAccI16); - - memcpy(&MONRData->DriveDirectionU8, p, sizeof (MONRData->DriveDirectionU8)); - p += sizeof (MONRData->DriveDirectionU8); - - memcpy(&MONRData->StateU8, p, sizeof (MONRData->StateU8)); - p += sizeof (MONRData->StateU8); - - memcpy(&MONRData->ReadyToArmU8, p, sizeof (MONRData->ReadyToArmU8)); - p += sizeof (MONRData->ReadyToArmU8); - - memcpy(&MONRData->ErrorStatusU8, p, sizeof (MONRData->ErrorStatusU8)); - p += sizeof (MONRData->ErrorStatusU8); - - // Footer - memcpy(&MONRData->CRC, p, sizeof (MONRData->CRC)); - p += sizeof (MONRData->CRC); - - // TODO: check on CRC - - if (debug == 1) { - LogPrint("MONR:"); - LogPrint("SyncWord = %x", MONRData->Header.SyncWordU16); - LogPrint("TransmitterId = %d", MONRData->Header.TransmitterIdU8); - LogPrint("PackageCounter = %d", MONRData->Header.MessageCounterU8); - LogPrint("AckReq = %d", MONRData->Header.AckReqProtVerU8); - LogPrint("MessageId = %d", MONRData->Header.MessageIdU16); - LogPrint("MessageLength = %d", MONRData->Header.MessageLengthU32); - LogPrint("ValueId = %d", MONRData->MonrStructValueIdU16); - LogPrint("ContentLength = %d", MONRData->MonrStructContentLengthU16); - LogPrint("GPSSOW = %d", MONRData->GPSQmsOfWeekU32); - LogPrint("XPosition = %d", MONRData->XPositionI32); - LogPrint("YPosition = %d", MONRData->YPositionI32); - LogPrint("ZPosition = %d", MONRData->ZPositionI32); - LogPrint("Heading = %d", MONRData->HeadingU16); - LogPrint("LongitudinalSpeed = %d", MONRData->LongitudinalSpeedI16); - LogPrint("LateralSpeed = %d", MONRData->LateralSpeedI16); - LogPrint("LongitudinalAcc = %d", MONRData->LongitudinalAccI16); - LogPrint("LateralAcc = %d", MONRData->LateralAccI16); - LogPrint("DriveDirection = %d", MONRData->DriveDirectionU8); - LogPrint("State = %d", MONRData->StateU8); - LogPrint("ReadyToArm = %d", MONRData->ReadyToArmU8); - LogPrint("ErrorStatus = %d", MONRData->ErrorStatusU8); - } - - return 0; + I32 MessageIndex = 0, i = 0; + dbl Data; + U16 Crc = 0, U16Data = 0; + I16 I16Data = 0; + U32 U32Data = 0; + I32 I32Data = 0; + U64 U64Data = 0; + U16 contentLength = 0; + U16 valueID = 0; + C8 *p = MonrData; + + // Decode ISO header + memcpy(&MONRData->Header.SyncWordU16, p, sizeof (MONRData->Header.SyncWordU16)); + p += sizeof (MONRData->Header.SyncWordU16); + + memcpy(&MONRData->Header.TransmitterIdU8, p, sizeof (MONRData->Header.TransmitterIdU8)); + p += sizeof (MONRData->Header.TransmitterIdU8); + + memcpy(&MONRData->Header.MessageCounterU8, p, sizeof (MONRData->Header.MessageCounterU8)); + p += sizeof (MONRData->Header.MessageCounterU8); + + memcpy(&MONRData->Header.AckReqProtVerU8, p, sizeof (MONRData->Header.AckReqProtVerU8)); + p += sizeof (MONRData->Header.AckReqProtVerU8); + + memcpy(&MONRData->Header.MessageIdU16, p, sizeof (MONRData->Header.MessageIdU16)); + p += sizeof (MONRData->Header.MessageIdU16); + + memcpy(&MONRData->Header.MessageLengthU32, p, sizeof (MONRData->Header.MessageLengthU32)); + p += sizeof (MONRData->Header.MessageLengthU32); + + // Decode content header + memcpy(&valueID, p, sizeof (valueID)); + if (valueID == VALUE_ID_MONR_STRUCT) { + memcpy(&MONRData->MonrStructValueIdU16, p, sizeof (MONRData->MonrStructValueIdU16)); + p += sizeof (MONRData->MonrStructValueIdU16); + + //memcpy(&contentLength, p, sizeof (contentLength)); + memcpy(&MONRData->MonrStructContentLengthU16, p, sizeof (MONRData->MonrStructContentLengthU16)); + p += sizeof (MONRData->MonrStructContentLengthU16); + + // TODO: check on content length + } + else if (debug) { + LogPrint("MONR message did not contain a content header"); + } + + // Decode content + memcpy(&MONRData->GPSQmsOfWeekU32, p, sizeof (MONRData->GPSQmsOfWeekU32)); + p += sizeof (MONRData->GPSQmsOfWeekU32); + + memcpy(&MONRData->XPositionI32, p, sizeof (MONRData->XPositionI32)); + p += sizeof (MONRData->XPositionI32); + + memcpy(&MONRData->YPositionI32, p, sizeof (MONRData->YPositionI32)); + p += sizeof (MONRData->YPositionI32); + + memcpy(&MONRData->ZPositionI32, p, sizeof (MONRData->ZPositionI32)); + p += sizeof (MONRData->ZPositionI32); + + memcpy(&MONRData->HeadingU16, p, sizeof (MONRData->HeadingU16)); + p += sizeof (MONRData->HeadingU16); + + memcpy(&MONRData->LongitudinalSpeedI16, p, sizeof (MONRData->LongitudinalSpeedI16)); + p += sizeof (MONRData->LongitudinalSpeedI16); + + memcpy(&MONRData->LateralSpeedI16, p, sizeof (MONRData->LateralSpeedI16)); + p += sizeof (MONRData->LateralSpeedI16); + + memcpy(&MONRData->LongitudinalAccI16, p, sizeof (MONRData->LongitudinalAccI16)); + p += sizeof (MONRData->LongitudinalAccI16); + + memcpy(&MONRData->LateralAccI16, p, sizeof (MONRData->LateralAccI16)); + p += sizeof (MONRData->LateralAccI16); + + memcpy(&MONRData->DriveDirectionU8, p, sizeof (MONRData->DriveDirectionU8)); + p += sizeof (MONRData->DriveDirectionU8); + + memcpy(&MONRData->StateU8, p, sizeof (MONRData->StateU8)); + p += sizeof (MONRData->StateU8); + + memcpy(&MONRData->ReadyToArmU8, p, sizeof (MONRData->ReadyToArmU8)); + p += sizeof (MONRData->ReadyToArmU8); + + memcpy(&MONRData->ErrorStatusU8, p, sizeof (MONRData->ErrorStatusU8)); + p += sizeof (MONRData->ErrorStatusU8); + + // Footer + memcpy(&MONRData->CRC, p, sizeof (MONRData->CRC)); + p += sizeof (MONRData->CRC); + + // TODO: check on CRC + + if (debug == 1) { + LogPrint("MONR:"); + LogPrint("SyncWord = %x", MONRData->Header.SyncWordU16); + LogPrint("TransmitterId = %d", MONRData->Header.TransmitterIdU8); + LogPrint("PackageCounter = %d", MONRData->Header.MessageCounterU8); + LogPrint("AckReq = %d", MONRData->Header.AckReqProtVerU8); + LogPrint("MessageId = %d", MONRData->Header.MessageIdU16); + LogPrint("MessageLength = %d", MONRData->Header.MessageLengthU32); + LogPrint("ValueId = %d", MONRData->MonrStructValueIdU16); + LogPrint("ContentLength = %d", MONRData->MonrStructContentLengthU16); + LogPrint("GPSSOW = %d", MONRData->GPSQmsOfWeekU32); + LogPrint("XPosition = %d", MONRData->XPositionI32); + LogPrint("YPosition = %d", MONRData->YPositionI32); + LogPrint("ZPosition = %d", MONRData->ZPositionI32); + LogPrint("Heading = %d", MONRData->HeadingU16); + LogPrint("LongitudinalSpeed = %d", MONRData->LongitudinalSpeedI16); + LogPrint("LateralSpeed = %d", MONRData->LateralSpeedI16); + LogPrint("LongitudinalAcc = %d", MONRData->LongitudinalAccI16); + LogPrint("LateralAcc = %d", MONRData->LateralAccI16); + LogPrint("DriveDirection = %d", MONRData->DriveDirectionU8); + LogPrint("State = %d", MONRData->StateU8); + LogPrint("ReadyToArm = %d", MONRData->ReadyToArmU8); + LogPrint("ErrorStatus = %d", MONRData->ErrorStatusU8); + } + + return 0; } I32 ObjectControlMONRToASCII(MONRType * MONRData, GeoPosition * OriginPosition, I32 Idn, C8 * Id, - C8 * Timestamp, C8 * XPosition, C8 * YPosition, C8 * ZPosition, - C8 * LongitudinalSpeed, C8 * LateralSpeed, C8 * LongitudinalAcc, C8 * LateralAcc, - C8 * Heading, C8 * DriveDirection, C8 * ObjectState, C8 * ReadyToArm, - C8 * ErrorStatus, C8 debug) { - char Buffer[6]; - long unsigned int MonrValueU64; - unsigned int MonrValueU32; - unsigned short MonrValueU16; - unsigned char MonrValueU8; - double iLlh[3] = { 0, 0, 0 }; - double xyz[3] = { 0, 0, 0 }; - double Llh[3] = { 0, 0, 0 }; - uint64_t ConvertGPStoUTC; - - bzero(Id, SMALL_BUFFER_SIZE_1); - bzero(Timestamp, SMALL_BUFFER_SIZE_0); - bzero(XPosition, SMALL_BUFFER_SIZE_0); - bzero(YPosition, SMALL_BUFFER_SIZE_0); - bzero(ZPosition, SMALL_BUFFER_SIZE_0); - bzero(LongitudinalSpeed, SMALL_BUFFER_SIZE_0); - bzero(LateralSpeed, SMALL_BUFFER_SIZE_0); - bzero(LongitudinalAcc, SMALL_BUFFER_SIZE_0); - bzero(LateralAcc, SMALL_BUFFER_SIZE_0); - bzero(Heading, SMALL_BUFFER_SIZE_0); - bzero(DriveDirection, SMALL_BUFFER_SIZE_1); - bzero(ObjectState, SMALL_BUFFER_SIZE_1); - bzero(ReadyToArm, SMALL_BUFFER_SIZE_1); - - - if (MONRData->Header.MessageIdU16 == COMMAND_MONR_CODE) { - //Index - sprintf(Id, "%" PRIu8, (C8) Idn); - - //Timestamp - MonrValueU64 = 0; - //for(i = 0; i <= 5; i++, j++) MonrValueU64 = *(MonrData+j) | (MonrValueU64 << 8); - ConvertGPStoUTC = sprintf(Timestamp, "%" PRIu32, MONRData->GPSQmsOfWeekU32); - - if (debug && MONRData->GPSQmsOfWeekU32 % 400 == 0) { - LogMessage(LOG_LEVEL_DEBUG, "MONR = %x-%x-%x-%x-%x-%x-%d-%d-%d-%d-%d-%d-%d-%d-%d-%d-%d-%d", - MONRData->Header.MessageIdU16, - MONRData->Header.SyncWordU16, - MONRData->Header.TransmitterIdU8, - MONRData->Header.MessageCounterU8, - MONRData->Header.AckReqProtVerU8, - MONRData->Header.MessageLengthU32, - MONRData->MonrStructValueIdU16, - MONRData->MonrStructContentLengthU16, - MONRData->GPSQmsOfWeekU32, - MONRData->XPositionI32, - MONRData->YPositionI32, - MONRData->ZPositionI32, - MONRData->LongitudinalSpeedI16, - MONRData->HeadingU16, - MONRData->DriveDirectionU8, - MONRData->StateU8, MONRData->ReadyToArmU8, MONRData->ErrorStatusU8); - } - - iLlh[0] = OriginPosition->Latitude; - iLlh[1] = OriginPosition->Longitude; - iLlh[2] = OriginPosition->Altitude; - - xyz[0] = ((dbl) MONRData->XPositionI32) / 1000; - xyz[1] = ((dbl) MONRData->YPositionI32) / 1000; - xyz[2] = ((dbl) MONRData->ZPositionI32) / 1000; - - enuToLlh(iLlh, xyz, Llh); - - //XPosition - //MonrValueU32 = 0; - //for(i = 0; i <= 3; i++, j++) MonrValueU32 = *(MonrData+j) | (MonrValueU32 << 8); - //sprintf(Latitude, "%" PRIi32, (I32)(Llh[0]*1e7)); - sprintf(XPosition, "%" PRIi32, MONRData->XPositionI32); - - //YPosition - //MonrValueU32 = 0; - //for(i = 0; i <= 3; i++, j++) MonrValueU32 = *(MonrData+j) | (MonrValueU32 << 8); - //sprintf(Longitude, "%" PRIi32, (I32)(Llh[1]*1e7)); - sprintf(YPosition, "%" PRIi32, MONRData->YPositionI32); - - //ZPosition - //MonrValueU32 = 0; - //for(i = 0; i <= 3; i++, j++) MonrValueU32 = *(MonrData+j) | (MonrValueU32 << 8); - //sprintf(Altitude, "%" PRIi32, (I32)(Llh[2])); - sprintf(ZPosition, "%" PRIi32, MONRData->ZPositionI32); - - //Speed - //MonrValueU16 = 0; - //for(i = 0; i <= 1; i++, j++) MonrValueU16 = *(MonrData+j) | (MonrValueU16 << 8); - sprintf(LongitudinalSpeed, "%" PRIi16, MONRData->LongitudinalSpeedI16); - - //LatSpeed - //MonrValueU16 = 0; - //for(i = 0; i <= 1; i++, j++) MonrValueU16 = *(MonrData+j) | (MonrValueU16 << 8); - sprintf(LateralSpeed, "%" PRIi16, MONRData->LateralSpeedI16); - - //LongAcc - //MonrValueU16 = 0; - //for(i = 0; i <= 1; i++, j++) MonrValueU16 = *(MonrData+j) | (MonrValueU16 << 8); - sprintf(LongitudinalAcc, "%" PRIi16, MONRData->LongitudinalAccI16); - - //LatAcc - //MonrValueU16 = 0; - //for(i = 0; i <= 1; i++, j++) MonrValueU16 = *(MonrData+j) | (MonrValueU16 << 8); - sprintf(LateralAcc, "%" PRIi16, MONRData->LateralAccI16); - - //Heading - //MonrValueU16 = 0; - //for(i = 0; i <= 1; i++, j++) MonrValueU16 = *(MonrData+j) | (MonrValueU16 << 8); - sprintf(Heading, "%" PRIu16, MONRData->HeadingU16); - - //Driving direction - //MonrValueU8 = (unsigned char)*(MonrData+j); - //printf("D: %d\n", MonrValueU8 ); - - sprintf(DriveDirection, "%" PRIu8, MONRData->DriveDirectionU8); - - //State - //MonrValueU8 = (unsigned char)*(MonrData+j); - sprintf(ObjectState, "%" PRIu8, MONRData->StateU8); - - //ReadyToArmU8 - //MonrValueU8 = (unsigned char)*(MonrData+j); - sprintf(ReadyToArm, "%" PRIu8, MONRData->ReadyToArmU8); - - //ErrorStatusU8 - //MonrValueU8 = (unsigned char)*(MonrData+j); - sprintf(ErrorStatus, "%" PRIu8, MONRData->ErrorStatusU8); - - } - - return 0; + C8 * Timestamp, C8 * XPosition, C8 * YPosition, C8 * ZPosition, + C8 * LongitudinalSpeed, C8 * LateralSpeed, C8 * LongitudinalAcc, C8 * LateralAcc, + C8 * Heading, C8 * DriveDirection, C8 * ObjectState, C8 * ReadyToArm, + C8 * ErrorStatus, C8 debug) { + char Buffer[6]; + long unsigned int MonrValueU64; + unsigned int MonrValueU32; + unsigned short MonrValueU16; + unsigned char MonrValueU8; + double iLlh[3] = { 0, 0, 0 }; + double xyz[3] = { 0, 0, 0 }; + double Llh[3] = { 0, 0, 0 }; + uint64_t ConvertGPStoUTC; + + bzero(Id, SMALL_BUFFER_SIZE_1); + bzero(Timestamp, SMALL_BUFFER_SIZE_0); + bzero(XPosition, SMALL_BUFFER_SIZE_0); + bzero(YPosition, SMALL_BUFFER_SIZE_0); + bzero(ZPosition, SMALL_BUFFER_SIZE_0); + bzero(LongitudinalSpeed, SMALL_BUFFER_SIZE_0); + bzero(LateralSpeed, SMALL_BUFFER_SIZE_0); + bzero(LongitudinalAcc, SMALL_BUFFER_SIZE_0); + bzero(LateralAcc, SMALL_BUFFER_SIZE_0); + bzero(Heading, SMALL_BUFFER_SIZE_0); + bzero(DriveDirection, SMALL_BUFFER_SIZE_1); + bzero(ObjectState, SMALL_BUFFER_SIZE_1); + bzero(ReadyToArm, SMALL_BUFFER_SIZE_1); + + + if (MONRData->Header.MessageIdU16 == COMMAND_MONR_CODE) { + //Index + sprintf(Id, "%" PRIu8, (C8) Idn); + + //Timestamp + MonrValueU64 = 0; + //for(i = 0; i <= 5; i++, j++) MonrValueU64 = *(MonrData+j) | (MonrValueU64 << 8); + ConvertGPStoUTC = sprintf(Timestamp, "%" PRIu32, MONRData->GPSQmsOfWeekU32); + + if (debug && MONRData->GPSQmsOfWeekU32 % 400 == 0) { + LogMessage(LOG_LEVEL_DEBUG, "MONR = %x-%x-%x-%x-%x-%x-%d-%d-%d-%d-%d-%d-%d-%d-%d-%d-%d-%d", + MONRData->Header.MessageIdU16, + MONRData->Header.SyncWordU16, + MONRData->Header.TransmitterIdU8, + MONRData->Header.MessageCounterU8, + MONRData->Header.AckReqProtVerU8, + MONRData->Header.MessageLengthU32, + MONRData->MonrStructValueIdU16, + MONRData->MonrStructContentLengthU16, + MONRData->GPSQmsOfWeekU32, + MONRData->XPositionI32, + MONRData->YPositionI32, + MONRData->ZPositionI32, + MONRData->LongitudinalSpeedI16, + MONRData->HeadingU16, + MONRData->DriveDirectionU8, + MONRData->StateU8, MONRData->ReadyToArmU8, MONRData->ErrorStatusU8); + } + + iLlh[0] = OriginPosition->Latitude; + iLlh[1] = OriginPosition->Longitude; + iLlh[2] = OriginPosition->Altitude; + + xyz[0] = ((dbl) MONRData->XPositionI32) / 1000; + xyz[1] = ((dbl) MONRData->YPositionI32) / 1000; + xyz[2] = ((dbl) MONRData->ZPositionI32) / 1000; + + enuToLlh(iLlh, xyz, Llh); + + //XPosition + //MonrValueU32 = 0; + //for(i = 0; i <= 3; i++, j++) MonrValueU32 = *(MonrData+j) | (MonrValueU32 << 8); + //sprintf(Latitude, "%" PRIi32, (I32)(Llh[0]*1e7)); + sprintf(XPosition, "%" PRIi32, MONRData->XPositionI32); + + //YPosition + //MonrValueU32 = 0; + //for(i = 0; i <= 3; i++, j++) MonrValueU32 = *(MonrData+j) | (MonrValueU32 << 8); + //sprintf(Longitude, "%" PRIi32, (I32)(Llh[1]*1e7)); + sprintf(YPosition, "%" PRIi32, MONRData->YPositionI32); + + //ZPosition + //MonrValueU32 = 0; + //for(i = 0; i <= 3; i++, j++) MonrValueU32 = *(MonrData+j) | (MonrValueU32 << 8); + //sprintf(Altitude, "%" PRIi32, (I32)(Llh[2])); + sprintf(ZPosition, "%" PRIi32, MONRData->ZPositionI32); + + //Speed + //MonrValueU16 = 0; + //for(i = 0; i <= 1; i++, j++) MonrValueU16 = *(MonrData+j) | (MonrValueU16 << 8); + sprintf(LongitudinalSpeed, "%" PRIi16, MONRData->LongitudinalSpeedI16); + + //LatSpeed + //MonrValueU16 = 0; + //for(i = 0; i <= 1; i++, j++) MonrValueU16 = *(MonrData+j) | (MonrValueU16 << 8); + sprintf(LateralSpeed, "%" PRIi16, MONRData->LateralSpeedI16); + + //LongAcc + //MonrValueU16 = 0; + //for(i = 0; i <= 1; i++, j++) MonrValueU16 = *(MonrData+j) | (MonrValueU16 << 8); + sprintf(LongitudinalAcc, "%" PRIi16, MONRData->LongitudinalAccI16); + + //LatAcc + //MonrValueU16 = 0; + //for(i = 0; i <= 1; i++, j++) MonrValueU16 = *(MonrData+j) | (MonrValueU16 << 8); + sprintf(LateralAcc, "%" PRIi16, MONRData->LateralAccI16); + + //Heading + //MonrValueU16 = 0; + //for(i = 0; i <= 1; i++, j++) MonrValueU16 = *(MonrData+j) | (MonrValueU16 << 8); + sprintf(Heading, "%" PRIu16, MONRData->HeadingU16); + + //Driving direction + //MonrValueU8 = (unsigned char)*(MonrData+j); + //printf("D: %d\n", MonrValueU8 ); + + sprintf(DriveDirection, "%" PRIu8, MONRData->DriveDirectionU8); + + //State + //MonrValueU8 = (unsigned char)*(MonrData+j); + sprintf(ObjectState, "%" PRIu8, MONRData->StateU8); + + //ReadyToArmU8 + //MonrValueU8 = (unsigned char)*(MonrData+j); + sprintf(ReadyToArm, "%" PRIu8, MONRData->ReadyToArmU8); + + //ErrorStatusU8 + //MonrValueU8 = (unsigned char)*(MonrData+j); + sprintf(ErrorStatus, "%" PRIu8, MONRData->ErrorStatusU8); + + } + + return 0; } I32 ObjectControlBuildOSEMMessage(C8 * MessageBuffer, OSEMType * OSEMData, TimeType * GPSTime, C8 * Latitude, - C8 * Longitude, C8 * Altitude, U8 debug) { - I32 MessageIndex = 0, i = 0; - dbl Data; - U16 Crc = 0; - C8 *p; - U32 ISODate = 0; - - bzero(MessageBuffer, COMMAND_OSEM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); - - OSEMData->Header.SyncWordU16 = SYNC_WORD; - OSEMData->Header.TransmitterIdU8 = 0; - OSEMData->Header.MessageCounterU8 = 0; - OSEMData->Header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; - OSEMData->Header.MessageIdU16 = COMMAND_OSEM_CODE; - OSEMData->Header.MessageLengthU32 = sizeof (OSEMType) - sizeof (HeaderType) - 4; - OSEMData->LatitudeValueIdU16 = VALUE_ID_LATITUDE; - OSEMData->LatitudeContentLengthU16 = 6; - OSEMData->LatitudeI64 = (I64) ((atof((const char *)Latitude) * 1e10)); - OSEMData->LongitudeValueIdU16 = VALUE_ID_LONGITUDE; - OSEMData->LongitudeContentLengthU16 = 6; - OSEMData->LongitudeI64 = (I64) ((atof((const char *)Longitude) * 1e10)); - OSEMData->AltitudeValueIdU16 = VALUE_ID_ALTITUDE; - OSEMData->AltitudeContentLengthU16 = 4; - OSEMData->AltitudeI32 = (I32) (atof((char *)Altitude) * 1e2); - OSEMData->DateValueIdU16 = VALUE_ID_DATE_ISO8601; - OSEMData->DateContentLengthU16 = 4; - OSEMData->DateU32 = - ((U32) GPSTime->YearU16 * 10000) + ((U32) GPSTime->MonthU8 * 100) + ((U32) GPSTime->DayU8); - OSEMData->GPSWeekValueIdU16 = VALUE_ID_GPS_WEEK; - OSEMData->GPSWeekContentLengthU16 = 2; - OSEMData->GPSWeekU16 = GPSTime->GPSWeekU16; - OSEMData->GPSSOWValueIdU16 = VALUE_ID_GPS_SECOND_OF_WEEK; - OSEMData->GPSSOWContentLengthU16 = 4; - OSEMData->GPSQmsOfWeekU32 = - ((GPSTime->GPSSecondsOfWeekU32 * 1000 + GPSTime->MillisecondU16) << 2) + GPSTime->MicroSecondU16; - OSEMData->MaxWayDeviationValueIdU16 = VALUE_ID_MAX_WAY_DEVIATION; - OSEMData->MaxWayDeviationContentLengthU16 = 2; - OSEMData->MaxWayDeviationU16 = 65535; - OSEMData->MaxLateralDeviationValueIdU16 = VALUE_ID_MAX_LATERAL_DEVIATION; - OSEMData->MaxLateralDeviationContentLengthU16 = 2; - OSEMData->MaxLateralDeviationU16 = 65535; - OSEMData->MinPosAccuracyContentLengthU16 = 2; - OSEMData->MinPosAccuracyValueIdU16 = VALUE_ID_MIN_POS_ACCURACY; - OSEMData->MinPosAccuracyU16 = 65535; - - if (!GPSTime->isGPSenabled) { - OSEMData->DateU32 = UtilgetIntDateFromMS(UtilgetCurrentUTCtimeMS()); - UtilgetCurrentGPStime(&OSEMData->GPSWeekU16, &OSEMData->GPSQmsOfWeekU32); - } - - p = (C8 *) OSEMData; - for (i = 0; i < 21; i++) - *(MessageBuffer + i) = *p++; - *p++; - *p++; - for (; i < 31; i++) - *(MessageBuffer + i) = *p++; - *p++; - *p++; - for (; i < sizeof (OSEMType) - 4; i++) - *(MessageBuffer + i) = *p++; - - Crc = crc_16((const C8 *)MessageBuffer, sizeof (OSEMType) - 4); - Crc = 0; - *(MessageBuffer + i++) = (U8) (Crc); - *(MessageBuffer + i++) = (U8) (Crc >> 8); - - MessageIndex = i; - - if (debug) { - // TODO: Change to log printout when byte thingy has been implemented - printf("OSEM total length = %d bytes (header+message+footer)\n", - (int)(COMMAND_OSEM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH)); - printf("----HEADER----\n"); - for (i = 0; i < sizeof (HeaderType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----MESSAGE----\n"); - for (; i < sizeof (OSEMType) - 4; i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----FOOTER----\n"); - for (; i < MessageIndex; i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n"); - printf("Latitude = %ld\n", OSEMData->LatitudeI64); - printf("Longitude = %ld\n", OSEMData->LongitudeI64); - printf("ISODate = %d\n", OSEMData->DateU32); - } - return MessageIndex; //Total number of bytes + C8 * Longitude, C8 * Altitude, U8 debug) { + I32 MessageIndex = 0, i = 0; + dbl Data; + U16 Crc = 0; + C8 *p; + U32 ISODate = 0; + + bzero(MessageBuffer, COMMAND_OSEM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); + + OSEMData->Header.SyncWordU16 = SYNC_WORD; + OSEMData->Header.TransmitterIdU8 = 0; + OSEMData->Header.MessageCounterU8 = 0; + OSEMData->Header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; + OSEMData->Header.MessageIdU16 = COMMAND_OSEM_CODE; + OSEMData->Header.MessageLengthU32 = sizeof (OSEMType) - sizeof (HeaderType) - 4; + OSEMData->LatitudeValueIdU16 = VALUE_ID_LATITUDE; + OSEMData->LatitudeContentLengthU16 = 6; + OSEMData->LatitudeI64 = (I64) ((atof((const char *)Latitude) * 1e10)); + OSEMData->LongitudeValueIdU16 = VALUE_ID_LONGITUDE; + OSEMData->LongitudeContentLengthU16 = 6; + OSEMData->LongitudeI64 = (I64) ((atof((const char *)Longitude) * 1e10)); + OSEMData->AltitudeValueIdU16 = VALUE_ID_ALTITUDE; + OSEMData->AltitudeContentLengthU16 = 4; + OSEMData->AltitudeI32 = (I32) (atof((char *)Altitude) * 1e2); + OSEMData->DateValueIdU16 = VALUE_ID_DATE_ISO8601; + OSEMData->DateContentLengthU16 = 4; + OSEMData->DateU32 = + ((U32) GPSTime->YearU16 * 10000) + ((U32) GPSTime->MonthU8 * 100) + ((U32) GPSTime->DayU8); + OSEMData->GPSWeekValueIdU16 = VALUE_ID_GPS_WEEK; + OSEMData->GPSWeekContentLengthU16 = 2; + OSEMData->GPSWeekU16 = GPSTime->GPSWeekU16; + OSEMData->GPSSOWValueIdU16 = VALUE_ID_GPS_SECOND_OF_WEEK; + OSEMData->GPSSOWContentLengthU16 = 4; + OSEMData->GPSQmsOfWeekU32 = + ((GPSTime->GPSSecondsOfWeekU32 * 1000 + GPSTime->MillisecondU16) << 2) + GPSTime->MicroSecondU16; + OSEMData->MaxWayDeviationValueIdU16 = VALUE_ID_MAX_WAY_DEVIATION; + OSEMData->MaxWayDeviationContentLengthU16 = 2; + OSEMData->MaxWayDeviationU16 = 65535; + OSEMData->MaxLateralDeviationValueIdU16 = VALUE_ID_MAX_LATERAL_DEVIATION; + OSEMData->MaxLateralDeviationContentLengthU16 = 2; + OSEMData->MaxLateralDeviationU16 = 65535; + OSEMData->MinPosAccuracyContentLengthU16 = 2; + OSEMData->MinPosAccuracyValueIdU16 = VALUE_ID_MIN_POS_ACCURACY; + OSEMData->MinPosAccuracyU16 = 65535; + + if (!GPSTime->isGPSenabled) { + OSEMData->DateU32 = UtilgetIntDateFromMS(UtilgetCurrentUTCtimeMS()); + UtilgetCurrentGPStime(&OSEMData->GPSWeekU16, &OSEMData->GPSQmsOfWeekU32); + } + + p = (C8 *) OSEMData; + for (i = 0; i < 21; i++) + *(MessageBuffer + i) = *p++; + *p++; + *p++; + for (; i < 31; i++) + *(MessageBuffer + i) = *p++; + *p++; + *p++; + for (; i < sizeof (OSEMType) - 4; i++) + *(MessageBuffer + i) = *p++; + + Crc = crc_16((const C8 *)MessageBuffer, sizeof (OSEMType) - 4); + Crc = 0; + *(MessageBuffer + i++) = (U8) (Crc); + *(MessageBuffer + i++) = (U8) (Crc >> 8); + + MessageIndex = i; + + if (debug) { + // TODO: Change to log printout when byte thingy has been implemented + printf("OSEM total length = %d bytes (header+message+footer)\n", + (int)(COMMAND_OSEM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH)); + printf("----HEADER----\n"); + for (i = 0; i < sizeof (HeaderType); i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n----MESSAGE----\n"); + for (; i < sizeof (OSEMType) - 4; i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n----FOOTER----\n"); + for (; i < MessageIndex; i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n"); + printf("Latitude = %ld\n", OSEMData->LatitudeI64); + printf("Longitude = %ld\n", OSEMData->LongitudeI64); + printf("ISODate = %d\n", OSEMData->DateU32); + } + return MessageIndex; //Total number of bytes } int ObjectControlOSEMtoASCII(OSEMType * OSEMData, char *GPSWeek, char *GPSLatitude, char *GPSLongitude, - char *GPSAltitude) { - // what do i want? in my mq? gps week, origin in lat and long coordinates - bzero(GPSWeek, SMALL_BUFFER_SIZE_0); - bzero(GPSLatitude, SMALL_BUFFER_SIZE_0); - bzero(GPSLongitude, SMALL_BUFFER_SIZE_0); - bzero(GPSAltitude, SMALL_BUFFER_SIZE_0); + char *GPSAltitude) { + // what do i want? in my mq? gps week, origin in lat and long coordinates + bzero(GPSWeek, SMALL_BUFFER_SIZE_0); + bzero(GPSLatitude, SMALL_BUFFER_SIZE_0); + bzero(GPSLongitude, SMALL_BUFFER_SIZE_0); + bzero(GPSAltitude, SMALL_BUFFER_SIZE_0); - if (OSEMData->Header.MessageIdU16 == COMMAND_OSEM_CODE) { - sprintf(GPSWeek, "%" PRIu16, OSEMData->GPSWeekU16); + if (OSEMData->Header.MessageIdU16 == COMMAND_OSEM_CODE) { + sprintf(GPSWeek, "%" PRIu16, OSEMData->GPSWeekU16); - sprintf(GPSLatitude, "%" PRIi64, OSEMData->LatitudeI64); + sprintf(GPSLatitude, "%" PRIi64, OSEMData->LatitudeI64); - sprintf(GPSLongitude, "%" PRIi64, OSEMData->LongitudeI64); + sprintf(GPSLongitude, "%" PRIi64, OSEMData->LongitudeI64); - sprintf(GPSAltitude, "%" PRIi32, OSEMData->AltitudeI32); - } - return 0; + sprintf(GPSAltitude, "%" PRIi32, OSEMData->AltitudeI32); + } + return 0; } int ObjectControlBuildSTRTMessage(C8 * MessageBuffer, STRTType * STRTData, TimeType * GPSTime, - U32 ScenarioStartTime, U32 DelayStart, U32 * OutgoingStartTime, U8 debug) { - I32 MessageIndex = 0, i = 0; - U16 Crc = 0; - C8 *p; - - bzero(MessageBuffer, COMMAND_STRT_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); - - STRTData->Header.SyncWordU16 = SYNC_WORD; - STRTData->Header.TransmitterIdU8 = 0; - STRTData->Header.MessageCounterU8 = 0; - STRTData->Header.AckReqProtVerU8 = 0; - STRTData->Header.MessageIdU16 = COMMAND_STRT_CODE; - STRTData->Header.MessageLengthU32 = sizeof (STRTType) - sizeof (HeaderType); - STRTData->StartTimeValueIdU16 = VALUE_ID_GPS_SECOND_OF_WEEK; - STRTData->StartTimeContentLengthU16 = sizeof (STRTData->StartTimeU32); - STRTData->StartTimeU32 = - ((GPSTime->GPSSecondsOfWeekU32 * 1000 + (U32) TimeControlGetMillisecond(GPSTime) + - ScenarioStartTime) << 2) + GPSTime->MicroSecondU16; - STRTData->GPSWeekValueIdU16 = VALUE_ID_GPS_WEEK; - STRTData->GPSWeekContentLengthU16 = sizeof (STRTData->GPSWeekU16); - STRTData->GPSWeekU16 = GPSTime->GPSWeekU16; - // STRTData->DelayStartValueIdU16 = VALUE_ID_RELATIVE_TIME; - // STRTData->DelayStartContentLengthU16 = 4; - // STRTData->DelayStartU32 = DelayStart; - - *OutgoingStartTime = (STRTData->StartTimeU32) >> 2; - - if (!GPSTime->isGPSenabled) { - UtilgetCurrentGPStime(NULL, &STRTData->StartTimeU32); - } - - p = (char *)STRTData; - for (i = 0; i < sizeof (STRTType); i++) - *(MessageBuffer + i) = *p++; - Crc = crc_16((const unsigned char *)MessageBuffer, sizeof (STRTType)); - Crc = 0; - *(MessageBuffer + i++) = (U8) (Crc); - *(MessageBuffer + i++) = (U8) (Crc >> 8); - MessageIndex = i; - - if (debug) { - // TODO: Change to log printout when byte thingy has been implemented - printf("STRT total length = %d bytes (header+message+footer)\n", - (int)(COMMAND_STRT_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH)); - printf("----HEADER----\n"); - for (i = 0; i < sizeof (HeaderType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----MESSAGE----\n"); - for (; i < sizeof (STRTType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----FOOTER----\n"); - for (; i < MessageIndex; i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n"); - } - - return MessageIndex; //Total number of bytes + U32 ScenarioStartTime, U32 DelayStart, U32 * OutgoingStartTime, U8 debug) { + I32 MessageIndex = 0, i = 0; + U16 Crc = 0; + C8 *p; + + bzero(MessageBuffer, COMMAND_STRT_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); + + STRTData->Header.SyncWordU16 = SYNC_WORD; + STRTData->Header.TransmitterIdU8 = 0; + STRTData->Header.MessageCounterU8 = 0; + STRTData->Header.AckReqProtVerU8 = 0; + STRTData->Header.MessageIdU16 = COMMAND_STRT_CODE; + STRTData->Header.MessageLengthU32 = sizeof (STRTType) - sizeof (HeaderType); + STRTData->StartTimeValueIdU16 = VALUE_ID_GPS_SECOND_OF_WEEK; + STRTData->StartTimeContentLengthU16 = sizeof (STRTData->StartTimeU32); + STRTData->StartTimeU32 = + ((GPSTime->GPSSecondsOfWeekU32 * 1000 + (U32) TimeControlGetMillisecond(GPSTime) + + ScenarioStartTime) << 2) + GPSTime->MicroSecondU16; + STRTData->GPSWeekValueIdU16 = VALUE_ID_GPS_WEEK; + STRTData->GPSWeekContentLengthU16 = sizeof (STRTData->GPSWeekU16); + STRTData->GPSWeekU16 = GPSTime->GPSWeekU16; + // STRTData->DelayStartValueIdU16 = VALUE_ID_RELATIVE_TIME; + // STRTData->DelayStartContentLengthU16 = 4; + // STRTData->DelayStartU32 = DelayStart; + + *OutgoingStartTime = (STRTData->StartTimeU32) >> 2; + + if (!GPSTime->isGPSenabled) { + UtilgetCurrentGPStime(NULL, &STRTData->StartTimeU32); + } + + p = (char *)STRTData; + for (i = 0; i < sizeof (STRTType); i++) + *(MessageBuffer + i) = *p++; + Crc = crc_16((const unsigned char *)MessageBuffer, sizeof (STRTType)); + Crc = 0; + *(MessageBuffer + i++) = (U8) (Crc); + *(MessageBuffer + i++) = (U8) (Crc >> 8); + MessageIndex = i; + + if (debug) { + // TODO: Change to log printout when byte thingy has been implemented + printf("STRT total length = %d bytes (header+message+footer)\n", + (int)(COMMAND_STRT_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH)); + printf("----HEADER----\n"); + for (i = 0; i < sizeof (HeaderType); i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n----MESSAGE----\n"); + for (; i < sizeof (STRTType); i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n----FOOTER----\n"); + for (; i < MessageIndex; i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n"); + } + + return MessageIndex; //Total number of bytes } I32 ObjectControlBuildOSTMMessage(C8 * MessageBuffer, OSTMType * OSTMData, C8 CommandOption, U8 debug) { - I32 MessageIndex = 0, i; - U16 Crc = 0; - C8 *p; - - bzero(MessageBuffer, COMMAND_OSTM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); - - OSTMData->Header.SyncWordU16 = SYNC_WORD; - OSTMData->Header.TransmitterIdU8 = 0; - OSTMData->Header.MessageCounterU8 = 0; - OSTMData->Header.AckReqProtVerU8 = 0; - OSTMData->Header.MessageIdU16 = COMMAND_OSTM_CODE; - OSTMData->Header.MessageLengthU32 = sizeof (OSTMType) - sizeof (HeaderType); - OSTMData->StateValueIdU16 = VALUE_ID_STATE_CHANGE_REQUEST; - OSTMData->StateContentLengthU16 = sizeof (OSTMData->StateU8); - OSTMData->StateU8 = (U8) CommandOption; - - p = (C8 *) OSTMData; - for (i = 0; i < sizeof (OSTMType); i++) - *(MessageBuffer + i) = *p++; - Crc = crc_16((const C8 *)MessageBuffer, sizeof (OSTMType)); - Crc = 0; - *(MessageBuffer + i++) = (U8) (Crc >> 8); - *(MessageBuffer + i++) = (U8) (Crc); - MessageIndex = i; - - if (debug) { - // TODO: Change to log printout when byte thingy has been implemented - printf("OSTM total length = %d bytes (header+message+footer)\n", - (int)(COMMAND_OSTM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH)); - printf("----HEADER----\n"); - for (i = 0; i < sizeof (HeaderType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----MESSAGE----\n"); - for (; i < sizeof (OSTMType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----FOOTER----\n"); - for (; i < MessageIndex; i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n"); - } - - return MessageIndex; //Total number of bytes + I32 MessageIndex = 0, i; + U16 Crc = 0; + C8 *p; + + bzero(MessageBuffer, COMMAND_OSTM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); + + OSTMData->Header.SyncWordU16 = SYNC_WORD; + OSTMData->Header.TransmitterIdU8 = 0; + OSTMData->Header.MessageCounterU8 = 0; + OSTMData->Header.AckReqProtVerU8 = 0; + OSTMData->Header.MessageIdU16 = COMMAND_OSTM_CODE; + OSTMData->Header.MessageLengthU32 = sizeof (OSTMType) - sizeof (HeaderType); + OSTMData->StateValueIdU16 = VALUE_ID_STATE_CHANGE_REQUEST; + OSTMData->StateContentLengthU16 = sizeof (OSTMData->StateU8); + OSTMData->StateU8 = (U8) CommandOption; + + p = (C8 *) OSTMData; + for (i = 0; i < sizeof (OSTMType); i++) + *(MessageBuffer + i) = *p++; + Crc = crc_16((const C8 *)MessageBuffer, sizeof (OSTMType)); + Crc = 0; + *(MessageBuffer + i++) = (U8) (Crc >> 8); + *(MessageBuffer + i++) = (U8) (Crc); + MessageIndex = i; + + if (debug) { + // TODO: Change to log printout when byte thingy has been implemented + printf("OSTM total length = %d bytes (header+message+footer)\n", + (int)(COMMAND_OSTM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH)); + printf("----HEADER----\n"); + for (i = 0; i < sizeof (HeaderType); i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n----MESSAGE----\n"); + for (; i < sizeof (OSTMType); i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n----FOOTER----\n"); + for (; i < MessageIndex; i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n"); + } + + return MessageIndex; //Total number of bytes } I32 ObjectControlBuildHEABMessage(C8 * MessageBuffer, HEABType * HEABData, TimeType * GPSTime, U8 CCStatus, - U8 debug) { - I32 MessageIndex = 0, i; - U16 Crc = 0; - C8 *p; - - bzero(MessageBuffer, COMMAND_HEAB_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); - - HEABData->Header.SyncWordU16 = SYNC_WORD; - HEABData->Header.TransmitterIdU8 = 0; - HEABData->Header.MessageCounterU8 = 0; - HEABData->Header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; - HEABData->Header.MessageIdU16 = COMMAND_HEAB_CODE; - HEABData->Header.MessageLengthU32 = sizeof (HEABType) - sizeof (HeaderType); - HEABData->HeabStructValueIdU16 = VALUE_ID_HEAB_STRUCT; - HEABData->HeabStructContentLengthU16 = sizeof (HEABType) - sizeof (HeaderType) - - sizeof (HEABData->HeabStructValueIdU16) - sizeof (HEABData->HeabStructContentLengthU16); - HEABData->GPSQmsOfWeekU32 = - ((GPSTime->GPSSecondsOfWeekU32 * 1000 + (U32) TimeControlGetMillisecond(GPSTime)) << 2) + - GPSTime->MicroSecondU16; - HEABData->CCStatusU8 = CCStatus; - - if (!GPSTime->isGPSenabled) { - UtilgetCurrentGPStime(NULL, &HEABData->GPSQmsOfWeekU32); - } - - p = (C8 *) HEABData; - for (i = 0; i < sizeof (HEABType); i++) - *(MessageBuffer + i) = *p++; - Crc = crc_16((const C8 *)MessageBuffer, sizeof (HEABType)); - Crc = 0; - *(MessageBuffer + i++) = (U8) (Crc); - *(MessageBuffer + i++) = (U8) (Crc >> 8); - MessageIndex = i; - - if (debug) { - // TODO: Change to log printout when byte thingy has been implemented - printf("HEAB total length = %d bytes (header+message+footer)\n", - (int)(COMMAND_HEAB_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH)); - printf("----HEADER----\n"); - for (i = 0; i < sizeof (HeaderType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----MESSAGE----\n"); - for (; i < sizeof (HEABType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----FOOTER----\n"); - for (; i < MessageIndex; i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n"); - } - - return MessageIndex; //Total number of bytes + U8 debug) { + I32 MessageIndex = 0, i; + U16 Crc = 0; + C8 *p; + + bzero(MessageBuffer, COMMAND_HEAB_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); + + HEABData->Header.SyncWordU16 = SYNC_WORD; + HEABData->Header.TransmitterIdU8 = 0; + HEABData->Header.MessageCounterU8 = 0; + HEABData->Header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; + HEABData->Header.MessageIdU16 = COMMAND_HEAB_CODE; + HEABData->Header.MessageLengthU32 = sizeof (HEABType) - sizeof (HeaderType); + HEABData->HeabStructValueIdU16 = VALUE_ID_HEAB_STRUCT; + HEABData->HeabStructContentLengthU16 = sizeof (HEABType) - sizeof (HeaderType) + - sizeof (HEABData->HeabStructValueIdU16) - sizeof (HEABData->HeabStructContentLengthU16); + HEABData->GPSQmsOfWeekU32 = + ((GPSTime->GPSSecondsOfWeekU32 * 1000 + (U32) TimeControlGetMillisecond(GPSTime)) << 2) + + GPSTime->MicroSecondU16; + HEABData->CCStatusU8 = CCStatus; + + if (!GPSTime->isGPSenabled) { + UtilgetCurrentGPStime(NULL, &HEABData->GPSQmsOfWeekU32); + } + + p = (C8 *) HEABData; + for (i = 0; i < sizeof (HEABType); i++) + *(MessageBuffer + i) = *p++; + Crc = crc_16((const C8 *)MessageBuffer, sizeof (HEABType)); + Crc = 0; + *(MessageBuffer + i++) = (U8) (Crc); + *(MessageBuffer + i++) = (U8) (Crc >> 8); + MessageIndex = i; + + if (debug) { + // TODO: Change to log printout when byte thingy has been implemented + printf("HEAB total length = %d bytes (header+message+footer)\n", + (int)(COMMAND_HEAB_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH)); + printf("----HEADER----\n"); + for (i = 0; i < sizeof (HeaderType); i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n----MESSAGE----\n"); + for (; i < sizeof (HEABType); i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n----FOOTER----\n"); + for (; i < MessageIndex; i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n"); + } + + return MessageIndex; //Total number of bytes } int ObjectControlBuildLLCMMessage(char *MessageBuffer, unsigned short Speed, unsigned short Curvature, - unsigned char Mode, char debug) { - int MessageIndex = 0; + unsigned char Mode, char debug) { + int MessageIndex = 0; - bzero(MessageBuffer, COMMAND_LLCM_MESSAGE_LENGTH + COMMAND_MESSAGE_HEADER_LENGTH); + bzero(MessageBuffer, COMMAND_LLCM_MESSAGE_LENGTH + COMMAND_MESSAGE_HEADER_LENGTH); - UtilAddOneByteMessageData(MessageBuffer, COMMAND_CODE_INDEX, COMMAND_LLCM_CODE); + UtilAddOneByteMessageData(MessageBuffer, COMMAND_CODE_INDEX, COMMAND_LLCM_CODE); - MessageIndex = - UtilAddTwoBytesMessageData(MessageBuffer, MessageIndex + COMMAND_MESSAGE_HEADER_LENGTH, Speed); + MessageIndex = + UtilAddTwoBytesMessageData(MessageBuffer, MessageIndex + COMMAND_MESSAGE_HEADER_LENGTH, Speed); - MessageIndex = UtilAddTwoBytesMessageData(MessageBuffer, MessageIndex, Curvature); + MessageIndex = UtilAddTwoBytesMessageData(MessageBuffer, MessageIndex, Curvature); - MessageIndex = UtilAddOneByteMessageData(MessageBuffer, MessageIndex, Mode); + MessageIndex = UtilAddOneByteMessageData(MessageBuffer, MessageIndex, Mode); - UtilAddFourBytesMessageData(MessageBuffer, COMMAND_MESSAGE_LENGTH_INDEX, - (unsigned int)MessageIndex - COMMAND_MESSAGE_HEADER_LENGTH); + UtilAddFourBytesMessageData(MessageBuffer, COMMAND_MESSAGE_LENGTH_INDEX, + (unsigned int)MessageIndex - COMMAND_MESSAGE_HEADER_LENGTH); - if (debug) { - int i = 0; + if (debug) { + int i = 0; - LogMessage(LOG_LEVEL_DEBUG, "LLCM:"); - for (i = 0; i < MessageIndex; i++) - LogMessage(LOG_LEVEL_DEBUG, "[%d]= %x", i, (unsigned char)MessageBuffer[i]); - } + LogMessage(LOG_LEVEL_DEBUG, "LLCM:"); + for (i = 0; i < MessageIndex; i++) + LogMessage(LOG_LEVEL_DEBUG, "[%d]= %x", i, (unsigned char)MessageBuffer[i]); + } - return MessageIndex; //Total number of bytes = COMMAND_MESSAGE_HEADER_LENGTH + message data count + return MessageIndex; //Total number of bytes = COMMAND_MESSAGE_HEADER_LENGTH + message data count } I32 ObjectControlBuildSYPMMessage(C8 * MessageBuffer, SYPMType * SYPMData, U32 SyncPoint, U32 StopTime, - U8 debug) { - - I32 MessageIndex = 0, i; - U16 Crc = 0; - C8 *p; - - bzero(MessageBuffer, COMMAND_SYPM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); - - SYPMData->Header.SyncWordU16 = SYNC_WORD; - SYPMData->Header.TransmitterIdU8 = 0; - SYPMData->Header.MessageCounterU8 = 0; - SYPMData->Header.AckReqProtVerU8 = 0; - SYPMData->Header.MessageIdU16 = COMMAND_SYPM_CODE; - SYPMData->Header.MessageLengthU32 = sizeof (SYPMType) - sizeof (HeaderType); - SYPMData->SyncPointTimeValueIdU16 = 1; - SYPMData->SyncPointTimeContentLengthU16 = 4; - SYPMData->SyncPointTimeU32 = SyncPoint; - SYPMData->FreezeTimeValueIdU16 = 2; - SYPMData->FreezeTimeContentLengthU16 = 4; - SYPMData->FreezeTimeU32 = StopTime; - - - p = (C8 *) SYPMData; - for (i = 0; i < sizeof (SYPMType); i++) - *(MessageBuffer + i) = *p++; - Crc = crc_16((const C8 *)MessageBuffer, sizeof (SYPMType)); - Crc = 0; - *(MessageBuffer + i++) = (U8) (Crc >> 8); - *(MessageBuffer + i++) = (U8) (Crc); - MessageIndex = i; - - if (debug) { - // TODO: Change to log printout when byte thingy has been implemented - printf("SYPM total length = %d bytes (header+message+footer)\n", - (int)(COMMAND_SYPM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH)); - printf("----HEADER----\n"); - for (i = 0; i < sizeof (HeaderType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----MESSAGE----\n"); - for (; i < sizeof (SYPMType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----FOOTER----\n"); - for (; i < MessageIndex; i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n"); - } - - return MessageIndex; //Total number of bytes + U8 debug) { + + I32 MessageIndex = 0, i; + U16 Crc = 0; + C8 *p; + + bzero(MessageBuffer, COMMAND_SYPM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); + + SYPMData->Header.SyncWordU16 = SYNC_WORD; + SYPMData->Header.TransmitterIdU8 = 0; + SYPMData->Header.MessageCounterU8 = 0; + SYPMData->Header.AckReqProtVerU8 = 0; + SYPMData->Header.MessageIdU16 = COMMAND_SYPM_CODE; + SYPMData->Header.MessageLengthU32 = sizeof (SYPMType) - sizeof (HeaderType); + SYPMData->SyncPointTimeValueIdU16 = 1; + SYPMData->SyncPointTimeContentLengthU16 = 4; + SYPMData->SyncPointTimeU32 = SyncPoint; + SYPMData->FreezeTimeValueIdU16 = 2; + SYPMData->FreezeTimeContentLengthU16 = 4; + SYPMData->FreezeTimeU32 = StopTime; + + + p = (C8 *) SYPMData; + for (i = 0; i < sizeof (SYPMType); i++) + *(MessageBuffer + i) = *p++; + Crc = crc_16((const C8 *)MessageBuffer, sizeof (SYPMType)); + Crc = 0; + *(MessageBuffer + i++) = (U8) (Crc >> 8); + *(MessageBuffer + i++) = (U8) (Crc); + MessageIndex = i; + + if (debug) { + // TODO: Change to log printout when byte thingy has been implemented + printf("SYPM total length = %d bytes (header+message+footer)\n", + (int)(COMMAND_SYPM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH)); + printf("----HEADER----\n"); + for (i = 0; i < sizeof (HeaderType); i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n----MESSAGE----\n"); + for (; i < sizeof (SYPMType); i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n----FOOTER----\n"); + for (; i < MessageIndex; i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n"); + } + + return MessageIndex; //Total number of bytes } I32 ObjectControlBuildMTSPMessage(C8 * MessageBuffer, MTSPType * MTSPData, U32 SyncTimestamp, U8 debug) { - I32 MessageIndex = 0, i; - U16 Crc = 0; - C8 *p; - - bzero(MessageBuffer, COMMAND_MTSP_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); - - MTSPData->Header.SyncWordU16 = SYNC_WORD; - MTSPData->Header.TransmitterIdU8 = 0; - MTSPData->Header.MessageCounterU8 = 0; - MTSPData->Header.AckReqProtVerU8 = 0; - MTSPData->Header.MessageIdU16 = COMMAND_MTSP_CODE; - MTSPData->Header.MessageLengthU32 = sizeof (MTSPType) - sizeof (HeaderType); - MTSPData->EstSyncPointTimeValueIdU16 = 1; - MTSPData->EstSyncPointTimeContentLengthU16 = 4; - MTSPData->EstSyncPointTimeU32 = SyncTimestamp; - - - p = (C8 *) MTSPData; - for (i = 0; i < sizeof (MTSPType); i++) - *(MessageBuffer + i) = *p++; - Crc = crc_16((const C8 *)MessageBuffer, sizeof (MTSPType)); - Crc = 0; - *(MessageBuffer + i++) = (U8) (Crc >> 8); - *(MessageBuffer + i++) = (U8) (Crc); - MessageIndex = i; - - if (debug) { - // TODO: Change to log printout when byte thingy has been implemented - printf("MTSPData total length = %d bytes (header+message+footer)\n", - (int)(COMMAND_MTSP_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH)); - printf("----HEADER----\n"); - for (i = 0; i < sizeof (HeaderType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----MESSAGE----\n"); - for (; i < sizeof (MTSPType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----FOOTER----\n"); - for (; i < MessageIndex; i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n"); - } - - return MessageIndex; //Total number of bytes + I32 MessageIndex = 0, i; + U16 Crc = 0; + C8 *p; + + bzero(MessageBuffer, COMMAND_MTSP_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); + + MTSPData->Header.SyncWordU16 = SYNC_WORD; + MTSPData->Header.TransmitterIdU8 = 0; + MTSPData->Header.MessageCounterU8 = 0; + MTSPData->Header.AckReqProtVerU8 = 0; + MTSPData->Header.MessageIdU16 = COMMAND_MTSP_CODE; + MTSPData->Header.MessageLengthU32 = sizeof (MTSPType) - sizeof (HeaderType); + MTSPData->EstSyncPointTimeValueIdU16 = 1; + MTSPData->EstSyncPointTimeContentLengthU16 = 4; + MTSPData->EstSyncPointTimeU32 = SyncTimestamp; + + + p = (C8 *) MTSPData; + for (i = 0; i < sizeof (MTSPType); i++) + *(MessageBuffer + i) = *p++; + Crc = crc_16((const C8 *)MessageBuffer, sizeof (MTSPType)); + Crc = 0; + *(MessageBuffer + i++) = (U8) (Crc >> 8); + *(MessageBuffer + i++) = (U8) (Crc); + MessageIndex = i; + + if (debug) { + // TODO: Change to log printout when byte thingy has been implemented + printf("MTSPData total length = %d bytes (header+message+footer)\n", + (int)(COMMAND_MTSP_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH)); + printf("----HEADER----\n"); + for (i = 0; i < sizeof (HeaderType); i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n----MESSAGE----\n"); + for (; i < sizeof (MTSPType); i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n----FOOTER----\n"); + for (; i < MessageIndex; i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n"); + } + + return MessageIndex; //Total number of bytes } I32 ObjectControlBuildTRAJMessageHeader(C8 * MessageBuffer, I32 * RowCount, HeaderType * HeaderData, - TRAJInfoType * TRAJInfoData, C8 * TrajFileHeader, U8 debug) { - I32 MessageIndex = 0, i, j; - U16 Crc = 0; - C8 *p; - C8 *token; - - - if (strlen(TrajFileHeader) >= 1) { - j = 0; - token = strtok(TrajFileHeader, ";"); - while (token != NULL) { - if (j == 1) { - TRAJInfoData->TrajectoryIDValueIdU16 = VALUE_ID_TRAJECTORY_ID; - TRAJInfoData->TrajectoryIDContentLengthU16 = 2; - TRAJInfoData->TrajectoryIDU16 = atoi(token); - } - else if (j == 2) { - TRAJInfoData->TrajectoryNameValueIdU16 = VALUE_ID_TRAJECTORY_NAME; - TRAJInfoData->TrajectoryNameContentLengthU16 = 64; - bzero(TRAJInfoData->TrajectoryNameC8, 64); - strncpy(TRAJInfoData->TrajectoryNameC8, token, strlen(token)); - } - else if (j == 3) { - TRAJInfoData->TrajectoryVersionValueIdU16 = VALUE_ID_TRAJECTORY_VERSION; - TRAJInfoData->TrajectoryVersionContentLengthU16 = 2; - TRAJInfoData->TrajectoryVersionU16 = atoi(token); - } - else if (j == 4) { - *RowCount = atoi(token); - } - - j++; - token = strtok(NULL, ";"); - } - } - - TRAJInfoData->IpAddressValueIdU16 = 0xA000; - TRAJInfoData->IpAddressContentLengthU16 = 4; - TRAJInfoData->IpAddressU32 = 0; - - bzero(MessageBuffer, COMMAND_MESSAGE_HEADER_LENGTH + COMMAND_TRAJ_INFO_ROW_MESSAGE_LENGTH); - - HeaderData->SyncWordU16 = SYNC_WORD; - HeaderData->TransmitterIdU8 = 0; - HeaderData->MessageCounterU8 = 0; - HeaderData->AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; - HeaderData->MessageIdU16 = COMMAND_DOTM_CODE; - HeaderData->MessageLengthU32 = - *RowCount * COMMAND_DOTM_ROW_MESSAGE_LENGTH + COMMAND_TRAJ_INFO_ROW_MESSAGE_LENGTH; - - p = (C8 *) HeaderData; - for (i = 0; i < COMMAND_MESSAGE_HEADER_LENGTH; i++) - *(MessageBuffer + i) = *p++; - - p = (C8 *) TRAJInfoData; - for (; i < COMMAND_MESSAGE_HEADER_LENGTH + COMMAND_TRAJ_INFO_ROW_MESSAGE_LENGTH; i++) - *(MessageBuffer + i) = *p++; - - MessageIndex = i; - - - if (debug) { - // TODO: Change to log printout when byte thingy has been implemented - printf("Header + TRAJInfo total length = %d bytes\n", - (int)(COMMAND_MESSAGE_HEADER_LENGTH + COMMAND_TRAJ_INFO_ROW_MESSAGE_LENGTH)); - printf("----HEADER + TRAJInfo----\n"); - for (i = 0; i < sizeof (HeaderType) + sizeof (TRAJInfoType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n"); - printf("DOTM message total length = %d bytes.\n", (int)HeaderData->MessageLengthU32); - printf("Traj file header = %s\n", TrajFileHeader); - printf("TrajectoryID = %d\n", TRAJInfoData->TrajectoryIDU16); - printf("TrajectoryName = %s\n", TRAJInfoData->TrajectoryNameC8); - printf("TrajectoryVersion = %d\n", TRAJInfoData->TrajectoryVersionU16); - printf("RowCount = %d\n", *RowCount); - printf("IpAddress = %d\n", TRAJInfoData->IpAddressU32); - - printf("\n----MESSAGE----\n"); - } - - return MessageIndex; //Total number of bytes = COMMAND_MESSAGE_HEADER_LENGTH + TRAJInfoType * TRAJInfoData, C8 * TrajFileHeader, U8 debug) { + I32 MessageIndex = 0, i, j; + U16 Crc = 0; + C8 *p; + C8 *token; + + + if (strlen(TrajFileHeader) >= 1) { + j = 0; + token = strtok(TrajFileHeader, ";"); + while (token != NULL) { + if (j == 1) { + TRAJInfoData->TrajectoryIDValueIdU16 = VALUE_ID_TRAJECTORY_ID; + TRAJInfoData->TrajectoryIDContentLengthU16 = 2; + TRAJInfoData->TrajectoryIDU16 = atoi(token); + } + else if (j == 2) { + TRAJInfoData->TrajectoryNameValueIdU16 = VALUE_ID_TRAJECTORY_NAME; + TRAJInfoData->TrajectoryNameContentLengthU16 = 64; + bzero(TRAJInfoData->TrajectoryNameC8, 64); + strncpy(TRAJInfoData->TrajectoryNameC8, token, strlen(token)); + } + else if (j == 3) { + TRAJInfoData->TrajectoryVersionValueIdU16 = VALUE_ID_TRAJECTORY_VERSION; + TRAJInfoData->TrajectoryVersionContentLengthU16 = 2; + TRAJInfoData->TrajectoryVersionU16 = atoi(token); + } + else if (j == 4) { + *RowCount = atoi(token); + } + + j++; + token = strtok(NULL, ";"); + } + } + + TRAJInfoData->IpAddressValueIdU16 = 0xA000; + TRAJInfoData->IpAddressContentLengthU16 = 4; + TRAJInfoData->IpAddressU32 = 0; + + bzero(MessageBuffer, COMMAND_MESSAGE_HEADER_LENGTH + COMMAND_TRAJ_INFO_ROW_MESSAGE_LENGTH); + + HeaderData->SyncWordU16 = SYNC_WORD; + HeaderData->TransmitterIdU8 = 0; + HeaderData->MessageCounterU8 = 0; + HeaderData->AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; + HeaderData->MessageIdU16 = COMMAND_DOTM_CODE; + HeaderData->MessageLengthU32 = + *RowCount * COMMAND_DOTM_ROW_MESSAGE_LENGTH + COMMAND_TRAJ_INFO_ROW_MESSAGE_LENGTH; + + p = (C8 *) HeaderData; + for (i = 0; i < COMMAND_MESSAGE_HEADER_LENGTH; i++) + *(MessageBuffer + i) = *p++; + + p = (C8 *) TRAJInfoData; + for (; i < COMMAND_MESSAGE_HEADER_LENGTH + COMMAND_TRAJ_INFO_ROW_MESSAGE_LENGTH; i++) + *(MessageBuffer + i) = *p++; + + MessageIndex = i; + + + if (debug) { + // TODO: Change to log printout when byte thingy has been implemented + printf("Header + TRAJInfo total length = %d bytes\n", + (int)(COMMAND_MESSAGE_HEADER_LENGTH + COMMAND_TRAJ_INFO_ROW_MESSAGE_LENGTH)); + printf("----HEADER + TRAJInfo----\n"); + for (i = 0; i < sizeof (HeaderType) + sizeof (TRAJInfoType); i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n"); + printf("DOTM message total length = %d bytes.\n", (int)HeaderData->MessageLengthU32); + printf("Traj file header = %s\n", TrajFileHeader); + printf("TrajectoryID = %d\n", TRAJInfoData->TrajectoryIDU16); + printf("TrajectoryName = %s\n", TRAJInfoData->TrajectoryNameC8); + printf("TrajectoryVersion = %d\n", TRAJInfoData->TrajectoryVersionU16); + printf("RowCount = %d\n", *RowCount); + printf("IpAddress = %d\n", TRAJInfoData->IpAddressU32); + + printf("\n----MESSAGE----\n"); + } + + return MessageIndex; //Total number of bytes = COMMAND_MESSAGE_HEADER_LENGTH } I32 ObjectControlSendTRAJMessage(C8 * Filename, I32 * Socket, I32 RowCount, C8 * IP, U32 Port, - DOTMType * DOTMData, U8 debug) { - FILE *fd; - - // Save socket settings and set it to blocking - int retval = fcntl(*Socket, F_GETFL); - - if (retval < 0) { - LogMessage(LOG_LEVEL_ERROR, "Error getting socket options with fcntl"); - return -1; - } - int socketOptions = retval; - - retval = fcntl(*Socket, F_SETFL, socketOptions & ~O_NONBLOCK); - if (retval < 0) { - LogMessage(LOG_LEVEL_ERROR, "Error setting socket options with fcntl"); - return -1; - } - - - fd = fopen(Filename, "r"); - if (fd == NULL) { - LogMessage(LOG_LEVEL_ERROR, "Unable to open file <%s>", Filename); - return -1; - } - - UtilReadLineCntSpecChars(fd, TrajBuffer); //Read first line - int Rest = 0, i = 0, MessageLength = 0, SumMessageLength = 0, Modulo = 0, Transmissions = 0; - - Transmissions = RowCount / COMMAND_DOTM_ROWS_IN_TRANSMISSION; - Rest = RowCount % COMMAND_DOTM_ROWS_IN_TRANSMISSION; - U16 CrcU16 = 0; - - - for (i = 0; i < Transmissions; i++) { - MessageLength = - ObjectControlBuildTRAJMessage(TrajBuffer, fd, COMMAND_DOTM_ROWS_IN_TRANSMISSION, DOTMData, debug); - - if (i == Transmissions && Rest == 0) { - TrajBuffer[MessageLength] = (U8) (CrcU16); - TrajBuffer[MessageLength + 1] = (U8) (CrcU16 >> 8); - MessageLength = MessageLength + 2; - UtilSendTCPData("Object Control", TrajBuffer, MessageLength, Socket, 0); - SumMessageLength = SumMessageLength + MessageLength; - } - else { - UtilSendTCPData("Object Control", TrajBuffer, MessageLength, Socket, 0); - SumMessageLength = SumMessageLength + MessageLength; - } - - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "Transmission %d: %d bytes sent", i, MessageLength); - } - - if (Rest > 0) { - MessageLength = ObjectControlBuildTRAJMessage(TrajBuffer, fd, Rest, DOTMData, debug); - TrajBuffer[MessageLength] = (U8) (CrcU16); - TrajBuffer[MessageLength + 1] = (U8) (CrcU16 >> 8); - MessageLength = MessageLength + 2; - UtilSendTCPData("Object Control", TrajBuffer, MessageLength, Socket, 0); - SumMessageLength = SumMessageLength + MessageLength; - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "Transmission %d: %d bytes sent.\n", i, MessageLength); - } - - LogMessage(LOG_LEVEL_INFO, "%d DOTM bytes sent to %s:%d", SumMessageLength, IP, Port); - fclose(fd); - - // Reset socket settings - retval = fcntl(*Socket, F_SETFL, socketOptions); - if (retval < 0) { - LogMessage(LOG_LEVEL_ERROR, "Error setting socket options with fcntl"); - return -1; - } - - - return 0; + DOTMType * DOTMData, U8 debug) { + FILE *fd; + + // Save socket settings and set it to blocking + int retval = fcntl(*Socket, F_GETFL); + + if (retval < 0) { + LogMessage(LOG_LEVEL_ERROR, "Error getting socket options with fcntl"); + return -1; + } + int socketOptions = retval; + + retval = fcntl(*Socket, F_SETFL, socketOptions & ~O_NONBLOCK); + if (retval < 0) { + LogMessage(LOG_LEVEL_ERROR, "Error setting socket options with fcntl"); + return -1; + } + + + fd = fopen(Filename, "r"); + if (fd == NULL) { + LogMessage(LOG_LEVEL_ERROR, "Unable to open file <%s>", Filename); + return -1; + } + + UtilReadLineCntSpecChars(fd, TrajBuffer); //Read first line + int Rest = 0, i = 0, MessageLength = 0, SumMessageLength = 0, Modulo = 0, Transmissions = 0; + + Transmissions = RowCount / COMMAND_DOTM_ROWS_IN_TRANSMISSION; + Rest = RowCount % COMMAND_DOTM_ROWS_IN_TRANSMISSION; + U16 CrcU16 = 0; + + + for (i = 0; i < Transmissions; i++) { + MessageLength = + ObjectControlBuildTRAJMessage(TrajBuffer, fd, COMMAND_DOTM_ROWS_IN_TRANSMISSION, DOTMData, debug); + + if (i == Transmissions && Rest == 0) { + TrajBuffer[MessageLength] = (U8) (CrcU16); + TrajBuffer[MessageLength + 1] = (U8) (CrcU16 >> 8); + MessageLength = MessageLength + 2; + UtilSendTCPData("Object Control", TrajBuffer, MessageLength, Socket, 0); + SumMessageLength = SumMessageLength + MessageLength; + } + else { + UtilSendTCPData("Object Control", TrajBuffer, MessageLength, Socket, 0); + SumMessageLength = SumMessageLength + MessageLength; + } + + if (debug) + LogMessage(LOG_LEVEL_DEBUG, "Transmission %d: %d bytes sent", i, MessageLength); + } + + if (Rest > 0) { + MessageLength = ObjectControlBuildTRAJMessage(TrajBuffer, fd, Rest, DOTMData, debug); + TrajBuffer[MessageLength] = (U8) (CrcU16); + TrajBuffer[MessageLength + 1] = (U8) (CrcU16 >> 8); + MessageLength = MessageLength + 2; + UtilSendTCPData("Object Control", TrajBuffer, MessageLength, Socket, 0); + SumMessageLength = SumMessageLength + MessageLength; + if (debug) + LogMessage(LOG_LEVEL_DEBUG, "Transmission %d: %d bytes sent.\n", i, MessageLength); + } + + LogMessage(LOG_LEVEL_INFO, "%d DOTM bytes sent to %s:%d", SumMessageLength, IP, Port); + fclose(fd); + + // Reset socket settings + retval = fcntl(*Socket, F_SETFL, socketOptions); + if (retval < 0) { + LogMessage(LOG_LEVEL_ERROR, "Error setting socket options with fcntl"); + return -1; + } + + + return 0; } I32 ObjectControlBuildTRAJMessage(C8 * MessageBuffer, FILE * fd, I32 RowCount, DOTMType * DOTMData, U8 debug) { - I32 MessageIndex = 0; - C8 RowBuffer[100]; - C8 DataBuffer[20]; - dbl Data; - C8 *src, *p; - U16 Crc = 0; - flt curv = 0; - C8 *pc; - - bzero(MessageBuffer, COMMAND_DOTM_ROW_MESSAGE_LENGTH * RowCount); - - I32 i = 0, j = 0, n = 0; - - for (i = 0; i < RowCount; i++) { - bzero(RowBuffer, 100); - UtilReadLineCntSpecChars(fd, RowBuffer); - - //Read to ';' in row = LINE;0.00;21.239000;39.045000;0.000000;-1.240001;0.029103;0.004005;0.000000;3;ENDLINE; - //Time - src = strchr(RowBuffer, ';'); - bzero(DataBuffer, 20); - strncpy(DataBuffer, src + 1, (U64) strchr(src + 1, ';') - (U64) src - 1); - Data = atof(DataBuffer) * 1e3; - DOTMData->RelativeTimeValueIdU16 = VALUE_ID_RELATIVE_TIME; - DOTMData->RelativeTimeContentLengthU16 = 4; - DOTMData->RelativeTimeU32 = (U32) Data; - if (debug) - printf("Time DataBuffer=%s float=%3.6f\n", DataBuffer, Data); - - //x - src = strchr(src + 1, ';'); - bzero(DataBuffer, 20); - strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); - Data = atof(DataBuffer) * 1e3; - DOTMData->XPositionValueIdU16 = VALUE_ID_X_POSITION; - DOTMData->XPositionContentLengthU16 = 4; - DOTMData->XPositionI32 = (I32) Data; - if (debug) - printf("X DataBuffer=%s float=%3.6f\n", DataBuffer, Data); - - //y - src = strchr(src + 1, ';'); - bzero(DataBuffer, 20); - strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); - Data = atof(DataBuffer) * 1e3; - DOTMData->YPositionValueIdU16 = VALUE_ID_Y_POSITION; - DOTMData->YPositionContentLengthU16 = 4; - DOTMData->YPositionI32 = (I32) Data; - if (debug) - printf("Y DataBuffer=%s float=%3.6f\n", DataBuffer, Data); - - //z - src = strchr(src + 1, ';'); - bzero(DataBuffer, 20); - strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); - Data = atof(DataBuffer) * 1e3; - DOTMData->ZPositionValueIdU16 = VALUE_ID_Z_POSITION; - DOTMData->ZPositionContentLengthU16 = 4; - DOTMData->ZPositionI32 = (I32) Data; - if (debug) - printf("Z DataBuffer=%s float=%3.6f\n", DataBuffer, Data); - - //Heading - src = strchr(src + 1, ';'); - bzero(DataBuffer, 20); - strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); - Data = UtilRadToDeg(atof(DataBuffer)); - Data = 450 - Data; //Turn heading back pi/2 - while (Data < 0) - Data += 360.0; - while (Data > 360) - Data -= 360.0; - DOTMData->HeadingValueIdU16 = VALUE_ID_HEADING; - DOTMData->HeadingContentLengthU16 = 2; - DOTMData->HeadingU16 = (U16) (Data * 1e2); - if (debug) - printf("Heading DataBuffer=%s float=%3.6f\n", DataBuffer, Data); - - //Longitudinal speed - src = strchr(src + 1, ';'); - bzero(DataBuffer, 20); - strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); - Data = atof(DataBuffer) * 1e2; - DOTMData->LongitudinalSpeedValueIdU16 = VALUE_ID_LONGITUDINAL_SPEED; - DOTMData->LongitudinalSpeedContentLengthU16 = 2; - DOTMData->LongitudinalSpeedI16 = (I16) Data; - if (debug) - printf("Long speed DataBuffer=%s float=%3.6f\n", DataBuffer, Data); - - //Lateral speed - src = strchr(src + 1, ';'); - bzero(DataBuffer, 20); - strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); - Data = atof(DataBuffer) * 1e2; - DOTMData->LateralSpeedValueIdU16 = VALUE_ID_LATERAL_SPEED; - DOTMData->LateralSpeedContentLengthU16 = 2; - DOTMData->LateralSpeedI16 = (I16) Data; - if (debug) - printf("Lat speed DataBuffer=%s float=%3.6f\n", DataBuffer, Data); - - //Longitudinal acceleration - src = strchr(src + 1, ';'); - bzero(DataBuffer, 20); - strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); - Data = atof(DataBuffer) * 1e3; - DOTMData->LongitudinalAccValueIdU16 = VALUE_ID_LONGITUDINAL_ACCELERATION; - DOTMData->LongitudinalAccContentLengthU16 = 2; - DOTMData->LongitudinalAccI16 = (I16) Data; - if (debug) - printf("Long acc DataBuffer=%s float=%3.6f\n", DataBuffer, Data); - - //Lateral acceleration - src = strchr(src + 1, ';'); - bzero(DataBuffer, 20); - strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); - Data = atof(DataBuffer) * 1e3; - DOTMData->LateralAccValueIdU16 = VALUE_ID_LATERAL_ACCELERATION; - DOTMData->LateralAccContentLengthU16 = 2; - DOTMData->LateralAccI16 = (I16) Data; - if (debug) - printf("Lat accDataBuffer=%s float=%3.6f\n", DataBuffer, Data); - - //Curvature - src = strchr(src + 1, ';'); - bzero(DataBuffer, 20); - strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); - //Data = atof(DataBuffer) * 3e4; - curv = atof(DataBuffer); - pc = (C8 *) & curv; - DOTMData->CurvatureValueIdU16 = VALUE_ID_CURVATURE; - DOTMData->CurvatureContentLengthU16 = 4; - //DOTMData->CurvatureI32 = (I32) Data; - DOTMData->CurvatureI32 = pc[0]; - DOTMData->CurvatureI32 = DOTMData->CurvatureI32 | ((I32) pc[1]) << 8; - DOTMData->CurvatureI32 = DOTMData->CurvatureI32 | ((I32) pc[2]) << 16; - DOTMData->CurvatureI32 = DOTMData->CurvatureI32 | ((I32) pc[3]) << 24; - - if (debug) - printf("Curv DataBuffer=%s float=%3.6f\n", DataBuffer, Data); - - p = (C8 *) DOTMData; - for (j = 0; j < sizeof (DOTMType); j++, n++) - *(MessageBuffer + n) = *p++; - MessageIndex = n; - } - - - if (debug) { - int i = 0; - - for (i = 0; i < MessageIndex; i++) { - // TODO: Write to log when bytes thingy has been implemented - if ((unsigned char)MessageBuffer[i] >= 0 && (unsigned char)MessageBuffer[i] <= 15) - printf("0"); - printf("%x-", (unsigned char)MessageBuffer[i]); - } - printf("\n"); - } - - return MessageIndex; //Total number of bytes + I32 MessageIndex = 0; + C8 RowBuffer[100]; + C8 DataBuffer[20]; + dbl Data; + C8 *src, *p; + U16 Crc = 0; + flt curv = 0; + C8 *pc; + + bzero(MessageBuffer, COMMAND_DOTM_ROW_MESSAGE_LENGTH * RowCount); + + I32 i = 0, j = 0, n = 0; + + for (i = 0; i < RowCount; i++) { + bzero(RowBuffer, 100); + UtilReadLineCntSpecChars(fd, RowBuffer); + + //Read to ';' in row = LINE;0.00;21.239000;39.045000;0.000000;-1.240001;0.029103;0.004005;0.000000;3;ENDLINE; + //Time + src = strchr(RowBuffer, ';'); + bzero(DataBuffer, 20); + strncpy(DataBuffer, src + 1, (U64) strchr(src + 1, ';') - (U64) src - 1); + Data = atof(DataBuffer) * 1e3; + DOTMData->RelativeTimeValueIdU16 = VALUE_ID_RELATIVE_TIME; + DOTMData->RelativeTimeContentLengthU16 = 4; + DOTMData->RelativeTimeU32 = (U32) Data; + if (debug) + printf("Time DataBuffer=%s float=%3.6f\n", DataBuffer, Data); + + //x + src = strchr(src + 1, ';'); + bzero(DataBuffer, 20); + strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); + Data = atof(DataBuffer) * 1e3; + DOTMData->XPositionValueIdU16 = VALUE_ID_X_POSITION; + DOTMData->XPositionContentLengthU16 = 4; + DOTMData->XPositionI32 = (I32) Data; + if (debug) + printf("X DataBuffer=%s float=%3.6f\n", DataBuffer, Data); + + //y + src = strchr(src + 1, ';'); + bzero(DataBuffer, 20); + strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); + Data = atof(DataBuffer) * 1e3; + DOTMData->YPositionValueIdU16 = VALUE_ID_Y_POSITION; + DOTMData->YPositionContentLengthU16 = 4; + DOTMData->YPositionI32 = (I32) Data; + if (debug) + printf("Y DataBuffer=%s float=%3.6f\n", DataBuffer, Data); + + //z + src = strchr(src + 1, ';'); + bzero(DataBuffer, 20); + strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); + Data = atof(DataBuffer) * 1e3; + DOTMData->ZPositionValueIdU16 = VALUE_ID_Z_POSITION; + DOTMData->ZPositionContentLengthU16 = 4; + DOTMData->ZPositionI32 = (I32) Data; + if (debug) + printf("Z DataBuffer=%s float=%3.6f\n", DataBuffer, Data); + + //Heading + src = strchr(src + 1, ';'); + bzero(DataBuffer, 20); + strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); + Data = UtilRadToDeg(atof(DataBuffer)); + Data = 450 - Data; //Turn heading back pi/2 + while (Data < 0) + Data += 360.0; + while (Data > 360) + Data -= 360.0; + DOTMData->HeadingValueIdU16 = VALUE_ID_HEADING; + DOTMData->HeadingContentLengthU16 = 2; + DOTMData->HeadingU16 = (U16) (Data * 1e2); + if (debug) + printf("Heading DataBuffer=%s float=%3.6f\n", DataBuffer, Data); + + //Longitudinal speed + src = strchr(src + 1, ';'); + bzero(DataBuffer, 20); + strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); + Data = atof(DataBuffer) * 1e2; + DOTMData->LongitudinalSpeedValueIdU16 = VALUE_ID_LONGITUDINAL_SPEED; + DOTMData->LongitudinalSpeedContentLengthU16 = 2; + DOTMData->LongitudinalSpeedI16 = (I16) Data; + if (debug) + printf("Long speed DataBuffer=%s float=%3.6f\n", DataBuffer, Data); + + //Lateral speed + src = strchr(src + 1, ';'); + bzero(DataBuffer, 20); + strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); + Data = atof(DataBuffer) * 1e2; + DOTMData->LateralSpeedValueIdU16 = VALUE_ID_LATERAL_SPEED; + DOTMData->LateralSpeedContentLengthU16 = 2; + DOTMData->LateralSpeedI16 = (I16) Data; + if (debug) + printf("Lat speed DataBuffer=%s float=%3.6f\n", DataBuffer, Data); + + //Longitudinal acceleration + src = strchr(src + 1, ';'); + bzero(DataBuffer, 20); + strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); + Data = atof(DataBuffer) * 1e3; + DOTMData->LongitudinalAccValueIdU16 = VALUE_ID_LONGITUDINAL_ACCELERATION; + DOTMData->LongitudinalAccContentLengthU16 = 2; + DOTMData->LongitudinalAccI16 = (I16) Data; + if (debug) + printf("Long acc DataBuffer=%s float=%3.6f\n", DataBuffer, Data); + + //Lateral acceleration + src = strchr(src + 1, ';'); + bzero(DataBuffer, 20); + strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); + Data = atof(DataBuffer) * 1e3; + DOTMData->LateralAccValueIdU16 = VALUE_ID_LATERAL_ACCELERATION; + DOTMData->LateralAccContentLengthU16 = 2; + DOTMData->LateralAccI16 = (I16) Data; + if (debug) + printf("Lat accDataBuffer=%s float=%3.6f\n", DataBuffer, Data); + + //Curvature + src = strchr(src + 1, ';'); + bzero(DataBuffer, 20); + strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); + //Data = atof(DataBuffer) * 3e4; + curv = atof(DataBuffer); + pc = (C8 *) & curv; + DOTMData->CurvatureValueIdU16 = VALUE_ID_CURVATURE; + DOTMData->CurvatureContentLengthU16 = 4; + //DOTMData->CurvatureI32 = (I32) Data; + DOTMData->CurvatureI32 = pc[0]; + DOTMData->CurvatureI32 = DOTMData->CurvatureI32 | ((I32) pc[1]) << 8; + DOTMData->CurvatureI32 = DOTMData->CurvatureI32 | ((I32) pc[2]) << 16; + DOTMData->CurvatureI32 = DOTMData->CurvatureI32 | ((I32) pc[3]) << 24; + + if (debug) + printf("Curv DataBuffer=%s float=%3.6f\n", DataBuffer, Data); + + p = (C8 *) DOTMData; + for (j = 0; j < sizeof (DOTMType); j++, n++) + *(MessageBuffer + n) = *p++; + MessageIndex = n; + } + + + if (debug) { + int i = 0; + + for (i = 0; i < MessageIndex; i++) { + // TODO: Write to log when bytes thingy has been implemented + if ((unsigned char)MessageBuffer[i] >= 0 && (unsigned char)MessageBuffer[i] <= 15) + printf("0"); + printf("%x-", (unsigned char)MessageBuffer[i]); + } + printf("\n"); + } + + return MessageIndex; //Total number of bytes } I32 ObjectControlBuildASPMessage(C8 * MessageBuffer, ASPType * ASPData, U8 debug) { - I32 MessageIndex = 0, i; - C8 *p; - - bzero(MessageBuffer, ASP_MESSAGE_LENGTH); - p = (C8 *) ASPData; - for (i = 0; i < sizeof (ASPType); i++) - *(MessageBuffer + i) = *p++; - MessageIndex = i; - - if (debug) { - // TODO: Write to log when bytes thingy has been implemented - printf("ASP total length = %d bytes \n", (int)(ASP_MESSAGE_LENGTH)); - printf("\n----MESSAGE----\n"); - for (i = 0; i < sizeof (ASPType); i++) - printf("%x ", (C8) MessageBuffer[i]); - printf("\n"); - } - - return MessageIndex; //Total number of bytes + I32 MessageIndex = 0, i; + C8 *p; + + bzero(MessageBuffer, ASP_MESSAGE_LENGTH); + p = (C8 *) ASPData; + for (i = 0; i < sizeof (ASPType); i++) + *(MessageBuffer + i) = *p++; + MessageIndex = i; + + if (debug) { + // TODO: Write to log when bytes thingy has been implemented + printf("ASP total length = %d bytes \n", (int)(ASP_MESSAGE_LENGTH)); + printf("\n----MESSAGE----\n"); + for (i = 0; i < sizeof (ASPType); i++) + printf("%x ", (C8) MessageBuffer[i]); + printf("\n"); + } + + return MessageIndex; //Total number of bytes } @@ -2264,100 +2280,100 @@ I32 ObjectControlBuildASPMessage(C8 * MessageBuffer, ASPType * ASPData, U8 debug * \return Length of sent message */ I32 ObjectControlSendACCMMessage(ACCMData * ACCM, I32 * socket, U8 debug) { - ACCMType isoACCM; - C8 messageBuffer[sizeof (isoACCM)]; - C8 *ptr = messageBuffer; - U32 messageSize = 0; + ACCMType isoACCM; + C8 messageBuffer[sizeof (isoACCM)]; + C8 *ptr = messageBuffer; + U32 messageSize = 0; - ObjectControlBuildACCMMessage(ACCM, &isoACCM, debug); + ObjectControlBuildACCMMessage(ACCM, &isoACCM, debug); - // Copy ACCM header to send buffer - memcpy(ptr, &isoACCM.header.SyncWordU16, sizeof (isoACCM.header.SyncWordU16)); - ptr += sizeof (isoACCM.header.SyncWordU16); + // Copy ACCM header to send buffer + memcpy(ptr, &isoACCM.header.SyncWordU16, sizeof (isoACCM.header.SyncWordU16)); + ptr += sizeof (isoACCM.header.SyncWordU16); - memcpy(ptr, &isoACCM.header.TransmitterIdU8, sizeof (isoACCM.header.TransmitterIdU8)); - ptr += sizeof (isoACCM.header.TransmitterIdU8); + memcpy(ptr, &isoACCM.header.TransmitterIdU8, sizeof (isoACCM.header.TransmitterIdU8)); + ptr += sizeof (isoACCM.header.TransmitterIdU8); - memcpy(ptr, &isoACCM.header.MessageCounterU8, sizeof (isoACCM.header.MessageCounterU8)); - ptr += sizeof (isoACCM.header.MessageCounterU8); + memcpy(ptr, &isoACCM.header.MessageCounterU8, sizeof (isoACCM.header.MessageCounterU8)); + ptr += sizeof (isoACCM.header.MessageCounterU8); - memcpy(ptr, &isoACCM.header.AckReqProtVerU8, sizeof (isoACCM.header.AckReqProtVerU8)); - ptr += sizeof (isoACCM.header.AckReqProtVerU8); + memcpy(ptr, &isoACCM.header.AckReqProtVerU8, sizeof (isoACCM.header.AckReqProtVerU8)); + ptr += sizeof (isoACCM.header.AckReqProtVerU8); - memcpy(ptr, &isoACCM.header.MessageIdU16, sizeof (isoACCM.header.MessageIdU16)); - ptr += sizeof (isoACCM.header.MessageIdU16); + memcpy(ptr, &isoACCM.header.MessageIdU16, sizeof (isoACCM.header.MessageIdU16)); + ptr += sizeof (isoACCM.header.MessageIdU16); - memcpy(ptr, &isoACCM.header.MessageLengthU32, sizeof (isoACCM.header.MessageLengthU32)); - ptr += sizeof (isoACCM.header.MessageLengthU32); + memcpy(ptr, &isoACCM.header.MessageLengthU32, sizeof (isoACCM.header.MessageLengthU32)); + ptr += sizeof (isoACCM.header.MessageLengthU32); - // Copy ACCM action ID to send buffer - memcpy(ptr, &isoACCM.actionIDValueID, sizeof (isoACCM.actionIDValueID)); - ptr += sizeof (isoACCM.actionIDValueID); + // Copy ACCM action ID to send buffer + memcpy(ptr, &isoACCM.actionIDValueID, sizeof (isoACCM.actionIDValueID)); + ptr += sizeof (isoACCM.actionIDValueID); - memcpy(ptr, &isoACCM.actionIDContentLength, sizeof (isoACCM.actionIDContentLength)); - ptr += sizeof (isoACCM.actionIDContentLength); + memcpy(ptr, &isoACCM.actionIDContentLength, sizeof (isoACCM.actionIDContentLength)); + ptr += sizeof (isoACCM.actionIDContentLength); - memcpy(ptr, &isoACCM.actionID, sizeof (isoACCM.actionID)); - ptr += sizeof (isoACCM.actionID); + memcpy(ptr, &isoACCM.actionID, sizeof (isoACCM.actionID)); + ptr += sizeof (isoACCM.actionID); - // Copy ACCM action type to send buffer - memcpy(ptr, &isoACCM.actionTypeValueID, sizeof (isoACCM.actionTypeValueID)); - ptr += sizeof (isoACCM.actionTypeValueID); + // Copy ACCM action type to send buffer + memcpy(ptr, &isoACCM.actionTypeValueID, sizeof (isoACCM.actionTypeValueID)); + ptr += sizeof (isoACCM.actionTypeValueID); - memcpy(ptr, &isoACCM.actionTypeContentLength, sizeof (isoACCM.actionTypeContentLength)); - ptr += sizeof (isoACCM.actionTypeContentLength); + memcpy(ptr, &isoACCM.actionTypeContentLength, sizeof (isoACCM.actionTypeContentLength)); + ptr += sizeof (isoACCM.actionTypeContentLength); - memcpy(ptr, &isoACCM.actionType, sizeof (isoACCM.actionType)); - ptr += sizeof (isoACCM.actionType); + memcpy(ptr, &isoACCM.actionType, sizeof (isoACCM.actionType)); + ptr += sizeof (isoACCM.actionType); - // Copy ACCM action parameter 1 to send buffer - memcpy(ptr, &isoACCM.actionTypeParameter1ValueID, sizeof (isoACCM.actionTypeParameter1ValueID)); - ptr += sizeof (isoACCM.actionTypeParameter1ValueID); + // Copy ACCM action parameter 1 to send buffer + memcpy(ptr, &isoACCM.actionTypeParameter1ValueID, sizeof (isoACCM.actionTypeParameter1ValueID)); + ptr += sizeof (isoACCM.actionTypeParameter1ValueID); - memcpy(ptr, &isoACCM.actionTypeParameter1ContentLength, - sizeof (isoACCM.actionTypeParameter1ContentLength)); - ptr += sizeof (isoACCM.actionTypeParameter1ContentLength); + memcpy(ptr, &isoACCM.actionTypeParameter1ContentLength, + sizeof (isoACCM.actionTypeParameter1ContentLength)); + ptr += sizeof (isoACCM.actionTypeParameter1ContentLength); - memcpy(ptr, &isoACCM.actionTypeParameter1, sizeof (isoACCM.actionTypeParameter1)); - ptr += sizeof (isoACCM.actionTypeParameter1); + memcpy(ptr, &isoACCM.actionTypeParameter1, sizeof (isoACCM.actionTypeParameter1)); + ptr += sizeof (isoACCM.actionTypeParameter1); - // Copy ACCM action parameter 2 to send buffer - memcpy(ptr, &isoACCM.actionTypeParameter2ValueID, sizeof (isoACCM.actionTypeParameter2ValueID)); - ptr += sizeof (isoACCM.actionTypeParameter2ValueID); + // Copy ACCM action parameter 2 to send buffer + memcpy(ptr, &isoACCM.actionTypeParameter2ValueID, sizeof (isoACCM.actionTypeParameter2ValueID)); + ptr += sizeof (isoACCM.actionTypeParameter2ValueID); - memcpy(ptr, &isoACCM.actionTypeParameter2ContentLength, - sizeof (isoACCM.actionTypeParameter2ContentLength)); - ptr += sizeof (isoACCM.actionTypeParameter2ContentLength); + memcpy(ptr, &isoACCM.actionTypeParameter2ContentLength, + sizeof (isoACCM.actionTypeParameter2ContentLength)); + ptr += sizeof (isoACCM.actionTypeParameter2ContentLength); - memcpy(ptr, &isoACCM.actionTypeParameter2, sizeof (isoACCM.actionTypeParameter2)); - ptr += sizeof (isoACCM.actionTypeParameter2); + memcpy(ptr, &isoACCM.actionTypeParameter2, sizeof (isoACCM.actionTypeParameter2)); + ptr += sizeof (isoACCM.actionTypeParameter2); - // Copy ACCM action parameter 3 to send buffer - memcpy(ptr, &isoACCM.actionTypeParameter3ValueID, sizeof (isoACCM.actionTypeParameter3ValueID)); - ptr += sizeof (isoACCM.actionTypeParameter3ValueID); + // Copy ACCM action parameter 3 to send buffer + memcpy(ptr, &isoACCM.actionTypeParameter3ValueID, sizeof (isoACCM.actionTypeParameter3ValueID)); + ptr += sizeof (isoACCM.actionTypeParameter3ValueID); - memcpy(ptr, &isoACCM.actionTypeParameter3ContentLength, - sizeof (isoACCM.actionTypeParameter3ContentLength)); - ptr += sizeof (isoACCM.actionTypeParameter3ContentLength); + memcpy(ptr, &isoACCM.actionTypeParameter3ContentLength, + sizeof (isoACCM.actionTypeParameter3ContentLength)); + ptr += sizeof (isoACCM.actionTypeParameter3ContentLength); - memcpy(ptr, &isoACCM.actionTypeParameter3, sizeof (isoACCM.actionTypeParameter3)); - ptr += sizeof (isoACCM.actionTypeParameter3); + memcpy(ptr, &isoACCM.actionTypeParameter3, sizeof (isoACCM.actionTypeParameter3)); + ptr += sizeof (isoACCM.actionTypeParameter3); - // Copy ACCM footer to send buffer - memcpy(ptr, &isoACCM.footer.Crc, sizeof (isoACCM.footer.Crc)); - ptr += sizeof (isoACCM.footer.Crc); + // Copy ACCM footer to send buffer + memcpy(ptr, &isoACCM.footer.Crc, sizeof (isoACCM.footer.Crc)); + ptr += sizeof (isoACCM.footer.Crc); - if (ptr > messageBuffer) - messageSize = (U32) (ptr - messageBuffer); + if (ptr > messageBuffer) + messageSize = (U32) (ptr - messageBuffer); - if (messageSize - sizeof (isoACCM.header) - sizeof (isoACCM.footer) != isoACCM.header.MessageLengthU32) - LogMessage(LOG_LEVEL_WARNING, "ACCM message sent with invalid message length"); + if (messageSize - sizeof (isoACCM.header) - sizeof (isoACCM.footer) != isoACCM.header.MessageLengthU32) + LogMessage(LOG_LEVEL_WARNING, "ACCM message sent with invalid message length"); - UtilSendTCPData(MODULE_NAME, messageBuffer, (I32) messageSize, socket, debug); + UtilSendTCPData(MODULE_NAME, messageBuffer, (I32) messageSize, socket, debug); - return (I32) messageSize; + return (I32) messageSize; } /*! @@ -2368,100 +2384,100 @@ I32 ObjectControlSendACCMMessage(ACCMData * ACCM, I32 * socket, U8 debug) { * \return Length of sent message */ I32 ObjectControlSendTRCMMessage(TRCMData * TRCM, I32 * socket, U8 debug) { - TRCMType isoTRCM; - C8 messageBuffer[sizeof (isoTRCM)]; - C8 *ptr = messageBuffer; - U32 messageSize = 0; + TRCMType isoTRCM; + C8 messageBuffer[sizeof (isoTRCM)]; + C8 *ptr = messageBuffer; + U32 messageSize = 0; - ObjectControlBuildTRCMMessage(TRCM, &isoTRCM, debug); + ObjectControlBuildTRCMMessage(TRCM, &isoTRCM, debug); - // Copy TRCM header to send buffer - memcpy(ptr, &isoTRCM.header.SyncWordU16, sizeof (isoTRCM.header.SyncWordU16)); - ptr += sizeof (isoTRCM.header.SyncWordU16); + // Copy TRCM header to send buffer + memcpy(ptr, &isoTRCM.header.SyncWordU16, sizeof (isoTRCM.header.SyncWordU16)); + ptr += sizeof (isoTRCM.header.SyncWordU16); - memcpy(ptr, &isoTRCM.header.TransmitterIdU8, sizeof (isoTRCM.header.TransmitterIdU8)); - ptr += sizeof (isoTRCM.header.TransmitterIdU8); + memcpy(ptr, &isoTRCM.header.TransmitterIdU8, sizeof (isoTRCM.header.TransmitterIdU8)); + ptr += sizeof (isoTRCM.header.TransmitterIdU8); - memcpy(ptr, &isoTRCM.header.MessageCounterU8, sizeof (isoTRCM.header.MessageCounterU8)); - ptr += sizeof (isoTRCM.header.MessageCounterU8); + memcpy(ptr, &isoTRCM.header.MessageCounterU8, sizeof (isoTRCM.header.MessageCounterU8)); + ptr += sizeof (isoTRCM.header.MessageCounterU8); - memcpy(ptr, &isoTRCM.header.AckReqProtVerU8, sizeof (isoTRCM.header.AckReqProtVerU8)); - ptr += sizeof (isoTRCM.header.AckReqProtVerU8); + memcpy(ptr, &isoTRCM.header.AckReqProtVerU8, sizeof (isoTRCM.header.AckReqProtVerU8)); + ptr += sizeof (isoTRCM.header.AckReqProtVerU8); - memcpy(ptr, &isoTRCM.header.MessageIdU16, sizeof (isoTRCM.header.MessageIdU16)); - ptr += sizeof (isoTRCM.header.MessageIdU16); + memcpy(ptr, &isoTRCM.header.MessageIdU16, sizeof (isoTRCM.header.MessageIdU16)); + ptr += sizeof (isoTRCM.header.MessageIdU16); - memcpy(ptr, &isoTRCM.header.MessageLengthU32, sizeof (isoTRCM.header.MessageLengthU32)); - ptr += sizeof (isoTRCM.header.MessageLengthU32); + memcpy(ptr, &isoTRCM.header.MessageLengthU32, sizeof (isoTRCM.header.MessageLengthU32)); + ptr += sizeof (isoTRCM.header.MessageLengthU32); - // Copy TRCM trigger ID to send buffer - memcpy(ptr, &isoTRCM.triggerIDValueID, sizeof (isoTRCM.triggerIDValueID)); - ptr += sizeof (isoTRCM.triggerIDValueID); + // Copy TRCM trigger ID to send buffer + memcpy(ptr, &isoTRCM.triggerIDValueID, sizeof (isoTRCM.triggerIDValueID)); + ptr += sizeof (isoTRCM.triggerIDValueID); - memcpy(ptr, &isoTRCM.triggerIDContentLength, sizeof (isoTRCM.triggerIDContentLength)); - ptr += sizeof (isoTRCM.triggerIDContentLength); + memcpy(ptr, &isoTRCM.triggerIDContentLength, sizeof (isoTRCM.triggerIDContentLength)); + ptr += sizeof (isoTRCM.triggerIDContentLength); - memcpy(ptr, &isoTRCM.triggerID, sizeof (isoTRCM.triggerID)); - ptr += sizeof (isoTRCM.triggerID); + memcpy(ptr, &isoTRCM.triggerID, sizeof (isoTRCM.triggerID)); + ptr += sizeof (isoTRCM.triggerID); - // Copy TRCM trigger type to send buffer - memcpy(ptr, &isoTRCM.triggerTypeValueID, sizeof (isoTRCM.triggerTypeValueID)); - ptr += sizeof (isoTRCM.triggerTypeValueID); + // Copy TRCM trigger type to send buffer + memcpy(ptr, &isoTRCM.triggerTypeValueID, sizeof (isoTRCM.triggerTypeValueID)); + ptr += sizeof (isoTRCM.triggerTypeValueID); - memcpy(ptr, &isoTRCM.triggerTypeContentLength, sizeof (isoTRCM.triggerTypeContentLength)); - ptr += sizeof (isoTRCM.triggerTypeContentLength); + memcpy(ptr, &isoTRCM.triggerTypeContentLength, sizeof (isoTRCM.triggerTypeContentLength)); + ptr += sizeof (isoTRCM.triggerTypeContentLength); - memcpy(ptr, &isoTRCM.triggerType, sizeof (isoTRCM.triggerType)); - ptr += sizeof (isoTRCM.triggerType); + memcpy(ptr, &isoTRCM.triggerType, sizeof (isoTRCM.triggerType)); + ptr += sizeof (isoTRCM.triggerType); - // Copy TRCM trigger parameter 1 to send buffer - memcpy(ptr, &isoTRCM.triggerTypeParameter1ValueID, sizeof (isoTRCM.triggerTypeParameter1ValueID)); - ptr += sizeof (isoTRCM.triggerTypeParameter1ValueID); + // Copy TRCM trigger parameter 1 to send buffer + memcpy(ptr, &isoTRCM.triggerTypeParameter1ValueID, sizeof (isoTRCM.triggerTypeParameter1ValueID)); + ptr += sizeof (isoTRCM.triggerTypeParameter1ValueID); - memcpy(ptr, &isoTRCM.triggerTypeParameter1ContentLength, - sizeof (isoTRCM.triggerTypeParameter1ContentLength)); - ptr += sizeof (isoTRCM.triggerTypeParameter1ContentLength); + memcpy(ptr, &isoTRCM.triggerTypeParameter1ContentLength, + sizeof (isoTRCM.triggerTypeParameter1ContentLength)); + ptr += sizeof (isoTRCM.triggerTypeParameter1ContentLength); - memcpy(ptr, &isoTRCM.triggerTypeParameter1, sizeof (isoTRCM.triggerTypeParameter1)); - ptr += sizeof (isoTRCM.triggerTypeParameter1); + memcpy(ptr, &isoTRCM.triggerTypeParameter1, sizeof (isoTRCM.triggerTypeParameter1)); + ptr += sizeof (isoTRCM.triggerTypeParameter1); - // Copy TRCM trigger parameter 2 to send buffer - memcpy(ptr, &isoTRCM.triggerTypeParameter2ValueID, sizeof (isoTRCM.triggerTypeParameter2ValueID)); - ptr += sizeof (isoTRCM.triggerTypeParameter2ValueID); + // Copy TRCM trigger parameter 2 to send buffer + memcpy(ptr, &isoTRCM.triggerTypeParameter2ValueID, sizeof (isoTRCM.triggerTypeParameter2ValueID)); + ptr += sizeof (isoTRCM.triggerTypeParameter2ValueID); - memcpy(ptr, &isoTRCM.triggerTypeParameter2ContentLength, - sizeof (isoTRCM.triggerTypeParameter2ContentLength)); - ptr += sizeof (isoTRCM.triggerTypeParameter2ContentLength); + memcpy(ptr, &isoTRCM.triggerTypeParameter2ContentLength, + sizeof (isoTRCM.triggerTypeParameter2ContentLength)); + ptr += sizeof (isoTRCM.triggerTypeParameter2ContentLength); - memcpy(ptr, &isoTRCM.triggerTypeParameter2, sizeof (isoTRCM.triggerTypeParameter2)); - ptr += sizeof (isoTRCM.triggerTypeParameter2); + memcpy(ptr, &isoTRCM.triggerTypeParameter2, sizeof (isoTRCM.triggerTypeParameter2)); + ptr += sizeof (isoTRCM.triggerTypeParameter2); - // Copy TRCM trigger parameter 3 to send buffer - memcpy(ptr, &isoTRCM.triggerTypeParameter3ValueID, sizeof (isoTRCM.triggerTypeParameter3ValueID)); - ptr += sizeof (isoTRCM.triggerTypeParameter3ValueID); + // Copy TRCM trigger parameter 3 to send buffer + memcpy(ptr, &isoTRCM.triggerTypeParameter3ValueID, sizeof (isoTRCM.triggerTypeParameter3ValueID)); + ptr += sizeof (isoTRCM.triggerTypeParameter3ValueID); - memcpy(ptr, &isoTRCM.triggerTypeParameter3ContentLength, - sizeof (isoTRCM.triggerTypeParameter3ContentLength)); - ptr += sizeof (isoTRCM.triggerTypeParameter3ContentLength); + memcpy(ptr, &isoTRCM.triggerTypeParameter3ContentLength, + sizeof (isoTRCM.triggerTypeParameter3ContentLength)); + ptr += sizeof (isoTRCM.triggerTypeParameter3ContentLength); - memcpy(ptr, &isoTRCM.triggerTypeParameter3, sizeof (isoTRCM.triggerTypeParameter3)); - ptr += sizeof (isoTRCM.triggerTypeParameter3); + memcpy(ptr, &isoTRCM.triggerTypeParameter3, sizeof (isoTRCM.triggerTypeParameter3)); + ptr += sizeof (isoTRCM.triggerTypeParameter3); - // Copy TRCM footer to send buffer - memcpy(ptr, &isoTRCM.footer.Crc, sizeof (isoTRCM.footer.Crc)); - ptr += sizeof (isoTRCM.footer.Crc); + // Copy TRCM footer to send buffer + memcpy(ptr, &isoTRCM.footer.Crc, sizeof (isoTRCM.footer.Crc)); + ptr += sizeof (isoTRCM.footer.Crc); - if (ptr > messageBuffer) - messageSize = (U32) (ptr - messageBuffer); + if (ptr > messageBuffer) + messageSize = (U32) (ptr - messageBuffer); - if (messageSize - sizeof (isoTRCM.header) - sizeof (isoTRCM.footer) != isoTRCM.header.MessageLengthU32) - LogMessage(LOG_LEVEL_WARNING, "TRCM message sent with invalid message length"); + if (messageSize - sizeof (isoTRCM.header) - sizeof (isoTRCM.footer) != isoTRCM.header.MessageLengthU32) + LogMessage(LOG_LEVEL_WARNING, "TRCM message sent with invalid message length"); - UtilSendTCPData(MODULE_NAME, messageBuffer, (I32) messageSize, socket, 0); + UtilSendTCPData(MODULE_NAME, messageBuffer, (I32) messageSize, socket, 0); - return (I32) messageSize; + return (I32) messageSize; } /*! @@ -2472,67 +2488,67 @@ I32 ObjectControlSendTRCMMessage(TRCMData * TRCM, I32 * socket, U8 debug) { * \return Length of sent message */ I32 ObjectControlSendEXACMessage(EXACData * EXAC, I32 * socket, U8 debug) { - EXACType isoEXAC; - C8 messageBuffer[sizeof (isoEXAC)]; - C8 *ptr = messageBuffer; - U32 messageSize = 0; + EXACType isoEXAC; + C8 messageBuffer[sizeof (isoEXAC)]; + C8 *ptr = messageBuffer; + U32 messageSize = 0; - ObjectControlBuildEXACMessage(EXAC, &isoEXAC, debug); + ObjectControlBuildEXACMessage(EXAC, &isoEXAC, debug); - // Copy EXAC header to send buffer - memcpy(ptr, &isoEXAC.header.SyncWordU16, sizeof (isoEXAC.header.SyncWordU16)); - ptr += sizeof (isoEXAC.header.SyncWordU16); + // Copy EXAC header to send buffer + memcpy(ptr, &isoEXAC.header.SyncWordU16, sizeof (isoEXAC.header.SyncWordU16)); + ptr += sizeof (isoEXAC.header.SyncWordU16); - memcpy(ptr, &isoEXAC.header.TransmitterIdU8, sizeof (isoEXAC.header.TransmitterIdU8)); - ptr += sizeof (isoEXAC.header.TransmitterIdU8); + memcpy(ptr, &isoEXAC.header.TransmitterIdU8, sizeof (isoEXAC.header.TransmitterIdU8)); + ptr += sizeof (isoEXAC.header.TransmitterIdU8); - memcpy(ptr, &isoEXAC.header.MessageCounterU8, sizeof (isoEXAC.header.MessageCounterU8)); - ptr += sizeof (isoEXAC.header.MessageCounterU8); + memcpy(ptr, &isoEXAC.header.MessageCounterU8, sizeof (isoEXAC.header.MessageCounterU8)); + ptr += sizeof (isoEXAC.header.MessageCounterU8); - memcpy(ptr, &isoEXAC.header.AckReqProtVerU8, sizeof (isoEXAC.header.AckReqProtVerU8)); - ptr += sizeof (isoEXAC.header.AckReqProtVerU8); + memcpy(ptr, &isoEXAC.header.AckReqProtVerU8, sizeof (isoEXAC.header.AckReqProtVerU8)); + ptr += sizeof (isoEXAC.header.AckReqProtVerU8); - memcpy(ptr, &isoEXAC.header.MessageIdU16, sizeof (isoEXAC.header.MessageIdU16)); - ptr += sizeof (isoEXAC.header.MessageIdU16); + memcpy(ptr, &isoEXAC.header.MessageIdU16, sizeof (isoEXAC.header.MessageIdU16)); + ptr += sizeof (isoEXAC.header.MessageIdU16); - memcpy(ptr, &isoEXAC.header.MessageLengthU32, sizeof (isoEXAC.header.MessageLengthU32)); - ptr += sizeof (isoEXAC.header.MessageLengthU32); + memcpy(ptr, &isoEXAC.header.MessageLengthU32, sizeof (isoEXAC.header.MessageLengthU32)); + ptr += sizeof (isoEXAC.header.MessageLengthU32); - // Copy EXAC action ID to send buffer - memcpy(ptr, &isoEXAC.actionIDValueID, sizeof (isoEXAC.actionIDValueID)); - ptr += sizeof (isoEXAC.actionIDValueID); + // Copy EXAC action ID to send buffer + memcpy(ptr, &isoEXAC.actionIDValueID, sizeof (isoEXAC.actionIDValueID)); + ptr += sizeof (isoEXAC.actionIDValueID); - memcpy(ptr, &isoEXAC.actionIDContentLength, sizeof (isoEXAC.actionIDContentLength)); - ptr += sizeof (isoEXAC.actionIDContentLength); + memcpy(ptr, &isoEXAC.actionIDContentLength, sizeof (isoEXAC.actionIDContentLength)); + ptr += sizeof (isoEXAC.actionIDContentLength); - memcpy(ptr, &isoEXAC.actionID, sizeof (isoEXAC.actionID)); - ptr += sizeof (isoEXAC.actionID); + memcpy(ptr, &isoEXAC.actionID, sizeof (isoEXAC.actionID)); + ptr += sizeof (isoEXAC.actionID); - // Copy EXAC action execution time to send buffer - memcpy(ptr, &isoEXAC.executionTime_qmsoWValueID, sizeof (isoEXAC.executionTime_qmsoWValueID)); - ptr += sizeof (isoEXAC.executionTime_qmsoWValueID); + // Copy EXAC action execution time to send buffer + memcpy(ptr, &isoEXAC.executionTime_qmsoWValueID, sizeof (isoEXAC.executionTime_qmsoWValueID)); + ptr += sizeof (isoEXAC.executionTime_qmsoWValueID); - memcpy(ptr, &isoEXAC.executionTime_qmsoWContentLength, sizeof (isoEXAC.executionTime_qmsoWContentLength)); - ptr += sizeof (isoEXAC.executionTime_qmsoWContentLength); + memcpy(ptr, &isoEXAC.executionTime_qmsoWContentLength, sizeof (isoEXAC.executionTime_qmsoWContentLength)); + ptr += sizeof (isoEXAC.executionTime_qmsoWContentLength); - memcpy(ptr, &isoEXAC.executionTime_qmsoW, sizeof (isoEXAC.executionTime_qmsoW)); - ptr += sizeof (isoEXAC.executionTime_qmsoW); + memcpy(ptr, &isoEXAC.executionTime_qmsoW, sizeof (isoEXAC.executionTime_qmsoW)); + ptr += sizeof (isoEXAC.executionTime_qmsoW); - // Copy EXAC footer to send buffer - memcpy(ptr, &isoEXAC.footer.Crc, sizeof (isoEXAC.footer.Crc)); - ptr += sizeof (isoEXAC.footer.Crc); + // Copy EXAC footer to send buffer + memcpy(ptr, &isoEXAC.footer.Crc, sizeof (isoEXAC.footer.Crc)); + ptr += sizeof (isoEXAC.footer.Crc); - if (ptr > messageBuffer) - messageSize = (U32) (ptr - messageBuffer); + if (ptr > messageBuffer) + messageSize = (U32) (ptr - messageBuffer); - if (messageSize - sizeof (isoEXAC.header) - sizeof (isoEXAC.footer) != isoEXAC.header.MessageLengthU32) - LogMessage(LOG_LEVEL_WARNING, "EXAC message sent with invalid message length"); + if (messageSize - sizeof (isoEXAC.header) - sizeof (isoEXAC.footer) != isoEXAC.header.MessageLengthU32) + LogMessage(LOG_LEVEL_WARNING, "EXAC message sent with invalid message length"); - UtilSendTCPData(MODULE_NAME, messageBuffer, (I32) messageSize, socket, 0); + UtilSendTCPData(MODULE_NAME, messageBuffer, (I32) messageSize, socket, 0); - return (I32) messageSize; + return (I32) messageSize; } @@ -2544,69 +2560,69 @@ I32 ObjectControlSendEXACMessage(EXACData * EXAC, I32 * socket, U8 debug) { * \return Byte size of ACCM struct */ I32 ObjectControlBuildACCMMessage(ACCMData * mqACCMData, ACCMType * ACCM, U8 debug) { - // Header - ACCM->header.SyncWordU16 = SYNC_WORD; - ACCM->header.TransmitterIdU8 = 0; - ACCM->header.MessageCounterU8 = 0; - ACCM->header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; - ACCM->header.MessageIdU16 = COMMAND_ACCM_CODE; - ACCM->header.MessageLengthU32 = sizeof (ACCMType) - sizeof (HeaderType) - sizeof (FooterType); - - // Data fields - ACCM->actionID = mqACCMData->actionID; - ACCM->actionType = mqACCMData->actionType; - ACCM->actionTypeParameter1 = mqACCMData->actionTypeParameter1; - ACCM->actionTypeParameter2 = mqACCMData->actionTypeParameter2; - ACCM->actionTypeParameter3 = mqACCMData->actionTypeParameter3; - - // Value ID fields - ACCM->actionIDValueID = VALUE_ID_ACTION_ID; - ACCM->actionTypeValueID = VALUE_ID_ACTION_TYPE; - ACCM->actionTypeParameter1ValueID = VALUE_ID_ACTION_TYPE_PARAM1; - ACCM->actionTypeParameter2ValueID = VALUE_ID_ACTION_TYPE_PARAM2; - ACCM->actionTypeParameter3ValueID = VALUE_ID_ACTION_TYPE_PARAM3; - - // Content length fields - ACCM->actionIDContentLength = sizeof (ACCM->actionID); - ACCM->actionTypeContentLength = sizeof (ACCM->actionType); - ACCM->actionTypeParameter1ContentLength = sizeof (ACCM->actionTypeParameter1); - ACCM->actionTypeParameter2ContentLength = sizeof (ACCM->actionTypeParameter2); - ACCM->actionTypeParameter3ContentLength = sizeof (ACCM->actionTypeParameter3); - - // Header content length - ACCM->header.MessageLengthU32 = sizeof (ACCM->actionID) + sizeof (ACCM->actionType) - + sizeof (ACCM->actionTypeParameter1) + sizeof (ACCM->actionTypeParameter2) + - sizeof (ACCM->actionTypeParameter3) - + sizeof (ACCM->actionIDValueID) + sizeof (ACCM->actionTypeValueID) - + sizeof (ACCM->actionTypeParameter1ValueID) + sizeof (ACCM->actionTypeParameter1ValueID) + - sizeof (ACCM->actionTypeParameter3ValueID) - + sizeof (ACCM->actionIDContentLength) + sizeof (ACCM->actionTypeContentLength) - + sizeof (ACCM->actionTypeParameter1ContentLength) + - sizeof (ACCM->actionTypeParameter1ContentLength) + sizeof (ACCM->actionTypeParameter3ContentLength); - - - // Footer (TODO) - ACCM->footer.Crc = 0; - - U32 messageLen = - ACCM->header.MessageLengthU32 + sizeof (ACCM->footer.Crc) + sizeof (ACCM->header.SyncWordU16) + - sizeof (ACCM->header.MessageIdU16) + sizeof (ACCM->header.AckReqProtVerU8) + - sizeof (ACCM->header.TransmitterIdU8) + sizeof (ACCM->header.MessageCounterU8) + - sizeof (ACCM->header.MessageLengthU32); - - if (debug) { - LogPrint - ("ACCM (%u bytes):\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x", - messageLen, ACCM->actionIDValueID, ACCM->actionIDContentLength, ACCM->actionID, - ACCM->actionTypeValueID, ACCM->actionTypeContentLength, ACCM->actionType, - ACCM->actionTypeParameter1ValueID, ACCM->actionTypeParameter1ContentLength, - ACCM->actionTypeParameter1, ACCM->actionTypeParameter2ValueID, - ACCM->actionTypeParameter2ContentLength, ACCM->actionTypeParameter2, - ACCM->actionTypeParameter3ValueID, ACCM->actionTypeParameter3ContentLength, - ACCM->actionTypeParameter3); - } - - return (I32) messageLen; + // Header + ACCM->header.SyncWordU16 = SYNC_WORD; + ACCM->header.TransmitterIdU8 = 0; + ACCM->header.MessageCounterU8 = 0; + ACCM->header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; + ACCM->header.MessageIdU16 = COMMAND_ACCM_CODE; + ACCM->header.MessageLengthU32 = sizeof (ACCMType) - sizeof (HeaderType) - sizeof (FooterType); + + // Data fields + ACCM->actionID = mqACCMData->actionID; + ACCM->actionType = mqACCMData->actionType; + ACCM->actionTypeParameter1 = mqACCMData->actionTypeParameter1; + ACCM->actionTypeParameter2 = mqACCMData->actionTypeParameter2; + ACCM->actionTypeParameter3 = mqACCMData->actionTypeParameter3; + + // Value ID fields + ACCM->actionIDValueID = VALUE_ID_ACTION_ID; + ACCM->actionTypeValueID = VALUE_ID_ACTION_TYPE; + ACCM->actionTypeParameter1ValueID = VALUE_ID_ACTION_TYPE_PARAM1; + ACCM->actionTypeParameter2ValueID = VALUE_ID_ACTION_TYPE_PARAM2; + ACCM->actionTypeParameter3ValueID = VALUE_ID_ACTION_TYPE_PARAM3; + + // Content length fields + ACCM->actionIDContentLength = sizeof (ACCM->actionID); + ACCM->actionTypeContentLength = sizeof (ACCM->actionType); + ACCM->actionTypeParameter1ContentLength = sizeof (ACCM->actionTypeParameter1); + ACCM->actionTypeParameter2ContentLength = sizeof (ACCM->actionTypeParameter2); + ACCM->actionTypeParameter3ContentLength = sizeof (ACCM->actionTypeParameter3); + + // Header content length + ACCM->header.MessageLengthU32 = sizeof (ACCM->actionID) + sizeof (ACCM->actionType) + + sizeof (ACCM->actionTypeParameter1) + sizeof (ACCM->actionTypeParameter2) + + sizeof (ACCM->actionTypeParameter3) + + sizeof (ACCM->actionIDValueID) + sizeof (ACCM->actionTypeValueID) + + sizeof (ACCM->actionTypeParameter1ValueID) + sizeof (ACCM->actionTypeParameter1ValueID) + + sizeof (ACCM->actionTypeParameter3ValueID) + + sizeof (ACCM->actionIDContentLength) + sizeof (ACCM->actionTypeContentLength) + + sizeof (ACCM->actionTypeParameter1ContentLength) + + sizeof (ACCM->actionTypeParameter1ContentLength) + sizeof (ACCM->actionTypeParameter3ContentLength); + + + // Footer (TODO) + ACCM->footer.Crc = 0; + + U32 messageLen = + ACCM->header.MessageLengthU32 + sizeof (ACCM->footer.Crc) + sizeof (ACCM->header.SyncWordU16) + + sizeof (ACCM->header.MessageIdU16) + sizeof (ACCM->header.AckReqProtVerU8) + + sizeof (ACCM->header.TransmitterIdU8) + sizeof (ACCM->header.MessageCounterU8) + + sizeof (ACCM->header.MessageLengthU32); + + if (debug) { + LogPrint + ("ACCM (%u bytes):\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x", + messageLen, ACCM->actionIDValueID, ACCM->actionIDContentLength, ACCM->actionID, + ACCM->actionTypeValueID, ACCM->actionTypeContentLength, ACCM->actionType, + ACCM->actionTypeParameter1ValueID, ACCM->actionTypeParameter1ContentLength, + ACCM->actionTypeParameter1, ACCM->actionTypeParameter2ValueID, + ACCM->actionTypeParameter2ContentLength, ACCM->actionTypeParameter2, + ACCM->actionTypeParameter3ValueID, ACCM->actionTypeParameter3ContentLength, + ACCM->actionTypeParameter3); + } + + return (I32) messageLen; } /*! @@ -2617,53 +2633,53 @@ I32 ObjectControlBuildACCMMessage(ACCMData * mqACCMData, ACCMType * ACCM, U8 deb * \return Byte size of EXAC struct */ I32 ObjectControlBuildEXACMessage(EXACData * mqEXACData, EXACType * EXAC, U8 debug) { - // TODO: Make system time follow GPS time (better) or pass the GSD pointer into here somehow - struct timeval systemTime; + // TODO: Make system time follow GPS time (better) or pass the GSD pointer into here somehow + struct timeval systemTime; - TimeSetToCurrentSystemTime(&systemTime); + TimeSetToCurrentSystemTime(&systemTime); - // Header - EXAC->header.SyncWordU16 = SYNC_WORD; - EXAC->header.TransmitterIdU8 = 0; - EXAC->header.MessageCounterU8 = 0; - EXAC->header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; - EXAC->header.MessageIdU16 = COMMAND_EXAC_CODE; + // Header + EXAC->header.SyncWordU16 = SYNC_WORD; + EXAC->header.TransmitterIdU8 = 0; + EXAC->header.MessageCounterU8 = 0; + EXAC->header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; + EXAC->header.MessageIdU16 = COMMAND_EXAC_CODE; - // Data fields - EXAC->actionID = mqEXACData->actionID; - EXAC->executionTime_qmsoW = mqEXACData->executionTime_qmsoW; + // Data fields + EXAC->actionID = mqEXACData->actionID; + EXAC->executionTime_qmsoW = mqEXACData->executionTime_qmsoW; - // Value ID fields - EXAC->actionIDValueID = VALUE_ID_ACTION_ID; - EXAC->executionTime_qmsoWValueID = VALUE_ID_ACTION_EXECUTE_TIME; + // Value ID fields + EXAC->actionIDValueID = VALUE_ID_ACTION_ID; + EXAC->executionTime_qmsoWValueID = VALUE_ID_ACTION_EXECUTE_TIME; - // Content length fields - EXAC->actionIDContentLength = sizeof (EXAC->actionID); - EXAC->executionTime_qmsoWContentLength = sizeof (EXAC->executionTime_qmsoW); + // Content length fields + EXAC->actionIDContentLength = sizeof (EXAC->actionID); + EXAC->executionTime_qmsoWContentLength = sizeof (EXAC->executionTime_qmsoW); - // Header message length - EXAC->header.MessageLengthU32 = sizeof (EXAC->actionID) + sizeof (EXAC->executionTime_qmsoW) - + sizeof (EXAC->actionIDValueID) + sizeof (EXAC->executionTime_qmsoWValueID) - + sizeof (EXAC->actionIDContentLength) + sizeof (EXAC->executionTime_qmsoWContentLength); + // Header message length + EXAC->header.MessageLengthU32 = sizeof (EXAC->actionID) + sizeof (EXAC->executionTime_qmsoW) + + sizeof (EXAC->actionIDValueID) + sizeof (EXAC->executionTime_qmsoWValueID) + + sizeof (EXAC->actionIDContentLength) + sizeof (EXAC->executionTime_qmsoWContentLength); - // Footer (TODO) - EXAC->footer.Crc = 0; + // Footer (TODO) + EXAC->footer.Crc = 0; - U32 messageLen = - EXAC->header.MessageLengthU32 + sizeof (EXAC->footer.Crc) + sizeof (EXAC->header.SyncWordU16) + - sizeof (EXAC->header.MessageIdU16) + sizeof (EXAC->header.AckReqProtVerU8) + - sizeof (EXAC->header.TransmitterIdU8) + sizeof (EXAC->header.MessageCounterU8) + - sizeof (EXAC->header.MessageLengthU32); + U32 messageLen = + EXAC->header.MessageLengthU32 + sizeof (EXAC->footer.Crc) + sizeof (EXAC->header.SyncWordU16) + + sizeof (EXAC->header.MessageIdU16) + sizeof (EXAC->header.AckReqProtVerU8) + + sizeof (EXAC->header.TransmitterIdU8) + sizeof (EXAC->header.MessageCounterU8) + + sizeof (EXAC->header.MessageLengthU32); - if (debug) { - LogPrint("EXAC (%u bytes):\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x", messageLen, - EXAC->actionIDValueID, EXAC->actionIDContentLength, EXAC->actionID, - EXAC->executionTime_qmsoWValueID, EXAC->executionTime_qmsoWContentLength, - EXAC->executionTime_qmsoW); - } + if (debug) { + LogPrint("EXAC (%u bytes):\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x", messageLen, + EXAC->actionIDValueID, EXAC->actionIDContentLength, EXAC->actionID, + EXAC->executionTime_qmsoWValueID, EXAC->executionTime_qmsoWContentLength, + EXAC->executionTime_qmsoW); + } - return (I32) messageLen; + return (I32) messageLen; } /*! @@ -2674,625 +2690,625 @@ I32 ObjectControlBuildEXACMessage(EXACData * mqEXACData, EXACType * EXAC, U8 deb * \return Byte size of TRCM struct */ I32 ObjectControlBuildTRCMMessage(TRCMData * mqTRCMData, TRCMType * TRCM, U8 debug) { - // Header - TRCM->header.SyncWordU16 = SYNC_WORD; - TRCM->header.TransmitterIdU8 = 0; - TRCM->header.MessageCounterU8 = 0; - TRCM->header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; - TRCM->header.MessageIdU16 = COMMAND_TRCM_CODE; - - - // Data fields - TRCM->triggerID = mqTRCMData->triggerID; - TRCM->triggerType = mqTRCMData->triggerType; - TRCM->triggerTypeParameter1 = mqTRCMData->triggerTypeParameter1; - TRCM->triggerTypeParameter2 = mqTRCMData->triggerTypeParameter2; - TRCM->triggerTypeParameter3 = mqTRCMData->triggerTypeParameter3; - - // Value ID fields - TRCM->triggerIDValueID = VALUE_ID_TRIGGER_ID; - TRCM->triggerIDValueID = VALUE_ID_TRIGGER_TYPE; - TRCM->triggerTypeParameter1ValueID = VALUE_ID_TRIGGER_TYPE_PARAM1; - TRCM->triggerTypeParameter2ValueID = VALUE_ID_TRIGGER_TYPE_PARAM2; - TRCM->triggerTypeParameter3ValueID = VALUE_ID_TRIGGER_TYPE_PARAM3; - - // Content length fields - TRCM->triggerIDContentLength = sizeof (TRCM->triggerID); - TRCM->triggerTypeContentLength = sizeof (TRCM->triggerType); - TRCM->triggerTypeParameter1ContentLength = sizeof (TRCM->triggerTypeParameter1); - TRCM->triggerTypeParameter2ContentLength = sizeof (TRCM->triggerTypeParameter2); - TRCM->triggerTypeParameter3ContentLength = sizeof (TRCM->triggerTypeParameter3); - - - // Message length in header - TRCM->header.MessageLengthU32 = sizeof (TRCM->triggerID) + sizeof (TRCM->triggerType) - + sizeof (TRCM->triggerTypeParameter1) + sizeof (TRCM->triggerTypeParameter2) + - sizeof (TRCM->triggerTypeParameter3) - + sizeof (TRCM->triggerIDValueID) + sizeof (TRCM->triggerTypeValueID) - + sizeof (TRCM->triggerTypeParameter1ValueID) + sizeof (TRCM->triggerTypeParameter1ValueID) + - sizeof (TRCM->triggerTypeParameter3ValueID) - + sizeof (TRCM->triggerIDContentLength) + sizeof (TRCM->triggerTypeContentLength) - + sizeof (TRCM->triggerTypeParameter1ContentLength) + - sizeof (TRCM->triggerTypeParameter1ContentLength) + sizeof (TRCM->triggerTypeParameter3ContentLength); - - - // Footer (TODO) - TRCM->footer.Crc = 0; - - U32 messageLen = - TRCM->header.MessageLengthU32 + sizeof (TRCM->footer.Crc) + sizeof (TRCM->header.SyncWordU16) + - sizeof (TRCM->header.MessageIdU16) + sizeof (TRCM->header.AckReqProtVerU8) + - sizeof (TRCM->header.TransmitterIdU8) + sizeof (TRCM->header.MessageCounterU8) + - sizeof (TRCM->header.MessageLengthU32); - if (debug) { - LogPrint - ("TRCM (%u bytes):\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x", - messageLen, TRCM->triggerIDValueID, TRCM->triggerIDContentLength, TRCM->triggerID, - TRCM->triggerTypeValueID, TRCM->triggerTypeContentLength, TRCM->triggerType, - TRCM->triggerTypeParameter1ValueID, TRCM->triggerTypeParameter1ContentLength, - TRCM->triggerTypeParameter1, TRCM->triggerTypeParameter2ValueID, - TRCM->triggerTypeParameter2ContentLength, TRCM->triggerTypeParameter2, - TRCM->triggerTypeParameter3ValueID, TRCM->triggerTypeParameter3ContentLength, - TRCM->triggerTypeParameter3); - } - - return (I32) messageLen; + // Header + TRCM->header.SyncWordU16 = SYNC_WORD; + TRCM->header.TransmitterIdU8 = 0; + TRCM->header.MessageCounterU8 = 0; + TRCM->header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; + TRCM->header.MessageIdU16 = COMMAND_TRCM_CODE; + + + // Data fields + TRCM->triggerID = mqTRCMData->triggerID; + TRCM->triggerType = mqTRCMData->triggerType; + TRCM->triggerTypeParameter1 = mqTRCMData->triggerTypeParameter1; + TRCM->triggerTypeParameter2 = mqTRCMData->triggerTypeParameter2; + TRCM->triggerTypeParameter3 = mqTRCMData->triggerTypeParameter3; + + // Value ID fields + TRCM->triggerIDValueID = VALUE_ID_TRIGGER_ID; + TRCM->triggerIDValueID = VALUE_ID_TRIGGER_TYPE; + TRCM->triggerTypeParameter1ValueID = VALUE_ID_TRIGGER_TYPE_PARAM1; + TRCM->triggerTypeParameter2ValueID = VALUE_ID_TRIGGER_TYPE_PARAM2; + TRCM->triggerTypeParameter3ValueID = VALUE_ID_TRIGGER_TYPE_PARAM3; + + // Content length fields + TRCM->triggerIDContentLength = sizeof (TRCM->triggerID); + TRCM->triggerTypeContentLength = sizeof (TRCM->triggerType); + TRCM->triggerTypeParameter1ContentLength = sizeof (TRCM->triggerTypeParameter1); + TRCM->triggerTypeParameter2ContentLength = sizeof (TRCM->triggerTypeParameter2); + TRCM->triggerTypeParameter3ContentLength = sizeof (TRCM->triggerTypeParameter3); + + + // Message length in header + TRCM->header.MessageLengthU32 = sizeof (TRCM->triggerID) + sizeof (TRCM->triggerType) + + sizeof (TRCM->triggerTypeParameter1) + sizeof (TRCM->triggerTypeParameter2) + + sizeof (TRCM->triggerTypeParameter3) + + sizeof (TRCM->triggerIDValueID) + sizeof (TRCM->triggerTypeValueID) + + sizeof (TRCM->triggerTypeParameter1ValueID) + sizeof (TRCM->triggerTypeParameter1ValueID) + + sizeof (TRCM->triggerTypeParameter3ValueID) + + sizeof (TRCM->triggerIDContentLength) + sizeof (TRCM->triggerTypeContentLength) + + sizeof (TRCM->triggerTypeParameter1ContentLength) + + sizeof (TRCM->triggerTypeParameter1ContentLength) + sizeof (TRCM->triggerTypeParameter3ContentLength); + + + // Footer (TODO) + TRCM->footer.Crc = 0; + + U32 messageLen = + TRCM->header.MessageLengthU32 + sizeof (TRCM->footer.Crc) + sizeof (TRCM->header.SyncWordU16) + + sizeof (TRCM->header.MessageIdU16) + sizeof (TRCM->header.AckReqProtVerU8) + + sizeof (TRCM->header.TransmitterIdU8) + sizeof (TRCM->header.MessageCounterU8) + + sizeof (TRCM->header.MessageLengthU32); + if (debug) { + LogPrint + ("TRCM (%u bytes):\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x", + messageLen, TRCM->triggerIDValueID, TRCM->triggerIDContentLength, TRCM->triggerID, + TRCM->triggerTypeValueID, TRCM->triggerTypeContentLength, TRCM->triggerType, + TRCM->triggerTypeParameter1ValueID, TRCM->triggerTypeParameter1ContentLength, + TRCM->triggerTypeParameter1, TRCM->triggerTypeParameter2ValueID, + TRCM->triggerTypeParameter2ContentLength, TRCM->triggerTypeParameter2, + TRCM->triggerTypeParameter3ValueID, TRCM->triggerTypeParameter3ContentLength, + TRCM->triggerTypeParameter3); + } + + return (I32) messageLen; } I32 ObjectControlSendDTMMessage(C8 * DTMData, I32 * Socket, I32 RowCount, C8 * IP, U32 Port, - DOTMType * DOTMData, U8 debug) { + DOTMType * DOTMData, U8 debug) { - U32 Rest = 0, i = 0, MessageLength = 0, SumMessageLength = 0, Modulo = 0, Transmissions = 0; - U16 CrcU16 = 0; + U32 Rest = 0, i = 0, MessageLength = 0, SumMessageLength = 0, Modulo = 0, Transmissions = 0; + U16 CrcU16 = 0; - MessageLength = - ObjectControlBuildDTMMessage(TrajBuffer, DTMData, COMMAND_DOTM_ROWS_IN_TRANSMISSION, DOTMData, 0); + MessageLength = + ObjectControlBuildDTMMessage(TrajBuffer, DTMData, COMMAND_DOTM_ROWS_IN_TRANSMISSION, DOTMData, 0); - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "Transmission %d: %d bytes sent", i, MessageLength); + if (debug) + LogMessage(LOG_LEVEL_DEBUG, "Transmission %d: %d bytes sent", i, MessageLength); - LogMessage(LOG_LEVEL_INFO, "%d DTM bytes sent to %s:%d", SumMessageLength, IP, Port); + LogMessage(LOG_LEVEL_INFO, "%d DTM bytes sent to %s:%d", SumMessageLength, IP, Port); - return 0; + return 0; } I32 ObjectControlBuildDTMMessage(C8 * MessageBuffer, C8 * DTMData, I32 RowCount, DOTMType * DOTMData, - U8 debug) { - I32 MessageIndex = 0; - U32 Data; - C8 *src, *p; - U16 Crc = 0; - - bzero(MessageBuffer, COMMAND_DOTM_ROW_MESSAGE_LENGTH * RowCount); - - I32 i = 0, j = 0, n = 0; - - for (i = 0; i < RowCount; i++) { - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "DOTM row:"); - //Time - Data = 0; - Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 3); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 2); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 1); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 0); - DOTMData->RelativeTimeValueIdU16 = VALUE_ID_RELATIVE_TIME; - DOTMData->RelativeTimeContentLengthU16 = 4; - DOTMData->RelativeTimeU32 = SwapU32((U32) Data); - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "Time=%d", DOTMData->RelativeTimeU32); - - //x - Data = 0; - Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 7); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 6); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 5); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 4); - DOTMData->XPositionValueIdU16 = VALUE_ID_X_POSITION; - DOTMData->XPositionContentLengthU16 = 4; - DOTMData->XPositionI32 = SwapI32((I32) Data); - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "X=%d", DOTMData->XPositionI32); - - //y - Data = 0; - Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 11); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 10); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 9); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 8); - DOTMData->YPositionValueIdU16 = VALUE_ID_Y_POSITION; - DOTMData->YPositionContentLengthU16 = 4; - DOTMData->YPositionI32 = SwapI32((I32) Data); - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "Y=%d", DOTMData->YPositionI32); - - //z - Data = 0; - Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 15); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 14); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 13); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 12); - DOTMData->ZPositionValueIdU16 = VALUE_ID_Z_POSITION; - DOTMData->ZPositionContentLengthU16 = 4; - DOTMData->ZPositionI32 = SwapI32((I32) Data); - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "Z=%d", DOTMData->ZPositionI32); - - //Heading - Data = 0; - Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 17); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 16); - //Data = UtilRadToDeg(Data); - //Data = 4500 - Data; //Turn heading back pi/2 - //while(Data<0) Data+=360.0; - //while(Data>3600) Data-=360.0; - DOTMData->HeadingValueIdU16 = VALUE_ID_HEADING; - DOTMData->HeadingContentLengthU16 = 2; - DOTMData->HeadingU16 = SwapU16((U16) (Data)); - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "Heading=%d", DOTMData->HeadingU16); - - //Longitudinal speed - Data = 0; - Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 19); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 18); - DOTMData->LongitudinalSpeedValueIdU16 = VALUE_ID_LONGITUDINAL_SPEED; - DOTMData->LongitudinalSpeedContentLengthU16 = 2; - DOTMData->LongitudinalSpeedI16 = SwapI16((I16) Data); - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "LongitudinalSpeedI16=%d", DOTMData->LongitudinalSpeedI16); - - //Lateral speed - Data = 0; - Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 21); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 20); - DOTMData->LateralSpeedValueIdU16 = VALUE_ID_LATERAL_SPEED; - DOTMData->LateralSpeedContentLengthU16 = 2; - DOTMData->LateralSpeedI16 = SwapI16((I16) Data); - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "LateralSpeedI16=%d", DOTMData->LateralSpeedI16); - - //Longitudinal acceleration - Data = 0; - Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 23); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 22); - DOTMData->LongitudinalAccValueIdU16 = VALUE_ID_LONGITUDINAL_ACCELERATION; - DOTMData->LongitudinalAccContentLengthU16 = 2; - DOTMData->LongitudinalAccI16 = SwapI16((I16) Data); - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "LongitudinalAccI16=%d", DOTMData->LongitudinalAccI16); - - //Lateral acceleration - Data = 0; - Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 25); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 24); - DOTMData->LateralAccValueIdU16 = VALUE_ID_LATERAL_ACCELERATION; - DOTMData->LateralAccContentLengthU16 = 2; - DOTMData->LateralAccI16 = SwapI16((I16) Data); - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "LateralAccI16=%d", DOTMData->LateralAccI16); - - //Curvature - Data = 0; - Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 29); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 28); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 27); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 26); - DOTMData->CurvatureValueIdU16 = VALUE_ID_CURVATURE; - DOTMData->CurvatureContentLengthU16 = 4; - DOTMData->CurvatureI32 = SwapI32((I32) Data); - if (debug) - printf("CurvatureI32=%d \n", DOTMData->CurvatureI32); - - p = (C8 *) DOTMData; - for (j = 0; j < sizeof (DOTMType); j++, n++) - *(MessageBuffer + n) = *p++; - MessageIndex = n; - } - - - Crc = crc_16((const C8 *)MessageBuffer, sizeof (DOTMType)); - Crc = 0; - *(MessageBuffer + MessageIndex++) = (U8) (Crc); - *(MessageBuffer + MessageIndex++) = (U8) (Crc >> 8); - - - if (debug) { - int i = 0; - - for (i = 0; i < MessageIndex; i++) { - // TODO: Write to log when bytes thingy has been implemented - if ((unsigned char)MessageBuffer[i] >= 0 && (unsigned char)MessageBuffer[i] <= 15) - printf("0"); - printf("%x-", (unsigned char)MessageBuffer[i]); - } - printf("\n"); - } - - return MessageIndex; //Total number of bytes + U8 debug) { + I32 MessageIndex = 0; + U32 Data; + C8 *src, *p; + U16 Crc = 0; + + bzero(MessageBuffer, COMMAND_DOTM_ROW_MESSAGE_LENGTH * RowCount); + + I32 i = 0, j = 0, n = 0; + + for (i = 0; i < RowCount; i++) { + if (debug) + LogMessage(LOG_LEVEL_DEBUG, "DOTM row:"); + //Time + Data = 0; + Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 3); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 2); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 1); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 0); + DOTMData->RelativeTimeValueIdU16 = VALUE_ID_RELATIVE_TIME; + DOTMData->RelativeTimeContentLengthU16 = 4; + DOTMData->RelativeTimeU32 = SwapU32((U32) Data); + if (debug) + LogMessage(LOG_LEVEL_DEBUG, "Time=%d", DOTMData->RelativeTimeU32); + + //x + Data = 0; + Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 7); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 6); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 5); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 4); + DOTMData->XPositionValueIdU16 = VALUE_ID_X_POSITION; + DOTMData->XPositionContentLengthU16 = 4; + DOTMData->XPositionI32 = SwapI32((I32) Data); + if (debug) + LogMessage(LOG_LEVEL_DEBUG, "X=%d", DOTMData->XPositionI32); + + //y + Data = 0; + Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 11); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 10); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 9); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 8); + DOTMData->YPositionValueIdU16 = VALUE_ID_Y_POSITION; + DOTMData->YPositionContentLengthU16 = 4; + DOTMData->YPositionI32 = SwapI32((I32) Data); + if (debug) + LogMessage(LOG_LEVEL_DEBUG, "Y=%d", DOTMData->YPositionI32); + + //z + Data = 0; + Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 15); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 14); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 13); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 12); + DOTMData->ZPositionValueIdU16 = VALUE_ID_Z_POSITION; + DOTMData->ZPositionContentLengthU16 = 4; + DOTMData->ZPositionI32 = SwapI32((I32) Data); + if (debug) + LogMessage(LOG_LEVEL_DEBUG, "Z=%d", DOTMData->ZPositionI32); + + //Heading + Data = 0; + Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 17); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 16); + //Data = UtilRadToDeg(Data); + //Data = 4500 - Data; //Turn heading back pi/2 + //while(Data<0) Data+=360.0; + //while(Data>3600) Data-=360.0; + DOTMData->HeadingValueIdU16 = VALUE_ID_HEADING; + DOTMData->HeadingContentLengthU16 = 2; + DOTMData->HeadingU16 = SwapU16((U16) (Data)); + if (debug) + LogMessage(LOG_LEVEL_DEBUG, "Heading=%d", DOTMData->HeadingU16); + + //Longitudinal speed + Data = 0; + Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 19); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 18); + DOTMData->LongitudinalSpeedValueIdU16 = VALUE_ID_LONGITUDINAL_SPEED; + DOTMData->LongitudinalSpeedContentLengthU16 = 2; + DOTMData->LongitudinalSpeedI16 = SwapI16((I16) Data); + if (debug) + LogMessage(LOG_LEVEL_DEBUG, "LongitudinalSpeedI16=%d", DOTMData->LongitudinalSpeedI16); + + //Lateral speed + Data = 0; + Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 21); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 20); + DOTMData->LateralSpeedValueIdU16 = VALUE_ID_LATERAL_SPEED; + DOTMData->LateralSpeedContentLengthU16 = 2; + DOTMData->LateralSpeedI16 = SwapI16((I16) Data); + if (debug) + LogMessage(LOG_LEVEL_DEBUG, "LateralSpeedI16=%d", DOTMData->LateralSpeedI16); + + //Longitudinal acceleration + Data = 0; + Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 23); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 22); + DOTMData->LongitudinalAccValueIdU16 = VALUE_ID_LONGITUDINAL_ACCELERATION; + DOTMData->LongitudinalAccContentLengthU16 = 2; + DOTMData->LongitudinalAccI16 = SwapI16((I16) Data); + if (debug) + LogMessage(LOG_LEVEL_DEBUG, "LongitudinalAccI16=%d", DOTMData->LongitudinalAccI16); + + //Lateral acceleration + Data = 0; + Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 25); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 24); + DOTMData->LateralAccValueIdU16 = VALUE_ID_LATERAL_ACCELERATION; + DOTMData->LateralAccContentLengthU16 = 2; + DOTMData->LateralAccI16 = SwapI16((I16) Data); + if (debug) + LogMessage(LOG_LEVEL_DEBUG, "LateralAccI16=%d", DOTMData->LateralAccI16); + + //Curvature + Data = 0; + Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 29); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 28); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 27); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 26); + DOTMData->CurvatureValueIdU16 = VALUE_ID_CURVATURE; + DOTMData->CurvatureContentLengthU16 = 4; + DOTMData->CurvatureI32 = SwapI32((I32) Data); + if (debug) + printf("CurvatureI32=%d \n", DOTMData->CurvatureI32); + + p = (C8 *) DOTMData; + for (j = 0; j < sizeof (DOTMType); j++, n++) + *(MessageBuffer + n) = *p++; + MessageIndex = n; + } + + + Crc = crc_16((const C8 *)MessageBuffer, sizeof (DOTMType)); + Crc = 0; + *(MessageBuffer + MessageIndex++) = (U8) (Crc); + *(MessageBuffer + MessageIndex++) = (U8) (Crc >> 8); + + + if (debug) { + int i = 0; + + for (i = 0; i < MessageIndex; i++) { + // TODO: Write to log when bytes thingy has been implemented + if ((unsigned char)MessageBuffer[i] >= 0 && (unsigned char)MessageBuffer[i] <= 15) + printf("0"); + printf("%x-", (unsigned char)MessageBuffer[i]); + } + printf("\n"); + } + + return MessageIndex; //Total number of bytes } static int iGetObjectIndexFromObjectIP(in_addr_t ipAddr, in_addr_t objectIPs[], unsigned int numberOfObjects) { - for (unsigned int i = 0; i < numberOfObjects; ++i) { - if (objectIPs[i] == ipAddr) - return (int)i; - } - return -1; + for (unsigned int i = 0; i < numberOfObjects; ++i) { + if (objectIPs[i] == ipAddr) + return (int)i; + } + return -1; } static I32 vConnectObject(int *sockfd, const char *name, const uint32_t port, U8 * Disconnect) { - struct sockaddr_in serv_addr; - struct hostent *server; + struct sockaddr_in serv_addr; + struct hostent *server; - char buffer[256]; - int iResult; + char buffer[256]; + int iResult; - *sockfd = socket(AF_INET, SOCK_STREAM, 0); + *sockfd = socket(AF_INET, SOCK_STREAM, 0); - if (*sockfd < 0) { - util_error("[ObjectControl] ERR: Failed to open control socket"); - } + if (*sockfd < 0) { + util_error("[ObjectControl] ERR: Failed to open control socket"); + } - server = gethostbyname(name); - if (server == NULL) { - util_error("[ObjectControl] ERR: Unknown host "); - } + server = gethostbyname(name); + if (server == NULL) { + util_error("[ObjectControl] ERR: Unknown host "); + } - bzero((char *)&serv_addr, sizeof (serv_addr)); - serv_addr.sin_family = AF_INET; + bzero((char *)&serv_addr, sizeof (serv_addr)); + serv_addr.sin_family = AF_INET; - bcopy((char *)server->h_addr, (char *)&serv_addr.sin_addr.s_addr, server->h_length); - serv_addr.sin_port = htons(port); + bcopy((char *)server->h_addr, (char *)&serv_addr.sin_addr.s_addr, server->h_length); + serv_addr.sin_port = htons(port); - LogMessage(LOG_LEVEL_INFO, "Attempting to connect to socket: %s %i", name, port); + LogMessage(LOG_LEVEL_INFO, "Attempting to connect to socket: %s %i", name, port); - // do - { - iResult = connect(*sockfd, (struct sockaddr *)&serv_addr, sizeof (serv_addr)); + // do + { + iResult = connect(*sockfd, (struct sockaddr *)&serv_addr, sizeof (serv_addr)); - /*if ( iResult < 0) - { - if(errno == ECONNREFUSED) - { - printf("WARNiNG: Was not able to connect to object, [IP: %s] [PORT: %d], %d retry in 3 sec...\n",name,port, *Disconnect); - fflush(stdout); - (void)sleep(3); - } - else - { - util_error("ERR: Failed to connect to control socket"); - } */ - } - //} while(iResult < 0 && *Disconnect == 0); + /*if ( iResult < 0) + { + if(errno == ECONNREFUSED) + { + printf("WARNiNG: Was not able to connect to object, [IP: %s] [PORT: %d], %d retry in 3 sec...\n",name,port, *Disconnect); + fflush(stdout); + (void)sleep(3); + } + else + { + util_error("ERR: Failed to connect to control socket"); + } */ + } + //} while(iResult < 0 && *Disconnect == 0); - LogMessage(LOG_LEVEL_INFO, "Connected to command socket: %s %i", name, port); - // Enable polling of status to detect remote disconnect - fcntl(*sockfd, F_SETFL, O_NONBLOCK); + LogMessage(LOG_LEVEL_INFO, "Connected to command socket: %s %i", name, port); + // Enable polling of status to detect remote disconnect + fcntl(*sockfd, F_SETFL, O_NONBLOCK); - return iResult; + return iResult; } static void vDisconnectObject(int *sockfd) { - close(*sockfd); + close(*sockfd); } static void vCreateSafetyChannel(const char *name, const uint32_t port, int *sockfd, struct sockaddr_in *addr) { - int result; - struct hostent *object; + int result; + struct hostent *object; - /* Connect to object safety socket */ - LogMessage(LOG_LEVEL_DEBUG, "Creating safety socket"); + /* Connect to object safety socket */ + LogMessage(LOG_LEVEL_DEBUG, "Creating safety socket"); - *sockfd = socket(AF_INET, SOCK_DGRAM, 0); - if (*sockfd < 0) { - util_error("ERR: Failed to connect to monitor socket"); - } + *sockfd = socket(AF_INET, SOCK_DGRAM, 0); + if (*sockfd < 0) { + util_error("ERR: Failed to connect to monitor socket"); + } - /* Set address to object */ - object = gethostbyname(name); + /* Set address to object */ + object = gethostbyname(name); - if (object == 0) { - util_error("ERR: Unknown host"); - } + if (object == 0) { + util_error("ERR: Unknown host"); + } - bcopy((char *)object->h_addr, (char *)&addr->sin_addr.s_addr, object->h_length); - addr->sin_family = AF_INET; - addr->sin_port = htons(port); + bcopy((char *)object->h_addr, (char *)&addr->sin_addr.s_addr, object->h_length); + addr->sin_family = AF_INET; + addr->sin_port = htons(port); - /* set socket to non-blocking */ - result = fcntl(*sockfd, F_SETFL, fcntl(*sockfd, F_GETFL, 0) | O_NONBLOCK); - if (result < 0) { - util_error("ERR: calling fcntl"); - } + /* set socket to non-blocking */ + result = fcntl(*sockfd, F_SETFL, fcntl(*sockfd, F_GETFL, 0) | O_NONBLOCK); + if (result < 0) { + util_error("ERR: calling fcntl"); + } - LogMessage(LOG_LEVEL_INFO, "Created socket and safety address: %s:%d", name, port); + LogMessage(LOG_LEVEL_INFO, "Created socket and safety address: %s:%d", name, port); } static void vCloseSafetyChannel(int *sockfd) { - close(*sockfd); + close(*sockfd); } static I32 vCheckRemoteDisconnected(int *sockfd) { - char dummy; - ssize_t x = recv(*sockfd, &dummy, 1, MSG_PEEK); - - // Remote has disconnected: EOF => x=0 - if (x == 0) { - return 1; - } - - if (x == -1) { - // Everything is normal - no communication has been received - if (errno == EAGAIN || errno == EWOULDBLOCK) - return 0; - - // Other error occurred - LogMessage(LOG_LEVEL_WARNING, "Error when checking connection status"); - return 1; - } - - // Something has been received on socket - if (x > 0) { - LogMessage(LOG_LEVEL_INFO, "Received unexpected communication from object on command channel"); - return 0; - } - - return 1; + char dummy; + ssize_t x = recv(*sockfd, &dummy, 1, MSG_PEEK); + + // Remote has disconnected: EOF => x=0 + if (x == 0) { + return 1; + } + + if (x == -1) { + // Everything is normal - no communication has been received + if (errno == EAGAIN || errno == EWOULDBLOCK) + return 0; + + // Other error occurred + LogMessage(LOG_LEVEL_WARNING, "Error when checking connection status"); + return 1; + } + + // Something has been received on socket + if (x > 0) { + LogMessage(LOG_LEVEL_INFO, "Received unexpected communication from object on command channel"); + return 0; + } + + return 1; } int ObjectControlSendUDPData(int *sockfd, struct sockaddr_in *addr, char *SendData, int Length, char debug) { - ssize_t result; + ssize_t result; - // TODO: Change to log write when bytes thingy has been implemented - if (debug) { - printf("Bytes sent: "); - int i = 0; + // TODO: Change to log write when bytes thingy has been implemented + if (debug) { + printf("Bytes sent: "); + int i = 0; - for (i = 0; i < Length; i++) - printf("%x-", (unsigned char)*(SendData + i)); - printf("\n"); - } + for (i = 0; i < Length; i++) + printf("%x-", (unsigned char)*(SendData + i)); + printf("\n"); + } - result = sendto(*sockfd, SendData, Length, 0, (const struct sockaddr *)addr, sizeof (struct sockaddr_in)); + result = sendto(*sockfd, SendData, Length, 0, (const struct sockaddr *)addr, sizeof (struct sockaddr_in)); - if (result < 0) { - util_error("ERR: Failed to send on monitor socket"); - } + if (result < 0) { + util_error("ERR: Failed to send on monitor socket"); + } - return 0; + return 0; } static size_t uiRecvMonitor(int *sockfd, char *buffer, size_t length) { - ssize_t result = 0; - size_t recvDataSize = 0; - - // Read until receive buffer is empty, return last read message - do { - result = recv(*sockfd, buffer, length, 0); - - if (result < 0) { - if (errno != EAGAIN && errno != EWOULDBLOCK) { - util_error("Failed to receive from monitor socket"); - } - } - else { - recvDataSize = (size_t) (result); - LogMessage(LOG_LEVEL_DEBUG, "Received: <%s>", buffer); - } - } while (result > 0); - - return recvDataSize; + ssize_t result = 0; + size_t recvDataSize = 0; + + // Read until receive buffer is empty, return last read message + do { + result = recv(*sockfd, buffer, length, 0); + + if (result < 0) { + if (errno != EAGAIN && errno != EWOULDBLOCK) { + util_error("Failed to receive from monitor socket"); + } + } + else { + recvDataSize = (size_t) (result); + LogMessage(LOG_LEVEL_DEBUG, "Received: <%s>", buffer); + } + } while (result > 0); + + return recvDataSize; } int iFindObjectsInfo(C8 object_traj_file[MAX_OBJECTS][MAX_FILE_PATH], - C8 object_address_name[MAX_OBJECTS][MAX_FILE_PATH], in_addr_t objectIPs[MAX_OBJECTS], - I32 * nbr_objects) { - DIR *traj_directory; - struct dirent *directory_entry; - int iForceObjectToLocalhost; - struct sockaddr_in sockaddr; - int result; - char trajPathDir[MAX_FILE_PATH]; - int retval = 0; - - UtilGetTrajDirectoryPath(trajPathDir, sizeof (trajPathDir)); - - iForceObjectToLocalhost = 0; - - traj_directory = opendir(trajPathDir); - if (traj_directory == NULL) { - util_error("ERR: Failed to open trajectory directory"); - } - - (void)iUtilGetIntParaConfFile("ForceObjectToLocalhost", &iForceObjectToLocalhost); - - while ((directory_entry = readdir(traj_directory)) && ((*nbr_objects) < MAX_OBJECTS)) { - - /* Check so it's not . or .. */ - if (strncmp(directory_entry->d_name, ".", 1) && (strstr(directory_entry->d_name, "sync") == NULL)) { - bzero(object_address_name[(*nbr_objects)], MAX_FILE_PATH); - - bzero(object_traj_file[(*nbr_objects)], MAX_FILE_PATH); - (void)strcat(object_traj_file[(*nbr_objects)], trajPathDir); - (void)strcat(object_traj_file[(*nbr_objects)], directory_entry->d_name); - - if (UtilCheckTrajectoryFileFormat - (object_traj_file[*nbr_objects], sizeof (object_traj_file[*nbr_objects]))) { - LogMessage(LOG_LEVEL_ERROR, "Trajectory file <%s> is not valid", - object_traj_file[*nbr_objects]); - retval = -1; - } - - if (0 == iForceObjectToLocalhost) { - (void)strncat(object_address_name[(*nbr_objects)], directory_entry->d_name, - strlen(directory_entry->d_name)); - result = inet_pton(AF_INET, object_address_name[*nbr_objects], &sockaddr.sin_addr); - if (result == -1) { - LogMessage(LOG_LEVEL_ERROR, "Invalid address family"); - retval = -1; - continue; - } - else if (result == 0) { - LogMessage(LOG_LEVEL_WARNING, "Address <%s> is not a valid IPv4 address", - object_address_name[*nbr_objects]); - retval = -1; - continue; - } - else - objectIPs[*nbr_objects] = sockaddr.sin_addr.s_addr; - } - else { - if (USE_TEST_HOST == 0) - (void)strcat(object_address_name[(*nbr_objects)], LOCALHOST); - else if (USE_TEST_HOST == 1) - (void)strcat(object_address_name[(*nbr_objects)], TESTHOST_IP); - - } - - ++(*nbr_objects); - } - } - (void)closedir(traj_directory); - return retval; + C8 object_address_name[MAX_OBJECTS][MAX_FILE_PATH], in_addr_t objectIPs[MAX_OBJECTS], + I32 * nbr_objects) { + DIR *traj_directory; + struct dirent *directory_entry; + int iForceObjectToLocalhost; + struct sockaddr_in sockaddr; + int result; + char trajPathDir[MAX_FILE_PATH]; + int retval = 0; + + UtilGetTrajDirectoryPath(trajPathDir, sizeof (trajPathDir)); + + iForceObjectToLocalhost = 0; + + traj_directory = opendir(trajPathDir); + if (traj_directory == NULL) { + util_error("ERR: Failed to open trajectory directory"); + } + + (void)iUtilGetIntParaConfFile("ForceObjectToLocalhost", &iForceObjectToLocalhost); + + while ((directory_entry = readdir(traj_directory)) && ((*nbr_objects) < MAX_OBJECTS)) { + + /* Check so it's not . or .. */ + if (strncmp(directory_entry->d_name, ".", 1) && (strstr(directory_entry->d_name, "sync") == NULL)) { + bzero(object_address_name[(*nbr_objects)], MAX_FILE_PATH); + + bzero(object_traj_file[(*nbr_objects)], MAX_FILE_PATH); + (void)strcat(object_traj_file[(*nbr_objects)], trajPathDir); + (void)strcat(object_traj_file[(*nbr_objects)], directory_entry->d_name); + + if (UtilCheckTrajectoryFileFormat + (object_traj_file[*nbr_objects], sizeof (object_traj_file[*nbr_objects]))) { + LogMessage(LOG_LEVEL_ERROR, "Trajectory file <%s> is not valid", + object_traj_file[*nbr_objects]); + retval = -1; + } + + if (0 == iForceObjectToLocalhost) { + (void)strncat(object_address_name[(*nbr_objects)], directory_entry->d_name, + strlen(directory_entry->d_name)); + result = inet_pton(AF_INET, object_address_name[*nbr_objects], &sockaddr.sin_addr); + if (result == -1) { + LogMessage(LOG_LEVEL_ERROR, "Invalid address family"); + retval = -1; + continue; + } + else if (result == 0) { + LogMessage(LOG_LEVEL_WARNING, "Address <%s> is not a valid IPv4 address", + object_address_name[*nbr_objects]); + retval = -1; + continue; + } + else + objectIPs[*nbr_objects] = sockaddr.sin_addr.s_addr; + } + else { + if (USE_TEST_HOST == 0) + (void)strcat(object_address_name[(*nbr_objects)], LOCALHOST); + else if (USE_TEST_HOST == 1) + (void)strcat(object_address_name[(*nbr_objects)], TESTHOST_IP); + + } + + ++(*nbr_objects); + } + } + (void)closedir(traj_directory); + return retval; } OBCState_t vGetState(GSDType * GSD) { - return DataDictionaryGetOBCStateU8(GSD); + return DataDictionaryGetOBCStateU8(GSD); } StateTransitionResult vSetState(OBCState_t requestedState, GSDType * GSD) { - StateTransition transitionFunction; - StateTransitionResult retval = TRANSITION_RESULT_UNDEFINED; - OBCState_t currentState = DataDictionaryGetOBCStateU8(GSD); - - // Always allow transitions to these two states - if (requestedState == OBC_STATE_ERROR || requestedState == OBC_STATE_UNDEFINED) { - if (DataDictionarySetOBCStateU8(GSD, requestedState) == WRITE_OK) { - LogMessage(LOG_LEVEL_WARNING, "Transitioning to state %u", (unsigned char)requestedState); - retval = TRANSITION_OK; - } - else - retval = TRANSITION_MEMORY_ERROR; - } - else if (requestedState == currentState) { - retval = TRANSITION_OK; - } - else { - transitionFunction = tGetTransition(currentState); - retval = transitionFunction(¤tState, requestedState); - if (retval != TRANSITION_INVALID) { - if (DataDictionarySetOBCStateU8(GSD, currentState) == WRITE_OK) { - LogMessage(LOG_LEVEL_INFO, "Transitioning to state %u", (unsigned char)requestedState); - retval = TRANSITION_OK; - } - else - retval = TRANSITION_MEMORY_ERROR; - } - } - - if (retval == TRANSITION_INVALID) { - LogMessage(LOG_LEVEL_WARNING, "Invalid transition requested: from %d to %d", currentState, - requestedState); - } - else if (retval == TRANSITION_MEMORY_ERROR) { - LogMessage(LOG_LEVEL_ERROR, "Unable to set state to %u in shared memory!!", requestedState); - } - return retval; + StateTransition transitionFunction; + StateTransitionResult retval = TRANSITION_RESULT_UNDEFINED; + OBCState_t currentState = DataDictionaryGetOBCStateU8(GSD); + + // Always allow transitions to these two states + if (requestedState == OBC_STATE_ERROR || requestedState == OBC_STATE_UNDEFINED) { + if (DataDictionarySetOBCStateU8(GSD, requestedState) == WRITE_OK) { + LogMessage(LOG_LEVEL_WARNING, "Transitioning to state %u", (unsigned char)requestedState); + retval = TRANSITION_OK; + } + else + retval = TRANSITION_MEMORY_ERROR; + } + else if (requestedState == currentState) { + retval = TRANSITION_OK; + } + else { + transitionFunction = tGetTransition(currentState); + retval = transitionFunction(¤tState, requestedState); + if (retval != TRANSITION_INVALID) { + if (DataDictionarySetOBCStateU8(GSD, currentState) == WRITE_OK) { + LogMessage(LOG_LEVEL_INFO, "Transitioning to state %u", (unsigned char)requestedState); + retval = TRANSITION_OK; + } + else + retval = TRANSITION_MEMORY_ERROR; + } + } + + if (retval == TRANSITION_INVALID) { + LogMessage(LOG_LEVEL_WARNING, "Invalid transition requested: from %d to %d", currentState, + requestedState); + } + else if (retval == TRANSITION_MEMORY_ERROR) { + LogMessage(LOG_LEVEL_ERROR, "Unable to set state to %u in shared memory!!", requestedState); + } + return retval; } StateTransition tGetTransition(OBCState_t fromState) { - switch (fromState) { - case OBC_STATE_IDLE: - return &tFromIdle; - case OBC_STATE_INITIALIZED: - return &tFromInitialized; - case OBC_STATE_CONNECTED: - return &tFromConnected; - case OBC_STATE_ARMED: - return &tFromArmed; - case OBC_STATE_RUNNING: - return &tFromRunning; - case OBC_STATE_ERROR: - return &tFromError; - case OBC_STATE_UNDEFINED: - return &tFromUndefined; - } + switch (fromState) { + case OBC_STATE_IDLE: + return &tFromIdle; + case OBC_STATE_INITIALIZED: + return &tFromInitialized; + case OBC_STATE_CONNECTED: + return &tFromConnected; + case OBC_STATE_ARMED: + return &tFromArmed; + case OBC_STATE_RUNNING: + return &tFromRunning; + case OBC_STATE_ERROR: + return &tFromError; + case OBC_STATE_UNDEFINED: + return &tFromUndefined; + } } StateTransitionResult tFromIdle(OBCState_t * currentState, OBCState_t requestedState) { - if (requestedState == OBC_STATE_INITIALIZED) { - *currentState = requestedState; - return TRANSITION_OK; - } - return TRANSITION_INVALID; + if (requestedState == OBC_STATE_INITIALIZED) { + *currentState = requestedState; + return TRANSITION_OK; + } + return TRANSITION_INVALID; } StateTransitionResult tFromInitialized(OBCState_t * currentState, OBCState_t requestedState) { - if (requestedState == OBC_STATE_CONNECTED || requestedState == OBC_STATE_IDLE) { - *currentState = requestedState; - return TRANSITION_OK; - } - return TRANSITION_INVALID; + if (requestedState == OBC_STATE_CONNECTED || requestedState == OBC_STATE_IDLE) { + *currentState = requestedState; + return TRANSITION_OK; + } + return TRANSITION_INVALID; } StateTransitionResult tFromConnected(OBCState_t * currentState, OBCState_t requestedState) { - if (requestedState == OBC_STATE_ARMED || requestedState == OBC_STATE_IDLE) { - *currentState = requestedState; - return TRANSITION_OK; - } - return TRANSITION_INVALID; + if (requestedState == OBC_STATE_ARMED || requestedState == OBC_STATE_IDLE) { + *currentState = requestedState; + return TRANSITION_OK; + } + return TRANSITION_INVALID; } StateTransitionResult tFromArmed(OBCState_t * currentState, OBCState_t requestedState) { - if (requestedState == OBC_STATE_CONNECTED || requestedState == OBC_STATE_RUNNING - || requestedState == OBC_STATE_IDLE) { - *currentState = requestedState; - return TRANSITION_OK; - } - return TRANSITION_INVALID; + if (requestedState == OBC_STATE_CONNECTED || requestedState == OBC_STATE_RUNNING + || requestedState == OBC_STATE_IDLE) { + *currentState = requestedState; + return TRANSITION_OK; + } + return TRANSITION_INVALID; } StateTransitionResult tFromRunning(OBCState_t * currentState, OBCState_t requestedState) { - if (requestedState == OBC_STATE_CONNECTED || requestedState == OBC_STATE_IDLE) { - *currentState = requestedState; - return TRANSITION_OK; - } - return TRANSITION_INVALID; + if (requestedState == OBC_STATE_CONNECTED || requestedState == OBC_STATE_IDLE) { + *currentState = requestedState; + return TRANSITION_OK; + } + return TRANSITION_INVALID; } StateTransitionResult tFromError(OBCState_t * currentState, OBCState_t requestedState) { - if (requestedState == OBC_STATE_IDLE) { - *currentState = requestedState; - return TRANSITION_OK; - } - return TRANSITION_INVALID; + if (requestedState == OBC_STATE_IDLE) { + *currentState = requestedState; + return TRANSITION_OK; + } + return TRANSITION_INVALID; } StateTransitionResult tFromUndefined(OBCState_t * currentState, OBCState_t requestedState) { - return TRANSITION_INVALID; + return TRANSITION_INVALID; } OBCState_t vInitializeState(OBCState_t firstState, GSDType * GSD) { - static int8_t isInitialized = 0; - - if (!isInitialized) { - isInitialized = 1; - if (DataDictionarySetOBCStateU8(GSD, firstState) != WRITE_OK) - util_error("Unable to write object control state to shared memory"); - } - else { - LogMessage(LOG_LEVEL_WARNING, "Object control state already initialized"); - } - return DataDictionaryGetOBCStateU8(GSD); + static int8_t isInitialized = 0; + + if (!isInitialized) { + isInitialized = 1; + if (DataDictionarySetOBCStateU8(GSD, firstState) != WRITE_OK) + util_error("Unable to write object control state to shared memory"); + } + else { + LogMessage(LOG_LEVEL_WARNING, "Object control state already initialized"); + } + return DataDictionaryGetOBCStateU8(GSD); } From 63a0c6a4d03e0661290fe1e849ae6a98188c46e6 Mon Sep 17 00:00:00 2001 From: alfaro01 Date: Thu, 16 Jan 2020 16:07:59 +0100 Subject: [PATCH 060/523] Memory now allocated dynamically. using Malloc. Also added TransmitID and GPSSecondsofWeek --- modules/Visualization/src/main.c | 52 +++++++++++++++++++++++++------- 1 file changed, 41 insertions(+), 11 deletions(-) diff --git a/modules/Visualization/src/main.c b/modules/Visualization/src/main.c index 997325fd7..26a5fc7b4 100644 --- a/modules/Visualization/src/main.c +++ b/modules/Visualization/src/main.c @@ -23,20 +23,37 @@ #define VISUAL_REPLAY_MODE 1 #define SMALL_ITEM_TEXT_BUFFER_SIZE 20 +#define MAX_DATE_STRLEN 25 // Maximum string length of a time stamp on the format "2035;12;31;24;59;59;1000" is 25 + /*------------------------------------------------------------ -- Function declarations. ------------------------------------------------------------*/ static void vConnectVisualizationChannel(int *sockfd, struct sockaddr_in *addr); static void vDisconnectVisualizationChannel(int *sockfd); -void vCreateVisualizationMessage(MonitorDataType *_monitorData, char *_visualizationMessage, int _debug); +void vCreateVisualizationMessage(MonitorDataType *_monitorData, char *_visualizationMessage, int _sizeOfVisualizationMessage, int _debug); -void vCreateVisualizationMessage(MonitorDataType *_monitorData, char *_visualizationMessage, int _debug) +void vCreateVisualizationMessage(MonitorDataType *_monitorData, char *_visualizationMessage, int _sizeOfVisualizationMessage, int _debug) { - sprintf(_visualizationMessage,"%d;%d;%d;%d", _monitorData->MONR.XPositionI32, _monitorData->MONR.YPositionI32, _monitorData->MONR.ZPositionI32, _monitorData->MONR.HeadingU16); + + //IP + char ipStringBuffer[INET_ADDRSTRLEN]; + sprintf(ipStringBuffer, "%s", inet_ntop(AF_INET, &_monitorData->ClientIP, ipStringBuffer, sizeof (ipStringBuffer))); + + //Build message from MonitorStruct + snprintf(_visualizationMessage, _sizeOfVisualizationMessage, "%s;%u;%u;%d;%d;%d;%u;", + ipStringBuffer, + _monitorData->MONR.Header.TransmitterIdU8, + _monitorData->MONR.GPSQmsOfWeekU32, + _monitorData->MONR.XPositionI32, + _monitorData->MONR.YPositionI32, + _monitorData->MONR.ZPositionI32, + _monitorData->MONR.HeadingU16); + if(_debug) { + LogMessage(LOG_LEVEL_INFO, "%s", _visualizationMessage); LogMessage(LOG_LEVEL_INFO, "X: %d", _monitorData->MONR.XPositionI32); LogMessage(LOG_LEVEL_INFO, "Y: %d", _monitorData->MONR.YPositionI32); LogMessage(LOG_LEVEL_INFO, "Z: %d", _monitorData->MONR.ZPositionI32); @@ -54,10 +71,15 @@ int main() { struct timespec remTime; MonitorDataType monitorData; - int sizeOfMessage = (sizeof (monitorData.MONR.XPositionI32) + - sizeof (monitorData.MONR.YPositionI32) + - sizeof (monitorData.MONR.ZPositionI32) + - sizeof (monitorData.MONR.HeadingU16)); + + int sizeOfVisualizationMessage = (INET_ADDRSTRLEN + + sizeof (monitorData.MONR.Header.TransmitterIdU8) + + sizeof (monitorData.MONR.XPositionI32) + + sizeof (monitorData.MONR.YPositionI32) + + sizeof (monitorData.MONR.ZPositionI32) + + sizeof (monitorData.MONR.HeadingU16) + + 6 + //Number of fields (;) + 1); //Required LogInit(MODULE_NAME, LOG_LEVEL_DEBUG); LogMessage(LOG_LEVEL_INFO, "Task running with PID: %u", getpid()); @@ -76,6 +98,8 @@ int main() { } while (true) { + + if (iCommRecv(&command, mqRecvData, MQ_MSG_SIZE, NULL) < 0) { util_error("Message bus receive error"); } @@ -100,16 +124,22 @@ int main() { break; case COMM_MONR: { + //Populate the monitorType UtilPopulateMonitorDataStruct(mqRecvData, (size_t) (sizeof (mqRecvData)), &monitorData, 0); + //Allocate memory + char *visualizationMessage = malloc(sizeOfVisualizationMessage * sizeof (char)); + //Create visualization message and insert values from the monitor datastruct above - char visualizationMessage[42]; - vCreateVisualizationMessage(&monitorData, visualizationMessage, 0); + vCreateVisualizationMessage(&monitorData, visualizationMessage, sizeOfVisualizationMessage, 0); //Send visualization message on the UDP socket - UtilSendUDPData("Visualization", &visual_server, &visual_server_addr, visualizationMessage, - sizeof (visualizationMessage), 0); + UtilSendUDPData((const uint8_t *)"Visualization", &visual_server, &visual_server_addr, visualizationMessage, + sizeOfVisualizationMessage, 0); + + //Free memory used by malloc + free(visualizationMessage); } break; From 5d2bc1dea67e64148506384d3257362a7bfd7e5d Mon Sep 17 00:00:00 2001 From: alfaro01 Date: Thu, 16 Jan 2020 16:10:30 +0100 Subject: [PATCH 061/523] Bugfix --- modules/Visualization/src/main.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/modules/Visualization/src/main.c b/modules/Visualization/src/main.c index 26a5fc7b4..fb227c42c 100644 --- a/modules/Visualization/src/main.c +++ b/modules/Visualization/src/main.c @@ -74,11 +74,12 @@ int main() { int sizeOfVisualizationMessage = (INET_ADDRSTRLEN + sizeof (monitorData.MONR.Header.TransmitterIdU8) + + sizeof (monitorData.MONR.GPSQmsOfWeekU32) + sizeof (monitorData.MONR.XPositionI32) + sizeof (monitorData.MONR.YPositionI32) + sizeof (monitorData.MONR.ZPositionI32) + sizeof (monitorData.MONR.HeadingU16) + - 6 + //Number of fields (;) + 8 + //Number of fields + 1 (;) 1); //Required LogInit(MODULE_NAME, LOG_LEVEL_DEBUG); From 63e6b59b09994892885de1ec44a70f8b63cb3a38 Mon Sep 17 00:00:00 2001 From: alfaro01 Date: Thu, 16 Jan 2020 16:26:40 +0100 Subject: [PATCH 062/523] Changed output on UDP to be in unison with Chronos2 output. --- modules/Visualization/src/main.c | 30 +++++++++++++++++------------- 1 file changed, 17 insertions(+), 13 deletions(-) diff --git a/modules/Visualization/src/main.c b/modules/Visualization/src/main.c index fb227c42c..a088d5d8e 100644 --- a/modules/Visualization/src/main.c +++ b/modules/Visualization/src/main.c @@ -41,25 +41,28 @@ void vCreateVisualizationMessage(MonitorDataType *_monitorData, char *_visualiza sprintf(ipStringBuffer, "%s", inet_ntop(AF_INET, &_monitorData->ClientIP, ipStringBuffer, sizeof (ipStringBuffer))); //Build message from MonitorStruct - snprintf(_visualizationMessage, _sizeOfVisualizationMessage, "%s;%u;%u;%d;%d;%d;%u;", - ipStringBuffer, - _monitorData->MONR.Header.TransmitterIdU8, - _monitorData->MONR.GPSQmsOfWeekU32, - _monitorData->MONR.XPositionI32, - _monitorData->MONR.YPositionI32, - _monitorData->MONR.ZPositionI32, - _monitorData->MONR.HeadingU16); + snprintf(_visualizationMessage, _sizeOfVisualizationMessage, "%s;%u;%d;%d;%d;%u;%d;%u;", + ipStringBuffer, + _monitorData->MONR.GPSQmsOfWeekU32, + _monitorData->MONR.XPositionI32, + _monitorData->MONR.YPositionI32, + _monitorData->MONR.ZPositionI32, + _monitorData->MONR.HeadingU16, + _monitorData->MONR.LongitudinalSpeedI16, + _monitorData->MONR.StateU8); if(_debug) { - LogMessage(LOG_LEVEL_INFO, "%s", _visualizationMessage); + //LogMessage(LOG_LEVEL_INFO, "%s", _visualizationMessage); + LogMessage(LOG_LEVEL_INFO, "IP: %s", ipStringBuffer); + LogMessage(LOG_LEVEL_INFO, "GPSQmsOfWeek: %u", _monitorData->MONR.GPSQmsOfWeekU32); LogMessage(LOG_LEVEL_INFO, "X: %d", _monitorData->MONR.XPositionI32); LogMessage(LOG_LEVEL_INFO, "Y: %d", _monitorData->MONR.YPositionI32); LogMessage(LOG_LEVEL_INFO, "Z: %d", _monitorData->MONR.ZPositionI32); - LogMessage(LOG_LEVEL_INFO, "Heading: %d", _monitorData->MONR.HeadingU16); - LogMessage(LOG_LEVEL_INFO, "LatAcc: %d", _monitorData->MONR.LateralAccI16); - LogMessage(LOG_LEVEL_INFO, "LongAcc: %d", _monitorData->MONR.LongitudinalAccI16); + LogMessage(LOG_LEVEL_INFO, "Heading: %u", _monitorData->MONR.HeadingU16); + LogMessage(LOG_LEVEL_INFO, "LongSpeed: %d", _monitorData->MONR.LongitudinalSpeedI16); + LogMessage(LOG_LEVEL_INFO, "State: %u", _monitorData->MONR.StateU8); } } @@ -73,12 +76,13 @@ int main() { MonitorDataType monitorData; int sizeOfVisualizationMessage = (INET_ADDRSTRLEN + - sizeof (monitorData.MONR.Header.TransmitterIdU8) + sizeof (monitorData.MONR.GPSQmsOfWeekU32) + sizeof (monitorData.MONR.XPositionI32) + sizeof (monitorData.MONR.YPositionI32) + sizeof (monitorData.MONR.ZPositionI32) + sizeof (monitorData.MONR.HeadingU16) + + sizeof (monitorData.MONR.LongitudinalSpeedI16) + + sizeof (monitorData.MONR.StateU8) + 8 + //Number of fields + 1 (;) 1); //Required From af3a3e0c5f5a3515e76b76e68c856da6aea03d48 Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Thu, 16 Jan 2020 16:52:33 +0100 Subject: [PATCH 063/523] Removed thing --- runServer.sh | 2 +- server/inc/datadictionary.h | 6 +++--- server/src/datadictionary.c | 17 ++++++++++++----- server/src/objectcontrol.c | 14 +++++++++----- 4 files changed, 25 insertions(+), 14 deletions(-) diff --git a/runServer.sh b/runServer.sh index 3b989f36f..19f3d7c45 100755 --- a/runServer.sh +++ b/runServer.sh @@ -2,7 +2,7 @@ #### User settings # Modify this array by adding more modules to include them in the execution -MODULES=(ScenarioControl) +MODULES=() #### # Save top directory diff --git a/server/inc/datadictionary.h b/server/inc/datadictionary.h index 3e87eb198..339911fda 100644 --- a/server/inc/datadictionary.h +++ b/server/inc/datadictionary.h @@ -120,9 +120,9 @@ ReadWriteAccess_t DataDictionarySetOBCStateU8(GSDType *GSD, OBCState_t OBCState) OBCState_t DataDictionaryGetOBCStateU8(GSDType *GSD); ReadWriteAccess_t DataDictionaryFreeMONR(GSDType *GSD); -ReadWriteAccess_t DataDictionaryInitMONR(GSDType *GSD); -ReadWriteAccess_t DataDictionarySetMONR(GSDType *GSD, MONRType * MONR, U8 size); -ReadWriteAccess_t DataDictionaryGetMONR(GSDType *GSD, MONRType * MONR, U8 size); +ReadWriteAccess_t DataDictionaryInitMONR(GSDType *GSD, U8 objectCount); +ReadWriteAccess_t DataDictionarySetMONR(GSDType *GSD, MONRType * MONR, U8 index); +ReadWriteAccess_t DataDictionaryGetMONR(GSDType *GSD, MONRType * MONR, U8 index); /*END of MONR*/ diff --git a/server/src/datadictionary.c b/server/src/datadictionary.c index ba7b9506e..c3d53c403 100644 --- a/server/src/datadictionary.c +++ b/server/src/datadictionary.c @@ -91,7 +91,7 @@ ReadWriteAccess_t DataDictionaryConstructor(GSDType * GSD) { Res = Res == READ_OK ? DataDictionaryInitMiscDataC8(GSD) : Res; DataDictionarySetOBCStateU8(GSD, OBC_STATE_UNDEFINED); - GSD->test = (MONRType**)malloc(sizeof(MONRType*)); + return Res; } @@ -1673,22 +1673,29 @@ OBCState_t DataDictionaryGetOBCStateU8(GSDType * GSD) { * \param objects number of objects that will transmitt monr * \return Result according to ::ReadWriteAccess_t */ -ReadWriteAccess_t DataDictionaryInitMONR(GSDType * GSD){ +ReadWriteAccess_t DataDictionaryInitMONR(GSDType * GSD, U8 objectCount){ ReadWriteAccess_t Res; Res = WRITE_OK; pthread_mutex_lock(&MONRMutex); - GSD->test = (MONRType*)malloc(sizeof(MONRType)); - LogPrint("Size of MONRtype = %d", sizeof (MONRType)); + MONRType *ptr = malloc(sizeof (MONRType) * objectCount); + GSD->test = ptr; + + LogPrint("Very good nice."); + + LogPrint("Very good nice 2."); + // Check if the memory has been successfully - if (GSD->test == NULL) { + /*if (GSD->test == NULL) { LogPrint(LOG_LEVEL_ERROR, "Coult not allocate memmory for MONR"); } else { // Memory has been successfully allocated LogPrint(LOG_LEVEL_INFO, "Allocated pointer for MONR."); } + */ + LogPrint("Thats the stuff"); pthread_mutex_unlock(&MONRMutex); return Res; diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 80fe48f11..a3f6912f1 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -488,7 +488,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { //Store MONR in GSD //UtilSendUDPData("ObjectControl", &ObjectControlUDPSocketfdI32, &simulator_addr, &MONRData, sizeof(MONRData), 0); - /*for (i = 0; + for (i = 0; i < (MONRData.Header.MessageLengthU32 + COMMAND_MESSAGE_HEADER_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); i++) @@ -496,10 +496,6 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { GSD->MONRSizeU8 = MONRData.Header.MessageLengthU32 + COMMAND_MESSAGE_HEADER_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH; - */ - - DataDictionarySetMONR(GSD, &MONRData, 0); - ObjectControlMONRToASCII(&MONRData, &OriginPosition, iIndex, Id, Timestamp, XPosition, YPosition, ZPosition, LongitudinalSpeed, LateralSpeed, @@ -734,6 +730,10 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { else if (iCommand == COMM_INIT) { LogMessage(LOG_LEVEL_INFO, "INIT received"); + LogMessage(LOG_LEVEL_INFO, "Try to do the thing"); + DataDictionaryInitMONR(GSD, 1); + LogMessage(LOG_LEVEL_INFO, "Did the thing"); + LOG_SEND(LogBuffer, "[ObjectControl] INIT received."); nbr_objects = 0; if (iFindObjectsInfo(object_traj_file, object_address_name, objectIPs, &nbr_objects) == 0) { @@ -815,6 +815,10 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { LogMessage(LOG_LEVEL_INFO, "CONNECT received"); LOG_SEND(LogBuffer, "[ObjectControl] CONNECT received."); + LogMessage(LOG_LEVEL_INFO, "Try to do the other thing"); + DataDictionaryFreeMONR(GSD); + LogMessage(LOG_LEVEL_INFO, "Did the thing"); + /* Connect and send drive files */ for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { From 3fb9f7dc9ead12ba528dafadd0ffbd331db81ef9 Mon Sep 17 00:00:00 2001 From: LukasWikander Date: Thu, 16 Jan 2020 17:37:27 +0100 Subject: [PATCH 064/523] Function description added --- server/src/systemcontrol.c | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index f6e79a324..0a5f9259d 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -1168,7 +1168,17 @@ SystemControlCommand_t SystemControlFindCommand(const char *CommandBuffer, return nocommand; } - +/*! + * \brief SystemControlReceiveUserControlData Performs similarly to the recv function (see manpage for recv) except that it + * only fills the input data buffer with messages ending with ";\r\n\r\n" and saves any remaining data in a local + * buffer awaiting the next call to this function. + * \param socket Socket on which MSCP HTTP communication is expected to arrive + * \param dataBuffer Data buffer where read data is to be stored + * \param dataBufferLength Maximum number of bytes possible to store in the data buffer + * \return Number of bytes printed to dataBuffer where 0 means that the connection has been severed. A return value of -1 + * constitutes an error with the appropriate errno has been set (see manpage for recv) with the addition of + * - ENOBUFS if the data buffer is too small to hold the received message + */ ssize_t SystemControlReceiveUserControlData(I32 socket, C8 * dataBuffer, size_t dataBufferLength) { static char recvBuffer[TCP_RECV_BUFFER_SIZE]; static size_t bytesInBuffer = 0; From 04a5f4e24a04604e529b6c1c6d54fe66f21c1b71 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 17 Jan 2020 13:16:52 +0100 Subject: [PATCH 065/523] Moved MONR typedef to iso22133 header --- server/inc/iso22133.h | 24 ++++++++++++++++++++++++ server/inc/util.h | 21 +-------------------- 2 files changed, 25 insertions(+), 20 deletions(-) diff --git a/server/inc/iso22133.h b/server/inc/iso22133.h index 478b0bbcd..cb5920450 100644 --- a/server/inc/iso22133.h +++ b/server/inc/iso22133.h @@ -29,6 +29,30 @@ typedef struct uint16_t Crc; } FooterType; //2 bytes + +//! *************************** MONR +typedef struct +{ + HeaderType header; + uint16_t monrStructValueID; + uint16_t monrStructContentLength; + uint32_t gpsQmsOfWeek; + int32_t xPosition; + int32_t yPosition; + int32_t zPosition; + uint16_t heading; + int16_t longitudinalSpeed; + int16_t lateralSpeed; + int16_t longitudinalAcc; + int16_t lateralAcc; + uint8_t driveDirection; + uint8_t state; + uint8_t readyToArm; + uint8_t errorStatus; + FooterType footer; +} MONRType; //41 bytes + + //! *************************** TRCM #define COMMAND_TRCM_CODE 0x0011 typedef struct diff --git a/server/inc/util.h b/server/inc/util.h index f85df3b8a..806d5168a 100644 --- a/server/inc/util.h +++ b/server/inc/util.h @@ -378,26 +378,7 @@ typedef struct U8 CCStatusU8; } HEABType; //16 bytes -typedef struct -{ - HeaderType Header; - U16 MonrStructValueIdU16; - U16 MonrStructContentLengthU16; - U32 GPSQmsOfWeekU32; - I32 XPositionI32; - I32 YPositionI32; - I32 ZPositionI32; - U16 HeadingU16; - I16 LongitudinalSpeedI16; - I16 LateralSpeedI16; - I16 LongitudinalAccI16; - I16 LateralAccI16; - U8 DriveDirectionU8; - U8 StateU8; - U8 ReadyToArmU8; - U8 ErrorStatusU8; - U16 CRC; -} MONRType; //41 bytes + typedef struct { From 35a4f94b59d891ac69ad4f41e7132e204b11ad37 Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Fri, 17 Jan 2020 13:30:13 +0100 Subject: [PATCH 066/523] Getting setting --- server/inc/util.h | 2 +- server/src/datadictionary.c | 32 +- server/src/objectcontrol.c | 29 +- server/src/systemcontrol.c | 3814 +++++++++++++++++------------------ server/src/timecontrol.c | 846 ++++---- server/src/util.c | 6 +- 6 files changed, 2362 insertions(+), 2367 deletions(-) diff --git a/server/inc/util.h b/server/inc/util.h index e9f11898e..138ec8de2 100644 --- a/server/inc/util.h +++ b/server/inc/util.h @@ -533,7 +533,7 @@ typedef struct U8 HEABSizeU8; U8 HEABData[100]; - MONRType **test; + MONRType *MonrMessages; //U8 OSTMSizeU8; //U8 OSTMData[100]; //U8 STRTSizeU8; diff --git a/server/src/datadictionary.c b/server/src/datadictionary.c index c3d53c403..b838db369 100644 --- a/server/src/datadictionary.c +++ b/server/src/datadictionary.c @@ -1680,12 +1680,10 @@ ReadWriteAccess_t DataDictionaryInitMONR(GSDType * GSD, U8 objectCount){ pthread_mutex_lock(&MONRMutex); MONRType *ptr = malloc(sizeof (MONRType) * objectCount); - GSD->test = ptr; + GSD->MonrMessages = ptr; LogPrint("Very good nice."); - LogPrint("Very good nice 2."); - // Check if the memory has been successfully /*if (GSD->test == NULL) { LogPrint(LOG_LEVEL_ERROR, "Coult not allocate memmory for MONR"); @@ -1695,8 +1693,6 @@ ReadWriteAccess_t DataDictionaryInitMONR(GSDType * GSD, U8 objectCount){ LogPrint(LOG_LEVEL_INFO, "Allocated pointer for MONR."); } */ - LogPrint("Thats the stuff"); - pthread_mutex_unlock(&MONRMutex); return Res; } @@ -1712,19 +1708,7 @@ ReadWriteAccess_t DataDictionaryFreeMONR(GSDType * GSD){ Res = WRITE_OK; pthread_mutex_lock(&MONRMutex); - - - free(GSD->test); - - // Check if the memory has been freed successfully - if (GSD->test == NULL) { - LogPrint(LOG_LEVEL_INFO, "MONR array freed"); - } - else { - // Memory has been successfully allocated - LogPrint(LOG_LEVEL_ERROR, "Could not free monr data struct"); - } - + free(GSD->MonrMessages); pthread_mutex_unlock(&MONRMutex); return Res; } @@ -1737,12 +1721,14 @@ ReadWriteAccess_t DataDictionaryFreeMONR(GSDType * GSD){ * \param id object id * \return Result according to ::ReadWriteAccess_t */ -ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, MONRType * MONR, U8 size) { +ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, MONRType * MONR, U8 id) { ReadWriteAccess_t Res; Res = WRITE_OK; + pthread_mutex_lock(&MONRMutex); - GSD->test = MONR; + MONRType *test = GSD->MonrMessages; + test[id] = *MONR; pthread_mutex_unlock(&MONRMutex); return Res; @@ -1755,9 +1741,11 @@ ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, MONRType * MONR, U8 size) * \param id requesed object id * \return Result according to ::ReadWriteAccess_t */ -ReadWriteAccess_t DataDictionaryGetMONR(GSDType * GSD, MONRType * MONR, U8 id) { +ReadWriteAccess_t DataDictionaryGetMONR(GSDType * GSD, MONRType *MONR, U8 id) { pthread_mutex_lock(&MONRMutex); - //*MONR = GSD->MONRdata[id]; + + *MONR = GSD->MonrMessages[id]; + pthread_mutex_unlock(&MONRMutex); return READ_OK; } diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index a3f6912f1..1873d5622 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -481,13 +481,19 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { "Fatal communication fault when sending MONR command - entering error state"); vSetState(OBC_STATE_ERROR, GSD); ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_ABORT; - } + } } ObjectControlBuildMONRMessage(buffer, &MONRData, 0); //Store MONR in GSD + DataDictionarySetMONR(GSD, &MONRData, iIndex); + + + //UtilSendUDPData("ObjectControl", &ObjectControlUDPSocketfdI32, &simulator_addr, &MONRData, sizeof(MONRData), 0); + + /* for (i = 0; i < (MONRData.Header.MessageLengthU32 + COMMAND_MESSAGE_HEADER_LENGTH + @@ -495,7 +501,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { GSD->MONRData[i] = buffer[i]; GSD->MONRSizeU8 = MONRData.Header.MessageLengthU32 + COMMAND_MESSAGE_HEADER_LENGTH + - COMMAND_MESSAGE_FOOTER_LENGTH; + COMMAND_MESSAGE_FOOTER_LENGTH; */ ObjectControlMONRToASCII(&MONRData, &OriginPosition, iIndex, Id, Timestamp, XPosition, YPosition, ZPosition, LongitudinalSpeed, LateralSpeed, @@ -705,8 +711,10 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { //GSD->STRTSizeU8 = (U8)MessageLength; STRTSentU8 = 1; } - //OBCState = OBC_STATE_INITIALIZED; //This is temporary! + //OBCState = OBC_STATE_INITIALIZED; //This is temporary //printf("OutgoingStartTimeU32 = %d\n", OutgoingStartTimeU32); + + GSD->ScenarioStartTimeU32 = OutgoingStartTimeU32; bzero(MiscText, SMALL_BUFFER_SIZE_0); sprintf(MiscText, "%" PRIu32, GSD->ScenarioStartTimeU32 << 2); @@ -730,10 +738,6 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { else if (iCommand == COMM_INIT) { LogMessage(LOG_LEVEL_INFO, "INIT received"); - LogMessage(LOG_LEVEL_INFO, "Try to do the thing"); - DataDictionaryInitMONR(GSD, 1); - LogMessage(LOG_LEVEL_INFO, "Did the thing"); - LOG_SEND(LogBuffer, "[ObjectControl] INIT received."); nbr_objects = 0; if (iFindObjectsInfo(object_traj_file, object_address_name, objectIPs, &nbr_objects) == 0) { @@ -789,6 +793,12 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { pcSendBuffer[0] = (uint8_t) iCommand; iCommSend(COMM_FAILURE, pcSendBuffer, sizeof (iCommand)); } + + + LogMessage(LOG_LEVEL_INFO, "Try to do the thing with %d objects", nbr_objects); + DataDictionaryInitMONR(GSD, nbr_objects); + LogMessage(LOG_LEVEL_INFO, "Did the thing with %d objects", nbr_objects); + } else if (iCommand == COMM_ACCM && vGetState(GSD) == OBC_STATE_CONNECTED) { UtilPopulateACCMDataStructFromMQ(pcRecvBuffer, sizeof (pcRecvBuffer), &mqACCMData); @@ -815,10 +825,6 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { LogMessage(LOG_LEVEL_INFO, "CONNECT received"); LOG_SEND(LogBuffer, "[ObjectControl] CONNECT received."); - LogMessage(LOG_LEVEL_INFO, "Try to do the other thing"); - DataDictionaryFreeMONR(GSD); - LogMessage(LOG_LEVEL_INFO, "Did the thing"); - /* Connect and send drive files */ for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { @@ -1061,6 +1067,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { vDisconnectObject(&socket_fds[iIndex]); } + DataDictionaryFreeMONR(GSD); //#endif //NOTCP LogMessage(LOG_LEVEL_INFO, "DISCONNECT received"); diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index 68adcba69..573b07ef7 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -41,13 +41,13 @@ -- Defines ------------------------------------------------------------*/ typedef enum { - SERVER_STATE_UNDEFINED, - SERVER_STATE_INITIALIZED, - SERVER_STATE_IDLE, - SERVER_STATE_READY, - SERVER_STATE_RUNNING, - SERVER_STATE_INWORK, - SERVER_STATE_ERROR, + SERVER_STATE_UNDEFINED, + SERVER_STATE_INITIALIZED, + SERVER_STATE_IDLE, + SERVER_STATE_READY, + SERVER_STATE_RUNNING, + SERVER_STATE_INWORK, + SERVER_STATE_ERROR, } ServerState_t; @@ -116,28 +116,28 @@ typedef enum { typedef enum { - Idle_0, GetServerStatus_0, ArmScenario_0, DisarmScenario_0, StartScenario_1, stop_0, AbortScenario_0, - InitializeScenario_0, - ConnectObject_0, DisconnectObject_0, GetServerParameterList_0, SetServerParameter_2, GetServerParameter_1, - DownloadFile_1, UploadFile_3, CheckFileDirectoryExist_1, GetDirectoryContent_1, - DeleteFileDirectory_1, CreateDirectory_1, GetTestOrigin_0, replay_1, control_0, Exit_0, start_ext_trigg_1, - nocommand + Idle_0, GetServerStatus_0, ArmScenario_0, DisarmScenario_0, StartScenario_1, stop_0, AbortScenario_0, + InitializeScenario_0, + ConnectObject_0, DisconnectObject_0, GetServerParameterList_0, SetServerParameter_2, GetServerParameter_1, + DownloadFile_1, UploadFile_3, CheckFileDirectoryExist_1, GetDirectoryContent_1, + DeleteFileDirectory_1, CreateDirectory_1, GetTestOrigin_0, replay_1, control_0, Exit_0, start_ext_trigg_1, + nocommand } SystemControlCommand_t; const char *SystemControlCommandsArr[] = { - "Idle_0", "GetServerStatus_0", "ArmScenario_0", "DisarmScenario_0", "StartScenario_1", "stop_0", - "AbortScenario_0", "InitializeScenario_0", - "ConnectObject_0", "DisconnectObject_0", "GetServerParameterList_0", "SetServerParameter_2", - "GetServerParameter_1", "DownloadFile_1", "UploadFile_3", "CheckFileDirectoryExist_1", - "GetDirectoryContent_1", - "DeleteFileDirectory_1", "CreateDirectory_1", "GetTestOrigin_0", "replay_1", "control_0", "Exit_0", - "start_ext_trigg_1" + "Idle_0", "GetServerStatus_0", "ArmScenario_0", "DisarmScenario_0", "StartScenario_1", "stop_0", + "AbortScenario_0", "InitializeScenario_0", + "ConnectObject_0", "DisconnectObject_0", "GetServerParameterList_0", "SetServerParameter_2", + "GetServerParameter_1", "DownloadFile_1", "UploadFile_3", "CheckFileDirectoryExist_1", + "GetDirectoryContent_1", + "DeleteFileDirectory_1", "CreateDirectory_1", "GetTestOrigin_0", "replay_1", "control_0", "Exit_0", + "start_ext_trigg_1" }; const char *SystemControlStatesArr[] = - { "UNDEFINED", "INITIALIZED", "IDLE", "READY", "RUNNING", "INWORK", "ERROR" }; + { "UNDEFINED", "INITIALIZED", "IDLE", "READY", "RUNNING", "INWORK", "ERROR" }; const char *SystemControlOBCStatesArr[] = - { "UNDEFINED", "IDLE", "INITIALIZED", "CONNECTED", "ARMED", "RUNNING", "ERROR" }; + { "UNDEFINED", "IDLE", "INITIALIZED", "CONNECTED", "ARMED", "RUNNING", "ERROR" }; const char *POSTRequestMandatoryContent[] = { "POST", "HTTP/1.1", "\r\n\r\n" }; @@ -152,41 +152,41 @@ C8 *STR_SYSTEM_CONTROL_TX_PACKET_SIZE = "1200"; ------------------------------------------------------------*/ //SystemControlCommand_t SystemControlFindCommandOld(const char* CommandBuffer, SystemControlCommand_t *CurrentCommand, int *ArgCount); SystemControlCommand_t SystemControlFindCommand(const char *CommandBuffer, - SystemControlCommand_t * CurrentCommand, int *ArgCount); + SystemControlCommand_t * CurrentCommand, int *ArgCount); static I32 SystemControlInitServer(int *ClientSocket, int *ServerHandle, struct in_addr *ip_addr); static I32 SystemControlConnectServer(int *sockfd, const char *name, const uint32_t port); static void SystemControlSendBytes(const char *data, int length, int *sockfd, int debug); void SystemControlSendControlResponse(U16 ResponseStatus, C8 * ResponseString, C8 * ResponseData, - I32 ResponseDataLength, I32 * Sockfd, U8 Debug); + I32 ResponseDataLength, I32 * Sockfd, U8 Debug); I32 SystemControlBuildControlResponse(U16 ResponseStatus, C8 * ResponseString, C8 * ResponseData, - I32 ResponseDataLength, U8 Debug); + I32 ResponseDataLength, U8 Debug); void SystemControlSendLog(C8 * LogString, I32 * Sockfd, U8 Debug); void SystemControlSendMONR(C8 * LogString, I32 * Sockfd, U8 Debug); static void SystemControlCreateProcessChannel(const C8 * name, const U32 port, I32 * sockfd, - struct sockaddr_in *addr); + struct sockaddr_in *addr); //I32 SystemControlSendUDPData(I32 *sockfd, struct sockaddr_in* addr, C8 *SendData, I32 Length, U8 debug); I32 SystemControlReadServerParameterList(C8 * ParameterList, U8 debug); I32 SystemControlGetServerParameter(GSDType * GSD, C8 * ParameterName, C8 * ReturnValue, U32 BufferLength, - U8 Debug); + U8 Debug); I32 SystemControlReadServerParameter(C8 * ParameterName, C8 * ReturnValue, U8 Debug); I32 SystemControlWriteServerParameter(C8 * ParameterName, C8 * NewValue, U8 Debug); I32 SystemControlSetServerParameter(GSDType * GSD, C8 * ParameterName, C8 * NewValue, U8 Debug); I32 SystemControlCheckFileDirectoryExist(C8 * ParameterName, C8 * ReturnValue, U8 Debug); I32 SystemControlUploadFile(C8 * Path, C8 * FileSize, C8 * PacketSize, C8 * ReturnValue, U8 Debug); I32 SystemControlReceiveRxData(I32 * sockfd, C8 * Path, C8 * FileSize, C8 * PacketSize, C8 * ReturnValue, - U8 Debug); + U8 Debug); I32 SystemControlDeleteFileDirectory(C8 * Path, C8 * ReturnValue, U8 Debug); I32 SystemControlBuildFileContentInfo(C8 * Path, C8 * ReturnValue, U8 Debug); I32 SystemControlSendFileContent(I32 * sockfd, C8 * Path, C8 * PacketSize, C8 * ReturnValue, U8 Remove, - U8 Debug); + U8 Debug); I32 SystemControlCreateDirectory(C8 * Path, C8 * ReturnValue, U8 Debug); I32 SystemControlBuildRVSSTimeChannelMessage(C8 * RVSSData, U32 * RVSSDataLengthU32, TimeType * GPSTime, - U8 Debug); + U8 Debug); I32 SystemControlBuildRVSSMaestroChannelMessage(C8 * RVSSData, U32 * RVSSDataLengthU32, GSDType * GSD, - U8 SysCtrlState, U8 Debug); + U8 SysCtrlState, U8 Debug); I32 SystemControlBuildRVSSAspChannelMessage(C8 * RVSSData, U32 * RVSSDataLengthU32, U8 Debug); I32 SystemControlBuildRVSSMONRChannelMessage(C8 * RVSSData, U32 * RVSSDataLengthU32, MonitorDataType MonrData, - U8 Debug); + U8 Debug); static C8 SystemControlVerifyHostAddress(char *ip); static void signalHandler(int signo); @@ -203,1298 +203,1298 @@ static volatile int iExit = 0; void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { - I32 ServerHandle; - I32 ClientSocket = 0; - I32 ClientResult = 0; - struct sockaddr_in RVSSChannelAddr; - struct in_addr ip_addr; - I32 RVSSChannelSocket; - MonitorDataType monrData; - - ServerState_t server_state = SERVER_STATE_UNDEFINED; - OBCState_t objectControlState = OBC_STATE_UNDEFINED; - SystemControlCommand_t SystemControlCommand = Idle_0; - SystemControlCommand_t PreviousSystemControlCommand = Idle_0; - - int CommandArgCount = 0, /*CurrentCommandArgCounter=0, */ CurrentInputArgCount = 0; - C8 pcBuffer[IPC_BUFFER_SIZE]; - char inchr; - struct timeval tvTime; - - ObjectPosition OP; - int i, i1; - char *StartPtr, *StopPtr, *CmdPtr, *OpeningQuotationMarkPtr, *ClosingQuotationMarkPtr, *StringPos; - struct timespec tTime; - enum COMMAND iCommand; - ssize_t bytesReceived = 0; - char pcRecvBuffer[SC_RECV_MESSAGE_BUFFER]; - char ObjectIP[SMALL_BUFFER_SIZE_16]; - char ObjectPort[SMALL_BUFFER_SIZE_6]; - char TriggId[SMALL_BUFFER_SIZE_6]; - char TriggAction[SMALL_BUFFER_SIZE_6]; - char TriggDelay[SMALL_BUFFER_SIZE_20]; - U64 uiTime; - U32 DelayedStartU32; - U8 ModeU8 = 0; - C8 TextBufferC8[SMALL_BUFFER_SIZE_20]; - C8 ServerIPC8[SMALL_BUFFER_SIZE_20]; - C8 UsernameC8[SMALL_BUFFER_SIZE_20]; - C8 PasswordC8[SMALL_BUFFER_SIZE_20]; - U16 ServerPortU16; - I32 ServerSocketI32 = 0; - ServiceSessionType SessionData; - C8 RemoteServerRxData[1024]; - struct timespec sleep_time, ref_time; - const struct timespec mqEmptyPollPeriod = { SC_SLEEP_TIME_EMPTY_MQ_S, SC_SLEEP_TIME_EMPTY_MQ_NS }; - const struct timespec mqNonEmptyPollPeriod = - { SC_SLEEP_TIME_NONEMPTY_MQ_S, SC_SLEEP_TIME_NONEMPTY_MQ_NS }; - struct timeval CurrentTimeStruct; - U64 CurrentTimeU64 = 0; - U64 TimeDiffU64 = 0; - U64 OldTimeU64 = 0; - U64 PollRateU64 = 0; - C8 ControlResponseBuffer[SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE]; - C8 TextBuffer20[SMALL_BUFFER_SIZE_20]; - C8 UserControlIPC8[SMALL_BUFFER_SIZE_20]; - struct timeval now; - U16 MilliU16 = 0, NowU16 = 0; - U64 GPSmsU64 = 0; - C8 ParameterListC8[SYSTEM_CONTROL_SERVER_PARAMETER_LIST_SIZE]; - U32 LengthU32 = 0; - C8 BinBuffer[SMALL_BUFFER_SIZE_1024]; - - C8 TxBuffer[SYSTEM_CONTROL_TX_PACKET_SIZE]; - - HTTPHeaderContent HTTPHeader; - - //C8 SIDSData[128][10000][8]; - - C8 RVSSData[SYSTEM_CONTROL_RVSS_DATA_BUFFER]; - U16 RVSSSendCounterU16 = 0; - U32 RVSSConfigU32; - U32 RVSSMessageLengthU32; - U16 PCDMessageCodeU16; - - LogInit(MODULE_NAME, logLevel); - LogMessage(LOG_LEVEL_INFO, "System control task running with PID: %i", getpid()); - - // Set up signal handlers - if (signal(SIGINT, signalHandler) == SIG_ERR) - util_error("Unable to initialize signal handler"); - - if (iCommInit()) - util_error("Unable to connect to message bus"); - - DataDictionaryGetRVSSConfigU32(GSD, &RVSSConfigU32); - LogMessage(LOG_LEVEL_INFO, "RVSSConfigU32 = %d", RVSSConfigU32); - - U8 RVSSRateU8; - dbl RVSSRateDbl; - - DataDictionaryGetRVSSRateU8(GSD, &RVSSRateU8); - RVSSRateDbl = RVSSRateU8; - RVSSRateDbl = (1 / RVSSRateDbl) * 1000; - LogMessage(LOG_LEVEL_INFO, "RVSSRateU8 = %d", RVSSRateU8); - - if (ModeU8 == 0) { - - } - else if (ModeU8 == 1) { - SessionData.SessionIdU32 = 0; - SessionData.UserIdU32 = 0; - SessionData.UserTypeU8 = 0; - - /* */ - PollRateU64 = SYSTEM_CONTROL_SERVICE_POLL_TIME_MS; - CurrentTimeU64 = - (uint64_t) CurrentTimeStruct.tv_sec * 1000 + (uint64_t) CurrentTimeStruct.tv_usec / 1000; - OldTimeU64 = CurrentTimeU64; - - } - - - while (!iExit) { - if (server_state == SERVER_STATE_ERROR) { - iCommSend(COMM_ABORT, NULL, 0); - continue; - } - - if (ModeU8 == 0) { - if (ClientSocket <= 0) { - if (server_state == SERVER_STATE_UNDEFINED) { - //Do some initialization - - //Send COMM_DATA_DICT to notify to update data from DataDictionary - iCommSend(COMM_DATA_DICT, ControlResponseBuffer, sizeof (ControlResponseBuffer)); - - server_state = SERVER_STATE_INITIALIZED; - } - - if (USE_LOCAL_USER_CONTROL == 0) { - - ClientResult = SystemControlInitServer(&ClientSocket, &ServerHandle, &ip_addr); - bzero(UserControlIPC8, SMALL_BUFFER_SIZE_20); - sprintf(UserControlIPC8, "%s", inet_ntoa(ip_addr)); - LogMessage(LOG_LEVEL_INFO, "UserControl IP address is %s", inet_ntoa(ip_addr)); - SystemControlCreateProcessChannel(UserControlIPC8, SYSTEM_CONTROL_PROCESS_PORT, - &RVSSChannelSocket, &RVSSChannelAddr); - - } - if (USE_LOCAL_USER_CONTROL == 1) { - ClientResult = - SystemControlConnectServer(&ClientSocket, LOCAL_USER_CONTROL_IP, - LOCAL_USER_CONTROL_PORT); - SystemControlCreateProcessChannel(LOCAL_USER_CONTROL_IP, SYSTEM_CONTROL_PROCESS_PORT, - &RVSSChannelSocket, &RVSSChannelAddr); - } - - server_state = SERVER_STATE_IDLE; - } - - PreviousSystemControlCommand = SystemControlCommand; - bzero(pcBuffer, IPC_BUFFER_SIZE); - - ClientResult = recv(ClientSocket, pcBuffer, IPC_BUFFER_SIZE, MSG_DONTWAIT); - - if (ClientResult <= -1) { - if (errno != EAGAIN && errno != EWOULDBLOCK) { - LogMessage(LOG_LEVEL_ERROR, "Failed to receive from command socket"); - LogMessage(LOG_LEVEL_ERROR, "Waiting 5 seconds before exiting"); - usleep(5000000); //Wait 5 sec before sending exit, just so ObjectControl can send abort in HEAB before exit - if (iCommSend(COMM_EXIT, NULL, 0) < 0) - util_error("Fatal communication fault when sending EXIT command"); - LogMessage(LOG_LEVEL_ERROR, "System control exiting"); - exit(EXIT_FAILURE); - } - } - else if (ClientResult == 0) { - LogMessage(LOG_LEVEL_INFO, "Client closed connection"); - close(ClientSocket); - ClientSocket = -1; - if (USE_LOCAL_USER_CONTROL == 0) { - close(ServerHandle); - ServerHandle = -1; - } - - SystemControlCommand = AbortScenario_0; //Oops no client is connected, go to AbortScenario_0 - server_state == SERVER_STATE_UNDEFINED; // TODO: Should this be an assignment? - } - else if (ClientResult > 0 && ClientResult < TCP_RECV_BUFFER_SIZE) { - // TODO: Move this entire decoding process into a separate function - for (i = 0; i < SYSTEM_CONTROL_ARG_MAX_COUNT; i++) - bzero(SystemControlArgument[i], SYSTEM_CONTROL_ARGUMENT_MAX_LENGTH); - - CurrentInputArgCount = 0; - StartPtr = pcBuffer; - StopPtr = pcBuffer; - CmdPtr = NULL; - StringPos = pcBuffer; - - // Check so that all POST request mandatory content is contained in the message - for (i = 0; - i < sizeof (POSTRequestMandatoryContent) / sizeof (POSTRequestMandatoryContent[0]); - ++i) { - - StringPos = strstr(StringPos, POSTRequestMandatoryContent[i]); - if (StringPos == NULL) { - CmdPtr = NULL; - break; - } - else { - CmdPtr = StringPos + strlen(POSTRequestMandatoryContent[i]); - } - } - - if (CmdPtr != NULL) { - // It is now known that the request contains "POST" and "\r\n\r\n", so we can decode the header - UtilDecodeHTTPRequestHeader(pcBuffer, &HTTPHeader); - - if (HTTPHeader.Host[0] == '\0') { - LogMessage(LOG_LEVEL_INFO, "Unspecified host in request <%s>", pcBuffer); - } - else if (SystemControlVerifyHostAddress(HTTPHeader.Host)) { - // Find opening parenthesis - StartPtr = strchr(CmdPtr, '('); - // If there was no opening or closing parenthesis, the format is not correct - if (StartPtr == NULL || strchr(StartPtr, ')') == NULL) - LogMessage(LOG_LEVEL_WARNING, - "Received command not conforming to MSCP standards"); - else { - StartPtr++; - while (StopPtr != NULL) { - StopPtr = (char *)strchr(StartPtr, ','); - - // If there are no commas past this point, just copy the rest - if (StopPtr == NULL) { - strncpy(SystemControlArgument[CurrentInputArgCount], StartPtr, - (uint64_t) strchr(StartPtr, ')') - (uint64_t) StartPtr); - } - // Otherwise, check if the comma we found was inside quotation marks - else { - OpeningQuotationMarkPtr = (char *)strchr(StartPtr, '"'); - - if (OpeningQuotationMarkPtr == NULL) { - // It was not within quotation marks: copy until the next comma - strncpy(SystemControlArgument[CurrentInputArgCount], StartPtr, - (uint64_t) StopPtr - (uint64_t) StartPtr); - } - else if (OpeningQuotationMarkPtr != NULL - && OpeningQuotationMarkPtr < StopPtr) { - // A quotation mark was found and it was before the next comma: find the closing quotation mark - ClosingQuotationMarkPtr = - (char *)strchr(OpeningQuotationMarkPtr + 1, '"'); - - - if (ClosingQuotationMarkPtr == NULL) { - CmdPtr = NULL; - StopPtr = NULL; - LogMessage(LOG_LEVEL_WARNING, - "Received MSCP command with single quotation mark"); - break; - } - else { - // Copy all arguments within quotation marks including the quotation marks - strncpy(SystemControlArgument[CurrentInputArgCount], - OpeningQuotationMarkPtr + 1, - (uint64_t) (ClosingQuotationMarkPtr) - - (uint64_t) (OpeningQuotationMarkPtr + 1)); - // Find next comma after closing quotation mark - StopPtr = strchr(ClosingQuotationMarkPtr, ','); - } - } - } - StartPtr = StopPtr + 1; - CurrentInputArgCount++; - } - - if (CmdPtr != NULL) - SystemControlFindCommand(CmdPtr, &SystemControlCommand, &CommandArgCount); - else - LogMessage(LOG_LEVEL_WARNING, "Invalid MSCP command received"); - } - } - else { - LogMessage(LOG_LEVEL_INFO, - "Request specified host <%s> not among known local addresses", - HTTPHeader.Host); - } - } - else { - LogMessage(LOG_LEVEL_WARNING, - "Received badly formatted HTTP request: <%s>, must contain \"POST\" and \"\\r\\n\\r\\n\"", - pcBuffer); - } - } - else { - LogMessage(LOG_LEVEL_WARNING, "Ignored received TCP message which was too large to handle"); - } - - } - else if (ModeU8 == 1) { /* use util.c function to call time - gettimeofday(&CurrentTimeStruct, NULL); - CurrentTimeU64 = (uint64_t)CurrentTimeStruct.tv_sec*1000 + (uint64_t)CurrentTimeStruct.tv_usec/1000; - */ - CurrentTimeU64 = UtilgetCurrentUTCtimeMS(); - TimeDiffU64 = CurrentTimeU64 - OldTimeU64; - } - - - objectControlState = DataDictionaryGetOBCStateU8(GSD); - - if (server_state == SERVER_STATE_INWORK) { - if (SystemControlCommand == AbortScenario_0) { - SystemControlCommand = SystemControlCommand; - } - else if (SystemControlCommand == GetServerStatus_0) { - LogMessage(LOG_LEVEL_INFO, "State: %s, OBCState: %s, PreviousCommand: %s", - SystemControlStatesArr[server_state], - SystemControlOBCStatesArr[objectControlState], - SystemControlCommandsArr[PreviousSystemControlCommand]); - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - ControlResponseBuffer[0] = server_state; - ControlResponseBuffer[1] = DataDictionaryGetOBCStateU8(GSD); //OBCStateU8; - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "GetServerStatus:", - ControlResponseBuffer, 2, &ClientSocket, 0); - SystemControlCommand = PreviousSystemControlCommand; - } - else if (SystemControlCommand != PreviousSystemControlCommand) { - LogMessage(LOG_LEVEL_WARNING, - "Command not allowed, SystemControl is busy in state %s, PreviousCommand: %s", - SystemControlStatesArr[server_state], - SystemControlCommandsArr[PreviousSystemControlCommand]); - SystemControlSendLog - ("[SystemControl] Command not allowed, SystemControl is busy in state INWORK.\n", - &ClientSocket, 0); - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE, "", - ControlResponseBuffer, 0, &ClientSocket, 0); - SystemControlCommand = PreviousSystemControlCommand; - } - } - - bzero(pcRecvBuffer, SC_RECV_MESSAGE_BUFFER); - bytesReceived = iCommRecv(&iCommand, pcRecvBuffer, SC_RECV_MESSAGE_BUFFER, NULL); - - switch (iCommand) { - case COMM_FAILURE: - if (server_state == SERVER_STATE_INWORK) { - enum COMMAND failedCommand = (enum COMMAND)pcRecvBuffer[0]; - - if (failedCommand == COMM_INIT && PreviousSystemControlCommand == InitializeScenario_0) { - server_state = SERVER_STATE_IDLE; - SystemControlCommand = Idle_0; - LogMessage(LOG_LEVEL_INFO, "Initialization failed"); - // TODO: report to user? - } - else { - LogMessage(LOG_LEVEL_ERROR, "Unhandled FAILURE (command: %u) reply in state %s", - pcRecvBuffer[0], SystemControlStatesArr[server_state]); - } - } - else { - LogMessage(LOG_LEVEL_WARNING, "Received unexpected FAILURE (command: %u) reply in state %s", - pcRecvBuffer[0], SystemControlStatesArr[server_state]); - // TODO: React more? - } - break; - case COMM_OBC_STATE: - break; - case COMM_LOG: - // This creates a problem in GUC: disabled it for now - //SystemControlSendLog(pcRecvBuffer, &ClientSocket, 0); - break; - case COMM_MONR: - // TODO: Decode - if (RVSSChannelSocket != 0 && RVSSConfigU32 & RVSS_MONR_CHANNEL && bytesReceived >= 0) { - UtilPopulateMonitorDataStruct(pcRecvBuffer, (size_t) bytesReceived, &monrData, 0); - SystemControlBuildRVSSMONRChannelMessage(RVSSData, &RVSSMessageLengthU32, monrData, 0); - UtilSendUDPData("SystemControl", &RVSSChannelSocket, &RVSSChannelAddr, RVSSData, - RVSSMessageLengthU32, 0); - } - break; - case COMM_INV: - break; - default: - LogMessage(LOG_LEVEL_WARNING, "Unhandled message bus command: %u", iCommand); - } - - switch (SystemControlCommand) { - // can you access GetServerParameterList_0, GetServerParameter_1, SetServerParameter_2 and DISarmScenario and Exit from the GUI - case Idle_0: - break; - case GetServerStatus_0: - if (SystemControlCommand != PreviousSystemControlCommand) { - LogMessage(LOG_LEVEL_INFO, "State: %s, OBCState: %s, %d", - SystemControlStatesArr[server_state], - SystemControlOBCStatesArr[objectControlState], DataDictionaryGetOBCStateU8(GSD)); - } - SystemControlCommand = Idle_0; - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - ControlResponseBuffer[0] = server_state; - ControlResponseBuffer[1] = DataDictionaryGetOBCStateU8(GSD); //OBCStateU8; - LogMessage(LOG_LEVEL_DEBUG, "GPSMillisecondsU64: %ld", GPSTime->GPSMillisecondsU64); // GPSTime just ticks from 0 up shouldent it be in the global GPStime? - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "GetServerStatus:", - ControlResponseBuffer, 2, &ClientSocket, 0); - break; - case GetServerParameterList_0: - SystemControlCommand = Idle_0; - bzero(ParameterListC8, SYSTEM_CONTROL_SERVER_PARAMETER_LIST_SIZE); - SystemControlReadServerParameterList(ParameterListC8, 0); - SystemControlSendControlResponse(strlen(ParameterListC8) > - 0 ? SYSTEM_CONTROL_RESPONSE_CODE_OK : - SYSTEM_CONTROL_RESPONSE_CODE_NO_DATA, "GetServerParameterList:", - ParameterListC8, strlen(ParameterListC8), &ClientSocket, 0); - break; - case GetTestOrigin_0: - SystemControlCommand = Idle_0; - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - DataDictionaryGetOriginLatitudeC8(GSD, TextBuffer20, SMALL_BUFFER_SIZE_20); - strcat(ControlResponseBuffer, TextBuffer20); - strcat(ControlResponseBuffer, ";"); - DataDictionaryGetOriginLongitudeC8(GSD, TextBuffer20, SMALL_BUFFER_SIZE_20); - strcat(ControlResponseBuffer, TextBuffer20); - strcat(ControlResponseBuffer, ";"); - DataDictionaryGetOriginAltitudeC8(GSD, TextBuffer20, SMALL_BUFFER_SIZE_20); - strcat(ControlResponseBuffer, TextBuffer20); - strcat(ControlResponseBuffer, ";"); - iCommSend(COMM_OSEM, ControlResponseBuffer, sizeof (ControlResponseBuffer)); - SystemControlSendControlResponse(strlen(ParameterListC8) > - 0 ? SYSTEM_CONTROL_RESPONSE_CODE_OK : - SYSTEM_CONTROL_RESPONSE_CODE_NO_DATA, "GetTestOrigin:", - ControlResponseBuffer, strlen(ControlResponseBuffer), - &ClientSocket, 0); - break; - case GetServerParameter_1: - if (CurrentInputArgCount == CommandArgCount) { - SystemControlCommand = Idle_0; - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlGetServerParameter(GSD, SystemControlArgument[0], ControlResponseBuffer, - SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE, 0); - SystemControlSendControlResponse(strlen(ControlResponseBuffer) > - 0 ? SYSTEM_CONTROL_RESPONSE_CODE_OK : - SYSTEM_CONTROL_RESPONSE_CODE_NO_DATA, "GetServerParameter:", - ControlResponseBuffer, strlen(ControlResponseBuffer), - &ClientSocket, 0); - } - else { - LogMessage(LOG_LEVEL_ERROR, "Wrong parameter count in GetServerParameter(Name)!"); - SystemControlCommand = Idle_0; - } - break; - case SetServerParameter_2: - if (CurrentInputArgCount == CommandArgCount) { - SystemControlCommand = Idle_0; - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlSetServerParameter(GSD, SystemControlArgument[0], SystemControlArgument[1], 1); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "SetServerParameter:", - ControlResponseBuffer, 0, &ClientSocket, 0); - //Send COMM_DATA_DICT to notify to update data from DataDictionary - iCommSend(COMM_DATA_DICT, ControlResponseBuffer, sizeof (ControlResponseBuffer)); - - } - else { - LogMessage(LOG_LEVEL_ERROR, "Wrong parameter count in SetServerParameter(Name, Value)!"); - SystemControlCommand = Idle_0; - } - break; - case CheckFileDirectoryExist_1: - if (CurrentInputArgCount == CommandArgCount) { - SystemControlCommand = Idle_0; - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlCheckFileDirectoryExist(SystemControlArgument[0], ControlResponseBuffer, 0); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "CheckFileDirectoryExist:", - ControlResponseBuffer, 1, &ClientSocket, 0); - - } - else { - LogMessage(LOG_LEVEL_ERROR, "Wrong parameter count in CheckFFExist(path)!"); - SystemControlCommand = Idle_0; - } - break; - case DeleteFileDirectory_1: - if (CurrentInputArgCount == CommandArgCount) { - SystemControlCommand = Idle_0; - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlDeleteFileDirectory(SystemControlArgument[0], ControlResponseBuffer, 0); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "DeleteFileDirectory:", - ControlResponseBuffer, 1, &ClientSocket, 0); - - } - else { - LogMessage(LOG_LEVEL_ERROR, "Wrong parameter count in DeleteFileDirectory(path)!"); - SystemControlCommand = Idle_0; - } - break; - case CreateDirectory_1: - if (CurrentInputArgCount == CommandArgCount) { - SystemControlCommand = Idle_0; - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlCreateDirectory(SystemControlArgument[0], ControlResponseBuffer, 0); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "CreateDirectory:", - ControlResponseBuffer, 1, &ClientSocket, 0); - - } - else { - LogMessage(LOG_LEVEL_ERROR, "Wrong parameter count in CreateDirectory(path)!"); - SystemControlCommand = Idle_0; - } - break; - case GetDirectoryContent_1: - if (CurrentInputArgCount == CommandArgCount) { - SystemControlCommand = Idle_0; - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlCheckFileDirectoryExist(SystemControlArgument[0], ControlResponseBuffer, 0); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "GetDirectoryContent:", - ControlResponseBuffer, 1, &ClientSocket, 0); - if (ControlResponseBuffer[0] == FOLDER_EXIST) { - UtilCreateDirContent(SystemControlArgument[0], "dir.info"); - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlBuildFileContentInfo("dir.info", ControlResponseBuffer, 0); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "GetDirectoryContent:", - ControlResponseBuffer, 4, &ClientSocket, 0); - SystemControlSendFileContent(&ClientSocket, "dir.info", STR_SYSTEM_CONTROL_TX_PACKET_SIZE, - ControlResponseBuffer, REMOVE_FILE, 0); - } - - } - else { - LogMessage(LOG_LEVEL_ERROR, "Wrong parameter count in GetDirectoryContent(path)!"); - SystemControlCommand = Idle_0; - } - break; - case DownloadFile_1: - if (CurrentInputArgCount == CommandArgCount) { - SystemControlCommand = Idle_0; - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlCheckFileDirectoryExist(SystemControlArgument[0], ControlResponseBuffer, 0); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "DownloadFile:", - ControlResponseBuffer, 1, &ClientSocket, 0); - if (ControlResponseBuffer[0] == FILE_EXIST) { - UtilCreateDirContent(SystemControlArgument[0], SystemControlArgument[0]); - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlBuildFileContentInfo(SystemControlArgument[0], ControlResponseBuffer, 0); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "SubDownloadFile:", - ControlResponseBuffer, 4, &ClientSocket, 0); - SystemControlSendFileContent(&ClientSocket, SystemControlArgument[0], - STR_SYSTEM_CONTROL_TX_PACKET_SIZE, ControlResponseBuffer, - KEEP_FILE, 0); - } - - } - else { - LogMessage(LOG_LEVEL_ERROR, "Wrong parameter count in GetDirectoryContent(path)!"); - SystemControlCommand = Idle_0; - } - break; - case UploadFile_3: - if (CurrentInputArgCount == CommandArgCount) { - SystemControlCommand = Idle_0; - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlUploadFile(SystemControlArgument[0], SystemControlArgument[1], - SystemControlArgument[2], ControlResponseBuffer, 0); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "UploadFile:", - ControlResponseBuffer, 1, &ClientSocket, 0); - LogMessage(LOG_LEVEL_DEBUG, "UploadFile filelength: %s", SystemControlArgument[1]); - if (ControlResponseBuffer[0] == SERVER_PREPARED_BIG_PACKET_SIZE) //Server is ready to receive data - { - LogMessage(LOG_LEVEL_INFO, "Receiving file: %s", SystemControlArgument[0]); - SystemControlReceiveRxData(&ClientSocket, SystemControlArgument[0], - SystemControlArgument[1], STR_SYSTEM_CONTROL_RX_PACKET_SIZE, - ControlResponseBuffer, 0); - } - else if (ControlResponseBuffer[0] == PATH_INVALID_MISSING) { - LogMessage(LOG_LEVEL_INFO, "Failed receiving file: %s", SystemControlArgument[0]); - SystemControlReceiveRxData(&ClientSocket, "file.tmp", SystemControlArgument[1], - STR_SYSTEM_CONTROL_RX_PACKET_SIZE, ControlResponseBuffer, 0); - SystemControlDeleteFileDirectory("file.tmp", ControlResponseBuffer, 0); - ControlResponseBuffer[0] = PATH_INVALID_MISSING; - } - else { - LogMessage(LOG_LEVEL_INFO, "Receiving file: %s", SystemControlArgument[0]); - SystemControlReceiveRxData(&ClientSocket, SystemControlArgument[0], - SystemControlArgument[1], SystemControlArgument[2], - ControlResponseBuffer, 0); - } - - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "UploadFile:", - ControlResponseBuffer, 1, &ClientSocket, 0); - - } - else { - LogMessage(LOG_LEVEL_ERROR, - "Wrong parameter count in PrepFileRx(path, filesize, packetsize)!"); - SystemControlCommand = Idle_0; - } - break; - case InitializeScenario_0: - if (server_state == SERVER_STATE_IDLE && objectControlState == OBC_STATE_IDLE) { - if (iCommSend(COMM_INIT, pcBuffer, strlen(pcBuffer) + 1) < 0) { - LogMessage(LOG_LEVEL_ERROR, "Fatal communication fault when sending INIT command"); - server_state = SERVER_STATE_ERROR; - } - server_state = SERVER_STATE_INWORK; - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "InitializeScenario:", - ControlResponseBuffer, 0, &ClientSocket, 0); - SystemControlSendLog("[SystemControl] Sending INIT.\n", &ClientSocket, 0); - } - else if (server_state == SERVER_STATE_INWORK && objectControlState == OBC_STATE_INITIALIZED) { - SystemControlSendLog - ("[SystemControl] Simulate that all objects becomes successfully configured.\n", - &ClientSocket, 0); - SystemControlCommand = Idle_0; - server_state = SERVER_STATE_IDLE; - } - else if (server_state == SERVER_STATE_IDLE) { - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE, - "InitializeScenario:", ControlResponseBuffer, 0, - &ClientSocket, 0); - SystemControlSendLog("[SystemControl] INIT received, state errors!\n", &ClientSocket, 0); - SystemControlCommand = PreviousSystemControlCommand; - } - break; - case ConnectObject_0: - if (server_state == SERVER_STATE_IDLE && objectControlState == OBC_STATE_INITIALIZED) { - if (iCommSend(COMM_CONNECT, pcBuffer, strlen(pcBuffer) + 1) < 0) { - LogMessage(LOG_LEVEL_ERROR, "Fatal communication fault when sending CONNECT command"); - server_state = SERVER_STATE_ERROR; - } - SystemControlCommand = Idle_0; - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "ConnectObject:", - ControlResponseBuffer, 0, &ClientSocket, 0); - SystemControlSendLog("[SystemControl] Sending CONNECT.\n", &ClientSocket, 0); - } - else if (server_state == SERVER_STATE_INWORK && objectControlState == OBC_STATE_CONNECTED) { - SystemControlSendLog("[SystemControl] Simulate that all objects are connected.\n", - &ClientSocket, 0); - SystemControlCommand = Idle_0; - server_state = SERVER_STATE_IDLE; - } - else if (server_state == SERVER_STATE_IDLE) { - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE, - "ConnectObject:", ControlResponseBuffer, 0, &ClientSocket, - 0); - SystemControlSendLog("[SystemControl] CONNECT received, state errors!\n", &ClientSocket, 0); - SystemControlCommand = PreviousSystemControlCommand; - } - break; - case DisconnectObject_0: - if (server_state == SERVER_STATE_IDLE) { - if (iCommSend(COMM_DISCONNECT, pcBuffer, strlen(pcBuffer) + 1) < 0) { - LogMessage(LOG_LEVEL_ERROR, "Fatal communication fault when sending DISCONNECT command"); - server_state = SERVER_STATE_ERROR; - } - SystemControlCommand = Idle_0; - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "DisconnectObject:", - ControlResponseBuffer, 0, &ClientSocket, 0); - SystemControlSendLog("[SystemControl] Sending DISCONNECT.\n", &ClientSocket, 0); - } - else { - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE, - "ConnectObject:", ControlResponseBuffer, 0, &ClientSocket, - 0); - SystemControlSendLog("[SystemControl] DISCONNECT received, state errors!\n", &ClientSocket, - 0); - SystemControlCommand = PreviousSystemControlCommand; - } - break; - case ArmScenario_0: - if (server_state == SERVER_STATE_IDLE && objectControlState == OBC_STATE_CONNECTED) { - server_state = SERVER_STATE_INWORK; - if (iCommSend(COMM_ARM, NULL, 0) < 0) { - LogMessage(LOG_LEVEL_ERROR, "Fatal communication fault when sending ARM command"); - server_state = SERVER_STATE_ERROR; - } - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "ArmScenario:", - ControlResponseBuffer, 0, &ClientSocket, 0); - SystemControlSendLog("[SystemControl] Sending ARM.\n", &ClientSocket, 0); - } - else if (server_state == SERVER_STATE_INWORK && objectControlState == OBC_STATE_ARMED) { - SystemControlSendLog("[SystemControl] Simulate that all objects become armed.\n", - &ClientSocket, 0); - - SystemControlCommand = Idle_0; - server_state = SERVER_STATE_IDLE; - } - else if (server_state == SERVER_STATE_IDLE) { - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE, - "ArmScenario:", ControlResponseBuffer, 0, &ClientSocket, 0); - SystemControlSendLog("[SystemControl] ARM received, state errors!\n", &ClientSocket, 0); - SystemControlCommand = PreviousSystemControlCommand; - } - break; - case DisarmScenario_0: - if (server_state == SERVER_STATE_IDLE && objectControlState == OBC_STATE_ARMED) { - server_state = SERVER_STATE_IDLE; - if (iCommSend(COMM_DISARM, NULL, 0) < 0) { - LogMessage(LOG_LEVEL_ERROR, "Fatal communication fault when sending DISARM command"); - server_state = SERVER_STATE_ERROR; - } - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "DisarmScenario:", - ControlResponseBuffer, 0, &ClientSocket, 0); - SystemControlSendLog("[SystemControl] Sending DISARM.\n", &ClientSocket, 0); - } - else if (server_state == SERVER_STATE_INWORK && objectControlState == OBC_STATE_CONNECTED) { - SystemControlSendLog("[SystemControl] Simulate that all objects become disarmed.\n", - &ClientSocket, 0); - SystemControlCommand = Idle_0; - server_state = SERVER_STATE_IDLE; - } - else if (server_state == SERVER_STATE_IDLE) { - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE, - "DisarmScenario:", ControlResponseBuffer, 0, &ClientSocket, - 0); - SystemControlSendLog("[SystemControl] DISARM received, state errors!\n", &ClientSocket, 0); - SystemControlCommand = PreviousSystemControlCommand; - } - break; - case StartScenario_1: - if (CurrentInputArgCount == CommandArgCount) { - if (server_state == SERVER_STATE_IDLE && objectControlState == OBC_STATE_ARMED) //Temporary! - { - bzero(pcBuffer, IPC_BUFFER_SIZE); - /* Lest use UTC time everywhere instead of etsi and gps time - gettimeofday(&tvTime, NULL); - uiTime = (uint64_t)tvTime.tv_sec*1000 + (uint64_t)tvTime.tv_usec/1000 - MS_FROM_1970_TO_2004_NO_LEAP_SECS + DIFF_LEAP_SECONDS_UTC_ETSI*1000; - */ - uiTime = UtilgetCurrentUTCtimeMS(); - if (TIME_COMPENSATE_LAGING_VM) - uiTime = uiTime - TIME_COMPENSATE_LAGING_VM_VAL; - - LogMessage(LOG_LEVEL_INFO, "Current timestamp (gtd): %lu", uiTime); - - //clock_gettime(CLOCK_MONOTONIC_COARSE, &tTime); - //clock_gettime(CLOCK_REALTIME, &tTime); - //uiTime = (uint64_t)tTime.tv_sec*1000 + (uint64_t)tTime.tv_nsec/1000000 - MS_FROM_1970_TO_2004_NO_LEAP_SECS + DIFF_LEAP_SECONDS_UTC_ETSI*1000; - //printf("[SystemControl] Current timestamp (cgt): %lu\n",uiTime ); - //printf("[SystemControl] Current timestamp: %lu\n",uiTime ); - - //uiTime += atoi(SystemControlArgument[0]); - uiTime = atoi(SystemControlArgument[0]); - DelayedStartU32 = atoi(SystemControlArgument[1]); - sprintf(pcBuffer, "%" PRIu8 ";%" PRIu64 ";%" PRIu32 ";", 0, uiTime, DelayedStartU32); - LogMessage(LOG_LEVEL_INFO, "Sending START <%s> (delayed +%s ms)", pcBuffer, - SystemControlArgument[1]); - - if (iCommSend(COMM_STRT, pcBuffer, strlen(pcBuffer) + 1) < 0) { - LogMessage(LOG_LEVEL_ERROR, "Fatal communication fault when sending STRT command"); - server_state = SERVER_STATE_ERROR; - } - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "StartScenario:", - ControlResponseBuffer, 0, &ClientSocket, 0); - server_state = SERVER_STATE_INWORK; - //server_state = SERVER_STATE_IDLE; //Temporary! - //SystemControlCommand = Idle_0; //Temporary! - } - else if (server_state == SERVER_STATE_INWORK && objectControlState == OBC_STATE_RUNNING) { - - SystemControlCommand = Idle_0; - server_state = SERVER_STATE_IDLE; - } - else if (server_state == SERVER_STATE_IDLE) { - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE, - "StartScenario:", ControlResponseBuffer, 0, - &ClientSocket, 0); - SystemControlSendLog("[SystemControl] START received, state errors!\n", &ClientSocket, 0); - SystemControlCommand = PreviousSystemControlCommand; - } - - } - else - LogMessage(LOG_LEVEL_WARNING, "START command parameter count error"); - break; - case stop_0: - if (iCommSend(COMM_STOP, NULL, 0) < 0) { - LogMessage(LOG_LEVEL_ERROR, "Fatal communication fault when sending STOP command"); - server_state = SERVER_STATE_ERROR; - } - else { - server_state = SERVER_STATE_IDLE; - SystemControlCommand = Idle_0; - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "stop:", - ControlResponseBuffer, 0, &ClientSocket, 0); - } - break; - case AbortScenario_0: - if (objectControlState == OBC_STATE_RUNNING - /* || strstr(SystemControlOBCStatesArr[OBCStateU8], "CONNECTED") != NULL - * || strstr(SystemControlOBCStatesArr[OBCStateU8], "ARMED") != NULL*/ ) - // Abort should only be allowed in running state - { - if (iCommSend(COMM_ABORT, NULL, 0) < 0) { - LogMessage(LOG_LEVEL_ERROR, "Fatal communication fault when sending ABORT command"); - server_state = SERVER_STATE_ERROR; - } - else { - server_state = SERVER_STATE_IDLE; - SystemControlCommand = Idle_0; - if (ClientSocket >= 0) { - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "AbortScenario:", - ControlResponseBuffer, 0, &ClientSocket, 0); - } - } - } - else { - if (ClientSocket >= 0) { - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE, - "AbortScenario:", ControlResponseBuffer, 0, - &ClientSocket, 0); - SystemControlSendLog("[SystemControl] ABORT received, state errors!\n", &ClientSocket, 0); - } - server_state = SERVER_STATE_IDLE; - SystemControlCommand = Idle_0; - } - break; - /* - case replay_1: - if(CurrentCommandArgCounter == CommandArgCount) - { - if(!strcmp(SystemControlArgument[CurrentCommandArgCounter],"-help")) - { - printf("[SystemControl] -----REPLAY-----\n"); - printf("[SystemControl] Syntax: replay [arg]\n"); - printf("[SystemControl] Ex: replay log/33/event.log\n"); - fflush(stdout); - } - else - { - (void)iCommSend(COMM_REPLAY, SystemControlArgument[CurrentCommandArgCounter]); - printf("[SystemControl] System control sending REPLAY on IPC <%s>\n", SystemControlArgument[CurrentCommandArgCounter]); - fflush(stdout); - } - SystemControlCommand = idle_0; - CurrentCommandArgCounter = 0; - } else CurrentCommandArgCounter ++; - break; - case control_0: - (void)iCommSend(COMM_CONTROL, NULL); - //printf("INF: System control sending CONTROL on IPC <%s>\n", pcBuffer); - fflush(stdout); - SystemControlCommand = idle_0; - CurrentCommandArgCounter = 0; - break; */ - case Exit_0: - - if (iCommSend(COMM_EXIT, NULL, 0) < 0) { - LogMessage(LOG_LEVEL_ERROR, "Fatal communication fault when sending EXIT command"); - server_state = SERVER_STATE_ERROR; - } - else { - iExit = 1; - GSD->ExitU8 = 1; - usleep(1000000); - SystemControlCommand = Idle_0; - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "Exit:", - ControlResponseBuffer, 0, &ClientSocket, 0); - close(ClientSocket); - ClientSocket = -1; - if (USE_LOCAL_USER_CONTROL == 0) { - close(ServerHandle); - ServerHandle = -1; - } - LogMessage(LOG_LEVEL_INFO, "Server closing"); - } - break; - - default: - - break; - } - - - - sleep_time.tv_sec = 0; - sleep_time.tv_nsec = SYSTEM_CONTROL_TASK_PERIOD_MS * 1000000; - ++RVSSSendCounterU16; - if (RVSSSendCounterU16 >= ((U16) RVSSRateDbl)) { - RVSSSendCounterU16 = 0; - DataDictionaryGetRVSSRateU8(GSD, &RVSSRateU8); - RVSSRateDbl = RVSSRateU8; - RVSSRateDbl = (1 / RVSSRateDbl) * 100; //This is strange!! Should be 1000, but if it is the RVSSData is sent to slow by a factor of 10. - - if (RVSSChannelSocket != 0 && RVSSSendCounterU16 == 0 && RVSSConfigU32 > 0) { - bzero(RVSSData, SYSTEM_CONTROL_RVSS_DATA_BUFFER); - - if (RVSSConfigU32 & RVSS_TIME_CHANNEL) { - SystemControlBuildRVSSTimeChannelMessage(RVSSData, &RVSSMessageLengthU32, GPSTime, 0); - UtilSendUDPData("SystemControl", &RVSSChannelSocket, &RVSSChannelAddr, RVSSData, - RVSSMessageLengthU32, 0); - } - - if (RVSSConfigU32 & RVSS_MAESTRO_CHANNEL) { - SystemControlBuildRVSSMaestroChannelMessage(RVSSData, &RVSSMessageLengthU32, GSD, - server_state, 0); - UtilSendUDPData("SystemControl", &RVSSChannelSocket, &RVSSChannelAddr, RVSSData, - RVSSMessageLengthU32, 0); - } - - if (RVSSConfigU32 & RVSS_ASP_CHANNEL) { - SystemControlBuildRVSSAspChannelMessage(RVSSData, &RVSSMessageLengthU32, 0); - UtilSendUDPData("SystemControl", &RVSSChannelSocket, &RVSSChannelAddr, RVSSData, - RVSSMessageLengthU32, 0); - } - - } - - - } - - sleep_time = (iCommand == COMM_INV - && server_state != SERVER_STATE_INWORK) ? mqEmptyPollPeriod : mqNonEmptyPollPeriod; - nanosleep(&sleep_time, &ref_time); - } - - (void)iCommClose(); - - LogMessage(LOG_LEVEL_INFO, "Exiting"); + I32 ServerHandle; + I32 ClientSocket = 0; + I32 ClientResult = 0; + struct sockaddr_in RVSSChannelAddr; + struct in_addr ip_addr; + I32 RVSSChannelSocket; + MonitorDataType monrData; + + ServerState_t server_state = SERVER_STATE_UNDEFINED; + OBCState_t objectControlState = OBC_STATE_UNDEFINED; + SystemControlCommand_t SystemControlCommand = Idle_0; + SystemControlCommand_t PreviousSystemControlCommand = Idle_0; + + int CommandArgCount = 0, /*CurrentCommandArgCounter=0, */ CurrentInputArgCount = 0; + C8 pcBuffer[IPC_BUFFER_SIZE]; + char inchr; + struct timeval tvTime; + + ObjectPosition OP; + int i, i1; + char *StartPtr, *StopPtr, *CmdPtr, *OpeningQuotationMarkPtr, *ClosingQuotationMarkPtr, *StringPos; + struct timespec tTime; + enum COMMAND iCommand; + ssize_t bytesReceived = 0; + char pcRecvBuffer[SC_RECV_MESSAGE_BUFFER]; + char ObjectIP[SMALL_BUFFER_SIZE_16]; + char ObjectPort[SMALL_BUFFER_SIZE_6]; + char TriggId[SMALL_BUFFER_SIZE_6]; + char TriggAction[SMALL_BUFFER_SIZE_6]; + char TriggDelay[SMALL_BUFFER_SIZE_20]; + U64 uiTime; + U32 DelayedStartU32; + U8 ModeU8 = 0; + C8 TextBufferC8[SMALL_BUFFER_SIZE_20]; + C8 ServerIPC8[SMALL_BUFFER_SIZE_20]; + C8 UsernameC8[SMALL_BUFFER_SIZE_20]; + C8 PasswordC8[SMALL_BUFFER_SIZE_20]; + U16 ServerPortU16; + I32 ServerSocketI32 = 0; + ServiceSessionType SessionData; + C8 RemoteServerRxData[1024]; + struct timespec sleep_time, ref_time; + const struct timespec mqEmptyPollPeriod = { SC_SLEEP_TIME_EMPTY_MQ_S, SC_SLEEP_TIME_EMPTY_MQ_NS }; + const struct timespec mqNonEmptyPollPeriod = + { SC_SLEEP_TIME_NONEMPTY_MQ_S, SC_SLEEP_TIME_NONEMPTY_MQ_NS }; + struct timeval CurrentTimeStruct; + U64 CurrentTimeU64 = 0; + U64 TimeDiffU64 = 0; + U64 OldTimeU64 = 0; + U64 PollRateU64 = 0; + C8 ControlResponseBuffer[SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE]; + C8 TextBuffer20[SMALL_BUFFER_SIZE_20]; + C8 UserControlIPC8[SMALL_BUFFER_SIZE_20]; + struct timeval now; + U16 MilliU16 = 0, NowU16 = 0; + U64 GPSmsU64 = 0; + C8 ParameterListC8[SYSTEM_CONTROL_SERVER_PARAMETER_LIST_SIZE]; + U32 LengthU32 = 0; + C8 BinBuffer[SMALL_BUFFER_SIZE_1024]; + + C8 TxBuffer[SYSTEM_CONTROL_TX_PACKET_SIZE]; + + HTTPHeaderContent HTTPHeader; + + //C8 SIDSData[128][10000][8]; + + C8 RVSSData[SYSTEM_CONTROL_RVSS_DATA_BUFFER]; + U16 RVSSSendCounterU16 = 0; + U32 RVSSConfigU32; + U32 RVSSMessageLengthU32; + U16 PCDMessageCodeU16; + + LogInit(MODULE_NAME, logLevel); + LogMessage(LOG_LEVEL_INFO, "System control task running with PID: %i", getpid()); + + // Set up signal handlers + if (signal(SIGINT, signalHandler) == SIG_ERR) + util_error("Unable to initialize signal handler"); + + if (iCommInit()) + util_error("Unable to connect to message bus"); + + DataDictionaryGetRVSSConfigU32(GSD, &RVSSConfigU32); + LogMessage(LOG_LEVEL_INFO, "RVSSConfigU32 = %d", RVSSConfigU32); + + U8 RVSSRateU8; + dbl RVSSRateDbl; + + DataDictionaryGetRVSSRateU8(GSD, &RVSSRateU8); + RVSSRateDbl = RVSSRateU8; + RVSSRateDbl = (1 / RVSSRateDbl) * 1000; + LogMessage(LOG_LEVEL_INFO, "RVSSRateU8 = %d", RVSSRateU8); + + if (ModeU8 == 0) { + + } + else if (ModeU8 == 1) { + SessionData.SessionIdU32 = 0; + SessionData.UserIdU32 = 0; + SessionData.UserTypeU8 = 0; + + /* */ + PollRateU64 = SYSTEM_CONTROL_SERVICE_POLL_TIME_MS; + CurrentTimeU64 = + (uint64_t) CurrentTimeStruct.tv_sec * 1000 + (uint64_t) CurrentTimeStruct.tv_usec / 1000; + OldTimeU64 = CurrentTimeU64; + + } + + + while (!iExit) { + if (server_state == SERVER_STATE_ERROR) { + iCommSend(COMM_ABORT, NULL, 0); + continue; + } + + if (ModeU8 == 0) { + if (ClientSocket <= 0) { + if (server_state == SERVER_STATE_UNDEFINED) { + //Do some initialization + + //Send COMM_DATA_DICT to notify to update data from DataDictionary + iCommSend(COMM_DATA_DICT, ControlResponseBuffer, sizeof (ControlResponseBuffer)); + + server_state = SERVER_STATE_INITIALIZED; + } + + if (USE_LOCAL_USER_CONTROL == 0) { + + ClientResult = SystemControlInitServer(&ClientSocket, &ServerHandle, &ip_addr); + bzero(UserControlIPC8, SMALL_BUFFER_SIZE_20); + sprintf(UserControlIPC8, "%s", inet_ntoa(ip_addr)); + LogMessage(LOG_LEVEL_INFO, "UserControl IP address is %s", inet_ntoa(ip_addr)); + SystemControlCreateProcessChannel(UserControlIPC8, SYSTEM_CONTROL_PROCESS_PORT, + &RVSSChannelSocket, &RVSSChannelAddr); + + } + if (USE_LOCAL_USER_CONTROL == 1) { + ClientResult = + SystemControlConnectServer(&ClientSocket, LOCAL_USER_CONTROL_IP, + LOCAL_USER_CONTROL_PORT); + SystemControlCreateProcessChannel(LOCAL_USER_CONTROL_IP, SYSTEM_CONTROL_PROCESS_PORT, + &RVSSChannelSocket, &RVSSChannelAddr); + } + + server_state = SERVER_STATE_IDLE; + } + + PreviousSystemControlCommand = SystemControlCommand; + bzero(pcBuffer, IPC_BUFFER_SIZE); + + ClientResult = recv(ClientSocket, pcBuffer, IPC_BUFFER_SIZE, MSG_DONTWAIT); + + if (ClientResult <= -1) { + if (errno != EAGAIN && errno != EWOULDBLOCK) { + LogMessage(LOG_LEVEL_ERROR, "Failed to receive from command socket"); + LogMessage(LOG_LEVEL_ERROR, "Waiting 5 seconds before exiting"); + usleep(5000000); //Wait 5 sec before sending exit, just so ObjectControl can send abort in HEAB before exit + if (iCommSend(COMM_EXIT, NULL, 0) < 0) + util_error("Fatal communication fault when sending EXIT command"); + LogMessage(LOG_LEVEL_ERROR, "System control exiting"); + exit(EXIT_FAILURE); + } + } + else if (ClientResult == 0) { + LogMessage(LOG_LEVEL_INFO, "Client closed connection"); + close(ClientSocket); + ClientSocket = -1; + if (USE_LOCAL_USER_CONTROL == 0) { + close(ServerHandle); + ServerHandle = -1; + } + + SystemControlCommand = AbortScenario_0; //Oops no client is connected, go to AbortScenario_0 + server_state == SERVER_STATE_UNDEFINED; // TODO: Should this be an assignment? + } + else if (ClientResult > 0 && ClientResult < TCP_RECV_BUFFER_SIZE) { + // TODO: Move this entire decoding process into a separate function + for (i = 0; i < SYSTEM_CONTROL_ARG_MAX_COUNT; i++) + bzero(SystemControlArgument[i], SYSTEM_CONTROL_ARGUMENT_MAX_LENGTH); + + CurrentInputArgCount = 0; + StartPtr = pcBuffer; + StopPtr = pcBuffer; + CmdPtr = NULL; + StringPos = pcBuffer; + + // Check so that all POST request mandatory content is contained in the message + for (i = 0; + i < sizeof (POSTRequestMandatoryContent) / sizeof (POSTRequestMandatoryContent[0]); + ++i) { + + StringPos = strstr(StringPos, POSTRequestMandatoryContent[i]); + if (StringPos == NULL) { + CmdPtr = NULL; + break; + } + else { + CmdPtr = StringPos + strlen(POSTRequestMandatoryContent[i]); + } + } + + if (CmdPtr != NULL) { + // It is now known that the request contains "POST" and "\r\n\r\n", so we can decode the header + UtilDecodeHTTPRequestHeader(pcBuffer, &HTTPHeader); + + if (HTTPHeader.Host[0] == '\0') { + LogMessage(LOG_LEVEL_INFO, "Unspecified host in request <%s>", pcBuffer); + } + else if (SystemControlVerifyHostAddress(HTTPHeader.Host)) { + // Find opening parenthesis + StartPtr = strchr(CmdPtr, '('); + // If there was no opening or closing parenthesis, the format is not correct + if (StartPtr == NULL || strchr(StartPtr, ')') == NULL) + LogMessage(LOG_LEVEL_WARNING, + "Received command not conforming to MSCP standards"); + else { + StartPtr++; + while (StopPtr != NULL) { + StopPtr = (char *)strchr(StartPtr, ','); + + // If there are no commas past this point, just copy the rest + if (StopPtr == NULL) { + strncpy(SystemControlArgument[CurrentInputArgCount], StartPtr, + (uint64_t) strchr(StartPtr, ')') - (uint64_t) StartPtr); + } + // Otherwise, check if the comma we found was inside quotation marks + else { + OpeningQuotationMarkPtr = (char *)strchr(StartPtr, '"'); + + if (OpeningQuotationMarkPtr == NULL) { + // It was not within quotation marks: copy until the next comma + strncpy(SystemControlArgument[CurrentInputArgCount], StartPtr, + (uint64_t) StopPtr - (uint64_t) StartPtr); + } + else if (OpeningQuotationMarkPtr != NULL + && OpeningQuotationMarkPtr < StopPtr) { + // A quotation mark was found and it was before the next comma: find the closing quotation mark + ClosingQuotationMarkPtr = + (char *)strchr(OpeningQuotationMarkPtr + 1, '"'); + + + if (ClosingQuotationMarkPtr == NULL) { + CmdPtr = NULL; + StopPtr = NULL; + LogMessage(LOG_LEVEL_WARNING, + "Received MSCP command with single quotation mark"); + break; + } + else { + // Copy all arguments within quotation marks including the quotation marks + strncpy(SystemControlArgument[CurrentInputArgCount], + OpeningQuotationMarkPtr + 1, + (uint64_t) (ClosingQuotationMarkPtr) - + (uint64_t) (OpeningQuotationMarkPtr + 1)); + // Find next comma after closing quotation mark + StopPtr = strchr(ClosingQuotationMarkPtr, ','); + } + } + } + StartPtr = StopPtr + 1; + CurrentInputArgCount++; + } + + if (CmdPtr != NULL) + SystemControlFindCommand(CmdPtr, &SystemControlCommand, &CommandArgCount); + else + LogMessage(LOG_LEVEL_WARNING, "Invalid MSCP command received"); + } + } + else { + LogMessage(LOG_LEVEL_INFO, + "Request specified host <%s> not among known local addresses", + HTTPHeader.Host); + } + } + else { + LogMessage(LOG_LEVEL_WARNING, + "Received badly formatted HTTP request: <%s>, must contain \"POST\" and \"\\r\\n\\r\\n\"", + pcBuffer); + } + } + else { + LogMessage(LOG_LEVEL_WARNING, "Ignored received TCP message which was too large to handle"); + } + + } + else if (ModeU8 == 1) { /* use util.c function to call time + gettimeofday(&CurrentTimeStruct, NULL); + CurrentTimeU64 = (uint64_t)CurrentTimeStruct.tv_sec*1000 + (uint64_t)CurrentTimeStruct.tv_usec/1000; + */ + CurrentTimeU64 = UtilgetCurrentUTCtimeMS(); + TimeDiffU64 = CurrentTimeU64 - OldTimeU64; + } + + + objectControlState = DataDictionaryGetOBCStateU8(GSD); + + if (server_state == SERVER_STATE_INWORK) { + if (SystemControlCommand == AbortScenario_0) { + SystemControlCommand = SystemControlCommand; + } + else if (SystemControlCommand == GetServerStatus_0) { + LogMessage(LOG_LEVEL_INFO, "State: %s, OBCState: %s, PreviousCommand: %s", + SystemControlStatesArr[server_state], + SystemControlOBCStatesArr[objectControlState], + SystemControlCommandsArr[PreviousSystemControlCommand]); + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + ControlResponseBuffer[0] = server_state; + ControlResponseBuffer[1] = DataDictionaryGetOBCStateU8(GSD); //OBCStateU8; + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "GetServerStatus:", + ControlResponseBuffer, 2, &ClientSocket, 0); + SystemControlCommand = PreviousSystemControlCommand; + } + else if (SystemControlCommand != PreviousSystemControlCommand) { + LogMessage(LOG_LEVEL_WARNING, + "Command not allowed, SystemControl is busy in state %s, PreviousCommand: %s", + SystemControlStatesArr[server_state], + SystemControlCommandsArr[PreviousSystemControlCommand]); + SystemControlSendLog + ("[SystemControl] Command not allowed, SystemControl is busy in state INWORK.\n", + &ClientSocket, 0); + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE, "", + ControlResponseBuffer, 0, &ClientSocket, 0); + SystemControlCommand = PreviousSystemControlCommand; + } + } + + bzero(pcRecvBuffer, SC_RECV_MESSAGE_BUFFER); + bytesReceived = iCommRecv(&iCommand, pcRecvBuffer, SC_RECV_MESSAGE_BUFFER, NULL); + + switch (iCommand) { + case COMM_FAILURE: + if (server_state == SERVER_STATE_INWORK) { + enum COMMAND failedCommand = (enum COMMAND)pcRecvBuffer[0]; + + if (failedCommand == COMM_INIT && PreviousSystemControlCommand == InitializeScenario_0) { + server_state = SERVER_STATE_IDLE; + SystemControlCommand = Idle_0; + LogMessage(LOG_LEVEL_INFO, "Initialization failed"); + // TODO: report to user? + } + else { + LogMessage(LOG_LEVEL_ERROR, "Unhandled FAILURE (command: %u) reply in state %s", + pcRecvBuffer[0], SystemControlStatesArr[server_state]); + } + } + else { + LogMessage(LOG_LEVEL_WARNING, "Received unexpected FAILURE (command: %u) reply in state %s", + pcRecvBuffer[0], SystemControlStatesArr[server_state]); + // TODO: React more? + } + break; + case COMM_OBC_STATE: + break; + case COMM_LOG: + // This creates a problem in GUC: disabled it for now + //SystemControlSendLog(pcRecvBuffer, &ClientSocket, 0); + break; + case COMM_MONR: + // TODO: Decode + if (RVSSChannelSocket != 0 && RVSSConfigU32 & RVSS_MONR_CHANNEL && bytesReceived >= 0) { + UtilPopulateMonitorDataStruct(pcRecvBuffer, (size_t) bytesReceived, &monrData, 0); + SystemControlBuildRVSSMONRChannelMessage(RVSSData, &RVSSMessageLengthU32, monrData, 0); + UtilSendUDPData("SystemControl", &RVSSChannelSocket, &RVSSChannelAddr, RVSSData, + RVSSMessageLengthU32, 0); + } + break; + case COMM_INV: + break; + default: + LogMessage(LOG_LEVEL_WARNING, "Unhandled message bus command: %u", iCommand); + } + + switch (SystemControlCommand) { + // can you access GetServerParameterList_0, GetServerParameter_1, SetServerParameter_2 and DISarmScenario and Exit from the GUI + case Idle_0: + break; + case GetServerStatus_0: + if (SystemControlCommand != PreviousSystemControlCommand) { + LogMessage(LOG_LEVEL_INFO, "State: %s, OBCState: %s, %d", + SystemControlStatesArr[server_state], + SystemControlOBCStatesArr[objectControlState], DataDictionaryGetOBCStateU8(GSD)); + } + SystemControlCommand = Idle_0; + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + ControlResponseBuffer[0] = server_state; + ControlResponseBuffer[1] = DataDictionaryGetOBCStateU8(GSD); //OBCStateU8; + LogMessage(LOG_LEVEL_DEBUG, "GPSMillisecondsU64: %ld", GPSTime->GPSMillisecondsU64); // GPSTime just ticks from 0 up shouldent it be in the global GPStime? + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "GetServerStatus:", + ControlResponseBuffer, 2, &ClientSocket, 0); + break; + case GetServerParameterList_0: + SystemControlCommand = Idle_0; + bzero(ParameterListC8, SYSTEM_CONTROL_SERVER_PARAMETER_LIST_SIZE); + SystemControlReadServerParameterList(ParameterListC8, 0); + SystemControlSendControlResponse(strlen(ParameterListC8) > + 0 ? SYSTEM_CONTROL_RESPONSE_CODE_OK : + SYSTEM_CONTROL_RESPONSE_CODE_NO_DATA, "GetServerParameterList:", + ParameterListC8, strlen(ParameterListC8), &ClientSocket, 0); + break; + case GetTestOrigin_0: + SystemControlCommand = Idle_0; + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + DataDictionaryGetOriginLatitudeC8(GSD, TextBuffer20, SMALL_BUFFER_SIZE_20); + strcat(ControlResponseBuffer, TextBuffer20); + strcat(ControlResponseBuffer, ";"); + DataDictionaryGetOriginLongitudeC8(GSD, TextBuffer20, SMALL_BUFFER_SIZE_20); + strcat(ControlResponseBuffer, TextBuffer20); + strcat(ControlResponseBuffer, ";"); + DataDictionaryGetOriginAltitudeC8(GSD, TextBuffer20, SMALL_BUFFER_SIZE_20); + strcat(ControlResponseBuffer, TextBuffer20); + strcat(ControlResponseBuffer, ";"); + iCommSend(COMM_OSEM, ControlResponseBuffer, sizeof (ControlResponseBuffer)); + SystemControlSendControlResponse(strlen(ParameterListC8) > + 0 ? SYSTEM_CONTROL_RESPONSE_CODE_OK : + SYSTEM_CONTROL_RESPONSE_CODE_NO_DATA, "GetTestOrigin:", + ControlResponseBuffer, strlen(ControlResponseBuffer), + &ClientSocket, 0); + break; + case GetServerParameter_1: + if (CurrentInputArgCount == CommandArgCount) { + SystemControlCommand = Idle_0; + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlGetServerParameter(GSD, SystemControlArgument[0], ControlResponseBuffer, + SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE, 0); + SystemControlSendControlResponse(strlen(ControlResponseBuffer) > + 0 ? SYSTEM_CONTROL_RESPONSE_CODE_OK : + SYSTEM_CONTROL_RESPONSE_CODE_NO_DATA, "GetServerParameter:", + ControlResponseBuffer, strlen(ControlResponseBuffer), + &ClientSocket, 0); + } + else { + LogMessage(LOG_LEVEL_ERROR, "Wrong parameter count in GetServerParameter(Name)!"); + SystemControlCommand = Idle_0; + } + break; + case SetServerParameter_2: + if (CurrentInputArgCount == CommandArgCount) { + SystemControlCommand = Idle_0; + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlSetServerParameter(GSD, SystemControlArgument[0], SystemControlArgument[1], 1); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "SetServerParameter:", + ControlResponseBuffer, 0, &ClientSocket, 0); + //Send COMM_DATA_DICT to notify to update data from DataDictionary + iCommSend(COMM_DATA_DICT, ControlResponseBuffer, sizeof (ControlResponseBuffer)); + + } + else { + LogMessage(LOG_LEVEL_ERROR, "Wrong parameter count in SetServerParameter(Name, Value)!"); + SystemControlCommand = Idle_0; + } + break; + case CheckFileDirectoryExist_1: + if (CurrentInputArgCount == CommandArgCount) { + SystemControlCommand = Idle_0; + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlCheckFileDirectoryExist(SystemControlArgument[0], ControlResponseBuffer, 0); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "CheckFileDirectoryExist:", + ControlResponseBuffer, 1, &ClientSocket, 0); + + } + else { + LogMessage(LOG_LEVEL_ERROR, "Wrong parameter count in CheckFFExist(path)!"); + SystemControlCommand = Idle_0; + } + break; + case DeleteFileDirectory_1: + if (CurrentInputArgCount == CommandArgCount) { + SystemControlCommand = Idle_0; + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlDeleteFileDirectory(SystemControlArgument[0], ControlResponseBuffer, 0); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "DeleteFileDirectory:", + ControlResponseBuffer, 1, &ClientSocket, 0); + + } + else { + LogMessage(LOG_LEVEL_ERROR, "Wrong parameter count in DeleteFileDirectory(path)!"); + SystemControlCommand = Idle_0; + } + break; + case CreateDirectory_1: + if (CurrentInputArgCount == CommandArgCount) { + SystemControlCommand = Idle_0; + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlCreateDirectory(SystemControlArgument[0], ControlResponseBuffer, 0); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "CreateDirectory:", + ControlResponseBuffer, 1, &ClientSocket, 0); + + } + else { + LogMessage(LOG_LEVEL_ERROR, "Wrong parameter count in CreateDirectory(path)!"); + SystemControlCommand = Idle_0; + } + break; + case GetDirectoryContent_1: + if (CurrentInputArgCount == CommandArgCount) { + SystemControlCommand = Idle_0; + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlCheckFileDirectoryExist(SystemControlArgument[0], ControlResponseBuffer, 0); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "GetDirectoryContent:", + ControlResponseBuffer, 1, &ClientSocket, 0); + if (ControlResponseBuffer[0] == FOLDER_EXIST) { + UtilCreateDirContent(SystemControlArgument[0], "dir.info"); + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlBuildFileContentInfo("dir.info", ControlResponseBuffer, 0); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "GetDirectoryContent:", + ControlResponseBuffer, 4, &ClientSocket, 0); + SystemControlSendFileContent(&ClientSocket, "dir.info", STR_SYSTEM_CONTROL_TX_PACKET_SIZE, + ControlResponseBuffer, REMOVE_FILE, 0); + } + + } + else { + LogMessage(LOG_LEVEL_ERROR, "Wrong parameter count in GetDirectoryContent(path)!"); + SystemControlCommand = Idle_0; + } + break; + case DownloadFile_1: + if (CurrentInputArgCount == CommandArgCount) { + SystemControlCommand = Idle_0; + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlCheckFileDirectoryExist(SystemControlArgument[0], ControlResponseBuffer, 0); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "DownloadFile:", + ControlResponseBuffer, 1, &ClientSocket, 0); + if (ControlResponseBuffer[0] == FILE_EXIST) { + UtilCreateDirContent(SystemControlArgument[0], SystemControlArgument[0]); + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlBuildFileContentInfo(SystemControlArgument[0], ControlResponseBuffer, 0); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "SubDownloadFile:", + ControlResponseBuffer, 4, &ClientSocket, 0); + SystemControlSendFileContent(&ClientSocket, SystemControlArgument[0], + STR_SYSTEM_CONTROL_TX_PACKET_SIZE, ControlResponseBuffer, + KEEP_FILE, 0); + } + + } + else { + LogMessage(LOG_LEVEL_ERROR, "Wrong parameter count in GetDirectoryContent(path)!"); + SystemControlCommand = Idle_0; + } + break; + case UploadFile_3: + if (CurrentInputArgCount == CommandArgCount) { + SystemControlCommand = Idle_0; + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlUploadFile(SystemControlArgument[0], SystemControlArgument[1], + SystemControlArgument[2], ControlResponseBuffer, 0); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "UploadFile:", + ControlResponseBuffer, 1, &ClientSocket, 0); + LogMessage(LOG_LEVEL_DEBUG, "UploadFile filelength: %s", SystemControlArgument[1]); + if (ControlResponseBuffer[0] == SERVER_PREPARED_BIG_PACKET_SIZE) //Server is ready to receive data + { + LogMessage(LOG_LEVEL_INFO, "Receiving file: %s", SystemControlArgument[0]); + SystemControlReceiveRxData(&ClientSocket, SystemControlArgument[0], + SystemControlArgument[1], STR_SYSTEM_CONTROL_RX_PACKET_SIZE, + ControlResponseBuffer, 0); + } + else if (ControlResponseBuffer[0] == PATH_INVALID_MISSING) { + LogMessage(LOG_LEVEL_INFO, "Failed receiving file: %s", SystemControlArgument[0]); + SystemControlReceiveRxData(&ClientSocket, "file.tmp", SystemControlArgument[1], + STR_SYSTEM_CONTROL_RX_PACKET_SIZE, ControlResponseBuffer, 0); + SystemControlDeleteFileDirectory("file.tmp", ControlResponseBuffer, 0); + ControlResponseBuffer[0] = PATH_INVALID_MISSING; + } + else { + LogMessage(LOG_LEVEL_INFO, "Receiving file: %s", SystemControlArgument[0]); + SystemControlReceiveRxData(&ClientSocket, SystemControlArgument[0], + SystemControlArgument[1], SystemControlArgument[2], + ControlResponseBuffer, 0); + } + + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "UploadFile:", + ControlResponseBuffer, 1, &ClientSocket, 0); + + } + else { + LogMessage(LOG_LEVEL_ERROR, + "Wrong parameter count in PrepFileRx(path, filesize, packetsize)!"); + SystemControlCommand = Idle_0; + } + break; + case InitializeScenario_0: + if (server_state == SERVER_STATE_IDLE && objectControlState == OBC_STATE_IDLE) { + if (iCommSend(COMM_INIT, pcBuffer, strlen(pcBuffer) + 1) < 0) { + LogMessage(LOG_LEVEL_ERROR, "Fatal communication fault when sending INIT command"); + server_state = SERVER_STATE_ERROR; + } + server_state = SERVER_STATE_INWORK; + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "InitializeScenario:", + ControlResponseBuffer, 0, &ClientSocket, 0); + SystemControlSendLog("[SystemControl] Sending INIT.\n", &ClientSocket, 0); + } + else if (server_state == SERVER_STATE_INWORK && objectControlState == OBC_STATE_INITIALIZED) { + SystemControlSendLog + ("[SystemControl] Simulate that all objects becomes successfully configured.\n", + &ClientSocket, 0); + SystemControlCommand = Idle_0; + server_state = SERVER_STATE_IDLE; + } + else if (server_state == SERVER_STATE_IDLE) { + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE, + "InitializeScenario:", ControlResponseBuffer, 0, + &ClientSocket, 0); + SystemControlSendLog("[SystemControl] INIT received, state errors!\n", &ClientSocket, 0); + SystemControlCommand = PreviousSystemControlCommand; + } + break; + case ConnectObject_0: + if (server_state == SERVER_STATE_IDLE && objectControlState == OBC_STATE_INITIALIZED) { + if (iCommSend(COMM_CONNECT, pcBuffer, strlen(pcBuffer) + 1) < 0) { + LogMessage(LOG_LEVEL_ERROR, "Fatal communication fault when sending CONNECT command"); + server_state = SERVER_STATE_ERROR; + } + SystemControlCommand = Idle_0; + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "ConnectObject:", + ControlResponseBuffer, 0, &ClientSocket, 0); + SystemControlSendLog("[SystemControl] Sending CONNECT.\n", &ClientSocket, 0); + } + else if (server_state == SERVER_STATE_INWORK && objectControlState == OBC_STATE_CONNECTED) { + SystemControlSendLog("[SystemControl] Simulate that all objects are connected.\n", + &ClientSocket, 0); + SystemControlCommand = Idle_0; + server_state = SERVER_STATE_IDLE; + } + else if (server_state == SERVER_STATE_IDLE) { + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE, + "ConnectObject:", ControlResponseBuffer, 0, &ClientSocket, + 0); + SystemControlSendLog("[SystemControl] CONNECT received, state errors!\n", &ClientSocket, 0); + SystemControlCommand = PreviousSystemControlCommand; + } + break; + case DisconnectObject_0: + if (server_state == SERVER_STATE_IDLE) { + if (iCommSend(COMM_DISCONNECT, pcBuffer, strlen(pcBuffer) + 1) < 0) { + LogMessage(LOG_LEVEL_ERROR, "Fatal communication fault when sending DISCONNECT command"); + server_state = SERVER_STATE_ERROR; + } + SystemControlCommand = Idle_0; + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "DisconnectObject:", + ControlResponseBuffer, 0, &ClientSocket, 0); + SystemControlSendLog("[SystemControl] Sending DISCONNECT.\n", &ClientSocket, 0); + } + else { + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE, + "ConnectObject:", ControlResponseBuffer, 0, &ClientSocket, + 0); + SystemControlSendLog("[SystemControl] DISCONNECT received, state errors!\n", &ClientSocket, + 0); + SystemControlCommand = PreviousSystemControlCommand; + } + break; + case ArmScenario_0: + if (server_state == SERVER_STATE_IDLE && objectControlState == OBC_STATE_CONNECTED) { + server_state = SERVER_STATE_INWORK; + if (iCommSend(COMM_ARM, NULL, 0) < 0) { + LogMessage(LOG_LEVEL_ERROR, "Fatal communication fault when sending ARM command"); + server_state = SERVER_STATE_ERROR; + } + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "ArmScenario:", + ControlResponseBuffer, 0, &ClientSocket, 0); + SystemControlSendLog("[SystemControl] Sending ARM.\n", &ClientSocket, 0); + } + else if (server_state == SERVER_STATE_INWORK && objectControlState == OBC_STATE_ARMED) { + SystemControlSendLog("[SystemControl] Simulate that all objects become armed.\n", + &ClientSocket, 0); + + SystemControlCommand = Idle_0; + server_state = SERVER_STATE_IDLE; + } + else if (server_state == SERVER_STATE_IDLE) { + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE, + "ArmScenario:", ControlResponseBuffer, 0, &ClientSocket, 0); + SystemControlSendLog("[SystemControl] ARM received, state errors!\n", &ClientSocket, 0); + SystemControlCommand = PreviousSystemControlCommand; + } + break; + case DisarmScenario_0: + if (server_state == SERVER_STATE_IDLE && objectControlState == OBC_STATE_ARMED) { + server_state = SERVER_STATE_IDLE; + if (iCommSend(COMM_DISARM, NULL, 0) < 0) { + LogMessage(LOG_LEVEL_ERROR, "Fatal communication fault when sending DISARM command"); + server_state = SERVER_STATE_ERROR; + } + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "DisarmScenario:", + ControlResponseBuffer, 0, &ClientSocket, 0); + SystemControlSendLog("[SystemControl] Sending DISARM.\n", &ClientSocket, 0); + } + else if (server_state == SERVER_STATE_INWORK && objectControlState == OBC_STATE_CONNECTED) { + SystemControlSendLog("[SystemControl] Simulate that all objects become disarmed.\n", + &ClientSocket, 0); + SystemControlCommand = Idle_0; + server_state = SERVER_STATE_IDLE; + } + else if (server_state == SERVER_STATE_IDLE) { + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE, + "DisarmScenario:", ControlResponseBuffer, 0, &ClientSocket, + 0); + SystemControlSendLog("[SystemControl] DISARM received, state errors!\n", &ClientSocket, 0); + SystemControlCommand = PreviousSystemControlCommand; + } + break; + case StartScenario_1: + if (CurrentInputArgCount == CommandArgCount) { + if (server_state == SERVER_STATE_IDLE && objectControlState == OBC_STATE_ARMED) //Temporary! + { + bzero(pcBuffer, IPC_BUFFER_SIZE); + /* Lest use UTC time everywhere instead of etsi and gps time + gettimeofday(&tvTime, NULL); + uiTime = (uint64_t)tvTime.tv_sec*1000 + (uint64_t)tvTime.tv_usec/1000 - MS_FROM_1970_TO_2004_NO_LEAP_SECS + DIFF_LEAP_SECONDS_UTC_ETSI*1000; + */ + uiTime = UtilgetCurrentUTCtimeMS(); + if (TIME_COMPENSATE_LAGING_VM) + uiTime = uiTime - TIME_COMPENSATE_LAGING_VM_VAL; + + LogMessage(LOG_LEVEL_INFO, "Current timestamp (gtd): %lu", uiTime); + + //clock_gettime(CLOCK_MONOTONIC_COARSE, &tTime); + //clock_gettime(CLOCK_REALTIME, &tTime); + //uiTime = (uint64_t)tTime.tv_sec*1000 + (uint64_t)tTime.tv_nsec/1000000 - MS_FROM_1970_TO_2004_NO_LEAP_SECS + DIFF_LEAP_SECONDS_UTC_ETSI*1000; + //printf("[SystemControl] Current timestamp (cgt): %lu\n",uiTime ); + //printf("[SystemControl] Current timestamp: %lu\n",uiTime ); + + //uiTime += atoi(SystemControlArgument[0]); + uiTime = atoi(SystemControlArgument[0]); + DelayedStartU32 = atoi(SystemControlArgument[1]); + sprintf(pcBuffer, "%" PRIu8 ";%" PRIu64 ";%" PRIu32 ";", 0, uiTime, DelayedStartU32); + LogMessage(LOG_LEVEL_INFO, "Sending START <%s> (delayed +%s ms)", pcBuffer, + SystemControlArgument[1]); + + if (iCommSend(COMM_STRT, pcBuffer, strlen(pcBuffer) + 1) < 0) { + LogMessage(LOG_LEVEL_ERROR, "Fatal communication fault when sending STRT command"); + server_state = SERVER_STATE_ERROR; + } + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "StartScenario:", + ControlResponseBuffer, 0, &ClientSocket, 0); + server_state = SERVER_STATE_INWORK; + //server_state = SERVER_STATE_IDLE; //Temporary! + //SystemControlCommand = Idle_0; //Temporary! + } + else if (server_state == SERVER_STATE_INWORK && objectControlState == OBC_STATE_RUNNING) { + + SystemControlCommand = Idle_0; + server_state = SERVER_STATE_IDLE; + } + else if (server_state == SERVER_STATE_IDLE) { + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE, + "StartScenario:", ControlResponseBuffer, 0, + &ClientSocket, 0); + SystemControlSendLog("[SystemControl] START received, state errors!\n", &ClientSocket, 0); + SystemControlCommand = PreviousSystemControlCommand; + } + + } + else + LogMessage(LOG_LEVEL_WARNING, "START command parameter count error"); + break; + case stop_0: + if (iCommSend(COMM_STOP, NULL, 0) < 0) { + LogMessage(LOG_LEVEL_ERROR, "Fatal communication fault when sending STOP command"); + server_state = SERVER_STATE_ERROR; + } + else { + server_state = SERVER_STATE_IDLE; + SystemControlCommand = Idle_0; + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "stop:", + ControlResponseBuffer, 0, &ClientSocket, 0); + } + break; + case AbortScenario_0: + if (objectControlState == OBC_STATE_RUNNING + /* || strstr(SystemControlOBCStatesArr[OBCStateU8], "CONNECTED") != NULL + * || strstr(SystemControlOBCStatesArr[OBCStateU8], "ARMED") != NULL*/ ) + // Abort should only be allowed in running state + { + if (iCommSend(COMM_ABORT, NULL, 0) < 0) { + LogMessage(LOG_LEVEL_ERROR, "Fatal communication fault when sending ABORT command"); + server_state = SERVER_STATE_ERROR; + } + else { + server_state = SERVER_STATE_IDLE; + SystemControlCommand = Idle_0; + if (ClientSocket >= 0) { + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "AbortScenario:", + ControlResponseBuffer, 0, &ClientSocket, 0); + } + } + } + else { + if (ClientSocket >= 0) { + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE, + "AbortScenario:", ControlResponseBuffer, 0, + &ClientSocket, 0); + SystemControlSendLog("[SystemControl] ABORT received, state errors!\n", &ClientSocket, 0); + } + server_state = SERVER_STATE_IDLE; + SystemControlCommand = Idle_0; + } + break; + /* + case replay_1: + if(CurrentCommandArgCounter == CommandArgCount) + { + if(!strcmp(SystemControlArgument[CurrentCommandArgCounter],"-help")) + { + printf("[SystemControl] -----REPLAY-----\n"); + printf("[SystemControl] Syntax: replay [arg]\n"); + printf("[SystemControl] Ex: replay log/33/event.log\n"); + fflush(stdout); + } + else + { + (void)iCommSend(COMM_REPLAY, SystemControlArgument[CurrentCommandArgCounter]); + printf("[SystemControl] System control sending REPLAY on IPC <%s>\n", SystemControlArgument[CurrentCommandArgCounter]); + fflush(stdout); + } + SystemControlCommand = idle_0; + CurrentCommandArgCounter = 0; + } else CurrentCommandArgCounter ++; + break; + case control_0: + (void)iCommSend(COMM_CONTROL, NULL); + //printf("INF: System control sending CONTROL on IPC <%s>\n", pcBuffer); + fflush(stdout); + SystemControlCommand = idle_0; + CurrentCommandArgCounter = 0; + break; */ + case Exit_0: + + if (iCommSend(COMM_EXIT, NULL, 0) < 0) { + LogMessage(LOG_LEVEL_ERROR, "Fatal communication fault when sending EXIT command"); + server_state = SERVER_STATE_ERROR; + } + else { + iExit = 1; + GSD->ExitU8 = 1; + usleep(1000000); + SystemControlCommand = Idle_0; + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "Exit:", + ControlResponseBuffer, 0, &ClientSocket, 0); + close(ClientSocket); + ClientSocket = -1; + if (USE_LOCAL_USER_CONTROL == 0) { + close(ServerHandle); + ServerHandle = -1; + } + LogMessage(LOG_LEVEL_INFO, "Server closing"); + } + break; + + default: + + break; + } + + + + sleep_time.tv_sec = 0; + sleep_time.tv_nsec = SYSTEM_CONTROL_TASK_PERIOD_MS * 1000000; + ++RVSSSendCounterU16; + if (RVSSSendCounterU16 >= ((U16) RVSSRateDbl)) { + RVSSSendCounterU16 = 0; + DataDictionaryGetRVSSRateU8(GSD, &RVSSRateU8); + RVSSRateDbl = RVSSRateU8; + RVSSRateDbl = (1 / RVSSRateDbl) * 100; //This is strange!! Should be 1000, but if it is the RVSSData is sent to slow by a factor of 10. + + if (RVSSChannelSocket != 0 && RVSSSendCounterU16 == 0 && RVSSConfigU32 > 0) { + bzero(RVSSData, SYSTEM_CONTROL_RVSS_DATA_BUFFER); + + if (RVSSConfigU32 & RVSS_TIME_CHANNEL) { + SystemControlBuildRVSSTimeChannelMessage(RVSSData, &RVSSMessageLengthU32, GPSTime, 0); + UtilSendUDPData("SystemControl", &RVSSChannelSocket, &RVSSChannelAddr, RVSSData, + RVSSMessageLengthU32, 0); + } + + if (RVSSConfigU32 & RVSS_MAESTRO_CHANNEL) { + SystemControlBuildRVSSMaestroChannelMessage(RVSSData, &RVSSMessageLengthU32, GSD, + server_state, 0); + UtilSendUDPData("SystemControl", &RVSSChannelSocket, &RVSSChannelAddr, RVSSData, + RVSSMessageLengthU32, 0); + } + + if (RVSSConfigU32 & RVSS_ASP_CHANNEL) { + SystemControlBuildRVSSAspChannelMessage(RVSSData, &RVSSMessageLengthU32, 0); + UtilSendUDPData("SystemControl", &RVSSChannelSocket, &RVSSChannelAddr, RVSSData, + RVSSMessageLengthU32, 0); + } + + } + + + } + + sleep_time = (iCommand == COMM_INV + && server_state != SERVER_STATE_INWORK) ? mqEmptyPollPeriod : mqNonEmptyPollPeriod; + nanosleep(&sleep_time, &ref_time); + } + + (void)iCommClose(); + + LogMessage(LOG_LEVEL_INFO, "Exiting"); } /*------------------------------------------------------------ -- Private functions ------------------------------------------------------------*/ void signalHandler(int signo) { - if (signo == SIGINT) { - LogMessage(LOG_LEVEL_WARNING, "Caught keyboard interrupt"); - iExit = 1; - } - else { - LogMessage(LOG_LEVEL_ERROR, "Caught unhandled signal"); - } + if (signo == SIGINT) { + LogMessage(LOG_LEVEL_WARNING, "Caught keyboard interrupt"); + iExit = 1; + } + else { + LogMessage(LOG_LEVEL_ERROR, "Caught unhandled signal"); + } } SystemControlCommand_t SystemControlFindCommand(const char *CommandBuffer, - SystemControlCommand_t * CurrentCommand, - int *CommandArgCount) { - - SystemControlCommand_t command; - char StrippedCommandBuffer[SYSTEM_CONTROL_COMMAND_MAX_LENGTH]; - - bzero(StrippedCommandBuffer, SYSTEM_CONTROL_COMMAND_MAX_LENGTH); - //printf("CommandBuffer: %s\n", CommandBuffer); - strncpy(StrippedCommandBuffer, CommandBuffer, - (uint64_t) strchr(CommandBuffer, '(') - (uint64_t) CommandBuffer); - //printf("StrippedCommandBuffer: %s\n", StrippedCommandBuffer); - - for (command = Idle_0; command != nocommand; command++) { - bzero(SystemControlCommandArgCnt, SYSTEM_CONTROL_ARG_CHAR_COUNT); - bzero(SystemControlStrippedCommand, SYSTEM_CONTROL_COMMAND_MAX_LENGTH); - strncpy(SystemControlStrippedCommand, SystemControlCommandsArr[(int)command], - (uint64_t) strchr(SystemControlCommandsArr[(int)command], - '_') - (uint64_t) SystemControlCommandsArr[(int)command]); - strncpy(SystemControlCommandArgCnt, strchr(SystemControlCommandsArr[(int)command], '_') + 1, - strlen(SystemControlCommandsArr[(int)command]) - - ((uint64_t) strchr(SystemControlCommandsArr[(int)command], '_') - - (uint64_t) SystemControlCommandsArr[(int)command] + 1)); - - if (!strcmp(SystemControlStrippedCommand, StrippedCommandBuffer)) { - { - *CommandArgCount = atoi(SystemControlCommandArgCnt); - *CurrentCommand = command; - return command; - } - } - } - return nocommand; + SystemControlCommand_t * CurrentCommand, + int *CommandArgCount) { + + SystemControlCommand_t command; + char StrippedCommandBuffer[SYSTEM_CONTROL_COMMAND_MAX_LENGTH]; + + bzero(StrippedCommandBuffer, SYSTEM_CONTROL_COMMAND_MAX_LENGTH); + //printf("CommandBuffer: %s\n", CommandBuffer); + strncpy(StrippedCommandBuffer, CommandBuffer, + (uint64_t) strchr(CommandBuffer, '(') - (uint64_t) CommandBuffer); + //printf("StrippedCommandBuffer: %s\n", StrippedCommandBuffer); + + for (command = Idle_0; command != nocommand; command++) { + bzero(SystemControlCommandArgCnt, SYSTEM_CONTROL_ARG_CHAR_COUNT); + bzero(SystemControlStrippedCommand, SYSTEM_CONTROL_COMMAND_MAX_LENGTH); + strncpy(SystemControlStrippedCommand, SystemControlCommandsArr[(int)command], + (uint64_t) strchr(SystemControlCommandsArr[(int)command], + '_') - (uint64_t) SystemControlCommandsArr[(int)command]); + strncpy(SystemControlCommandArgCnt, strchr(SystemControlCommandsArr[(int)command], '_') + 1, + strlen(SystemControlCommandsArr[(int)command]) - + ((uint64_t) strchr(SystemControlCommandsArr[(int)command], '_') - + (uint64_t) SystemControlCommandsArr[(int)command] + 1)); + + if (!strcmp(SystemControlStrippedCommand, StrippedCommandBuffer)) { + { + *CommandArgCount = atoi(SystemControlCommandArgCnt); + *CurrentCommand = command; + return command; + } + } + } + return nocommand; } void SystemControlSendMONR(C8 * MONRStr, I32 * Sockfd, U8 Debug) { - int i, n, j, t; - C8 Length[4]; - C8 Header[2] = { 0, 2 }; - C8 Data[SYSTEM_CONTROL_SEND_BUFFER_SIZE]; - - bzero(Data, SYSTEM_CONTROL_SEND_BUFFER_SIZE); - n = 2 + strlen(MONRStr); - Length[0] = (C8) (n >> 24); - Length[1] = (C8) (n >> 16); - Length[2] = (C8) (n >> 8); - Length[3] = (C8) n; - - - if (n + 4 < SYSTEM_CONTROL_SEND_BUFFER_SIZE) { - for (i = 0, j = 0; i < 4; i++, j++) - Data[j] = Length[i]; - for (i = 0; i < 2; i++, j++) - Data[j] = Header[i]; - t = strlen(MONRStr); - for (i = 0; i < t; i++, j++) - Data[j] = *(MONRStr + i); - //SystemControlSendBytes(Data, n + 4, Sockfd, 0); - UtilSendTCPData("System Control", Data, n + 4, Sockfd, 0); - } - else - LogMessage(LOG_LEVEL_ERROR, "MONR string longer than %d bytes!", SYSTEM_CONTROL_SEND_BUFFER_SIZE); + int i, n, j, t; + C8 Length[4]; + C8 Header[2] = { 0, 2 }; + C8 Data[SYSTEM_CONTROL_SEND_BUFFER_SIZE]; + + bzero(Data, SYSTEM_CONTROL_SEND_BUFFER_SIZE); + n = 2 + strlen(MONRStr); + Length[0] = (C8) (n >> 24); + Length[1] = (C8) (n >> 16); + Length[2] = (C8) (n >> 8); + Length[3] = (C8) n; + + + if (n + 4 < SYSTEM_CONTROL_SEND_BUFFER_SIZE) { + for (i = 0, j = 0; i < 4; i++, j++) + Data[j] = Length[i]; + for (i = 0; i < 2; i++, j++) + Data[j] = Header[i]; + t = strlen(MONRStr); + for (i = 0; i < t; i++, j++) + Data[j] = *(MONRStr + i); + //SystemControlSendBytes(Data, n + 4, Sockfd, 0); + UtilSendTCPData("System Control", Data, n + 4, Sockfd, 0); + } + else + LogMessage(LOG_LEVEL_ERROR, "MONR string longer than %d bytes!", SYSTEM_CONTROL_SEND_BUFFER_SIZE); } void SystemControlSendLog(C8 * LogString, I32 * Sockfd, U8 Debug) { - int i, n, j, t; - C8 Length[4]; - C8 Header[2] = { 0, 2 }; - C8 Data[SYSTEM_CONTROL_SEND_BUFFER_SIZE]; - - bzero(Data, SYSTEM_CONTROL_SEND_BUFFER_SIZE); - n = 2 + strlen(LogString); - Length[0] = (C8) (n >> 24); - Length[1] = (C8) (n >> 16); - Length[2] = (C8) (n >> 8); - Length[3] = (C8) n; - - //SystemControlSendBytes(Length, 4, Sockfd, 0); - //SystemControlSendBytes(Header, 5, Sockfd, 0); - //SystemControlSendBytes(LogString, strlen(LogString), Sockfd, 0); - - - if (n + 4 < SYSTEM_CONTROL_SEND_BUFFER_SIZE) { - for (i = 0, j = 0; i < 4; i++, j++) - Data[j] = Length[i]; - for (i = 0; i < 2; i++, j++) - Data[j] = Header[i]; - t = strlen(LogString); - for (i = 0; i < t; i++, j++) - Data[j] = *(LogString + i); - //SystemControlSendBytes(Data, n + 4, Sockfd, 0); - UtilSendTCPData("System Control", Data, n + 4, Sockfd, 0); - } - else - LogMessage(LOG_LEVEL_ERROR, "Log string longer than %d bytes!", SYSTEM_CONTROL_SEND_BUFFER_SIZE); + int i, n, j, t; + C8 Length[4]; + C8 Header[2] = { 0, 2 }; + C8 Data[SYSTEM_CONTROL_SEND_BUFFER_SIZE]; + + bzero(Data, SYSTEM_CONTROL_SEND_BUFFER_SIZE); + n = 2 + strlen(LogString); + Length[0] = (C8) (n >> 24); + Length[1] = (C8) (n >> 16); + Length[2] = (C8) (n >> 8); + Length[3] = (C8) n; + + //SystemControlSendBytes(Length, 4, Sockfd, 0); + //SystemControlSendBytes(Header, 5, Sockfd, 0); + //SystemControlSendBytes(LogString, strlen(LogString), Sockfd, 0); + + + if (n + 4 < SYSTEM_CONTROL_SEND_BUFFER_SIZE) { + for (i = 0, j = 0; i < 4; i++, j++) + Data[j] = Length[i]; + for (i = 0; i < 2; i++, j++) + Data[j] = Header[i]; + t = strlen(LogString); + for (i = 0; i < t; i++, j++) + Data[j] = *(LogString + i); + //SystemControlSendBytes(Data, n + 4, Sockfd, 0); + UtilSendTCPData("System Control", Data, n + 4, Sockfd, 0); + } + else + LogMessage(LOG_LEVEL_ERROR, "Log string longer than %d bytes!", SYSTEM_CONTROL_SEND_BUFFER_SIZE); } void SystemControlSendControlResponse(U16 ResponseStatus, C8 * ResponseString, C8 * ResponseData, - I32 ResponseDataLength, I32 * Sockfd, U8 Debug) { - int i, n, j, t; - C8 Length[4]; - C8 Status[2]; - C8 Data[SYSTEM_CONTROL_SEND_BUFFER_SIZE]; - - bzero(Data, SYSTEM_CONTROL_SEND_BUFFER_SIZE); - n = 2 + strlen(ResponseString) + ResponseDataLength; - Length[0] = (C8) (n >> 24); - Length[1] = (C8) (n >> 16); - Length[2] = (C8) (n >> 8); - Length[3] = (C8) n; - Status[0] = (C8) (ResponseStatus >> 8); - Status[1] = (C8) ResponseStatus; - - if (n + 4 < SYSTEM_CONTROL_SEND_BUFFER_SIZE) { - for (i = 0, j = 0; i < 4; i++, j++) - Data[j] = Length[i]; - for (i = 0; i < 2; i++, j++) - Data[j] = Status[i]; - t = strlen(ResponseString); - for (i = 0; i < t; i++, j++) - Data[j] = *(ResponseString + i); - for (i = 0; i < ResponseDataLength; i++, j++) - Data[j] = ResponseData[i]; - - if (Debug) { - for (i = 0; i < n + 4; i++) - printf("%x-", Data[i]); - printf("\n"); - } - - //SystemControlSendBytes(Data, n + 4, Sockfd, 0); - UtilSendTCPData("System Control", Data, n + 4, Sockfd, 0); - } - else - LogMessage(LOG_LEVEL_ERROR, "Response data more than %d bytes!", SYSTEM_CONTROL_SEND_BUFFER_SIZE); + I32 ResponseDataLength, I32 * Sockfd, U8 Debug) { + int i, n, j, t; + C8 Length[4]; + C8 Status[2]; + C8 Data[SYSTEM_CONTROL_SEND_BUFFER_SIZE]; + + bzero(Data, SYSTEM_CONTROL_SEND_BUFFER_SIZE); + n = 2 + strlen(ResponseString) + ResponseDataLength; + Length[0] = (C8) (n >> 24); + Length[1] = (C8) (n >> 16); + Length[2] = (C8) (n >> 8); + Length[3] = (C8) n; + Status[0] = (C8) (ResponseStatus >> 8); + Status[1] = (C8) ResponseStatus; + + if (n + 4 < SYSTEM_CONTROL_SEND_BUFFER_SIZE) { + for (i = 0, j = 0; i < 4; i++, j++) + Data[j] = Length[i]; + for (i = 0; i < 2; i++, j++) + Data[j] = Status[i]; + t = strlen(ResponseString); + for (i = 0; i < t; i++, j++) + Data[j] = *(ResponseString + i); + for (i = 0; i < ResponseDataLength; i++, j++) + Data[j] = ResponseData[i]; + + if (Debug) { + for (i = 0; i < n + 4; i++) + printf("%x-", Data[i]); + printf("\n"); + } + + //SystemControlSendBytes(Data, n + 4, Sockfd, 0); + UtilSendTCPData("System Control", Data, n + 4, Sockfd, 0); + } + else + LogMessage(LOG_LEVEL_ERROR, "Response data more than %d bytes!", SYSTEM_CONTROL_SEND_BUFFER_SIZE); } I32 SystemControlBuildControlResponse(U16 ResponseStatus, C8 * ResponseString, C8 * ResponseData, - I32 ResponseDataLength, U8 Debug) { - int i = 0, n = 0, j = 0, t = 0; - C8 Length[4]; - C8 Status[2]; - C8 Data[SYSTEM_CONTROL_SEND_BUFFER_SIZE]; - - bzero(Data, SYSTEM_CONTROL_SEND_BUFFER_SIZE); - n = 2 + strlen(ResponseString) + ResponseDataLength; - Length[0] = (C8) (n >> 24); - Length[1] = (C8) (n >> 16); - Length[2] = (C8) (n >> 8); - Length[3] = (C8) n; - Status[0] = (C8) (ResponseStatus >> 8); - Status[1] = (C8) ResponseStatus; - - if (n + 4 < SYSTEM_CONTROL_SEND_BUFFER_SIZE) { - for (i = 0, j = 0; i < 4; i++, j++) - Data[j] = Length[i]; - for (i = 0; i < 2; i++, j++) - Data[j] = Status[i]; - t = strlen(ResponseString); - for (i = 0; i < t; i++, j++) - Data[j] = *(ResponseString + i); - for (i = 0; i < ResponseDataLength; i++, j++) - Data[j] = ResponseData[i]; - - for (i = 0; i < n; i++) - *(ResponseData + i) = Data[i]; //Copy back - - if (Debug) { - for (i = 0; i < n + 4; i++) - printf("%x-", Data[i]); - printf("\n"); - } - - } - else - LogMessage(LOG_LEVEL_ERROR, "Response data more than %d bytes!", SYSTEM_CONTROL_SEND_BUFFER_SIZE); - - - - - return n; + I32 ResponseDataLength, U8 Debug) { + int i = 0, n = 0, j = 0, t = 0; + C8 Length[4]; + C8 Status[2]; + C8 Data[SYSTEM_CONTROL_SEND_BUFFER_SIZE]; + + bzero(Data, SYSTEM_CONTROL_SEND_BUFFER_SIZE); + n = 2 + strlen(ResponseString) + ResponseDataLength; + Length[0] = (C8) (n >> 24); + Length[1] = (C8) (n >> 16); + Length[2] = (C8) (n >> 8); + Length[3] = (C8) n; + Status[0] = (C8) (ResponseStatus >> 8); + Status[1] = (C8) ResponseStatus; + + if (n + 4 < SYSTEM_CONTROL_SEND_BUFFER_SIZE) { + for (i = 0, j = 0; i < 4; i++, j++) + Data[j] = Length[i]; + for (i = 0; i < 2; i++, j++) + Data[j] = Status[i]; + t = strlen(ResponseString); + for (i = 0; i < t; i++, j++) + Data[j] = *(ResponseString + i); + for (i = 0; i < ResponseDataLength; i++, j++) + Data[j] = ResponseData[i]; + + for (i = 0; i < n; i++) + *(ResponseData + i) = Data[i]; //Copy back + + if (Debug) { + for (i = 0; i < n + 4; i++) + printf("%x-", Data[i]); + printf("\n"); + } + + } + else + LogMessage(LOG_LEVEL_ERROR, "Response data more than %d bytes!", SYSTEM_CONTROL_SEND_BUFFER_SIZE); + + + + + return n; } static void SystemControlSendBytes(const char *data, int length, int *sockfd, int debug) { - int i, n; + int i, n; - if (debug == 1) { - printf("Bytes sent: "); - int i = 0; + if (debug == 1) { + printf("Bytes sent: "); + int i = 0; - for (i = 0; i < length; i++) - printf("%d ", (C8) * (data + i)); - printf("\n"); - } + for (i = 0; i < length; i++) + printf("%d ", (C8) * (data + i)); + printf("\n"); + } - n = write(*sockfd, data, length); - if (n < 0) { - util_error("[SystemControl] ERR: Failed to send on control socket"); - } + n = write(*sockfd, data, length); + if (n < 0) { + util_error("[SystemControl] ERR: Failed to send on control socket"); + } } static I32 SystemControlInitServer(int *ClientSocket, int *ServerHandle, struct in_addr *ip_addr) { - struct sockaddr_in command_server_addr; - struct sockaddr_in cli_addr; - socklen_t cli_length; - unsigned int control_port = SYSTEM_CONTROL_CONTROL_PORT; - int optval = 1; - int result = 0; - int sockFlags = 0; - - /* Init user control socket */ - LogMessage(LOG_LEVEL_INFO, "Init control socket"); - - *ServerHandle = socket(AF_INET, SOCK_STREAM, 0); - if (*ServerHandle < 0) { - perror("[SystemControl] ERR: Failed to create control socket"); - exit(1); - } + struct sockaddr_in command_server_addr; + struct sockaddr_in cli_addr; + socklen_t cli_length; + unsigned int control_port = SYSTEM_CONTROL_CONTROL_PORT; + int optval = 1; + int result = 0; + int sockFlags = 0; + + /* Init user control socket */ + LogMessage(LOG_LEVEL_INFO, "Init control socket"); + + *ServerHandle = socket(AF_INET, SOCK_STREAM, 0); + if (*ServerHandle < 0) { + perror("[SystemControl] ERR: Failed to create control socket"); + exit(1); + } - bzero((char *)&command_server_addr, sizeof (command_server_addr)); + bzero((char *)&command_server_addr, sizeof (command_server_addr)); - command_server_addr.sin_family = AF_INET; - command_server_addr.sin_addr.s_addr = INADDR_ANY; - command_server_addr.sin_port = htons(control_port); + command_server_addr.sin_family = AF_INET; + command_server_addr.sin_addr.s_addr = INADDR_ANY; + command_server_addr.sin_port = htons(control_port); - optval = 1; - result = setsockopt(*ServerHandle, SOL_SOCKET, SO_REUSEADDR, &optval, sizeof optval); + optval = 1; + result = setsockopt(*ServerHandle, SOL_SOCKET, SO_REUSEADDR, &optval, sizeof optval); - if (result < 0) { - perror("[SystemControl] ERR: Failed to call setsockopt"); - exit(1); - } + if (result < 0) { + perror("[SystemControl] ERR: Failed to call setsockopt"); + exit(1); + } - if (bind(*ServerHandle, (struct sockaddr *)&command_server_addr, sizeof (command_server_addr)) < 0) { - perror("[SystemControl] ERR: Failed to bind to control socket"); - exit(1); - } + if (bind(*ServerHandle, (struct sockaddr *)&command_server_addr, sizeof (command_server_addr)) < 0) { + perror("[SystemControl] ERR: Failed to bind to control socket"); + exit(1); + } - /* Monitor and control sockets up. Wait for central to connect to control socket to get server address */ - LogMessage(LOG_LEVEL_INFO, "Listening for connection from client..."); + /* Monitor and control sockets up. Wait for central to connect to control socket to get server address */ + LogMessage(LOG_LEVEL_INFO, "Listening for connection from client..."); - listen(*ServerHandle, 1); - cli_length = sizeof (cli_addr); + listen(*ServerHandle, 1); + cli_length = sizeof (cli_addr); - /* Set socket to nonblocking */ - sockFlags = fcntl(*ServerHandle, F_GETFL, 0); - if (sockFlags == -1) - util_error("Error calling fcntl"); + /* Set socket to nonblocking */ + sockFlags = fcntl(*ServerHandle, F_GETFL, 0); + if (sockFlags == -1) + util_error("Error calling fcntl"); - sockFlags = sockFlags | O_NONBLOCK; - if (fcntl(*ServerHandle, F_SETFL, sockFlags)) - util_error("Error calling fcntl"); + sockFlags = sockFlags | O_NONBLOCK; + if (fcntl(*ServerHandle, F_SETFL, sockFlags)) + util_error("Error calling fcntl"); - do { - *ClientSocket = accept(*ServerHandle, (struct sockaddr *)&cli_addr, &cli_length); - if ((*ClientSocket == -1 && errno != EAGAIN && errno != EWOULDBLOCK) || iExit) - util_error("Failed to establish connection"); - } while (*ClientSocket == -1); + do { + *ClientSocket = accept(*ServerHandle, (struct sockaddr *)&cli_addr, &cli_length); + if ((*ClientSocket == -1 && errno != EAGAIN && errno != EWOULDBLOCK) || iExit) + util_error("Failed to establish connection"); + } while (*ClientSocket == -1); - LogMessage(LOG_LEVEL_INFO, "Connection established: %s:%i", inet_ntoa(cli_addr.sin_addr), - htons(command_server_addr.sin_port)); + LogMessage(LOG_LEVEL_INFO, "Connection established: %s:%i", inet_ntoa(cli_addr.sin_addr), + htons(command_server_addr.sin_port)); - ip_addr->s_addr = cli_addr.sin_addr.s_addr; //Set IP-address of Usercontrol + ip_addr->s_addr = cli_addr.sin_addr.s_addr; //Set IP-address of Usercontrol - if (*ClientSocket < 0) { - perror("[SystemControl] ERR: Failed to accept from central"); - exit(1); - } + if (*ClientSocket < 0) { + perror("[SystemControl] ERR: Failed to accept from central"); + exit(1); + } - return result; + return result; } static I32 SystemControlConnectServer(int *sockfd, const char *name, const uint32_t port) { - struct sockaddr_in serv_addr; - struct hostent *server; + struct sockaddr_in serv_addr; + struct hostent *server; - char buffer[256]; - int iResult; + char buffer[256]; + int iResult; - *sockfd = socket(AF_INET, SOCK_STREAM, 0); + *sockfd = socket(AF_INET, SOCK_STREAM, 0); - if (*sockfd < 0) { - util_error("[SystemControl] ERR: Failed to open control socket"); - } + if (*sockfd < 0) { + util_error("[SystemControl] ERR: Failed to open control socket"); + } - server = gethostbyname(name); - if (server == NULL) { - util_error("[SystemControl] ERR: Unknown host "); - } + server = gethostbyname(name); + if (server == NULL) { + util_error("[SystemControl] ERR: Unknown host "); + } - bzero((char *)&serv_addr, sizeof (serv_addr)); - serv_addr.sin_family = AF_INET; + bzero((char *)&serv_addr, sizeof (serv_addr)); + serv_addr.sin_family = AF_INET; - bcopy((char *)server->h_addr, (char *)&serv_addr.sin_addr.s_addr, server->h_length); - serv_addr.sin_port = htons(port); + bcopy((char *)server->h_addr, (char *)&serv_addr.sin_addr.s_addr, server->h_length); + serv_addr.sin_port = htons(port); - LogMessage(LOG_LEVEL_INFO, "Attempting to connect to control socket: %s:%i", name, port); + LogMessage(LOG_LEVEL_INFO, "Attempting to connect to control socket: %s:%i", name, port); - do { - iResult = connect(*sockfd, (struct sockaddr *)&serv_addr, sizeof (serv_addr)); + do { + iResult = connect(*sockfd, (struct sockaddr *)&serv_addr, sizeof (serv_addr)); - if (iResult < 0) { - if (errno == ECONNREFUSED) { - LogMessage(LOG_LEVEL_WARNING, "Unable to connect to UserControl, retrying in 3 sec..."); - (void)sleep(3); - } - else { - util_error("[SystemControl] ERR: Failed to connect to control socket"); - } - } - } while (iResult < 0); + if (iResult < 0) { + if (errno == ECONNREFUSED) { + LogMessage(LOG_LEVEL_WARNING, "Unable to connect to UserControl, retrying in 3 sec..."); + (void)sleep(3); + } + else { + util_error("[SystemControl] ERR: Failed to connect to control socket"); + } + } + } while (iResult < 0); - iResult = fcntl(*sockfd, F_SETFL, fcntl(*sockfd, F_GETFL, 0) | O_NONBLOCK); + iResult = fcntl(*sockfd, F_SETFL, fcntl(*sockfd, F_GETFL, 0) | O_NONBLOCK); - LogMessage(LOG_LEVEL_DEBUG, "Maestro connected to UserControl: %s:%i", name, port); - return iResult; + LogMessage(LOG_LEVEL_DEBUG, "Maestro connected to UserControl: %s:%i", name, port); + return iResult; } static void SystemControlCreateProcessChannel(const C8 * name, const U32 port, I32 * sockfd, - struct sockaddr_in *addr) { - int result; - struct hostent *object; + struct sockaddr_in *addr) { + int result; + struct hostent *object; - /* Connect to object safety socket */ + /* Connect to object safety socket */ - LogMessage(LOG_LEVEL_DEBUG, "Creating process channel socket"); + LogMessage(LOG_LEVEL_DEBUG, "Creating process channel socket"); - *sockfd = socket(AF_INET, SOCK_DGRAM, 0); - if (*sockfd < 0) { - util_error("[SystemControl] ERR: Failed to connect to process channel socket"); - } + *sockfd = socket(AF_INET, SOCK_DGRAM, 0); + if (*sockfd < 0) { + util_error("[SystemControl] ERR: Failed to connect to process channel socket"); + } - /* Set address to object */ - object = gethostbyname(name); + /* Set address to object */ + object = gethostbyname(name); - if (object == 0) { - util_error("[SystemControl] ERR: Unknown host"); - } + if (object == 0) { + util_error("[SystemControl] ERR: Unknown host"); + } - bcopy((char *)object->h_addr, (char *)&addr->sin_addr.s_addr, object->h_length); - addr->sin_family = AF_INET; - addr->sin_port = htons(port); + bcopy((char *)object->h_addr, (char *)&addr->sin_addr.s_addr, object->h_length); + addr->sin_family = AF_INET; + addr->sin_port = htons(port); - /* set socket to non-blocking */ - result = fcntl(*sockfd, F_SETFL, fcntl(*sockfd, F_GETFL, 0) | O_NONBLOCK); - if (result < 0) { - util_error("[SystemControl] ERR: calling fcntl"); - } + /* set socket to non-blocking */ + result = fcntl(*sockfd, F_SETFL, fcntl(*sockfd, F_GETFL, 0) | O_NONBLOCK); + if (result < 0) { + util_error("[SystemControl] ERR: calling fcntl"); + } - LogMessage(LOG_LEVEL_INFO, "Created process channel socket and address: %s:%d", name, port); + LogMessage(LOG_LEVEL_INFO, "Created process channel socket and address: %s:%d", name, port); } /*! @@ -1503,45 +1503,45 @@ static void SystemControlCreateProcessChannel(const C8 * name, const U32 port, I * \return true if match, false if not */ C8 SystemControlVerifyHostAddress(char *addr) { - struct ifaddrs *ifaddr, *ifa; - int family, s, n; - char host[NI_MAXHOST]; - - if (getifaddrs(&ifaddr) == -1) { - LogMessage(LOG_LEVEL_ERROR, "Could not get interface data"); - freeifaddrs(ifaddr); - return 0; - } - - // Iterate over linked list using ifa - for (ifa = ifaddr, n = 0; ifa != NULL; ifa = ifa->ifa_next, n++) { - if (ifa->ifa_addr == NULL) { - // Interface had no addresses, skip to next - continue; - } - - family = ifa->ifa_addr->sa_family; - if (family == AF_INET || family == AF_INET6) { - s = getnameinfo(ifa->ifa_addr, - (family == AF_INET) ? sizeof (struct sockaddr_in) : - sizeof (struct sockaddr_in6), host, NI_MAXHOST, NULL, 0, NI_NUMERICHOST); - if (s != 0) { - LogMessage(LOG_LEVEL_ERROR, "getnameinfo() failed: %s", gai_strerror(s)); - continue; - } - - if (strcmp(host, addr) == 0) - return 1; - else - continue; - } - else { - continue; - } - } - - freeifaddrs(ifaddr); - return 0; + struct ifaddrs *ifaddr, *ifa; + int family, s, n; + char host[NI_MAXHOST]; + + if (getifaddrs(&ifaddr) == -1) { + LogMessage(LOG_LEVEL_ERROR, "Could not get interface data"); + freeifaddrs(ifaddr); + return 0; + } + + // Iterate over linked list using ifa + for (ifa = ifaddr, n = 0; ifa != NULL; ifa = ifa->ifa_next, n++) { + if (ifa->ifa_addr == NULL) { + // Interface had no addresses, skip to next + continue; + } + + family = ifa->ifa_addr->sa_family; + if (family == AF_INET || family == AF_INET6) { + s = getnameinfo(ifa->ifa_addr, + (family == AF_INET) ? sizeof (struct sockaddr_in) : + sizeof (struct sockaddr_in6), host, NI_MAXHOST, NULL, 0, NI_NUMERICHOST); + if (s != 0) { + LogMessage(LOG_LEVEL_ERROR, "getnameinfo() failed: %s", gai_strerror(s)); + continue; + } + + if (strcmp(host, addr) == 0) + return 1; + else + continue; + } + else { + continue; + } + } + + freeifaddrs(ifaddr); + return 0; } /* @@ -1561,445 +1561,445 @@ I32 SystemControlSendUDPData(I32 *sockfd, struct sockaddr_in* addr, C8 *SendData */ I32 SystemControlGetServerParameter(GSDType * GSD, C8 * ParameterName, C8 * ReturnValue, U32 BufferLength, - U8 Debug) { - bzero(ReturnValue, 20); - dbl ValueDbl = 0; - U32 ValueU32 = 0; - U16 ValueU16 = 0; - U8 ValueU8 = 0; - - if (strcmp("OrigoLatitude", ParameterName) == 0) { - DataDictionaryGetOriginLatitudeDbl(GSD, &ValueDbl); - sprintf(ReturnValue, "%3.12f", ValueDbl); - } - else if (strcmp("OrigoLongitude", ParameterName) == 0) { - DataDictionaryGetOriginLongitudeDbl(GSD, &ValueDbl); - sprintf(ReturnValue, "%3.12f", ValueDbl); - } - else if (strcmp("OrigoAltitude", ParameterName) == 0) { - DataDictionaryGetOriginAltitudeDbl(GSD, &ValueDbl); - sprintf(ReturnValue, "%3.12f", ValueDbl); - } - else if (strcmp("VisualizationServerName", ParameterName) == 0) { - DataDictionaryGetVisualizationServerC8(GSD, ReturnValue, BufferLength); - } - else if (strcmp("ForceObjectToLocalhost", ParameterName) == 0) { - DataDictionaryGetForceToLocalhostU8(GSD, &ValueU8); - sprintf(ReturnValue, "%" PRIu8, ValueU8); - } - else if (strcmp("ASPMaxTimeDiff", ParameterName) == 0) { - DataDictionaryGetASPMaxTimeDiffDbl(GSD, &ValueDbl); - sprintf(ReturnValue, "%3.3f", ValueDbl); - } - else if (strcmp("ASPMaxTrajDiff", ParameterName) == 0) { - DataDictionaryGetASPMaxTrajDiffDbl(GSD, &ValueDbl); - sprintf(ReturnValue, "%3.3f", ValueDbl); - } - else if (strcmp("ASPStepBackCount", ParameterName) == 0) { - DataDictionaryGetASPStepBackCountU32(GSD, &ValueU32); - sprintf(ReturnValue, "%" PRIu32, ValueU32); - } - else if (strcmp("ASPFilterLevel", ParameterName) == 0) { - DataDictionaryGetASPFilterLevelDbl(GSD, &ValueDbl); - sprintf(ReturnValue, "%3.3f", ValueDbl); - } - else if (strcmp("ASPMaxDeltaTime", ParameterName) == 0) { - DataDictionaryGetASPMaxDeltaTimeDbl(GSD, &ValueDbl); - sprintf(ReturnValue, "%3.3f", ValueDbl); - } - else if (strcmp("TimeServerIP", ParameterName) == 0) { - DataDictionaryGetTimeServerIPC8(GSD, ReturnValue, BufferLength); - } - else if (strcmp("TimeServerPort", ParameterName) == 0) { - DataDictionaryGetTimeServerPortU16(GSD, &ValueU16); - sprintf(ReturnValue, "%" PRIu16, ValueU16); - } - else if (strcmp("SimulatorIP", ParameterName) == 0) { - DataDictionaryGetSimulatorIPC8(GSD, ReturnValue, BufferLength); - } - else if (strcmp("SimulatorTCPPort", ParameterName) == 0) { - DataDictionaryGetSimulatorTCPPortU16(GSD, &ValueU16); - sprintf(ReturnValue, "%" PRIu16, ValueU16); - } - else if (strcmp("SimulatorUDPPort", ParameterName) == 0) { - DataDictionaryGetSimulatorUDPPortU16(GSD, &ValueU16); - sprintf(ReturnValue, "%" PRIu16, ValueU16); - } - else if (strcmp("SimulatorMode", ParameterName) == 0) { - DataDictionaryGetSimulatorModeU8(GSD, &ValueU8); - sprintf(ReturnValue, "%" PRIu8, ValueU8); - } - else if (strcmp("VOILReceivers", ParameterName) == 0) { - DataDictionaryGetVOILReceiversC8(GSD, ReturnValue, BufferLength); - } - else if (strcmp("DTMReceivers", ParameterName) == 0) { - DataDictionaryGetDTMReceiversC8(GSD, ReturnValue, BufferLength); - } - else if (strcmp("SupervisorIP", ParameterName) == 0) { - DataDictionaryGetExternalSupervisorIPC8(GSD, ReturnValue, BufferLength); - } - else if (strcmp("SupervisorTCPPort", ParameterName) == 0) { - DataDictionaryGetSupervisorTCPPortU16(GSD, &ValueU16); - sprintf(ReturnValue, "%" PRIu16, ValueU16); - } - else if (strcmp("MiscData", ParameterName) == 0) { - DataDictionaryGetMiscDataC8(GSD, ReturnValue, BufferLength); - } - else if (strcmp("RVSSConfig", ParameterName) == 0) { - DataDictionaryGetRVSSConfigU32(GSD, &ValueU32); - sprintf(ReturnValue, "%" PRIu32, ValueU32); - } - else if (strcmp("RVSSRate", ParameterName) == 0) { - DataDictionaryGetRVSSRateU8(GSD, &ValueU8); - sprintf(ReturnValue, "%" PRIu8, ValueU8); - } + U8 Debug) { + bzero(ReturnValue, 20); + dbl ValueDbl = 0; + U32 ValueU32 = 0; + U16 ValueU16 = 0; + U8 ValueU8 = 0; + + if (strcmp("OrigoLatitude", ParameterName) == 0) { + DataDictionaryGetOriginLatitudeDbl(GSD, &ValueDbl); + sprintf(ReturnValue, "%3.12f", ValueDbl); + } + else if (strcmp("OrigoLongitude", ParameterName) == 0) { + DataDictionaryGetOriginLongitudeDbl(GSD, &ValueDbl); + sprintf(ReturnValue, "%3.12f", ValueDbl); + } + else if (strcmp("OrigoAltitude", ParameterName) == 0) { + DataDictionaryGetOriginAltitudeDbl(GSD, &ValueDbl); + sprintf(ReturnValue, "%3.12f", ValueDbl); + } + else if (strcmp("VisualizationServerName", ParameterName) == 0) { + DataDictionaryGetVisualizationServerC8(GSD, ReturnValue, BufferLength); + } + else if (strcmp("ForceObjectToLocalhost", ParameterName) == 0) { + DataDictionaryGetForceToLocalhostU8(GSD, &ValueU8); + sprintf(ReturnValue, "%" PRIu8, ValueU8); + } + else if (strcmp("ASPMaxTimeDiff", ParameterName) == 0) { + DataDictionaryGetASPMaxTimeDiffDbl(GSD, &ValueDbl); + sprintf(ReturnValue, "%3.3f", ValueDbl); + } + else if (strcmp("ASPMaxTrajDiff", ParameterName) == 0) { + DataDictionaryGetASPMaxTrajDiffDbl(GSD, &ValueDbl); + sprintf(ReturnValue, "%3.3f", ValueDbl); + } + else if (strcmp("ASPStepBackCount", ParameterName) == 0) { + DataDictionaryGetASPStepBackCountU32(GSD, &ValueU32); + sprintf(ReturnValue, "%" PRIu32, ValueU32); + } + else if (strcmp("ASPFilterLevel", ParameterName) == 0) { + DataDictionaryGetASPFilterLevelDbl(GSD, &ValueDbl); + sprintf(ReturnValue, "%3.3f", ValueDbl); + } + else if (strcmp("ASPMaxDeltaTime", ParameterName) == 0) { + DataDictionaryGetASPMaxDeltaTimeDbl(GSD, &ValueDbl); + sprintf(ReturnValue, "%3.3f", ValueDbl); + } + else if (strcmp("TimeServerIP", ParameterName) == 0) { + DataDictionaryGetTimeServerIPC8(GSD, ReturnValue, BufferLength); + } + else if (strcmp("TimeServerPort", ParameterName) == 0) { + DataDictionaryGetTimeServerPortU16(GSD, &ValueU16); + sprintf(ReturnValue, "%" PRIu16, ValueU16); + } + else if (strcmp("SimulatorIP", ParameterName) == 0) { + DataDictionaryGetSimulatorIPC8(GSD, ReturnValue, BufferLength); + } + else if (strcmp("SimulatorTCPPort", ParameterName) == 0) { + DataDictionaryGetSimulatorTCPPortU16(GSD, &ValueU16); + sprintf(ReturnValue, "%" PRIu16, ValueU16); + } + else if (strcmp("SimulatorUDPPort", ParameterName) == 0) { + DataDictionaryGetSimulatorUDPPortU16(GSD, &ValueU16); + sprintf(ReturnValue, "%" PRIu16, ValueU16); + } + else if (strcmp("SimulatorMode", ParameterName) == 0) { + DataDictionaryGetSimulatorModeU8(GSD, &ValueU8); + sprintf(ReturnValue, "%" PRIu8, ValueU8); + } + else if (strcmp("VOILReceivers", ParameterName) == 0) { + DataDictionaryGetVOILReceiversC8(GSD, ReturnValue, BufferLength); + } + else if (strcmp("DTMReceivers", ParameterName) == 0) { + DataDictionaryGetDTMReceiversC8(GSD, ReturnValue, BufferLength); + } + else if (strcmp("SupervisorIP", ParameterName) == 0) { + DataDictionaryGetExternalSupervisorIPC8(GSD, ReturnValue, BufferLength); + } + else if (strcmp("SupervisorTCPPort", ParameterName) == 0) { + DataDictionaryGetSupervisorTCPPortU16(GSD, &ValueU16); + sprintf(ReturnValue, "%" PRIu16, ValueU16); + } + else if (strcmp("MiscData", ParameterName) == 0) { + DataDictionaryGetMiscDataC8(GSD, ReturnValue, BufferLength); + } + else if (strcmp("RVSSConfig", ParameterName) == 0) { + DataDictionaryGetRVSSConfigU32(GSD, &ValueU32); + sprintf(ReturnValue, "%" PRIu32, ValueU32); + } + else if (strcmp("RVSSRate", ParameterName) == 0) { + DataDictionaryGetRVSSRateU8(GSD, &ValueU8); + sprintf(ReturnValue, "%" PRIu8, ValueU8); + } } I32 SystemControlSetServerParameter(GSDType * GSD, C8 * ParameterName, C8 * NewValue, U8 Debug) { - if (Debug) - printf("[SystemControl] SetServerParameter: %s = %s\n", ParameterName, NewValue); - if (strcmp("OrigoLatitude", ParameterName) == 0) - DataDictionarySetOriginLatitudeDbl(GSD, NewValue); - else if (strcmp("OrigoLongitude", ParameterName) == 0) - DataDictionarySetOriginLongitudeDbl(GSD, NewValue); - else if (strcmp("OrigoAltitude", ParameterName) == 0) - DataDictionarySetOriginAltitudeDbl(GSD, NewValue); - else if (strcmp("VisualizationServerName", ParameterName) == 0) - DataDictionarySetVisualizationServerU32(GSD, NewValue); - else if (strcmp("ForceObjectToLocalhost", ParameterName) == 0) - DataDictionarySetForceToLocalhostU8(GSD, NewValue); - else if (strcmp("ASPMaxTimeDiff", ParameterName) == 0) - DataDictionarySetASPMaxTimeDiffDbl(GSD, NewValue); - else if (strcmp("ASPMaxTrajDiff", ParameterName) == 0) - DataDictionarySetASPMaxTrajDiffDbl(GSD, NewValue); - else if (strcmp("ASPStepBackCount", ParameterName) == 0) - DataDictionarySetASPStepBackCountU32(GSD, NewValue); - else if (strcmp("ASPFilterLevel", ParameterName) == 0) - DataDictionarySetASPFilterLevelDbl(GSD, NewValue); - else if (strcmp("ASPMaxDeltaTime", ParameterName) == 0) - DataDictionarySetASPMaxDeltaTimeDbl(GSD, NewValue); - else if (strcmp("TimeServerIP", ParameterName) == 0) - DataDictionarySetTimeServerIPU32(GSD, NewValue); - else if (strcmp("TimeServerPort", ParameterName) == 0) - DataDictionarySetTimeServerPortU16(GSD, NewValue); - else if (strcmp("SimulatorIP", ParameterName) == 0) - DataDictionarySetSimulatorIPU32(GSD, NewValue); - else if (strcmp("SimulatorTCPPort", ParameterName) == 0) - DataDictionarySetSimulatorTCPPortU16(GSD, NewValue); - else if (strcmp("SimulatorUDPPort", ParameterName) == 0) - DataDictionarySetSimulatorUDPPortU16(GSD, NewValue); - else if (strcmp("SimulatorMode", ParameterName) == 0) - DataDictionarySetSimulatorModeU8(GSD, NewValue); - else if (strcmp("VOILReceivers", ParameterName) == 0) - DataDictionarySetVOILReceiversC8(GSD, NewValue); - else if (strcmp("DTMReceivers", ParameterName) == 0) - DataDictionarySetDTMReceiversC8(GSD, NewValue); - else if (strcmp("SupervisorIP", ParameterName) == 0) - DataDictionarySetExternalSupervisorIPU32(GSD, NewValue); - else if (strcmp("SupervisorTCPPort", ParameterName) == 0) - DataDictionarySetSupervisorTCPPortU16(GSD, NewValue); - else if (strcmp("MiscData", ParameterName) == 0) - DataDictionarySetMiscDataC8(GSD, NewValue); - else if (strcmp("RVSSConfig", ParameterName) == 0) - DataDictionarySetRVSSConfigU32(GSD, (U32) atoi(NewValue)); - else if (strcmp("RVSSRate", ParameterName) == 0) - DataDictionarySetRVSSRateU8(GSD, (U32) atoi(NewValue)); + if (Debug) + printf("[SystemControl] SetServerParameter: %s = %s\n", ParameterName, NewValue); + if (strcmp("OrigoLatitude", ParameterName) == 0) + DataDictionarySetOriginLatitudeDbl(GSD, NewValue); + else if (strcmp("OrigoLongitude", ParameterName) == 0) + DataDictionarySetOriginLongitudeDbl(GSD, NewValue); + else if (strcmp("OrigoAltitude", ParameterName) == 0) + DataDictionarySetOriginAltitudeDbl(GSD, NewValue); + else if (strcmp("VisualizationServerName", ParameterName) == 0) + DataDictionarySetVisualizationServerU32(GSD, NewValue); + else if (strcmp("ForceObjectToLocalhost", ParameterName) == 0) + DataDictionarySetForceToLocalhostU8(GSD, NewValue); + else if (strcmp("ASPMaxTimeDiff", ParameterName) == 0) + DataDictionarySetASPMaxTimeDiffDbl(GSD, NewValue); + else if (strcmp("ASPMaxTrajDiff", ParameterName) == 0) + DataDictionarySetASPMaxTrajDiffDbl(GSD, NewValue); + else if (strcmp("ASPStepBackCount", ParameterName) == 0) + DataDictionarySetASPStepBackCountU32(GSD, NewValue); + else if (strcmp("ASPFilterLevel", ParameterName) == 0) + DataDictionarySetASPFilterLevelDbl(GSD, NewValue); + else if (strcmp("ASPMaxDeltaTime", ParameterName) == 0) + DataDictionarySetASPMaxDeltaTimeDbl(GSD, NewValue); + else if (strcmp("TimeServerIP", ParameterName) == 0) + DataDictionarySetTimeServerIPU32(GSD, NewValue); + else if (strcmp("TimeServerPort", ParameterName) == 0) + DataDictionarySetTimeServerPortU16(GSD, NewValue); + else if (strcmp("SimulatorIP", ParameterName) == 0) + DataDictionarySetSimulatorIPU32(GSD, NewValue); + else if (strcmp("SimulatorTCPPort", ParameterName) == 0) + DataDictionarySetSimulatorTCPPortU16(GSD, NewValue); + else if (strcmp("SimulatorUDPPort", ParameterName) == 0) + DataDictionarySetSimulatorUDPPortU16(GSD, NewValue); + else if (strcmp("SimulatorMode", ParameterName) == 0) + DataDictionarySetSimulatorModeU8(GSD, NewValue); + else if (strcmp("VOILReceivers", ParameterName) == 0) + DataDictionarySetVOILReceiversC8(GSD, NewValue); + else if (strcmp("DTMReceivers", ParameterName) == 0) + DataDictionarySetDTMReceiversC8(GSD, NewValue); + else if (strcmp("SupervisorIP", ParameterName) == 0) + DataDictionarySetExternalSupervisorIPU32(GSD, NewValue); + else if (strcmp("SupervisorTCPPort", ParameterName) == 0) + DataDictionarySetSupervisorTCPPortU16(GSD, NewValue); + else if (strcmp("MiscData", ParameterName) == 0) + DataDictionarySetMiscDataC8(GSD, NewValue); + else if (strcmp("RVSSConfig", ParameterName) == 0) + DataDictionarySetRVSSConfigU32(GSD, (U32) atoi(NewValue)); + else if (strcmp("RVSSRate", ParameterName) == 0) + DataDictionarySetRVSSRateU8(GSD, (U32) atoi(NewValue)); } I32 SystemControlWriteServerParameter(C8 * ParameterName, C8 * NewValue, U8 Debug) { - I32 RowCount, i; - C8 Parameter[SMALL_BUFFER_SIZE_64]; - C8 Row[SMALL_BUFFER_SIZE_128]; - C8 NewRow[SMALL_BUFFER_SIZE_128]; - FILE *fd, *TempFd; - C8 *ptr1, *ptr2; - U8 ParameterFound = 0; - char confPathFileDir[MAX_FILE_PATH]; - char tempConfPathFileDir[MAX_FILE_PATH]; - const char TEMP_FILE_NAME[] = "temp-" MODULE_NAME ".conf"; - - UtilGetConfDirectoryPath(confPathFileDir, sizeof (confPathFileDir)); - strcpy(tempConfPathFileDir, confPathFileDir); - strcat(confPathFileDir, CONF_FILE_NAME); - strcat(tempConfPathFileDir, TEMP_FILE_NAME); - - bzero(Parameter, SMALL_BUFFER_SIZE_64); - - strcat(Parameter, ParameterName); - strcat(Parameter, "="); - - //Remove temporary file - remove(tempConfPathFileDir); - - //Create temporary file - TempFd = fopen(tempConfPathFileDir, "w+"); - - //Open configuration file - fd = fopen(confPathFileDir, "r"); - - if (fd > 0) { - RowCount = UtilCountFileRows(fd); - fclose(fd); - fd = fopen(confPathFileDir, "r"); - - for (i = 0; i < RowCount; i++) { - bzero(Row, SMALL_BUFFER_SIZE_128); - UtilReadLine(fd, Row); - - ptr1 = strstr(Row, Parameter); - ptr2 = strstr(Row, "//"); - if (ptr2 == NULL) - ptr2 = ptr1; //No comment found - if (ptr1 != NULL && (U64) ptr2 >= (U64) ptr1 && ParameterFound == 0) { - ParameterFound = 1; - bzero(NewRow, SMALL_BUFFER_SIZE_128); - strncpy(NewRow, Row, (U64) ptr1 - (U64) Row + strlen(Parameter)); - strcat(NewRow, NewValue); - if ((U64) ptr2 > (U64) ptr1) { - strcat(NewRow, " "); // Add space - strcat(NewRow, ptr2); // Add the comment - } - - if (Debug) { - LogMessage(LOG_LEVEL_DEBUG, "Changed parameter: %s", NewRow); - } - - strcat(NewRow, "\n"); - (void)fwrite(NewRow, 1, strlen(NewRow), TempFd); - - } - else { - strcat(Row, "\n"); - (void)fwrite(Row, 1, strlen(Row), TempFd); - } - } - fclose(TempFd); - fclose(fd); - - //Remove test.conf - remove(confPathFileDir); - - //Rename temp.conf to test.conf - rename(tempConfPathFileDir, confPathFileDir); - - //Remove temporary file - remove(tempConfPathFileDir); - } - - return 0; + I32 RowCount, i; + C8 Parameter[SMALL_BUFFER_SIZE_64]; + C8 Row[SMALL_BUFFER_SIZE_128]; + C8 NewRow[SMALL_BUFFER_SIZE_128]; + FILE *fd, *TempFd; + C8 *ptr1, *ptr2; + U8 ParameterFound = 0; + char confPathFileDir[MAX_FILE_PATH]; + char tempConfPathFileDir[MAX_FILE_PATH]; + const char TEMP_FILE_NAME[] = "temp-" MODULE_NAME ".conf"; + + UtilGetConfDirectoryPath(confPathFileDir, sizeof (confPathFileDir)); + strcpy(tempConfPathFileDir, confPathFileDir); + strcat(confPathFileDir, CONF_FILE_NAME); + strcat(tempConfPathFileDir, TEMP_FILE_NAME); + + bzero(Parameter, SMALL_BUFFER_SIZE_64); + + strcat(Parameter, ParameterName); + strcat(Parameter, "="); + + //Remove temporary file + remove(tempConfPathFileDir); + + //Create temporary file + TempFd = fopen(tempConfPathFileDir, "w+"); + + //Open configuration file + fd = fopen(confPathFileDir, "r"); + + if (fd > 0) { + RowCount = UtilCountFileRows(fd); + fclose(fd); + fd = fopen(confPathFileDir, "r"); + + for (i = 0; i < RowCount; i++) { + bzero(Row, SMALL_BUFFER_SIZE_128); + UtilReadLine(fd, Row); + + ptr1 = strstr(Row, Parameter); + ptr2 = strstr(Row, "//"); + if (ptr2 == NULL) + ptr2 = ptr1; //No comment found + if (ptr1 != NULL && (U64) ptr2 >= (U64) ptr1 && ParameterFound == 0) { + ParameterFound = 1; + bzero(NewRow, SMALL_BUFFER_SIZE_128); + strncpy(NewRow, Row, (U64) ptr1 - (U64) Row + strlen(Parameter)); + strcat(NewRow, NewValue); + if ((U64) ptr2 > (U64) ptr1) { + strcat(NewRow, " "); // Add space + strcat(NewRow, ptr2); // Add the comment + } + + if (Debug) { + LogMessage(LOG_LEVEL_DEBUG, "Changed parameter: %s", NewRow); + } + + strcat(NewRow, "\n"); + (void)fwrite(NewRow, 1, strlen(NewRow), TempFd); + + } + else { + strcat(Row, "\n"); + (void)fwrite(Row, 1, strlen(Row), TempFd); + } + } + fclose(TempFd); + fclose(fd); + + //Remove test.conf + remove(confPathFileDir); + + //Rename temp.conf to test.conf + rename(tempConfPathFileDir, confPathFileDir); + + //Remove temporary file + remove(tempConfPathFileDir); + } + + return 0; } I32 SystemControlReadServerParameter(C8 * ParameterName, C8 * ReturnValue, U8 Debug) { - I32 RowCount, i; - C8 TextBuffer[SMALL_BUFFER_SIZE_128]; - char confPathDir[MAX_FILE_PATH]; + I32 RowCount, i; + C8 TextBuffer[SMALL_BUFFER_SIZE_128]; + char confPathDir[MAX_FILE_PATH]; - UtilGetConfDirectoryPath(confPathDir, sizeof (confPathDir)); - strcat(confPathDir, CONF_FILE_NAME); + UtilGetConfDirectoryPath(confPathDir, sizeof (confPathDir)); + strcat(confPathDir, CONF_FILE_NAME); - bzero(TextBuffer, SMALL_BUFFER_SIZE_128); + bzero(TextBuffer, SMALL_BUFFER_SIZE_128); - strcat(TextBuffer, ParameterName); - strcat(TextBuffer, "="); + strcat(TextBuffer, ParameterName); + strcat(TextBuffer, "="); - UtilSearchTextFile(confPathDir, TextBuffer, "", ReturnValue); + UtilSearchTextFile(confPathDir, TextBuffer, "", ReturnValue); - if (Debug) { - LogPrint("%s = %s\n", ParameterName, ReturnValue); - } + if (Debug) { + LogPrint("%s = %s\n", ParameterName, ReturnValue); + } - return strlen(ReturnValue); + return strlen(ReturnValue); } I32 SystemControlReadServerParameterList(C8 * ParameterList, U8 Debug) { - I32 RowCount, i; - C8 TextBuffer[SMALL_BUFFER_SIZE_128]; - FILE *fd; - char confPathDir[MAX_FILE_PATH]; - - UtilGetConfDirectoryPath(confPathDir, sizeof (confPathDir)); - strcat(confPathDir, CONF_FILE_NAME); - - fd = fopen(confPathDir, "r"); - if (fd > 0) { - RowCount = UtilCountFileRows(fd); - fclose(fd); - fd = fopen(confPathDir, "r"); - - for (i = 0; i < RowCount; i++) { - bzero(TextBuffer, SMALL_BUFFER_SIZE_128); - UtilReadLineCntSpecChars(fd, TextBuffer); - if (strlen(TextBuffer) > 0) { - strcat(ParameterList, TextBuffer); - strcat(ParameterList, ";"); - } - } - - fclose(fd); - } - - if (Debug) { - LogMessage(LOG_LEVEL_INFO, "ParameterList = %s\n", ParameterList); - } - - return strlen(ParameterList); + I32 RowCount, i; + C8 TextBuffer[SMALL_BUFFER_SIZE_128]; + FILE *fd; + char confPathDir[MAX_FILE_PATH]; + + UtilGetConfDirectoryPath(confPathDir, sizeof (confPathDir)); + strcat(confPathDir, CONF_FILE_NAME); + + fd = fopen(confPathDir, "r"); + if (fd > 0) { + RowCount = UtilCountFileRows(fd); + fclose(fd); + fd = fopen(confPathDir, "r"); + + for (i = 0; i < RowCount; i++) { + bzero(TextBuffer, SMALL_BUFFER_SIZE_128); + UtilReadLineCntSpecChars(fd, TextBuffer); + if (strlen(TextBuffer) > 0) { + strcat(ParameterList, TextBuffer); + strcat(ParameterList, ";"); + } + } + + fclose(fd); + } + + if (Debug) { + LogMessage(LOG_LEVEL_INFO, "ParameterList = %s\n", ParameterList); + } + + return strlen(ParameterList); } I32 SystemControlBuildFileContentInfo(C8 * Path, C8 * ReturnValue, U8 Debug) { - struct stat st; - C8 CompletePath[MAX_FILE_PATH]; + struct stat st; + C8 CompletePath[MAX_FILE_PATH]; - bzero(CompletePath, MAX_FILE_PATH); - UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); - strcat(CompletePath, Path); + bzero(CompletePath, MAX_FILE_PATH); + UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); + strcat(CompletePath, Path); - stat(CompletePath, &st); - *(ReturnValue + 0) = (U8) (st.st_size >> 24); - *(ReturnValue + 1) = (U8) (st.st_size >> 16); - *(ReturnValue + 2) = (U8) (st.st_size >> 8); - *(ReturnValue + 3) = (U8) st.st_size; + stat(CompletePath, &st); + *(ReturnValue + 0) = (U8) (st.st_size >> 24); + *(ReturnValue + 1) = (U8) (st.st_size >> 16); + *(ReturnValue + 2) = (U8) (st.st_size >> 8); + *(ReturnValue + 3) = (U8) st.st_size; - if (Debug) - LogMessage(LOG_LEVEL_DEBUG, "Filesize %d of %s", (I32) st.st_size, CompletePath); + if (Debug) + LogMessage(LOG_LEVEL_DEBUG, "Filesize %d of %s", (I32) st.st_size, CompletePath); - return 0; + return 0; } I32 SystemControlCheckFileDirectoryExist(C8 * ParameterName, C8 * ReturnValue, U8 Debug) { - DIR *pDir; - FILE *fd; - C8 CompletePath[MAX_FILE_PATH]; + DIR *pDir; + FILE *fd; + C8 CompletePath[MAX_FILE_PATH]; - bzero(CompletePath, MAX_FILE_PATH); - UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); - strcat(CompletePath, ParameterName); + bzero(CompletePath, MAX_FILE_PATH); + UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); + strcat(CompletePath, ParameterName); - *ReturnValue = PATH_INVALID_MISSING; + *ReturnValue = PATH_INVALID_MISSING; - pDir = opendir(CompletePath); - if (pDir == NULL) { - fd = fopen(CompletePath, "r"); - if (fd != NULL) { - *ReturnValue = FILE_EXIST; //File exist - fclose(fd); - } - } - else { - *ReturnValue = FOLDER_EXIST; //Directory exist - closedir(pDir); - } + pDir = opendir(CompletePath); + if (pDir == NULL) { + fd = fopen(CompletePath, "r"); + if (fd != NULL) { + *ReturnValue = FILE_EXIST; //File exist + fclose(fd); + } + } + else { + *ReturnValue = FOLDER_EXIST; //Directory exist + closedir(pDir); + } - if (Debug) - LogPrint("%d %s", *ReturnValue, CompletePath); + if (Debug) + LogPrint("%d %s", *ReturnValue, CompletePath); - return 0; + return 0; } I32 SystemControlDeleteFileDirectory(C8 * Path, C8 * ReturnValue, U8 Debug) { - DIR *pDir; - FILE *fd; - C8 CompletePath[MAX_FILE_PATH]; - - bzero(CompletePath, MAX_FILE_PATH); - UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); - strcat(CompletePath, Path); - - *ReturnValue = PATH_INVALID_MISSING; - - pDir = opendir(CompletePath); - if (pDir == NULL) { - fd = fopen(CompletePath, "r"); - if (fd == NULL) { - *ReturnValue = PATH_INVALID_MISSING; //Missing file - } - else { - if (0 == remove(CompletePath)) //Delete file - { - *ReturnValue = SUCCEDED_DELETE; - } - else { - *ReturnValue = FAILED_DELETE; - } - } - } - else { - if (0 == remove(CompletePath)) //Delete directory - { - *ReturnValue = SUCCEDED_DELETE; - } - else { - *ReturnValue = FAILED_DELETE; - } - } - - if (*ReturnValue == SUCCEDED_DELETE) - LogMessage(LOG_LEVEL_INFO, "Deleted %s", CompletePath); - else if (*ReturnValue == FAILED_DELETE) - LogMessage(LOG_LEVEL_INFO, "Failed to delete %s", CompletePath); - - return 0; + DIR *pDir; + FILE *fd; + C8 CompletePath[MAX_FILE_PATH]; + + bzero(CompletePath, MAX_FILE_PATH); + UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); + strcat(CompletePath, Path); + + *ReturnValue = PATH_INVALID_MISSING; + + pDir = opendir(CompletePath); + if (pDir == NULL) { + fd = fopen(CompletePath, "r"); + if (fd == NULL) { + *ReturnValue = PATH_INVALID_MISSING; //Missing file + } + else { + if (0 == remove(CompletePath)) //Delete file + { + *ReturnValue = SUCCEDED_DELETE; + } + else { + *ReturnValue = FAILED_DELETE; + } + } + } + else { + if (0 == remove(CompletePath)) //Delete directory + { + *ReturnValue = SUCCEDED_DELETE; + } + else { + *ReturnValue = FAILED_DELETE; + } + } + + if (*ReturnValue == SUCCEDED_DELETE) + LogMessage(LOG_LEVEL_INFO, "Deleted %s", CompletePath); + else if (*ReturnValue == FAILED_DELETE) + LogMessage(LOG_LEVEL_INFO, "Failed to delete %s", CompletePath); + + return 0; } I32 SystemControlCreateDirectory(C8 * Path, C8 * ReturnValue, U8 Debug) { - DIR *pDir; - FILE *fd; - C8 CompletePath[MAX_FILE_PATH]; - - bzero(CompletePath, MAX_FILE_PATH); - UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); - strcat(CompletePath, Path); - - *ReturnValue = PATH_INVALID_MISSING; - - pDir = opendir(CompletePath); - if (pDir == NULL) { - fd = fopen(CompletePath, "r"); - if (fd != NULL) { - *ReturnValue = FILE_EXIST; //This is a file! - fclose(fd); - } - else { - if (0 == mkdir(CompletePath, S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH)) //Make the new directory - { - *ReturnValue = SUCCEDED_CREATE_FOLDER; - } - else { - *ReturnValue = FAILED_CREATE_FOLDER; - } - } - } - else { - *ReturnValue = FOLDER_EXIST; //Directory exist - closedir(pDir); - } - - if (Debug) - LogPrint("%d %s", *(ReturnValue), CompletePath); - - if (*ReturnValue == SUCCEDED_CREATE_FOLDER) - LogMessage(LOG_LEVEL_INFO, "Directory created: %s", CompletePath); - - return 0; + DIR *pDir; + FILE *fd; + C8 CompletePath[MAX_FILE_PATH]; + + bzero(CompletePath, MAX_FILE_PATH); + UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); + strcat(CompletePath, Path); + + *ReturnValue = PATH_INVALID_MISSING; + + pDir = opendir(CompletePath); + if (pDir == NULL) { + fd = fopen(CompletePath, "r"); + if (fd != NULL) { + *ReturnValue = FILE_EXIST; //This is a file! + fclose(fd); + } + else { + if (0 == mkdir(CompletePath, S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH)) //Make the new directory + { + *ReturnValue = SUCCEDED_CREATE_FOLDER; + } + else { + *ReturnValue = FAILED_CREATE_FOLDER; + } + } + } + else { + *ReturnValue = FOLDER_EXIST; //Directory exist + closedir(pDir); + } + + if (Debug) + LogPrint("%d %s", *(ReturnValue), CompletePath); + + if (*ReturnValue == SUCCEDED_CREATE_FOLDER) + LogMessage(LOG_LEVEL_INFO, "Directory created: %s", CompletePath); + + return 0; } @@ -2007,212 +2007,212 @@ I32 SystemControlCreateDirectory(C8 * Path, C8 * ReturnValue, U8 Debug) { I32 SystemControlUploadFile(C8 * Path, C8 * FileSize, C8 * PacketSize, C8 * ReturnValue, U8 Debug) { - FILE *fd; - C8 CompletePath[MAX_FILE_PATH]; - - bzero(CompletePath, MAX_FILE_PATH); - UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); - strcat(CompletePath, Path); - - if (Debug) { - LogMessage(LOG_LEVEL_DEBUG, "Upload file:"); - LogMessage(LOG_LEVEL_DEBUG, "%s", Path); - LogMessage(LOG_LEVEL_DEBUG, "%s", FileSize); - LogMessage(LOG_LEVEL_DEBUG, "%s", PacketSize); - LogMessage(LOG_LEVEL_DEBUG, "%s", CompletePath); - } - - if (atoi(PacketSize) > SYSTEM_CONTROL_RX_PACKET_SIZE) //Check packet size - { - *ReturnValue = SERVER_PREPARED_BIG_PACKET_SIZE; - return 0; - } - - fd = fopen(CompletePath, "r"); - if (fd != NULL) { - fclose(fd); - remove(CompletePath); //Remove file if exist - } - - fd = fopen(CompletePath, "w+"); //Create the file - if (fd != NULL) { - *ReturnValue = SERVER_PREPARED; //Server prepared - fclose(fd); - return 0; - } - else { - //ok, path invalid create temporary file - bzero(CompletePath, MAX_FILE_PATH); - UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); - strcat(CompletePath, "file.tmp"); - fd = fopen(CompletePath, "r"); - if (fd != NULL) { - fclose(fd); - remove(CompletePath); //Remove file if exist - } - fd = fopen(CompletePath, "w+"); //Create the temporary file - - *ReturnValue = PATH_INVALID_MISSING; - - return 0; - } - - return 0; -} + FILE *fd; + C8 CompletePath[MAX_FILE_PATH]; -I32 SystemControlReceiveRxData(I32 * sockfd, C8 * Path, C8 * FileSize, C8 * PacketSize, C8 * ReturnValue, - U8 Debug) { + bzero(CompletePath, MAX_FILE_PATH); + UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); + strcat(CompletePath, Path); - FILE *fd; - C8 CompletePath[MAX_FILE_PATH]; + if (Debug) { + LogMessage(LOG_LEVEL_DEBUG, "Upload file:"); + LogMessage(LOG_LEVEL_DEBUG, "%s", Path); + LogMessage(LOG_LEVEL_DEBUG, "%s", FileSize); + LogMessage(LOG_LEVEL_DEBUG, "%s", PacketSize); + LogMessage(LOG_LEVEL_DEBUG, "%s", CompletePath); + } - bzero(CompletePath, MAX_FILE_PATH); - UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); - strcat(CompletePath, Path); - U32 FileSizeU32 = atoi(FileSize); - U16 PacketSizeU16 = atoi(PacketSize); - I32 ClientStatus = 0, Time1 = 0, Time2 = 0, TimeDiff = 0, i = 0, j = 0; - C8 RxBuffer[SYSTEM_CONTROL_RX_PACKET_SIZE]; - U32 TotalRxCount = 0, TransmissionCount = (U32) (FileSizeU32 / PacketSizeU16), RestCount = - FileSizeU32 % PacketSizeU16; - struct timeval CurTime; + if (atoi(PacketSize) > SYSTEM_CONTROL_RX_PACKET_SIZE) //Check packet size + { + *ReturnValue = SERVER_PREPARED_BIG_PACKET_SIZE; + return 0; + } + fd = fopen(CompletePath, "r"); + if (fd != NULL) { + fclose(fd); + remove(CompletePath); //Remove file if exist + } - if (Debug) { - LogMessage(LOG_LEVEL_DEBUG, "Receive Rx data:"); - LogMessage(LOG_LEVEL_DEBUG, "%s", Path); - LogMessage(LOG_LEVEL_DEBUG, "%s", FileSize); - LogMessage(LOG_LEVEL_DEBUG, "%s", PacketSize); - LogMessage(LOG_LEVEL_DEBUG, "%s", CompletePath); - } + fd = fopen(CompletePath, "w+"); //Create the file + if (fd != NULL) { + *ReturnValue = SERVER_PREPARED; //Server prepared + fclose(fd); + return 0; + } + else { + //ok, path invalid create temporary file + bzero(CompletePath, MAX_FILE_PATH); + UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); + strcat(CompletePath, "file.tmp"); + fd = fopen(CompletePath, "r"); + if (fd != NULL) { + fclose(fd); + remove(CompletePath); //Remove file if exist + } + fd = fopen(CompletePath, "w+"); //Create the temporary file + + *ReturnValue = PATH_INVALID_MISSING; + + return 0; + } + + return 0; +} +I32 SystemControlReceiveRxData(I32 * sockfd, C8 * Path, C8 * FileSize, C8 * PacketSize, C8 * ReturnValue, + U8 Debug) { + + FILE *fd; + C8 CompletePath[MAX_FILE_PATH]; + + bzero(CompletePath, MAX_FILE_PATH); + UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); + strcat(CompletePath, Path); + U32 FileSizeU32 = atoi(FileSize); + U16 PacketSizeU16 = atoi(PacketSize); + I32 ClientStatus = 0, Time1 = 0, Time2 = 0, TimeDiff = 0, i = 0, j = 0; + C8 RxBuffer[SYSTEM_CONTROL_RX_PACKET_SIZE]; + U32 TotalRxCount = 0, TransmissionCount = (U32) (FileSizeU32 / PacketSizeU16), RestCount = + FileSizeU32 % PacketSizeU16; + struct timeval CurTime; + + + if (Debug) { + LogMessage(LOG_LEVEL_DEBUG, "Receive Rx data:"); + LogMessage(LOG_LEVEL_DEBUG, "%s", Path); + LogMessage(LOG_LEVEL_DEBUG, "%s", FileSize); + LogMessage(LOG_LEVEL_DEBUG, "%s", PacketSize); + LogMessage(LOG_LEVEL_DEBUG, "%s", CompletePath); + } - fd = fopen(CompletePath, "w+"); - if (fd != NULL) { - gettimeofday(&CurTime, NULL); - Time1 = CurTime.tv_sec * 1000 + CurTime.tv_usec / 1000; + fd = fopen(CompletePath, "w+"); + if (fd != NULL) { - while (TotalRxCount < FileSizeU32 && TimeDiff < 3000) { - gettimeofday(&CurTime, NULL); - Time2 = CurTime.tv_sec * 1000 + CurTime.tv_usec / 1000; + gettimeofday(&CurTime, NULL); + Time1 = CurTime.tv_sec * 1000 + CurTime.tv_usec / 1000; + while (TotalRxCount < FileSizeU32 && TimeDiff < 3000) { + gettimeofday(&CurTime, NULL); + Time2 = CurTime.tv_sec * 1000 + CurTime.tv_usec / 1000; - if (i == TransmissionCount) { - PacketSizeU16 = RestCount; - } - bzero(RxBuffer, PacketSizeU16); - ClientStatus = recv(*sockfd, RxBuffer, PacketSizeU16, MSG_WAITALL); + if (i == TransmissionCount) { + PacketSizeU16 = RestCount; + } + bzero(RxBuffer, PacketSizeU16); + ClientStatus = recv(*sockfd, RxBuffer, PacketSizeU16, MSG_WAITALL); - if (ClientStatus > 0) { - i++; - fwrite(RxBuffer, 1, ClientStatus, fd); - fflush(fd); - if (Debug) { - printf("%d, %d, %d, %d, %d, %d :", i, ClientStatus, TotalRxCount, TimeDiff, PacketSizeU16, - FileSizeU32); - for (j = 0; j < 10; j++) - printf("%x-", RxBuffer[j]); - printf("...\n"); - } - TotalRxCount = TotalRxCount + ClientStatus; - gettimeofday(&CurTime, NULL); - Time1 = CurTime.tv_sec * 1000 + CurTime.tv_usec / 1000; - } + if (ClientStatus > 0) { + i++; + fwrite(RxBuffer, 1, ClientStatus, fd); + fflush(fd); + if (Debug) { + printf("%d, %d, %d, %d, %d, %d :", i, ClientStatus, TotalRxCount, TimeDiff, PacketSizeU16, + FileSizeU32); + for (j = 0; j < 10; j++) + printf("%x-", RxBuffer[j]); + printf("...\n"); + } + TotalRxCount = TotalRxCount + ClientStatus; + gettimeofday(&CurTime, NULL); + Time1 = CurTime.tv_sec * 1000 + CurTime.tv_usec / 1000; + } - TimeDiff = abs(Time1 - Time2); - } - fclose(fd); + TimeDiff = abs(Time1 - Time2); + } - if (TotalRxCount == FileSizeU32) { - *ReturnValue = FILE_UPLOADED; - } - else if (TotalRxCount > FileSizeU32) { - *ReturnValue = FILE_TO_MUCH_DATA; - remove(CompletePath); - LogMessage(LOG_LEVEL_INFO, "CORRUPT FILE, REMOVING..."); - } - else { - *ReturnValue = TIME_OUT; - remove(CompletePath); - LogMessage(LOG_LEVEL_INFO, "CORRUPT FILE, REMOVING..."); - } + fclose(fd); - LogMessage(LOG_LEVEL_DEBUG, "Rec count = %d, Req count = %d", TotalRxCount, FileSizeU32); + if (TotalRxCount == FileSizeU32) { + *ReturnValue = FILE_UPLOADED; + } + else if (TotalRxCount > FileSizeU32) { + *ReturnValue = FILE_TO_MUCH_DATA; + remove(CompletePath); + LogMessage(LOG_LEVEL_INFO, "CORRUPT FILE, REMOVING..."); + } + else { + *ReturnValue = TIME_OUT; + remove(CompletePath); + LogMessage(LOG_LEVEL_INFO, "CORRUPT FILE, REMOVING..."); + } - } + LogMessage(LOG_LEVEL_DEBUG, "Rec count = %d, Req count = %d", TotalRxCount, FileSizeU32); + + } - return 0; + return 0; } I32 SystemControlSendFileContent(I32 * sockfd, C8 * Path, C8 * PacketSize, C8 * ReturnValue, U8 Remove, - U8 Debug) { - FILE *fd; - C8 CompletePath[MAX_FILE_PATH]; - - bzero(CompletePath, MAX_FILE_PATH); - UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); - strcat(CompletePath, Path); - U32 FileSizeU32 = 0; - U16 PacketSizeU16 = atoi(PacketSize); - I32 ClientStatus = 0, Time1 = 0, Time2 = 0, TimeDiff = 0, i = 0, j = 0; - C8 TxBuffer[SYSTEM_CONTROL_TX_PACKET_SIZE]; - U32 TotalRxCount = 0, TransmissionCount = 0, RestCount = 0; - struct timeval CurTime; - struct stat st; - - stat(CompletePath, &st); - TransmissionCount = (U32) (st.st_size) / PacketSizeU16; - RestCount = (U32) (st.st_size) % PacketSizeU16; - - if (Debug) { - LogPrint("Send file content:"); - LogPrint("%s", Path); - LogPrint("%s", PacketSize); - LogPrint("%s", CompletePath); - } - - fd = fopen(CompletePath, "r"); - - if (fd != NULL) { - - for (i = 0; i < TransmissionCount; i++) { - bzero(TxBuffer, PacketSizeU16); - fread(TxBuffer, 1, PacketSizeU16, fd); - //SystemControlSendBytes(TxBuffer, PacketSizeU16, sockfd, 0); //Send a packet - UtilSendTCPData("System Control", TxBuffer, PacketSizeU16, sockfd, 0); - } - - if (RestCount > 0) { - bzero(TxBuffer, PacketSizeU16); - fread(TxBuffer, 1, RestCount, fd); - //SystemControlSendBytes(TxBuffer, RestCount, sockfd, 0); //Send the rest - UtilSendTCPData("System Control", TxBuffer, RestCount, sockfd, 0); - } - - fclose(fd); - - if (Remove) - remove(CompletePath); - - LogMessage(LOG_LEVEL_INFO, "Sent file: %s, total size = %d, transmissions = %d", CompletePath, - (PacketSizeU16 * TransmissionCount + RestCount), i + 1); - - } - - return 0; + U8 Debug) { + FILE *fd; + C8 CompletePath[MAX_FILE_PATH]; + + bzero(CompletePath, MAX_FILE_PATH); + UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); + strcat(CompletePath, Path); + U32 FileSizeU32 = 0; + U16 PacketSizeU16 = atoi(PacketSize); + I32 ClientStatus = 0, Time1 = 0, Time2 = 0, TimeDiff = 0, i = 0, j = 0; + C8 TxBuffer[SYSTEM_CONTROL_TX_PACKET_SIZE]; + U32 TotalRxCount = 0, TransmissionCount = 0, RestCount = 0; + struct timeval CurTime; + struct stat st; + + stat(CompletePath, &st); + TransmissionCount = (U32) (st.st_size) / PacketSizeU16; + RestCount = (U32) (st.st_size) % PacketSizeU16; + + if (Debug) { + LogPrint("Send file content:"); + LogPrint("%s", Path); + LogPrint("%s", PacketSize); + LogPrint("%s", CompletePath); + } + + fd = fopen(CompletePath, "r"); + + if (fd != NULL) { + + for (i = 0; i < TransmissionCount; i++) { + bzero(TxBuffer, PacketSizeU16); + fread(TxBuffer, 1, PacketSizeU16, fd); + //SystemControlSendBytes(TxBuffer, PacketSizeU16, sockfd, 0); //Send a packet + UtilSendTCPData("System Control", TxBuffer, PacketSizeU16, sockfd, 0); + } + + if (RestCount > 0) { + bzero(TxBuffer, PacketSizeU16); + fread(TxBuffer, 1, RestCount, fd); + //SystemControlSendBytes(TxBuffer, RestCount, sockfd, 0); //Send the rest + UtilSendTCPData("System Control", TxBuffer, RestCount, sockfd, 0); + } + + fclose(fd); + + if (Remove) + remove(CompletePath); + + LogMessage(LOG_LEVEL_INFO, "Sent file: %s, total size = %d, transmissions = %d", CompletePath, + (PacketSizeU16 * TransmissionCount + RestCount), i + 1); + + } + + return 0; } /* SystemControlBuildRVSSTimeChannelMessage builds a message from data in *GPSTime. The message is stored in *RVSSData. -See the architecture document for the protocol of RVSS. +See the architecture document for the protocol of RVSS. - *RVSSData the buffer the message - *RVSSDataLengthU32 the length of the message @@ -2220,47 +2220,47 @@ See the architecture document for the protocol of RVSS. - Debug enable(1)/disable(0) debug printouts (Not used) */ I32 SystemControlBuildRVSSTimeChannelMessage(C8 * RVSSData, U32 * RVSSDataLengthU32, TimeType * GPSTime, - U8 Debug) { - I32 MessageIndex = 0, i; - C8 *p; - - RVSSTimeType RVSSTimeData; - - RVSSTimeData.MessageLengthU32 = SwapU32((U32) sizeof (RVSSTimeType) - 4); - RVSSTimeData.ChannelCodeU32 = SwapU32((U32) RVSS_TIME_CHANNEL); - RVSSTimeData.YearU16 = SwapU16(GPSTime->YearU16); - RVSSTimeData.MonthU8 = GPSTime->MonthU8; - RVSSTimeData.DayU8 = GPSTime->DayU8; - RVSSTimeData.HourU8 = GPSTime->HourU8; - RVSSTimeData.MinuteU8 = GPSTime->MinuteU8; - RVSSTimeData.SecondU8 = GPSTime->SecondU8; - RVSSTimeData.MillisecondU16 = SwapU16(TimeControlGetMillisecond(GPSTime)); - RVSSTimeData.SecondCounterU32 = SwapU32(GPSTime->SecondCounterU32); - RVSSTimeData.GPSMillisecondsU64 = - SwapU64(GPSTime->GPSMillisecondsU64 + (U64) TimeControlGetMillisecond(GPSTime)); - RVSSTimeData.GPSMinutesU32 = SwapU32(GPSTime->GPSMinutesU32); - RVSSTimeData.GPSWeekU16 = SwapU16(GPSTime->GPSWeekU16); - RVSSTimeData.GPSSecondsOfWeekU32 = SwapU32(GPSTime->GPSSecondsOfWeekU32); - RVSSTimeData.GPSSecondsOfDayU32 = SwapU32(GPSTime->GPSSecondsOfDayU32); - RVSSTimeData.FixQualityU8 = GPSTime->FixQualityU8; - RVSSTimeData.NSatellitesU8 = GPSTime->NSatellitesU8; - - p = (C8 *) & RVSSTimeData; - for (i = 0; i < sizeof (RVSSTimeType); i++) - *(RVSSData + i) = *p++; - *RVSSDataLengthU32 = i; - - if (Debug) { - - } - - return 0; + U8 Debug) { + I32 MessageIndex = 0, i; + C8 *p; + + RVSSTimeType RVSSTimeData; + + RVSSTimeData.MessageLengthU32 = SwapU32((U32) sizeof (RVSSTimeType) - 4); + RVSSTimeData.ChannelCodeU32 = SwapU32((U32) RVSS_TIME_CHANNEL); + RVSSTimeData.YearU16 = SwapU16(GPSTime->YearU16); + RVSSTimeData.MonthU8 = GPSTime->MonthU8; + RVSSTimeData.DayU8 = GPSTime->DayU8; + RVSSTimeData.HourU8 = GPSTime->HourU8; + RVSSTimeData.MinuteU8 = GPSTime->MinuteU8; + RVSSTimeData.SecondU8 = GPSTime->SecondU8; + RVSSTimeData.MillisecondU16 = SwapU16(TimeControlGetMillisecond(GPSTime)); + RVSSTimeData.SecondCounterU32 = SwapU32(GPSTime->SecondCounterU32); + RVSSTimeData.GPSMillisecondsU64 = + SwapU64(GPSTime->GPSMillisecondsU64 + (U64) TimeControlGetMillisecond(GPSTime)); + RVSSTimeData.GPSMinutesU32 = SwapU32(GPSTime->GPSMinutesU32); + RVSSTimeData.GPSWeekU16 = SwapU16(GPSTime->GPSWeekU16); + RVSSTimeData.GPSSecondsOfWeekU32 = SwapU32(GPSTime->GPSSecondsOfWeekU32); + RVSSTimeData.GPSSecondsOfDayU32 = SwapU32(GPSTime->GPSSecondsOfDayU32); + RVSSTimeData.FixQualityU8 = GPSTime->FixQualityU8; + RVSSTimeData.NSatellitesU8 = GPSTime->NSatellitesU8; + + p = (C8 *) & RVSSTimeData; + for (i = 0; i < sizeof (RVSSTimeType); i++) + *(RVSSData + i) = *p++; + *RVSSDataLengthU32 = i; + + if (Debug) { + + } + + return 0; } /* SystemControlBuildRVSSMaestroChannelMessage builds a message from OBCState in *GSD and SysCtrlState. The message is stored in *RVSSData. -See the architecture document for the protocol of RVSS. +See the architecture document for the protocol of RVSS. - *RVSSData the buffer the message - *RVSSDataLengthU32 the length of the message @@ -2269,28 +2269,28 @@ See the architecture document for the protocol of RVSS. - Debug enable(1)/disable(0) debug printouts (Not used) */ I32 SystemControlBuildRVSSMaestroChannelMessage(C8 * RVSSData, U32 * RVSSDataLengthU32, GSDType * GSD, - U8 SysCtrlState, U8 Debug) { - I32 MessageIndex = 0, i; - C8 *p; + U8 SysCtrlState, U8 Debug) { + I32 MessageIndex = 0, i; + C8 *p; - RVSSMaestroType RVSSMaestroData; + RVSSMaestroType RVSSMaestroData; - RVSSMaestroData.MessageLengthU32 = SwapU32((U32) sizeof (RVSSMaestroType) - 4); - RVSSMaestroData.ChannelCodeU32 = SwapU32((U32) RVSS_MAESTRO_CHANNEL); - RVSSMaestroData.OBCStateU8 = DataDictionaryGetOBCStateU8(GSD); - RVSSMaestroData.SysCtrlStateU8 = SysCtrlState; + RVSSMaestroData.MessageLengthU32 = SwapU32((U32) sizeof (RVSSMaestroType) - 4); + RVSSMaestroData.ChannelCodeU32 = SwapU32((U32) RVSS_MAESTRO_CHANNEL); + RVSSMaestroData.OBCStateU8 = DataDictionaryGetOBCStateU8(GSD); + RVSSMaestroData.SysCtrlStateU8 = SysCtrlState; - p = (C8 *) & RVSSMaestroData; - for (i = 0; i < sizeof (RVSSMaestroType); i++) - *(RVSSData + i) = *p++; - *RVSSDataLengthU32 = i; + p = (C8 *) & RVSSMaestroData; + for (i = 0; i < sizeof (RVSSMaestroType); i++) + *(RVSSData + i) = *p++; + *RVSSDataLengthU32 = i; - if (Debug) { + if (Debug) { - } + } - return 0; + return 0; } @@ -2298,7 +2298,7 @@ I32 SystemControlBuildRVSSMaestroChannelMessage(C8 * RVSSData, U32 * RVSSDataLen #define MAX_MONR_STRING_LENGTH 116 /* SystemControlBuildRVSSMONRChannelMessage builds a message from data in *MonrData. The message is stored in *RVSSData. -See the architecture document for the protocol of RVSS. +See the architecture document for the protocol of RVSS. - *RVSSData the buffer the message - *RVSSDataLengthU32 the length of the message @@ -2307,35 +2307,35 @@ See the architecture document for the protocol of RVSS. */ I32 SystemControlBuildRVSSMONRChannelMessage(C8 * RVSSData, U32 * RVSSDataLengthU32, MonitorDataType MonrData, - U8 Debug) { - I32 MessageLength = 0; - char MonrDataString[MAX_MONR_STRING_LENGTH]; + U8 Debug) { + I32 MessageLength = 0; + char MonrDataString[MAX_MONR_STRING_LENGTH]; - // TODO: Convert MonrData to string - UtilMonitorDataToString(MonrData, MonrDataString, sizeof (MonrDataString)); + // TODO: Convert MonrData to string + UtilMonitorDataToString(MonrData, MonrDataString, sizeof (MonrDataString)); - MessageLength = strlen(MonrDataString) + 8; - bzero(RVSSData, MessageLength); - *RVSSDataLengthU32 = MessageLength; + MessageLength = strlen(MonrDataString) + 8; + bzero(RVSSData, MessageLength); + *RVSSDataLengthU32 = MessageLength; - *(RVSSData + 0) = (C8) (MessageLength >> 24); - *(RVSSData + 1) = (C8) (MessageLength >> 16); - *(RVSSData + 2) = (C8) (MessageLength >> 8); - *(RVSSData + 3) = (C8) (MessageLength); - *(RVSSData + 7) = (C8) (RVSS_MONR_CHANNEL); - strcat(RVSSData + 8, MonrDataString); + *(RVSSData + 0) = (C8) (MessageLength >> 24); + *(RVSSData + 1) = (C8) (MessageLength >> 16); + *(RVSSData + 2) = (C8) (MessageLength >> 8); + *(RVSSData + 3) = (C8) (MessageLength); + *(RVSSData + 7) = (C8) (RVSS_MONR_CHANNEL); + strcat(RVSSData + 8, MonrDataString); - if (Debug) { + if (Debug) { - } + } - return 0; + return 0; } /* SystemControlBuildRVSSAspChannelMessage shall be used for sending ASP-debug data. The message is stored in *RVSSData. -See the architecture document for the protocol of RVSS. +See the architecture document for the protocol of RVSS. - *RVSSData the buffer the message - *RVSSDataLengthU32 the length of the message @@ -2344,10 +2344,10 @@ See the architecture document for the protocol of RVSS. */ I32 SystemControlBuildRVSSAspChannelMessage(C8 * RVSSData, U32 * RVSSDataLengthU32, U8 Debug) { - RVSSTimeType RVSSTimeData; + RVSSTimeType RVSSTimeData; - return 0; + return 0; } diff --git a/server/src/timecontrol.c b/server/src/timecontrol.c index ebdb0abe5..54dd5d516 100644 --- a/server/src/timecontrol.c +++ b/server/src/timecontrol.c @@ -57,9 +57,9 @@ ------------------------------------------------------------*/ static int TimeControlCreateTimeChannel(const char *name, const uint32_t port, int *sockfd, - struct sockaddr_in *addr); + struct sockaddr_in *addr); static int TimeControlSendUDPData(int *sockfd, struct sockaddr_in *addr, C8 * SendData, int Length, - char debug); + char debug); //static void TimeControlRecvTime(int* sockfd, char* buffer, int length, int* recievedNewData); static void TimeControlRecvTime(int *sockfd, C8 * buffer, int length, int *recievedNewData); U32 TimeControlIPStringToInt(C8 * IP); @@ -78,466 +78,466 @@ static volatile int iExit = 0; ------------------------------------------------------------*/ void timecontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { - C8 TextBufferC8[TIME_CONTROL_HOSTNAME_BUFFER_SIZE]; - C8 ServerIPC8[TIME_CONTROL_HOSTNAME_BUFFER_SIZE]; - U16 ServerPortU16; - I32 SocketfdI32 = -1; - struct sockaddr_in time_addr; - - I32 result; - C8 TimeBuffer[TIME_CONTROL_RECEIVE_BUFFER_SIZE]; - C8 LogBuffer[LOG_BUFFER_LENGTH]; - I32 ReceivedNewData, i; - C8 SendData[TIME_INTERVAL_NUMBER_BYTES] = { 0, 0, 3, 0xe8 }; - //C8 SendData[4] = {0, 0, 0, 1}; - struct timespec sleep_time, ref_time; - C8 MqRecvBuffer[MBUS_MAX_DATALEN]; - struct timeval tv, ExecTime; - struct tm *tm; - - U32 IpU32; - U8 PrevSecondU8; - U16 CurrentMilliSecondU16, PrevMilliSecondU16; - U8 CycleCount = 0; - - enum COMMAND command; - char busReceiveBuffer[MBUS_MAX_DATALEN]; - - // Create log - LogInit(MODULE_NAME, logLevel); - LogMessage(LOG_LEVEL_INFO, "Time control task running with PID: %i", getpid()); - - // Set up signal handlers - if (signal(SIGINT, signalHandler) == SIG_ERR) - util_error("Unable to initialize signal handler"); - - // Set up message bus connection - if (iCommInit()) - util_error("Unable to initialize message bus connection"); - - GPSTime->isGPSenabled = 0; - - gettimeofday(&ExecTime, NULL); - CurrentMilliSecondU16 = (U16) (ExecTime.tv_usec / 1000); - PrevMilliSecondU16 = CurrentMilliSecondU16; - // Set time server IP - DataDictionaryGetTimeServerIPC8(GSD, ServerIPC8, TIME_CONTROL_HOSTNAME_BUFFER_SIZE); - DataDictionaryGetTimeServerIPU32(GSD, &IpU32); - - // Set time server port - DataDictionaryGetTimeServerPortU16(GSD, &ServerPortU16); - - // If time server is specified, connect to it - if (IpU32 != 0) { - LogMessage(LOG_LEVEL_INFO, "Connecting to time server..."); - - if (TimeControlCreateTimeChannel(ServerIPC8, ServerPortU16, &SocketfdI32, &time_addr)) { - LogMessage(LOG_LEVEL_INFO, "Using time server reference"); - TimeControlSendUDPData(&SocketfdI32, &time_addr, SendData, TIME_INTERVAL_NUMBER_BYTES, 0); - GPSTime->isGPSenabled = 1; - } - else { - LogMessage(LOG_LEVEL_INFO, "Defaulting to system time"); - - // Send warning over MQ - LOG_SEND(LogBuffer, "Unable to connect to time server"); - } - - } - - if (!GPSTime->isGPSenabled) { - LogMessage(LOG_LEVEL_INFO, "Initializing with system time"); - - gettimeofday(&tv, NULL); - - GPSTime->MicroSecondU16 = 0; - GPSTime->GPSMillisecondsU64 = - tv.tv_sec * 1000 + tv.tv_usec / 1000 - MS_TIME_DIFF_UTC_GPS + MS_LEAP_SEC_DIFF_UTC_GPS; - GPSTime->GPSWeekU16 = (U16) (GPSTime->GPSMillisecondsU64 / WEEK_TIME_MS); - GPSTime->GPSSecondsOfWeekU32 = - (U32) ((GPSTime->GPSMillisecondsU64 - (U64) (GPSTime->GPSWeekU16) * WEEK_TIME_MS) / 1000); - GPSTime->GPSSecondsOfDayU32 = (GPSTime->GPSMillisecondsU64 % DAY_TIME_MS) / 1000; - GPSTime->GPSMinutesU32 = (GPSTime->GPSMillisecondsU64 / 1000) / 60; - GPSTime->isTimeInitializedU8 = 1; - } - - while (!iExit) { - - // Ignore any commands received, just empty the bus - iCommRecv(&command, busReceiveBuffer, sizeof (busReceiveBuffer), NULL); - - gettimeofday(&ExecTime, NULL); - CurrentMilliSecondU16 = (U16) (ExecTime.tv_usec / 1000); - if (CurrentMilliSecondU16 < PrevMilliSecondU16) { - GSD->TimeControlExecTimeU16 = CurrentMilliSecondU16 + (1000 - PrevMilliSecondU16); - //printf("%d\n", GSD->TimeControlExecTimeU16); - } - else { - GSD->TimeControlExecTimeU16 = abs(PrevMilliSecondU16 - CurrentMilliSecondU16); - //printf("%d\n", GSD->TimeControlExecTimeU16); - } - PrevMilliSecondU16 = CurrentMilliSecondU16; - - if (GPSTime->isGPSenabled) { - bzero(TimeBuffer, TIME_CONTROL_RECEIVE_BUFFER_SIZE); - TimeControlRecvTime(&SocketfdI32, TimeBuffer, TIME_CONTROL_RECEIVE_BUFFER_SIZE, &ReceivedNewData); - - if (ReceivedNewData) - TimeControlDecodeTimeBuffer(GPSTime, TimeBuffer, 0); - } - else if (!GPSTime->isGPSenabled) { - gettimeofday(&tv, NULL); - - tm = localtime(&tv.tv_sec); - - // Add 1900 to get the right year value - GPSTime->YearU16 = (U16) tm->tm_year + 1900; - // Months are 0 based in struct tm - GPSTime->MonthU8 = (U8) tm->tm_mon + 1; - GPSTime->DayU8 = (U8) tm->tm_mday; - GPSTime->HourU8 = (U8) tm->tm_hour; - GPSTime->MinuteU8 = (U8) tm->tm_min; - GPSTime->SecondU8 = (U8) tm->tm_sec; - GPSTime->MillisecondU16 = (U16) (tv.tv_usec / 1000); - - GPSTime->LocalMillisecondU16 = (U16) (tv.tv_usec / 1000); - - GPSTime->GPSMillisecondsU64 = GPSTime->GPSMillisecondsU64 + 1000; - - if (GPSTime->SecondU8 != PrevSecondU8) { - PrevSecondU8 = GPSTime->SecondU8; - GPSTime->SecondCounterU32++; - if (GPSTime->GPSSecondsOfDayU32 >= 86400) - GPSTime->GPSSecondsOfDayU32 = 0; - else - GPSTime->GPSSecondsOfDayU32++; - - if (GPSTime->GPSSecondsOfWeekU32 >= 604800) { - GPSTime->GPSSecondsOfWeekU32 = 0; - GPSTime->GPSWeekU16++; - } - else - GPSTime->GPSSecondsOfWeekU32++; - - if (GPSTime->SecondU8 == 0) - GPSTime->GPSMinutesU32++; - } - } - - if (GSD->ExitU8 == 1) { - if (GPSTime->isGPSenabled) { - SendData[0] = 0; - SendData[1] = 0; - SendData[2] = 0; - SendData[3] = 0; - TimeControlSendUDPData(&SocketfdI32, &time_addr, SendData, TIME_INTERVAL_NUMBER_BYTES, 0); - } - iExit = 1; - (void)iCommClose(); - } - - if (ReceivedNewData && GPSTime->isGPSenabled) { - /* Make call periodic */ - sleep_time.tv_sec = SLEEP_TIME_GPS_CONNECTED_S; - sleep_time.tv_nsec = SLEEP_TIME_GPS_CONNECTED_NS; - nanosleep(&sleep_time, &ref_time); - } - else if (!GPSTime->isGPSenabled) { - sleep_time.tv_sec = SLEEP_TIME_NO_GPS_CONNECTED_S; - sleep_time.tv_nsec = SLEEP_TIME_NO_GPS_CONNECTED_NS; - nanosleep(&sleep_time, &ref_time); - } - } - - LogMessage(LOG_LEVEL_INFO, "Time control exiting"); + C8 TextBufferC8[TIME_CONTROL_HOSTNAME_BUFFER_SIZE]; + C8 ServerIPC8[TIME_CONTROL_HOSTNAME_BUFFER_SIZE]; + U16 ServerPortU16; + I32 SocketfdI32 = -1; + struct sockaddr_in time_addr; + + I32 result; + C8 TimeBuffer[TIME_CONTROL_RECEIVE_BUFFER_SIZE]; + C8 LogBuffer[LOG_BUFFER_LENGTH]; + I32 ReceivedNewData, i; + C8 SendData[TIME_INTERVAL_NUMBER_BYTES] = { 0, 0, 3, 0xe8 }; + //C8 SendData[4] = {0, 0, 0, 1}; + struct timespec sleep_time, ref_time; + C8 MqRecvBuffer[MBUS_MAX_DATALEN]; + struct timeval tv, ExecTime; + struct tm *tm; + + U32 IpU32; + U8 PrevSecondU8; + U16 CurrentMilliSecondU16, PrevMilliSecondU16; + U8 CycleCount = 0; + + enum COMMAND command; + char busReceiveBuffer[MBUS_MAX_DATALEN]; + + // Create log + LogInit(MODULE_NAME, logLevel); + LogMessage(LOG_LEVEL_INFO, "Time control task running with PID: %i", getpid()); + + // Set up signal handlers + if (signal(SIGINT, signalHandler) == SIG_ERR) + util_error("Unable to initialize signal handler"); + + // Set up message bus connection + if (iCommInit()) + util_error("Unable to initialize message bus connection"); + + GPSTime->isGPSenabled = 0; + + gettimeofday(&ExecTime, NULL); + CurrentMilliSecondU16 = (U16) (ExecTime.tv_usec / 1000); + PrevMilliSecondU16 = CurrentMilliSecondU16; + // Set time server IP + DataDictionaryGetTimeServerIPC8(GSD, ServerIPC8, TIME_CONTROL_HOSTNAME_BUFFER_SIZE); + DataDictionaryGetTimeServerIPU32(GSD, &IpU32); + + // Set time server port + DataDictionaryGetTimeServerPortU16(GSD, &ServerPortU16); + + // If time server is specified, connect to it + if (IpU32 != 0) { + LogMessage(LOG_LEVEL_INFO, "Connecting to time server..."); + + if (TimeControlCreateTimeChannel(ServerIPC8, ServerPortU16, &SocketfdI32, &time_addr)) { + LogMessage(LOG_LEVEL_INFO, "Using time server reference"); + TimeControlSendUDPData(&SocketfdI32, &time_addr, SendData, TIME_INTERVAL_NUMBER_BYTES, 0); + GPSTime->isGPSenabled = 1; + } + else { + LogMessage(LOG_LEVEL_INFO, "Defaulting to system time"); + + // Send warning over MQ + LOG_SEND(LogBuffer, "Unable to connect to time server"); + } + + } + + if (!GPSTime->isGPSenabled) { + LogMessage(LOG_LEVEL_INFO, "Initializing with system time"); + + gettimeofday(&tv, NULL); + + GPSTime->MicroSecondU16 = 0; + GPSTime->GPSMillisecondsU64 = + tv.tv_sec * 1000 + tv.tv_usec / 1000 - MS_TIME_DIFF_UTC_GPS + MS_LEAP_SEC_DIFF_UTC_GPS; + GPSTime->GPSWeekU16 = (U16) (GPSTime->GPSMillisecondsU64 / WEEK_TIME_MS); + GPSTime->GPSSecondsOfWeekU32 = + (U32) ((GPSTime->GPSMillisecondsU64 - (U64) (GPSTime->GPSWeekU16) * WEEK_TIME_MS) / 1000); + GPSTime->GPSSecondsOfDayU32 = (GPSTime->GPSMillisecondsU64 % DAY_TIME_MS) / 1000; + GPSTime->GPSMinutesU32 = (GPSTime->GPSMillisecondsU64 / 1000) / 60; + GPSTime->isTimeInitializedU8 = 1; + } + + while (!iExit) { + + // Ignore any commands received, just empty the bus + iCommRecv(&command, busReceiveBuffer, sizeof (busReceiveBuffer), NULL); + + gettimeofday(&ExecTime, NULL); + CurrentMilliSecondU16 = (U16) (ExecTime.tv_usec / 1000); + if (CurrentMilliSecondU16 < PrevMilliSecondU16) { + GSD->TimeControlExecTimeU16 = CurrentMilliSecondU16 + (1000 - PrevMilliSecondU16); + //printf("%d\n", GSD->TimeControlExecTimeU16); + } + else { + GSD->TimeControlExecTimeU16 = abs(PrevMilliSecondU16 - CurrentMilliSecondU16); + //printf("%d\n", GSD->TimeControlExecTimeU16); + } + PrevMilliSecondU16 = CurrentMilliSecondU16; + + if (GPSTime->isGPSenabled) { + bzero(TimeBuffer, TIME_CONTROL_RECEIVE_BUFFER_SIZE); + TimeControlRecvTime(&SocketfdI32, TimeBuffer, TIME_CONTROL_RECEIVE_BUFFER_SIZE, &ReceivedNewData); + + if (ReceivedNewData) + TimeControlDecodeTimeBuffer(GPSTime, TimeBuffer, 0); + } + else if (!GPSTime->isGPSenabled) { + gettimeofday(&tv, NULL); + + tm = localtime(&tv.tv_sec); + + // Add 1900 to get the right year value + GPSTime->YearU16 = (U16) tm->tm_year + 1900; + // Months are 0 based in struct tm + GPSTime->MonthU8 = (U8) tm->tm_mon + 1; + GPSTime->DayU8 = (U8) tm->tm_mday; + GPSTime->HourU8 = (U8) tm->tm_hour; + GPSTime->MinuteU8 = (U8) tm->tm_min; + GPSTime->SecondU8 = (U8) tm->tm_sec; + GPSTime->MillisecondU16 = (U16) (tv.tv_usec / 1000); + + GPSTime->LocalMillisecondU16 = (U16) (tv.tv_usec / 1000); + + GPSTime->GPSMillisecondsU64 = GPSTime->GPSMillisecondsU64 + 1000; + + if (GPSTime->SecondU8 != PrevSecondU8) { + PrevSecondU8 = GPSTime->SecondU8; + GPSTime->SecondCounterU32++; + if (GPSTime->GPSSecondsOfDayU32 >= 86400) + GPSTime->GPSSecondsOfDayU32 = 0; + else + GPSTime->GPSSecondsOfDayU32++; + + if (GPSTime->GPSSecondsOfWeekU32 >= 604800) { + GPSTime->GPSSecondsOfWeekU32 = 0; + GPSTime->GPSWeekU16++; + } + else + GPSTime->GPSSecondsOfWeekU32++; + + if (GPSTime->SecondU8 == 0) + GPSTime->GPSMinutesU32++; + } + } + + if (GSD->ExitU8 == 1) { + if (GPSTime->isGPSenabled) { + SendData[0] = 0; + SendData[1] = 0; + SendData[2] = 0; + SendData[3] = 0; + TimeControlSendUDPData(&SocketfdI32, &time_addr, SendData, TIME_INTERVAL_NUMBER_BYTES, 0); + } + iExit = 1; + (void)iCommClose(); + } + + if (ReceivedNewData && GPSTime->isGPSenabled) { + /* Make call periodic */ + sleep_time.tv_sec = SLEEP_TIME_GPS_CONNECTED_S; + sleep_time.tv_nsec = SLEEP_TIME_GPS_CONNECTED_NS; + nanosleep(&sleep_time, &ref_time); + } + else if (!GPSTime->isGPSenabled) { + sleep_time.tv_sec = SLEEP_TIME_NO_GPS_CONNECTED_S; + sleep_time.tv_nsec = SLEEP_TIME_NO_GPS_CONNECTED_NS; + nanosleep(&sleep_time, &ref_time); + } + } + + LogMessage(LOG_LEVEL_INFO, "Time control exiting"); } void signalHandler(int signo) { - if (signo == SIGINT) { - LogMessage(LOG_LEVEL_WARNING, "Caught keyboard interrupt"); - iExit = 1; - } - else { - LogMessage(LOG_LEVEL_ERROR, "Caught unhandled signal"); - } + if (signo == SIGINT) { + LogMessage(LOG_LEVEL_WARNING, "Caught keyboard interrupt"); + iExit = 1; + } + else { + LogMessage(LOG_LEVEL_ERROR, "Caught unhandled signal"); + } } U16 TimeControlGetMillisecond(TimeType * GPSTime) { - struct timeval now; - U16 MilliU16 = 0, NowU16 = 0; + struct timeval now; + U16 MilliU16 = 0, NowU16 = 0; - gettimeofday(&now, NULL); - NowU16 = (U16) (now.tv_usec / 1000); - //if(NowU16 >= GPSTime->LocalMillisecondU16) MilliU16 = NowU16 - GPSTime->LocalMillisecondU16; - //else if(NowU16 < GPSTime->LocalMillisecondU16) MilliU16 = 1000 + ((I16)NowU16 - (I16)GPSTime->LocalMillisecondU16); + gettimeofday(&now, NULL); + NowU16 = (U16) (now.tv_usec / 1000); + //if(NowU16 >= GPSTime->LocalMillisecondU16) MilliU16 = NowU16 - GPSTime->LocalMillisecondU16; + //else if(NowU16 < GPSTime->LocalMillisecondU16) MilliU16 = 1000 + ((I16)NowU16 - (I16)GPSTime->LocalMillisecondU16); - if (NowU16 >= GPSTime->LocalMillisecondU16) - MilliU16 = NowU16 - GPSTime->LocalMillisecondU16; - else if (NowU16 < GPSTime->LocalMillisecondU16) - MilliU16 = 1000 - GPSTime->LocalMillisecondU16 + NowU16; + if (NowU16 >= GPSTime->LocalMillisecondU16) + MilliU16 = NowU16 - GPSTime->LocalMillisecondU16; + else if (NowU16 < GPSTime->LocalMillisecondU16) + MilliU16 = 1000 - GPSTime->LocalMillisecondU16 + NowU16; - //printf("Result= %d, now= %d, local= %d \n", MilliU16, NowU16, GPSTime->LocalMillisecondU16); - return MilliU16; + //printf("Result= %d, now= %d, local= %d \n", MilliU16, NowU16, GPSTime->LocalMillisecondU16); + return MilliU16; } static int TimeControlCreateTimeChannel(const char *name, const uint32_t port, int *sockfd, - struct sockaddr_in *addr) { - int result; - struct hostent *object; - C8 packetIntervalMs[TIME_INTERVAL_NUMBER_BYTES] = { 0, 0, 0, 100 }; // Make server send with this interval while waiting for first reply - C8 timeBuffer[TIME_CONTROL_RECEIVE_BUFFER_SIZE]; - int receivedNewData = 0; - struct timeval timeout = { REPLY_TIMEOUT_S, 0 }; - struct timeval tEnd, tCurr; - TimeType tempGPSTime; - - LogMessage(LOG_LEVEL_INFO, "Specified time server address: %s:%d", name, port); - /* Connect to object safety socket */ - - *sockfd = socket(AF_INET, SOCK_DGRAM, 0); - if (*sockfd < 0) { - util_error("Failed to connect to time socket"); - } - - /* Set address to object */ - object = gethostbyname(name); - - if (object == NULL) { - util_error("Unknown host"); - } - - bcopy((char *)object->h_addr, (char *)&addr->sin_addr.s_addr, object->h_length); - addr->sin_family = AF_INET; - addr->sin_port = htons(port); - - - if (setsockopt(*sockfd, SOL_SOCKET, SO_RCVTIMEO, (char *)&timeout, sizeof (timeout)) < 0) - util_error("Setsockopt failed"); - - - - /* set socket to non-blocking */ - result = fcntl(*sockfd, F_SETFL, fcntl(*sockfd, F_GETFL, 0) | O_NONBLOCK); - if (result < 0) { - util_error("Error calling fcntl"); - } - LogMessage(LOG_LEVEL_INFO, "Created socket and time address: %s:%d", name, port); - - // Check for existence of remote server - LogMessage(LOG_LEVEL_INFO, "Awaiting reply from time server..."); - // Set send interval to be as short as possible to minimise wait for reply - TimeControlSendUDPData(sockfd, addr, packetIntervalMs, TIME_INTERVAL_NUMBER_BYTES, 0); - - // Set time to stop waiting for reply - gettimeofday(&tEnd, NULL); - timeradd(&tEnd, &timeout, &tEnd); - - do { - gettimeofday(&tCurr, NULL); - TimeControlRecvTime(sockfd, timeBuffer, TIME_CONTROL_RECEIVE_BUFFER_SIZE, &receivedNewData); - if (receivedNewData) { - TimeControlDecodeTimeBuffer(&tempGPSTime, timeBuffer, 0); - switch (tempGPSTime.FixQualityU8) { - case FIX_QUALITY_NONE: - LogMessage(LOG_LEVEL_WARNING, "Received reply from time server: no satellite fix"); - return 0; - case FIX_QUALITY_BASIC: - LogMessage(LOG_LEVEL_INFO, - "Received reply from time server: non-differential fix on %d satellite(s)", - tempGPSTime.NSatellitesU8); - return 1; - case FIX_QUALITY_DIFFERENTIAL: - LogMessage(LOG_LEVEL_INFO, - "Received reply from time server: differential fix on %d satellite(s)", - tempGPSTime.NSatellitesU8); - return 1; - default: - LogMessage(LOG_LEVEL_ERROR, - "Received reply from time server: unexpected fix quality parameter"); - return 0; - } - } - } while (timercmp(&tCurr, &tEnd, <)); - - LogMessage(LOG_LEVEL_WARNING, "Unable to connect to specified time server: %s:%d", name, port); - return 0; + struct sockaddr_in *addr) { + int result; + struct hostent *object; + C8 packetIntervalMs[TIME_INTERVAL_NUMBER_BYTES] = { 0, 0, 0, 100 }; // Make server send with this interval while waiting for first reply + C8 timeBuffer[TIME_CONTROL_RECEIVE_BUFFER_SIZE]; + int receivedNewData = 0; + struct timeval timeout = { REPLY_TIMEOUT_S, 0 }; + struct timeval tEnd, tCurr; + TimeType tempGPSTime; + + LogMessage(LOG_LEVEL_INFO, "Specified time server address: %s:%d", name, port); + /* Connect to object safety socket */ + + *sockfd = socket(AF_INET, SOCK_DGRAM, 0); + if (*sockfd < 0) { + util_error("Failed to connect to time socket"); + } + + /* Set address to object */ + object = gethostbyname(name); + + if (object == NULL) { + util_error("Unknown host"); + } + + bcopy((char *)object->h_addr, (char *)&addr->sin_addr.s_addr, object->h_length); + addr->sin_family = AF_INET; + addr->sin_port = htons(port); + + + if (setsockopt(*sockfd, SOL_SOCKET, SO_RCVTIMEO, (char *)&timeout, sizeof (timeout)) < 0) + util_error("Setsockopt failed"); + + + + /* set socket to non-blocking */ + result = fcntl(*sockfd, F_SETFL, fcntl(*sockfd, F_GETFL, 0) | O_NONBLOCK); + if (result < 0) { + util_error("Error calling fcntl"); + } + LogMessage(LOG_LEVEL_INFO, "Created socket and time address: %s:%d", name, port); + + // Check for existence of remote server + LogMessage(LOG_LEVEL_INFO, "Awaiting reply from time server..."); + // Set send interval to be as short as possible to minimise wait for reply + TimeControlSendUDPData(sockfd, addr, packetIntervalMs, TIME_INTERVAL_NUMBER_BYTES, 0); + + // Set time to stop waiting for reply + gettimeofday(&tEnd, NULL); + timeradd(&tEnd, &timeout, &tEnd); + + do { + gettimeofday(&tCurr, NULL); + TimeControlRecvTime(sockfd, timeBuffer, TIME_CONTROL_RECEIVE_BUFFER_SIZE, &receivedNewData); + if (receivedNewData) { + TimeControlDecodeTimeBuffer(&tempGPSTime, timeBuffer, 0); + switch (tempGPSTime.FixQualityU8) { + case FIX_QUALITY_NONE: + LogMessage(LOG_LEVEL_WARNING, "Received reply from time server: no satellite fix"); + return 0; + case FIX_QUALITY_BASIC: + LogMessage(LOG_LEVEL_INFO, + "Received reply from time server: non-differential fix on %d satellite(s)", + tempGPSTime.NSatellitesU8); + return 1; + case FIX_QUALITY_DIFFERENTIAL: + LogMessage(LOG_LEVEL_INFO, + "Received reply from time server: differential fix on %d satellite(s)", + tempGPSTime.NSatellitesU8); + return 1; + default: + LogMessage(LOG_LEVEL_ERROR, + "Received reply from time server: unexpected fix quality parameter"); + return 0; + } + } + } while (timercmp(&tCurr, &tEnd, <)); + + LogMessage(LOG_LEVEL_WARNING, "Unable to connect to specified time server: %s:%d", name, port); + return 0; } U32 TimeControlIPStringToInt(C8 * IP) { - C8 *p, *ps; - C8 Buffer[3]; - U32 IpU32 = 0; + C8 *p, *ps; + C8 Buffer[3]; + U32 IpU32 = 0; - ps = IP; - p = strchr(IP, '.'); - if (p != NULL) { - bzero(Buffer, 3); - strncpy(Buffer, ps, (U64) p - (U64) ps); - IpU32 = (IpU32 | (U32) atoi(Buffer)) << 8; + ps = IP; + p = strchr(IP, '.'); + if (p != NULL) { + bzero(Buffer, 3); + strncpy(Buffer, ps, (U64) p - (U64) ps); + IpU32 = (IpU32 | (U32) atoi(Buffer)) << 8; - ps = p + 1; - p = strchr(ps, '.'); - bzero(Buffer, 3); - strncpy(Buffer, ps, (U64) p - (U64) ps); + ps = p + 1; + p = strchr(ps, '.'); + bzero(Buffer, 3); + strncpy(Buffer, ps, (U64) p - (U64) ps); - IpU32 = (IpU32 | (U32) atoi(Buffer)) << 8; + IpU32 = (IpU32 | (U32) atoi(Buffer)) << 8; - ps = p + 1; - p = strchr(ps, '.'); - bzero(Buffer, 3); - strncpy(Buffer, ps, (U64) p - (U64) ps); + ps = p + 1; + p = strchr(ps, '.'); + bzero(Buffer, 3); + strncpy(Buffer, ps, (U64) p - (U64) ps); - IpU32 = (IpU32 | (U32) atoi(Buffer)) << 8; + IpU32 = (IpU32 | (U32) atoi(Buffer)) << 8; - ps = p + 1; - p = strchr(ps, 0); - bzero(Buffer, 3); - strncpy(Buffer, ps, (U64) p - (U64) ps); + ps = p + 1; + p = strchr(ps, 0); + bzero(Buffer, 3); + strncpy(Buffer, ps, (U64) p - (U64) ps); - IpU32 = (IpU32 | (U32) atoi(Buffer)); + IpU32 = (IpU32 | (U32) atoi(Buffer)); - //printf("IpU32 = %x\n", IpU32); - } + //printf("IpU32 = %x\n", IpU32); + } - return IpU32; + return IpU32; } static int TimeControlSendUDPData(int *sockfd, struct sockaddr_in *addr, C8 * SendData, int Length, - char debug) { - int result, i; + char debug) { + int result, i; - result = sendto(*sockfd, SendData, Length, 0, (const struct sockaddr *)addr, sizeof (struct sockaddr_in)); + result = sendto(*sockfd, SendData, Length, 0, (const struct sockaddr *)addr, sizeof (struct sockaddr_in)); - if (debug) { - // TODO: Change to log write when bytes thingy has been implemented - for (i = 0; i < Length; i++) - printf("[%d]=%x ", i, (C8) * (SendData + i)); - printf("\n"); - } + if (debug) { + // TODO: Change to log write when bytes thingy has been implemented + for (i = 0; i < Length; i++) + printf("[%d]=%x ", i, (C8) * (SendData + i)); + printf("\n"); + } - if (result < 0) { - util_error("Failed to send on time socket"); - } + if (result < 0) { + util_error("Failed to send on time socket"); + } - return 0; + return 0; } static void TimeControlRecvTime(int *sockfd, C8 * buffer, int length, int *receivedNewData) { - int result; - - *receivedNewData = 0; - do { - result = recv(*sockfd, buffer, length, 0); - - if (result < 0) { - // If we received a _real_ error, report it. Otherwise, nothing was received - if (errno != EAGAIN && errno != EWOULDBLOCK) - util_error("Failed to receive from time socket"); - else - return; - } - else if (result == 0) { - // EOF received - LogMessage(LOG_LEVEL_ERROR, "Time server disconnected"); - *receivedNewData = 0; - return; - } - else { - // If message size is equal to what is expected according to the format, keep reading until the newest has been read - if (result == length) { - *receivedNewData = 1; - LogMessage(LOG_LEVEL_DEBUG, "Received data: <%s>, result=%d", buffer, result); - } - else { - *receivedNewData = 0; - LogMessage(LOG_LEVEL_ERROR, "Received badly formatted message from time server"); - } - - } - } while (result > 0); - return; + int result; + + *receivedNewData = 0; + do { + result = recv(*sockfd, buffer, length, 0); + + if (result < 0) { + // If we received a _real_ error, report it. Otherwise, nothing was received + if (errno != EAGAIN && errno != EWOULDBLOCK) + util_error("Failed to receive from time socket"); + else + return; + } + else if (result == 0) { + // EOF received + LogMessage(LOG_LEVEL_ERROR, "Time server disconnected"); + *receivedNewData = 0; + return; + } + else { + // If message size is equal to what is expected according to the format, keep reading until the newest has been read + if (result == length) { + *receivedNewData = 1; + LogMessage(LOG_LEVEL_DEBUG, "Received data: <%s>, result=%d", buffer, result); + } + else { + *receivedNewData = 0; + LogMessage(LOG_LEVEL_ERROR, "Received badly formatted message from time server"); + } + + } + } while (result > 0); + return; } static void TimeControlDecodeTimeBuffer(TimeType * GPSTime, C8 * TimeBuffer, C8 debug) { - struct timeval tv; - - gettimeofday(&tv, NULL); - - GPSTime->ProtocolVersionU8 = TimeBuffer[0]; - GPSTime->YearU16 = ((U16) TimeBuffer[1]) << 8 | TimeBuffer[2]; - GPSTime->MonthU8 = TimeBuffer[3]; - GPSTime->DayU8 = TimeBuffer[4]; - GPSTime->HourU8 = TimeBuffer[5]; - GPSTime->MinuteU8 = TimeBuffer[6]; - GPSTime->SecondU8 = TimeBuffer[7]; - GPSTime->MillisecondU16 = ((U16) TimeBuffer[8]) << 8 | TimeBuffer[9]; - GPSTime->MicroSecondU16 = 0; - GPSTime->SecondCounterU32 = - ((U32) TimeBuffer[10]) << 24 | ((U32) TimeBuffer[11]) << 16 | ((U32) TimeBuffer[12]) << 8 | - TimeBuffer[13]; - GPSTime->GPSMillisecondsU64 = - ((U64) TimeBuffer[14]) << 56 | ((U64) TimeBuffer[15]) << 48 | ((U64) TimeBuffer[16]) << 40 | ((U64) - TimeBuffer - [17]) << - 32 | ((U64) TimeBuffer[18]) << 24 | ((U64) TimeBuffer[19]) << 16 | ((U64) TimeBuffer[20]) << 8 | - TimeBuffer[21]; - GPSTime->GPSMinutesU32 = - ((U32) TimeBuffer[22]) << 24 | ((U32) TimeBuffer[23]) << 16 | ((U32) TimeBuffer[24]) << 8 | - TimeBuffer[25]; - GPSTime->GPSWeekU16 = ((U16) TimeBuffer[26]) << 8 | TimeBuffer[27]; - GPSTime->GPSSecondsOfWeekU32 = - ((U32) TimeBuffer[28]) << 24 | ((U32) TimeBuffer[29]) << 16 | ((U32) TimeBuffer[30]) << 8 | - TimeBuffer[31] + MS_LEAP_SEC_DIFF_UTC_GPS / 1000; - GPSTime->GPSSecondsOfDayU32 = - ((U32) TimeBuffer[32]) << 24 | ((U32) TimeBuffer[33]) << 16 | ((U32) TimeBuffer[34]) << 8 | - TimeBuffer[35]; - GPSTime->ETSIMillisecondsU64 = - ((U64) TimeBuffer[36]) << 56 | ((U64) TimeBuffer[37]) << 48 | ((U64) TimeBuffer[38]) << 40 | ((U64) - TimeBuffer - [39]) << - 32 | ((U64) TimeBuffer[40]) << 24 | ((U64) TimeBuffer[41]) << 16 | ((U64) TimeBuffer[42]) << 8 | - TimeBuffer[43]; - GPSTime->LatitudeU32 = - ((U32) TimeBuffer[44]) << 24 | ((U32) TimeBuffer[45]) << 16 | ((U32) TimeBuffer[46]) << 8 | - TimeBuffer[47]; - GPSTime->LongitudeU32 = - ((U32) TimeBuffer[48]) << 24 | ((U32) TimeBuffer[49]) << 16 | ((U32) TimeBuffer[50]) << 8 | - TimeBuffer[51]; - GPSTime->FixQualityU8 = TimeBuffer[52]; - GPSTime->NSatellitesU8 = TimeBuffer[53]; - - gettimeofday(&tv, NULL); - - GPSTime->LocalMillisecondU16 = (U16) (tv.tv_usec / 1000); - - GPSTime->isTimeInitializedU8 = 1; - - if (debug) { - //TimeControlGetMillisecond(GPSTime); - //LogPrintBytes(TimeBuffer,0,TIME_CONTROL_RECEIVE_BUFFER_SIZE); - //LogPrint("ProtocolVersionU8: %d", GPSTime->ProtocolVersionU8); - LogPrint("YearU16: %d", GPSTime->YearU16); - LogPrint("MonthU8: %d - %d", GPSTime->MonthU8, TimeBuffer[3]); - LogPrint("DayU8: %d", GPSTime->DayU8); - LogPrint("Time: %d:%d:%d", GPSTime->HourU8, GPSTime->MinuteU8, GPSTime->SecondU8); - //LogPrint("MinuteU8: %d", GPSTime->MinuteU8); - //LogPrint("SecondU8: %d", GPSTime->SecondU8); - //LogPrint("MillisecondU16: %d", GPSTime->MillisecondU16); - //LogPrint("SecondCounterU32: %d", GPSTime->SecondCounterU32); - //LogPrint("GPSMillisecondsU64: %ld", GPSTime->GPSMillisecondsU64); - //LogPrint("GPSMinutesU32: %d", GPSTime->GPSMinutesU32); - //LogPrint("GPSWeekU16: %d", GPSTime->GPSWeekU16); - //LogPrint("GPSSecondsOfWeekU32: %d", GPSTime->GPSSecondsOfWeekU32); - //LogPrint("GPSSecondsOfDayU32: %d", GPSTime->GPSSecondsOfDayU32); - //LogPrint("ETSIMillisecondsU64: %ld", GPSTime->ETSIMillisecondsU64); - //LogPrint("LatitudeU32: %d", GPSTime->LatitudeU32); - //LogPrint("LongitudeU32: %d", GPSTime->LongitudeU32); - //LogPrint("LocalMillisecondU16: %d", GPSTime->LocalMillisecondU16); - //LogPrint("FixQualityU8: %d", GPSTime->FixQualityU8); - //LogPrint("NSatellitesU8: %d", GPSTime->NSatellitesU8); - } + struct timeval tv; + + gettimeofday(&tv, NULL); + + GPSTime->ProtocolVersionU8 = TimeBuffer[0]; + GPSTime->YearU16 = ((U16) TimeBuffer[1]) << 8 | TimeBuffer[2]; + GPSTime->MonthU8 = TimeBuffer[3]; + GPSTime->DayU8 = TimeBuffer[4]; + GPSTime->HourU8 = TimeBuffer[5]; + GPSTime->MinuteU8 = TimeBuffer[6]; + GPSTime->SecondU8 = TimeBuffer[7]; + GPSTime->MillisecondU16 = ((U16) TimeBuffer[8]) << 8 | TimeBuffer[9]; + GPSTime->MicroSecondU16 = 0; + GPSTime->SecondCounterU32 = + ((U32) TimeBuffer[10]) << 24 | ((U32) TimeBuffer[11]) << 16 | ((U32) TimeBuffer[12]) << 8 | + TimeBuffer[13]; + GPSTime->GPSMillisecondsU64 = + ((U64) TimeBuffer[14]) << 56 | ((U64) TimeBuffer[15]) << 48 | ((U64) TimeBuffer[16]) << 40 | ((U64) + TimeBuffer + [17]) << + 32 | ((U64) TimeBuffer[18]) << 24 | ((U64) TimeBuffer[19]) << 16 | ((U64) TimeBuffer[20]) << 8 | + TimeBuffer[21]; + GPSTime->GPSMinutesU32 = + ((U32) TimeBuffer[22]) << 24 | ((U32) TimeBuffer[23]) << 16 | ((U32) TimeBuffer[24]) << 8 | + TimeBuffer[25]; + GPSTime->GPSWeekU16 = ((U16) TimeBuffer[26]) << 8 | TimeBuffer[27]; + GPSTime->GPSSecondsOfWeekU32 = + ((U32) TimeBuffer[28]) << 24 | ((U32) TimeBuffer[29]) << 16 | ((U32) TimeBuffer[30]) << 8 | + TimeBuffer[31] + MS_LEAP_SEC_DIFF_UTC_GPS / 1000; + GPSTime->GPSSecondsOfDayU32 = + ((U32) TimeBuffer[32]) << 24 | ((U32) TimeBuffer[33]) << 16 | ((U32) TimeBuffer[34]) << 8 | + TimeBuffer[35]; + GPSTime->ETSIMillisecondsU64 = + ((U64) TimeBuffer[36]) << 56 | ((U64) TimeBuffer[37]) << 48 | ((U64) TimeBuffer[38]) << 40 | ((U64) + TimeBuffer + [39]) << + 32 | ((U64) TimeBuffer[40]) << 24 | ((U64) TimeBuffer[41]) << 16 | ((U64) TimeBuffer[42]) << 8 | + TimeBuffer[43]; + GPSTime->LatitudeU32 = + ((U32) TimeBuffer[44]) << 24 | ((U32) TimeBuffer[45]) << 16 | ((U32) TimeBuffer[46]) << 8 | + TimeBuffer[47]; + GPSTime->LongitudeU32 = + ((U32) TimeBuffer[48]) << 24 | ((U32) TimeBuffer[49]) << 16 | ((U32) TimeBuffer[50]) << 8 | + TimeBuffer[51]; + GPSTime->FixQualityU8 = TimeBuffer[52]; + GPSTime->NSatellitesU8 = TimeBuffer[53]; + + gettimeofday(&tv, NULL); + + GPSTime->LocalMillisecondU16 = (U16) (tv.tv_usec / 1000); + + GPSTime->isTimeInitializedU8 = 1; + + if (debug) { + //TimeControlGetMillisecond(GPSTime); + //LogPrintBytes(TimeBuffer,0,TIME_CONTROL_RECEIVE_BUFFER_SIZE); + //LogPrint("ProtocolVersionU8: %d", GPSTime->ProtocolVersionU8); + LogPrint("YearU16: %d", GPSTime->YearU16); + LogPrint("MonthU8: %d - %d", GPSTime->MonthU8, TimeBuffer[3]); + LogPrint("DayU8: %d", GPSTime->DayU8); + LogPrint("Time: %d:%d:%d", GPSTime->HourU8, GPSTime->MinuteU8, GPSTime->SecondU8); + //LogPrint("MinuteU8: %d", GPSTime->MinuteU8); + //LogPrint("SecondU8: %d", GPSTime->SecondU8); + //LogPrint("MillisecondU16: %d", GPSTime->MillisecondU16); + //LogPrint("SecondCounterU32: %d", GPSTime->SecondCounterU32); + //LogPrint("GPSMillisecondsU64: %ld", GPSTime->GPSMillisecondsU64); + //LogPrint("GPSMinutesU32: %d", GPSTime->GPSMinutesU32); + //LogPrint("GPSWeekU16: %d", GPSTime->GPSWeekU16); + //LogPrint("GPSSecondsOfWeekU32: %d", GPSTime->GPSSecondsOfWeekU32); + //LogPrint("GPSSecondsOfDayU32: %d", GPSTime->GPSSecondsOfDayU32); + //LogPrint("ETSIMillisecondsU64: %ld", GPSTime->ETSIMillisecondsU64); + //LogPrint("LatitudeU32: %d", GPSTime->LatitudeU32); + //LogPrint("LongitudeU32: %d", GPSTime->LongitudeU32); + //LogPrint("LocalMillisecondU16: %d", GPSTime->LocalMillisecondU16); + //LogPrint("FixQualityU8: %d", GPSTime->FixQualityU8); + //LogPrint("NSatellitesU8: %d", GPSTime->NSatellitesU8); + } } diff --git a/server/src/util.c b/server/src/util.c index ed2634a18..37f15394b 100644 --- a/server/src/util.c +++ b/server/src/util.c @@ -2086,9 +2086,9 @@ int iCommSend(const enum COMMAND iCommand, const char *cpData, size_t dataLength case MQBUS_OK: return 0; case MQBUS_MQ_FULL: - LogMessage(LOG_LEVEL_WARNING, - "Attempted to write to full message queue - message may be lost: <%d><%s>", iCommand, - cpData); + LogMessage(LOG_LEVEL_WARNING, + "Attempted to write to full message queue - message may be lost: <%d><%s>", iCommand, + cpData); return 1; case MQBUS_INVALID_INPUT_ARGUMENT: LogMessage(LOG_LEVEL_WARNING, "Invalid message queue message length"); From 2dea5dd3fd05e975d925e1dd9c545d57ff45255b Mon Sep 17 00:00:00 2001 From: alfaro01 Date: Fri, 17 Jan 2020 16:57:41 +0100 Subject: [PATCH 067/523] Fixed cmakelist version --- modules/Visualization/CMakeLists.txt | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/modules/Visualization/CMakeLists.txt b/modules/Visualization/CMakeLists.txt index 332f31024..38d96ae0e 100644 --- a/modules/Visualization/CMakeLists.txt +++ b/modules/Visualization/CMakeLists.txt @@ -1,8 +1,13 @@ -cmake_minimum_required(VERSION 2.8) +cmake_minimum_required(VERSION 3.1) project(Visualization) # This module is an example of how to set up a new module external to the Maestro executable +# Set C and C++ dialects +SET(CMAKE_C_STANDARD 11) +SET(CMAKE_C_STANDARD_REQUIRED ON) +SET(CMAKE_CXX_STANDARD 11) +SET(CMAKE_CXX_STANDARD_REQUIRED ON) include_directories(inc) include_directories(../../util/C/logging) From a77cf04db7e0c99f9e84cce1eee4daf9250a7585 Mon Sep 17 00:00:00 2001 From: alfaro01 Date: Fri, 17 Jan 2020 17:02:02 +0100 Subject: [PATCH 068/523] removed COMM_MONI --- modules/Visualization/src/main.c | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/modules/Visualization/src/main.c b/modules/Visualization/src/main.c index a088d5d8e..8b220173d 100644 --- a/modules/Visualization/src/main.c +++ b/modules/Visualization/src/main.c @@ -124,9 +124,7 @@ int main() { switch (command) { case COMM_INIT: break; - case COMM_MONI: - // Ignore old style MONR data - break; + case COMM_MONR: { From 361683440949868bfda300434b69ba6b98494f2a Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 20 Jan 2020 13:30:19 +0100 Subject: [PATCH 069/523] Moved MONR struct value ID definition to iso22133.h --- server/inc/iso22133.h | 6 +++++- server/inc/util.h | 1 - 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/server/inc/iso22133.h b/server/inc/iso22133.h index cb5920450..91b77f1de 100644 --- a/server/inc/iso22133.h +++ b/server/inc/iso22133.h @@ -31,6 +31,7 @@ typedef struct //! *************************** MONR +#define COMMAND_MONR_CODE 0x0006 typedef struct { HeaderType header; @@ -50,7 +51,10 @@ typedef struct uint8_t readyToArm; uint8_t errorStatus; FooterType footer; -} MONRType; //41 bytes +} MONRType; + +//! MONR value IDs +#define VALUE_ID_MONR_STRUCT 0x80 //! *************************** TRCM diff --git a/server/inc/util.h b/server/inc/util.h index 806d5168a..9cd936b57 100644 --- a/server/inc/util.h +++ b/server/inc/util.h @@ -138,7 +138,6 @@ extern "C"{ #define VALUE_ID_GPS_SECOND_OF_WEEK 0x2 #define VALUE_ID_GPS_WEEK 0x3 #define VALUE_ID_DATE_ISO8601 0x4 -#define VALUE_ID_MONR_STRUCT 0x80 #define VALUE_ID_X_POSITION 0x10 #define VALUE_ID_Y_POSITION 0x11 #define VALUE_ID_Z_POSITION 0x12 From c49ca8ca632850c62fd9273e6952d2ceaffd5554 Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Mon, 20 Jan 2020 13:56:23 +0100 Subject: [PATCH 070/523] noice --- server/src/objectcontrol.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index cc3666058..cf98901fd 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -874,7 +874,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { } - } while (iResult < 0 && DisconnectU8 == 0); + } while (iExit == 0 && iResult < 0 && DisconnectU8 == 0); if (iResult >= 0) { /* Send OSEM command in mq so that we get some information like GPSweek, origin (latitude,logitude,altitude in gps coordinates) */ From f96210ee82d4b81862b17f2561eb25de2803570d Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Mon, 20 Jan 2020 13:59:46 +0100 Subject: [PATCH 071/523] Changed back to util_error --- server/src/objectcontrol.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index cf98901fd..a8b7eed8e 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -843,22 +843,22 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { (void)sleep(3); // TODO: Move this to the rest of the sleep operations? Also, remove the hardcoded 3 break; case EADDRINUSE: - LogMessage(LOG_LEVEL_ERROR, "[ObjectControl] Local address/port already in use"); + util_error("[ObjectControl] Local address/port already in use"); break; case EALREADY: - LogMessage(LOG_LEVEL_ERROR, "[ObjectControl] Previous connection attempt still in progress"); + util_error("[ObjectControl] Previous connection attempt still in progress"); break; case EISCONN: - LogMessage(LOG_LEVEL_ERROR, "[ObjectControl] Socket is already connected"); + util_error("[ObjectControl] Socket is already connected"); break; case ENETUNREACH: - LogMessage(LOG_LEVEL_ERROR, "[ObjectControl] Network unreachable"); + util_error("[ObjectControl] Network unreachable"); break; case ETIMEDOUT: - LogMessage(LOG_LEVEL_ERROR, "[ObjectControl] Connection timed out"); + util_error("[ObjectControl] Connection timed out"); break; default: - LogMessage(LOG_LEVEL_ERROR, "ERR: Failed to connect to control socket"); + util_error("ERR: Failed to connect to control socket"); break; } From bdef4ddae59276412c44e66bac7142dc3a13ce53 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 20 Jan 2020 14:03:31 +0100 Subject: [PATCH 072/523] Implemented MONR builder function in iso22133.c --- server/src/iso22133.c | 209 ++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 209 insertions(+) diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 6311e1478..2dea820a1 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -1,6 +1,215 @@ #include "iso22133.h" +#include "string.h" +#include "logging.h" +#include "errno.h" + static const uint8_t SupportedProtocolVersions[] = { 2 }; + +// ************************** static functions +static int32_t buildISOHeader(const char * MessageBuffer, const size_t length, HeaderType * HeaderData, char Debug); +static int32_t buildISOFooter(const char * MessageBuffer, const size_t length, FooterType * HeaderData, char Debug); + + +// ************************** function definitions void getSupportedISOProtocolVersions(const uint8_t ** supportedProtocolVersions, size_t * nProtocols) { *supportedProtocolVersions = SupportedProtocolVersions; *nProtocols = sizeof (SupportedProtocolVersions) / sizeof (SupportedProtocolVersions[0]); } + + + +int32_t buildMONRMessage(const char * MonrData, const size_t length, MONRType * MONRData, char debug) { + + const char *p = MonrData; + const uint16_t ExpectedMONRStructSize = (uint16_t) (sizeof (*MONRData) - sizeof (MONRData->header) + - sizeof (MONRData->footer.Crc) - + sizeof (MONRData->monrStructValueID) + - sizeof (MONRData->monrStructContentLength)); + + // Decode ISO header + if (buildISOHeader(p, length, &MONRData->header, 0) == -1) { + memset(MONRData, 0, sizeof (*MONRData)); + return -1; + } + p += sizeof (MONRData->header); + + // Decode content header + memcpy(&MONRData->monrStructValueID, p, sizeof (MONRData->monrStructValueID)); + p += sizeof (MONRData->monrStructValueID); + + if (MONRData->monrStructValueID != VALUE_ID_MONR_STRUCT) { + errno = EINVAL; + LogMessage(LOG_LEVEL_ERROR, "Attempted to pass non-MONR struct into MONR parsing function"); + memset(MONRData, 0, sizeof (*MONRData)); + return -1; + } + + memcpy(&MONRData->monrStructContentLength, p, sizeof (MONRData->monrStructContentLength)); + p += sizeof (MONRData->monrStructContentLength); + + if (MONRData->monrStructContentLength != ExpectedMONRStructSize) { + errno = EINVAL; + LogMessage(LOG_LEVEL_ERROR, "MONR content length %u differs from the expected length %u", + MONRData->monrStructContentLength, ExpectedMONRStructSize); + memset(MONRData, 0, sizeof (*MONRData)); + return -1; + } + + // Decode content + memcpy(&MONRData->gpsQmsOfWeek, p, sizeof (MONRData->gpsQmsOfWeek)); + p += sizeof (MONRData->gpsQmsOfWeek); + + memcpy(&MONRData->xPosition, p, sizeof (MONRData->xPosition)); + p += sizeof (MONRData->xPosition); + + memcpy(&MONRData->yPosition, p, sizeof (MONRData->yPosition)); + p += sizeof (MONRData->yPosition); + + memcpy(&MONRData->zPosition, p, sizeof (MONRData->zPosition)); + p += sizeof (MONRData->zPosition); + + memcpy(&MONRData->heading, p, sizeof (MONRData->heading)); + p += sizeof (MONRData->heading); + + memcpy(&MONRData->longitudinalSpeed, p, sizeof (MONRData->longitudinalSpeed)); + p += sizeof (MONRData->longitudinalSpeed); + + memcpy(&MONRData->lateralSpeed, p, sizeof (MONRData->lateralSpeed)); + p += sizeof (MONRData->lateralSpeed); + + memcpy(&MONRData->longitudinalAcc, p, sizeof (MONRData->longitudinalAcc)); + p += sizeof (MONRData->longitudinalAcc); + + memcpy(&MONRData->lateralAcc, p, sizeof (MONRData->lateralAcc)); + p += sizeof (MONRData->lateralAcc); + + memcpy(&MONRData->driveDirection, p, sizeof (MONRData->driveDirection)); + p += sizeof (MONRData->driveDirection); + + memcpy(&MONRData->state, p, sizeof (MONRData->state)); + p += sizeof (MONRData->state); + + memcpy(&MONRData->readyToArm, p, sizeof (MONRData->readyToArm)); + p += sizeof (MONRData->readyToArm); + + memcpy(&MONRData->errorStatus, p, sizeof (MONRData->errorStatus)); + p += sizeof (MONRData->errorStatus); + + // Footer + if (buildISOFooter(p, length-(size_t)(p-MonrData), &MONRData->footer, 0) == -1) { + memset(MONRData, 0, sizeof (*MONRData)); + return -1; + } + p += sizeof (MONRData->footer); + + if (debug == 1) { + LogPrint("MONR:"); + LogPrint("SyncWord = %x", MONRData->header.SyncWordU16); + LogPrint("TransmitterId = %d", MONRData->header.TransmitterIdU8); + LogPrint("PackageCounter = %d", MONRData->header.MessageCounterU8); + LogPrint("AckReq = %d", MONRData->header.AckReqProtVerU8); + LogPrint("MessageId = %d", MONRData->header.MessageIdU16); + LogPrint("MessageLength = %d", MONRData->header.MessageLengthU32); + LogPrint("ValueId = %d", MONRData->monrStructValueID); + LogPrint("ContentLength = %d", MONRData->monrStructContentLength); + LogPrint("GPSSOW = %d", MONRData->gpsQmsOfWeek); + LogPrint("XPosition = %d", MONRData->xPosition); + LogPrint("YPosition = %d", MONRData->yPosition); + LogPrint("ZPosition = %d", MONRData->zPosition); + LogPrint("Heading = %d", MONRData->heading); + LogPrint("LongitudinalSpeed = %d", MONRData->longitudinalSpeed); + LogPrint("LateralSpeed = %d", MONRData->lateralSpeed); + LogPrint("LongitudinalAcc = %d", MONRData->longitudinalAcc); + LogPrint("LateralAcc = %d", MONRData->lateralAcc); + LogPrint("DriveDirection = %d", MONRData->driveDirection); + LogPrint("State = %d", MONRData->state); + LogPrint("ReadyToArm = %d", MONRData->readyToArm); + LogPrint("ErrorStatus = %d", MONRData->errorStatus); + } + + return 0; +} + + +int32_t buildISOHeader(const char* MessageBuffer, const size_t length, HeaderType * HeaderData, char Debug) { + const char *p = MessageBuffer; + int32_t retval = 0; + const char ProtocolVersionBitmask = 0x7F; + char messageProtocolVersion = 0; + char isProtocolVersionSupported = 0; + const uint8_t *supportedProtocolVersions; + size_t nSupportedProtocols = 0; + + if (length < sizeof (HeaderData)) { + errno = EINVAL; + LogMessage(LOG_LEVEL_ERROR, "Too little raw data to fill ISO header"); + memset(HeaderData, 0, sizeof (*HeaderData)); + return -1; + } + + // Decode ISO header + memcpy(&HeaderData->SyncWordU16, p, sizeof (HeaderData->SyncWordU16)); + p += sizeof (HeaderData->SyncWordU16); + + if (HeaderData->SyncWordU16 != ISO_SYNC_WORD) { + errno = EINVAL; + LogMessage(LOG_LEVEL_ERROR, "Sync word error when decoding ISO header"); + memset(HeaderData, 0, sizeof (*HeaderData)); + return -1; + } + + memcpy(&HeaderData->TransmitterIdU8, p, sizeof (HeaderData->TransmitterIdU8)); + p += sizeof (HeaderData->TransmitterIdU8); + + memcpy(&HeaderData->MessageCounterU8, p, sizeof (HeaderData->MessageCounterU8)); + p += sizeof (HeaderData->MessageCounterU8); + + memcpy(&HeaderData->AckReqProtVerU8, p, sizeof (HeaderData->AckReqProtVerU8)); + p += sizeof (HeaderData->AckReqProtVerU8); + + // Loop over permitted protocol versions + messageProtocolVersion = HeaderData->AckReqProtVerU8 & ProtocolVersionBitmask; + getSupportedISOProtocolVersions(&supportedProtocolVersions, &nSupportedProtocols); + for (size_t i = 0; i < nSupportedProtocols; ++i) { + if (supportedProtocolVersions[i] == messageProtocolVersion) { + isProtocolVersionSupported = 1; + break; + } + } + + if (!isProtocolVersionSupported) { + errno = EPROTONOSUPPORT; + LogMessage(LOG_LEVEL_WARNING, "Protocol version %u not supported", messageProtocolVersion); + retval = -1; + } + + memcpy(&HeaderData->MessageIdU16, p, sizeof (HeaderData->MessageIdU16)); + p += sizeof (HeaderData->MessageIdU16); + + memcpy(&HeaderData->MessageLengthU32, p, sizeof (HeaderData->MessageLengthU32)); + p += sizeof (HeaderData->MessageLengthU32); + + if (Debug) { + LogPrint("SyncWordU16 = 0x%x", HeaderData->SyncWordU16); + LogPrint("TransmitterIdU8 = 0x%x", HeaderData->TransmitterIdU8); + LogPrint("MessageCounterU8 = 0x%x", HeaderData->MessageCounterU8); + LogPrint("AckReqProtVerU8 = 0x%x", HeaderData->AckReqProtVerU8); + LogPrint("MessageIdU16 = 0x%x", HeaderData->MessageIdU16); + LogPrint("MessageLengthU32 = 0x%x", HeaderData->MessageLengthU32); + } + + return retval; +} + + +int32_t buildISOFooter(const char* MessageBuffer, const size_t length, FooterType * HeaderData, char Debug) { + + if (length < sizeof (HeaderData->Crc)) { + LogMessage(LOG_LEVEL_ERROR, "Too little raw data to fill ISO footer"); + return -1; + } + memcpy(&HeaderData->Crc, MessageBuffer, sizeof (HeaderData->Crc)); + + // TODO: check on CRC + return 0; +} From 9bcba0158807d75082fd3cc678c4ca99a684ebfa Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Mon, 20 Jan 2020 14:03:42 +0100 Subject: [PATCH 073/523] Removed empty lines --- server/src/objectcontrol.c | 2 -- 1 file changed, 2 deletions(-) diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index a8b7eed8e..0a1cc6f2c 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -810,8 +810,6 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { LogMessage(LOG_LEVEL_INFO, "CONNECT received"); LOG_SEND(LogBuffer, "[ObjectControl] CONNECT received."); - - /* Connect and send drive files */ for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { From 5400dfd3be94fc7c40093439651eb06a3b5b351e Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 20 Jan 2020 14:19:33 +0100 Subject: [PATCH 074/523] Removed MONR builder function from object control --- server/inc/iso22133.h | 11 +++- server/src/iso22133.c | 57 ++++++++++-------- server/src/objectcontrol.c | 118 +------------------------------------ 3 files changed, 42 insertions(+), 144 deletions(-) diff --git a/server/inc/iso22133.h b/server/inc/iso22133.h index 380eaa8b0..a3444b7fd 100644 --- a/server/inc/iso22133.h +++ b/server/inc/iso22133.h @@ -250,8 +250,17 @@ typedef struct #pragma pack(pop) -void getSupportedISOProtocolVersions(const uint8_t** supportedProtocolVersions, size_t* nProtocols); +typedef enum { + MESSAGE_OK, + MESSAGE_LENGTH_ERROR, + MESSAGE_CRC_ERROR, + MESSAGE_VERSION_ERROR, + MESSAGE_VALUE_ID_ERROR, + MESSAGE_SYNC_WORD_ERROR +} ISOMessageReturnValue; +void getSupportedISOProtocolVersions(const uint8_t** supportedProtocolVersions, size_t* nProtocols); +ISOMessageReturnValue buildMONRMessage(const char * MonrData, const size_t length, MONRType * MONRData, char debug); #ifdef __cplusplus } diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 2dea820a1..c63deae09 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -6,8 +6,8 @@ static const uint8_t SupportedProtocolVersions[] = { 2 }; // ************************** static functions -static int32_t buildISOHeader(const char * MessageBuffer, const size_t length, HeaderType * HeaderData, char Debug); -static int32_t buildISOFooter(const char * MessageBuffer, const size_t length, FooterType * HeaderData, char Debug); +static ISOMessageReturnValue buildISOHeader(const char * MessageBuffer, const size_t length, HeaderType * HeaderData, char Debug); +static ISOMessageReturnValue buildISOFooter(const char * MessageBuffer, const size_t length, FooterType * HeaderData, char debug); // ************************** function definitions @@ -17,19 +17,26 @@ void getSupportedISOProtocolVersions(const uint8_t ** supportedProtocolVersions, } - -int32_t buildMONRMessage(const char * MonrData, const size_t length, MONRType * MONRData, char debug) { +/*! + * \brief buildMONRMessage Fills a MONRType struct from a buffer of raw data + * \param MonrData Raw data to be decoded + * \param MONRData Struct to be filled + * \param debug Flag for enabling of debugging + * \return value according to ::ISOMessageReturnValue + */ +ISOMessageReturnValue buildMONRMessage(const char * MonrData, const size_t length, MONRType * MONRData, char debug) { const char *p = MonrData; const uint16_t ExpectedMONRStructSize = (uint16_t) (sizeof (*MONRData) - sizeof (MONRData->header) - sizeof (MONRData->footer.Crc) - sizeof (MONRData->monrStructValueID) - sizeof (MONRData->monrStructContentLength)); + ISOMessageReturnValue retval = MESSAGE_OK; // Decode ISO header - if (buildISOHeader(p, length, &MONRData->header, 0) == -1) { + if ((retval = buildISOHeader(p, length, &MONRData->header, 0)) != MESSAGE_OK) { memset(MONRData, 0, sizeof (*MONRData)); - return -1; + return retval; } p += sizeof (MONRData->header); @@ -38,21 +45,19 @@ int32_t buildMONRMessage(const char * MonrData, const size_t length, MONRType * p += sizeof (MONRData->monrStructValueID); if (MONRData->monrStructValueID != VALUE_ID_MONR_STRUCT) { - errno = EINVAL; LogMessage(LOG_LEVEL_ERROR, "Attempted to pass non-MONR struct into MONR parsing function"); memset(MONRData, 0, sizeof (*MONRData)); - return -1; + return MESSAGE_VALUE_ID_ERROR; } memcpy(&MONRData->monrStructContentLength, p, sizeof (MONRData->monrStructContentLength)); p += sizeof (MONRData->monrStructContentLength); if (MONRData->monrStructContentLength != ExpectedMONRStructSize) { - errno = EINVAL; LogMessage(LOG_LEVEL_ERROR, "MONR content length %u differs from the expected length %u", MONRData->monrStructContentLength, ExpectedMONRStructSize); memset(MONRData, 0, sizeof (*MONRData)); - return -1; + return MESSAGE_LENGTH_ERROR; } // Decode content @@ -96,9 +101,9 @@ int32_t buildMONRMessage(const char * MonrData, const size_t length, MONRType * p += sizeof (MONRData->errorStatus); // Footer - if (buildISOFooter(p, length-(size_t)(p-MonrData), &MONRData->footer, 0) == -1) { + if ((retval = buildISOFooter(p, length-(size_t)(p-MonrData), &MONRData->footer, 0)) != MESSAGE_OK) { memset(MONRData, 0, sizeof (*MONRData)); - return -1; + return retval; } p += sizeof (MONRData->footer); @@ -127,13 +132,13 @@ int32_t buildMONRMessage(const char * MonrData, const size_t length, MONRType * LogPrint("ErrorStatus = %d", MONRData->errorStatus); } - return 0; + return retval; } -int32_t buildISOHeader(const char* MessageBuffer, const size_t length, HeaderType * HeaderData, char Debug) { +ISOMessageReturnValue buildISOHeader(const char* MessageBuffer, const size_t length, HeaderType * HeaderData, char Debug) { const char *p = MessageBuffer; - int32_t retval = 0; + ISOMessageReturnValue retval = MESSAGE_OK; const char ProtocolVersionBitmask = 0x7F; char messageProtocolVersion = 0; char isProtocolVersionSupported = 0; @@ -141,10 +146,9 @@ int32_t buildISOHeader(const char* MessageBuffer, const size_t length, HeaderTyp size_t nSupportedProtocols = 0; if (length < sizeof (HeaderData)) { - errno = EINVAL; LogMessage(LOG_LEVEL_ERROR, "Too little raw data to fill ISO header"); memset(HeaderData, 0, sizeof (*HeaderData)); - return -1; + return MESSAGE_LENGTH_ERROR; } // Decode ISO header @@ -152,10 +156,9 @@ int32_t buildISOHeader(const char* MessageBuffer, const size_t length, HeaderTyp p += sizeof (HeaderData->SyncWordU16); if (HeaderData->SyncWordU16 != ISO_SYNC_WORD) { - errno = EINVAL; LogMessage(LOG_LEVEL_ERROR, "Sync word error when decoding ISO header"); memset(HeaderData, 0, sizeof (*HeaderData)); - return -1; + return MESSAGE_SYNC_WORD_ERROR; } memcpy(&HeaderData->TransmitterIdU8, p, sizeof (HeaderData->TransmitterIdU8)); @@ -178,9 +181,10 @@ int32_t buildISOHeader(const char* MessageBuffer, const size_t length, HeaderTyp } if (!isProtocolVersionSupported) { - errno = EPROTONOSUPPORT; LogMessage(LOG_LEVEL_WARNING, "Protocol version %u not supported", messageProtocolVersion); - retval = -1; + retval = MESSAGE_VERSION_ERROR; + memset(HeaderData, 0, sizeof (*HeaderData)); + return retval; } memcpy(&HeaderData->MessageIdU16, p, sizeof (HeaderData->MessageIdU16)); @@ -202,14 +206,15 @@ int32_t buildISOHeader(const char* MessageBuffer, const size_t length, HeaderTyp } -int32_t buildISOFooter(const char* MessageBuffer, const size_t length, FooterType * HeaderData, char Debug) { +ISOMessageReturnValue buildISOFooter(const char* MessageBuffer, const size_t length, FooterType * FooterData, char debug) { - if (length < sizeof (HeaderData->Crc)) { + if (length < sizeof (FooterData->Crc)) { LogMessage(LOG_LEVEL_ERROR, "Too little raw data to fill ISO footer"); - return -1; + memset(FooterData, 0, sizeof (*FooterData)); + return MESSAGE_LENGTH_ERROR; } - memcpy(&HeaderData->Crc, MessageBuffer, sizeof (HeaderData->Crc)); + memcpy(&FooterData->Crc, MessageBuffer, sizeof (FooterData->Crc)); // TODO: check on CRC - return 0; + return MESSAGE_OK; } diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 786918971..ae1192736 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -196,7 +196,6 @@ I32 ObjectControlMONRToASCII(MONRType * MONRData, GeoPosition * OriginPosition, C8 * LongitudinalSpeed, C8 * LateralSpeed, C8 * LongitudinalAcc, C8 * LateralAcc, C8 * Heading, C8 * DriveDirection, C8 * ObjectState, C8 * ReadyToArm, C8 * ErrorStatus, C8 debug); -I32 ObjectControlBuildMONRMessage(C8 * MonrData, const size_t length, MONRType * MONRData, U8 debug); I32 ObjectControlBuildVOILMessage(C8 * MessageBuffer, VOILType * VOILData, C8 * SimData, U8 debug); I32 ObjectControlSendDTMMessage(C8 * DTMData, I32 * Socket, I32 RowCount, C8 * IP, U32 Port, DOTMType * DOTMData, U8 debug); @@ -468,7 +467,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { object_address_name[iIndex], object_udp_port[iIndex], receivedMONRData, buffer); - if (ObjectControlBuildMONRMessage(buffer, receivedMONRData, &MONRData, 0) == -1) { + if (buildMONRMessage(buffer, receivedMONRData, &MONRData, 0) != MESSAGE_OK) { // TODO react on error continue; } @@ -1257,121 +1256,6 @@ I32 ObjectControlBuildVOILMessage(C8 * MessageBuffer, VOILType * VOILData, C8 * } -/*! - * \brief ObjectControlBuildMONRMessage Fills a MONRType struct from a buffer of raw data - * \param MonrData Raw data to be decoded - * \param MONRData Struct to be filled - * \param debug Flag for enabling of debugging - * \return 0 on success, -1 otherwise - */ -I32 ObjectControlBuildMONRMessage(C8 * MonrData, const size_t length, MONRType * MONRData, U8 debug) { - C8 *p = MonrData; - const U16 ExpectedMONRStructSize = (U16) (sizeof (*MONRData) - sizeof (MONRData->Header) - - sizeof (MONRData->CRC) - - sizeof (MONRData->MonrStructValueIdU16) - - sizeof (MONRData->MonrStructContentLengthU16)); - - if (UtilISOBuildHeader(MonrData, length, &MONRData->Header, 0) == -1) { - memset(MONRData, 0, sizeof (*MONRData)); - return -1; - } - p += sizeof (MONRData->Header); - - // Decode content header - memcpy(&MONRData->MonrStructValueIdU16, p, sizeof (MONRData->MonrStructValueIdU16)); - p += sizeof (MONRData->MonrStructValueIdU16); - - if (MONRData->MonrStructValueIdU16 != VALUE_ID_MONR_STRUCT) { - errno = EINVAL; - LogMessage(LOG_LEVEL_ERROR, "Attempted to pass non-MONR struct into MONR parsing function"); - memset(MONRData, 0, sizeof (*MONRData)); - return -1; - } - - memcpy(&MONRData->MonrStructContentLengthU16, p, sizeof (MONRData->MonrStructContentLengthU16)); - p += sizeof (MONRData->MonrStructContentLengthU16); - - if (MONRData->MonrStructContentLengthU16 != ExpectedMONRStructSize) { - errno = EINVAL; - LogMessage(LOG_LEVEL_ERROR, "MONR content length %u differs from the expected length %u", - MONRData->MonrStructContentLengthU16, ExpectedMONRStructSize); - memset(MONRData, 0, sizeof (*MONRData)); - return -1; - } - - // Decode content - memcpy(&MONRData->GPSQmsOfWeekU32, p, sizeof (MONRData->GPSQmsOfWeekU32)); - p += sizeof (MONRData->GPSQmsOfWeekU32); - - memcpy(&MONRData->XPositionI32, p, sizeof (MONRData->XPositionI32)); - p += sizeof (MONRData->XPositionI32); - - memcpy(&MONRData->YPositionI32, p, sizeof (MONRData->YPositionI32)); - p += sizeof (MONRData->YPositionI32); - - memcpy(&MONRData->ZPositionI32, p, sizeof (MONRData->ZPositionI32)); - p += sizeof (MONRData->ZPositionI32); - - memcpy(&MONRData->HeadingU16, p, sizeof (MONRData->HeadingU16)); - p += sizeof (MONRData->HeadingU16); - - memcpy(&MONRData->LongitudinalSpeedI16, p, sizeof (MONRData->LongitudinalSpeedI16)); - p += sizeof (MONRData->LongitudinalSpeedI16); - - memcpy(&MONRData->LateralSpeedI16, p, sizeof (MONRData->LateralSpeedI16)); - p += sizeof (MONRData->LateralSpeedI16); - - memcpy(&MONRData->LongitudinalAccI16, p, sizeof (MONRData->LongitudinalAccI16)); - p += sizeof (MONRData->LongitudinalAccI16); - - memcpy(&MONRData->LateralAccI16, p, sizeof (MONRData->LateralAccI16)); - p += sizeof (MONRData->LateralAccI16); - - memcpy(&MONRData->DriveDirectionU8, p, sizeof (MONRData->DriveDirectionU8)); - p += sizeof (MONRData->DriveDirectionU8); - - memcpy(&MONRData->StateU8, p, sizeof (MONRData->StateU8)); - p += sizeof (MONRData->StateU8); - - memcpy(&MONRData->ReadyToArmU8, p, sizeof (MONRData->ReadyToArmU8)); - p += sizeof (MONRData->ReadyToArmU8); - - memcpy(&MONRData->ErrorStatusU8, p, sizeof (MONRData->ErrorStatusU8)); - p += sizeof (MONRData->ErrorStatusU8); - - // Footer - memcpy(&MONRData->CRC, p, sizeof (MONRData->CRC)); - p += sizeof (MONRData->CRC); - - // TODO: check on CRC - - if (debug == 1) { - LogPrint("MONR:"); - LogPrint("SyncWord = %x", MONRData->Header.SyncWordU16); - LogPrint("TransmitterId = %d", MONRData->Header.TransmitterIdU8); - LogPrint("PackageCounter = %d", MONRData->Header.MessageCounterU8); - LogPrint("AckReq = %d", MONRData->Header.AckReqProtVerU8); - LogPrint("MessageId = %d", MONRData->Header.MessageIdU16); - LogPrint("MessageLength = %d", MONRData->Header.MessageLengthU32); - LogPrint("ValueId = %d", MONRData->MonrStructValueIdU16); - LogPrint("ContentLength = %d", MONRData->MonrStructContentLengthU16); - LogPrint("GPSSOW = %d", MONRData->GPSQmsOfWeekU32); - LogPrint("XPosition = %d", MONRData->XPositionI32); - LogPrint("YPosition = %d", MONRData->YPositionI32); - LogPrint("ZPosition = %d", MONRData->ZPositionI32); - LogPrint("Heading = %d", MONRData->HeadingU16); - LogPrint("LongitudinalSpeed = %d", MONRData->LongitudinalSpeedI16); - LogPrint("LateralSpeed = %d", MONRData->LateralSpeedI16); - LogPrint("LongitudinalAcc = %d", MONRData->LongitudinalAccI16); - LogPrint("LateralAcc = %d", MONRData->LateralAccI16); - LogPrint("DriveDirection = %d", MONRData->DriveDirectionU8); - LogPrint("State = %d", MONRData->StateU8); - LogPrint("ReadyToArm = %d", MONRData->ReadyToArmU8); - LogPrint("ErrorStatus = %d", MONRData->ErrorStatusU8); - } - - return 0; -} I32 ObjectControlMONRToASCII(MONRType * MONRData, GeoPosition * OriginPosition, I32 Idn, C8 * Id, From 86303f1bd1b72d97ad5f1a118a1065a498d66419 Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Mon, 20 Jan 2020 14:23:26 +0100 Subject: [PATCH 075/523] get/set object count --- server/inc/datadictionary.h | 3 +++ server/inc/util.h | 1 + server/src/datadictionary.c | 33 +++++++++++++++++++++++++++++++++ 3 files changed, 37 insertions(+) diff --git a/server/inc/datadictionary.h b/server/inc/datadictionary.h index 339911fda..02e598b1c 100644 --- a/server/inc/datadictionary.h +++ b/server/inc/datadictionary.h @@ -124,6 +124,9 @@ ReadWriteAccess_t DataDictionaryInitMONR(GSDType *GSD, U8 objectCount); ReadWriteAccess_t DataDictionarySetMONR(GSDType *GSD, MONRType * MONR, U8 index); ReadWriteAccess_t DataDictionaryGetMONR(GSDType *GSD, MONRType * MONR, U8 index); +ReadWriteAccess_t DataDictionarySetNumberOfObjectsU8(GSDType * GSD, U8 *NumberOfObjects); +ReadWriteAccess_t DataDictionaryNumberOfObjectsU8(GSDType * GSD, U8 *NumberOfObjects); + /*END of MONR*/ ReadWriteAccess_t DataDictionaryConstructor(GSDType *GSD); diff --git a/server/inc/util.h b/server/inc/util.h index 138ec8de2..b97aacfaa 100644 --- a/server/inc/util.h +++ b/server/inc/util.h @@ -533,6 +533,7 @@ typedef struct U8 HEABSizeU8; U8 HEABData[100]; + U8 numberOfObjects; MONRType *MonrMessages; //U8 OSTMSizeU8; //U8 OSTMData[100]; diff --git a/server/src/datadictionary.c b/server/src/datadictionary.c index b838db369..867b910ab 100644 --- a/server/src/datadictionary.c +++ b/server/src/datadictionary.c @@ -44,6 +44,7 @@ static pthread_mutex_t ASPDataMutex = PTHREAD_MUTEX_INITIALIZER; static pthread_mutex_t MiscDataMutex = PTHREAD_MUTEX_INITIALIZER; static pthread_mutex_t OBCStateMutex = PTHREAD_MUTEX_INITIALIZER; static pthread_mutex_t MONRMutex = PTHREAD_MUTEX_INITIALIZER; +static pthread_mutex_t numberOfObjectsMutex = PTHREAD_MUTEX_INITIALIZER; @@ -1753,6 +1754,38 @@ ReadWriteAccess_t DataDictionaryGetMONR(GSDType * GSD, MONRType *MONR, U8 id) { /*END of MONR*/ +/*NbrOfObjects*/ +/*! + * \brief DataDictionarySetOBCStateU8 Parses input variable and sets variable to corresponding value + * \param GSD Pointer to shared allocated memory + * \param OBCState + * \return Result according to ::ReadWriteAccess_t + */ +ReadWriteAccess_t DataDictionarySetNumberOfObjectsU8(GSDType * GSD, U8 NumberOfObjects) { + ReadWriteAccess_t Res; + + Res = WRITE_OK; + pthread_mutex_lock(&numberOfObjectsMutex); + GSD->numberOfObjects = NumberOfObjects; + pthread_mutex_unlock(&numberOfObjectsMutex); + return Res; +} + +/*! + * \brief DataDictionaryGetOBCStateU8 Reads variable from shared memory + * \param GSD Pointer to shared allocated memory + * \return Current object control state according to ::OBCState_t + */ +U8 DataDictionaryNumberOfObjectsU8(GSDType * GSD) { + U8 Ret; + pthread_mutex_lock(&numberOfObjectsMutex); + Ret = GSD->OBCStateU8; + pthread_mutex_unlock(&numberOfObjectsMutex); + return Ret; +} + +/*END of NbrOfObjects*/ + /*! * \brief DataDictionarySearchParameter Searches for parameters in the configuration file and returns From 5eb30feda3035b717b4784005ea02bc2c438527a Mon Sep 17 00:00:00 2001 From: alfaro01 Date: Mon, 20 Jan 2020 15:05:14 +0100 Subject: [PATCH 076/523] formatting --- modules/Visualization/src/main.c | 111 +++++++++++++++---------------- 1 file changed, 52 insertions(+), 59 deletions(-) diff --git a/modules/Visualization/src/main.c b/modules/Visualization/src/main.c index 8b220173d..eaa72bf14 100644 --- a/modules/Visualization/src/main.c +++ b/modules/Visualization/src/main.c @@ -31,39 +31,40 @@ ------------------------------------------------------------*/ static void vConnectVisualizationChannel(int *sockfd, struct sockaddr_in *addr); static void vDisconnectVisualizationChannel(int *sockfd); -void vCreateVisualizationMessage(MonitorDataType *_monitorData, char *_visualizationMessage, int _sizeOfVisualizationMessage, int _debug); - -void vCreateVisualizationMessage(MonitorDataType *_monitorData, char *_visualizationMessage, int _sizeOfVisualizationMessage, int _debug) -{ - - //IP - char ipStringBuffer[INET_ADDRSTRLEN]; - sprintf(ipStringBuffer, "%s", inet_ntop(AF_INET, &_monitorData->ClientIP, ipStringBuffer, sizeof (ipStringBuffer))); - - //Build message from MonitorStruct - snprintf(_visualizationMessage, _sizeOfVisualizationMessage, "%s;%u;%d;%d;%d;%u;%d;%u;", - ipStringBuffer, - _monitorData->MONR.GPSQmsOfWeekU32, - _monitorData->MONR.XPositionI32, - _monitorData->MONR.YPositionI32, - _monitorData->MONR.ZPositionI32, - _monitorData->MONR.HeadingU16, - _monitorData->MONR.LongitudinalSpeedI16, - _monitorData->MONR.StateU8); - - - if(_debug) - { - //LogMessage(LOG_LEVEL_INFO, "%s", _visualizationMessage); - LogMessage(LOG_LEVEL_INFO, "IP: %s", ipStringBuffer); - LogMessage(LOG_LEVEL_INFO, "GPSQmsOfWeek: %u", _monitorData->MONR.GPSQmsOfWeekU32); - LogMessage(LOG_LEVEL_INFO, "X: %d", _monitorData->MONR.XPositionI32); - LogMessage(LOG_LEVEL_INFO, "Y: %d", _monitorData->MONR.YPositionI32); - LogMessage(LOG_LEVEL_INFO, "Z: %d", _monitorData->MONR.ZPositionI32); - LogMessage(LOG_LEVEL_INFO, "Heading: %u", _monitorData->MONR.HeadingU16); - LogMessage(LOG_LEVEL_INFO, "LongSpeed: %d", _monitorData->MONR.LongitudinalSpeedI16); - LogMessage(LOG_LEVEL_INFO, "State: %u", _monitorData->MONR.StateU8); - } +void vCreateVisualizationMessage(MonitorDataType * _monitorData, char *_visualizationMessage, + int _sizeOfVisualizationMessage, int _debug); + +void vCreateVisualizationMessage(MonitorDataType * _monitorData, char *_visualizationMessage, + int _sizeOfVisualizationMessage, int _debug) { + + //IP + char ipStringBuffer[INET_ADDRSTRLEN]; + + sprintf(ipStringBuffer, "%s", + inet_ntop(AF_INET, &_monitorData->ClientIP, ipStringBuffer, sizeof (ipStringBuffer))); + + //Build message from MonitorStruct + snprintf(_visualizationMessage, _sizeOfVisualizationMessage, "%s;%u;%d;%d;%d;%u;%d;%u;", + ipStringBuffer, + _monitorData->MONR.GPSQmsOfWeekU32, + _monitorData->MONR.XPositionI32, + _monitorData->MONR.YPositionI32, + _monitorData->MONR.ZPositionI32, + _monitorData->MONR.HeadingU16, + _monitorData->MONR.LongitudinalSpeedI16, _monitorData->MONR.StateU8); + + + if (_debug) { + //LogMessage(LOG_LEVEL_INFO, "%s", _visualizationMessage); + LogMessage(LOG_LEVEL_INFO, "IP: %s", ipStringBuffer); + LogMessage(LOG_LEVEL_INFO, "GPSQmsOfWeek: %u", _monitorData->MONR.GPSQmsOfWeekU32); + LogMessage(LOG_LEVEL_INFO, "X: %d", _monitorData->MONR.XPositionI32); + LogMessage(LOG_LEVEL_INFO, "Y: %d", _monitorData->MONR.YPositionI32); + LogMessage(LOG_LEVEL_INFO, "Z: %d", _monitorData->MONR.ZPositionI32); + LogMessage(LOG_LEVEL_INFO, "Heading: %u", _monitorData->MONR.HeadingU16); + LogMessage(LOG_LEVEL_INFO, "LongSpeed: %d", _monitorData->MONR.LongitudinalSpeedI16); + LogMessage(LOG_LEVEL_INFO, "State: %u", _monitorData->MONR.StateU8); + } } int main() { @@ -73,18 +74,10 @@ int main() { const struct timespec abortWaitTime = { 1, 0 }; struct timespec remTime; - MonitorDataType monitorData; + MonitorDataType monitorData; - int sizeOfVisualizationMessage = (INET_ADDRSTRLEN + - sizeof (monitorData.MONR.GPSQmsOfWeekU32) + - sizeof (monitorData.MONR.XPositionI32) + - sizeof (monitorData.MONR.YPositionI32) + - sizeof (monitorData.MONR.ZPositionI32) + - sizeof (monitorData.MONR.HeadingU16) + - sizeof (monitorData.MONR.LongitudinalSpeedI16) + - sizeof (monitorData.MONR.StateU8) + - 8 + //Number of fields + 1 (;) - 1); //Required + int sizeOfVisualizationMessage = (INET_ADDRSTRLEN + sizeof (monitorData.MONR.GPSQmsOfWeekU32) + sizeof (monitorData.MONR.XPositionI32) + sizeof (monitorData.MONR.YPositionI32) + sizeof (monitorData.MONR.ZPositionI32) + sizeof (monitorData.MONR.HeadingU16) + sizeof (monitorData.MONR.LongitudinalSpeedI16) + sizeof (monitorData.MONR.StateU8) + 8 + //Number of fields + 1 (;) + 1); //Required LogInit(MODULE_NAME, LOG_LEVEL_DEBUG); LogMessage(LOG_LEVEL_INFO, "Task running with PID: %u", getpid()); @@ -94,7 +87,7 @@ int main() { vConnectVisualizationChannel(&visual_server, &visual_server_addr); - I32 iExit = 0; + I32 iExit = 0; // Initialize message bus connection @@ -125,26 +118,26 @@ int main() { case COMM_INIT: break; - case COMM_MONR: - { + case COMM_MONR: + { - //Populate the monitorType - UtilPopulateMonitorDataStruct(mqRecvData, (size_t) (sizeof (mqRecvData)), &monitorData, 0); + //Populate the monitorType + UtilPopulateMonitorDataStruct(mqRecvData, (size_t)(sizeof (mqRecvData)), &monitorData, 0); - //Allocate memory - char *visualizationMessage = malloc(sizeOfVisualizationMessage * sizeof (char)); + //Allocate memory + char *visualizationMessage = malloc(sizeOfVisualizationMessage * sizeof (char)); - //Create visualization message and insert values from the monitor datastruct above - vCreateVisualizationMessage(&monitorData, visualizationMessage, sizeOfVisualizationMessage, 0); + //Create visualization message and insert values from the monitor datastruct above + vCreateVisualizationMessage(&monitorData, visualizationMessage, sizeOfVisualizationMessage, 0); - //Send visualization message on the UDP socket - UtilSendUDPData((const uint8_t *)"Visualization", &visual_server, &visual_server_addr, visualizationMessage, - sizeOfVisualizationMessage, 0); + //Send visualization message on the UDP socket + UtilSendUDPData((const uint8_t *)"Visualization", &visual_server, &visual_server_addr, + visualizationMessage, sizeOfVisualizationMessage, 0); - //Free memory used by malloc - free(visualizationMessage); + //Free memory used by malloc + free(visualizationMessage); - } + } break; case COMM_LOG: break; From d1a16d682ce71c97bb8a4606bd6b5fca1d6a3ab6 Mon Sep 17 00:00:00 2001 From: alfaro01 Date: Mon, 20 Jan 2020 15:10:29 +0100 Subject: [PATCH 077/523] Formatting again --- server/src/logger.c | 10 +++++----- server/src/main.c | 3 +-- server/src/objectcontrol.c | 5 ++--- server/src/systemcontrol.c | 6 +++--- server/src/util.c | 8 +++++--- 5 files changed, 16 insertions(+), 16 deletions(-) diff --git a/server/src/logger.c b/server/src/logger.c index 01c6f4612..ef569eb43 100644 --- a/server/src/logger.c +++ b/server/src/logger.c @@ -574,7 +574,7 @@ void vLogMonitorData(char *commandData, ssize_t commandDatalen, struct timeval r TimeSetToCurrentSystemTime(&systemTime); - UtilPopulateMonitorDataStruct(commandData, (size_t) (commandDatalen), &monitorData, debug); + UtilPopulateMonitorDataStruct(commandData, (size_t)(commandDatalen), &monitorData, debug); TimeSetToGPStime(&monrTime, TimeGetAsGPSweek(&systemTime), monitorData.MONR.GPSQmsOfWeekU32); bzero(DateBuffer, sizeof (DateBuffer)); @@ -638,21 +638,21 @@ void vLogScenarioControlData(enum COMMAND command, unsigned char *commandData, s switch (command) { case COMM_TREO: - UtilPopulateTREODataStructFromMQ(commandData, (size_t) commandDatalen, &treo); + UtilPopulateTREODataStructFromMQ(commandData, (size_t)commandDatalen, &treo); LogMessage(LOG_LEVEL_INFO, "Trigger event occurred, ID %u", treo.triggerID); TimeSetToGPStime(&messageTimeField, TimeGetAsGPSweek(&systemTime), treo.timestamp_qmsow); fprintf(filefd, "%u;%ld;", treo.triggerID, TimeGetAsGPSms(&messageTimeField)); fprintf(filefd, "%s", inet_ntop(AF_INET, &treo.ip, ipStringBuffer, sizeof (ipStringBuffer))); break; case COMM_EXAC: - UtilPopulateEXACDataStructFromMQ(commandData, (size_t) commandDatalen, &exac); + UtilPopulateEXACDataStructFromMQ(commandData, (size_t)commandDatalen, &exac); LogMessage(LOG_LEVEL_INFO, "Action execute request detected, ID %u", exac.actionID); TimeSetToGPStime(&messageTimeField, TimeGetAsGPSweek(&systemTime), exac.executionTime_qmsoW); fprintf(filefd, "%u;%ld;", exac.actionID, TimeGetAsGPSms(&messageTimeField)); fprintf(filefd, "%s", inet_ntop(AF_INET, &exac.ip, ipStringBuffer, sizeof (ipStringBuffer))); break; case COMM_TRCM: - UtilPopulateTRCMDataStructFromMQ(commandData, (size_t) commandDatalen, &trcm); + UtilPopulateTRCMDataStructFromMQ(commandData, (size_t)commandDatalen, &trcm); LogMessage(LOG_LEVEL_INFO, "Trigger configuration for ID %u received, of type %u", trcm.triggerID, trcm.triggerType); fprintf(filefd, "%u;%u;%u;%u;%u", trcm.triggerID, trcm.triggerType, trcm.triggerTypeParameter1, @@ -660,7 +660,7 @@ void vLogScenarioControlData(enum COMMAND command, unsigned char *commandData, s fprintf(filefd, "%s", inet_ntop(AF_INET, &trcm.ip, ipStringBuffer, sizeof (ipStringBuffer))); break; case COMM_ACCM: - UtilPopulateACCMDataStructFromMQ(commandData, (size_t) commandDatalen, &accm); + UtilPopulateACCMDataStructFromMQ(commandData, (size_t)commandDatalen, &accm); LogMessage(LOG_LEVEL_INFO, "Action configuration for ID %u received, of type %u", accm.actionID, accm.actionType); fprintf(filefd, "%u;%u;%u;%u;%u", accm.actionID, accm.actionType, accm.actionTypeParameter1, diff --git a/server/src/main.c b/server/src/main.c index eca12b059..2146b136f 100644 --- a/server/src/main.c +++ b/server/src/main.c @@ -41,7 +41,7 @@ /*------------------------------------------------------------ -- Types ------------------------------------------------------------*/ -typedef void (*ModuleTask) (TimeType *, GSDType *, LOG_LEVEL); //!< Function pointer type for module "main" functions +typedef void (*ModuleTask)(TimeType *, GSDType *, LOG_LEVEL); //!< Function pointer type for module "main" functions typedef struct { LOG_LEVEL commonLogLevel; //!< Logging level of the server. int extraMessageQueues; //!< Number of extra message queues to create on startup. @@ -161,7 +161,6 @@ int main(int argc, char *argv[]) { if (shutdownMessageQueueBus()) util_error("Unable to successfully clean up message bus resources"); else - exit(EXIT_SUCCESS); } diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 87542be21..3e0be4f62 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -475,8 +475,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { sizeof (in_addr_t)); // Send MONR message as bytes - if (iCommSend(COMM_MONR, buffer, (size_t) (receivedMONRData) + sizeof (in_addr_t)) < - 0) { + if (iCommSend(COMM_MONR, buffer, (size_t)(receivedMONRData) + sizeof (in_addr_t)) < 0) { LogMessage(LOG_LEVEL_ERROR, "Fatal communication fault when sending MONR command - entering error state"); vSetState(OBC_STATE_ERROR, GSD); @@ -3094,7 +3093,7 @@ static size_t uiRecvMonitor(int *sockfd, char *buffer, size_t length) { } } else { - recvDataSize = (size_t) (result); + recvDataSize = (size_t)(result); LogMessage(LOG_LEVEL_DEBUG, "Received: <%s>", buffer); } } while (result > 0); diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index 0a5f9259d..db0ae83bc 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -567,7 +567,7 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { case COMM_MONR: // TODO: Decode if (RVSSChannelSocket != 0 && RVSSConfigU32 & RVSS_MONR_CHANNEL && bytesReceived >= 0) { - UtilPopulateMonitorDataStruct(pcRecvBuffer, (size_t) bytesReceived, &monrData, 0); + UtilPopulateMonitorDataStruct(pcRecvBuffer, (size_t)bytesReceived, &monrData, 0); SystemControlBuildRVSSMONRChannelMessage(RVSSData, &RVSSMessageLengthU32, monrData, 0); UtilSendUDPData("SystemControl", &RVSSChannelSocket, &RVSSChannelAddr, RVSSData, RVSSMessageLengthU32, 0); @@ -1189,13 +1189,13 @@ ssize_t SystemControlReceiveUserControlData(I32 socket, C8 * dataBuffer, size_t readResult = recv(socket, recvBuffer + bytesInBuffer, sizeof (recvBuffer) - bytesInBuffer, MSG_DONTWAIT); if (readResult > 0) { - bytesInBuffer += (size_t) readResult; + bytesInBuffer += (size_t)readResult; } if (bytesInBuffer > 0) { if ((endOfMessage = strstr(recvBuffer, endOfMessagePattern)) != NULL) { endOfMessage += sizeof (endOfMessagePattern) - 1; - messageLength = (size_t) (endOfMessage - recvBuffer); + messageLength = (size_t)(endOfMessage - recvBuffer); } else { messageLength = 0; diff --git a/server/src/util.c b/server/src/util.c index ed2634a18..09298dddd 100644 --- a/server/src/util.c +++ b/server/src/util.c @@ -321,7 +321,7 @@ uint64_t UtilgetETSIfromUTCMS(uint64_t utc_sec, uint64_t utc_usec) { void UtilgetDateTimeFromUTCtime(int64_t utc_ms, char *buffer, int size_t) { time_t time_seconds = utc_ms / 1000; - if (size_t < 26) + if (size_t <26) return; strcpy(buffer, ctime(&time_seconds)); } @@ -338,6 +338,7 @@ void UtilgetDateTimefromUTCCSVformat(int64_t utc_ms, char *buffer, int size_t) { ms = round(tmp_ms * 1000); strftime(buffer, size_t, "%Y;%m;%d;%H;%M;%S;", &date_time); + sprintf(tmp_buffer_ms, "%" PRIi64, ms); strcat(buffer, tmp_buffer_ms); } @@ -354,6 +355,7 @@ void UtilgetDateTimeFromUTCForMapNameCreation(int64_t utc_ms, char *buffer, int ms = round(tmp_ms * 1000); strftime(buffer, size_t, "%Y-%m-%d_%H:%M:%S:", &date_time); + sprintf(tmp_buffer_ms, "%" PRIi64, ms); strcat(buffer, tmp_buffer_ms); } @@ -1902,7 +1904,7 @@ int iCommClose() { * \param timeRecv Receive time output variable * \return Size (in bytes) of received data */ -ssize_t iCommRecv(enum COMMAND * command, char *data, const size_t messageSize, struct timeval * timeRecv) { +ssize_t iCommRecv(enum COMMAND *command, char *data, const size_t messageSize, struct timeval *timeRecv) { char message[MQ_MSG_SIZE]; ssize_t result = MQBusRecv(message, MQ_MSG_SIZE); size_t dataLength = 0; @@ -1921,7 +1923,7 @@ ssize_t iCommRecv(enum COMMAND * command, char *data, const size_t messageSize, *command = (unsigned char)message[0]; memcpy(&dataLength, message + sizeof (char), sizeof (dataLength)); - if (dataLength != (size_t) (result)) { + if (dataLength != (size_t)(result)) { LogMessage(LOG_LEVEL_ERROR, "Received message with invalid length specification field: %d bytes, but %d bytes were received", dataLength, result); From c800495f92ad7ad887a0894478475430fdd27e2f Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 20 Jan 2020 15:14:45 +0100 Subject: [PATCH 078/523] Moved ASCII printer function to iso22133.c --- server/inc/iso22133.h | 13 ++- server/inc/util.h | 1 - server/src/iso22133.c | 211 ++++++++++++++++++++++++++---------------- server/src/util.c | 83 +---------------- 4 files changed, 146 insertions(+), 162 deletions(-) diff --git a/server/inc/iso22133.h b/server/inc/iso22133.h index a3444b7fd..fed36bcf1 100644 --- a/server/inc/iso22133.h +++ b/server/inc/iso22133.h @@ -253,14 +253,25 @@ typedef struct typedef enum { MESSAGE_OK, MESSAGE_LENGTH_ERROR, + MESSAGE_TYPE_ERROR, MESSAGE_CRC_ERROR, MESSAGE_VERSION_ERROR, MESSAGE_VALUE_ID_ERROR, MESSAGE_SYNC_WORD_ERROR } ISOMessageReturnValue; +typedef enum { + MESSAGE_ID_MONR = 0x0006, + MESSAGE_ID_TRCM = 0x0011, + MESSAGE_ID_ACCM = 0x0012, + MESSAGE_ID_TREO = 0x0013, + MESSAGE_ID_EXAC = 0x0014, + MESSAGE_ID_CATA = 0x0015 +} ISOMessageID; + void getSupportedISOProtocolVersions(const uint8_t** supportedProtocolVersions, size_t* nProtocols); -ISOMessageReturnValue buildMONRMessage(const char * MonrData, const size_t length, MONRType * MONRData, char debug); +ISOMessageReturnValue buildMONRMessage(const char * MonrData, const size_t length, MONRType * MONRData, const char debug); +ISOMessageReturnValue MONRToASCII(const MONRType * MONRData, char * asciiBuffer, const size_t bufferLength, const char debug); #ifdef __cplusplus } diff --git a/server/inc/util.h b/server/inc/util.h index 748f0803c..6a12efbb0 100644 --- a/server/inc/util.h +++ b/server/inc/util.h @@ -875,7 +875,6 @@ U32 UtilHexTextToBinary(U32 DataLength, C8 *Text, C8 *Binary, U8 Debug); U32 UtilCreateDirContent(C8* DirPath, C8* TempPath); U16 UtilGetMillisecond(TimeType *GPSTime); -I32 UtilISOBuildHeader(C8 *MessageBuffer, const size_t length, HeaderType *HeaderData, U8 Debug); I32 UtilISOBuildINSUPMessage(C8* MessageBuffer, INSUPType *INSUPData, C8 CommandOption, U8 Debug); I32 UtilISOBuildHEABMessage(C8* MessageBuffer, HEABType *HEABData, TimeType *GPSTime, U8 CCStatus, U8 Debug); I32 UtilISOBuildTRAJMessageHeader(C8* MessageBuffer, I32 RowCount, HeaderType *HeaderData, TRAJInfoType *TRAJInfoData, U8 Debug); diff --git a/server/src/iso22133.c b/server/src/iso22133.c index c63deae09..34d3db933 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -6,8 +6,8 @@ static const uint8_t SupportedProtocolVersions[] = { 2 }; // ************************** static functions -static ISOMessageReturnValue buildISOHeader(const char * MessageBuffer, const size_t length, HeaderType * HeaderData, char Debug); -static ISOMessageReturnValue buildISOFooter(const char * MessageBuffer, const size_t length, FooterType * HeaderData, char debug); +static ISOMessageReturnValue buildISOHeader(const char * MessageBuffer, const size_t length, HeaderType * HeaderData, const char debug); +static ISOMessageReturnValue buildISOFooter(const char * MessageBuffer, const size_t length, FooterType * HeaderData, const char debug); // ************************** function definitions @@ -16,6 +16,104 @@ void getSupportedISOProtocolVersions(const uint8_t ** supportedProtocolVersions, *nProtocols = sizeof (SupportedProtocolVersions) / sizeof (SupportedProtocolVersions[0]); } +/*! + * \brief buildISOHeader Convert data in a buffer to an ISO header + * \param MessageBuffer Buffer containing raw data to be converted + * \param length Length of buffer + * \param HeaderData Struct in which to store resulting data + * \param debug Flag for enabling debugging of this function + * \return value according to ::ISOMessageReturnValue + */ +ISOMessageReturnValue buildISOHeader(const char* MessageBuffer, const size_t length, HeaderType * HeaderData, const char debug) { + const char *p = MessageBuffer; + ISOMessageReturnValue retval = MESSAGE_OK; + const char ProtocolVersionBitmask = 0x7F; + char messageProtocolVersion = 0; + char isProtocolVersionSupported = 0; + const uint8_t *supportedProtocolVersions; + size_t nSupportedProtocols = 0; + + if (length < sizeof (HeaderData)) { + LogMessage(LOG_LEVEL_ERROR, "Too little raw data to fill ISO header"); + memset(HeaderData, 0, sizeof (*HeaderData)); + return MESSAGE_LENGTH_ERROR; + } + + // Decode ISO header + memcpy(&HeaderData->SyncWordU16, p, sizeof (HeaderData->SyncWordU16)); + p += sizeof (HeaderData->SyncWordU16); + + if (HeaderData->SyncWordU16 != ISO_SYNC_WORD) { + LogMessage(LOG_LEVEL_ERROR, "Sync word error when decoding ISO header"); + memset(HeaderData, 0, sizeof (*HeaderData)); + return MESSAGE_SYNC_WORD_ERROR; + } + + memcpy(&HeaderData->TransmitterIdU8, p, sizeof (HeaderData->TransmitterIdU8)); + p += sizeof (HeaderData->TransmitterIdU8); + + memcpy(&HeaderData->MessageCounterU8, p, sizeof (HeaderData->MessageCounterU8)); + p += sizeof (HeaderData->MessageCounterU8); + + memcpy(&HeaderData->AckReqProtVerU8, p, sizeof (HeaderData->AckReqProtVerU8)); + p += sizeof (HeaderData->AckReqProtVerU8); + + // Loop over permitted protocol versions + messageProtocolVersion = HeaderData->AckReqProtVerU8 & ProtocolVersionBitmask; + getSupportedISOProtocolVersions(&supportedProtocolVersions, &nSupportedProtocols); + for (size_t i = 0; i < nSupportedProtocols; ++i) { + if (supportedProtocolVersions[i] == messageProtocolVersion) { + isProtocolVersionSupported = 1; + break; + } + } + + if (!isProtocolVersionSupported) { + LogMessage(LOG_LEVEL_WARNING, "Protocol version %u not supported", messageProtocolVersion); + retval = MESSAGE_VERSION_ERROR; + memset(HeaderData, 0, sizeof (*HeaderData)); + return retval; + } + + memcpy(&HeaderData->MessageIdU16, p, sizeof (HeaderData->MessageIdU16)); + p += sizeof (HeaderData->MessageIdU16); + + memcpy(&HeaderData->MessageLengthU32, p, sizeof (HeaderData->MessageLengthU32)); + p += sizeof (HeaderData->MessageLengthU32); + + if (debug) { + LogPrint("SyncWordU16 = 0x%x", HeaderData->SyncWordU16); + LogPrint("TransmitterIdU8 = 0x%x", HeaderData->TransmitterIdU8); + LogPrint("MessageCounterU8 = 0x%x", HeaderData->MessageCounterU8); + LogPrint("AckReqProtVerU8 = 0x%x", HeaderData->AckReqProtVerU8); + LogPrint("MessageIdU16 = 0x%x", HeaderData->MessageIdU16); + LogPrint("MessageLengthU32 = 0x%x", HeaderData->MessageLengthU32); + } + + return retval; +} + +/*! + * \brief buildISOFooter Convert data in a buffer to an ISO footer + * \param MessageBuffer Buffer containing raw data to be converted + * \param length Length of buffer + * \param HeaderData Struct in which to store resulting data + * \param debug Flag for enabling debugging of this function + * \return value according to ::ISOMessageReturnValue + */ +ISOMessageReturnValue buildISOFooter(const char* MessageBuffer, const size_t length, FooterType * FooterData, const char debug) { + + if (length < sizeof (FooterData->Crc)) { + LogMessage(LOG_LEVEL_ERROR, "Too little raw data to fill ISO footer"); + memset(FooterData, 0, sizeof (*FooterData)); + return MESSAGE_LENGTH_ERROR; + } + memcpy(&FooterData->Crc, MessageBuffer, sizeof (FooterData->Crc)); + + // TODO: check on CRC + return MESSAGE_OK; +} + /*! * \brief buildMONRMessage Fills a MONRType struct from a buffer of raw data @@ -24,7 +122,7 @@ void getSupportedISOProtocolVersions(const uint8_t ** supportedProtocolVersions, * \param debug Flag for enabling of debugging * \return value according to ::ISOMessageReturnValue */ -ISOMessageReturnValue buildMONRMessage(const char * MonrData, const size_t length, MONRType * MONRData, char debug) { +ISOMessageReturnValue buildMONRMessage(const char * MonrData, const size_t length, MONRType * MONRData, const char debug) { const char *p = MonrData; const uint16_t ExpectedMONRStructSize = (uint16_t) (sizeof (*MONRData) - sizeof (MONRData->header) @@ -34,12 +132,17 @@ ISOMessageReturnValue buildMONRMessage(const char * MonrData, const size_t lengt ISOMessageReturnValue retval = MESSAGE_OK; // Decode ISO header - if ((retval = buildISOHeader(p, length, &MONRData->header, 0)) != MESSAGE_OK) { + if ((retval = buildISOHeader(p, length, &MONRData->header, debug)) != MESSAGE_OK) { memset(MONRData, 0, sizeof (*MONRData)); return retval; } p += sizeof (MONRData->header); + if (MONRData->header.MessageIdU16 != MESSAGE_ID_MONR) { + memset(MONRData, 0, sizeof (*MONRData)); + return MESSAGE_TYPE_ERROR; + } + // Decode content header memcpy(&MONRData->monrStructValueID, p, sizeof (MONRData->monrStructValueID)); p += sizeof (MONRData->monrStructValueID); @@ -101,7 +204,7 @@ ISOMessageReturnValue buildMONRMessage(const char * MonrData, const size_t lengt p += sizeof (MONRData->errorStatus); // Footer - if ((retval = buildISOFooter(p, length-(size_t)(p-MonrData), &MONRData->footer, 0)) != MESSAGE_OK) { + if ((retval = buildISOFooter(p, length-(size_t)(p-MonrData), &MONRData->footer, debug)) != MESSAGE_OK) { memset(MONRData, 0, sizeof (*MONRData)); return retval; } @@ -136,85 +239,31 @@ ISOMessageReturnValue buildMONRMessage(const char * MonrData, const size_t lengt } -ISOMessageReturnValue buildISOHeader(const char* MessageBuffer, const size_t length, HeaderType * HeaderData, char Debug) { - const char *p = MessageBuffer; - ISOMessageReturnValue retval = MESSAGE_OK; - const char ProtocolVersionBitmask = 0x7F; - char messageProtocolVersion = 0; - char isProtocolVersionSupported = 0; - const uint8_t *supportedProtocolVersions; - size_t nSupportedProtocols = 0; - - if (length < sizeof (HeaderData)) { - LogMessage(LOG_LEVEL_ERROR, "Too little raw data to fill ISO header"); - memset(HeaderData, 0, sizeof (*HeaderData)); - return MESSAGE_LENGTH_ERROR; - } - - // Decode ISO header - memcpy(&HeaderData->SyncWordU16, p, sizeof (HeaderData->SyncWordU16)); - p += sizeof (HeaderData->SyncWordU16); - - if (HeaderData->SyncWordU16 != ISO_SYNC_WORD) { - LogMessage(LOG_LEVEL_ERROR, "Sync word error when decoding ISO header"); - memset(HeaderData, 0, sizeof (*HeaderData)); - return MESSAGE_SYNC_WORD_ERROR; - } - - memcpy(&HeaderData->TransmitterIdU8, p, sizeof (HeaderData->TransmitterIdU8)); - p += sizeof (HeaderData->TransmitterIdU8); - - memcpy(&HeaderData->MessageCounterU8, p, sizeof (HeaderData->MessageCounterU8)); - p += sizeof (HeaderData->MessageCounterU8); - - memcpy(&HeaderData->AckReqProtVerU8, p, sizeof (HeaderData->AckReqProtVerU8)); - p += sizeof (HeaderData->AckReqProtVerU8); - - // Loop over permitted protocol versions - messageProtocolVersion = HeaderData->AckReqProtVerU8 & ProtocolVersionBitmask; - getSupportedISOProtocolVersions(&supportedProtocolVersions, &nSupportedProtocols); - for (size_t i = 0; i < nSupportedProtocols; ++i) { - if (supportedProtocolVersions[i] == messageProtocolVersion) { - isProtocolVersionSupported = 1; - break; - } - } - - if (!isProtocolVersionSupported) { - LogMessage(LOG_LEVEL_WARNING, "Protocol version %u not supported", messageProtocolVersion); - retval = MESSAGE_VERSION_ERROR; - memset(HeaderData, 0, sizeof (*HeaderData)); - return retval; - } - - memcpy(&HeaderData->MessageIdU16, p, sizeof (HeaderData->MessageIdU16)); - p += sizeof (HeaderData->MessageIdU16); - - memcpy(&HeaderData->MessageLengthU32, p, sizeof (HeaderData->MessageLengthU32)); - p += sizeof (HeaderData->MessageLengthU32); +/*! + * \brief MONRToASCII Converts a MONR struct into human readable ASCII text + * \param MONRData Struct containing MONR data + * \param asciiBuffer Buffer in which to print ASCII text representation + * \param bufferLength Length of ASCII buffer + * \param debug Flag for enabling debugging + * \return value according to ::ISOMessageReturnValue + */ +ISOMessageReturnValue MONRToASCII(const MONRType * MONRData, char * asciiBuffer, const size_t bufferLength, const char debug) { - if (Debug) { - LogPrint("SyncWordU16 = 0x%x", HeaderData->SyncWordU16); - LogPrint("TransmitterIdU8 = 0x%x", HeaderData->TransmitterIdU8); - LogPrint("MessageCounterU8 = 0x%x", HeaderData->MessageCounterU8); - LogPrint("AckReqProtVerU8 = 0x%x", HeaderData->AckReqProtVerU8); - LogPrint("MessageIdU16 = 0x%x", HeaderData->MessageIdU16); - LogPrint("MessageLengthU32 = 0x%x", HeaderData->MessageLengthU32); + memset(asciiBuffer, 0, bufferLength); + if (MONRData->header.MessageIdU16 != MESSAGE_ID_MONR) { + LogMessage(LOG_LEVEL_ERROR, "Attempted to pass non-MONR struct into MONR parsing function"); + return MESSAGE_TYPE_ERROR; } - return retval; -} - + sprintf(asciiBuffer + strlen(asciiBuffer), "%u;", MONRData->gpsQmsOfWeek); + sprintf(asciiBuffer + strlen(asciiBuffer), "%d;%d;%d;%u;", + MONRData->xPosition, MONRData->yPosition, MONRData->zPosition, + MONRData->heading); + sprintf(asciiBuffer + strlen(asciiBuffer), "%d;%d;%d;%d;", MONRData->longitudinalSpeed, + MONRData->lateralSpeed, MONRData->longitudinalAcc, MONRData->lateralAcc); + sprintf(asciiBuffer + strlen(asciiBuffer), "%u;%u;%u;%u;", MONRData->driveDirection, + MONRData->state, MONRData->readyToArm, MONRData->errorStatus); -ISOMessageReturnValue buildISOFooter(const char* MessageBuffer, const size_t length, FooterType * FooterData, char debug) { - - if (length < sizeof (FooterData->Crc)) { - LogMessage(LOG_LEVEL_ERROR, "Too little raw data to fill ISO footer"); - memset(FooterData, 0, sizeof (*FooterData)); - return MESSAGE_LENGTH_ERROR; - } - memcpy(&FooterData->Crc, MessageBuffer, sizeof (FooterData->Crc)); - - // TODO: check on CRC return MESSAGE_OK; } + diff --git a/server/src/util.c b/server/src/util.c index f450d523d..5c2ed2c8d 100644 --- a/server/src/util.c +++ b/server/src/util.c @@ -720,14 +720,10 @@ int UtilMonitorDataToString(MonitorDataType monrData, char *monrString, size_t s inet_ntop(AF_INET, &monrData.ClientIP, monrString, (stringLength > UINT_MAX) ? UINT_MAX : (socklen_t) stringLength); strcat(monrString, ";0;"); - sprintf(monrString + strlen(monrString), "%u;", monrData.MONR.GPSQmsOfWeekU32); - sprintf(monrString + strlen(monrString), "%d;%d;%d;%u;", - monrData.MONR.XPositionI32, monrData.MONR.YPositionI32, monrData.MONR.ZPositionI32, - monrData.MONR.HeadingU16); - sprintf(monrString + strlen(monrString), "%d;%d;%d;%d;", monrData.MONR.LongitudinalSpeedI16, - monrData.MONR.LateralSpeedI16, monrData.MONR.LongitudinalAccI16, monrData.MONR.LateralAccI16); - sprintf(monrString + strlen(monrString), "%u;%u;%u;%u;", monrData.MONR.DriveDirectionU8, - monrData.MONR.StateU8, monrData.MONR.ReadyToArmU8, monrData.MONR.ErrorStatusU8); + if (MONRToASCII(&monrData.MONR, monrString + strlen(monrString), stringLength - strlen(monrString), 0) != MESSAGE_OK) { + memset(monrString, 0, stringLength); + return -1; + } return 0; } @@ -3648,77 +3644,6 @@ I32 UtilISOBuildTRAJMessage(C8 * MessageBuffer, C8 * DTMData, I32 RowCount, DOTM return MessageIndex; //Total number of bytes } -I32 UtilISOBuildHeader(C8 * MessageBuffer, const size_t length, HeaderType * HeaderData, U8 Debug) { - C8 *p = MessageBuffer; - I32 retval = 0; - const U8 ProtocolVersionBitmask = 0x7F; - U8 messageProtocolVersion = 0; - U8 isProtocolVersionSupported = 0; - const uint8_t *supportedProtocolVersions; - size_t nSupportedProtocols = 0; - - if (length < sizeof (HeaderData)) { - errno = EINVAL; - LogMessage(LOG_LEVEL_ERROR, "Too little raw data to fill ISO header"); - memset(HeaderData, 0, sizeof (*HeaderData)); - return -1; - } - - // Decode ISO header - memcpy(&HeaderData->SyncWordU16, p, sizeof (HeaderData->SyncWordU16)); - p += sizeof (HeaderData->SyncWordU16); - - if (HeaderData->SyncWordU16 != ISO_SYNC_WORD) { - errno = EINVAL; - LogMessage(LOG_LEVEL_ERROR, "Sync word error when decoding ISO header"); - memset(HeaderData, 0, sizeof (*HeaderData)); - return -1; - } - - memcpy(&HeaderData->TransmitterIdU8, p, sizeof (HeaderData->TransmitterIdU8)); - p += sizeof (HeaderData->TransmitterIdU8); - - memcpy(&HeaderData->MessageCounterU8, p, sizeof (HeaderData->MessageCounterU8)); - p += sizeof (HeaderData->MessageCounterU8); - - memcpy(&HeaderData->AckReqProtVerU8, p, sizeof (HeaderData->AckReqProtVerU8)); - p += sizeof (HeaderData->AckReqProtVerU8); - - // Loop over permitted protocol versions - messageProtocolVersion = HeaderData->AckReqProtVerU8 & ProtocolVersionBitmask; - getSupportedISOProtocolVersions(&supportedProtocolVersions, &nSupportedProtocols); - for (size_t i = 0; i < nSupportedProtocols; ++i) { - if (supportedProtocolVersions[i] == messageProtocolVersion) { - isProtocolVersionSupported = 1; - break; - } - } - - if (!isProtocolVersionSupported) { - errno = EPROTONOSUPPORT; - LogMessage(LOG_LEVEL_WARNING, "Protocol version %u not supported", messageProtocolVersion); - retval = -1; - } - - memcpy(&HeaderData->MessageIdU16, p, sizeof (HeaderData->MessageIdU16)); - p += sizeof (HeaderData->MessageIdU16); - - memcpy(&HeaderData->MessageLengthU32, p, sizeof (HeaderData->MessageLengthU32)); - p += sizeof (HeaderData->MessageLengthU32); - - if (Debug) { - LogPrint("SyncWordU16 = 0x%x", HeaderData->SyncWordU16); - LogPrint("TransmitterIdU8 = 0x%x", HeaderData->TransmitterIdU8); - LogPrint("MessageCounterU8 = 0x%x", HeaderData->MessageCounterU8); - LogPrint("AckReqProtVerU8 = 0x%x", HeaderData->AckReqProtVerU8); - LogPrint("MessageIdU16 = 0x%x", HeaderData->MessageIdU16); - LogPrint("MessageLengthU32 = 0x%x", HeaderData->MessageLengthU32); - } - - return retval; -} - - /* UtilWriteConfigurationParameter updates parameters in the file test.conf. From 17cb447ad96822cc798daa06a15c56c47be3e93a Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 20 Jan 2020 16:18:29 +0100 Subject: [PATCH 079/523] Removed old MONR to ASCII function --- server/src/objectcontrol.c | 144 +------------------------------------ 1 file changed, 1 insertion(+), 143 deletions(-) diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index ae1192736..18e6f9675 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -191,11 +191,6 @@ I32 ObjectControlBuildTRAJMessage(C8 * MessageBuffer, FILE * fd, I32 RowCount, D I32 ObjectControlSendTRAJMessage(C8 * Filename, I32 * Socket, I32 RowCount, C8 * IP, U32 Port, DOTMType * DOTMData, U8 debug); int ObjectControlSendUDPData(int *sockfd, struct sockaddr_in *addr, char *SendData, int Length, char debug); -I32 ObjectControlMONRToASCII(MONRType * MONRData, GeoPosition * OriginPosition, I32 Idn, C8 * Id, - C8 * Timestamp, C8 * XPosition, C8 * YPosition, C8 * ZPosition, - C8 * LongitudinalSpeed, C8 * LateralSpeed, C8 * LongitudinalAcc, C8 * LateralAcc, - C8 * Heading, C8 * DriveDirection, C8 * ObjectState, C8 * ReadyToArm, - C8 * ErrorStatus, C8 debug); I32 ObjectControlBuildVOILMessage(C8 * MessageBuffer, VOILType * VOILData, C8 * SimData, U8 debug); I32 ObjectControlSendDTMMessage(C8 * DTMData, I32 * Socket, I32 RowCount, C8 * IP, U32 Port, DOTMType * DOTMData, U8 debug); @@ -498,7 +493,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { GSD->MONRSizeU8 = MONRData.Header.MessageLengthU32 + COMMAND_MESSAGE_HEADER_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH; - + MONRToASCII(&MONRData, buffer, sizeof (buffer), 0); ObjectControlMONRToASCII(&MONRData, &OriginPosition, iIndex, Id, Timestamp, XPosition, YPosition, ZPosition, LongitudinalSpeed, LateralSpeed, LongitudinalAcc, LateralAcc, Heading, DriveDirection, @@ -1257,143 +1252,6 @@ I32 ObjectControlBuildVOILMessage(C8 * MessageBuffer, VOILType * VOILData, C8 * } - -I32 ObjectControlMONRToASCII(MONRType * MONRData, GeoPosition * OriginPosition, I32 Idn, C8 * Id, - C8 * Timestamp, C8 * XPosition, C8 * YPosition, C8 * ZPosition, - C8 * LongitudinalSpeed, C8 * LateralSpeed, C8 * LongitudinalAcc, C8 * LateralAcc, - C8 * Heading, C8 * DriveDirection, C8 * ObjectState, C8 * ReadyToArm, - C8 * ErrorStatus, C8 debug) { - char Buffer[6]; - long unsigned int MonrValueU64; - unsigned int MonrValueU32; - unsigned short MonrValueU16; - unsigned char MonrValueU8; - double iLlh[3] = { 0, 0, 0 }; - double xyz[3] = { 0, 0, 0 }; - double Llh[3] = { 0, 0, 0 }; - uint64_t ConvertGPStoUTC; - - bzero(Id, SMALL_BUFFER_SIZE_1); - bzero(Timestamp, SMALL_BUFFER_SIZE_0); - bzero(XPosition, SMALL_BUFFER_SIZE_0); - bzero(YPosition, SMALL_BUFFER_SIZE_0); - bzero(ZPosition, SMALL_BUFFER_SIZE_0); - bzero(LongitudinalSpeed, SMALL_BUFFER_SIZE_0); - bzero(LateralSpeed, SMALL_BUFFER_SIZE_0); - bzero(LongitudinalAcc, SMALL_BUFFER_SIZE_0); - bzero(LateralAcc, SMALL_BUFFER_SIZE_0); - bzero(Heading, SMALL_BUFFER_SIZE_0); - bzero(DriveDirection, SMALL_BUFFER_SIZE_1); - bzero(ObjectState, SMALL_BUFFER_SIZE_1); - bzero(ReadyToArm, SMALL_BUFFER_SIZE_1); - - - if (MONRData->Header.MessageIdU16 == COMMAND_MONR_CODE) { - //Index - sprintf(Id, "%" PRIu8, (C8) Idn); - - //Timestamp - MonrValueU64 = 0; - //for(i = 0; i <= 5; i++, j++) MonrValueU64 = *(MonrData+j) | (MonrValueU64 << 8); - ConvertGPStoUTC = sprintf(Timestamp, "%" PRIu32, MONRData->GPSQmsOfWeekU32); - - if (debug && MONRData->GPSQmsOfWeekU32 % 400 == 0) { - LogMessage(LOG_LEVEL_DEBUG, "MONR = %x-%x-%x-%x-%x-%x-%d-%d-%d-%d-%d-%d-%d-%d-%d-%d-%d-%d", - MONRData->Header.MessageIdU16, - MONRData->Header.SyncWordU16, - MONRData->Header.TransmitterIdU8, - MONRData->Header.MessageCounterU8, - MONRData->Header.AckReqProtVerU8, - MONRData->Header.MessageLengthU32, - MONRData->MonrStructValueIdU16, - MONRData->MonrStructContentLengthU16, - MONRData->GPSQmsOfWeekU32, - MONRData->XPositionI32, - MONRData->YPositionI32, - MONRData->ZPositionI32, - MONRData->LongitudinalSpeedI16, - MONRData->HeadingU16, - MONRData->DriveDirectionU8, - MONRData->StateU8, MONRData->ReadyToArmU8, MONRData->ErrorStatusU8); - } - - iLlh[0] = OriginPosition->Latitude; - iLlh[1] = OriginPosition->Longitude; - iLlh[2] = OriginPosition->Altitude; - - xyz[0] = ((dbl) MONRData->XPositionI32) / 1000; - xyz[1] = ((dbl) MONRData->YPositionI32) / 1000; - xyz[2] = ((dbl) MONRData->ZPositionI32) / 1000; - - enuToLlh(iLlh, xyz, Llh); - - //XPosition - //MonrValueU32 = 0; - //for(i = 0; i <= 3; i++, j++) MonrValueU32 = *(MonrData+j) | (MonrValueU32 << 8); - //sprintf(Latitude, "%" PRIi32, (I32)(Llh[0]*1e7)); - sprintf(XPosition, "%" PRIi32, MONRData->XPositionI32); - - //YPosition - //MonrValueU32 = 0; - //for(i = 0; i <= 3; i++, j++) MonrValueU32 = *(MonrData+j) | (MonrValueU32 << 8); - //sprintf(Longitude, "%" PRIi32, (I32)(Llh[1]*1e7)); - sprintf(YPosition, "%" PRIi32, MONRData->YPositionI32); - - //ZPosition - //MonrValueU32 = 0; - //for(i = 0; i <= 3; i++, j++) MonrValueU32 = *(MonrData+j) | (MonrValueU32 << 8); - //sprintf(Altitude, "%" PRIi32, (I32)(Llh[2])); - sprintf(ZPosition, "%" PRIi32, MONRData->ZPositionI32); - - //Speed - //MonrValueU16 = 0; - //for(i = 0; i <= 1; i++, j++) MonrValueU16 = *(MonrData+j) | (MonrValueU16 << 8); - sprintf(LongitudinalSpeed, "%" PRIi16, MONRData->LongitudinalSpeedI16); - - //LatSpeed - //MonrValueU16 = 0; - //for(i = 0; i <= 1; i++, j++) MonrValueU16 = *(MonrData+j) | (MonrValueU16 << 8); - sprintf(LateralSpeed, "%" PRIi16, MONRData->LateralSpeedI16); - - //LongAcc - //MonrValueU16 = 0; - //for(i = 0; i <= 1; i++, j++) MonrValueU16 = *(MonrData+j) | (MonrValueU16 << 8); - sprintf(LongitudinalAcc, "%" PRIi16, MONRData->LongitudinalAccI16); - - //LatAcc - //MonrValueU16 = 0; - //for(i = 0; i <= 1; i++, j++) MonrValueU16 = *(MonrData+j) | (MonrValueU16 << 8); - sprintf(LateralAcc, "%" PRIi16, MONRData->LateralAccI16); - - //Heading - //MonrValueU16 = 0; - //for(i = 0; i <= 1; i++, j++) MonrValueU16 = *(MonrData+j) | (MonrValueU16 << 8); - sprintf(Heading, "%" PRIu16, MONRData->HeadingU16); - - //Driving direction - //MonrValueU8 = (unsigned char)*(MonrData+j); - //printf("D: %d\n", MonrValueU8 ); - - sprintf(DriveDirection, "%" PRIu8, MONRData->DriveDirectionU8); - - //State - //MonrValueU8 = (unsigned char)*(MonrData+j); - sprintf(ObjectState, "%" PRIu8, MONRData->StateU8); - - //ReadyToArmU8 - //MonrValueU8 = (unsigned char)*(MonrData+j); - sprintf(ReadyToArm, "%" PRIu8, MONRData->ReadyToArmU8); - - //ErrorStatusU8 - //MonrValueU8 = (unsigned char)*(MonrData+j); - sprintf(ErrorStatus, "%" PRIu8, MONRData->ErrorStatusU8); - - } - - return 0; -} - - I32 ObjectControlBuildOSEMMessage(C8 * MessageBuffer, OSEMType * OSEMData, TimeType * GPSTime, C8 * Latitude, C8 * Longitude, C8 * Altitude, U8 debug) { I32 MessageIndex = 0, i = 0; From ea5a6b5feb1d6d4fffc94e54c7e0ed096bc764aa Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 20 Jan 2020 16:28:40 +0100 Subject: [PATCH 080/523] Stopped using old functions in OC --- server/src/objectcontrol.c | 48 +++++++------------------------------- 1 file changed, 8 insertions(+), 40 deletions(-) diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 18e6f9675..105b84d4d 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -487,48 +487,16 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { //UtilSendUDPData("ObjectControl", &ObjectControlUDPSocketfdI32, &simulator_addr, &MONRData, sizeof(MONRData), 0); for (i = 0; i < - (MONRData.Header.MessageLengthU32 + COMMAND_MESSAGE_HEADER_LENGTH + - COMMAND_MESSAGE_FOOTER_LENGTH); i++) + (MONRData.header.MessageLengthU32 + sizeof (MONRData.header) + + sizeof (MONRData.footer)); i++) GSD->MONRData[i] = buffer[i]; GSD->MONRSizeU8 = - MONRData.Header.MessageLengthU32 + COMMAND_MESSAGE_HEADER_LENGTH + - COMMAND_MESSAGE_FOOTER_LENGTH; - MONRToASCII(&MONRData, buffer, sizeof (buffer), 0); - ObjectControlMONRToASCII(&MONRData, &OriginPosition, iIndex, Id, Timestamp, XPosition, - YPosition, ZPosition, LongitudinalSpeed, LateralSpeed, - LongitudinalAcc, LateralAcc, Heading, DriveDirection, - ObjectState, ReadyToArm, ErrorStatus, 1); - bzero(buffer, OBJECT_MESS_BUFFER_SIZE); - strcat(buffer, object_address_name[iIndex]); - strcat(buffer, ";"); - strcat(buffer, "0"); - strcat(buffer, ";"); - strcat(buffer, Timestamp); - strcat(buffer, ";"); - strcat(buffer, XPosition); - strcat(buffer, ";"); - strcat(buffer, YPosition); - strcat(buffer, ";"); - strcat(buffer, ZPosition); - strcat(buffer, ";"); - strcat(buffer, Heading); - strcat(buffer, ";"); - strcat(buffer, LongitudinalSpeed); - strcat(buffer, ";"); - strcat(buffer, LateralSpeed); - strcat(buffer, ";"); - strcat(buffer, LongitudinalAcc); - strcat(buffer, ";"); - strcat(buffer, LateralAcc); - strcat(buffer, ";"); - strcat(buffer, DriveDirection); - strcat(buffer, ";"); - strcat(buffer, ObjectState); - strcat(buffer, ";"); - strcat(buffer, ReadyToArm); - strcat(buffer, ";"); - strcat(buffer, ErrorStatus); - strcat(buffer, ";"); + MONRData.header.MessageLengthU32 + sizeof (MONRData.header) + + sizeof (MONRData.footer); + memset(buffer, 0, sizeof (buffer)); + memcpy(buffer, object_address_name[iIndex], strlen(object_address_name[iIndex])); + strcat(buffer, ";0;"); + MONRToASCII(&MONRData, buffer+strlen(buffer), sizeof (buffer)-strlen(buffer), 0); if (ASPData.MTSPU32 != 0) { From 7ef21849e488a100aced9eb42c421eb1ae51e6c1 Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Mon, 20 Jan 2020 16:31:35 +0100 Subject: [PATCH 081/523] get/set --- server/inc/datadictionary.h | 2 +- server/src/datadictionary.c | 14 +++++--------- 2 files changed, 6 insertions(+), 10 deletions(-) diff --git a/server/inc/datadictionary.h b/server/inc/datadictionary.h index 02e598b1c..1cbadeb6f 100644 --- a/server/inc/datadictionary.h +++ b/server/inc/datadictionary.h @@ -125,7 +125,7 @@ ReadWriteAccess_t DataDictionarySetMONR(GSDType *GSD, MONRType * MONR, U8 index) ReadWriteAccess_t DataDictionaryGetMONR(GSDType *GSD, MONRType * MONR, U8 index); ReadWriteAccess_t DataDictionarySetNumberOfObjectsU8(GSDType * GSD, U8 *NumberOfObjects); -ReadWriteAccess_t DataDictionaryNumberOfObjectsU8(GSDType * GSD, U8 *NumberOfObjects); +ReadWriteAccess_t DataDictionaryGetNumberOfObjectsU8(GSDType * GSD, U8 *NumberOfObjects); /*END of MONR*/ diff --git a/server/src/datadictionary.c b/server/src/datadictionary.c index 867b910ab..d60301a15 100644 --- a/server/src/datadictionary.c +++ b/server/src/datadictionary.c @@ -1744,9 +1744,7 @@ ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, MONRType * MONR, U8 id) { */ ReadWriteAccess_t DataDictionaryGetMONR(GSDType * GSD, MONRType *MONR, U8 id) { pthread_mutex_lock(&MONRMutex); - *MONR = GSD->MonrMessages[id]; - pthread_mutex_unlock(&MONRMutex); return READ_OK; } @@ -1761,12 +1759,11 @@ ReadWriteAccess_t DataDictionaryGetMONR(GSDType * GSD, MONRType *MONR, U8 id) { * \param OBCState * \return Result according to ::ReadWriteAccess_t */ -ReadWriteAccess_t DataDictionarySetNumberOfObjectsU8(GSDType * GSD, U8 NumberOfObjects) { +ReadWriteAccess_t DataDictionarySetNumberOfObjectsU8(GSDType * GSD, U8 *numberOfObjects) { ReadWriteAccess_t Res; - Res = WRITE_OK; pthread_mutex_lock(&numberOfObjectsMutex); - GSD->numberOfObjects = NumberOfObjects; + GSD->numberOfObjects = *numberOfObjects; pthread_mutex_unlock(&numberOfObjectsMutex); return Res; } @@ -1776,12 +1773,11 @@ ReadWriteAccess_t DataDictionarySetNumberOfObjectsU8(GSDType * GSD, U8 NumberOfO * \param GSD Pointer to shared allocated memory * \return Current object control state according to ::OBCState_t */ -U8 DataDictionaryNumberOfObjectsU8(GSDType * GSD) { - U8 Ret; +ReadWriteAccess_t DataDictionaryGetNumberOfObjectsU8(GSDType * GSD, U8 *numberOfObjects) { pthread_mutex_lock(&numberOfObjectsMutex); - Ret = GSD->OBCStateU8; + *numberOfObjects = GSD->numberOfObjects; pthread_mutex_unlock(&numberOfObjectsMutex); - return Ret; + return READ_OK; } /*END of NbrOfObjects*/ From d02d49c0394513061ff190c4de4693244a82b83d Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 20 Jan 2020 16:33:25 +0100 Subject: [PATCH 082/523] Ran another code formatter --- modules/Visualization/src/main.c | 2 +- server/src/logger.c | 10 +++++----- server/src/main.c | 2 +- server/src/objectcontrol.c | 5 +++-- server/src/systemcontrol.c | 6 +++--- server/src/util.c | 6 +++--- 6 files changed, 16 insertions(+), 15 deletions(-) diff --git a/modules/Visualization/src/main.c b/modules/Visualization/src/main.c index eaa72bf14..799a71ac5 100644 --- a/modules/Visualization/src/main.c +++ b/modules/Visualization/src/main.c @@ -122,7 +122,7 @@ int main() { { //Populate the monitorType - UtilPopulateMonitorDataStruct(mqRecvData, (size_t)(sizeof (mqRecvData)), &monitorData, 0); + UtilPopulateMonitorDataStruct(mqRecvData, (size_t) (sizeof (mqRecvData)), &monitorData, 0); //Allocate memory char *visualizationMessage = malloc(sizeOfVisualizationMessage * sizeof (char)); diff --git a/server/src/logger.c b/server/src/logger.c index ef569eb43..01c6f4612 100644 --- a/server/src/logger.c +++ b/server/src/logger.c @@ -574,7 +574,7 @@ void vLogMonitorData(char *commandData, ssize_t commandDatalen, struct timeval r TimeSetToCurrentSystemTime(&systemTime); - UtilPopulateMonitorDataStruct(commandData, (size_t)(commandDatalen), &monitorData, debug); + UtilPopulateMonitorDataStruct(commandData, (size_t) (commandDatalen), &monitorData, debug); TimeSetToGPStime(&monrTime, TimeGetAsGPSweek(&systemTime), monitorData.MONR.GPSQmsOfWeekU32); bzero(DateBuffer, sizeof (DateBuffer)); @@ -638,21 +638,21 @@ void vLogScenarioControlData(enum COMMAND command, unsigned char *commandData, s switch (command) { case COMM_TREO: - UtilPopulateTREODataStructFromMQ(commandData, (size_t)commandDatalen, &treo); + UtilPopulateTREODataStructFromMQ(commandData, (size_t) commandDatalen, &treo); LogMessage(LOG_LEVEL_INFO, "Trigger event occurred, ID %u", treo.triggerID); TimeSetToGPStime(&messageTimeField, TimeGetAsGPSweek(&systemTime), treo.timestamp_qmsow); fprintf(filefd, "%u;%ld;", treo.triggerID, TimeGetAsGPSms(&messageTimeField)); fprintf(filefd, "%s", inet_ntop(AF_INET, &treo.ip, ipStringBuffer, sizeof (ipStringBuffer))); break; case COMM_EXAC: - UtilPopulateEXACDataStructFromMQ(commandData, (size_t)commandDatalen, &exac); + UtilPopulateEXACDataStructFromMQ(commandData, (size_t) commandDatalen, &exac); LogMessage(LOG_LEVEL_INFO, "Action execute request detected, ID %u", exac.actionID); TimeSetToGPStime(&messageTimeField, TimeGetAsGPSweek(&systemTime), exac.executionTime_qmsoW); fprintf(filefd, "%u;%ld;", exac.actionID, TimeGetAsGPSms(&messageTimeField)); fprintf(filefd, "%s", inet_ntop(AF_INET, &exac.ip, ipStringBuffer, sizeof (ipStringBuffer))); break; case COMM_TRCM: - UtilPopulateTRCMDataStructFromMQ(commandData, (size_t)commandDatalen, &trcm); + UtilPopulateTRCMDataStructFromMQ(commandData, (size_t) commandDatalen, &trcm); LogMessage(LOG_LEVEL_INFO, "Trigger configuration for ID %u received, of type %u", trcm.triggerID, trcm.triggerType); fprintf(filefd, "%u;%u;%u;%u;%u", trcm.triggerID, trcm.triggerType, trcm.triggerTypeParameter1, @@ -660,7 +660,7 @@ void vLogScenarioControlData(enum COMMAND command, unsigned char *commandData, s fprintf(filefd, "%s", inet_ntop(AF_INET, &trcm.ip, ipStringBuffer, sizeof (ipStringBuffer))); break; case COMM_ACCM: - UtilPopulateACCMDataStructFromMQ(commandData, (size_t)commandDatalen, &accm); + UtilPopulateACCMDataStructFromMQ(commandData, (size_t) commandDatalen, &accm); LogMessage(LOG_LEVEL_INFO, "Action configuration for ID %u received, of type %u", accm.actionID, accm.actionType); fprintf(filefd, "%u;%u;%u;%u;%u", accm.actionID, accm.actionType, accm.actionTypeParameter1, diff --git a/server/src/main.c b/server/src/main.c index 2146b136f..7038d3746 100644 --- a/server/src/main.c +++ b/server/src/main.c @@ -41,7 +41,7 @@ /*------------------------------------------------------------ -- Types ------------------------------------------------------------*/ -typedef void (*ModuleTask)(TimeType *, GSDType *, LOG_LEVEL); //!< Function pointer type for module "main" functions +typedef void (*ModuleTask) (TimeType *, GSDType *, LOG_LEVEL); //!< Function pointer type for module "main" functions typedef struct { LOG_LEVEL commonLogLevel; //!< Logging level of the server. int extraMessageQueues; //!< Number of extra message queues to create on startup. diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 3e0be4f62..87542be21 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -475,7 +475,8 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { sizeof (in_addr_t)); // Send MONR message as bytes - if (iCommSend(COMM_MONR, buffer, (size_t)(receivedMONRData) + sizeof (in_addr_t)) < 0) { + if (iCommSend(COMM_MONR, buffer, (size_t) (receivedMONRData) + sizeof (in_addr_t)) < + 0) { LogMessage(LOG_LEVEL_ERROR, "Fatal communication fault when sending MONR command - entering error state"); vSetState(OBC_STATE_ERROR, GSD); @@ -3093,7 +3094,7 @@ static size_t uiRecvMonitor(int *sockfd, char *buffer, size_t length) { } } else { - recvDataSize = (size_t)(result); + recvDataSize = (size_t) (result); LogMessage(LOG_LEVEL_DEBUG, "Received: <%s>", buffer); } } while (result > 0); diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index db0ae83bc..0a5f9259d 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -567,7 +567,7 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { case COMM_MONR: // TODO: Decode if (RVSSChannelSocket != 0 && RVSSConfigU32 & RVSS_MONR_CHANNEL && bytesReceived >= 0) { - UtilPopulateMonitorDataStruct(pcRecvBuffer, (size_t)bytesReceived, &monrData, 0); + UtilPopulateMonitorDataStruct(pcRecvBuffer, (size_t) bytesReceived, &monrData, 0); SystemControlBuildRVSSMONRChannelMessage(RVSSData, &RVSSMessageLengthU32, monrData, 0); UtilSendUDPData("SystemControl", &RVSSChannelSocket, &RVSSChannelAddr, RVSSData, RVSSMessageLengthU32, 0); @@ -1189,13 +1189,13 @@ ssize_t SystemControlReceiveUserControlData(I32 socket, C8 * dataBuffer, size_t readResult = recv(socket, recvBuffer + bytesInBuffer, sizeof (recvBuffer) - bytesInBuffer, MSG_DONTWAIT); if (readResult > 0) { - bytesInBuffer += (size_t)readResult; + bytesInBuffer += (size_t) readResult; } if (bytesInBuffer > 0) { if ((endOfMessage = strstr(recvBuffer, endOfMessagePattern)) != NULL) { endOfMessage += sizeof (endOfMessagePattern) - 1; - messageLength = (size_t)(endOfMessage - recvBuffer); + messageLength = (size_t) (endOfMessage - recvBuffer); } else { messageLength = 0; diff --git a/server/src/util.c b/server/src/util.c index 09298dddd..0ddb36942 100644 --- a/server/src/util.c +++ b/server/src/util.c @@ -321,7 +321,7 @@ uint64_t UtilgetETSIfromUTCMS(uint64_t utc_sec, uint64_t utc_usec) { void UtilgetDateTimeFromUTCtime(int64_t utc_ms, char *buffer, int size_t) { time_t time_seconds = utc_ms / 1000; - if (size_t <26) + if (size_t < 26) return; strcpy(buffer, ctime(&time_seconds)); } @@ -1904,7 +1904,7 @@ int iCommClose() { * \param timeRecv Receive time output variable * \return Size (in bytes) of received data */ -ssize_t iCommRecv(enum COMMAND *command, char *data, const size_t messageSize, struct timeval *timeRecv) { +ssize_t iCommRecv(enum COMMAND * command, char *data, const size_t messageSize, struct timeval * timeRecv) { char message[MQ_MSG_SIZE]; ssize_t result = MQBusRecv(message, MQ_MSG_SIZE); size_t dataLength = 0; @@ -1923,7 +1923,7 @@ ssize_t iCommRecv(enum COMMAND *command, char *data, const size_t messageSize, s *command = (unsigned char)message[0]; memcpy(&dataLength, message + sizeof (char), sizeof (dataLength)); - if (dataLength != (size_t)(result)) { + if (dataLength != (size_t) (result)) { LogMessage(LOG_LEVEL_ERROR, "Received message with invalid length specification field: %d bytes, but %d bytes were received", dataLength, result); From 53e5ee11869375b4c452ce162cd0cba3914d3147 Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Mon, 20 Jan 2020 16:35:35 +0100 Subject: [PATCH 083/523] removed empty lines --- runServer.sh | 2 +- server/inc/datadictionary.h | 2 -- server/src/datadictionary.c | 23 ----------------------- 3 files changed, 1 insertion(+), 26 deletions(-) diff --git a/runServer.sh b/runServer.sh index 19f3d7c45..3b989f36f 100755 --- a/runServer.sh +++ b/runServer.sh @@ -2,7 +2,7 @@ #### User settings # Modify this array by adding more modules to include them in the execution -MODULES=() +MODULES=(ScenarioControl) #### # Save top directory diff --git a/server/inc/datadictionary.h b/server/inc/datadictionary.h index 1cbadeb6f..7865e7854 100644 --- a/server/inc/datadictionary.h +++ b/server/inc/datadictionary.h @@ -127,8 +127,6 @@ ReadWriteAccess_t DataDictionaryGetMONR(GSDType *GSD, MONRType * MONR, U8 index) ReadWriteAccess_t DataDictionarySetNumberOfObjectsU8(GSDType * GSD, U8 *NumberOfObjects); ReadWriteAccess_t DataDictionaryGetNumberOfObjectsU8(GSDType * GSD, U8 *NumberOfObjects); -/*END of MONR*/ - ReadWriteAccess_t DataDictionaryConstructor(GSDType *GSD); #endif diff --git a/server/src/datadictionary.c b/server/src/datadictionary.c index d60301a15..01c6db282 100644 --- a/server/src/datadictionary.c +++ b/server/src/datadictionary.c @@ -93,7 +93,6 @@ ReadWriteAccess_t DataDictionaryConstructor(GSDType * GSD) { DataDictionarySetOBCStateU8(GSD, OBC_STATE_UNDEFINED); - return Res; } @@ -1603,7 +1602,6 @@ ReadWriteAccess_t DataDictionaryInitMiscDataC8(GSDType * GSD) { */ ReadWriteAccess_t DataDictionarySetMiscDataC8(GSDType * GSD, C8 * MiscData) { ReadWriteAccess_t Res; - if (UtilWriteConfigurationParameter("MiscData", MiscData, 0)) { Res = WRITE_OK; pthread_mutex_lock(&MiscDataMutex); @@ -1642,7 +1640,6 @@ ReadWriteAccess_t DataDictionaryGetMiscDataC8(GSDType * GSD, U8 * MiscData, U32 */ ReadWriteAccess_t DataDictionarySetOBCStateU8(GSDType * GSD, OBCState_t OBCState) { ReadWriteAccess_t Res; - Res = WRITE_OK; pthread_mutex_lock(&OBCStateMutex); GSD->OBCStateU8 = OBCState; @@ -1657,17 +1654,14 @@ ReadWriteAccess_t DataDictionarySetOBCStateU8(GSDType * GSD, OBCState_t OBCState */ OBCState_t DataDictionaryGetOBCStateU8(GSDType * GSD) { OBCState_t Ret; - pthread_mutex_lock(&OBCStateMutex); Ret = GSD->OBCStateU8; pthread_mutex_unlock(&OBCStateMutex); - return Ret; } /*END OBCState*/ - /*! * \brief DataDictionaryInitMONR inits a data structure for saving object monr * \param GSD Pointer to shared allocated memory @@ -1676,24 +1670,11 @@ OBCState_t DataDictionaryGetOBCStateU8(GSDType * GSD) { */ ReadWriteAccess_t DataDictionaryInitMONR(GSDType * GSD, U8 objectCount){ ReadWriteAccess_t Res; - Res = WRITE_OK; pthread_mutex_lock(&MONRMutex); - MONRType *ptr = malloc(sizeof (MONRType) * objectCount); GSD->MonrMessages = ptr; - LogPrint("Very good nice."); - - // Check if the memory has been successfully - /*if (GSD->test == NULL) { - LogPrint(LOG_LEVEL_ERROR, "Coult not allocate memmory for MONR"); - } - else { - // Memory has been successfully allocated - LogPrint(LOG_LEVEL_INFO, "Allocated pointer for MONR."); - } - */ pthread_mutex_unlock(&MONRMutex); return Res; } @@ -1706,7 +1687,6 @@ ReadWriteAccess_t DataDictionaryInitMONR(GSDType * GSD, U8 objectCount){ */ ReadWriteAccess_t DataDictionaryFreeMONR(GSDType * GSD){ ReadWriteAccess_t Res; - Res = WRITE_OK; pthread_mutex_lock(&MONRMutex); free(GSD->MonrMessages); @@ -1724,14 +1704,11 @@ ReadWriteAccess_t DataDictionaryFreeMONR(GSDType * GSD){ */ ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, MONRType * MONR, U8 id) { ReadWriteAccess_t Res; - Res = WRITE_OK; - pthread_mutex_lock(&MONRMutex); MONRType *test = GSD->MonrMessages; test[id] = *MONR; pthread_mutex_unlock(&MONRMutex); - return Res; } From 122347bb4a807adaa546dbd17bec2cff94cbf3d1 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 20 Jan 2020 17:31:24 +0100 Subject: [PATCH 084/523] Moved ASCII to MONR function to iso22133.c --- server/inc/iso22133.h | 1 + server/src/iso22133.c | 62 +++++++++++++++++++++++ server/src/util.c | 114 +++++++++++++++--------------------------- 3 files changed, 103 insertions(+), 74 deletions(-) diff --git a/server/inc/iso22133.h b/server/inc/iso22133.h index fed36bcf1..bf02bde1e 100644 --- a/server/inc/iso22133.h +++ b/server/inc/iso22133.h @@ -272,6 +272,7 @@ typedef enum { void getSupportedISOProtocolVersions(const uint8_t** supportedProtocolVersions, size_t* nProtocols); ISOMessageReturnValue buildMONRMessage(const char * MonrData, const size_t length, MONRType * MONRData, const char debug); ISOMessageReturnValue MONRToASCII(const MONRType * MONRData, char * asciiBuffer, const size_t bufferLength, const char debug); +ISOMessageReturnValue ASCIIToMONR(const char * asciiBuffer, MONRType * MONRData, const char debug); #ifdef __cplusplus } diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 34d3db933..ca8baf5ea 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -2,6 +2,7 @@ #include "string.h" #include "logging.h" #include "errno.h" +#include "stdlib.h" static const uint8_t SupportedProtocolVersions[] = { 2 }; @@ -267,3 +268,64 @@ ISOMessageReturnValue MONRToASCII(const MONRType * MONRData, char * asciiBuffer, return MESSAGE_OK; } + +/*! + * \brief MONRToASCII Converts an ASCII string into a MONR struct + * \param asciiBuffer Buffer containing ASCII text representation + * \param MONRData Struct containing MONR data + * \param debug Flag for enabling debugging + * \return value according to ::ISOMessageReturnValue + */ +ISOMessageReturnValue ASCIIToMONR(const char * asciiBuffer, MONRType * MONRData, const char debug) { + + const size_t bufferLength = strlen(asciiBuffer); + const char *token; + const char delim[] = ";"; + const int NumberBaseDecimal = 10; + char * copy = strdup(asciiBuffer); + + memset(MONRData, 0, sizeof (*MONRData)); + + token = strtok(copy, delim); + + token = strtok(NULL, delim); + MONRData->gpsQmsOfWeek = (uint32_t) strtoul(token, NULL, NumberBaseDecimal); + + token = strtok(NULL, delim); + MONRData->xPosition = (int32_t) strtol(token, NULL, NumberBaseDecimal); + + token = strtok(NULL, delim); + MONRData->yPosition = (int32_t) strtol(token, NULL, NumberBaseDecimal); + + token = strtok(NULL, delim); + MONRData->zPosition = (int32_t) strtol(token, NULL, NumberBaseDecimal); + + token = strtok(NULL, delim); + MONRData->heading = (uint16_t) strtoul(token, NULL, NumberBaseDecimal); + + token = strtok(NULL, delim); + MONRData->longitudinalSpeed = (int16_t) strtol(token, NULL, NumberBaseDecimal); + + token = strtok(NULL, delim); + MONRData->lateralSpeed = (int16_t) strtol(token, NULL, NumberBaseDecimal); + + token = strtok(NULL, delim); + MONRData->longitudinalAcc = (int16_t) strtol(token, NULL, NumberBaseDecimal); + + token = strtok(NULL, delim); + MONRData->lateralAcc = (int16_t) strtol(token, NULL, NumberBaseDecimal); + + token = strtok(NULL, delim); + MONRData->driveDirection = (uint8_t) strtoul(token, NULL, NumberBaseDecimal); + + token = strtok(NULL, delim); + MONRData->state = (uint8_t) strtoul(token, NULL, NumberBaseDecimal); + + token = strtok(NULL, delim); + MONRData->readyToArm = (uint8_t) strtoul(token, NULL, NumberBaseDecimal); + + token = strtok(NULL, delim); + MONRData->errorStatus = (uint8_t) strtoul(token, NULL, NumberBaseDecimal); + + return MESSAGE_OK; +} diff --git a/server/src/util.c b/server/src/util.c index 5c2ed2c8d..3e49e8d8f 100644 --- a/server/src/util.c +++ b/server/src/util.c @@ -695,15 +695,15 @@ int UtilSetSlaveObject(ObjectPosition * OP, char *Filename, char debug) { CartesianPosition MONRToCartesianPosition(MonitorDataType MONR) { CartesianPosition retval; - retval.xCoord_m = MONR.MONR.XPositionI32 / 1000.0; - retval.yCoord_m = MONR.MONR.YPositionI32 / 1000.0; - retval.zCoord_m = MONR.MONR.ZPositionI32 / 1000.0; - if (MONR.MONR.HeadingU16 == 36001) { // 36001: unavailable + retval.xCoord_m = MONR.MONR.xPosition / 1000.0; + retval.yCoord_m = MONR.MONR.yPosition / 1000.0; + retval.zCoord_m = MONR.MONR.zPosition / 1000.0; + if (MONR.MONR.heading == 36001) { // 36001: unavailable LogMessage(LOG_LEVEL_DEBUG, "MONR heading unavailable, assuming 0"); retval.heading_deg = 0.0; } else { - retval.heading_deg = MONR.MONR.HeadingU16 / 100.0; + retval.heading_deg = MONR.MONR.heading / 100.0; } return retval; } @@ -737,10 +737,10 @@ int UtilMonitorDataToString(MonitorDataType monrData, char *monrString, size_t s int UtilStringToMonitorData(const char *monrString, size_t stringLength, MonitorDataType * monrData) { const char *token; const char delim[] = ";"; - const int NumberBaseDecimal = 10; struct in_addr addr; + char * copy = strdup(monrString); - token = strtok(monrString, delim); + token = strtok(copy, delim); // IP address inet_pton(AF_INET, token, &addr); @@ -751,44 +751,10 @@ int UtilStringToMonitorData(const char *monrString, size_t stringLength, Monitor // MONR data token = strtok(NULL, delim); - monrData->MONR.GPSQmsOfWeekU32 = (U32) strtoul(token, NULL, NumberBaseDecimal); - - token = strtok(NULL, delim); - monrData->MONR.XPositionI32 = (I32) strtol(token, NULL, NumberBaseDecimal); - - token = strtok(NULL, delim); - monrData->MONR.YPositionI32 = (I32) strtol(token, NULL, NumberBaseDecimal); - - token = strtok(NULL, delim); - monrData->MONR.ZPositionI32 = (I32) strtol(token, NULL, NumberBaseDecimal); - - token = strtok(NULL, delim); - monrData->MONR.HeadingU16 = (U16) strtoul(token, NULL, NumberBaseDecimal); - - token = strtok(NULL, delim); - monrData->MONR.LongitudinalSpeedI16 = (I16) strtol(token, NULL, NumberBaseDecimal); - - token = strtok(NULL, delim); - monrData->MONR.LateralSpeedI16 = (I16) strtol(token, NULL, NumberBaseDecimal); - - token = strtok(NULL, delim); - monrData->MONR.LongitudinalAccI16 = (I16) strtol(token, NULL, NumberBaseDecimal); - - token = strtok(NULL, delim); - monrData->MONR.LateralAccI16 = (I16) strtol(token, NULL, NumberBaseDecimal); - - token = strtok(NULL, delim); - monrData->MONR.DriveDirectionU8 = (U8) strtoul(token, NULL, NumberBaseDecimal); - - token = strtok(NULL, delim); - monrData->MONR.StateU8 = (U8) strtoul(token, NULL, NumberBaseDecimal); - - token = strtok(NULL, delim); - monrData->MONR.ReadyToArmU8 = (U8) strtoul(token, NULL, NumberBaseDecimal); - - token = strtok(NULL, delim); - monrData->MONR.ErrorStatusU8 = (U8) strtoul(token, NULL, NumberBaseDecimal); - return 0; + if (ASCIIToMONR(token, &monrData->MONR, 0) == MESSAGE_OK) + return 0; + else + return -1; } /*! @@ -3753,9 +3719,9 @@ I32 UtilPopulateMonitorDataStruct(C8 * rawMONR, size_t rawMONRsize, MonitorDataT C8 *rdPtr = rawMONR, *monrStruct; // Pointer to keep track of where in rawMONR we are currently reading U16 contentLength = 0; in_addr_t IPData = 0; - const size_t monrPacketSize = sizeof (monitorData->MONR) - sizeof (monitorData->MONR.Header) - - sizeof (monitorData->MONR.CRC) - sizeof (monitorData->MONR.MonrStructValueIdU16) - - sizeof (monitorData->MONR.MonrStructContentLengthU16); + const size_t monrPacketSize = sizeof (monitorData->MONR) - sizeof (monitorData->MONR.header) + - sizeof (monitorData->MONR.footer.Crc) - sizeof (monitorData->MONR.monrStructValueID) + - sizeof (monitorData->MONR.monrStructContentLength); if (rawMONRsize < sizeof (MONRType)) { LogMessage(LOG_LEVEL_ERROR, "Raw MONR array too small to hold all necessary MONR data, %d < %d.", @@ -3765,21 +3731,21 @@ I32 UtilPopulateMonitorDataStruct(C8 * rawMONR, size_t rawMONRsize, MonitorDataT // ISO message header memcpy(&U16Data, rdPtr, sizeof (U16Data)); - monitorData->MONR.Header.SyncWordU16 = U16Data; + monitorData->MONR.header.SyncWordU16 = U16Data; rdPtr += sizeof (U16Data); U16Data = 0; - monitorData->MONR.Header.TransmitterIdU8 = *(rdPtr++); - monitorData->MONR.Header.MessageCounterU8 = *(rdPtr++); - monitorData->MONR.Header.AckReqProtVerU8 = *(rdPtr++); + monitorData->MONR.header.TransmitterIdU8 = *(rdPtr++); + monitorData->MONR.header.MessageCounterU8 = *(rdPtr++); + monitorData->MONR.header.AckReqProtVerU8 = *(rdPtr++); memcpy(&U16Data, rdPtr, sizeof (U16Data)); - monitorData->MONR.Header.MessageIdU16 = U16Data; + monitorData->MONR.header.MessageIdU16 = U16Data; rdPtr += sizeof (U16Data); U16Data = 0; memcpy(&U32Data, rdPtr, sizeof (U32Data)); - monitorData->MONR.Header.MessageLengthU32 = U32Data; + monitorData->MONR.header.MessageLengthU32 = U32Data; rdPtr += sizeof (U32Data); U32Data = 0; @@ -3813,57 +3779,57 @@ I32 UtilPopulateMonitorDataStruct(C8 * rawMONR, size_t rawMONRsize, MonitorDataT U16Data = 0; memcpy(&U32Data, rdPtr, sizeof (U32Data)); - monitorData->MONR.GPSQmsOfWeekU32 = U32Data; + monitorData->MONR.gpsQmsOfWeek = U32Data; rdPtr += sizeof (U32Data); U32Data = 0; memcpy(&I32Data, rdPtr, sizeof (I32Data)); - monitorData->MONR.XPositionI32 = I32Data; + monitorData->MONR.xPosition = I32Data; rdPtr += sizeof (I32Data); I32Data = 0; memcpy(&I32Data, rdPtr, sizeof (I32Data)); - monitorData->MONR.YPositionI32 = I32Data; + monitorData->MONR.yPosition = I32Data; rdPtr += sizeof (I32Data); I32Data = 0; memcpy(&I32Data, rdPtr, sizeof (I32Data)); - monitorData->MONR.ZPositionI32 = I32Data; + monitorData->MONR.zPosition = I32Data; rdPtr += sizeof (I32Data); I32Data = 0; memcpy(&U16Data, rdPtr, sizeof (U16Data)); - monitorData->MONR.HeadingU16 = U16Data; + monitorData->MONR.heading = U16Data; rdPtr += sizeof (U16Data); U16Data = 0; memcpy(&I16Data, rdPtr, sizeof (I16Data)); - monitorData->MONR.LongitudinalSpeedI16 = I16Data; + monitorData->MONR.longitudinalSpeed = I16Data; rdPtr += sizeof (I16Data); I16Data = 0; memcpy(&I16Data, rdPtr, sizeof (I16Data)); - monitorData->MONR.LateralSpeedI16 = I16Data; + monitorData->MONR.lateralSpeed = I16Data; rdPtr += sizeof (I16Data); I16Data = 0; memcpy(&I16Data, rdPtr, sizeof (I16Data)); - monitorData->MONR.LongitudinalAccI16 = I16Data; + monitorData->MONR.longitudinalAcc = I16Data; rdPtr += sizeof (I16Data); I16Data = 0; memcpy(&I16Data, rdPtr, sizeof (I16Data)); - monitorData->MONR.LateralAccI16 = I16Data; + monitorData->MONR.lateralAcc = I16Data; rdPtr += sizeof (I16Data); I16Data = 0; - monitorData->MONR.DriveDirectionU8 = *(rdPtr++); - monitorData->MONR.StateU8 = *(rdPtr++); - monitorData->MONR.ReadyToArmU8 = *(rdPtr++); - monitorData->MONR.ErrorStatusU8 = *(rdPtr++); + monitorData->MONR.driveDirection = *(rdPtr++); + monitorData->MONR.state = *(rdPtr++); + monitorData->MONR.readyToArm = *(rdPtr++); + monitorData->MONR.errorStatus = *(rdPtr++); memcpy(&U16Data, rdPtr, sizeof (U16Data)); - monitorData->MONR.CRC = U16Data; + monitorData->MONR.footer.Crc = U16Data; rdPtr += sizeof (U16Data); U16Data = 0; @@ -3874,12 +3840,12 @@ I32 UtilPopulateMonitorDataStruct(C8 * rawMONR, size_t rawMONRsize, MonitorDataT if (debug == 1) { LogPrint("MONR:"); - LogPrint("SyncWord = %d", monitorData->MONR.Header.SyncWordU16); - LogPrint("TransmitterId = %d", monitorData->MONR.Header.TransmitterIdU8); - LogPrint("PackageCounter = %d", monitorData->MONR.Header.MessageCounterU8); - LogPrint("AckReq = %d", monitorData->MONR.Header.AckReqProtVerU8); - LogPrint("MessageLength = %d", monitorData->MONR.Header.MessageLengthU32); - LogPrint("GPSQMSOW = %u", monitorData->MONR.GPSQmsOfWeekU32); + LogPrint("SyncWord = %d", monitorData->MONR.header.SyncWordU16); + LogPrint("TransmitterId = %d", monitorData->MONR.header.TransmitterIdU8); + LogPrint("PackageCounter = %d", monitorData->MONR.header.MessageCounterU8); + LogPrint("AckReq = %d", monitorData->MONR.header.AckReqProtVerU8); + LogPrint("MessageLength = %d", monitorData->MONR.header.MessageLengthU32); + LogPrint("GPSQMSOW = %u", monitorData->MONR.gpsQmsOfWeek); } return 0; From 9668108e38bbc889a9b19ae7cbb62c47a346651d Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 20 Jan 2020 17:33:33 +0100 Subject: [PATCH 085/523] Refactoring in logger.c --- server/src/logger.c | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/server/src/logger.c b/server/src/logger.c index 01c6f4612..4ebaf2702 100644 --- a/server/src/logger.c +++ b/server/src/logger.c @@ -237,7 +237,7 @@ void logger_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { MonitorDataType monrData; UtilStringToMonitorData(pcReadBuffer, sizeof (pcReadBuffer), &monrData); - NewTimestamp = monrData.MONR.GPSQmsOfWeekU32; + NewTimestamp = monrData.MONR.gpsQmsOfWeek; if (!FirstIteration) { /* Wait a little bit */ sleep_time.tv_sec = 0; @@ -575,7 +575,7 @@ void vLogMonitorData(char *commandData, ssize_t commandDatalen, struct timeval r TimeSetToCurrentSystemTime(&systemTime); UtilPopulateMonitorDataStruct(commandData, (size_t) (commandDatalen), &monitorData, debug); - TimeSetToGPStime(&monrTime, TimeGetAsGPSweek(&systemTime), monitorData.MONR.GPSQmsOfWeekU32); + TimeSetToGPStime(&monrTime, TimeGetAsGPSweek(&systemTime), monitorData.MONR.gpsQmsOfWeek); bzero(DateBuffer, sizeof (DateBuffer)); TimeGetAsDateTime(&recvTime, "%Y;%m;%d;%H;%M;%S;%q", DateBuffer, sizeof (DateBuffer)); @@ -590,13 +590,13 @@ void vLogMonitorData(char *commandData, ssize_t commandDatalen, struct timeval r (unsigned char)COMM_MONR); fprintf(filefd, "%s;", inet_ntop(AF_INET, &monitorData.ClientIP, ipStringBuffer, sizeof (ipStringBuffer))); - fprintf(filefd, "%u;%u;", monitorData.MONR.Header.TransmitterIdU8, monitorData.MONR.GPSQmsOfWeekU32); - fprintf(filefd, "%d;%d;%d;%u;", monitorData.MONR.XPositionI32, monitorData.MONR.YPositionI32, - monitorData.MONR.ZPositionI32, monitorData.MONR.HeadingU16); - fprintf(filefd, "%d;%d;", monitorData.MONR.LongitudinalSpeedI16, monitorData.MONR.LateralSpeedI16); - fprintf(filefd, "%d;%d;", monitorData.MONR.LongitudinalAccI16, monitorData.MONR.LateralAccI16); - fprintf(filefd, "%u;%u;%u;%u;\n", monitorData.MONR.DriveDirectionU8, monitorData.MONR.StateU8, - monitorData.MONR.ReadyToArmU8, monitorData.MONR.ErrorStatusU8); + fprintf(filefd, "%u;%u;", monitorData.MONR.header.TransmitterIdU8, monitorData.MONR.gpsQmsOfWeek); + fprintf(filefd, "%d;%d;%d;%u;", monitorData.MONR.xPosition, monitorData.MONR.yPosition, + monitorData.MONR.zPosition, monitorData.MONR.heading); + fprintf(filefd, "%d;%d;", monitorData.MONR.longitudinalSpeed, monitorData.MONR.lateralSpeed); + fprintf(filefd, "%d;%d;", monitorData.MONR.longitudinalAcc, monitorData.MONR.lateralAcc); + fprintf(filefd, "%u;%u;%u;%u;\n", monitorData.MONR.driveDirection, monitorData.MONR.state, + monitorData.MONR.readyToArm, monitorData.MONR.errorStatus); fclose(filefd); } From 7c3f4d58ef184ed955d8e7cdcb2dc1bd54ac4ce5 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 20 Jan 2020 17:35:33 +0100 Subject: [PATCH 086/523] Refactoring in modules --- modules/ScenarioControl/src/scenario.cpp | 4 ++-- modules/Supervision/src/main.cpp | 8 ++++---- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/modules/ScenarioControl/src/scenario.cpp b/modules/ScenarioControl/src/scenario.cpp index 8a7e6691a..3b159042d 100644 --- a/modules/ScenarioControl/src/scenario.cpp +++ b/modules/ScenarioControl/src/scenario.cpp @@ -435,8 +435,8 @@ Scenario::ScenarioReturnCode_t Scenario::updateTrigger(const MonitorDataType &mo case Trigger::TriggerTypeCode_t::TRIGGER_BRAKE: struct timeval monrTime, currentTime; TimeSetToCurrentSystemTime(¤tTime); - TimeSetToGPStime(&monrTime, TimeGetAsGPSweek(¤tTime), monr.MONR.GPSQmsOfWeekU32); - tp->update(static_cast(monr.MONR.LongitudinalSpeedI16)/100.0, monrTime); + TimeSetToGPStime(&monrTime, TimeGetAsGPSweek(¤tTime), monr.MONR.gpsQmsOfWeek); + tp->update(static_cast(monr.MONR.longitudinalSpeed)/100.0, monrTime); break; default: LogMessage(LOG_LEVEL_WARNING, "Unhandled trigger type in update: %s", diff --git a/modules/Supervision/src/main.cpp b/modules/Supervision/src/main.cpp index 98e0183fd..b451d4624 100644 --- a/modules/Supervision/src/main.cpp +++ b/modules/Supervision/src/main.cpp @@ -444,8 +444,8 @@ bool isViolatingGeofence(const MonitorDataType &MONRdata, std::vector const CartesianPosition monrPoint = { - MONRdata.MONR.XPositionI32 / 1000.0, MONRdata.MONR.YPositionI32 / 1000.0, - MONRdata.MONR.ZPositionI32 / 1000.0, 0.0 + MONRdata.MONR.xPosition / 1000.0, MONRdata.MONR.yPosition / 1000.0, + MONRdata.MONR.zPosition / 1000.0, 0.0 }; char isInPolygon = 0; int retval = false; @@ -467,11 +467,11 @@ bool isViolatingGeofence(const MonitorDataType &MONRdata, std::vector if (geofence.isPermitted) LogMessage(LOG_LEVEL_WARNING, "Object with MONR transmitter ID %u is outside a permitted area %s", - MONRdata.MONR.Header.TransmitterIdU8, geofence.name.c_str()); + MONRdata.MONR.header.TransmitterIdU8, geofence.name.c_str()); else LogMessage(LOG_LEVEL_WARNING, "Object with MONR transmitter ID %u is inside a forbidden area %s", - MONRdata.MONR.Header.TransmitterIdU8, geofence.name.c_str()); + MONRdata.MONR.header.TransmitterIdU8, geofence.name.c_str()); retval = true; } } From 2eba37a0a2305ff4f12aab65bc3dcaeb7051c9e9 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 20 Jan 2020 17:37:20 +0100 Subject: [PATCH 087/523] Updated ASP section of code to use binary data instead of strings --- server/src/objectcontrol.c | 25 +++++++++---------------- 1 file changed, 9 insertions(+), 16 deletions(-) diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 105b84d4d..a7f16248d 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -258,13 +258,8 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { U8 iForceObjectToLocalhostU8 = 0; FILE *fd; - C8 Id[SMALL_BUFFER_SIZE_0]; - C8 GPSWeek[SMALL_BUFFER_SIZE_0], Timestamp[SMALL_BUFFER_SIZE_0], XPosition[SMALL_BUFFER_SIZE_0], - YPosition[SMALL_BUFFER_SIZE_0], ZPosition[SMALL_BUFFER_SIZE_0]; - C8 Speed[SMALL_BUFFER_SIZE_0], LongitudinalSpeed[SMALL_BUFFER_SIZE_0], LateralSpeed[SMALL_BUFFER_SIZE_0], - LongitudinalAcc[SMALL_BUFFER_SIZE_0], LateralAcc[SMALL_BUFFER_SIZE_0]; - C8 Heading[SMALL_BUFFER_SIZE_0], DriveDirection[SMALL_BUFFER_SIZE_1], ObjectState[SMALL_BUFFER_SIZE_1], - ReadyToArm[SMALL_BUFFER_SIZE_1], MTSP[SMALL_BUFFER_SIZE_0], ErrorStatus[SMALL_BUFFER_SIZE_0]; + C8 Timestamp[SMALL_BUFFER_SIZE_0]; + C8 GPSWeek[SMALL_BUFFER_SIZE_0]; I32 MessageLength; C8 *MiscPtr; C8 MiscText[SMALL_BUFFER_SIZE_0]; @@ -501,10 +496,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { if (ASPData.MTSPU32 != 0) { //Add MTSP to MONR if not 0 - bzero(MTSP, SMALL_BUFFER_SIZE_0); - sprintf(MTSP, "%" PRIu32, ASPData.MTSPU32); - strcat(buffer, MTSP); - strcat(buffer, ";"); + sprintf(buffer+strlen(buffer), "%" PRIu32 ";", ASPData.MTSPU32); } //Ok, let's do the ASP @@ -520,13 +512,14 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { TimeCap1 = (uint64_t) CurrentTimeStruct.tv_sec * 1000 + (uint64_t) CurrentTimeStruct.tv_usec / 1000; //Calculate initial timestamp - OP[iIndex].x = ((dbl) atoi(XPosition)) / 1000; //Set x and y on OP (ObjectPosition) - OP[iIndex].y = ((dbl) atoi(YPosition)) / 1000; + OP[iIndex].x = ((double) MONRData.xPosition) / 1000; //Set x and y on OP (ObjectPosition) + OP[iIndex].y = ((double) MONRData.yPosition) / 1000; //OP[iIndex].OrigoDistance = sqrt(pow(OP[iIndex].x,2) + pow(OP[iIndex].y,2)); //Calculate hypotenuse + // TODO: check use of this function since it should take two lat/long points but is here used with x/y UtilCalcPositionDelta(OriginLatitudeDbl, OriginLongitudeDbl, - atof(XPosition) / 1e7, atof(YPosition) / 1e7, &OP[iIndex]); + MONRData.xPosition / 1e7, MONRData.yPosition / 1e7, &OP[iIndex]); if (OP[iIndex].BestFoundTrajectoryIndex <= OP[iIndex].SyncIndex) { ASPData.CurrentTimeU32 = CurrentTimeU32; @@ -575,7 +568,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { //ObjectControlBuildASPMessage(buffer, &ASPData, 0); DataDictionarySetRVSSAsp(GSD, &ASPData); - if (atoi(Timestamp) % ASPDebugRate == 0) { + if (MONRData.gpsQmsOfWeek % ASPDebugRate == 0) { printf("%d, %d, %3.3f, %s, %s\n", CurrentTimeU32, StartTimeU32, ASPData.TimeToSyncPointDbl, object_address_name[iIndex], ASP[i].MasterIP); @@ -592,7 +585,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { } } } - OP[iIndex].Speed = atof(Speed); + OP[iIndex].Speed = sqrt(pow(MONRData.lateralSpeed, 2) + pow(MONRData.longitudinalSpeed, 2)); } else if (receivedMONRData > 0) LogMessage(LOG_LEVEL_INFO, "MONR length error (should be %d but is %ld) from %s.", From bad59dbbe6854792503abde2917e7271a685fcb3 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 20 Jan 2020 17:41:28 +0100 Subject: [PATCH 088/523] Moved static functionality away from header file --- server/inc/iso22133.h | 1 - server/src/iso22133.c | 12 ++---------- 2 files changed, 2 insertions(+), 11 deletions(-) diff --git a/server/inc/iso22133.h b/server/inc/iso22133.h index bf02bde1e..d676e36d4 100644 --- a/server/inc/iso22133.h +++ b/server/inc/iso22133.h @@ -269,7 +269,6 @@ typedef enum { MESSAGE_ID_CATA = 0x0015 } ISOMessageID; -void getSupportedISOProtocolVersions(const uint8_t** supportedProtocolVersions, size_t* nProtocols); ISOMessageReturnValue buildMONRMessage(const char * MonrData, const size_t length, MONRType * MONRData, const char debug); ISOMessageReturnValue MONRToASCII(const MONRType * MONRData, char * asciiBuffer, const size_t bufferLength, const char debug); ISOMessageReturnValue ASCIIToMONR(const char * asciiBuffer, MONRType * MONRData, const char debug); diff --git a/server/src/iso22133.c b/server/src/iso22133.c index ca8baf5ea..4ab39a8f7 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -10,12 +10,7 @@ static const uint8_t SupportedProtocolVersions[] = { 2 }; static ISOMessageReturnValue buildISOHeader(const char * MessageBuffer, const size_t length, HeaderType * HeaderData, const char debug); static ISOMessageReturnValue buildISOFooter(const char * MessageBuffer, const size_t length, FooterType * HeaderData, const char debug); - // ************************** function definitions -void getSupportedISOProtocolVersions(const uint8_t ** supportedProtocolVersions, size_t * nProtocols) { - *supportedProtocolVersions = SupportedProtocolVersions; - *nProtocols = sizeof (SupportedProtocolVersions) / sizeof (SupportedProtocolVersions[0]); -} /*! * \brief buildISOHeader Convert data in a buffer to an ISO header @@ -31,8 +26,6 @@ ISOMessageReturnValue buildISOHeader(const char* MessageBuffer, const size_t len const char ProtocolVersionBitmask = 0x7F; char messageProtocolVersion = 0; char isProtocolVersionSupported = 0; - const uint8_t *supportedProtocolVersions; - size_t nSupportedProtocols = 0; if (length < sizeof (HeaderData)) { LogMessage(LOG_LEVEL_ERROR, "Too little raw data to fill ISO header"); @@ -61,9 +54,8 @@ ISOMessageReturnValue buildISOHeader(const char* MessageBuffer, const size_t len // Loop over permitted protocol versions messageProtocolVersion = HeaderData->AckReqProtVerU8 & ProtocolVersionBitmask; - getSupportedISOProtocolVersions(&supportedProtocolVersions, &nSupportedProtocols); - for (size_t i = 0; i < nSupportedProtocols; ++i) { - if (supportedProtocolVersions[i] == messageProtocolVersion) { + for (size_t i = 0; i < sizeof (SupportedProtocolVersions) / sizeof (SupportedProtocolVersions[0]); ++i) { + if (SupportedProtocolVersions[i] == messageProtocolVersion) { isProtocolVersionSupported = 1; break; } From 7f237cd5fa70ddfa19c36da2f357b58c7069a759 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 20 Jan 2020 18:20:25 +0100 Subject: [PATCH 089/523] Implemented a function which maps raw data to an enum ISO message value --- server/inc/iso22133.h | 26 +++++++++++++++++++++++++- server/src/iso22133.c | 21 +++++++++++++++++++++ 2 files changed, 46 insertions(+), 1 deletion(-) diff --git a/server/inc/iso22133.h b/server/inc/iso22133.h index d676e36d4..0701cd555 100644 --- a/server/inc/iso22133.h +++ b/server/inc/iso22133.h @@ -261,17 +261,41 @@ typedef enum { } ISOMessageReturnValue; typedef enum { + MESSAGE_ID_INVALID = 0x0000, + MESSAGE_ID_TRAJ = 0x0001, + MESSAGE_ID_OSEM = 0x0002, + MESSAGE_ID_OSTM = 0x0003, + MESSAGE_ID_STRT = 0x0004, + MESSAGE_ID_HEAB = 0x0005, MESSAGE_ID_MONR = 0x0006, + MESSAGE_ID_MONR2 = 0x0007, + MESSAGE_ID_SOWM = 0x0008, + MESSAGE_ID_INFO = 0x0009, + MESSAGE_ID_RCMM = 0x000A, + MESSAGE_ID_SYPM = 0x000B, + MESSAGE_ID_MTSP = 0x000C, MESSAGE_ID_TRCM = 0x0011, MESSAGE_ID_ACCM = 0x0012, MESSAGE_ID_TREO = 0x0013, MESSAGE_ID_EXAC = 0x0014, - MESSAGE_ID_CATA = 0x0015 + MESSAGE_ID_CATA = 0x0015, + MESSAGE_ID_RCCM = 0x0020, + MESSAGE_ID_RCRT = 0x0021, + MESSAGE_ID_PIME = 0x0030, + MESSAGE_ID_COSE = 0x0031, + MESSAGE_ID_MOMA = 0x0032, + MESSAGE_ID_RESERVE_RANGE_1_LOWER_LIMIT = 0x0100, + MESSAGE_ID_RESERVE_RANGE_1_UPPER_LIMIT = 0x0FFF, + MESSAGE_ID_RESERVE_RANGE_2_LOWER_LIMIT = 0xF000, + MESSAGE_ID_RESERVE_RANGE_2_UPPER_LIMIT = 0xFFFF, + MESSAGE_ID_VENDOR_SPECIFIC_LOWER_LIMIT = 0xA100, + MESSAGE_ID_VENDOR_SPECIFIC_UPPER_LIMIT = 0xBFFF } ISOMessageID; ISOMessageReturnValue buildMONRMessage(const char * MonrData, const size_t length, MONRType * MONRData, const char debug); ISOMessageReturnValue MONRToASCII(const MONRType * MONRData, char * asciiBuffer, const size_t bufferLength, const char debug); ISOMessageReturnValue ASCIIToMONR(const char * asciiBuffer, MONRType * MONRData, const char debug); +ISOMessageID getISOMessageType(const char * messageData, const size_t length, const char debug); #ifdef __cplusplus } diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 4ab39a8f7..d2af012a9 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -9,6 +9,7 @@ static const uint8_t SupportedProtocolVersions[] = { 2 }; // ************************** static functions static ISOMessageReturnValue buildISOHeader(const char * MessageBuffer, const size_t length, HeaderType * HeaderData, const char debug); static ISOMessageReturnValue buildISOFooter(const char * MessageBuffer, const size_t length, FooterType * HeaderData, const char debug); +static char isValidMessageId(const uint16_t id); // ************************** function definitions @@ -108,6 +109,26 @@ ISOMessageReturnValue buildISOFooter(const char* MessageBuffer, const size_t len } +char isValidMessageID(const uint16_t id) { + return id == MESSAGE_ID_MONR || id == MESSAGE_ID_HEAB || id == MESSAGE_ID_TRAJ || id == MESSAGE_ID_OSEM + || id == MESSAGE_ID_OSTM || id == MESSAGE_ID_STRT || id == MESSAGE_ID_MONR2 || id == MESSAGE_ID_SOWM + || 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_VENDOR_SPECIFIC_LOWER_LIMIT && id <= MESSAGE_ID_VENDOR_SPECIFIC_UPPER_LIMIT); +} + + +ISOMessageID getISOMessageType(const char * messageData, const size_t length, const char debug) { + HeaderType header; + buildISOHeader(messageData, length, &header, debug); + if (isValidMessageId(header.MessageIdU16)) + return (ISOMessageID) header.MessageIdU16; + else + return MESSAGE_ID_INVALID; +} + /*! * \brief buildMONRMessage Fills a MONRType struct from a buffer of raw data * \param MonrData Raw data to be decoded From 936f9f89c39dee9cd7626289dd04300624ecb270 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 20 Jan 2020 18:28:06 +0100 Subject: [PATCH 090/523] Added function comments --- server/src/iso22133.c | 24 ++++++++++++++++++++---- 1 file changed, 20 insertions(+), 4 deletions(-) diff --git a/server/src/iso22133.c b/server/src/iso22133.c index d2af012a9..f6576b2c8 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -108,7 +108,12 @@ ISOMessageReturnValue buildISOFooter(const char* MessageBuffer, const size_t len return MESSAGE_OK; } - +/*! + * \brief isValidMessageID Determines if specified id is a valid ISO message ID. The reserved range is deemed + * invalid and vendor specific range is deemed valid. + * \param id An ISO message id to be checked + * \return 1 if valid, 0 if not + */ char isValidMessageID(const uint16_t id) { return id == MESSAGE_ID_MONR || id == MESSAGE_ID_HEAB || id == MESSAGE_ID_TRAJ || id == MESSAGE_ID_OSEM || id == MESSAGE_ID_OSTM || id == MESSAGE_ID_STRT || id == MESSAGE_ID_MONR2 || id == MESSAGE_ID_SOWM @@ -119,14 +124,25 @@ char isValidMessageID(const uint16_t id) { || (id >= MESSAGE_ID_VENDOR_SPECIFIC_LOWER_LIMIT && id <= MESSAGE_ID_VENDOR_SPECIFIC_UPPER_LIMIT); } - +/*! + * \brief getISOMessageType Determines the ISO message type of a raw data buffer + * \param messageData Buffer containing raw data to be parsed into an ISO message + * \param length Size of buffer to be parsed + * \param debug Flag for enabling debugging information + * \return Value according to ::ISOMessageID + */ ISOMessageID getISOMessageType(const char * messageData, const size_t length, const char debug) { HeaderType header; - buildISOHeader(messageData, length, &header, debug); + if (buildISOHeader(messageData, length, &header, debug) != MESSAGE_OK) { + LogMessage(LOG_LEVEL_ERROR, "Unable to parse raw data into ISO message header"); + return MESSAGE_ID_INVALID; + } if (isValidMessageId(header.MessageIdU16)) return (ISOMessageID) header.MessageIdU16; - else + else { + LogMessage(LOG_LEVEL_WARNING, "Message ID %u does not match any known ISO message", header.MessageIdU16); return MESSAGE_ID_INVALID; + } } /*! From e05c403907af0ff802910f45073bcd98357dfd58 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 20 Jan 2020 18:29:09 +0100 Subject: [PATCH 091/523] Ran code formatter --- server/src/iso22133.c | 34 ++++++++++++++++++++-------------- server/src/objectcontrol.c | 14 ++++++++------ server/src/util.c | 5 +++-- 3 files changed, 31 insertions(+), 22 deletions(-) diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 4ab39a8f7..56c7f58c3 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -7,8 +7,10 @@ static const uint8_t SupportedProtocolVersions[] = { 2 }; // ************************** static functions -static ISOMessageReturnValue buildISOHeader(const char * MessageBuffer, const size_t length, HeaderType * HeaderData, const char debug); -static ISOMessageReturnValue buildISOFooter(const char * MessageBuffer, const size_t length, FooterType * HeaderData, const char debug); +static ISOMessageReturnValue buildISOHeader(const char *MessageBuffer, const size_t length, + HeaderType * HeaderData, const char debug); +static ISOMessageReturnValue buildISOFooter(const char *MessageBuffer, const size_t length, + FooterType * HeaderData, const char debug); // ************************** function definitions @@ -20,7 +22,8 @@ static ISOMessageReturnValue buildISOFooter(const char * MessageBuffer, const si * \param debug Flag for enabling debugging of this function * \return value according to ::ISOMessageReturnValue */ -ISOMessageReturnValue buildISOHeader(const char* MessageBuffer, const size_t length, HeaderType * HeaderData, const char debug) { +ISOMessageReturnValue buildISOHeader(const char *MessageBuffer, const size_t length, HeaderType * HeaderData, + const char debug) { const char *p = MessageBuffer; ISOMessageReturnValue retval = MESSAGE_OK; const char ProtocolVersionBitmask = 0x7F; @@ -94,7 +97,8 @@ ISOMessageReturnValue buildISOHeader(const char* MessageBuffer, const size_t len * \param debug Flag for enabling debugging of this function * \return value according to ::ISOMessageReturnValue */ -ISOMessageReturnValue buildISOFooter(const char* MessageBuffer, const size_t length, FooterType * FooterData, const char debug) { +ISOMessageReturnValue buildISOFooter(const char *MessageBuffer, const size_t length, FooterType * FooterData, + const char debug) { if (length < sizeof (FooterData->Crc)) { LogMessage(LOG_LEVEL_ERROR, "Too little raw data to fill ISO footer"); @@ -115,13 +119,14 @@ ISOMessageReturnValue buildISOFooter(const char* MessageBuffer, const size_t len * \param debug Flag for enabling of debugging * \return value according to ::ISOMessageReturnValue */ -ISOMessageReturnValue buildMONRMessage(const char * MonrData, const size_t length, MONRType * MONRData, const char debug) { +ISOMessageReturnValue buildMONRMessage(const char *MonrData, const size_t length, MONRType * MONRData, + const char debug) { const char *p = MonrData; const uint16_t ExpectedMONRStructSize = (uint16_t) (sizeof (*MONRData) - sizeof (MONRData->header) - - sizeof (MONRData->footer.Crc) - - sizeof (MONRData->monrStructValueID) - - sizeof (MONRData->monrStructContentLength)); + - sizeof (MONRData->footer.Crc) - + sizeof (MONRData->monrStructValueID) + - sizeof (MONRData->monrStructContentLength)); ISOMessageReturnValue retval = MESSAGE_OK; // Decode ISO header @@ -197,7 +202,8 @@ ISOMessageReturnValue buildMONRMessage(const char * MonrData, const size_t lengt p += sizeof (MONRData->errorStatus); // Footer - if ((retval = buildISOFooter(p, length-(size_t)(p-MonrData), &MONRData->footer, debug)) != MESSAGE_OK) { + if ((retval = + buildISOFooter(p, length - (size_t) (p - MonrData), &MONRData->footer, debug)) != MESSAGE_OK) { memset(MONRData, 0, sizeof (*MONRData)); return retval; } @@ -240,7 +246,8 @@ ISOMessageReturnValue buildMONRMessage(const char * MonrData, const size_t lengt * \param debug Flag for enabling debugging * \return value according to ::ISOMessageReturnValue */ -ISOMessageReturnValue MONRToASCII(const MONRType * MONRData, char * asciiBuffer, const size_t bufferLength, const char debug) { +ISOMessageReturnValue MONRToASCII(const MONRType * MONRData, char *asciiBuffer, const size_t bufferLength, + const char debug) { memset(asciiBuffer, 0, bufferLength); if (MONRData->header.MessageIdU16 != MESSAGE_ID_MONR) { @@ -250,8 +257,7 @@ ISOMessageReturnValue MONRToASCII(const MONRType * MONRData, char * asciiBuffer, sprintf(asciiBuffer + strlen(asciiBuffer), "%u;", MONRData->gpsQmsOfWeek); sprintf(asciiBuffer + strlen(asciiBuffer), "%d;%d;%d;%u;", - MONRData->xPosition, MONRData->yPosition, MONRData->zPosition, - MONRData->heading); + MONRData->xPosition, MONRData->yPosition, MONRData->zPosition, MONRData->heading); sprintf(asciiBuffer + strlen(asciiBuffer), "%d;%d;%d;%d;", MONRData->longitudinalSpeed, MONRData->lateralSpeed, MONRData->longitudinalAcc, MONRData->lateralAcc); sprintf(asciiBuffer + strlen(asciiBuffer), "%u;%u;%u;%u;", MONRData->driveDirection, @@ -268,13 +274,13 @@ ISOMessageReturnValue MONRToASCII(const MONRType * MONRData, char * asciiBuffer, * \param debug Flag for enabling debugging * \return value according to ::ISOMessageReturnValue */ -ISOMessageReturnValue ASCIIToMONR(const char * asciiBuffer, MONRType * MONRData, const char debug) { +ISOMessageReturnValue ASCIIToMONR(const char *asciiBuffer, MONRType * MONRData, const char debug) { const size_t bufferLength = strlen(asciiBuffer); const char *token; const char delim[] = ";"; const int NumberBaseDecimal = 10; - char * copy = strdup(asciiBuffer); + char *copy = strdup(asciiBuffer); memset(MONRData, 0, sizeof (*MONRData)); diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index a7f16248d..dbd367d88 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -491,12 +491,12 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { memset(buffer, 0, sizeof (buffer)); memcpy(buffer, object_address_name[iIndex], strlen(object_address_name[iIndex])); strcat(buffer, ";0;"); - MONRToASCII(&MONRData, buffer+strlen(buffer), sizeof (buffer)-strlen(buffer), 0); + MONRToASCII(&MONRData, buffer + strlen(buffer), sizeof (buffer) - strlen(buffer), 0); if (ASPData.MTSPU32 != 0) { //Add MTSP to MONR if not 0 - sprintf(buffer+strlen(buffer), "%" PRIu32 ";", ASPData.MTSPU32); + sprintf(buffer + strlen(buffer), "%" PRIu32 ";", ASPData.MTSPU32); } //Ok, let's do the ASP @@ -512,14 +512,15 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { TimeCap1 = (uint64_t) CurrentTimeStruct.tv_sec * 1000 + (uint64_t) CurrentTimeStruct.tv_usec / 1000; //Calculate initial timestamp - OP[iIndex].x = ((double) MONRData.xPosition) / 1000; //Set x and y on OP (ObjectPosition) - OP[iIndex].y = ((double) MONRData.yPosition) / 1000; + OP[iIndex].x = ((double)MONRData.xPosition) / 1000; //Set x and y on OP (ObjectPosition) + OP[iIndex].y = ((double)MONRData.yPosition) / 1000; //OP[iIndex].OrigoDistance = sqrt(pow(OP[iIndex].x,2) + pow(OP[iIndex].y,2)); //Calculate hypotenuse // TODO: check use of this function since it should take two lat/long points but is here used with x/y UtilCalcPositionDelta(OriginLatitudeDbl, OriginLongitudeDbl, - MONRData.xPosition / 1e7, MONRData.yPosition / 1e7, &OP[iIndex]); + MONRData.xPosition / 1e7, MONRData.yPosition / 1e7, + &OP[iIndex]); if (OP[iIndex].BestFoundTrajectoryIndex <= OP[iIndex].SyncIndex) { ASPData.CurrentTimeU32 = CurrentTimeU32; @@ -585,7 +586,8 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { } } } - OP[iIndex].Speed = sqrt(pow(MONRData.lateralSpeed, 2) + pow(MONRData.longitudinalSpeed, 2)); + OP[iIndex].Speed = + sqrt(pow(MONRData.lateralSpeed, 2) + pow(MONRData.longitudinalSpeed, 2)); } else if (receivedMONRData > 0) LogMessage(LOG_LEVEL_INFO, "MONR length error (should be %d but is %ld) from %s.", diff --git a/server/src/util.c b/server/src/util.c index 3e49e8d8f..21ad73993 100644 --- a/server/src/util.c +++ b/server/src/util.c @@ -720,7 +720,8 @@ int UtilMonitorDataToString(MonitorDataType monrData, char *monrString, size_t s inet_ntop(AF_INET, &monrData.ClientIP, monrString, (stringLength > UINT_MAX) ? UINT_MAX : (socklen_t) stringLength); strcat(monrString, ";0;"); - if (MONRToASCII(&monrData.MONR, monrString + strlen(monrString), stringLength - strlen(monrString), 0) != MESSAGE_OK) { + if (MONRToASCII(&monrData.MONR, monrString + strlen(monrString), stringLength - strlen(monrString), 0) != + MESSAGE_OK) { memset(monrString, 0, stringLength); return -1; } @@ -738,7 +739,7 @@ int UtilStringToMonitorData(const char *monrString, size_t stringLength, Monitor const char *token; const char delim[] = ";"; struct in_addr addr; - char * copy = strdup(monrString); + char *copy = strdup(monrString); token = strtok(copy, delim); From b46900af8f8e5ef74d7c6288c9cc36db61f71c00 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 20 Jan 2020 18:30:50 +0100 Subject: [PATCH 092/523] Ran code formatter --- server/src/iso22133.c | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/server/src/iso22133.c b/server/src/iso22133.c index d287fabe7..41829ebb7 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -120,12 +120,12 @@ ISOMessageReturnValue buildISOFooter(const char *MessageBuffer, const size_t len */ char isValidMessageID(const uint16_t id) { return id == MESSAGE_ID_MONR || id == MESSAGE_ID_HEAB || id == MESSAGE_ID_TRAJ || id == MESSAGE_ID_OSEM - || id == MESSAGE_ID_OSTM || id == MESSAGE_ID_STRT || id == MESSAGE_ID_MONR2 || id == MESSAGE_ID_SOWM - || 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_VENDOR_SPECIFIC_LOWER_LIMIT && id <= MESSAGE_ID_VENDOR_SPECIFIC_UPPER_LIMIT); + || id == MESSAGE_ID_OSTM || id == MESSAGE_ID_STRT || id == MESSAGE_ID_MONR2 || id == MESSAGE_ID_SOWM + || 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_VENDOR_SPECIFIC_LOWER_LIMIT && id <= MESSAGE_ID_VENDOR_SPECIFIC_UPPER_LIMIT); } /*! @@ -135,8 +135,9 @@ char isValidMessageID(const uint16_t id) { * \param debug Flag for enabling debugging information * \return Value according to ::ISOMessageID */ -ISOMessageID getISOMessageType(const char * messageData, const size_t length, const char debug) { +ISOMessageID getISOMessageType(const char *messageData, const size_t length, const char debug) { HeaderType header; + if (buildISOHeader(messageData, length, &header, debug) != MESSAGE_OK) { LogMessage(LOG_LEVEL_ERROR, "Unable to parse raw data into ISO message header"); return MESSAGE_ID_INVALID; @@ -144,7 +145,8 @@ ISOMessageID getISOMessageType(const char * messageData, const size_t length, co if (isValidMessageId(header.MessageIdU16)) return (ISOMessageID) header.MessageIdU16; else { - LogMessage(LOG_LEVEL_WARNING, "Message ID %u does not match any known ISO message", header.MessageIdU16); + LogMessage(LOG_LEVEL_WARNING, "Message ID %u does not match any known ISO message", + header.MessageIdU16); return MESSAGE_ID_INVALID; } } From db1aa50c7507ef9607305c91697e60c8d40132bf Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 21 Jan 2020 08:54:15 +0100 Subject: [PATCH 093/523] Fixed typo --- server/src/iso22133.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 41829ebb7..5c181cf16 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -11,7 +11,7 @@ static ISOMessageReturnValue buildISOHeader(const char *MessageBuffer, const siz HeaderType * HeaderData, const char debug); static ISOMessageReturnValue buildISOFooter(const char *MessageBuffer, const size_t length, FooterType * HeaderData, const char debug); -static char isValidMessageId(const uint16_t id); +static char isValidMessageID(const uint16_t id); // ************************** function definitions @@ -142,7 +142,7 @@ ISOMessageID getISOMessageType(const char *messageData, const size_t length, con LogMessage(LOG_LEVEL_ERROR, "Unable to parse raw data into ISO message header"); return MESSAGE_ID_INVALID; } - if (isValidMessageId(header.MessageIdU16)) + if (isValidMessageID(header.MessageIdU16)) return (ISOMessageID) header.MessageIdU16; else { LogMessage(LOG_LEVEL_WARNING, "Message ID %u does not match any known ISO message", From 182828e5e814d4f9f81e8d84d6f49abab18de4de Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 21 Jan 2020 08:59:53 +0100 Subject: [PATCH 094/523] Refactor --- server/inc/iso22133.h | 2 +- server/src/iso22133.c | 2 +- server/src/objectcontrol.c | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/server/inc/iso22133.h b/server/inc/iso22133.h index d676e36d4..f338c09f4 100644 --- a/server/inc/iso22133.h +++ b/server/inc/iso22133.h @@ -269,7 +269,7 @@ typedef enum { MESSAGE_ID_CATA = 0x0015 } ISOMessageID; -ISOMessageReturnValue buildMONRMessage(const char * MonrData, const size_t length, MONRType * MONRData, const char debug); +ISOMessageReturnValue decodeMONRMessage(const char * MonrData, const size_t length, MONRType * MONRData, const char debug); ISOMessageReturnValue MONRToASCII(const MONRType * MONRData, char * asciiBuffer, const size_t bufferLength, const char debug); ISOMessageReturnValue ASCIIToMONR(const char * asciiBuffer, MONRType * MONRData, const char debug); diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 56c7f58c3..2a6b7ac57 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -119,7 +119,7 @@ ISOMessageReturnValue buildISOFooter(const char *MessageBuffer, const size_t len * \param debug Flag for enabling of debugging * \return value according to ::ISOMessageReturnValue */ -ISOMessageReturnValue buildMONRMessage(const char *MonrData, const size_t length, MONRType * MONRData, +ISOMessageReturnValue decodeMONRMessage(const char *MonrData, const size_t length, MONRType * MONRData, const char debug) { const char *p = MonrData; diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index dbd367d88..f884ac120 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -457,7 +457,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { object_address_name[iIndex], object_udp_port[iIndex], receivedMONRData, buffer); - if (buildMONRMessage(buffer, receivedMONRData, &MONRData, 0) != MESSAGE_OK) { + if (decodeMONRMessage(buffer, receivedMONRData, &MONRData, 0) != MESSAGE_OK) { // TODO react on error continue; } From 32676fd69ce5b3aebdbc21e45aaeb3f805dc7ffc Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 21 Jan 2020 10:46:29 +0100 Subject: [PATCH 095/523] Moved STRT typedef to iso22133 --- server/inc/iso22133.h | 14 ++++++++++++++ server/inc/util.h | 14 -------------- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/server/inc/iso22133.h b/server/inc/iso22133.h index b5859fb7f..668d81d71 100644 --- a/server/inc/iso22133.h +++ b/server/inc/iso22133.h @@ -32,6 +32,19 @@ typedef struct } FooterType; //2 bytes +typedef struct +{ + HeaderType Header; + uint16_t StartTimeValueIdU16; + uint16_t StartTimeContentLengthU16; + uint32_t StartTimeU32; + uint16_t GPSWeekValueIdU16; + uint16_t GPSWeekContentLengthU16; + uint16_t GPSWeekU16; + FooterType footer; +} STRTType; //27 bytes + + //! *************************** MONR #define COMMAND_MONR_CODE 0x0006 typedef struct @@ -293,6 +306,7 @@ typedef enum { } ISOMessageID; ISOMessageReturnValue decodeMONRMessage(const char * MonrData, const size_t length, MONRType * MONRData, const char debug); +ISOMessageReturnValue encodeSTRTMessage(const STRTType * STRTData, char * strtData, const size_t length, const char debug); ISOMessageReturnValue MONRToASCII(const MONRType * MONRData, char * asciiBuffer, const size_t bufferLength, const char debug); ISOMessageReturnValue ASCIIToMONR(const char * asciiBuffer, MONRType * MONRData, const char debug); ISOMessageID getISOMessageType(const char * messageData, const size_t length, const char debug); diff --git a/server/inc/util.h b/server/inc/util.h index 6a12efbb0..499d8f9d2 100644 --- a/server/inc/util.h +++ b/server/inc/util.h @@ -313,20 +313,6 @@ typedef struct U16 MinPosAccuracyU16; } OSEMType; //85 bytes -typedef struct -{ - HeaderType Header; - U16 StartTimeValueIdU16; - U16 StartTimeContentLengthU16; - U32 StartTimeU32; - U16 GPSWeekValueIdU16; - U16 GPSWeekContentLengthU16; - U16 GPSWeekU16; - // U16 DelayStartValueIdU16; - // U16 DelayStartContentLengthU16; - // U32 DelayStartU32; -} STRTType; //27 bytes - typedef struct { HeaderType Header; From f80f26aa3bd7940e375fb429dcc1fa4f246e257a Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 21 Jan 2020 10:47:31 +0100 Subject: [PATCH 096/523] Refactoring --- server/inc/iso22133.h | 330 +++++++++++++++++++++--------------------- 1 file changed, 165 insertions(+), 165 deletions(-) diff --git a/server/inc/iso22133.h b/server/inc/iso22133.h index 668d81d71..933b89972 100644 --- a/server/inc/iso22133.h +++ b/server/inc/iso22133.h @@ -18,30 +18,30 @@ extern "C" { #pragma pack(push,1) typedef struct { - uint16_t SyncWordU16; - uint8_t TransmitterIdU8; - uint8_t MessageCounterU8; - uint8_t AckReqProtVerU8; - uint16_t MessageIdU16; - uint32_t MessageLengthU32; + uint16_t SyncWordU16; + uint8_t TransmitterIdU8; + uint8_t MessageCounterU8; + uint8_t AckReqProtVerU8; + uint16_t MessageIdU16; + uint32_t MessageLengthU32; } HeaderType; //11 bytes typedef struct { - uint16_t Crc; + uint16_t Crc; } FooterType; //2 bytes typedef struct { - HeaderType Header; - uint16_t StartTimeValueIdU16; - uint16_t StartTimeContentLengthU16; - uint32_t StartTimeU32; - uint16_t GPSWeekValueIdU16; - uint16_t GPSWeekContentLengthU16; - uint16_t GPSWeekU16; - FooterType footer; + HeaderType Header; + uint16_t StartTimeValueIdU16; + uint16_t StartTimeContentLengthU16; + uint32_t StartTimeU32; + uint16_t GPSWeekValueIdU16; + uint16_t GPSWeekContentLengthU16; + uint16_t GPSWeekU16; + FooterType footer; } STRTType; //27 bytes @@ -49,23 +49,23 @@ typedef struct #define COMMAND_MONR_CODE 0x0006 typedef struct { - HeaderType header; - uint16_t monrStructValueID; - uint16_t monrStructContentLength; - uint32_t gpsQmsOfWeek; - int32_t xPosition; - int32_t yPosition; - int32_t zPosition; - uint16_t heading; - int16_t longitudinalSpeed; - int16_t lateralSpeed; - int16_t longitudinalAcc; - int16_t lateralAcc; - uint8_t driveDirection; - uint8_t state; - uint8_t readyToArm; - uint8_t errorStatus; - FooterType footer; + HeaderType header; + uint16_t monrStructValueID; + uint16_t monrStructContentLength; + uint32_t gpsQmsOfWeek; + int32_t xPosition; + int32_t yPosition; + int32_t zPosition; + uint16_t heading; + int16_t longitudinalSpeed; + int16_t lateralSpeed; + int16_t longitudinalAcc; + int16_t lateralAcc; + uint8_t driveDirection; + uint8_t state; + uint8_t readyToArm; + uint8_t errorStatus; + FooterType footer; } MONRType; //! MONR value IDs @@ -76,79 +76,79 @@ typedef struct #define COMMAND_TRCM_CODE 0x0011 typedef struct { - HeaderType header; - uint16_t triggerIDValueID; - uint16_t triggerIDContentLength; - uint16_t triggerID; - uint16_t triggerTypeValueID; - uint16_t triggerTypeContentLength; - uint16_t triggerType; - uint16_t triggerTypeParameter1ValueID; - uint16_t triggerTypeParameter1ContentLength; - uint32_t triggerTypeParameter1; - uint16_t triggerTypeParameter2ValueID; - uint16_t triggerTypeParameter2ContentLength; - uint32_t triggerTypeParameter2; - uint16_t triggerTypeParameter3ValueID; - uint16_t triggerTypeParameter3ContentLength; - uint32_t triggerTypeParameter3; - FooterType footer; + HeaderType header; + uint16_t triggerIDValueID; + uint16_t triggerIDContentLength; + uint16_t triggerID; + uint16_t triggerTypeValueID; + uint16_t triggerTypeContentLength; + uint16_t triggerType; + uint16_t triggerTypeParameter1ValueID; + uint16_t triggerTypeParameter1ContentLength; + uint32_t triggerTypeParameter1; + uint16_t triggerTypeParameter2ValueID; + uint16_t triggerTypeParameter2ContentLength; + uint32_t triggerTypeParameter2; + uint16_t triggerTypeParameter3ValueID; + uint16_t triggerTypeParameter3ContentLength; + uint32_t triggerTypeParameter3; + FooterType footer; } TRCMType; typedef enum { - TRIGGER_UNDEFINED = 0x0000, - TRIGGER_TYPE_1 = 0x0001, - TRIGGER_SPEED = 0x0010, - TRIGGER_DISTANCE = 0x0020, - TRIGGER_ACCELERATION = 0x0030, - TRIGGER_LANE_CHANGED = 0x0040, - TRIGGER_LANE_OFFSET = 0x0050, - TRIGGER_POSITION_REACHED = 0x0060, - TRIGGER_POSITION_LEFT = 0x0061, - TRIGGER_POSITION_OFFSET = 0x0062, - TRIGGER_STEERING_ANGLE = 0x0070, - TRIGGER_THROTTLE_VALUE = 0x0080, - TRIGGER_BRAKE = 0x0090, - TRIGGER_ACTIVE_TRAJECTORY = 0x00A0, - TRIGGER_OTHER_OBJECT_FEATURE = 0x00B0, - TRIGGER_INFRASTRUCTURE = 0x00C0, - TRIGGER_TEST_SCENARIO_EVENT = 0x00D0, - TRIGGER_MISC_DIGITAL_INPUT = 0x00E0, - TRIGGER_MISC_ANALOG_INPUT = 0x00F0, - TRIGGER_TIMER_EVENT_OCCURRED = 0x0100, - TRIGGER_MODE_CHANGED = 0x0110, - TRIGGER_UNAVAILABLE = 0xFFFF + TRIGGER_UNDEFINED = 0x0000, + TRIGGER_TYPE_1 = 0x0001, + TRIGGER_SPEED = 0x0010, + TRIGGER_DISTANCE = 0x0020, + TRIGGER_ACCELERATION = 0x0030, + TRIGGER_LANE_CHANGED = 0x0040, + TRIGGER_LANE_OFFSET = 0x0050, + TRIGGER_POSITION_REACHED = 0x0060, + TRIGGER_POSITION_LEFT = 0x0061, + TRIGGER_POSITION_OFFSET = 0x0062, + TRIGGER_STEERING_ANGLE = 0x0070, + TRIGGER_THROTTLE_VALUE = 0x0080, + TRIGGER_BRAKE = 0x0090, + TRIGGER_ACTIVE_TRAJECTORY = 0x00A0, + TRIGGER_OTHER_OBJECT_FEATURE = 0x00B0, + TRIGGER_INFRASTRUCTURE = 0x00C0, + TRIGGER_TEST_SCENARIO_EVENT = 0x00D0, + TRIGGER_MISC_DIGITAL_INPUT = 0x00E0, + TRIGGER_MISC_ANALOG_INPUT = 0x00F0, + TRIGGER_TIMER_EVENT_OCCURRED = 0x0100, + TRIGGER_MODE_CHANGED = 0x0110, + TRIGGER_UNAVAILABLE = 0xFFFF } TriggerType_t; typedef enum { - TRIGGER_PARAMETER_FALSE = 0x00000000, - TRIGGER_PARAMETER_TRUE = 0x00000001, - TRIGGER_PARAMETER_RELEASED = 0x00000010, - TRIGGER_PARAMETER_PRESSED = 0x00000011, - TRIGGER_PARAMETER_LOW = 0x00000020, - TRIGGER_PARAMETER_HIGH = 0x00000021, - TRIGGER_PARAMETER_RISING_EDGE = 0x00000022, - TRIGGER_PARAMETER_FALLING_EDGE = 0x00000023, - TRIGGER_PARAMETER_ANY_EDGE = 0x00000024, - TRIGGER_PARAMETER_RELATIVE = 0x00000030, - TRIGGER_PARAMETER_ABSOLUTE = 0x00000031, - TRIGGER_PARAMETER_VALUE = 0x00000040, - TRIGGER_PARAMETER_MIN = 0x00000050, - TRIGGER_PARAMETER_MAX = 0x00000051, - TRIGGER_PARAMETER_MEAN = 0x00000052, - TRIGGER_PARAMETER_EQUAL_TO = 0x00000060, - TRIGGER_PARAMETER_GREATER_THAN = 0x00000061, - TRIGGER_PARAMETER_GREATER_THAN_OR_EQUAL_TO = 0x00000062, - TRIGGER_PARAMETER_LESS_THAN = 0x00000063, - TRIGGER_PARAMETER_LESS_THAN_OR_EQUAL_TO = 0x00000064, - TRIGGER_PARAMETER_NOT_EQUAL_TO = 0x00000065, - TRIGGER_PARAMETER_X = 0x00000070, - TRIGGER_PARAMETER_Y = 0x00000071, - TRIGGER_PARAMETER_Z = 0x00000072, - TRIGGER_PARAMETER_TIME = 0x00000080, - TRIGGER_PARAMETER_DATE = 0x00000081, - TRIGGER_PARAMETER_RULE = 0x000000A0, - TRIGGER_PARAMETER_UNAVAILABLE = 0xFFFFFFFF + TRIGGER_PARAMETER_FALSE = 0x00000000, + TRIGGER_PARAMETER_TRUE = 0x00000001, + TRIGGER_PARAMETER_RELEASED = 0x00000010, + TRIGGER_PARAMETER_PRESSED = 0x00000011, + TRIGGER_PARAMETER_LOW = 0x00000020, + TRIGGER_PARAMETER_HIGH = 0x00000021, + TRIGGER_PARAMETER_RISING_EDGE = 0x00000022, + TRIGGER_PARAMETER_FALLING_EDGE = 0x00000023, + TRIGGER_PARAMETER_ANY_EDGE = 0x00000024, + TRIGGER_PARAMETER_RELATIVE = 0x00000030, + TRIGGER_PARAMETER_ABSOLUTE = 0x00000031, + TRIGGER_PARAMETER_VALUE = 0x00000040, + TRIGGER_PARAMETER_MIN = 0x00000050, + TRIGGER_PARAMETER_MAX = 0x00000051, + TRIGGER_PARAMETER_MEAN = 0x00000052, + TRIGGER_PARAMETER_EQUAL_TO = 0x00000060, + TRIGGER_PARAMETER_GREATER_THAN = 0x00000061, + TRIGGER_PARAMETER_GREATER_THAN_OR_EQUAL_TO = 0x00000062, + TRIGGER_PARAMETER_LESS_THAN = 0x00000063, + TRIGGER_PARAMETER_LESS_THAN_OR_EQUAL_TO = 0x00000064, + TRIGGER_PARAMETER_NOT_EQUAL_TO = 0x00000065, + TRIGGER_PARAMETER_X = 0x00000070, + TRIGGER_PARAMETER_Y = 0x00000071, + TRIGGER_PARAMETER_Z = 0x00000072, + TRIGGER_PARAMETER_TIME = 0x00000080, + TRIGGER_PARAMETER_DATE = 0x00000081, + TRIGGER_PARAMETER_RULE = 0x000000A0, + TRIGGER_PARAMETER_UNAVAILABLE = 0xFFFFFFFF } TriggerTypeParameter_t; @@ -156,62 +156,62 @@ typedef enum { #define COMMAND_ACCM_CODE 0x0012 typedef struct { - HeaderType header; - uint16_t actionIDValueID; - uint16_t actionIDContentLength; - uint16_t actionID; - uint16_t actionTypeValueID; - uint16_t actionTypeContentLength; - uint16_t actionType; - uint16_t actionTypeParameter1ValueID; - uint16_t actionTypeParameter1ContentLength; - uint32_t actionTypeParameter1; - uint16_t actionTypeParameter2ValueID; - uint16_t actionTypeParameter2ContentLength; - uint32_t actionTypeParameter2; - uint16_t actionTypeParameter3ValueID; - uint16_t actionTypeParameter3ContentLength; - uint32_t actionTypeParameter3; - FooterType footer; + HeaderType header; + uint16_t actionIDValueID; + uint16_t actionIDContentLength; + uint16_t actionID; + uint16_t actionTypeValueID; + uint16_t actionTypeContentLength; + uint16_t actionType; + uint16_t actionTypeParameter1ValueID; + uint16_t actionTypeParameter1ContentLength; + uint32_t actionTypeParameter1; + uint16_t actionTypeParameter2ValueID; + uint16_t actionTypeParameter2ContentLength; + uint32_t actionTypeParameter2; + uint16_t actionTypeParameter3ValueID; + uint16_t actionTypeParameter3ContentLength; + uint32_t actionTypeParameter3; + FooterType footer; } ACCMType; typedef enum { - ACTION_NONE = 0x0000, - ACTION_TYPE_1 = 0x0001, - ACTION_TYPE_2 = 0x0002, - ACTION_SET_SPEED = 0x0010, - ACTION_SET_DISTANCE = 0x0020, - ACTION_SET_ACCELERATION = 0x0030, - ACTION_LANE_CHANGE = 0x0040, - ACTION_LANE_OFFSET = 0x0050, - ACTION_SET_POSITION = 0x0060, - ACTION_SET_STEERING_ANGLE = 0x0070, - ACTION_SET_TRHOTTLE_VALUE = 0x0080, - ACTION_BRAKE = 0x0090, - ACTION_FOLLOW_TRAJECTORY = 0x00A0, - ACTION_OTHER_OBJECT_FEATURE = 0x00B0, - ACTION_INFRASTRUCTURE = 0x00C0, - ACTION_TEST_SCENARIO_COMMAND = 0x00D0, - ACTION_MISC_DIGITAL_OUTPUT = 0x00E0, - ACTION_MISC_ANALOG_OUTPUT = 0x00F0, - ACTION_START_TIMER = 0x0100, - ACTION_MODE_CHANGE = 0x0110, - ACTION_UNAVAILABLE = 0xFFFF + ACTION_NONE = 0x0000, + ACTION_TYPE_1 = 0x0001, + ACTION_TYPE_2 = 0x0002, + ACTION_SET_SPEED = 0x0010, + ACTION_SET_DISTANCE = 0x0020, + ACTION_SET_ACCELERATION = 0x0030, + ACTION_LANE_CHANGE = 0x0040, + ACTION_LANE_OFFSET = 0x0050, + ACTION_SET_POSITION = 0x0060, + ACTION_SET_STEERING_ANGLE = 0x0070, + ACTION_SET_TRHOTTLE_VALUE = 0x0080, + ACTION_BRAKE = 0x0090, + ACTION_FOLLOW_TRAJECTORY = 0x00A0, + ACTION_OTHER_OBJECT_FEATURE = 0x00B0, + ACTION_INFRASTRUCTURE = 0x00C0, + ACTION_TEST_SCENARIO_COMMAND = 0x00D0, + ACTION_MISC_DIGITAL_OUTPUT = 0x00E0, + ACTION_MISC_ANALOG_OUTPUT = 0x00F0, + ACTION_START_TIMER = 0x0100, + ACTION_MODE_CHANGE = 0x0110, + ACTION_UNAVAILABLE = 0xFFFF } ActionType_t; typedef enum { - ACTION_PARAMETER_SET_FALSE = 0x00000000, - ACTION_PARAMETER_SET_TRUE = 0x00000001, - ACTION_PARAMETER_RELEASE = 0x00000010, - ACTION_PARAMETER_PRESS = 0x00000011, - ACTION_PARAMETER_SET_VALUE = 0x00000020, - ACTION_PARAMETER_MIN = 0x00000040, - ACTION_PARAMETER_MAX = 0x00000041, - ACTION_PARAMETER_X = 0x00000070, - ACTION_PARAMETER_Y = 0x00000071, - ACTION_PARAMETER_Z = 0x00000072, - ACTION_PARAMETER_VS_BRAKE_WARNING = 0xA0000000, - ACTION_PARAMETER_UNAVAILABLE = 0xFFFFFFFF + ACTION_PARAMETER_SET_FALSE = 0x00000000, + ACTION_PARAMETER_SET_TRUE = 0x00000001, + ACTION_PARAMETER_RELEASE = 0x00000010, + ACTION_PARAMETER_PRESS = 0x00000011, + ACTION_PARAMETER_SET_VALUE = 0x00000020, + ACTION_PARAMETER_MIN = 0x00000040, + ACTION_PARAMETER_MAX = 0x00000041, + ACTION_PARAMETER_X = 0x00000070, + ACTION_PARAMETER_Y = 0x00000071, + ACTION_PARAMETER_Z = 0x00000072, + ACTION_PARAMETER_VS_BRAKE_WARNING = 0xA0000000, + ACTION_PARAMETER_UNAVAILABLE = 0xFFFFFFFF } ActionTypeParameter_t; @@ -219,14 +219,14 @@ typedef enum { #define COMMAND_TREO_CODE 0x0013 typedef struct { - HeaderType header; - uint16_t triggerIDValueID; - uint16_t triggerIDContentLength; - uint16_t triggerID; - uint16_t timestamp_qmsowValueID; - uint16_t timestamp_qmsowContentLength; - uint32_t timestamp_qmsow; - FooterType footer; + HeaderType header; + uint16_t triggerIDValueID; + uint16_t triggerIDContentLength; + uint16_t triggerID; + uint16_t timestamp_qmsowValueID; + uint16_t timestamp_qmsowContentLength; + uint32_t timestamp_qmsow; + FooterType footer; } TREOType; @@ -234,14 +234,14 @@ typedef struct #define COMMAND_EXAC_CODE 0x0014 typedef struct { - HeaderType header; - uint16_t actionIDValueID; - uint16_t actionIDContentLength; - uint16_t actionID; - uint16_t executionTime_qmsoWValueID; - uint16_t executionTime_qmsoWContentLength; - uint32_t executionTime_qmsoW; - FooterType footer; + HeaderType header; + uint16_t actionIDValueID; + uint16_t actionIDContentLength; + uint16_t actionID; + uint16_t executionTime_qmsoWValueID; + uint16_t executionTime_qmsoWContentLength; + uint32_t executionTime_qmsoW; + FooterType footer; } EXACType; From 3cc450a2cace56bc321aee4f01ad5350150a90c0 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 21 Jan 2020 12:06:33 +0100 Subject: [PATCH 097/523] Moved STRT builder function from OC to iso22133 and implemented everything except the time calculations --- server/inc/iso22133.h | 7 ++- server/src/iso22133.c | 112 +++++++++++++++++++++++++++++++++---- server/src/objectcontrol.c | 60 -------------------- 3 files changed, 106 insertions(+), 73 deletions(-) diff --git a/server/inc/iso22133.h b/server/inc/iso22133.h index 933b89972..c75503a20 100644 --- a/server/inc/iso22133.h +++ b/server/inc/iso22133.h @@ -12,6 +12,7 @@ extern "C" { #include #include +#include #define ISO_SYNC_WORD 0x7E7E @@ -44,6 +45,10 @@ typedef struct FooterType footer; } STRTType; //27 bytes +//! STRT value IDs +#define VALUE_ID_STRT_GPS_QMS_OF_WEEK 0x0002 +#define VALUE_ID_STRT_GPS_WEEK 0x0003 + //! *************************** MONR #define COMMAND_MONR_CODE 0x0006 @@ -306,7 +311,7 @@ typedef enum { } ISOMessageID; ISOMessageReturnValue decodeMONRMessage(const char * MonrData, const size_t length, MONRType * MONRData, const char debug); -ISOMessageReturnValue encodeSTRTMessage(const STRTType * STRTData, char * strtData, const size_t length, const char debug); +ssize_t encodeSTRTMessage(const STRTType * STRTData, char * strtData, const size_t length, const char debug); ISOMessageReturnValue MONRToASCII(const MONRType * MONRData, char * asciiBuffer, const size_t bufferLength, const char debug); ISOMessageReturnValue ASCIIToMONR(const char * asciiBuffer, MONRType * MONRData, const char debug); ISOMessageID getISOMessageType(const char * messageData, const size_t length, const char debug); diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 479e1d846..7a3c7147f 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -1,29 +1,31 @@ #include "iso22133.h" -#include "string.h" #include "logging.h" -#include "errno.h" -#include "stdlib.h" +#include +#include +#include static const uint8_t SupportedProtocolVersions[] = { 2 }; // ************************** static functions -static ISOMessageReturnValue buildISOHeader(const char *MessageBuffer, const size_t length, +static ISOMessageReturnValue decodeISOHeader(const char *MessageBuffer, const size_t length, HeaderType * HeaderData, const char debug); -static ISOMessageReturnValue buildISOFooter(const char *MessageBuffer, const size_t length, +static ISOMessageReturnValue decodeISOFooter(const char *MessageBuffer, const size_t length, FooterType * HeaderData, const char debug); +static HeaderType buildISOHeader(ISOMessageID id, uint32_t messageLength, const char debug); +static FooterType buildISOFooter(const void * message, const size_t sizeExclFooter, const char debug); static char isValidMessageID(const uint16_t id); // ************************** function definitions /*! - * \brief buildISOHeader Convert data in a buffer to an ISO header + * \brief decodeISOHeader Convert data in a buffer to an ISO header * \param MessageBuffer Buffer containing raw data to be converted * \param length Length of buffer * \param HeaderData Struct in which to store resulting data * \param debug Flag for enabling debugging of this function * \return value according to ::ISOMessageReturnValue */ -ISOMessageReturnValue buildISOHeader(const char *MessageBuffer, const size_t length, HeaderType * HeaderData, +ISOMessageReturnValue decodeISOHeader(const char *MessageBuffer, const size_t length, HeaderType * HeaderData, const char debug) { const char *p = MessageBuffer; ISOMessageReturnValue retval = MESSAGE_OK; @@ -91,14 +93,14 @@ ISOMessageReturnValue buildISOHeader(const char *MessageBuffer, const size_t len } /*! - * \brief buildISOFooter Convert data in a buffer to an ISO footer + * \brief decodeISOFooter Convert data in a buffer to an ISO footer * \param MessageBuffer Buffer containing raw data to be converted * \param length Length of buffer * \param HeaderData Struct in which to store resulting data * \param debug Flag for enabling debugging of this function * \return value according to ::ISOMessageReturnValue */ -ISOMessageReturnValue buildISOFooter(const char *MessageBuffer, const size_t length, FooterType * FooterData, +ISOMessageReturnValue decodeISOFooter(const char *MessageBuffer, const size_t length, FooterType * FooterData, const char debug) { if (length < sizeof (FooterData->Crc)) { @@ -112,6 +114,37 @@ ISOMessageReturnValue buildISOFooter(const char *MessageBuffer, const size_t len return MESSAGE_OK; } +HeaderType buildISOHeader(ISOMessageID id, uint32_t messageLength, const char debug) { + HeaderType header; + header.SyncWordU16 = ISO_SYNC_WORD; + header.TransmitterIdU8 = 0; + header.MessageCounterU8 = 0; + header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; + header.MessageIdU16 = (uint16_t) id; + header.MessageLengthU32 = messageLength; + + if (debug) { + LogPrint("ISO header:\n\tSync word: 0x%x\n\tTransmitter ID: %u\n\tMessage counter: %u\n\t" + "Ack request | Protocol version: 0x%x\n\tMessage ID: 0x%x\n\tMessage length: %u", + header.SyncWordU16, header.TransmitterIdU8, header.MessageCounterU8, header.AckReqProtVerU8, + header.MessageIdU16, header.MessageLengthU32); + } + + return header; +} + +FooterType buildISOFooter(const void * message, const size_t messageSize, const char debug) { + FooterType footer; + // TODO: Calculate CRC - remembering that message begins with header and messageSize will include header and footer + footer.Crc = 0; + + if (debug) { + LogPrint("ISO footer:\n\tCRC: 0x%x", footer.Crc); + } + + return footer; +} + /*! * \brief isValidMessageID Determines if specified id is a valid ISO message ID. The reserved range is deemed * invalid and vendor specific range is deemed valid. @@ -138,7 +171,7 @@ char isValidMessageID(const uint16_t id) { ISOMessageID getISOMessageType(const char *messageData, const size_t length, const char debug) { HeaderType header; - if (buildISOHeader(messageData, length, &header, debug) != MESSAGE_OK) { + if (decodeISOHeader(messageData, length, &header, debug) != MESSAGE_OK) { LogMessage(LOG_LEVEL_ERROR, "Unable to parse raw data into ISO message header"); return MESSAGE_ID_INVALID; } @@ -151,6 +184,61 @@ ISOMessageID getISOMessageType(const char *messageData, const size_t length, con } } + + +ssize_t encodeSTRTMessage(char * strtDataBuffer, const size_t bufferLength, TimeType * GPSTime, + uint32_t ScenarioStartTime, uint32_t DelayStart, uint32_t * OutgoingStartTime, const char debug) { + I32 MessageIndex = 0, i = 0; + U16 Crc = 0; + C8 *p; + STRTType STRTData; + + memset(strtDataBuffer, 0, bufferLength); + + if (bufferLength < sizeof (STRTType)) { + LogMessage(LOG_LEVEL_ERROR, "Buffer too small to hold necessary STRT data"); + return -1; + } + + STRTData.Header = buildISOHeader(MESSAGE_ID_STRT, sizeof (STRTType) - sizeof (HeaderType) - sizeof (FooterType), + debug); + + STRTData.StartTimeValueIdU16 = VALUE_ID_STRT_GPS_QMS_OF_WEEK; + STRTData.StartTimeContentLengthU16 = sizeof (STRTData.StartTimeU32); + + STRTData.GPSWeekValueIdU16 = VALUE_ID_STRT_GPS_WEEK; + STRTData.GPSWeekContentLengthU16 = sizeof (STRTData.GPSWeekU16); + + STRTData->StartTimeU32 = + ((GPSTime->GPSSecondsOfWeekU32 * 1000 + (U32) TimeControlGetMillisecond(GPSTime) + + ScenarioStartTime) << 2) + GPSTime->MicroSecondU16; + + STRTData->GPSWeekU16 = GPSTime->GPSWeekU16; + + *OutgoingStartTime = (STRTData->StartTimeU32) >> 2; + + if (!GPSTime->isGPSenabled) { + UtilgetCurrentGPStime(NULL, &STRTData->StartTimeU32); + } + + if (debug) { + LogPrint("STRT message:\n\tGPS second of week value ID: 0x%x\n\t" + "GPS second of week content length: %u\n\tGPS second of week: %u qms\n\t" + "GPS week value ID: 0x%x\n\tGPS week content length: %u\n\t" + "GPS week: %u", STRTData.StartTimeValueIdU16, STRTData.StartTimeContentLengthU16, + STRTData.StartTimeU32, STRTData.GPSWeekValueIdU16, STRTData.GPSWeekContentLengthU16, + STRTData.GPSWeekU16); + } + + STRTData.footer = buildISOFooter(&STRTData, sizeof (STRTType), debug); + + memcpy(strtDataBuffer, &STRTData, sizeof (STRTType)); + + return sizeof (STRTType); +} + + + /*! * \brief buildMONRMessage Fills a MONRType struct from a buffer of raw data * \param MonrData Raw data to be decoded @@ -169,7 +257,7 @@ ISOMessageReturnValue decodeMONRMessage(const char *MonrData, const size_t lengt ISOMessageReturnValue retval = MESSAGE_OK; // Decode ISO header - if ((retval = buildISOHeader(p, length, &MONRData->header, debug)) != MESSAGE_OK) { + if ((retval = decodeISOHeader(p, length, &MONRData->header, debug)) != MESSAGE_OK) { memset(MONRData, 0, sizeof (*MONRData)); return retval; } @@ -242,7 +330,7 @@ ISOMessageReturnValue decodeMONRMessage(const char *MonrData, const size_t lengt // Footer if ((retval = - buildISOFooter(p, length - (size_t) (p - MonrData), &MONRData->footer, debug)) != MESSAGE_OK) { + decodeISOFooter(p, length - (size_t) (p - MonrData), &MONRData->footer, debug)) != MESSAGE_OK) { memset(MONRData, 0, sizeof (*MONRData)); return retval; } diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index f884ac120..69c549b05 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -1325,66 +1325,6 @@ int ObjectControlOSEMtoASCII(OSEMType * OSEMData, char *GPSWeek, char *GPSLatitu } return 0; } -int ObjectControlBuildSTRTMessage(C8 * MessageBuffer, STRTType * STRTData, TimeType * GPSTime, - U32 ScenarioStartTime, U32 DelayStart, U32 * OutgoingStartTime, U8 debug) { - I32 MessageIndex = 0, i = 0; - U16 Crc = 0; - C8 *p; - - bzero(MessageBuffer, COMMAND_STRT_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); - - STRTData->Header.SyncWordU16 = ISO_SYNC_WORD; - STRTData->Header.TransmitterIdU8 = 0; - STRTData->Header.MessageCounterU8 = 0; - STRTData->Header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; - STRTData->Header.MessageIdU16 = COMMAND_STRT_CODE; - STRTData->Header.MessageLengthU32 = sizeof (STRTType) - sizeof (HeaderType); - STRTData->StartTimeValueIdU16 = VALUE_ID_GPS_SECOND_OF_WEEK; - STRTData->StartTimeContentLengthU16 = sizeof (STRTData->StartTimeU32); - STRTData->StartTimeU32 = - ((GPSTime->GPSSecondsOfWeekU32 * 1000 + (U32) TimeControlGetMillisecond(GPSTime) + - ScenarioStartTime) << 2) + GPSTime->MicroSecondU16; - STRTData->GPSWeekValueIdU16 = VALUE_ID_GPS_WEEK; - STRTData->GPSWeekContentLengthU16 = sizeof (STRTData->GPSWeekU16); - STRTData->GPSWeekU16 = GPSTime->GPSWeekU16; - // STRTData->DelayStartValueIdU16 = VALUE_ID_RELATIVE_TIME; - // STRTData->DelayStartContentLengthU16 = 4; - // STRTData->DelayStartU32 = DelayStart; - - *OutgoingStartTime = (STRTData->StartTimeU32) >> 2; - - if (!GPSTime->isGPSenabled) { - UtilgetCurrentGPStime(NULL, &STRTData->StartTimeU32); - } - - p = (char *)STRTData; - for (i = 0; i < sizeof (STRTType); i++) - *(MessageBuffer + i) = *p++; - Crc = crc_16((const unsigned char *)MessageBuffer, sizeof (STRTType)); - Crc = 0; - *(MessageBuffer + i++) = (U8) (Crc); - *(MessageBuffer + i++) = (U8) (Crc >> 8); - MessageIndex = i; - - if (debug) { - // TODO: Change to log printout when byte thingy has been implemented - printf("STRT total length = %d bytes (header+message+footer)\n", - (int)(COMMAND_STRT_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH)); - printf("----HEADER----\n"); - for (i = 0; i < sizeof (HeaderType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----MESSAGE----\n"); - for (; i < sizeof (STRTType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----FOOTER----\n"); - for (; i < MessageIndex; i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n"); - } - - return MessageIndex; //Total number of bytes -} - I32 ObjectControlBuildOSTMMessage(C8 * MessageBuffer, OSTMType * OSTMData, C8 CommandOption, U8 debug) { I32 MessageIndex = 0, i; From 4d04eafd8c105a717f90d30dbbd8593d039a1687 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 21 Jan 2020 12:23:47 +0100 Subject: [PATCH 098/523] STRT content --- server/inc/iso22133.h | 3 ++- server/src/iso22133.c | 38 +++++++++++++++++++------------------- 2 files changed, 21 insertions(+), 20 deletions(-) diff --git a/server/inc/iso22133.h b/server/inc/iso22133.h index c75503a20..c20f6a464 100644 --- a/server/inc/iso22133.h +++ b/server/inc/iso22133.h @@ -311,7 +311,8 @@ typedef enum { } ISOMessageID; ISOMessageReturnValue decodeMONRMessage(const char * MonrData, const size_t length, MONRType * MONRData, const char debug); -ssize_t encodeSTRTMessage(const STRTType * STRTData, char * strtData, const size_t length, const char debug); +ssize_t encodeSTRTMessage(const uint32_t startTimeGPSqmsOW, const uint16_t startGPSWeek, char * strtDataBuffer, + const size_t bufferLength, const char debug); ISOMessageReturnValue MONRToASCII(const MONRType * MONRData, char * asciiBuffer, const size_t bufferLength, const char debug); ISOMessageReturnValue ASCIIToMONR(const char * asciiBuffer, MONRType * MONRData, const char debug); ISOMessageID getISOMessageType(const char * messageData, const size_t length, const char debug); diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 7a3c7147f..f36bdb778 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -6,7 +6,7 @@ static const uint8_t SupportedProtocolVersions[] = { 2 }; -// ************************** static functions +// ************************** static function declarations static ISOMessageReturnValue decodeISOHeader(const char *MessageBuffer, const size_t length, HeaderType * HeaderData, const char debug); static ISOMessageReturnValue decodeISOFooter(const char *MessageBuffer, const size_t length, @@ -186,11 +186,9 @@ ISOMessageID getISOMessageType(const char *messageData, const size_t length, con -ssize_t encodeSTRTMessage(char * strtDataBuffer, const size_t bufferLength, TimeType * GPSTime, - uint32_t ScenarioStartTime, uint32_t DelayStart, uint32_t * OutgoingStartTime, const char debug) { - I32 MessageIndex = 0, i = 0; - U16 Crc = 0; - C8 *p; +ssize_t encodeSTRTMessage(const uint32_t startTimeGPSqmsOW, const uint16_t startGPSWeek, char * strtDataBuffer, + const size_t bufferLength, const char debug) { + STRTType STRTData; memset(strtDataBuffer, 0, bufferLength); @@ -205,21 +203,23 @@ ssize_t encodeSTRTMessage(char * strtDataBuffer, const size_t bufferLength, Time STRTData.StartTimeValueIdU16 = VALUE_ID_STRT_GPS_QMS_OF_WEEK; STRTData.StartTimeContentLengthU16 = sizeof (STRTData.StartTimeU32); - + STRTData.StartTimeU32 = startTimeGPSqmsOW; STRTData.GPSWeekValueIdU16 = VALUE_ID_STRT_GPS_WEEK; STRTData.GPSWeekContentLengthU16 = sizeof (STRTData.GPSWeekU16); - - STRTData->StartTimeU32 = - ((GPSTime->GPSSecondsOfWeekU32 * 1000 + (U32) TimeControlGetMillisecond(GPSTime) + - ScenarioStartTime) << 2) + GPSTime->MicroSecondU16; - - STRTData->GPSWeekU16 = GPSTime->GPSWeekU16; - - *OutgoingStartTime = (STRTData->StartTimeU32) >> 2; - - if (!GPSTime->isGPSenabled) { - UtilgetCurrentGPStime(NULL, &STRTData->StartTimeU32); - } + STRTData.GPSWeekU16 = startGPSWeek; + + // TODO: Delete + //STRTData->StartTimeU32 = + // ((GPSTime->GPSSecondsOfWeekU32 * 1000 + (U32) TimeControlGetMillisecond(GPSTime) + + // ScenarioStartTime) << 2) + GPSTime->MicroSecondU16; + // + //STRTData->GPSWeekU16 = GPSTime->GPSWeekU16; + // + //*OutgoingStartTime = (STRTData->StartTimeU32) >> 2; + // + //if (!GPSTime->isGPSenabled) { + // UtilgetCurrentGPStime(NULL, &STRTData->StartTimeU32); + //} if (debug) { LogPrint("STRT message:\n\tGPS second of week value ID: 0x%x\n\t" From 614081b6c0221d6eea73b8779557705aefa4201f Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 21 Jan 2020 12:30:52 +0100 Subject: [PATCH 099/523] Function descriptions and refactoring --- server/src/iso22133.c | 21 ++++++++++++++++++--- 1 file changed, 18 insertions(+), 3 deletions(-) diff --git a/server/src/iso22133.c b/server/src/iso22133.c index f36bdb778..5188e576b 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -114,6 +114,13 @@ ISOMessageReturnValue decodeISOFooter(const char *MessageBuffer, const size_t le return MESSAGE_OK; } +/*! + * \brief buildISOHeader Constructs an ISO header based on the supplied message ID and content length + * \param id Message ID of the message for which the header is to be used + * \param messageLength Length of the message excluding header and footer + * \param debug Flag for enabling debugging + * \return A struct containing ISO header data + */ HeaderType buildISOHeader(ISOMessageID id, uint32_t messageLength, const char debug) { HeaderType header; header.SyncWordU16 = ISO_SYNC_WORD; @@ -124,7 +131,7 @@ HeaderType buildISOHeader(ISOMessageID id, uint32_t messageLength, const char de header.MessageLengthU32 = messageLength; if (debug) { - LogPrint("ISO header:\n\tSync word: 0x%x\n\tTransmitter ID: %u\n\tMessage counter: %u\n\t" + LogPrint("Encoded ISO header:\n\tSync word: 0x%x\n\tTransmitter ID: %u\n\tMessage counter: %u\n\t" "Ack request | Protocol version: 0x%x\n\tMessage ID: 0x%x\n\tMessage length: %u", header.SyncWordU16, header.TransmitterIdU8, header.MessageCounterU8, header.AckReqProtVerU8, header.MessageIdU16, header.MessageLengthU32); @@ -139,7 +146,7 @@ FooterType buildISOFooter(const void * message, const size_t messageSize, const footer.Crc = 0; if (debug) { - LogPrint("ISO footer:\n\tCRC: 0x%x", footer.Crc); + LogPrint("Encoded ISO footer:\n\tCRC: 0x%x", footer.Crc); } return footer; @@ -185,7 +192,15 @@ ISOMessageID getISOMessageType(const char *messageData, const size_t length, con } - +/*! + * \brief encodeSTRTMessage Constructs an ISO STRT message based on start time parameters + * \param startTimeGPSqmsOW Quarter milliseconds of week when recipient of message shall start + * \param startGPSWeek GPS week when recipient shall start + * \param strtDataBuffer Data buffer in which to place encoded STRT message + * \param bufferLength Size of data buffer in which to place encoded STRT message + * \param debug Flag for enabling debugging + * \return number of bytes written to the data buffer, or -1 if an error occurred + */ ssize_t encodeSTRTMessage(const uint32_t startTimeGPSqmsOW, const uint16_t startGPSWeek, char * strtDataBuffer, const size_t bufferLength, const char debug) { From 009fa18a229676935306ed30ea1a4e9086da354e Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 21 Jan 2020 13:38:38 +0100 Subject: [PATCH 100/523] Started using the new STRT builder function in object control. Refactored some MQ communication. --- server/inc/util.h | 3 ++- server/src/iso22133.c | 13 ------------- server/src/objectcontrol.c | 24 +++++++++--------------- server/src/systemcontrol.c | 34 +++++++++++++--------------------- 4 files changed, 24 insertions(+), 50 deletions(-) diff --git a/server/inc/util.h b/server/inc/util.h index 499d8f9d2..27036391c 100644 --- a/server/inc/util.h +++ b/server/inc/util.h @@ -68,7 +68,8 @@ extern "C"{ #define TRAJ_MASTER_LATE -2 #define TIME_COMPENSATE_LAGING_VM 0 -#define TIME_COMPENSATE_LAGING_VM_VAL 106407 +#define VIRTUAL_MACHINE_LAG_COMPENSATION_S 106 +#define VIRTUAL_MACHINE_LAG_COMPENSATION_US 407000 #define MAX_ROW_SIZE 1024 diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 5188e576b..55280972d 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -223,19 +223,6 @@ ssize_t encodeSTRTMessage(const uint32_t startTimeGPSqmsOW, const uint16_t start STRTData.GPSWeekContentLengthU16 = sizeof (STRTData.GPSWeekU16); STRTData.GPSWeekU16 = startGPSWeek; - // TODO: Delete - //STRTData->StartTimeU32 = - // ((GPSTime->GPSSecondsOfWeekU32 * 1000 + (U32) TimeControlGetMillisecond(GPSTime) + - // ScenarioStartTime) << 2) + GPSTime->MicroSecondU16; - // - //STRTData->GPSWeekU16 = GPSTime->GPSWeekU16; - // - //*OutgoingStartTime = (STRTData->StartTimeU32) >> 2; - // - //if (!GPSTime->isGPSenabled) { - // UtilgetCurrentGPStime(NULL, &STRTData->StartTimeU32); - //} - if (debug) { LogPrint("STRT message:\n\tGPS second of week value ID: 0x%x\n\t" "GPS second of week content length: %u\n\tGPS second of week: %u qms\n\t" diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 69c549b05..72ea5f4c8 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -264,8 +264,6 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { C8 *MiscPtr; C8 MiscText[SMALL_BUFFER_SIZE_0]; U32 StartTimeU32 = 0; - U32 OutgoingStartTimeU32 = 0; - U32 DelayedStartU32 = 0; U32 CurrentTimeU32 = 0; U32 OldTimeU32 = 0; U64 TimeCap1, TimeCap2; @@ -636,24 +634,20 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { } else if (iCommand == COMM_STRT && (vGetState(GSD) == OBC_STATE_ARMED) /*|| OBC_STATE_INITIALIZED) */ ) //OBC_STATE_INITIALIZED is temporary! { - bzero(Timestamp, SMALL_BUFFER_SIZE_0); - MiscPtr = strchr(pcRecvBuffer, ';'); - strncpy(Timestamp, MiscPtr + 1, (uint64_t) strchr(MiscPtr + 1, ';') - (uint64_t) MiscPtr - 1); - StartTimeU32 = atol(Timestamp); - bzero(Timestamp, SMALL_BUFFER_SIZE_0); - MiscPtr += 1; - MiscPtr = strchr(pcRecvBuffer, ';'); - strncpy(Timestamp, MiscPtr + 1, (uint64_t) strchr(MiscPtr + 1, ';') - (uint64_t) MiscPtr - 1); - DelayedStartU32 = atoi(Timestamp); + struct timeval startTime, startDelay; + MiscPtr = pcRecvBuffer; + TimeSetToUTCms(&startTime, (int64_t) strtoul(MiscPtr, &MiscPtr, 10)); + TimeSetToUTCms(&startDelay, (int64_t) strtoul(MiscPtr+1, NULL, 10)); + timeradd(&startTime, &startDelay, &startTime); + MessageLength = (int) encodeSTRTMessage(TimeGetAsGPSqmsOfWeek(&startTime), TimeGetAsGPSweek(&startTime), MessageBuffer, sizeof (MessageBuffer), 0); + ASPData.MTSPU32 = 0; ASPData.TimeToSyncPointDbl = 0; SearchStartIndex = -1; ASPData.PrevTimeToSyncPointDbl = 0; OldTimeU32 = CurrentTimeU32; ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_OK; //Set server to READY - MessageLength = - ObjectControlBuildSTRTMessage(MessageBuffer, &STRTData, GPSTime, (U32) StartTimeU32, - DelayedStartU32, &OutgoingStartTimeU32, 0); + for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { UtilSendTCPData("Object Control", MessageBuffer, MessageLength, &socket_fds[iIndex], 0); } @@ -667,7 +661,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { } //OBCState = OBC_STATE_INITIALIZED; //This is temporary! //printf("OutgoingStartTimeU32 = %d\n", OutgoingStartTimeU32); - GSD->ScenarioStartTimeU32 = OutgoingStartTimeU32; + GSD->ScenarioStartTimeU32 = TimeGetAsGPSqmsOfWeek(&startTime) >> 2; bzero(MiscText, SMALL_BUFFER_SIZE_0); sprintf(MiscText, "%" PRIu32, GSD->ScenarioStartTimeU32 << 2); LOG_SEND(LogBuffer, "[ObjectControl] START received <%s>, GPS time <%s>", pcRecvBuffer, diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index 0a5f9259d..a8d958314 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -33,6 +33,7 @@ #include #include "systemcontrol.h" +#include "maestroTime.h" #include "timecontrol.h" #include "datadictionary.h" @@ -221,6 +222,8 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { C8 pcBuffer[IPC_BUFFER_SIZE]; char inchr; struct timeval tvTime; + const struct timeval VirtualMachineLagCompensation = {VIRTUAL_MACHINE_LAG_COMPENSATION_S, + VIRTUAL_MACHINE_LAG_COMPENSATION_US}; ObjectPosition OP; int i, i1; @@ -918,28 +921,17 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { if (server_state == SERVER_STATE_IDLE && objectControlState == OBC_STATE_ARMED) //Temporary! { bzero(pcBuffer, IPC_BUFFER_SIZE); - /* Lest use UTC time everywhere instead of etsi and gps time - gettimeofday(&tvTime, NULL); - uiTime = (uint64_t)tvTime.tv_sec*1000 + (uint64_t)tvTime.tv_usec/1000 - MS_FROM_1970_TO_2004_NO_LEAP_SECS + DIFF_LEAP_SECONDS_UTC_ETSI*1000; - */ - uiTime = UtilgetCurrentUTCtimeMS(); + TimeSetToCurrentSystemTime(&tvTime); + if (TIME_COMPENSATE_LAGING_VM) - uiTime = uiTime - TIME_COMPENSATE_LAGING_VM_VAL; - - LogMessage(LOG_LEVEL_INFO, "Current timestamp (gtd): %lu", uiTime); - - //clock_gettime(CLOCK_MONOTONIC_COARSE, &tTime); - //clock_gettime(CLOCK_REALTIME, &tTime); - //uiTime = (uint64_t)tTime.tv_sec*1000 + (uint64_t)tTime.tv_nsec/1000000 - MS_FROM_1970_TO_2004_NO_LEAP_SECS + DIFF_LEAP_SECONDS_UTC_ETSI*1000; - //printf("[SystemControl] Current timestamp (cgt): %lu\n",uiTime ); - //printf("[SystemControl] Current timestamp: %lu\n",uiTime ); - - //uiTime += atoi(SystemControlArgument[0]); - uiTime = atoi(SystemControlArgument[0]); - DelayedStartU32 = atoi(SystemControlArgument[1]); - sprintf(pcBuffer, "%" PRIu8 ";%" PRIu64 ";%" PRIu32 ";", 0, uiTime, DelayedStartU32); - LogMessage(LOG_LEVEL_INFO, "Sending START <%s> (delayed +%s ms)", pcBuffer, - SystemControlArgument[1]); + timersub(&tvTime, &VirtualMachineLagCompensation, &tvTime); + + LogMessage(LOG_LEVEL_INFO, "Current timestamp (epoch): %lu", TimeGetAsUTCms(&tvTime)); + + DelayedStartU32 = atoi(SystemControlArgument[0]); + sprintf(pcBuffer, "%" PRIi64 ";%" PRIu32 ";", TimeGetAsUTCms(&tvTime), DelayedStartU32); + LogMessage(LOG_LEVEL_INFO, "Sending START <%s> (delayed +%u ms)", pcBuffer, + DelayedStartU32); if (iCommSend(COMM_STRT, pcBuffer, strlen(pcBuffer) + 1) < 0) { LogMessage(LOG_LEVEL_ERROR, "Fatal communication fault when sending STRT command"); From fa7b21f28681e10fa8ba67c704471c6624143be3 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 21 Jan 2020 13:43:26 +0100 Subject: [PATCH 101/523] Moved protocol version definition to iso22133.h --- server/inc/iso22133.h | 3 +++ server/inc/util.h | 3 --- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/server/inc/iso22133.h b/server/inc/iso22133.h index c20f6a464..83ec23238 100644 --- a/server/inc/iso22133.h +++ b/server/inc/iso22133.h @@ -14,6 +14,9 @@ extern "C" { #include #include +#define ISO_PROTOCOL_VERSION 2 +#define ACK_REQ 0 + #define ISO_SYNC_WORD 0x7E7E #pragma pack(push,1) diff --git a/server/inc/util.h b/server/inc/util.h index 27036391c..8c1fa784a 100644 --- a/server/inc/util.h +++ b/server/inc/util.h @@ -39,9 +39,6 @@ extern "C"{ ------------------------------------------------------------*/ #define MaestroVersion "0.4.1" -#define ISO_PROTOCOL_VERSION 2 -#define ACK_REQ 0 - #define MBUS_MAX_DATALEN (MQ_MSG_SIZE-1) // Message queue data minus one byte for the command #define SAFETY_CHANNEL_PORT 53240 From ed810bf4e10e34ef044dd858a3a4c6d5ff3d31cf Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 21 Jan 2020 13:43:54 +0100 Subject: [PATCH 102/523] Ran code formatter --- server/src/iso22133.c | 22 ++++++++++++---------- server/src/objectcontrol.c | 7 +++++-- server/src/systemcontrol.c | 6 ++++-- 3 files changed, 21 insertions(+), 14 deletions(-) diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 55280972d..4ee3a3fba 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -8,11 +8,11 @@ static const uint8_t SupportedProtocolVersions[] = { 2 }; // ************************** static function declarations static ISOMessageReturnValue decodeISOHeader(const char *MessageBuffer, const size_t length, - HeaderType * HeaderData, const char debug); + HeaderType * HeaderData, const char debug); static ISOMessageReturnValue decodeISOFooter(const char *MessageBuffer, const size_t length, - FooterType * HeaderData, const char debug); + FooterType * HeaderData, const char debug); static HeaderType buildISOHeader(ISOMessageID id, uint32_t messageLength, const char debug); -static FooterType buildISOFooter(const void * message, const size_t sizeExclFooter, const char debug); +static FooterType buildISOFooter(const void *message, const size_t sizeExclFooter, const char debug); static char isValidMessageID(const uint16_t id); // ************************** function definitions @@ -26,7 +26,7 @@ static char isValidMessageID(const uint16_t id); * \return value according to ::ISOMessageReturnValue */ ISOMessageReturnValue decodeISOHeader(const char *MessageBuffer, const size_t length, HeaderType * HeaderData, - const char debug) { + const char debug) { const char *p = MessageBuffer; ISOMessageReturnValue retval = MESSAGE_OK; const char ProtocolVersionBitmask = 0x7F; @@ -101,7 +101,7 @@ ISOMessageReturnValue decodeISOHeader(const char *MessageBuffer, const size_t le * \return value according to ::ISOMessageReturnValue */ ISOMessageReturnValue decodeISOFooter(const char *MessageBuffer, const size_t length, FooterType * FooterData, - const char debug) { + const char debug) { if (length < sizeof (FooterData->Crc)) { LogMessage(LOG_LEVEL_ERROR, "Too little raw data to fill ISO footer"); @@ -123,6 +123,7 @@ ISOMessageReturnValue decodeISOFooter(const char *MessageBuffer, const size_t le */ HeaderType buildISOHeader(ISOMessageID id, uint32_t messageLength, const char debug) { HeaderType header; + header.SyncWordU16 = ISO_SYNC_WORD; header.TransmitterIdU8 = 0; header.MessageCounterU8 = 0; @@ -140,8 +141,9 @@ HeaderType buildISOHeader(ISOMessageID id, uint32_t messageLength, const char de return header; } -FooterType buildISOFooter(const void * message, const size_t messageSize, const char debug) { +FooterType buildISOFooter(const void *message, const size_t messageSize, const char debug) { FooterType footer; + // TODO: Calculate CRC - remembering that message begins with header and messageSize will include header and footer footer.Crc = 0; @@ -201,7 +203,7 @@ ISOMessageID getISOMessageType(const char *messageData, const size_t length, con * \param debug Flag for enabling debugging * \return number of bytes written to the data buffer, or -1 if an error occurred */ -ssize_t encodeSTRTMessage(const uint32_t startTimeGPSqmsOW, const uint16_t startGPSWeek, char * strtDataBuffer, +ssize_t encodeSTRTMessage(const uint32_t startTimeGPSqmsOW, const uint16_t startGPSWeek, char *strtDataBuffer, const size_t bufferLength, const char debug) { STRTType STRTData; @@ -213,8 +215,8 @@ ssize_t encodeSTRTMessage(const uint32_t startTimeGPSqmsOW, const uint16_t start return -1; } - STRTData.Header = buildISOHeader(MESSAGE_ID_STRT, sizeof (STRTType) - sizeof (HeaderType) - sizeof (FooterType), - debug); + STRTData.Header = + buildISOHeader(MESSAGE_ID_STRT, sizeof (STRTType) - sizeof (HeaderType) - sizeof (FooterType), debug); STRTData.StartTimeValueIdU16 = VALUE_ID_STRT_GPS_QMS_OF_WEEK; STRTData.StartTimeContentLengthU16 = sizeof (STRTData.StartTimeU32); @@ -249,7 +251,7 @@ ssize_t encodeSTRTMessage(const uint32_t startTimeGPSqmsOW, const uint16_t start * \return value according to ::ISOMessageReturnValue */ ISOMessageReturnValue decodeMONRMessage(const char *MonrData, const size_t length, MONRType * MONRData, - const char debug) { + const char debug) { const char *p = MonrData; const uint16_t ExpectedMONRStructSize = (uint16_t) (sizeof (*MONRData) - sizeof (MONRData->header) diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 72ea5f4c8..c74e72fdd 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -635,11 +635,14 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { else if (iCommand == COMM_STRT && (vGetState(GSD) == OBC_STATE_ARMED) /*|| OBC_STATE_INITIALIZED) */ ) //OBC_STATE_INITIALIZED is temporary! { struct timeval startTime, startDelay; + MiscPtr = pcRecvBuffer; TimeSetToUTCms(&startTime, (int64_t) strtoul(MiscPtr, &MiscPtr, 10)); - TimeSetToUTCms(&startDelay, (int64_t) strtoul(MiscPtr+1, NULL, 10)); + TimeSetToUTCms(&startDelay, (int64_t) strtoul(MiscPtr + 1, NULL, 10)); timeradd(&startTime, &startDelay, &startTime); - MessageLength = (int) encodeSTRTMessage(TimeGetAsGPSqmsOfWeek(&startTime), TimeGetAsGPSweek(&startTime), MessageBuffer, sizeof (MessageBuffer), 0); + MessageLength = + (int)encodeSTRTMessage(TimeGetAsGPSqmsOfWeek(&startTime), TimeGetAsGPSweek(&startTime), + MessageBuffer, sizeof (MessageBuffer), 0); ASPData.MTSPU32 = 0; ASPData.TimeToSyncPointDbl = 0; diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index a8d958314..cf8f816a7 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -222,8 +222,10 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { C8 pcBuffer[IPC_BUFFER_SIZE]; char inchr; struct timeval tvTime; - const struct timeval VirtualMachineLagCompensation = {VIRTUAL_MACHINE_LAG_COMPENSATION_S, - VIRTUAL_MACHINE_LAG_COMPENSATION_US}; + + const struct timeval VirtualMachineLagCompensation = { VIRTUAL_MACHINE_LAG_COMPENSATION_S, + VIRTUAL_MACHINE_LAG_COMPENSATION_US + }; ObjectPosition OP; int i, i1; From 09064e0174633d4d46dda77fa47dfbe614606ec9 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 21 Jan 2020 14:08:12 +0100 Subject: [PATCH 103/523] Ran code formatter --- server/src/iso22133.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 2a6b7ac57..24473a40e 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -120,7 +120,7 @@ ISOMessageReturnValue buildISOFooter(const char *MessageBuffer, const size_t len * \return value according to ::ISOMessageReturnValue */ ISOMessageReturnValue decodeMONRMessage(const char *MonrData, const size_t length, MONRType * MONRData, - const char debug) { + const char debug) { const char *p = MonrData; const uint16_t ExpectedMONRStructSize = (uint16_t) (sizeof (*MONRData) - sizeof (MONRData->header) From 37f1d8b9a329b4fd9361e0ba50ecc577c169628f Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Tue, 21 Jan 2020 14:12:06 +0100 Subject: [PATCH 104/523] Adding things --- server/src/datadictionary.c | 9 ++++----- server/src/objectcontrol.c | 3 +-- server/src/systemcontrol.c | 10 ++++++++-- 3 files changed, 13 insertions(+), 9 deletions(-) diff --git a/server/src/datadictionary.c b/server/src/datadictionary.c index 01c6db282..677ec41d4 100644 --- a/server/src/datadictionary.c +++ b/server/src/datadictionary.c @@ -1672,9 +1672,8 @@ ReadWriteAccess_t DataDictionaryInitMONR(GSDType * GSD, U8 objectCount){ ReadWriteAccess_t Res; Res = WRITE_OK; pthread_mutex_lock(&MONRMutex); - MONRType *ptr = malloc(sizeof (MONRType) * objectCount); + MONRType *ptr = malloc(sizeof(MONRType) * objectCount); GSD->MonrMessages = ptr; - LogPrint("Very good nice."); pthread_mutex_unlock(&MONRMutex); return Res; } @@ -1706,8 +1705,8 @@ ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, MONRType * MONR, U8 id) { ReadWriteAccess_t Res; Res = WRITE_OK; pthread_mutex_lock(&MONRMutex); - MONRType *test = GSD->MonrMessages; - test[id] = *MONR; + GSD->MonrMessages[id].HeadingU16 = MONR->HeadingU16; + LogPrint("Setting %d for %d in ptr %p", GSD->MonrMessages[id].HeadingU16, GSD->MonrMessages->Header.TransmitterIdU8, GSD->MonrMessages); pthread_mutex_unlock(&MONRMutex); return Res; } @@ -1721,7 +1720,7 @@ ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, MONRType * MONR, U8 id) { */ ReadWriteAccess_t DataDictionaryGetMONR(GSDType * GSD, MONRType *MONR, U8 id) { pthread_mutex_lock(&MONRMutex); - *MONR = GSD->MonrMessages[id]; + LogPrint("Getting %d for %d in ptr %p", GSD->MonrMessages[id].HeadingU16, GSD->MonrMessages->Header.TransmitterIdU8, GSD->MonrMessages); pthread_mutex_unlock(&MONRMutex); return READ_OK; } diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 1873d5622..00d7a3065 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -490,7 +490,6 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { DataDictionarySetMONR(GSD, &MONRData, iIndex); - //UtilSendUDPData("ObjectControl", &ObjectControlUDPSocketfdI32, &simulator_addr, &MONRData, sizeof(MONRData), 0); /* @@ -887,7 +886,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { } - } while (iResult < 0 && DisconnectU8 == 0); + } while (iExit == 0 && iResult < 0 && DisconnectU8 == 0); if (iResult >= 0) { /* Send OSEM command in mq so that we get some information like GPSweek, origin (latitude,logitude,altitude in gps coordinates) */ diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index 573b07ef7..0b901627e 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -565,12 +565,18 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { break; case COMM_MONR: // TODO: Decode - if (RVSSChannelSocket != 0 && RVSSConfigU32 & RVSS_MONR_CHANNEL && bytesReceived >= 0) { + /*if (RVSSChannelSocket != 0 && RVSSConfigU32 & RVSS_MONR_CHANNEL && bytesReceived >= 0) { UtilPopulateMonitorDataStruct(pcRecvBuffer, (size_t) bytesReceived, &monrData, 0); SystemControlBuildRVSSMONRChannelMessage(RVSSData, &RVSSMessageLengthU32, monrData, 0); UtilSendUDPData("SystemControl", &RVSSChannelSocket, &RVSSChannelAddr, RVSSData, RVSSMessageLengthU32, 0); - } + }*/ + DataDictionaryGetMONR(GSD, &monrData.MONR, 0); + DataDictionaryGetMONR(GSD, &monrData.MONR, 1); + + SystemControlBuildRVSSMONRChannelMessage(RVSSData, &RVSSMessageLengthU32, monrData, 0); + UtilSendUDPData("SystemControl", &RVSSChannelSocket, &RVSSChannelAddr, RVSSData, + RVSSMessageLengthU32, 0); break; case COMM_INV: break; From 341ab7f4940037dbb75114ff9ee4c0daaea11dad Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Wed, 22 Jan 2020 10:35:00 +0100 Subject: [PATCH 105/523] Cannot read in SC, works in OC. --- server/src/datadictionary.c | 8 ++++---- server/src/objectcontrol.c | 9 ++++++--- 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/server/src/datadictionary.c b/server/src/datadictionary.c index 677ec41d4..6d2e5b421 100644 --- a/server/src/datadictionary.c +++ b/server/src/datadictionary.c @@ -1672,8 +1672,8 @@ ReadWriteAccess_t DataDictionaryInitMONR(GSDType * GSD, U8 objectCount){ ReadWriteAccess_t Res; Res = WRITE_OK; pthread_mutex_lock(&MONRMutex); - MONRType *ptr = malloc(sizeof(MONRType) * objectCount); - GSD->MonrMessages = ptr; + GSD->MonrMessages = malloc(sizeof(MONRType) * objectCount); + //GSD->MonrMessages = ptr; pthread_mutex_unlock(&MONRMutex); return Res; } @@ -1706,7 +1706,7 @@ ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, MONRType * MONR, U8 id) { Res = WRITE_OK; pthread_mutex_lock(&MONRMutex); GSD->MonrMessages[id].HeadingU16 = MONR->HeadingU16; - LogPrint("Setting %d for %d in ptr %p", GSD->MonrMessages[id].HeadingU16, GSD->MonrMessages->Header.TransmitterIdU8, GSD->MonrMessages); + LogPrint("Setting %d for %d in ptr %p", GSD->MonrMessages[id].HeadingU16, id, GSD->MonrMessages); pthread_mutex_unlock(&MONRMutex); return Res; } @@ -1720,7 +1720,7 @@ ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, MONRType * MONR, U8 id) { */ ReadWriteAccess_t DataDictionaryGetMONR(GSDType * GSD, MONRType *MONR, U8 id) { pthread_mutex_lock(&MONRMutex); - LogPrint("Getting %d for %d in ptr %p", GSD->MonrMessages[id].HeadingU16, GSD->MonrMessages->Header.TransmitterIdU8, GSD->MonrMessages); + LogPrint("Getting %d for %d in ptr %p", GSD->MonrMessages[id].HeadingU16, id, GSD->MonrMessages); pthread_mutex_unlock(&MONRMutex); return READ_OK; } diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 00d7a3065..772a2bf16 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -486,9 +486,14 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { ObjectControlBuildMONRMessage(buffer, &MONRData, 0); + LogPrint("****** BEFORE SETTING ******"); + DataDictionaryGetMONR(GSD, &MONRData, iIndex); + LogPrint("****************"); + //Store MONR in GSD DataDictionarySetMONR(GSD, &MONRData, iIndex); + DataDictionaryGetMONR(GSD, &MONRData, iIndex); //UtilSendUDPData("ObjectControl", &ObjectControlUDPSocketfdI32, &simulator_addr, &MONRData, sizeof(MONRData), 0); @@ -793,10 +798,8 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { iCommSend(COMM_FAILURE, pcSendBuffer, sizeof (iCommand)); } - - LogMessage(LOG_LEVEL_INFO, "Try to do the thing with %d objects", nbr_objects); + //Allocate memory in data dict DataDictionaryInitMONR(GSD, nbr_objects); - LogMessage(LOG_LEVEL_INFO, "Did the thing with %d objects", nbr_objects); } else if (iCommand == COMM_ACCM && vGetState(GSD) == OBC_STATE_CONNECTED) { From 390cf3a0f0bc1df2fd4a9801137e56e250dfdc65 Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Wed, 22 Jan 2020 14:48:00 +0100 Subject: [PATCH 106/523] Removed sleep(3) --- server/src/objectcontrol.c | 1 - 1 file changed, 1 deletion(-) diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 0a1cc6f2c..3941bbafa 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -838,7 +838,6 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { "[ObjectControl] Was not able to connect to object, [IP: %s] [PORT: %d], retry in %d sec...", object_address_name[iIndex], object_tcp_port[iIndex], (!(1 & DisconnectU8)) * 3); - (void)sleep(3); // TODO: Move this to the rest of the sleep operations? Also, remove the hardcoded 3 break; case EADDRINUSE: util_error("[ObjectControl] Local address/port already in use"); From 9a1a123603db5eddcb2284d209531d255f122f27 Mon Sep 17 00:00:00 2001 From: LukasWikander Date: Wed, 22 Jan 2020 17:26:27 +0100 Subject: [PATCH 107/523] Update objectcontrol.c --- server/src/objectcontrol.c | 1 + 1 file changed, 1 insertion(+) diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 786918971..a1ab90dde 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -1260,6 +1260,7 @@ I32 ObjectControlBuildVOILMessage(C8 * MessageBuffer, VOILType * VOILData, C8 * /*! * \brief ObjectControlBuildMONRMessage Fills a MONRType struct from a buffer of raw data * \param MonrData Raw data to be decoded + * \param length Length of the raw data buffer * \param MONRData Struct to be filled * \param debug Flag for enabling of debugging * \return 0 on success, -1 otherwise From 37cfb37ab4c568d12d5b582553de0bf04150a8cb Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 22 Jan 2020 19:29:33 +0100 Subject: [PATCH 108/523] Added use of little endian-to-host endian functions on ISO header and MONR fields of 2 bytes and larger --- server/src/iso22133.c | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 24473a40e..b2beb8d38 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -38,6 +38,7 @@ ISOMessageReturnValue buildISOHeader(const char *MessageBuffer, const size_t len // Decode ISO header memcpy(&HeaderData->SyncWordU16, p, sizeof (HeaderData->SyncWordU16)); + HeaderData->SyncWordU16 = le16toh(HeaderData->SyncWordU16); p += sizeof (HeaderData->SyncWordU16); if (HeaderData->SyncWordU16 != ISO_SYNC_WORD) { @@ -73,9 +74,11 @@ ISOMessageReturnValue buildISOHeader(const char *MessageBuffer, const size_t len memcpy(&HeaderData->MessageIdU16, p, sizeof (HeaderData->MessageIdU16)); p += sizeof (HeaderData->MessageIdU16); + HeaderData->MessageIdU16 = le16toh(HeaderData->MessageIdU16); memcpy(&HeaderData->MessageLengthU32, p, sizeof (HeaderData->MessageLengthU32)); p += sizeof (HeaderData->MessageLengthU32); + HeaderData->MessageLengthU32 = le32toh(HeaderData->MessageLengthU32); if (debug) { LogPrint("SyncWordU16 = 0x%x", HeaderData->SyncWordU16); @@ -106,6 +109,7 @@ ISOMessageReturnValue buildISOFooter(const char *MessageBuffer, const size_t len return MESSAGE_LENGTH_ERROR; } memcpy(&FooterData->Crc, MessageBuffer, sizeof (FooterData->Crc)); + FooterData->Crc = le16toh(FooterData->Crc); // TODO: check on CRC return MESSAGE_OK; @@ -144,6 +148,7 @@ ISOMessageReturnValue decodeMONRMessage(const char *MonrData, const size_t lengt // Decode content header memcpy(&MONRData->monrStructValueID, p, sizeof (MONRData->monrStructValueID)); p += sizeof (MONRData->monrStructValueID); + MONRData->monrStructValueID = le16toh(MONRData->monrStructValueID); if (MONRData->monrStructValueID != VALUE_ID_MONR_STRUCT) { LogMessage(LOG_LEVEL_ERROR, "Attempted to pass non-MONR struct into MONR parsing function"); @@ -153,6 +158,7 @@ ISOMessageReturnValue decodeMONRMessage(const char *MonrData, const size_t lengt memcpy(&MONRData->monrStructContentLength, p, sizeof (MONRData->monrStructContentLength)); p += sizeof (MONRData->monrStructContentLength); + MONRData->monrStructContentLength = le16toh(MONRData->monrStructContentLength); if (MONRData->monrStructContentLength != ExpectedMONRStructSize) { LogMessage(LOG_LEVEL_ERROR, "MONR content length %u differs from the expected length %u", @@ -164,30 +170,39 @@ ISOMessageReturnValue decodeMONRMessage(const char *MonrData, const size_t lengt // Decode content memcpy(&MONRData->gpsQmsOfWeek, p, sizeof (MONRData->gpsQmsOfWeek)); p += sizeof (MONRData->gpsQmsOfWeek); + MONRData->gpsQmsOfWeek = le32toh(MONRData->gpsQmsOfWeek); memcpy(&MONRData->xPosition, p, sizeof (MONRData->xPosition)); p += sizeof (MONRData->xPosition); + MONRData->xPosition = (int32_t) le32toh(MONRData->xPosition); memcpy(&MONRData->yPosition, p, sizeof (MONRData->yPosition)); p += sizeof (MONRData->yPosition); + MONRData->yPosition = (int32_t) le32toh(MONRData->yPosition); memcpy(&MONRData->zPosition, p, sizeof (MONRData->zPosition)); p += sizeof (MONRData->zPosition); + MONRData->zPosition = (int32_t) le32toh(MONRData->zPosition); memcpy(&MONRData->heading, p, sizeof (MONRData->heading)); p += sizeof (MONRData->heading); + MONRData->heading = le16toh(MONRData->heading); memcpy(&MONRData->longitudinalSpeed, p, sizeof (MONRData->longitudinalSpeed)); p += sizeof (MONRData->longitudinalSpeed); + MONRData->longitudinalSpeed = (int16_t) le16toh(MONRData->longitudinalSpeed); memcpy(&MONRData->lateralSpeed, p, sizeof (MONRData->lateralSpeed)); p += sizeof (MONRData->lateralSpeed); + MONRData->lateralSpeed = (int16_t) le16toh(MONRData->lateralSpeed); memcpy(&MONRData->longitudinalAcc, p, sizeof (MONRData->longitudinalAcc)); p += sizeof (MONRData->longitudinalAcc); + MONRData->longitudinalAcc = (int16_t) le16toh(MONRData->longitudinalAcc); memcpy(&MONRData->lateralAcc, p, sizeof (MONRData->lateralAcc)); p += sizeof (MONRData->lateralAcc); + MONRData->lateralAcc = (int16_t) le16toh(MONRData->lateralAcc); memcpy(&MONRData->driveDirection, p, sizeof (MONRData->driveDirection)); p += sizeof (MONRData->driveDirection); @@ -207,7 +222,6 @@ ISOMessageReturnValue decodeMONRMessage(const char *MonrData, const size_t lengt memset(MONRData, 0, sizeof (*MONRData)); return retval; } - p += sizeof (MONRData->footer); if (debug == 1) { LogPrint("MONR:"); From f9b0d0919a70556703d756b7626997d7d8dfc372 Mon Sep 17 00:00:00 2001 From: alfaro01 Date: Thu, 23 Jan 2020 12:42:24 +0100 Subject: [PATCH 109/523] attempt att fixinf wrongs string size bug. --- modules/Visualization/src/main.c | 32 +++++++++++++++++++++++++------- 1 file changed, 25 insertions(+), 7 deletions(-) diff --git a/modules/Visualization/src/main.c b/modules/Visualization/src/main.c index 799a71ac5..1c909a788 100644 --- a/modules/Visualization/src/main.c +++ b/modules/Visualization/src/main.c @@ -40,18 +40,36 @@ void vCreateVisualizationMessage(MonitorDataType * _monitorData, char *_visualiz //IP char ipStringBuffer[INET_ADDRSTRLEN]; + char GPSMsOfWeekString[4]; + printf(GPSMsOfWeekString, "%u", _monitorData->MONR.GPSQmsOfWeekU32); + char xPosString[4]; + printf(xPosString, "%d", _monitorData->MONR.XPositionI32); + char yPosString[4]; + printf(yPosString, "%d", _monitorData->MONR.YPositionI32); + char zPosString[4]; + printf(zPosString, "%d", _monitorData->MONR.ZPositionI32); + char headingString[2]; + printf(headingString, "%u", _monitorData->MONR.HeadingU16); + char longSpeedString[2]; + printf(longSpeedString, "%d", _monitorData->MONR.LongitudinalSpeedI16); + char stateString[1]; + printf(stateString, "%u", _monitorData->MONR.StateU8); + + + sprintf(ipStringBuffer, "%s", inet_ntop(AF_INET, &_monitorData->ClientIP, ipStringBuffer, sizeof (ipStringBuffer))); //Build message from MonitorStruct - snprintf(_visualizationMessage, _sizeOfVisualizationMessage, "%s;%u;%d;%d;%d;%u;%d;%u;", + snprintf(_visualizationMessage, _sizeOfVisualizationMessage, "%c;%c;%c;%c;%c;%c;%c;%c;", ipStringBuffer, - _monitorData->MONR.GPSQmsOfWeekU32, - _monitorData->MONR.XPositionI32, - _monitorData->MONR.YPositionI32, - _monitorData->MONR.ZPositionI32, - _monitorData->MONR.HeadingU16, - _monitorData->MONR.LongitudinalSpeedI16, _monitorData->MONR.StateU8); + GPSMsOfWeekString, + xPosString, + yPosString, + zPosString, + headingString, + longSpeedString, + stateString); if (_debug) { From 847bcc0840ada13949eefc3fc35c4d6fe6a4e79c Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 23 Jan 2020 12:47:21 +0100 Subject: [PATCH 110/523] Removed definitions of object control STRT builder functions --- server/inc/objectcontrol.h | 1 - server/src/objectcontrol.c | 2 -- 2 files changed, 3 deletions(-) diff --git a/server/inc/objectcontrol.h b/server/inc/objectcontrol.h index 8ba56708b..b5a661c31 100644 --- a/server/inc/objectcontrol.h +++ b/server/inc/objectcontrol.h @@ -22,7 +22,6 @@ void objectcontrol_task(TimeType *GPSTime, GSDType *GSD, LOG_LEVEL logLevel); I32 ObjectControlBuildOSEMMessage(C8* MessageBuffer, OSEMType *OSEMData, TimeType *GPSTime, C8 *Latitude, C8 *Longitude, C8 *Altitude, U8 debug); -I32 ObjectControlBuildSTRTMessage(C8* MessageBuffer, STRTType *STRTData, TimeType *GPSTime, U32 ScenarioStartTime, U32 DelayStart, U32 *OutgoingStartTime, U8 debug); I32 ObjectControlBuildOSTMMessage(C8* MessageBuffer, OSTMType *OSTMData, C8 CommandOption, U8 debug); I32 ObjectControlBuildHEABMessage(C8* MessageBuffer, HEABType *HEABData, TimeType *GPSTime, U8 CCStatus, U8 debug); int ObjectControlBuildLLCMMessage(char* MessageBuffer, unsigned short Speed, unsigned short Curvature, unsigned char Mode, char debug); diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index c74e72fdd..4693d49be 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -175,8 +175,6 @@ I32 ObjectControlBuildOSEMMessage(C8 * MessageBuffer, OSEMType * OSEMData, TimeT static size_t uiRecvMonitor(int *sockfd, char *buffer, size_t length); static int iGetObjectIndexFromObjectIP(in_addr_t ipAddr, in_addr_t objectIPs[], unsigned int numberOfObjects); static void signalHandler(int signo); -I32 ObjectControlBuildSTRTMessage(C8 * MessageBuffer, STRTType * STRTData, TimeType * GPSTime, - U32 ScenarioStartTime, U32 DelayStart, U32 * OutgoingStartTime, U8 debug); I32 ObjectControlBuildOSTMMessage(C8 * MessageBuffer, OSTMType * OSTMData, C8 CommandOption, U8 debug); I32 ObjectControlBuildHEABMessage(C8 * MessageBuffer, HEABType * HEABData, TimeType * GPSTime, U8 CCStatus, U8 debug); From b63addab1b710357ae641bcbcc066b14f50d7427 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 23 Jan 2020 12:49:08 +0100 Subject: [PATCH 111/523] Refactoring --- server/inc/iso22133.h | 2 +- server/src/iso22133.c | 2 +- server/src/objectcontrol.c | 1 - 3 files changed, 2 insertions(+), 3 deletions(-) diff --git a/server/inc/iso22133.h b/server/inc/iso22133.h index 83ec23238..0a00665ae 100644 --- a/server/inc/iso22133.h +++ b/server/inc/iso22133.h @@ -38,7 +38,7 @@ typedef struct typedef struct { - HeaderType Header; + HeaderType header; uint16_t StartTimeValueIdU16; uint16_t StartTimeContentLengthU16; uint32_t StartTimeU32; diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 4ee3a3fba..a83a7c819 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -215,7 +215,7 @@ ssize_t encodeSTRTMessage(const uint32_t startTimeGPSqmsOW, const uint16_t start return -1; } - STRTData.Header = + STRTData.header = buildISOHeader(MESSAGE_ID_STRT, sizeof (STRTType) - sizeof (HeaderType) - sizeof (FooterType), debug); STRTData.StartTimeValueIdU16 = VALUE_ID_STRT_GPS_QMS_OF_WEEK; diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 4693d49be..59b41e295 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -283,7 +283,6 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { C8 ObjectPort[SMALL_BUFFER_SIZE_0]; HeaderType HeaderData; OSEMType OSEMData; - STRTType STRTData; OSTMType OSTMData; HEABType HEABData; MONRType MONRData; From 97ea6227be7ce5af4be589bc6f69d41d7b53dc60 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 23 Jan 2020 12:57:23 +0100 Subject: [PATCH 112/523] Added som in-code comments --- server/src/iso22133.c | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/server/src/iso22133.c b/server/src/iso22133.c index a83a7c819..85fadcf4c 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -33,6 +33,7 @@ ISOMessageReturnValue decodeISOHeader(const char *MessageBuffer, const size_t le char messageProtocolVersion = 0; char isProtocolVersionSupported = 0; + // If not enough data to fill header, generate error if (length < sizeof (HeaderData)) { LogMessage(LOG_LEVEL_ERROR, "Too little raw data to fill ISO header"); memset(HeaderData, 0, sizeof (*HeaderData)); @@ -43,6 +44,7 @@ ISOMessageReturnValue decodeISOHeader(const char *MessageBuffer, const size_t le memcpy(&HeaderData->SyncWordU16, p, sizeof (HeaderData->SyncWordU16)); p += sizeof (HeaderData->SyncWordU16); + // If sync word is not correct, generate error if (HeaderData->SyncWordU16 != ISO_SYNC_WORD) { LogMessage(LOG_LEVEL_ERROR, "Sync word error when decoding ISO header"); memset(HeaderData, 0, sizeof (*HeaderData)); @@ -58,7 +60,7 @@ ISOMessageReturnValue decodeISOHeader(const char *MessageBuffer, const size_t le memcpy(&HeaderData->AckReqProtVerU8, p, sizeof (HeaderData->AckReqProtVerU8)); p += sizeof (HeaderData->AckReqProtVerU8); - // Loop over permitted protocol versions + // Loop over permitted protocol versions to see if current version is among them messageProtocolVersion = HeaderData->AckReqProtVerU8 & ProtocolVersionBitmask; for (size_t i = 0; i < sizeof (SupportedProtocolVersions) / sizeof (SupportedProtocolVersions[0]); ++i) { if (SupportedProtocolVersions[i] == messageProtocolVersion) { @@ -67,6 +69,7 @@ ISOMessageReturnValue decodeISOHeader(const char *MessageBuffer, const size_t le } } + // Generate error if protocol version not supported if (!isProtocolVersionSupported) { LogMessage(LOG_LEVEL_WARNING, "Protocol version %u not supported", messageProtocolVersion); retval = MESSAGE_VERSION_ERROR; @@ -103,6 +106,7 @@ ISOMessageReturnValue decodeISOHeader(const char *MessageBuffer, const size_t le ISOMessageReturnValue decodeISOFooter(const char *MessageBuffer, const size_t length, FooterType * FooterData, const char debug) { + // If too little data, generate error if (length < sizeof (FooterData->Crc)) { LogMessage(LOG_LEVEL_ERROR, "Too little raw data to fill ISO footer"); memset(FooterData, 0, sizeof (*FooterData)); @@ -180,10 +184,13 @@ char isValidMessageID(const uint16_t id) { ISOMessageID getISOMessageType(const char *messageData, const size_t length, const char debug) { HeaderType header; + // Create header if (decodeISOHeader(messageData, length, &header, debug) != MESSAGE_OK) { LogMessage(LOG_LEVEL_ERROR, "Unable to parse raw data into ISO message header"); return MESSAGE_ID_INVALID; } + + // Check if header contains valid message ID, if so return it if (isValidMessageID(header.MessageIdU16)) return (ISOMessageID) header.MessageIdU16; else { @@ -210,6 +217,7 @@ ssize_t encodeSTRTMessage(const uint32_t startTimeGPSqmsOW, const uint16_t start memset(strtDataBuffer, 0, bufferLength); + // If buffer too small to hold STRT data, generate an error if (bufferLength < sizeof (STRTType)) { LogMessage(LOG_LEVEL_ERROR, "Buffer too small to hold necessary STRT data"); return -1; @@ -267,6 +275,7 @@ ISOMessageReturnValue decodeMONRMessage(const char *MonrData, const size_t lengt } p += sizeof (MONRData->header); + // If message is not a MONR message, generate an error if (MONRData->header.MessageIdU16 != MESSAGE_ID_MONR) { memset(MONRData, 0, sizeof (*MONRData)); return MESSAGE_TYPE_ERROR; @@ -276,6 +285,7 @@ ISOMessageReturnValue decodeMONRMessage(const char *MonrData, const size_t lengt memcpy(&MONRData->monrStructValueID, p, sizeof (MONRData->monrStructValueID)); p += sizeof (MONRData->monrStructValueID); + // If content is not a MONR struct or an unexpected size, generate an error if (MONRData->monrStructValueID != VALUE_ID_MONR_STRUCT) { LogMessage(LOG_LEVEL_ERROR, "Attempted to pass non-MONR struct into MONR parsing function"); memset(MONRData, 0, sizeof (*MONRData)); @@ -332,7 +342,7 @@ ISOMessageReturnValue decodeMONRMessage(const char *MonrData, const size_t lengt memcpy(&MONRData->errorStatus, p, sizeof (MONRData->errorStatus)); p += sizeof (MONRData->errorStatus); - // Footer + // Decode footer if ((retval = decodeISOFooter(p, length - (size_t) (p - MonrData), &MONRData->footer, debug)) != MESSAGE_OK) { memset(MONRData, 0, sizeof (*MONRData)); From b43c62b5458bc200ead3a8b52aa9c820b26a7ac9 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 23 Jan 2020 13:05:54 +0100 Subject: [PATCH 113/523] Added endianness fixing to STRT builder function --- server/src/iso22133.c | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 5efff5963..3a0fc9cc5 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -154,6 +154,7 @@ FooterType buildISOFooter(const void *message, const size_t messageSize, const c // TODO: Calculate CRC - remembering that message begins with header and messageSize will include header and footer footer.Crc = 0; + footer.Crc = htole16(footer.Crc); if (debug) { LogPrint("Encoded ISO footer:\n\tCRC: 0x%x", footer.Crc); @@ -230,12 +231,19 @@ ssize_t encodeSTRTMessage(const uint32_t startTimeGPSqmsOW, const uint16_t start STRTData.header = buildISOHeader(MESSAGE_ID_STRT, sizeof (STRTType) - sizeof (HeaderType) - sizeof (FooterType), debug); + // Fill contents, ensuring to swap from host endianness to little endian STRTData.StartTimeValueIdU16 = VALUE_ID_STRT_GPS_QMS_OF_WEEK; + STRTData.StartTimeValueIdU16 = htole16(STRTData.StartTimeValueIdU16); STRTData.StartTimeContentLengthU16 = sizeof (STRTData.StartTimeU32); + STRTData.StartTimeContentLengthU16 = htole16(STRTData.StartTimeContentLengthU16); STRTData.StartTimeU32 = startTimeGPSqmsOW; + STRTData.StartTimeU32 = htole32(STRTData.StartTimeU32); STRTData.GPSWeekValueIdU16 = VALUE_ID_STRT_GPS_WEEK; + STRTData.GPSWeekValueIdU16 = htole16(STRTData.GPSWeekValueIdU16); STRTData.GPSWeekContentLengthU16 = sizeof (STRTData.GPSWeekU16); + STRTData.GPSWeekContentLengthU16 = htole16(STRTData.GPSWeekContentLengthU16); STRTData.GPSWeekU16 = startGPSWeek; + STRTData.GPSWeekU16 = htole16(STRTData.GPSWeekU16); if (debug) { LogPrint("STRT message:\n\tGPS second of week value ID: 0x%x\n\t" From 312eea05519f5efad510cebe3814d015e72b0c81 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 23 Jan 2020 13:10:11 +0100 Subject: [PATCH 114/523] Moved typedef to iso22133.h --- server/inc/iso22133.h | 36 ++++++++++++++++++++++++++++++++++++ server/inc/util.h | 33 --------------------------------- 2 files changed, 36 insertions(+), 33 deletions(-) diff --git a/server/inc/iso22133.h b/server/inc/iso22133.h index 0a00665ae..5dc1af6f7 100644 --- a/server/inc/iso22133.h +++ b/server/inc/iso22133.h @@ -36,6 +36,42 @@ typedef struct } FooterType; //2 bytes +//! *************************** OSEM +typedef struct +{ + HeaderType Header; + uint16_t LatitudeValueIdU16; + uint16_t LatitudeContentLengthU16; + int64_t LatitudeI64; + uint16_t LongitudeValueIdU16; + uint16_t LongitudeContentLengthU16; + int64_t LongitudeI64; + uint16_t AltitudeValueIdU16; + uint16_t AltitudeContentLengthU16; + int32_t AltitudeI32; + uint16_t DateValueIdU16; + uint16_t DateContentLengthU16; + uint32_t DateU32; + uint16_t GPSWeekValueIdU16; + uint16_t GPSWeekContentLengthU16; + uint16_t GPSWeekU16; + uint16_t GPSSOWValueIdU16; + uint16_t GPSSOWContentLengthU16; + uint32_t GPSQmsOfWeekU32; + uint16_t MaxWayDeviationValueIdU16; + uint16_t MaxWayDeviationContentLengthU16; + uint16_t MaxWayDeviationU16; + uint16_t MaxLateralDeviationValueIdU16; + uint16_t MaxLateralDeviationContentLengthU16; + uint16_t MaxLateralDeviationU16; + uint16_t MinPosAccuracyValueIdU16; + uint16_t MinPosAccuracyContentLengthU16; + uint16_t MinPosAccuracyU16; +} OSEMType; //85 bytes + +//! OSEM value IDs + +//! *************************** STRT typedef struct { HeaderType header; diff --git a/server/inc/util.h b/server/inc/util.h index 8c1fa784a..3e69dfd80 100644 --- a/server/inc/util.h +++ b/server/inc/util.h @@ -278,39 +278,6 @@ typedef struct } CartesianPosition; - -typedef struct -{ - HeaderType Header; - U16 LatitudeValueIdU16; - U16 LatitudeContentLengthU16; - I64 LatitudeI64; - U16 LongitudeValueIdU16; - U16 LongitudeContentLengthU16; - I64 LongitudeI64; - U16 AltitudeValueIdU16; - U16 AltitudeContentLengthU16; - I32 AltitudeI32; - U16 DateValueIdU16; - U16 DateContentLengthU16; - U32 DateU32; - U16 GPSWeekValueIdU16; - U16 GPSWeekContentLengthU16; - U16 GPSWeekU16; - U16 GPSSOWValueIdU16; - U16 GPSSOWContentLengthU16; - U32 GPSQmsOfWeekU32; - U16 MaxWayDeviationValueIdU16; - U16 MaxWayDeviationContentLengthU16; - U16 MaxWayDeviationU16; - U16 MaxLateralDeviationValueIdU16; - U16 MaxLateralDeviationContentLengthU16; - U16 MaxLateralDeviationU16; - U16 MinPosAccuracyValueIdU16; - U16 MinPosAccuracyContentLengthU16; - U16 MinPosAccuracyU16; -} OSEMType; //85 bytes - typedef struct { HeaderType Header; From 5a06e1fd2f06112e0aa80dcc042701fdedaf22ff Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 23 Jan 2020 13:10:37 +0100 Subject: [PATCH 115/523] Removed unnecessary define --- server/inc/iso22133.h | 1 - 1 file changed, 1 deletion(-) diff --git a/server/inc/iso22133.h b/server/inc/iso22133.h index 5dc1af6f7..2d6c7d022 100644 --- a/server/inc/iso22133.h +++ b/server/inc/iso22133.h @@ -90,7 +90,6 @@ typedef struct //! *************************** MONR -#define COMMAND_MONR_CODE 0x0006 typedef struct { HeaderType header; From 54ad1cdd471eb8bf13b79c32a0a7f456c09a490e Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 23 Jan 2020 13:12:27 +0100 Subject: [PATCH 116/523] Removed a bunch of unused defines --- server/src/objectcontrol.c | 15 --------------- 1 file changed, 15 deletions(-) diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 59b41e295..94cdd24eb 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -75,23 +75,12 @@ #define COMMAND_DOTM_ROWS_IN_TRANSMISSION 40 #define COMMAND_DTM_BYTES_IN_ROW 30 - -#define COMMAND_OSEM_CODE 2 -#define COMMAND_OSEM_NOFV 3 -#define COMMAND_OSEM_MESSAGE_LENGTH sizeof(OSEMType)-4 - #define COMMAND_OSTM_CODE 3 #define COMMAND_OSTM_NOFV 1 #define COMMAND_OSTM_MESSAGE_LENGTH sizeof(OSTMType) #define COMMAND_OSTM_OPT_SET_ARMED_STATE 2 #define COMMAND_OSTM_OPT_SET_DISARMED_STATE 3 -#define COMMAND_STRT_CODE 4 -#define COMMAND_STRT_NOFV 1 -#define COMMAND_STRT_MESSAGE_LENGTH sizeof(STRTType) -#define COMMAND_STRT_OPT_START_IMMEDIATELY 1 -#define COMMAND_STRT_OPT_START_AT_TIMESTAMP 2 - #define COMMAND_HEAB_CODE 5 #define COMMAND_HEAB_NOFV 2 #define COMMAND_HEAB_MESSAGE_LENGTH sizeof(HEABType) @@ -99,10 +88,6 @@ #define COMMAND_HEAB_OPT_SERVER_STATUS_OK 1 #define COMMAND_HEAB_OPT_SERVER_STATUS_ABORT 2 -#define COMMAND_MONR_CODE 6 -#define COMMAND_MONR_NOFV 12 -#define COMMAND_MONR_MESSAGE_LENGTH sizeof(MONRType) - #define COMMAND_VOIL_CODE 0xA100 //#define COMMAND_VOIL_NOFV 2 #define COMMAND_VOIL_MESSAGE_LENGTH (16 * sizeof(Sim1Type) + sizeof(HeaderType) + 5) From 469be3c609876ce24fff0751fcf9e21c7a109311 Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Thu, 23 Jan 2020 15:42:03 +0100 Subject: [PATCH 117/523] Char transfer working --- server/inc/datadictionary.h | 2 + server/inc/util.h | 21 +- server/src/datadictionary.c | 1630 ++++++++++++++++++----------------- server/src/objectcontrol.c | 4 - server/src/systemcontrol.c | 1 - 5 files changed, 849 insertions(+), 809 deletions(-) diff --git a/server/inc/datadictionary.h b/server/inc/datadictionary.h index 7865e7854..f390d627f 100644 --- a/server/inc/datadictionary.h +++ b/server/inc/datadictionary.h @@ -10,6 +10,8 @@ #ifndef __DATADICTIONARY_H_INCLUDED__ #define __DATADICTIONARY_H_INCLUDED__ +#define SHARED_MEMORY_PATH "/dev/shm/maestro/" + #include "util.h" /*------------------------------------------------------------ -- Function declarations. diff --git a/server/inc/util.h b/server/inc/util.h index b97aacfaa..987b220fd 100644 --- a/server/inc/util.h +++ b/server/inc/util.h @@ -196,7 +196,7 @@ extern "C"{ // The do - while loop makes sure that each function call is properly handled using macros #define LOG_SEND(buf, ...) \ - do {sprintf(buf,__VA_ARGS__);iCommSend(COMM_LOG,buf,strlen(buf)+1);LogMessage(LOG_LEVEL_INFO,buf);fflush(stdout);} while (0) + do {sprintf(buf,__VA_ARGS__);iCommSend(COMM_LOG,buf,strlen(buf)+1);LogMessage(LOG_LEVEL_INFO,buf);fflush(stdout);} while (0) #define GetCurrentDir getcwd #define MAX_PATH_LENGTH 255 @@ -535,6 +535,7 @@ typedef struct U8 numberOfObjects; MONRType *MonrMessages; + char *memory; //U8 OSTMSizeU8; //U8 OSTMData[100]; //U8 STRTSizeU8; @@ -590,18 +591,18 @@ typedef struct typedef struct { - char Type; + char Type; double Latitude; - double Longitude; - double OrigoDistance; + double Longitude; + double OrigoDistance; double OldOrigoDistance; - double DeltaOrigoDistance; + double DeltaOrigoDistance; double x; - double y; - double z; - int CalcIterations; - double ForwardAzimuth1; - double ForwardAzimuth2; + double y; + double z; + int CalcIterations; + double ForwardAzimuth1; + double ForwardAzimuth2; int TrajectoryPositionCount; I32 SyncIndex; double SyncTime; diff --git a/server/src/datadictionary.c b/server/src/datadictionary.c index 6d2e5b421..d2d9e3c3f 100644 --- a/server/src/datadictionary.c +++ b/server/src/datadictionary.c @@ -13,7 +13,9 @@ ------------------------------------------------------------*/ #include #include - +#include +#include +#include #include "datadictionary.h" #include "logging.h" @@ -65,31 +67,31 @@ Initialization data that is configurable is stored in test.conf. * \return Error code defined by ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryConstructor(GSDType * GSD) { - ReadWriteAccess_t Res = READ_OK; - - Res = Res == READ_OK ? DataDictionaryInitOriginLatitudeDbl(GSD) : Res; - Res = Res == READ_OK ? DataDictionaryInitOriginLongitudeDbl(GSD) : Res; - Res = Res == READ_OK ? DataDictionaryInitOriginAltitudeDbl(GSD) : Res; - Res = Res == READ_OK ? DataDictionaryInitVisualizationServerU32(GSD) : Res; - Res = Res == READ_OK ? DataDictionaryInitForceToLocalhostU8(GSD) : Res; - Res = Res == READ_OK ? DataDictionaryInitASPMaxTimeDiffDbl(GSD) : Res; - Res = Res == READ_OK ? DataDictionaryInitASPMaxTrajDiffDbl(GSD) : Res; - Res = Res == READ_OK ? DataDictionaryInitASPStepBackCountU32(GSD) : Res; - Res = Res == READ_OK ? DataDictionaryInitASPFilterLevelDbl(GSD) : Res; - Res = Res == READ_OK ? DataDictionaryInitASPMaxDeltaTimeDbl(GSD) : Res; - Res = Res == READ_OK ? DataDictionaryInitTimeServerIPU32(GSD) : Res; - Res = Res == READ_OK ? DataDictionaryInitTimeServerPortU16(GSD) : Res; - Res = Res == READ_OK ? DataDictionaryInitSimulatorIPU32(GSD) : Res; - Res = Res == READ_OK ? DataDictionaryInitSimulatorTCPPortU16(GSD) : Res; - Res = Res == READ_OK ? DataDictionaryInitSimulatorUDPPortU16(GSD) : Res; - Res = Res == READ_OK ? DataDictionaryInitSimulatorModeU8(GSD) : Res; + ReadWriteAccess_t Res = READ_OK; + + Res = Res == READ_OK ? DataDictionaryInitOriginLatitudeDbl(GSD) : Res; + Res = Res == READ_OK ? DataDictionaryInitOriginLongitudeDbl(GSD) : Res; + Res = Res == READ_OK ? DataDictionaryInitOriginAltitudeDbl(GSD) : Res; + Res = Res == READ_OK ? DataDictionaryInitVisualizationServerU32(GSD) : Res; + Res = Res == READ_OK ? DataDictionaryInitForceToLocalhostU8(GSD) : Res; + Res = Res == READ_OK ? DataDictionaryInitASPMaxTimeDiffDbl(GSD) : Res; + Res = Res == READ_OK ? DataDictionaryInitASPMaxTrajDiffDbl(GSD) : Res; + Res = Res == READ_OK ? DataDictionaryInitASPStepBackCountU32(GSD) : Res; + Res = Res == READ_OK ? DataDictionaryInitASPFilterLevelDbl(GSD) : Res; + Res = Res == READ_OK ? DataDictionaryInitASPMaxDeltaTimeDbl(GSD) : Res; + Res = Res == READ_OK ? DataDictionaryInitTimeServerIPU32(GSD) : Res; + Res = Res == READ_OK ? DataDictionaryInitTimeServerPortU16(GSD) : Res; + Res = Res == READ_OK ? DataDictionaryInitSimulatorIPU32(GSD) : Res; + Res = Res == READ_OK ? DataDictionaryInitSimulatorTCPPortU16(GSD) : Res; + Res = Res == READ_OK ? DataDictionaryInitSimulatorUDPPortU16(GSD) : Res; + Res = Res == READ_OK ? DataDictionaryInitSimulatorModeU8(GSD) : Res; Res = Res == READ_OK ? DataDictionaryInitVOILReceiversC8(GSD) : Res; - Res = Res == READ_OK ? DataDictionaryInitDTMReceiversC8(GSD) : Res; - Res = Res == READ_OK ? DataDictionaryInitExternalSupervisorIPU32(GSD) : Res; - Res = Res == READ_OK ? DataDictionaryInitRVSSConfigU32(GSD) : Res; - Res = Res == READ_OK ? DataDictionaryInitRVSSRateU8(GSD) : Res; - Res = Res == READ_OK ? DataDictionaryInitSupervisorTCPPortU16(GSD) : Res; - Res = Res == READ_OK ? DataDictionaryInitMiscDataC8(GSD) : Res; + Res = Res == READ_OK ? DataDictionaryInitDTMReceiversC8(GSD) : Res; + Res = Res == READ_OK ? DataDictionaryInitExternalSupervisorIPU32(GSD) : Res; + Res = Res == READ_OK ? DataDictionaryInitRVSSConfigU32(GSD) : Res; + Res = Res == READ_OK ? DataDictionaryInitRVSSRateU8(GSD) : Res; + Res = Res == READ_OK ? DataDictionaryInitSupervisorTCPPortU16(GSD) : Res; + Res = Res == READ_OK ? DataDictionaryInitMiscDataC8(GSD) : Res; DataDictionarySetOBCStateU8(GSD, OBC_STATE_UNDEFINED); @@ -103,23 +105,23 @@ ReadWriteAccess_t DataDictionaryConstructor(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitOriginLatitudeDbl(GSDType * GSD) { - ReadWriteAccess_t Res = UNDEFINED; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; - - if (DataDictionarySearchParameter("OrigoLatitude=", ResultBufferC8)) { - Res = READ_OK; - pthread_mutex_lock(&OriginLatitudeMutex); - GSD->OriginLatitudeDbl = atof(ResultBufferC8); - bzero(GSD->OriginLatitudeC8, DD_CONTROL_BUFFER_SIZE_20); - strcat(GSD->OriginLatitudeC8, ResultBufferC8); - pthread_mutex_unlock(&OriginLatitudeMutex); - } - else { - Res = PARAMETER_NOTFOUND; - LogMessage(LOG_LEVEL_ERROR, "OriginLatitude not found!"); - } + ReadWriteAccess_t Res = UNDEFINED; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + + if (DataDictionarySearchParameter("OrigoLatitude=", ResultBufferC8)) { + Res = READ_OK; + pthread_mutex_lock(&OriginLatitudeMutex); + GSD->OriginLatitudeDbl = atof(ResultBufferC8); + bzero(GSD->OriginLatitudeC8, DD_CONTROL_BUFFER_SIZE_20); + strcat(GSD->OriginLatitudeC8, ResultBufferC8); + pthread_mutex_unlock(&OriginLatitudeMutex); + } + else { + Res = PARAMETER_NOTFOUND; + LogMessage(LOG_LEVEL_ERROR, "OriginLatitude not found!"); + } - return Res; + return Res; } /*! @@ -129,19 +131,19 @@ ReadWriteAccess_t DataDictionaryInitOriginLatitudeDbl(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetOriginLatitudeDbl(GSDType * GSD, C8 * Latitude) { - ReadWriteAccess_t Res; + ReadWriteAccess_t Res; - if (UtilWriteConfigurationParameter("OrigoLatitude", Latitude, 0)) { - Res = WRITE_OK; - pthread_mutex_lock(&OriginLatitudeMutex); - GSD->OriginLatitudeDbl = atof(Latitude); - bzero(GSD->OriginLatitudeC8, DD_CONTROL_BUFFER_SIZE_20); - strcat(GSD->OriginLatitudeC8, Latitude); - pthread_mutex_unlock(&OriginLatitudeMutex); - } - else - Res = PARAMETER_NOTFOUND; - return Res; + if (UtilWriteConfigurationParameter("OrigoLatitude", Latitude, 0)) { + Res = WRITE_OK; + pthread_mutex_lock(&OriginLatitudeMutex); + GSD->OriginLatitudeDbl = atof(Latitude); + bzero(GSD->OriginLatitudeC8, DD_CONTROL_BUFFER_SIZE_20); + strcat(GSD->OriginLatitudeC8, Latitude); + pthread_mutex_unlock(&OriginLatitudeMutex); + } + else + Res = PARAMETER_NOTFOUND; + return Res; } /*! @@ -151,10 +153,10 @@ ReadWriteAccess_t DataDictionarySetOriginLatitudeDbl(GSDType * GSD, C8 * Latitud * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetOriginLatitudeDbl(GSDType * GSD, dbl * Latitude) { - pthread_mutex_lock(&OriginLatitudeMutex); - *Latitude = GSD->OriginLatitudeDbl; - pthread_mutex_unlock(&OriginLatitudeMutex); - return READ_OK; + pthread_mutex_lock(&OriginLatitudeMutex); + *Latitude = GSD->OriginLatitudeDbl; + pthread_mutex_unlock(&OriginLatitudeMutex); + return READ_OK; } /*! @@ -164,11 +166,11 @@ ReadWriteAccess_t DataDictionaryGetOriginLatitudeDbl(GSDType * GSD, dbl * Latitu * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetOriginLatitudeC8(GSDType * GSD, C8 * Latitude, U32 BuffLen) { - pthread_mutex_lock(&OriginLatitudeMutex); - bzero(Latitude, BuffLen); - strcat(Latitude, GSD->OriginLatitudeC8); - pthread_mutex_unlock(&OriginLatitudeMutex); - return READ_OK; + pthread_mutex_lock(&OriginLatitudeMutex); + bzero(Latitude, BuffLen); + strcat(Latitude, GSD->OriginLatitudeC8); + pthread_mutex_unlock(&OriginLatitudeMutex); + return READ_OK; } /*END of Origin Latitude*/ @@ -180,23 +182,23 @@ ReadWriteAccess_t DataDictionaryGetOriginLatitudeC8(GSDType * GSD, C8 * Latitude * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitOriginLongitudeDbl(GSDType * GSD) { - ReadWriteAccess_t Res = UNDEFINED; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; - - if (DataDictionarySearchParameter("OrigoLongitude=", ResultBufferC8)) { - Res = READ_OK; - pthread_mutex_lock(&OriginLongitudeMutex); - GSD->OriginLongitudeDbl = atof(ResultBufferC8); - bzero(GSD->OriginLongitudeC8, DD_CONTROL_BUFFER_SIZE_20); - strcat(GSD->OriginLongitudeC8, ResultBufferC8); - pthread_mutex_unlock(&OriginLongitudeMutex); - } - else { - Res = PARAMETER_NOTFOUND; - LogMessage(LOG_LEVEL_ERROR, "OriginLongitude not found!"); - } + ReadWriteAccess_t Res = UNDEFINED; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + + if (DataDictionarySearchParameter("OrigoLongitude=", ResultBufferC8)) { + Res = READ_OK; + pthread_mutex_lock(&OriginLongitudeMutex); + GSD->OriginLongitudeDbl = atof(ResultBufferC8); + bzero(GSD->OriginLongitudeC8, DD_CONTROL_BUFFER_SIZE_20); + strcat(GSD->OriginLongitudeC8, ResultBufferC8); + pthread_mutex_unlock(&OriginLongitudeMutex); + } + else { + Res = PARAMETER_NOTFOUND; + LogMessage(LOG_LEVEL_ERROR, "OriginLongitude not found!"); + } - return Res; + return Res; } /*! @@ -206,19 +208,19 @@ ReadWriteAccess_t DataDictionaryInitOriginLongitudeDbl(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetOriginLongitudeDbl(GSDType * GSD, C8 * Longitude) { - ReadWriteAccess_t Res; + ReadWriteAccess_t Res; - if (UtilWriteConfigurationParameter("OrigoLongitude", Longitude, 0)) { - Res = WRITE_OK; - pthread_mutex_lock(&OriginLongitudeMutex); - GSD->OriginLongitudeDbl = atof(Longitude); - bzero(GSD->OriginLongitudeC8, DD_CONTROL_BUFFER_SIZE_20); - strcat(GSD->OriginLongitudeC8, Longitude); - pthread_mutex_unlock(&OriginLongitudeMutex); - } - else - Res = PARAMETER_NOTFOUND; - return Res; + if (UtilWriteConfigurationParameter("OrigoLongitude", Longitude, 0)) { + Res = WRITE_OK; + pthread_mutex_lock(&OriginLongitudeMutex); + GSD->OriginLongitudeDbl = atof(Longitude); + bzero(GSD->OriginLongitudeC8, DD_CONTROL_BUFFER_SIZE_20); + strcat(GSD->OriginLongitudeC8, Longitude); + pthread_mutex_unlock(&OriginLongitudeMutex); + } + else + Res = PARAMETER_NOTFOUND; + return Res; } /*! @@ -228,10 +230,10 @@ ReadWriteAccess_t DataDictionarySetOriginLongitudeDbl(GSDType * GSD, C8 * Longit * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetOriginLongitudeDbl(GSDType * GSD, dbl * Longitude) { - pthread_mutex_lock(&OriginLongitudeMutex); - *Longitude = GSD->OriginLongitudeDbl; - pthread_mutex_unlock(&OriginLongitudeMutex); - return READ_OK; + pthread_mutex_lock(&OriginLongitudeMutex); + *Longitude = GSD->OriginLongitudeDbl; + pthread_mutex_unlock(&OriginLongitudeMutex); + return READ_OK; } /*! @@ -241,11 +243,11 @@ ReadWriteAccess_t DataDictionaryGetOriginLongitudeDbl(GSDType * GSD, dbl * Longi * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetOriginLongitudeC8(GSDType * GSD, C8 * Longitude, U32 BuffLen) { - pthread_mutex_lock(&OriginLongitudeMutex); - bzero(Longitude, BuffLen); - strcat(Longitude, GSD->OriginLongitudeC8); - pthread_mutex_unlock(&OriginLongitudeMutex); - return READ_OK; + pthread_mutex_lock(&OriginLongitudeMutex); + bzero(Longitude, BuffLen); + strcat(Longitude, GSD->OriginLongitudeC8); + pthread_mutex_unlock(&OriginLongitudeMutex); + return READ_OK; } /*END of Origin Longitude*/ @@ -257,23 +259,23 @@ ReadWriteAccess_t DataDictionaryGetOriginLongitudeC8(GSDType * GSD, C8 * Longitu * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitOriginAltitudeDbl(GSDType * GSD) { - ReadWriteAccess_t Res = UNDEFINED; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; - - if (DataDictionarySearchParameter("OrigoAltitude=", ResultBufferC8)) { - Res = READ_OK; - pthread_mutex_lock(&OriginAltitudeMutex); - GSD->OriginAltitudeDbl = atof(ResultBufferC8); - bzero(GSD->OriginAltitudeC8, DD_CONTROL_BUFFER_SIZE_20); - strcat(GSD->OriginAltitudeC8, ResultBufferC8); - pthread_mutex_unlock(&OriginAltitudeMutex); - } - else { - Res = PARAMETER_NOTFOUND; - LogMessage(LOG_LEVEL_ERROR, "OriginAltitude not found!"); - } + ReadWriteAccess_t Res = UNDEFINED; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + + if (DataDictionarySearchParameter("OrigoAltitude=", ResultBufferC8)) { + Res = READ_OK; + pthread_mutex_lock(&OriginAltitudeMutex); + GSD->OriginAltitudeDbl = atof(ResultBufferC8); + bzero(GSD->OriginAltitudeC8, DD_CONTROL_BUFFER_SIZE_20); + strcat(GSD->OriginAltitudeC8, ResultBufferC8); + pthread_mutex_unlock(&OriginAltitudeMutex); + } + else { + Res = PARAMETER_NOTFOUND; + LogMessage(LOG_LEVEL_ERROR, "OriginAltitude not found!"); + } - return Res; + return Res; } /*! @@ -283,19 +285,19 @@ ReadWriteAccess_t DataDictionaryInitOriginAltitudeDbl(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetOriginAltitudeDbl(GSDType * GSD, C8 * Altitude) { - ReadWriteAccess_t Res; + ReadWriteAccess_t Res; - if (UtilWriteConfigurationParameter("OrigoAltitude", Altitude, 0)) { - Res = WRITE_OK; - pthread_mutex_lock(&OriginAltitudeMutex); - GSD->OriginAltitudeDbl = atof(Altitude); - bzero(GSD->OriginAltitudeC8, DD_CONTROL_BUFFER_SIZE_20); - strcat(GSD->OriginAltitudeC8, Altitude); - pthread_mutex_unlock(&OriginAltitudeMutex); - } - else - Res = PARAMETER_NOTFOUND; - return Res; + if (UtilWriteConfigurationParameter("OrigoAltitude", Altitude, 0)) { + Res = WRITE_OK; + pthread_mutex_lock(&OriginAltitudeMutex); + GSD->OriginAltitudeDbl = atof(Altitude); + bzero(GSD->OriginAltitudeC8, DD_CONTROL_BUFFER_SIZE_20); + strcat(GSD->OriginAltitudeC8, Altitude); + pthread_mutex_unlock(&OriginAltitudeMutex); + } + else + Res = PARAMETER_NOTFOUND; + return Res; } /*! @@ -305,10 +307,10 @@ ReadWriteAccess_t DataDictionarySetOriginAltitudeDbl(GSDType * GSD, C8 * Altitud * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetOriginAltitudeDbl(GSDType * GSD, dbl * Altitude) { - pthread_mutex_lock(&OriginAltitudeMutex); - *Altitude = GSD->OriginAltitudeDbl; - pthread_mutex_unlock(&OriginAltitudeMutex); - return READ_OK; + pthread_mutex_lock(&OriginAltitudeMutex); + *Altitude = GSD->OriginAltitudeDbl; + pthread_mutex_unlock(&OriginAltitudeMutex); + return READ_OK; } /*! @@ -318,11 +320,11 @@ ReadWriteAccess_t DataDictionaryGetOriginAltitudeDbl(GSDType * GSD, dbl * Altitu * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetOriginAltitudeC8(GSDType * GSD, C8 * Altitude, U32 BuffLen) { - pthread_mutex_lock(&OriginAltitudeMutex); - bzero(Altitude, BuffLen); - strcat(Altitude, GSD->OriginAltitudeC8); - pthread_mutex_unlock(&OriginAltitudeMutex); - return READ_OK; + pthread_mutex_lock(&OriginAltitudeMutex); + bzero(Altitude, BuffLen); + strcat(Altitude, GSD->OriginAltitudeC8); + pthread_mutex_unlock(&OriginAltitudeMutex); + return READ_OK; } /*END of Origin Altitude*/ @@ -334,23 +336,23 @@ ReadWriteAccess_t DataDictionaryGetOriginAltitudeC8(GSDType * GSD, C8 * Altitude * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitVisualizationServerU32(GSDType * GSD) { - ReadWriteAccess_t Res = UNDEFINED; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; - - if (DataDictionarySearchParameter("VisualizationServerName=", ResultBufferC8)) { - Res = READ_OK; - pthread_mutex_lock(&VisualizationServerMutex); - GSD->VisualizationServerU32 = UtilIPStringToInt(ResultBufferC8); - bzero(GSD->VisualizationServerC8, DD_CONTROL_BUFFER_SIZE_20); - strcat(GSD->VisualizationServerC8, ResultBufferC8); - pthread_mutex_unlock(&VisualizationServerMutex); - } - else { - Res = PARAMETER_NOTFOUND; - LogMessage(LOG_LEVEL_ERROR, "VisualizationServerName not found!"); - } + ReadWriteAccess_t Res = UNDEFINED; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + + if (DataDictionarySearchParameter("VisualizationServerName=", ResultBufferC8)) { + Res = READ_OK; + pthread_mutex_lock(&VisualizationServerMutex); + GSD->VisualizationServerU32 = UtilIPStringToInt(ResultBufferC8); + bzero(GSD->VisualizationServerC8, DD_CONTROL_BUFFER_SIZE_20); + strcat(GSD->VisualizationServerC8, ResultBufferC8); + pthread_mutex_unlock(&VisualizationServerMutex); + } + else { + Res = PARAMETER_NOTFOUND; + LogMessage(LOG_LEVEL_ERROR, "VisualizationServerName not found!"); + } - return Res; + return Res; } /*! @@ -360,19 +362,19 @@ ReadWriteAccess_t DataDictionaryInitVisualizationServerU32(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetVisualizationServerU32(GSDType * GSD, C8 * IP) { - ReadWriteAccess_t Res; + ReadWriteAccess_t Res; - if (UtilWriteConfigurationParameter("VisualizationServerName", IP, 0)) { - Res = WRITE_OK; - pthread_mutex_lock(&VisualizationServerMutex); - GSD->VisualizationServerU32 = UtilIPStringToInt(IP); - bzero(GSD->VisualizationServerC8, DD_CONTROL_BUFFER_SIZE_20); - strcat(GSD->VisualizationServerC8, IP); - pthread_mutex_unlock(&VisualizationServerMutex); - } - else - Res = PARAMETER_NOTFOUND; - return Res; + if (UtilWriteConfigurationParameter("VisualizationServerName", IP, 0)) { + Res = WRITE_OK; + pthread_mutex_lock(&VisualizationServerMutex); + GSD->VisualizationServerU32 = UtilIPStringToInt(IP); + bzero(GSD->VisualizationServerC8, DD_CONTROL_BUFFER_SIZE_20); + strcat(GSD->VisualizationServerC8, IP); + pthread_mutex_unlock(&VisualizationServerMutex); + } + else + Res = PARAMETER_NOTFOUND; + return Res; } /*! @@ -382,10 +384,10 @@ ReadWriteAccess_t DataDictionarySetVisualizationServerU32(GSDType * GSD, C8 * IP * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetVisualizationServerU32(GSDType * GSD, U32 * IP) { - pthread_mutex_lock(&VisualizationServerMutex); - *IP = GSD->VisualizationServerU32; - pthread_mutex_unlock(&VisualizationServerMutex); - return READ_OK; + pthread_mutex_lock(&VisualizationServerMutex); + *IP = GSD->VisualizationServerU32; + pthread_mutex_unlock(&VisualizationServerMutex); + return READ_OK; } @@ -396,11 +398,11 @@ ReadWriteAccess_t DataDictionaryGetVisualizationServerU32(GSDType * GSD, U32 * I * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetVisualizationServerC8(GSDType * GSD, C8 * IP, U32 BuffLen) { - pthread_mutex_lock(&VisualizationServerMutex); - bzero(IP, BuffLen); - strcat(IP, GSD->VisualizationServerC8); - pthread_mutex_unlock(&VisualizationServerMutex); - return READ_OK; + pthread_mutex_lock(&VisualizationServerMutex); + bzero(IP, BuffLen); + strcat(IP, GSD->VisualizationServerC8); + pthread_mutex_unlock(&VisualizationServerMutex); + return READ_OK; } /*END of VisualizationServer*/ @@ -413,21 +415,21 @@ ReadWriteAccess_t DataDictionaryGetVisualizationServerC8(GSDType * GSD, C8 * IP, * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitForceToLocalhostU8(GSDType * GSD) { - ReadWriteAccess_t Res = UNDEFINED; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + ReadWriteAccess_t Res = UNDEFINED; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + + if (DataDictionarySearchParameter("ForceObjectToLocalhost=", ResultBufferC8)) { + Res = READ_OK; + pthread_mutex_lock(&ForceObjectToLocalhostMutex); + GSD->ForceObjectToLocalhostU8 = atoi(ResultBufferC8); + pthread_mutex_unlock(&ForceObjectToLocalhostMutex); + } + else { + Res = PARAMETER_NOTFOUND; + LogMessage(LOG_LEVEL_ERROR, "ForceObjectToLocalhost not found!"); + } - if (DataDictionarySearchParameter("ForceObjectToLocalhost=", ResultBufferC8)) { - Res = READ_OK; - pthread_mutex_lock(&ForceObjectToLocalhostMutex); - GSD->ForceObjectToLocalhostU8 = atoi(ResultBufferC8); - pthread_mutex_unlock(&ForceObjectToLocalhostMutex); - } - else { - Res = PARAMETER_NOTFOUND; - LogMessage(LOG_LEVEL_ERROR, "ForceObjectToLocalhost not found!"); - } - - return Res; + return Res; } /*! @@ -437,17 +439,17 @@ ReadWriteAccess_t DataDictionaryInitForceToLocalhostU8(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetForceToLocalhostU8(GSDType * GSD, C8 * ForceLocalhost) { - ReadWriteAccess_t Res; + ReadWriteAccess_t Res; - if (UtilWriteConfigurationParameter("ForceObjectToLocalhost", ForceLocalhost, 0)) { - Res = WRITE_OK; - pthread_mutex_lock(&ForceObjectToLocalhostMutex); - GSD->ForceObjectToLocalhostU8 = atoi(ForceLocalhost); - pthread_mutex_unlock(&ForceObjectToLocalhostMutex); - } - else - Res = PARAMETER_NOTFOUND; - return Res; + if (UtilWriteConfigurationParameter("ForceObjectToLocalhost", ForceLocalhost, 0)) { + Res = WRITE_OK; + pthread_mutex_lock(&ForceObjectToLocalhostMutex); + GSD->ForceObjectToLocalhostU8 = atoi(ForceLocalhost); + pthread_mutex_unlock(&ForceObjectToLocalhostMutex); + } + else + Res = PARAMETER_NOTFOUND; + return Res; } /*! @@ -457,10 +459,10 @@ ReadWriteAccess_t DataDictionarySetForceToLocalhostU8(GSDType * GSD, C8 * ForceL * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetForceToLocalhostU8(GSDType * GSD, U8 * ForceLocalhost) { - pthread_mutex_lock(&ForceObjectToLocalhostMutex); - *ForceLocalhost = GSD->ForceObjectToLocalhostU8; - pthread_mutex_unlock(&ForceObjectToLocalhostMutex); - return READ_OK; + pthread_mutex_lock(&ForceObjectToLocalhostMutex); + *ForceLocalhost = GSD->ForceObjectToLocalhostU8; + pthread_mutex_unlock(&ForceObjectToLocalhostMutex); + return READ_OK; } /*END of ForceToLocalhost*/ @@ -472,21 +474,21 @@ ReadWriteAccess_t DataDictionaryGetForceToLocalhostU8(GSDType * GSD, U8 * ForceL * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitASPMaxTimeDiffDbl(GSDType * GSD) { - ReadWriteAccess_t Res = UNDEFINED; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; - - if (DataDictionarySearchParameter("ASPMaxTimeDiff=", ResultBufferC8)) { - Res = READ_OK; - pthread_mutex_lock(&ASPMaxTimeDiffMutex); - GSD->ASPMaxTimeDiffDbl = atof(ResultBufferC8); - pthread_mutex_unlock(&ASPMaxTimeDiffMutex); - } - else { - Res = PARAMETER_NOTFOUND; - LogMessage(LOG_LEVEL_ERROR, "ASPMaxTimeDiff not found!"); - } + ReadWriteAccess_t Res = UNDEFINED; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + + if (DataDictionarySearchParameter("ASPMaxTimeDiff=", ResultBufferC8)) { + Res = READ_OK; + pthread_mutex_lock(&ASPMaxTimeDiffMutex); + GSD->ASPMaxTimeDiffDbl = atof(ResultBufferC8); + pthread_mutex_unlock(&ASPMaxTimeDiffMutex); + } + else { + Res = PARAMETER_NOTFOUND; + LogMessage(LOG_LEVEL_ERROR, "ASPMaxTimeDiff not found!"); + } - return Res; + return Res; } /*! @@ -496,17 +498,17 @@ ReadWriteAccess_t DataDictionaryInitASPMaxTimeDiffDbl(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetASPMaxTimeDiffDbl(GSDType * GSD, C8 * ASPMaxTimeDiff) { - ReadWriteAccess_t Res; + ReadWriteAccess_t Res; - if (UtilWriteConfigurationParameter("ASPMaxTimeDiff", ASPMaxTimeDiff, 0)) { - Res = WRITE_OK; - pthread_mutex_lock(&ASPMaxTimeDiffMutex); - GSD->ASPMaxTimeDiffDbl = atof(ASPMaxTimeDiff); - pthread_mutex_unlock(&ASPMaxTimeDiffMutex); - } - else - Res = PARAMETER_NOTFOUND; - return Res; + if (UtilWriteConfigurationParameter("ASPMaxTimeDiff", ASPMaxTimeDiff, 0)) { + Res = WRITE_OK; + pthread_mutex_lock(&ASPMaxTimeDiffMutex); + GSD->ASPMaxTimeDiffDbl = atof(ASPMaxTimeDiff); + pthread_mutex_unlock(&ASPMaxTimeDiffMutex); + } + else + Res = PARAMETER_NOTFOUND; + return Res; } /*! @@ -516,10 +518,10 @@ ReadWriteAccess_t DataDictionarySetASPMaxTimeDiffDbl(GSDType * GSD, C8 * ASPMaxT * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetASPMaxTimeDiffDbl(GSDType * GSD, dbl * ASPMaxTimeDiff) { - pthread_mutex_lock(&ASPMaxTimeDiffMutex); - *ASPMaxTimeDiff = GSD->ASPMaxTimeDiffDbl; - pthread_mutex_unlock(&ASPMaxTimeDiffMutex); - return READ_OK; + pthread_mutex_lock(&ASPMaxTimeDiffMutex); + *ASPMaxTimeDiff = GSD->ASPMaxTimeDiffDbl; + pthread_mutex_unlock(&ASPMaxTimeDiffMutex); + return READ_OK; } /*END of ASPMaxTimeDiff*/ @@ -531,21 +533,21 @@ ReadWriteAccess_t DataDictionaryGetASPMaxTimeDiffDbl(GSDType * GSD, dbl * ASPMax * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitASPMaxTrajDiffDbl(GSDType * GSD) { - ReadWriteAccess_t Res = UNDEFINED; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; - - if (DataDictionarySearchParameter("ASPMaxTrajDiff=", ResultBufferC8)) { - Res = READ_OK; - pthread_mutex_lock(&ASPMaxTrajDiffMutex); - GSD->ASPMaxTrajDiffDbl = atof(ResultBufferC8); - pthread_mutex_unlock(&ASPMaxTrajDiffMutex); - } - else { - Res = PARAMETER_NOTFOUND; - LogMessage(LOG_LEVEL_ERROR, "ASPMaxTrajDiff not found!"); - } + ReadWriteAccess_t Res = UNDEFINED; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + + if (DataDictionarySearchParameter("ASPMaxTrajDiff=", ResultBufferC8)) { + Res = READ_OK; + pthread_mutex_lock(&ASPMaxTrajDiffMutex); + GSD->ASPMaxTrajDiffDbl = atof(ResultBufferC8); + pthread_mutex_unlock(&ASPMaxTrajDiffMutex); + } + else { + Res = PARAMETER_NOTFOUND; + LogMessage(LOG_LEVEL_ERROR, "ASPMaxTrajDiff not found!"); + } - return Res; + return Res; } /*! @@ -555,17 +557,17 @@ ReadWriteAccess_t DataDictionaryInitASPMaxTrajDiffDbl(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetASPMaxTrajDiffDbl(GSDType * GSD, C8 * ASPMaxTrajDiff) { - ReadWriteAccess_t Res; + ReadWriteAccess_t Res; - if (UtilWriteConfigurationParameter("ASPMaxTrajDiff", ASPMaxTrajDiff, 0)) { - Res = WRITE_OK; - pthread_mutex_lock(&ASPMaxTrajDiffMutex); - GSD->ASPMaxTrajDiffDbl = atof(ASPMaxTrajDiff); - pthread_mutex_unlock(&ASPMaxTrajDiffMutex); - } - else - Res = PARAMETER_NOTFOUND; - return Res; + if (UtilWriteConfigurationParameter("ASPMaxTrajDiff", ASPMaxTrajDiff, 0)) { + Res = WRITE_OK; + pthread_mutex_lock(&ASPMaxTrajDiffMutex); + GSD->ASPMaxTrajDiffDbl = atof(ASPMaxTrajDiff); + pthread_mutex_unlock(&ASPMaxTrajDiffMutex); + } + else + Res = PARAMETER_NOTFOUND; + return Res; } /*! @@ -575,10 +577,10 @@ ReadWriteAccess_t DataDictionarySetASPMaxTrajDiffDbl(GSDType * GSD, C8 * ASPMaxT * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetASPMaxTrajDiffDbl(GSDType * GSD, dbl * ASPMaxTrajDiff) { - pthread_mutex_lock(&ASPMaxTrajDiffMutex); - *ASPMaxTrajDiff = GSD->ASPMaxTrajDiffDbl; - pthread_mutex_unlock(&ASPMaxTrajDiffMutex); - return READ_OK; + pthread_mutex_lock(&ASPMaxTrajDiffMutex); + *ASPMaxTrajDiff = GSD->ASPMaxTrajDiffDbl; + pthread_mutex_unlock(&ASPMaxTrajDiffMutex); + return READ_OK; } /*END of ASPMaxTrajDiff*/ @@ -591,21 +593,21 @@ ReadWriteAccess_t DataDictionaryGetASPMaxTrajDiffDbl(GSDType * GSD, dbl * ASPMax * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitASPStepBackCountU32(GSDType * GSD) { - ReadWriteAccess_t Res = UNDEFINED; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; - - if (DataDictionarySearchParameter("ASPStepBackCount=", ResultBufferC8)) { - Res = READ_OK; - pthread_mutex_lock(&ASPStepBackCountMutex); - GSD->ASPStepBackCountU32 = atoi(ResultBufferC8); - pthread_mutex_unlock(&ASPStepBackCountMutex); - } - else { - Res = PARAMETER_NOTFOUND; - LogMessage(LOG_LEVEL_ERROR, "ASPStepBackCount not found!"); - } + ReadWriteAccess_t Res = UNDEFINED; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + + if (DataDictionarySearchParameter("ASPStepBackCount=", ResultBufferC8)) { + Res = READ_OK; + pthread_mutex_lock(&ASPStepBackCountMutex); + GSD->ASPStepBackCountU32 = atoi(ResultBufferC8); + pthread_mutex_unlock(&ASPStepBackCountMutex); + } + else { + Res = PARAMETER_NOTFOUND; + LogMessage(LOG_LEVEL_ERROR, "ASPStepBackCount not found!"); + } - return Res; + return Res; } /*! @@ -615,17 +617,17 @@ ReadWriteAccess_t DataDictionaryInitASPStepBackCountU32(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetASPStepBackCountU32(GSDType * GSD, C8 * ASPStepBackCount) { - ReadWriteAccess_t Res; + ReadWriteAccess_t Res; - if (UtilWriteConfigurationParameter("ASPStepBackCount", ASPStepBackCount, 0)) { - Res = WRITE_OK; - pthread_mutex_lock(&ASPStepBackCountMutex); - GSD->ASPStepBackCountU32 = atoi(ASPStepBackCount); - pthread_mutex_unlock(&ASPStepBackCountMutex); - } - else - Res = PARAMETER_NOTFOUND; - return Res; + if (UtilWriteConfigurationParameter("ASPStepBackCount", ASPStepBackCount, 0)) { + Res = WRITE_OK; + pthread_mutex_lock(&ASPStepBackCountMutex); + GSD->ASPStepBackCountU32 = atoi(ASPStepBackCount); + pthread_mutex_unlock(&ASPStepBackCountMutex); + } + else + Res = PARAMETER_NOTFOUND; + return Res; } /*! @@ -635,10 +637,10 @@ ReadWriteAccess_t DataDictionarySetASPStepBackCountU32(GSDType * GSD, C8 * ASPSt * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetASPStepBackCountU32(GSDType * GSD, U32 * ASPStepBackCount) { - pthread_mutex_lock(&ASPStepBackCountMutex); - *ASPStepBackCount = GSD->ASPStepBackCountU32; - pthread_mutex_unlock(&ASPStepBackCountMutex); - return READ_OK; + pthread_mutex_lock(&ASPStepBackCountMutex); + *ASPStepBackCount = GSD->ASPStepBackCountU32; + pthread_mutex_unlock(&ASPStepBackCountMutex); + return READ_OK; } /*END of ASPStepBackCount*/ @@ -650,21 +652,21 @@ ReadWriteAccess_t DataDictionaryGetASPStepBackCountU32(GSDType * GSD, U32 * ASPS * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitASPFilterLevelDbl(GSDType * GSD) { - ReadWriteAccess_t Res = UNDEFINED; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; - - if (DataDictionarySearchParameter("ASPFilterLevel=", ResultBufferC8)) { - Res = READ_OK; - pthread_mutex_lock(&ASPFilterLevelMutex); - GSD->ASPFilterLevelDbl = atof(ResultBufferC8); - pthread_mutex_unlock(&ASPFilterLevelMutex); - } - else { - Res = PARAMETER_NOTFOUND; - LogMessage(LOG_LEVEL_ERROR, "ASPFilterLevel not found!"); - } + ReadWriteAccess_t Res = UNDEFINED; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + + if (DataDictionarySearchParameter("ASPFilterLevel=", ResultBufferC8)) { + Res = READ_OK; + pthread_mutex_lock(&ASPFilterLevelMutex); + GSD->ASPFilterLevelDbl = atof(ResultBufferC8); + pthread_mutex_unlock(&ASPFilterLevelMutex); + } + else { + Res = PARAMETER_NOTFOUND; + LogMessage(LOG_LEVEL_ERROR, "ASPFilterLevel not found!"); + } - return Res; + return Res; } /*! @@ -674,17 +676,17 @@ ReadWriteAccess_t DataDictionaryInitASPFilterLevelDbl(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetASPFilterLevelDbl(GSDType * GSD, C8 * ASPFilterLevel) { - ReadWriteAccess_t Res; + ReadWriteAccess_t Res; - if (UtilWriteConfigurationParameter("ASPFilterLevel", ASPFilterLevel, 0)) { - Res = WRITE_OK; - pthread_mutex_lock(&ASPFilterLevelMutex); - GSD->ASPFilterLevelDbl = atof(ASPFilterLevel); - pthread_mutex_unlock(&ASPFilterLevelMutex); - } - else - Res = PARAMETER_NOTFOUND; - return Res; + if (UtilWriteConfigurationParameter("ASPFilterLevel", ASPFilterLevel, 0)) { + Res = WRITE_OK; + pthread_mutex_lock(&ASPFilterLevelMutex); + GSD->ASPFilterLevelDbl = atof(ASPFilterLevel); + pthread_mutex_unlock(&ASPFilterLevelMutex); + } + else + Res = PARAMETER_NOTFOUND; + return Res; } /*! @@ -694,10 +696,10 @@ ReadWriteAccess_t DataDictionarySetASPFilterLevelDbl(GSDType * GSD, C8 * ASPFilt * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetASPFilterLevelDbl(GSDType * GSD, dbl * ASPFilterLevel) { - pthread_mutex_lock(&ASPFilterLevelMutex); - *ASPFilterLevel = GSD->ASPFilterLevelDbl; - pthread_mutex_unlock(&ASPFilterLevelMutex); - return READ_OK; + pthread_mutex_lock(&ASPFilterLevelMutex); + *ASPFilterLevel = GSD->ASPFilterLevelDbl; + pthread_mutex_unlock(&ASPFilterLevelMutex); + return READ_OK; } /*END of ASPFilterLevel*/ @@ -709,21 +711,21 @@ ReadWriteAccess_t DataDictionaryGetASPFilterLevelDbl(GSDType * GSD, dbl * ASPFil * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitASPMaxDeltaTimeDbl(GSDType * GSD) { - ReadWriteAccess_t Res = UNDEFINED; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + ReadWriteAccess_t Res = UNDEFINED; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + + if (DataDictionarySearchParameter("ASPMaxDeltaTime=", ResultBufferC8)) { + Res = READ_OK; + pthread_mutex_lock(&ASPMaxDeltaTimeMutex); + GSD->ASPMaxDeltaTimeDbl = atof(ResultBufferC8); + pthread_mutex_unlock(&ASPMaxDeltaTimeMutex); + } + else { + Res = PARAMETER_NOTFOUND; + LogMessage(LOG_LEVEL_ERROR, "ASPMaxDeltaTime not found!"); + } - if (DataDictionarySearchParameter("ASPMaxDeltaTime=", ResultBufferC8)) { - Res = READ_OK; - pthread_mutex_lock(&ASPMaxDeltaTimeMutex); - GSD->ASPMaxDeltaTimeDbl = atof(ResultBufferC8); - pthread_mutex_unlock(&ASPMaxDeltaTimeMutex); - } - else { - Res = PARAMETER_NOTFOUND; - LogMessage(LOG_LEVEL_ERROR, "ASPMaxDeltaTime not found!"); - } - - return Res; + return Res; } /*! @@ -733,17 +735,17 @@ ReadWriteAccess_t DataDictionaryInitASPMaxDeltaTimeDbl(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetASPMaxDeltaTimeDbl(GSDType * GSD, C8 * ASPMaxDeltaTime) { - ReadWriteAccess_t Res; + ReadWriteAccess_t Res; - if (UtilWriteConfigurationParameter("ASPMaxDeltaTime", ASPMaxDeltaTime, 0)) { - Res = WRITE_OK; - pthread_mutex_lock(&ASPMaxDeltaTimeMutex); - GSD->ASPMaxDeltaTimeDbl = atof(ASPMaxDeltaTime); - pthread_mutex_unlock(&ASPMaxDeltaTimeMutex); - } - else - Res = PARAMETER_NOTFOUND; - return Res; + if (UtilWriteConfigurationParameter("ASPMaxDeltaTime", ASPMaxDeltaTime, 0)) { + Res = WRITE_OK; + pthread_mutex_lock(&ASPMaxDeltaTimeMutex); + GSD->ASPMaxDeltaTimeDbl = atof(ASPMaxDeltaTime); + pthread_mutex_unlock(&ASPMaxDeltaTimeMutex); + } + else + Res = PARAMETER_NOTFOUND; + return Res; } /*! @@ -753,10 +755,10 @@ ReadWriteAccess_t DataDictionarySetASPMaxDeltaTimeDbl(GSDType * GSD, C8 * ASPMax * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetASPMaxDeltaTimeDbl(GSDType * GSD, dbl * ASPMaxDeltaTime) { - pthread_mutex_lock(&ASPMaxDeltaTimeMutex); - *ASPMaxDeltaTime = GSD->ASPMaxDeltaTimeDbl; - pthread_mutex_unlock(&ASPMaxDeltaTimeMutex); - return READ_OK; + pthread_mutex_lock(&ASPMaxDeltaTimeMutex); + *ASPMaxDeltaTime = GSD->ASPMaxDeltaTimeDbl; + pthread_mutex_unlock(&ASPMaxDeltaTimeMutex); + return READ_OK; } /*END of ASPFilterLevel*/ @@ -769,23 +771,23 @@ ReadWriteAccess_t DataDictionaryGetASPMaxDeltaTimeDbl(GSDType * GSD, dbl * ASPMa * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitTimeServerIPU32(GSDType * GSD) { - ReadWriteAccess_t Res = UNDEFINED; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + ReadWriteAccess_t Res = UNDEFINED; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + + if (DataDictionarySearchParameter("TimeServerIP=", ResultBufferC8)) { + Res = READ_OK; + pthread_mutex_lock(&TimeServerIPMutex); + GSD->TimeServerIPU32 = UtilIPStringToInt(ResultBufferC8); + bzero(GSD->TimeServerIPC8, DD_CONTROL_BUFFER_SIZE_20); + strcat(GSD->TimeServerIPC8, ResultBufferC8); + pthread_mutex_unlock(&TimeServerIPMutex); + } + else { + Res = PARAMETER_NOTFOUND; + LogMessage(LOG_LEVEL_ERROR, "TimeServerIP not found!"); + } - if (DataDictionarySearchParameter("TimeServerIP=", ResultBufferC8)) { - Res = READ_OK; - pthread_mutex_lock(&TimeServerIPMutex); - GSD->TimeServerIPU32 = UtilIPStringToInt(ResultBufferC8); - bzero(GSD->TimeServerIPC8, DD_CONTROL_BUFFER_SIZE_20); - strcat(GSD->TimeServerIPC8, ResultBufferC8); - pthread_mutex_unlock(&TimeServerIPMutex); - } - else { - Res = PARAMETER_NOTFOUND; - LogMessage(LOG_LEVEL_ERROR, "TimeServerIP not found!"); - } - - return Res; + return Res; } /*! @@ -795,19 +797,19 @@ ReadWriteAccess_t DataDictionaryInitTimeServerIPU32(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetTimeServerIPU32(GSDType * GSD, C8 * TimeServerIP) { - ReadWriteAccess_t Res; + ReadWriteAccess_t Res; - if (UtilWriteConfigurationParameter("TimeServerIP", TimeServerIP, 0)) { - Res = WRITE_OK; - pthread_mutex_lock(&TimeServerIPMutex); - GSD->TimeServerIPU32 = UtilIPStringToInt(TimeServerIP); - bzero(GSD->TimeServerIPC8, DD_CONTROL_BUFFER_SIZE_20); - strcat(GSD->TimeServerIPC8, TimeServerIP); - pthread_mutex_unlock(&TimeServerIPMutex); - } - else - Res = PARAMETER_NOTFOUND; - return Res; + if (UtilWriteConfigurationParameter("TimeServerIP", TimeServerIP, 0)) { + Res = WRITE_OK; + pthread_mutex_lock(&TimeServerIPMutex); + GSD->TimeServerIPU32 = UtilIPStringToInt(TimeServerIP); + bzero(GSD->TimeServerIPC8, DD_CONTROL_BUFFER_SIZE_20); + strcat(GSD->TimeServerIPC8, TimeServerIP); + pthread_mutex_unlock(&TimeServerIPMutex); + } + else + Res = PARAMETER_NOTFOUND; + return Res; } /*! @@ -817,10 +819,10 @@ ReadWriteAccess_t DataDictionarySetTimeServerIPU32(GSDType * GSD, C8 * TimeServe * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetTimeServerIPU32(GSDType * GSD, U32 * TimeServerIP) { - pthread_mutex_lock(&TimeServerIPMutex); - *TimeServerIP = GSD->TimeServerIPU32; - pthread_mutex_unlock(&TimeServerIPMutex); - return READ_OK; + pthread_mutex_lock(&TimeServerIPMutex); + *TimeServerIP = GSD->TimeServerIPU32; + pthread_mutex_unlock(&TimeServerIPMutex); + return READ_OK; } /*! @@ -830,11 +832,11 @@ ReadWriteAccess_t DataDictionaryGetTimeServerIPU32(GSDType * GSD, U32 * TimeServ * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetTimeServerIPC8(GSDType * GSD, C8 * TimeServerIP, U32 BuffLen) { - pthread_mutex_lock(&TimeServerIPMutex); - bzero(TimeServerIP, BuffLen); - strcat(TimeServerIP, GSD->TimeServerIPC8); - pthread_mutex_unlock(&TimeServerIPMutex); - return READ_OK; + pthread_mutex_lock(&TimeServerIPMutex); + bzero(TimeServerIP, BuffLen); + strcat(TimeServerIP, GSD->TimeServerIPC8); + pthread_mutex_unlock(&TimeServerIPMutex); + return READ_OK; } /*END of TimeServerIP*/ @@ -847,21 +849,21 @@ ReadWriteAccess_t DataDictionaryGetTimeServerIPC8(GSDType * GSD, C8 * TimeServer * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitTimeServerPortU16(GSDType * GSD) { - ReadWriteAccess_t Res = UNDEFINED; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + ReadWriteAccess_t Res = UNDEFINED; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + + if (DataDictionarySearchParameter("TimeServerPort=", ResultBufferC8)) { + Res = READ_OK; + pthread_mutex_lock(&TimeServerPortMutex); + GSD->TimeServerPortU16 = atoi(ResultBufferC8); + pthread_mutex_unlock(&TimeServerPortMutex); + } + else { + Res = PARAMETER_NOTFOUND; + LogMessage(LOG_LEVEL_ERROR, "TimeServerPort not found!"); + } - if (DataDictionarySearchParameter("TimeServerPort=", ResultBufferC8)) { - Res = READ_OK; - pthread_mutex_lock(&TimeServerPortMutex); - GSD->TimeServerPortU16 = atoi(ResultBufferC8); - pthread_mutex_unlock(&TimeServerPortMutex); - } - else { - Res = PARAMETER_NOTFOUND; - LogMessage(LOG_LEVEL_ERROR, "TimeServerPort not found!"); - } - - return Res; + return Res; } /*! @@ -871,17 +873,17 @@ ReadWriteAccess_t DataDictionaryInitTimeServerPortU16(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetTimeServerPortU16(GSDType * GSD, C8 * TimeServerPort) { - ReadWriteAccess_t Res; + ReadWriteAccess_t Res; - if (UtilWriteConfigurationParameter("TimeServerPort", TimeServerPort, 0)) { - Res = WRITE_OK; - pthread_mutex_lock(&TimeServerPortMutex); - GSD->TimeServerPortU16 = atoi(TimeServerPort); - pthread_mutex_unlock(&TimeServerPortMutex); - } - else - Res = PARAMETER_NOTFOUND; - return Res; + if (UtilWriteConfigurationParameter("TimeServerPort", TimeServerPort, 0)) { + Res = WRITE_OK; + pthread_mutex_lock(&TimeServerPortMutex); + GSD->TimeServerPortU16 = atoi(TimeServerPort); + pthread_mutex_unlock(&TimeServerPortMutex); + } + else + Res = PARAMETER_NOTFOUND; + return Res; } /*! @@ -891,10 +893,10 @@ ReadWriteAccess_t DataDictionarySetTimeServerPortU16(GSDType * GSD, C8 * TimeSer * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetTimeServerPortU16(GSDType * GSD, U16 * TimeServerPort) { - pthread_mutex_lock(&TimeServerPortMutex); - *TimeServerPort = GSD->TimeServerPortU16; - pthread_mutex_unlock(&TimeServerPortMutex); - return READ_OK; + pthread_mutex_lock(&TimeServerPortMutex); + *TimeServerPort = GSD->TimeServerPortU16; + pthread_mutex_unlock(&TimeServerPortMutex); + return READ_OK; } /*END of TimeServerPort*/ @@ -907,23 +909,23 @@ ReadWriteAccess_t DataDictionaryGetTimeServerPortU16(GSDType * GSD, U16 * TimeSe * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitSimulatorIPU32(GSDType * GSD) { - ReadWriteAccess_t Res = UNDEFINED; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + ReadWriteAccess_t Res = UNDEFINED; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + + if (DataDictionarySearchParameter("SimulatorIP=", ResultBufferC8)) { + Res = READ_OK; + pthread_mutex_lock(&SimulatorIPMutex); + GSD->SimulatorIPU32 = UtilIPStringToInt(ResultBufferC8); + bzero(GSD->SimulatorIPC8, DD_CONTROL_BUFFER_SIZE_20); + strcat(GSD->SimulatorIPC8, ResultBufferC8); + pthread_mutex_unlock(&SimulatorIPMutex); + } + else { + Res = PARAMETER_NOTFOUND; + LogMessage(LOG_LEVEL_ERROR, "SimulatorIP not found!"); + } - if (DataDictionarySearchParameter("SimulatorIP=", ResultBufferC8)) { - Res = READ_OK; - pthread_mutex_lock(&SimulatorIPMutex); - GSD->SimulatorIPU32 = UtilIPStringToInt(ResultBufferC8); - bzero(GSD->SimulatorIPC8, DD_CONTROL_BUFFER_SIZE_20); - strcat(GSD->SimulatorIPC8, ResultBufferC8); - pthread_mutex_unlock(&SimulatorIPMutex); - } - else { - Res = PARAMETER_NOTFOUND; - LogMessage(LOG_LEVEL_ERROR, "SimulatorIP not found!"); - } - - return Res; + return Res; } /*! @@ -933,19 +935,19 @@ ReadWriteAccess_t DataDictionaryInitSimulatorIPU32(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetSimulatorIPU32(GSDType * GSD, C8 * SimulatorIP) { - ReadWriteAccess_t Res; + ReadWriteAccess_t Res; - if (UtilWriteConfigurationParameter("SimulatorIP", SimulatorIP, 0)) { - Res = WRITE_OK; - pthread_mutex_lock(&SimulatorIPMutex); - GSD->SimulatorIPU32 = UtilIPStringToInt(SimulatorIP); - bzero(GSD->SimulatorIPC8, DD_CONTROL_BUFFER_SIZE_20); - strcat(GSD->SimulatorIPC8, SimulatorIP); - pthread_mutex_unlock(&SimulatorIPMutex); - } - else - Res = PARAMETER_NOTFOUND; - return Res; + if (UtilWriteConfigurationParameter("SimulatorIP", SimulatorIP, 0)) { + Res = WRITE_OK; + pthread_mutex_lock(&SimulatorIPMutex); + GSD->SimulatorIPU32 = UtilIPStringToInt(SimulatorIP); + bzero(GSD->SimulatorIPC8, DD_CONTROL_BUFFER_SIZE_20); + strcat(GSD->SimulatorIPC8, SimulatorIP); + pthread_mutex_unlock(&SimulatorIPMutex); + } + else + Res = PARAMETER_NOTFOUND; + return Res; } /*! @@ -955,10 +957,10 @@ ReadWriteAccess_t DataDictionarySetSimulatorIPU32(GSDType * GSD, C8 * SimulatorI * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetSimulatorIPU32(GSDType * GSD, U32 * SimulatorIP) { - pthread_mutex_lock(&SimulatorIPMutex); - *SimulatorIP = GSD->SimulatorIPU32; - pthread_mutex_unlock(&SimulatorIPMutex); - return READ_OK; + pthread_mutex_lock(&SimulatorIPMutex); + *SimulatorIP = GSD->SimulatorIPU32; + pthread_mutex_unlock(&SimulatorIPMutex); + return READ_OK; } /*! @@ -968,11 +970,11 @@ ReadWriteAccess_t DataDictionaryGetSimulatorIPU32(GSDType * GSD, U32 * Simulator * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetSimulatorIPC8(GSDType * GSD, C8 * SimulatorIP, U32 BuffLen) { - pthread_mutex_lock(&SimulatorIPMutex); - bzero(SimulatorIP, BuffLen); - strcat(SimulatorIP, GSD->SimulatorIPC8); - pthread_mutex_unlock(&SimulatorIPMutex); - return READ_OK; + pthread_mutex_lock(&SimulatorIPMutex); + bzero(SimulatorIP, BuffLen); + strcat(SimulatorIP, GSD->SimulatorIPC8); + pthread_mutex_unlock(&SimulatorIPMutex); + return READ_OK; } /*END of SimulatorIP*/ @@ -984,21 +986,21 @@ ReadWriteAccess_t DataDictionaryGetSimulatorIPC8(GSDType * GSD, C8 * SimulatorIP * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitSimulatorTCPPortU16(GSDType * GSD) { - ReadWriteAccess_t Res = UNDEFINED; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + ReadWriteAccess_t Res = UNDEFINED; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + + if (DataDictionarySearchParameter("SimulatorTCPPort=", ResultBufferC8)) { + Res = READ_OK; + pthread_mutex_lock(&SimulatorTCPPortMutex); + GSD->SimulatorTCPPortU16 = atoi(ResultBufferC8); + pthread_mutex_unlock(&SimulatorTCPPortMutex); + } + else { + Res = PARAMETER_NOTFOUND; + LogMessage(LOG_LEVEL_ERROR, "SimulatorTCPPort not found!"); + } - if (DataDictionarySearchParameter("SimulatorTCPPort=", ResultBufferC8)) { - Res = READ_OK; - pthread_mutex_lock(&SimulatorTCPPortMutex); - GSD->SimulatorTCPPortU16 = atoi(ResultBufferC8); - pthread_mutex_unlock(&SimulatorTCPPortMutex); - } - else { - Res = PARAMETER_NOTFOUND; - LogMessage(LOG_LEVEL_ERROR, "SimulatorTCPPort not found!"); - } - - return Res; + return Res; } /*! @@ -1008,17 +1010,17 @@ ReadWriteAccess_t DataDictionaryInitSimulatorTCPPortU16(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetSimulatorTCPPortU16(GSDType * GSD, C8 * SimulatorTCPPort) { - ReadWriteAccess_t Res; + ReadWriteAccess_t Res; - if (UtilWriteConfigurationParameter("SimulatorTCPPort", SimulatorTCPPort, 0)) { - Res = WRITE_OK; - pthread_mutex_lock(&SimulatorTCPPortMutex); - GSD->SimulatorTCPPortU16 = atoi(SimulatorTCPPort); - pthread_mutex_unlock(&SimulatorTCPPortMutex); - } - else - Res = PARAMETER_NOTFOUND; - return Res; + if (UtilWriteConfigurationParameter("SimulatorTCPPort", SimulatorTCPPort, 0)) { + Res = WRITE_OK; + pthread_mutex_lock(&SimulatorTCPPortMutex); + GSD->SimulatorTCPPortU16 = atoi(SimulatorTCPPort); + pthread_mutex_unlock(&SimulatorTCPPortMutex); + } + else + Res = PARAMETER_NOTFOUND; + return Res; } /*! @@ -1028,10 +1030,10 @@ ReadWriteAccess_t DataDictionarySetSimulatorTCPPortU16(GSDType * GSD, C8 * Simul * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetSimulatorTCPPortU16(GSDType * GSD, U16 * SimulatorTCPPort) { - pthread_mutex_lock(&SimulatorTCPPortMutex); - *SimulatorTCPPort = GSD->SimulatorTCPPortU16; - pthread_mutex_unlock(&SimulatorTCPPortMutex); - return READ_OK; + pthread_mutex_lock(&SimulatorTCPPortMutex); + *SimulatorTCPPort = GSD->SimulatorTCPPortU16; + pthread_mutex_unlock(&SimulatorTCPPortMutex); + return READ_OK; } /*END of SimulatorTCPPort*/ @@ -1043,21 +1045,21 @@ ReadWriteAccess_t DataDictionaryGetSimulatorTCPPortU16(GSDType * GSD, U16 * Simu * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitSimulatorUDPPortU16(GSDType * GSD) { - ReadWriteAccess_t Res = UNDEFINED; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + ReadWriteAccess_t Res = UNDEFINED; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + + if (DataDictionarySearchParameter("SimulatorUDPPort=", ResultBufferC8)) { + Res = READ_OK; + pthread_mutex_lock(&SimulatorUDPPortMutex); + GSD->SimulatorUDPPortU16 = atoi(ResultBufferC8); + pthread_mutex_unlock(&SimulatorUDPPortMutex); + } + else { + Res = PARAMETER_NOTFOUND; + LogMessage(LOG_LEVEL_ERROR, "SimulatorUDPPort not found!"); + } - if (DataDictionarySearchParameter("SimulatorUDPPort=", ResultBufferC8)) { - Res = READ_OK; - pthread_mutex_lock(&SimulatorUDPPortMutex); - GSD->SimulatorUDPPortU16 = atoi(ResultBufferC8); - pthread_mutex_unlock(&SimulatorUDPPortMutex); - } - else { - Res = PARAMETER_NOTFOUND; - LogMessage(LOG_LEVEL_ERROR, "SimulatorUDPPort not found!"); - } - - return Res; + return Res; } /*! @@ -1067,17 +1069,17 @@ ReadWriteAccess_t DataDictionaryInitSimulatorUDPPortU16(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetSimulatorUDPPortU16(GSDType * GSD, C8 * SimulatorUDPPort) { - ReadWriteAccess_t Res; + ReadWriteAccess_t Res; - if (UtilWriteConfigurationParameter("SimulatorUDPPort", SimulatorUDPPort, 0)) { - Res = WRITE_OK; - pthread_mutex_lock(&SimulatorUDPPortMutex); - GSD->SimulatorUDPPortU16 = atoi(SimulatorUDPPort); - pthread_mutex_unlock(&SimulatorUDPPortMutex); - } - else - Res = PARAMETER_NOTFOUND; - return Res; + if (UtilWriteConfigurationParameter("SimulatorUDPPort", SimulatorUDPPort, 0)) { + Res = WRITE_OK; + pthread_mutex_lock(&SimulatorUDPPortMutex); + GSD->SimulatorUDPPortU16 = atoi(SimulatorUDPPort); + pthread_mutex_unlock(&SimulatorUDPPortMutex); + } + else + Res = PARAMETER_NOTFOUND; + return Res; } /*! @@ -1087,10 +1089,10 @@ ReadWriteAccess_t DataDictionarySetSimulatorUDPPortU16(GSDType * GSD, C8 * Simul * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetSimulatorUDPPortU16(GSDType * GSD, U16 * SimulatorUDPPort) { - pthread_mutex_lock(&SimulatorUDPPortMutex); - *SimulatorUDPPort = GSD->SimulatorUDPPortU16; - pthread_mutex_unlock(&SimulatorUDPPortMutex); - return READ_OK; + pthread_mutex_lock(&SimulatorUDPPortMutex); + *SimulatorUDPPort = GSD->SimulatorUDPPortU16; + pthread_mutex_unlock(&SimulatorUDPPortMutex); + return READ_OK; } /*END of SimulatorUDPPort*/ @@ -1102,21 +1104,21 @@ ReadWriteAccess_t DataDictionaryGetSimulatorUDPPortU16(GSDType * GSD, U16 * Simu * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitSimulatorModeU8(GSDType * GSD) { - ReadWriteAccess_t Res = UNDEFINED; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; - - if (DataDictionarySearchParameter("SimulatorMode=", ResultBufferC8)) { - Res = READ_OK; - pthread_mutex_lock(&SimulatorModeMutex); - GSD->SimulatorModeU8 = atoi(ResultBufferC8); - pthread_mutex_unlock(&SimulatorModeMutex); - } - else { - Res = PARAMETER_NOTFOUND; - LogMessage(LOG_LEVEL_ERROR, "SimulatorMode not found!"); - } + ReadWriteAccess_t Res = UNDEFINED; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + + if (DataDictionarySearchParameter("SimulatorMode=", ResultBufferC8)) { + Res = READ_OK; + pthread_mutex_lock(&SimulatorModeMutex); + GSD->SimulatorModeU8 = atoi(ResultBufferC8); + pthread_mutex_unlock(&SimulatorModeMutex); + } + else { + Res = PARAMETER_NOTFOUND; + LogMessage(LOG_LEVEL_ERROR, "SimulatorMode not found!"); + } - return Res; + return Res; } /*! @@ -1126,17 +1128,17 @@ ReadWriteAccess_t DataDictionaryInitSimulatorModeU8(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetSimulatorModeU8(GSDType * GSD, C8 * SimulatorMode) { - ReadWriteAccess_t Res; + ReadWriteAccess_t Res; - if (UtilWriteConfigurationParameter("SimulatorMode", SimulatorMode, 0)) { - Res = WRITE_OK; - pthread_mutex_lock(&SimulatorModeMutex); - GSD->SimulatorModeU8 = atoi(SimulatorMode); - pthread_mutex_unlock(&SimulatorModeMutex); - } - else - Res = PARAMETER_NOTFOUND; - return Res; + if (UtilWriteConfigurationParameter("SimulatorMode", SimulatorMode, 0)) { + Res = WRITE_OK; + pthread_mutex_lock(&SimulatorModeMutex); + GSD->SimulatorModeU8 = atoi(SimulatorMode); + pthread_mutex_unlock(&SimulatorModeMutex); + } + else + Res = PARAMETER_NOTFOUND; + return Res; } /*! @@ -1146,10 +1148,10 @@ ReadWriteAccess_t DataDictionarySetSimulatorModeU8(GSDType * GSD, C8 * Simulator * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetSimulatorModeU8(GSDType * GSD, U8 * SimulatorMode) { - pthread_mutex_lock(&SimulatorModeMutex); - *SimulatorMode = GSD->SimulatorModeU8; - pthread_mutex_unlock(&SimulatorModeMutex); - return READ_OK; + pthread_mutex_lock(&SimulatorModeMutex); + *SimulatorMode = GSD->SimulatorModeU8; + pthread_mutex_unlock(&SimulatorModeMutex); + return READ_OK; } /*END of SimulatorMode*/ @@ -1161,21 +1163,21 @@ ReadWriteAccess_t DataDictionaryGetSimulatorModeU8(GSDType * GSD, U8 * Simulator * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitVOILReceiversC8(GSDType * GSD) { - ReadWriteAccess_t Res = UNDEFINED; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_1024]; - - if (DataDictionarySearchParameter("VOILReceivers=", ResultBufferC8)) { - Res = READ_OK; - pthread_mutex_lock(&VOILReceiversMutex); - strcpy(GSD->VOILReceiversC8, ResultBufferC8); - pthread_mutex_unlock(&VOILReceiversMutex); - } - else { - Res = PARAMETER_NOTFOUND; - LogMessage(LOG_LEVEL_ERROR, "VOILReceivers not found!"); - } + ReadWriteAccess_t Res = UNDEFINED; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_1024]; + + if (DataDictionarySearchParameter("VOILReceivers=", ResultBufferC8)) { + Res = READ_OK; + pthread_mutex_lock(&VOILReceiversMutex); + strcpy(GSD->VOILReceiversC8, ResultBufferC8); + pthread_mutex_unlock(&VOILReceiversMutex); + } + else { + Res = PARAMETER_NOTFOUND; + LogMessage(LOG_LEVEL_ERROR, "VOILReceivers not found!"); + } - return Res; + return Res; } /*! @@ -1185,17 +1187,17 @@ ReadWriteAccess_t DataDictionaryInitVOILReceiversC8(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetVOILReceiversC8(GSDType * GSD, C8 * VOILReceivers) { - ReadWriteAccess_t Res; + ReadWriteAccess_t Res; - if (UtilWriteConfigurationParameter("VOILReceivers", VOILReceivers, 0)) { - Res = WRITE_OK; - pthread_mutex_lock(&VOILReceiversMutex); - strcpy(GSD->VOILReceiversC8, VOILReceivers); - pthread_mutex_unlock(&VOILReceiversMutex); - } - else - Res = PARAMETER_NOTFOUND; - return Res; + if (UtilWriteConfigurationParameter("VOILReceivers", VOILReceivers, 0)) { + Res = WRITE_OK; + pthread_mutex_lock(&VOILReceiversMutex); + strcpy(GSD->VOILReceiversC8, VOILReceivers); + pthread_mutex_unlock(&VOILReceiversMutex); + } + else + Res = PARAMETER_NOTFOUND; + return Res; } /*! @@ -1205,11 +1207,11 @@ ReadWriteAccess_t DataDictionarySetVOILReceiversC8(GSDType * GSD, C8 * VOILRecei * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetVOILReceiversC8(GSDType * GSD, U8 * VOILReceivers, U32 BuffLen) { - pthread_mutex_lock(&VOILReceiversMutex); - bzero(VOILReceivers, BuffLen); - strcpy(VOILReceivers, GSD->VOILReceiversC8); - pthread_mutex_unlock(&VOILReceiversMutex); - return READ_OK; + pthread_mutex_lock(&VOILReceiversMutex); + bzero(VOILReceivers, BuffLen); + strcpy(VOILReceivers, GSD->VOILReceiversC8); + pthread_mutex_unlock(&VOILReceiversMutex); + return READ_OK; } /*END of VOILReceivers*/ @@ -1221,21 +1223,21 @@ ReadWriteAccess_t DataDictionaryGetVOILReceiversC8(GSDType * GSD, U8 * VOILRecei * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitDTMReceiversC8(GSDType * GSD) { - ReadWriteAccess_t Res = UNDEFINED; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_1024]; - - if (DataDictionarySearchParameter("DTMReceivers=", ResultBufferC8)) { - Res = READ_OK; - pthread_mutex_lock(&DTMReceiversMutex); - strcpy(GSD->DTMReceiversC8, ResultBufferC8); - pthread_mutex_unlock(&DTMReceiversMutex); - } - else { - Res = PARAMETER_NOTFOUND; - LogMessage(LOG_LEVEL_ERROR, "DTMReceivers not found!"); - } + ReadWriteAccess_t Res = UNDEFINED; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_1024]; + + if (DataDictionarySearchParameter("DTMReceivers=", ResultBufferC8)) { + Res = READ_OK; + pthread_mutex_lock(&DTMReceiversMutex); + strcpy(GSD->DTMReceiversC8, ResultBufferC8); + pthread_mutex_unlock(&DTMReceiversMutex); + } + else { + Res = PARAMETER_NOTFOUND; + LogMessage(LOG_LEVEL_ERROR, "DTMReceivers not found!"); + } - return Res; + return Res; } /*! @@ -1245,17 +1247,17 @@ ReadWriteAccess_t DataDictionaryInitDTMReceiversC8(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetDTMReceiversC8(GSDType * GSD, C8 * DTMReceivers) { - ReadWriteAccess_t Res; + ReadWriteAccess_t Res; - if (UtilWriteConfigurationParameter("DTMReceivers", DTMReceivers, 0)) { - Res = WRITE_OK; - pthread_mutex_lock(&DTMReceiversMutex); - strcpy(GSD->DTMReceiversC8, DTMReceivers); - pthread_mutex_unlock(&DTMReceiversMutex); - } - else - Res = PARAMETER_NOTFOUND; - return Res; + if (UtilWriteConfigurationParameter("DTMReceivers", DTMReceivers, 0)) { + Res = WRITE_OK; + pthread_mutex_lock(&DTMReceiversMutex); + strcpy(GSD->DTMReceiversC8, DTMReceivers); + pthread_mutex_unlock(&DTMReceiversMutex); + } + else + Res = PARAMETER_NOTFOUND; + return Res; } /*! @@ -1265,11 +1267,11 @@ ReadWriteAccess_t DataDictionarySetDTMReceiversC8(GSDType * GSD, C8 * DTMReceive * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetDTMReceiversC8(GSDType * GSD, U8 * DTMReceivers, U32 BuffLen) { - pthread_mutex_lock(&DTMReceiversMutex); - bzero(DTMReceivers, BuffLen); - strcpy(DTMReceivers, GSD->DTMReceiversC8); - pthread_mutex_unlock(&DTMReceiversMutex); - return READ_OK; + pthread_mutex_lock(&DTMReceiversMutex); + bzero(DTMReceivers, BuffLen); + strcpy(DTMReceivers, GSD->DTMReceiversC8); + pthread_mutex_unlock(&DTMReceiversMutex); + return READ_OK; } /*END of DTMReceivers*/ @@ -1281,24 +1283,24 @@ ReadWriteAccess_t DataDictionaryGetDTMReceiversC8(GSDType * GSD, U8 * DTMReceive * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitExternalSupervisorIPU32(GSDType * GSD) { - ReadWriteAccess_t Res = UNDEFINED; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + ReadWriteAccess_t Res = UNDEFINED; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + + if (DataDictionarySearchParameter("SupervisorIP=", ResultBufferC8)) { + Res = READ_OK; + pthread_mutex_lock(&ExternalSupervisorIPMutex); + GSD->ExternalSupervisorIPU32 = UtilIPStringToInt(ResultBufferC8); + bzero(GSD->ExternalSupervisorIPC8, DD_CONTROL_BUFFER_SIZE_20); + strcat(GSD->ExternalSupervisorIPC8, ResultBufferC8); + pthread_mutex_unlock(&ExternalSupervisorIPMutex); + //LogMessage(LOG_LEVEL_ERROR,"Supervisor IP: %s", ResultBufferC8); + } + else { + Res = PARAMETER_NOTFOUND; + LogMessage(LOG_LEVEL_ERROR, "Supervisor IP not found!"); + } - if (DataDictionarySearchParameter("SupervisorIP=", ResultBufferC8)) { - Res = READ_OK; - pthread_mutex_lock(&ExternalSupervisorIPMutex); - GSD->ExternalSupervisorIPU32 = UtilIPStringToInt(ResultBufferC8); - bzero(GSD->ExternalSupervisorIPC8, DD_CONTROL_BUFFER_SIZE_20); - strcat(GSD->ExternalSupervisorIPC8, ResultBufferC8); - pthread_mutex_unlock(&ExternalSupervisorIPMutex); - //LogMessage(LOG_LEVEL_ERROR,"Supervisor IP: %s", ResultBufferC8); - } - else { - Res = PARAMETER_NOTFOUND; - LogMessage(LOG_LEVEL_ERROR, "Supervisor IP not found!"); - } - - return Res; + return Res; } /*! @@ -1308,19 +1310,19 @@ ReadWriteAccess_t DataDictionaryInitExternalSupervisorIPU32(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetExternalSupervisorIPU32(GSDType * GSD, C8 * IP) { - ReadWriteAccess_t Res; + ReadWriteAccess_t Res; - if (UtilWriteConfigurationParameter("SupervisorIP", IP, 0)) { - Res = WRITE_OK; - pthread_mutex_lock(&ExternalSupervisorIPMutex); - GSD->ExternalSupervisorIPU32 = UtilIPStringToInt(IP); - bzero(GSD->ExternalSupervisorIPC8, DD_CONTROL_BUFFER_SIZE_20); - strcat(GSD->ExternalSupervisorIPC8, IP); - pthread_mutex_unlock(&ExternalSupervisorIPMutex); - } - else - Res = PARAMETER_NOTFOUND; - return Res; + if (UtilWriteConfigurationParameter("SupervisorIP", IP, 0)) { + Res = WRITE_OK; + pthread_mutex_lock(&ExternalSupervisorIPMutex); + GSD->ExternalSupervisorIPU32 = UtilIPStringToInt(IP); + bzero(GSD->ExternalSupervisorIPC8, DD_CONTROL_BUFFER_SIZE_20); + strcat(GSD->ExternalSupervisorIPC8, IP); + pthread_mutex_unlock(&ExternalSupervisorIPMutex); + } + else + Res = PARAMETER_NOTFOUND; + return Res; } /*! @@ -1330,10 +1332,10 @@ ReadWriteAccess_t DataDictionarySetExternalSupervisorIPU32(GSDType * GSD, C8 * I * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetExternalSupervisorIPU32(GSDType * GSD, U32 * IP) { - pthread_mutex_lock(&ExternalSupervisorIPMutex); - *IP = GSD->ExternalSupervisorIPU32; - pthread_mutex_unlock(&ExternalSupervisorIPMutex); - return READ_OK; + pthread_mutex_lock(&ExternalSupervisorIPMutex); + *IP = GSD->ExternalSupervisorIPU32; + pthread_mutex_unlock(&ExternalSupervisorIPMutex); + return READ_OK; } /*! @@ -1343,11 +1345,11 @@ ReadWriteAccess_t DataDictionaryGetExternalSupervisorIPU32(GSDType * GSD, U32 * * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetExternalSupervisorIPC8(GSDType * GSD, C8 * IP, U32 BuffLen) { - pthread_mutex_lock(&ExternalSupervisorIPMutex); - bzero(IP, BuffLen); - strcat(IP, GSD->ExternalSupervisorIPC8); - pthread_mutex_unlock(&ExternalSupervisorIPMutex); - return READ_OK; + pthread_mutex_lock(&ExternalSupervisorIPMutex); + bzero(IP, BuffLen); + strcat(IP, GSD->ExternalSupervisorIPC8); + pthread_mutex_unlock(&ExternalSupervisorIPMutex); + return READ_OK; } /*END of External Supervisor IP*/ @@ -1359,21 +1361,21 @@ ReadWriteAccess_t DataDictionaryGetExternalSupervisorIPC8(GSDType * GSD, C8 * IP * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitSupervisorTCPPortU16(GSDType * GSD) { - ReadWriteAccess_t Res = UNDEFINED; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; - - if (DataDictionarySearchParameter("SupervisorTCPPort=", ResultBufferC8)) { - Res = READ_OK; - pthread_mutex_lock(&SupervisorTCPPortMutex); - GSD->SupervisorTCPPortU16 = atoi(ResultBufferC8); - pthread_mutex_unlock(&SupervisorTCPPortMutex); - } - else { - Res = PARAMETER_NOTFOUND; - LogMessage(LOG_LEVEL_ERROR, "SupervisorTCPPort not found!"); - } + ReadWriteAccess_t Res = UNDEFINED; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + + if (DataDictionarySearchParameter("SupervisorTCPPort=", ResultBufferC8)) { + Res = READ_OK; + pthread_mutex_lock(&SupervisorTCPPortMutex); + GSD->SupervisorTCPPortU16 = atoi(ResultBufferC8); + pthread_mutex_unlock(&SupervisorTCPPortMutex); + } + else { + Res = PARAMETER_NOTFOUND; + LogMessage(LOG_LEVEL_ERROR, "SupervisorTCPPort not found!"); + } - return Res; + return Res; } /*! @@ -1383,17 +1385,17 @@ ReadWriteAccess_t DataDictionaryInitSupervisorTCPPortU16(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetSupervisorTCPPortU16(GSDType * GSD, C8 * SupervisorTCPPort) { - ReadWriteAccess_t Res; + ReadWriteAccess_t Res; - if (UtilWriteConfigurationParameter("SupervisorTCPPort", SupervisorTCPPort, 0)) { - Res = WRITE_OK; - pthread_mutex_lock(&SupervisorTCPPortMutex); - GSD->SupervisorTCPPortU16 = atoi(SupervisorTCPPort); - pthread_mutex_unlock(&SupervisorTCPPortMutex); - } - else - Res = PARAMETER_NOTFOUND; - return Res; + if (UtilWriteConfigurationParameter("SupervisorTCPPort", SupervisorTCPPort, 0)) { + Res = WRITE_OK; + pthread_mutex_lock(&SupervisorTCPPortMutex); + GSD->SupervisorTCPPortU16 = atoi(SupervisorTCPPort); + pthread_mutex_unlock(&SupervisorTCPPortMutex); + } + else + Res = PARAMETER_NOTFOUND; + return Res; } /*! @@ -1403,10 +1405,10 @@ ReadWriteAccess_t DataDictionarySetSupervisorTCPPortU16(GSDType * GSD, C8 * Supe * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetSupervisorTCPPortU16(GSDType * GSD, U16 * SupervisorTCPPort) { - pthread_mutex_lock(&SupervisorTCPPortMutex); - *SupervisorTCPPort = GSD->SupervisorTCPPortU16; - pthread_mutex_unlock(&SupervisorTCPPortMutex); - return READ_OK; + pthread_mutex_lock(&SupervisorTCPPortMutex); + *SupervisorTCPPort = GSD->SupervisorTCPPortU16; + pthread_mutex_unlock(&SupervisorTCPPortMutex); + return READ_OK; } /*END of External SupervisorTCPPort*/ @@ -1418,22 +1420,22 @@ ReadWriteAccess_t DataDictionaryGetSupervisorTCPPortU16(GSDType * GSD, U16 * Sup * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitRVSSConfigU32(GSDType * GSD) { - ReadWriteAccess_t Res = UNDEFINED; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + ReadWriteAccess_t Res = UNDEFINED; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + + if (DataDictionarySearchParameter("RVSSConfig=", ResultBufferC8)) { + Res = READ_OK; + pthread_mutex_lock(&DataDictionaryRVSSConfigMutex); + GSD->DataDictionaryRVSSConfigU32 = atoi(ResultBufferC8); + pthread_mutex_unlock(&DataDictionaryRVSSConfigMutex); + //LogMessage(LOG_LEVEL_ERROR,"RVSSConfig: %s", ResultBufferC8); + } + else { + Res = PARAMETER_NOTFOUND; + LogMessage(LOG_LEVEL_ERROR, "RVSSConfig not found!"); + } - if (DataDictionarySearchParameter("RVSSConfig=", ResultBufferC8)) { - Res = READ_OK; - pthread_mutex_lock(&DataDictionaryRVSSConfigMutex); - GSD->DataDictionaryRVSSConfigU32 = atoi(ResultBufferC8); - pthread_mutex_unlock(&DataDictionaryRVSSConfigMutex); - //LogMessage(LOG_LEVEL_ERROR,"RVSSConfig: %s", ResultBufferC8); - } - else { - Res = PARAMETER_NOTFOUND; - LogMessage(LOG_LEVEL_ERROR, "RVSSConfig not found!"); - } - - return Res; + return Res; } /*! @@ -1443,21 +1445,21 @@ ReadWriteAccess_t DataDictionaryInitRVSSConfigU32(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetRVSSConfigU32(GSDType * GSD, U32 RVSSConfig) { - ReadWriteAccess_t Res; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; - - bzero(ResultBufferC8, DD_CONTROL_BUFFER_SIZE_20); - sprintf(ResultBufferC8, "%" PRIu32, RVSSConfig); - - if (UtilWriteConfigurationParameter("RVSSConfig", ResultBufferC8, 0)) { - Res = WRITE_OK; - pthread_mutex_lock(&DataDictionaryRVSSConfigMutex); - GSD->DataDictionaryRVSSConfigU32 = RVSSConfig; - pthread_mutex_unlock(&DataDictionaryRVSSConfigMutex); - } - else - Res = PARAMETER_NOTFOUND; - return Res; + ReadWriteAccess_t Res; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + + bzero(ResultBufferC8, DD_CONTROL_BUFFER_SIZE_20); + sprintf(ResultBufferC8, "%" PRIu32, RVSSConfig); + + if (UtilWriteConfigurationParameter("RVSSConfig", ResultBufferC8, 0)) { + Res = WRITE_OK; + pthread_mutex_lock(&DataDictionaryRVSSConfigMutex); + GSD->DataDictionaryRVSSConfigU32 = RVSSConfig; + pthread_mutex_unlock(&DataDictionaryRVSSConfigMutex); + } + else + Res = PARAMETER_NOTFOUND; + return Res; } /*! @@ -1467,10 +1469,10 @@ ReadWriteAccess_t DataDictionarySetRVSSConfigU32(GSDType * GSD, U32 RVSSConfig) * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetRVSSConfigU32(GSDType * GSD, U32 * RVSSConfig) { - pthread_mutex_lock(&DataDictionaryRVSSConfigMutex); - *RVSSConfig = GSD->DataDictionaryRVSSConfigU32; - pthread_mutex_unlock(&DataDictionaryRVSSConfigMutex); - return READ_OK; + pthread_mutex_lock(&DataDictionaryRVSSConfigMutex); + *RVSSConfig = GSD->DataDictionaryRVSSConfigU32; + pthread_mutex_unlock(&DataDictionaryRVSSConfigMutex); + return READ_OK; } /*END of Runtime Variable Subscription Service (RVSS) Configuration**/ @@ -1483,22 +1485,22 @@ ReadWriteAccess_t DataDictionaryGetRVSSConfigU32(GSDType * GSD, U32 * RVSSConfig * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitRVSSRateU8(GSDType * GSD) { - ReadWriteAccess_t Res = UNDEFINED; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; - - if (DataDictionarySearchParameter("RVSSRate=", ResultBufferC8)) { - Res = READ_OK; - pthread_mutex_lock(&DataDictionaryRVSSRateMutex); - GSD->DataDictionaryRVSSRateU8 = (U8) atoi(ResultBufferC8); - pthread_mutex_unlock(&DataDictionaryRVSSRateMutex); - //LogMessage(LOG_LEVEL_ERROR,"RVSSRate: %s", ResultBufferC8); - } - else { - Res = PARAMETER_NOTFOUND; - LogMessage(LOG_LEVEL_ERROR, "RVSSRate not found!"); - } + ReadWriteAccess_t Res = UNDEFINED; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + + if (DataDictionarySearchParameter("RVSSRate=", ResultBufferC8)) { + Res = READ_OK; + pthread_mutex_lock(&DataDictionaryRVSSRateMutex); + GSD->DataDictionaryRVSSRateU8 = (U8) atoi(ResultBufferC8); + pthread_mutex_unlock(&DataDictionaryRVSSRateMutex); + //LogMessage(LOG_LEVEL_ERROR,"RVSSRate: %s", ResultBufferC8); + } + else { + Res = PARAMETER_NOTFOUND; + LogMessage(LOG_LEVEL_ERROR, "RVSSRate not found!"); + } - return Res; + return Res; } /*! @@ -1508,21 +1510,21 @@ ReadWriteAccess_t DataDictionaryInitRVSSRateU8(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetRVSSRateU8(GSDType * GSD, U8 RVSSRate) { - ReadWriteAccess_t Res; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; - - bzero(ResultBufferC8, DD_CONTROL_BUFFER_SIZE_20); - sprintf(ResultBufferC8, "%" PRIu8, RVSSRate); - - if (UtilWriteConfigurationParameter("RVSSRate", ResultBufferC8, 0)) { - Res = WRITE_OK; - pthread_mutex_lock(&DataDictionaryRVSSRateMutex); - GSD->DataDictionaryRVSSRateU8 = RVSSRate; - pthread_mutex_unlock(&DataDictionaryRVSSRateMutex); - } - else - Res = PARAMETER_NOTFOUND; - return Res; + ReadWriteAccess_t Res; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + + bzero(ResultBufferC8, DD_CONTROL_BUFFER_SIZE_20); + sprintf(ResultBufferC8, "%" PRIu8, RVSSRate); + + if (UtilWriteConfigurationParameter("RVSSRate", ResultBufferC8, 0)) { + Res = WRITE_OK; + pthread_mutex_lock(&DataDictionaryRVSSRateMutex); + GSD->DataDictionaryRVSSRateU8 = RVSSRate; + pthread_mutex_unlock(&DataDictionaryRVSSRateMutex); + } + else + Res = PARAMETER_NOTFOUND; + return Res; } /*! @@ -1532,10 +1534,10 @@ ReadWriteAccess_t DataDictionarySetRVSSRateU8(GSDType * GSD, U8 RVSSRate) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetRVSSRateU8(GSDType * GSD, U8 * RVSSRate) { - pthread_mutex_lock(&DataDictionaryRVSSRateMutex); - *RVSSRate = GSD->DataDictionaryRVSSRateU8; - pthread_mutex_unlock(&DataDictionaryRVSSRateMutex); - return READ_OK; + pthread_mutex_lock(&DataDictionaryRVSSRateMutex); + *RVSSRate = GSD->DataDictionaryRVSSRateU8; + pthread_mutex_unlock(&DataDictionaryRVSSRateMutex); + return READ_OK; } /*END of Runtime Variable Subscription Service (RVSS) Rate**/ @@ -1549,10 +1551,10 @@ ReadWriteAccess_t DataDictionaryGetRVSSRateU8(GSDType * GSD, U8 * RVSSRate) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetRVSSAsp(GSDType * GSD, ASPType * ASPD) { - pthread_mutex_lock(&ASPDataMutex); - GSD->ASPData = *ASPD; - pthread_mutex_unlock(&ASPDataMutex); - return WRITE_OK; + pthread_mutex_lock(&ASPDataMutex); + GSD->ASPData = *ASPD; + pthread_mutex_unlock(&ASPDataMutex); + return WRITE_OK; } /*! @@ -1562,10 +1564,10 @@ ReadWriteAccess_t DataDictionarySetRVSSAsp(GSDType * GSD, ASPType * ASPD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetRVSSAsp(GSDType * GSD, ASPType * ASPD) { - pthread_mutex_lock(&ASPDataMutex); - *ASPD = GSD->ASPData; - pthread_mutex_unlock(&ASPDataMutex); - return READ_OK; + pthread_mutex_lock(&ASPDataMutex); + *ASPD = GSD->ASPData; + pthread_mutex_unlock(&ASPDataMutex); + return READ_OK; } /*END ASPDebug*/ @@ -1577,21 +1579,21 @@ ReadWriteAccess_t DataDictionaryGetRVSSAsp(GSDType * GSD, ASPType * ASPD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitMiscDataC8(GSDType * GSD) { - ReadWriteAccess_t Res = UNDEFINED; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; - - if (DataDictionarySearchParameter("MiscData=", ResultBufferC8)) { - Res = READ_OK; - pthread_mutex_lock(&MiscDataMutex); - strcpy(GSD->MiscDataC8, ResultBufferC8); - pthread_mutex_unlock(&MiscDataMutex); - } - else { - Res = PARAMETER_NOTFOUND; - LogMessage(LOG_LEVEL_ERROR, "MiscData not found!"); - } + ReadWriteAccess_t Res = UNDEFINED; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + + if (DataDictionarySearchParameter("MiscData=", ResultBufferC8)) { + Res = READ_OK; + pthread_mutex_lock(&MiscDataMutex); + strcpy(GSD->MiscDataC8, ResultBufferC8); + pthread_mutex_unlock(&MiscDataMutex); + } + else { + Res = PARAMETER_NOTFOUND; + LogMessage(LOG_LEVEL_ERROR, "MiscData not found!"); + } - return Res; + return Res; } /*! @@ -1601,17 +1603,17 @@ ReadWriteAccess_t DataDictionaryInitMiscDataC8(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetMiscDataC8(GSDType * GSD, C8 * MiscData) { - ReadWriteAccess_t Res; - if (UtilWriteConfigurationParameter("MiscData", MiscData, 0)) { - Res = WRITE_OK; - pthread_mutex_lock(&MiscDataMutex); - bzero(GSD->MiscDataC8, DD_CONTROL_BUFFER_SIZE_1024); - strcpy(GSD->MiscDataC8, MiscData); - pthread_mutex_unlock(&MiscDataMutex); - } - else - Res = PARAMETER_NOTFOUND; - return Res; + ReadWriteAccess_t Res; + if (UtilWriteConfigurationParameter("MiscData", MiscData, 0)) { + Res = WRITE_OK; + pthread_mutex_lock(&MiscDataMutex); + bzero(GSD->MiscDataC8, DD_CONTROL_BUFFER_SIZE_1024); + strcpy(GSD->MiscDataC8, MiscData); + pthread_mutex_unlock(&MiscDataMutex); + } + else + Res = PARAMETER_NOTFOUND; + return Res; } /*! @@ -1621,11 +1623,11 @@ ReadWriteAccess_t DataDictionarySetMiscDataC8(GSDType * GSD, C8 * MiscData) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetMiscDataC8(GSDType * GSD, U8 * MiscData, U32 BuffLen) { - pthread_mutex_lock(&MiscDataMutex); - bzero(MiscData, BuffLen); - strcpy(MiscData, GSD->MiscDataC8); - pthread_mutex_unlock(&MiscDataMutex); - return READ_OK; + pthread_mutex_lock(&MiscDataMutex); + bzero(MiscData, BuffLen); + strcpy(MiscData, GSD->MiscDataC8); + pthread_mutex_unlock(&MiscDataMutex); + return READ_OK; } /*END of MiscData*/ @@ -1639,12 +1641,12 @@ ReadWriteAccess_t DataDictionaryGetMiscDataC8(GSDType * GSD, U8 * MiscData, U32 * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetOBCStateU8(GSDType * GSD, OBCState_t OBCState) { - ReadWriteAccess_t Res; - Res = WRITE_OK; - pthread_mutex_lock(&OBCStateMutex); - GSD->OBCStateU8 = OBCState; - pthread_mutex_unlock(&OBCStateMutex); - return Res; + ReadWriteAccess_t Res; + Res = WRITE_OK; + pthread_mutex_lock(&OBCStateMutex); + GSD->OBCStateU8 = OBCState; + pthread_mutex_unlock(&OBCStateMutex); + return Res; } /*! @@ -1653,11 +1655,11 @@ ReadWriteAccess_t DataDictionarySetOBCStateU8(GSDType * GSD, OBCState_t OBCState * \return Current object control state according to ::OBCState_t */ OBCState_t DataDictionaryGetOBCStateU8(GSDType * GSD) { - OBCState_t Ret; - pthread_mutex_lock(&OBCStateMutex); - Ret = GSD->OBCStateU8; - pthread_mutex_unlock(&OBCStateMutex); - return Ret; + OBCState_t Ret; + pthread_mutex_lock(&OBCStateMutex); + Ret = GSD->OBCStateU8; + pthread_mutex_unlock(&OBCStateMutex); + return Ret; } /*END OBCState*/ @@ -1671,29 +1673,36 @@ OBCState_t DataDictionaryGetOBCStateU8(GSDType * GSD) { ReadWriteAccess_t DataDictionaryInitMONR(GSDType * GSD, U8 objectCount){ ReadWriteAccess_t Res; Res = WRITE_OK; - pthread_mutex_lock(&MONRMutex); - GSD->MonrMessages = malloc(sizeof(MONRType) * objectCount); - //GSD->MonrMessages = ptr; - pthread_mutex_unlock(&MONRMutex); - return Res; -} + char filePath[PATH_MAX]; + int fd; + struct stat st; -/*! - * \brief DataDictionaryInitMONR inits a data structure for saving object monr - * \param GSD Pointer to shared allocated memory - * \param objects number of objects that will transmitt monr - * \return Result according to ::ReadWriteAccess_t - */ -ReadWriteAccess_t DataDictionaryFreeMONR(GSDType * GSD){ - ReadWriteAccess_t Res; - Res = WRITE_OK; pthread_mutex_lock(&MONRMutex); - free(GSD->MonrMessages); + sprintf(filePath, "%stest%d.mem", SHARED_MEMORY_PATH, 5); + fd = open(filePath, O_RDWR | O_CREAT, S_IRUSR | S_IWUSR); + stat(filePath, &st); + + printf("File size on open: %ld\n", st.st_size); + + lseek(fd, 6 - 1, SEEK_SET); + write(fd, "", 1); + + stat(filePath, &st); + printf("File size after writing: %ld\n", st.st_size); + + // Map memory to created file + GSD->memory = (char *) mmap(NULL, 6, PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0); + + GSD->memory[0] = 'A'; + + LogPrint("Initing %c", GSD->memory[0]); + + //munmap(memory, 6); + pthread_mutex_unlock(&MONRMutex); return Res; } - /*! * \brief DataDictionarySetMONR Parses input variable and sets variable to corresponding value * \param GSD Pointer to shared allocated memory @@ -1705,8 +1714,12 @@ ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, MONRType * MONR, U8 id) { ReadWriteAccess_t Res; Res = WRITE_OK; pthread_mutex_lock(&MONRMutex); - GSD->MonrMessages[id].HeadingU16 = MONR->HeadingU16; - LogPrint("Setting %d for %d in ptr %p", GSD->MonrMessages[id].HeadingU16, id, GSD->MonrMessages); + + //GSD->MonrMessages[id].HeadingU16 = MONR->HeadingU16; + + GSD->memory[0] = 'A' + (random() % 26); + LogPrint("Setting %c", GSD->memory[0]); + pthread_mutex_unlock(&MONRMutex); return Res; } @@ -1719,12 +1732,41 @@ ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, MONRType * MONR, U8 id) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetMONR(GSDType * GSD, MONRType *MONR, U8 id) { + struct stat st; + char filePath[PATH_MAX]; + int fd; + pthread_mutex_lock(&MONRMutex); - LogPrint("Getting %d for %d in ptr %p", GSD->MonrMessages[id].HeadingU16, id, GSD->MonrMessages); + sprintf(filePath, "%stest%d.mem", SHARED_MEMORY_PATH, 5); + fd = open(filePath, O_RDWR | O_CREAT, S_IRUSR | S_IWUSR); + stat(filePath, &st); + lseek(fd, 6 - 1, SEEK_SET); + write(fd, "", 1); + stat(filePath, &st); + + GSD->memory = (char *) mmap(NULL, 6, PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0); + + //GSD->memory = (char *) mmap(NULL, 6, PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0); + LogPrint("Getting %c", GSD->memory[0]); pthread_mutex_unlock(&MONRMutex); return READ_OK; } +/*! + * \brief DataDictionaryInitMONR inits a data structure for saving object monr + * \param GSD Pointer to shared allocated memory + * \param objects number of objects that will transmitt monr + * \return Result according to ::ReadWriteAccess_t + */ +ReadWriteAccess_t DataDictionaryFreeMONR(GSDType * GSD){ + ReadWriteAccess_t Res; + Res = WRITE_OK; + pthread_mutex_lock(&MONRMutex); + free(GSD->MonrMessages); + pthread_mutex_unlock(&MONRMutex); + return Res; +} + /*END of MONR*/ @@ -1767,11 +1809,11 @@ the parameter value. * \return Length of read parameter string */ U64 DataDictionarySearchParameter(C8 * ParameterName, C8 * ResultBuffer) { - char confPathDir[MAX_FILE_PATH]; + char confPathDir[MAX_FILE_PATH]; - UtilGetConfDirectoryPath(confPathDir, sizeof (confPathDir)); - strcat(confPathDir, CONF_FILE_NAME); - bzero(ResultBuffer, DD_CONTROL_BUFFER_SIZE_20); - UtilSearchTextFile(confPathDir, ParameterName, "", ResultBuffer); - return strlen(ResultBuffer); + UtilGetConfDirectoryPath(confPathDir, sizeof (confPathDir)); + strcat(confPathDir, CONF_FILE_NAME); + bzero(ResultBuffer, DD_CONTROL_BUFFER_SIZE_20); + UtilSearchTextFile(confPathDir, ParameterName, "", ResultBuffer); + return strlen(ResultBuffer); } diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 772a2bf16..30ac48638 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -486,10 +486,6 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { ObjectControlBuildMONRMessage(buffer, &MONRData, 0); - LogPrint("****** BEFORE SETTING ******"); - DataDictionaryGetMONR(GSD, &MONRData, iIndex); - LogPrint("****************"); - //Store MONR in GSD DataDictionarySetMONR(GSD, &MONRData, iIndex); diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index 0b901627e..7ea41557b 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -572,7 +572,6 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { RVSSMessageLengthU32, 0); }*/ DataDictionaryGetMONR(GSD, &monrData.MONR, 0); - DataDictionaryGetMONR(GSD, &monrData.MONR, 1); SystemControlBuildRVSSMONRChannelMessage(RVSSData, &RVSSMessageLengthU32, monrData, 0); UtilSendUDPData("SystemControl", &RVSSChannelSocket, &RVSSChannelAddr, RVSSData, From 70a605f00440e311846d94382997d192e7f6afab Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Thu, 23 Jan 2020 15:57:14 +0100 Subject: [PATCH 118/523] Sending monr --- server/src/datadictionary.c | 21 ++++++++++----------- 1 file changed, 10 insertions(+), 11 deletions(-) diff --git a/server/src/datadictionary.c b/server/src/datadictionary.c index d2d9e3c3f..d35a38bfb 100644 --- a/server/src/datadictionary.c +++ b/server/src/datadictionary.c @@ -1684,18 +1684,17 @@ ReadWriteAccess_t DataDictionaryInitMONR(GSDType * GSD, U8 objectCount){ printf("File size on open: %ld\n", st.st_size); - lseek(fd, 6 - 1, SEEK_SET); + lseek(fd, (sizeof (MONRType) * objectCount) - 1, SEEK_SET); write(fd, "", 1); stat(filePath, &st); printf("File size after writing: %ld\n", st.st_size); // Map memory to created file - GSD->memory = (char *) mmap(NULL, 6, PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0); + GSD->MonrMessages = (MONRType *) mmap(NULL, (sizeof (MONRType) * objectCount), PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0); - GSD->memory[0] = 'A'; - LogPrint("Initing %c", GSD->memory[0]); + LogPrint("Initing"); //munmap(memory, 6); @@ -1717,8 +1716,8 @@ ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, MONRType * MONR, U8 id) { //GSD->MonrMessages[id].HeadingU16 = MONR->HeadingU16; - GSD->memory[0] = 'A' + (random() % 26); - LogPrint("Setting %c", GSD->memory[0]); + GSD->MonrMessages[id] = *MONR; + LogPrint("Setting %d", GSD->MonrMessages[id].HeadingU16); pthread_mutex_unlock(&MONRMutex); return Res; @@ -1740,14 +1739,14 @@ ReadWriteAccess_t DataDictionaryGetMONR(GSDType * GSD, MONRType *MONR, U8 id) { sprintf(filePath, "%stest%d.mem", SHARED_MEMORY_PATH, 5); fd = open(filePath, O_RDWR | O_CREAT, S_IRUSR | S_IWUSR); stat(filePath, &st); - lseek(fd, 6 - 1, SEEK_SET); - write(fd, "", 1); - stat(filePath, &st); - GSD->memory = (char *) mmap(NULL, 6, PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0); + // Map memory to created file + GSD->MonrMessages = (MONRType *) mmap(NULL, sizeof (MONRType), PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0); + + *MONR = GSD->MonrMessages[id]; //GSD->memory = (char *) mmap(NULL, 6, PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0); - LogPrint("Getting %c", GSD->memory[0]); + LogPrint("Getting heading: %d for object[%d]", MONR->HeadingU16, id); pthread_mutex_unlock(&MONRMutex); return READ_OK; } From fd65139fb9719051c42eb816520c5904358e633a Mon Sep 17 00:00:00 2001 From: alfaro01 Date: Thu, 23 Jan 2020 17:03:54 +0100 Subject: [PATCH 119/523] Added MQDisconnect and fixed bufferSizeBug --- modules/Visualization/src/main.c | 89 ++++++++++++++++++++++---------- 1 file changed, 61 insertions(+), 28 deletions(-) diff --git a/modules/Visualization/src/main.c b/modules/Visualization/src/main.c index 1c909a788..2418da0e0 100644 --- a/modules/Visualization/src/main.c +++ b/modules/Visualization/src/main.c @@ -8,7 +8,7 @@ #include #include #include - +#include #include "logging.h" #include "util.h" @@ -21,6 +21,7 @@ #define VISUAL_SERVER_PORT 53250 #define VISUAL_CONTROL_MODE 0 #define VISUAL_REPLAY_MODE 1 +#define ENOUGH_BUFFER_SIZE 64 #define SMALL_ITEM_TEXT_BUFFER_SIZE 20 #define MAX_DATE_STRLEN 25 // Maximum string length of a time stamp on the format "2035;12;31;24;59;59;1000" is 25 @@ -33,35 +34,34 @@ static void vConnectVisualizationChannel(int *sockfd, struct sockaddr_in *addr); static void vDisconnectVisualizationChannel(int *sockfd); void vCreateVisualizationMessage(MonitorDataType * _monitorData, char *_visualizationMessage, int _sizeOfVisualizationMessage, int _debug); +static void signalHandler(int signo); + +I32 iExit = 0; void vCreateVisualizationMessage(MonitorDataType * _monitorData, char *_visualizationMessage, int _sizeOfVisualizationMessage, int _debug) { - //IP char ipStringBuffer[INET_ADDRSTRLEN]; - - char GPSMsOfWeekString[4]; - printf(GPSMsOfWeekString, "%u", _monitorData->MONR.GPSQmsOfWeekU32); - char xPosString[4]; - printf(xPosString, "%d", _monitorData->MONR.XPositionI32); - char yPosString[4]; - printf(yPosString, "%d", _monitorData->MONR.YPositionI32); - char zPosString[4]; - printf(zPosString, "%d", _monitorData->MONR.ZPositionI32); - char headingString[2]; - printf(headingString, "%u", _monitorData->MONR.HeadingU16); - char longSpeedString[2]; - printf(longSpeedString, "%d", _monitorData->MONR.LongitudinalSpeedI16); - char stateString[1]; - printf(stateString, "%u", _monitorData->MONR.StateU8); + sprintf(ipStringBuffer, "%s", + inet_ntop(AF_INET, &_monitorData->ClientIP, ipStringBuffer, sizeof (ipStringBuffer))); + char GPSMsOfWeekString[ENOUGH_BUFFER_SIZE]; + sprintf(GPSMsOfWeekString, "%u", _monitorData->MONR.GPSQmsOfWeekU32); + char xPosString[ENOUGH_BUFFER_SIZE]; + sprintf(xPosString, "%d", _monitorData->MONR.XPositionI32); + char yPosString[ENOUGH_BUFFER_SIZE]; + sprintf(yPosString, "%d", _monitorData->MONR.YPositionI32); + char zPosString[ENOUGH_BUFFER_SIZE]; + sprintf(zPosString, "%d", _monitorData->MONR.ZPositionI32); + char headingString[ENOUGH_BUFFER_SIZE]; + sprintf(headingString, "%u", _monitorData->MONR.HeadingU16); + char longSpeedString[ENOUGH_BUFFER_SIZE]; + sprintf(longSpeedString, "%d", _monitorData->MONR.LongitudinalSpeedI16); + char stateString[ENOUGH_BUFFER_SIZE]; + sprintf(stateString, "%u", _monitorData->MONR.StateU8); - - sprintf(ipStringBuffer, "%s", - inet_ntop(AF_INET, &_monitorData->ClientIP, ipStringBuffer, sizeof (ipStringBuffer))); - //Build message from MonitorStruct - snprintf(_visualizationMessage, _sizeOfVisualizationMessage, "%c;%c;%c;%c;%c;%c;%c;%c;", + snprintf(_visualizationMessage, _sizeOfVisualizationMessage, "%s;%s;%s;%s;%s;%s;%s;%s;", ipStringBuffer, GPSMsOfWeekString, xPosString, @@ -82,6 +82,7 @@ void vCreateVisualizationMessage(MonitorDataType * _monitorData, char *_visualiz LogMessage(LOG_LEVEL_INFO, "Heading: %u", _monitorData->MONR.HeadingU16); LogMessage(LOG_LEVEL_INFO, "LongSpeed: %d", _monitorData->MONR.LongitudinalSpeedI16); LogMessage(LOG_LEVEL_INFO, "State: %u", _monitorData->MONR.StateU8); + LogMessage(LOG_LEVEL_INFO, "MESSAGE-SIZE = %d",_sizeOfVisualizationMessage); } } @@ -94,8 +95,6 @@ int main() { MonitorDataType monitorData; - int sizeOfVisualizationMessage = (INET_ADDRSTRLEN + sizeof (monitorData.MONR.GPSQmsOfWeekU32) + sizeof (monitorData.MONR.XPositionI32) + sizeof (monitorData.MONR.YPositionI32) + sizeof (monitorData.MONR.ZPositionI32) + sizeof (monitorData.MONR.HeadingU16) + sizeof (monitorData.MONR.LongitudinalSpeedI16) + sizeof (monitorData.MONR.StateU8) + 8 + //Number of fields + 1 (;) - 1); //Required LogInit(MODULE_NAME, LOG_LEVEL_DEBUG); LogMessage(LOG_LEVEL_INFO, "Task running with PID: %u", getpid()); @@ -105,7 +104,11 @@ int main() { vConnectVisualizationChannel(&visual_server, &visual_server_addr); - I32 iExit = 0; + //Setup signal handlers + if (signal(SIGINT, signalHandler) == SIG_ERR) + util_error("Unable to initialize signal handler"); + + // Initialize message bus connection @@ -113,7 +116,7 @@ int main() { nanosleep(&sleepTimePeriod, &remTime); } - while (true) { + while (!iExit) { if (iCommRecv(&command, mqRecvData, MQ_MSG_SIZE, NULL) < 0) { @@ -142,15 +145,31 @@ int main() { //Populate the monitorType UtilPopulateMonitorDataStruct(mqRecvData, (size_t) (sizeof (mqRecvData)), &monitorData, 0); + char dummy[1]; + int sizeOfVisualizationMessage; + + //Calculate size of incoming buffer + sizeOfVisualizationMessage = snprintf(dummy, sizeof(dummy),"%u;%d;%d;%d;%u;%d;%u;", + monitorData.MONR.GPSQmsOfWeekU32, + monitorData.MONR.XPositionI32, + monitorData.MONR.YPositionI32, + monitorData.MONR.ZPositionI32, + monitorData.MONR.HeadingU16, + monitorData.MONR.LongitudinalSpeedI16, + monitorData.MONR.StateU8 + ); + sizeOfVisualizationMessage += INET_ADDRSTRLEN; + sizeOfVisualizationMessage += 8; //(;) + //Allocate memory char *visualizationMessage = malloc(sizeOfVisualizationMessage * sizeof (char)); //Create visualization message and insert values from the monitor datastruct above - vCreateVisualizationMessage(&monitorData, visualizationMessage, sizeOfVisualizationMessage, 0); + vCreateVisualizationMessage(&monitorData, visualizationMessage, sizeOfVisualizationMessage, 0); //Send visualization message on the UDP socket UtilSendUDPData((const uint8_t *)"Visualization", &visual_server, &visual_server_addr, - visualizationMessage, sizeOfVisualizationMessage, 0); + visualizationMessage, strlen(visualizationMessage), 0); //Free memory used by malloc free(visualizationMessage); @@ -170,6 +189,9 @@ int main() { } } + //Return MQBus to "stack" + MQBusDisconnect(); + return 0; } @@ -177,6 +199,17 @@ int main() { /*------------------------------------------------------------ -- Private functions ------------------------------------------------------------*/ +void signalHandler(int signo) { + if (signo == SIGINT) { + LogMessage(LOG_LEVEL_WARNING, "Caught keyboard interrupt"); + iExit = 1; + } + else { + LogMessage(LOG_LEVEL_ERROR, "Caught unhandled signal"); + } +} + + static void vConnectVisualizationChannel(int *sockfd, struct sockaddr_in *addr) { struct hostent *server; char pcTempBuffer[MAX_UTIL_VARIBLE_SIZE]; From bf30b1b7dd6bb33213f5e5baffae8b06ccfc3964 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 24 Jan 2020 09:46:10 +0100 Subject: [PATCH 120/523] Implemented OSEM encoder message --- server/inc/iso22133.h | 32 +++++++++ server/src/iso22133.c | 161 +++++++++++++++++++++++++++++++++++++++++- 2 files changed, 192 insertions(+), 1 deletion(-) diff --git a/server/inc/iso22133.h b/server/inc/iso22133.h index 2d6c7d022..d78e28a20 100644 --- a/server/inc/iso22133.h +++ b/server/inc/iso22133.h @@ -36,6 +36,26 @@ typedef struct } FooterType; //2 bytes + +//! Predefined integer values with special meaning +#define LATITUDE_UNAVAILABLE_VALUE 900000000001 +#define LATITUDE_ONE_DEGREE_VALUE 10000000000 +#define LONGITUDE_UNAVAILABLE_VALUE 1800000000001 +#define LONGITUDE_ONE_DEGREE_VALUE 10000000000 +#define ALTITUDE_UNAVAILABLE_VALUE 800001 +#define ALTITUDE_ONE_METER_VALUE 100 +#define DATE_UNAVAILABLE_VALUE 0 +#define GPS_WEEK_UNAVAILABLE_VALUE 10001 +#define GPS_SECOND_OF_WEEK_UNAVAILABLE_VALUE 2419200000 +#define MAX_WAY_DEVIATION_UNAVAILABLE_VALUE 65535 +#define MAX_WAY_DEVIATION_ONE_METER_VALUE 1000 +#define MAX_LATERAL_DEVIATION_UNAVAILABLE_VALUE 65535 +#define MAX_LATERAL_DEVIATION_ONE_METER_VALUE 1000 +#define MIN_POSITIONING_ACCURACY_NOT_REQUIRED_VALUE 0 +#define MIN_POSITIONING_ACCURACY_ONE_METER_VALUE 1000 // ISO specification unclear on this value + + + //! *************************** OSEM typedef struct { @@ -67,9 +87,20 @@ typedef struct uint16_t MinPosAccuracyValueIdU16; uint16_t MinPosAccuracyContentLengthU16; uint16_t MinPosAccuracyU16; + FooterType footer; } OSEMType; //85 bytes //! OSEM value IDs +#define VALUE_ID_OSEM_LATITUDE 0x0020 +#define VALUE_ID_OSEM_LONGITUDE 0x0021 +#define VALUE_ID_OSEM_ALTITUDE 0x0022 +#define VALUE_ID_OSEM_DATE 0x0004 +#define VALUE_ID_OSEM_GPS_WEEK 0x0003 +#define VALUE_ID_OSEM_GPS_QUARTER_MILLISECOND_OF_WEEK 0x0002 +#define VALUE_ID_OSEM_MAX_WAY_DEVIATION 0x0070 +#define VALUE_ID_OSEM_MAX_LATERAL_DEVIATION 0x0072 +#define VALUE_ID_OSEM_MIN_POSITIONING_ACCURACY 0x0074 + //! *************************** STRT typedef struct @@ -351,6 +382,7 @@ typedef enum { ISOMessageReturnValue decodeMONRMessage(const char * MonrData, const size_t length, MONRType * MONRData, const char debug); ssize_t encodeSTRTMessage(const uint32_t startTimeGPSqmsOW, const uint16_t startGPSWeek, char * strtDataBuffer, const size_t bufferLength, const char debug); +ssize_t encodeOSEMMessage(const double * latitude_deg, const double * longitude_deg, const float * altitude_m, const float * maxPositionDeviation_m, const float * maxLateralDeviation_m, const float * minimumPositioningAccuracy_m, char * osemDataBuffer, const size_t bufferLength, const char debug); ISOMessageReturnValue MONRToASCII(const MONRType * MONRData, char * asciiBuffer, const size_t bufferLength, const char debug); ISOMessageReturnValue ASCIIToMONR(const char * asciiBuffer, MONRType * MONRData, const char debug); ISOMessageID getISOMessageType(const char * messageData, const size_t length, const char debug); diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 3a0fc9cc5..8b0862b94 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -1,11 +1,22 @@ #include "iso22133.h" #include "logging.h" +#include "maestroTime.h" #include #include #include +#include static const uint8_t SupportedProtocolVersions[] = { 2 }; +// ************************* Byte swapper definitions for 6 byte values +#if __BYTE_ORDER == __LITTLE_ENDIAN +#define le48toh(x) __uint64_identity (x) +#define htole48(x) __uint64_identity (x) +#else +#define le48toh(x) (__bswap_64(x) >> 16) +#define htole48(x) (__bswap_64(x) >> 16) +#endif + // ************************** static function declarations static ISOMessageReturnValue decodeISOHeader(const char *MessageBuffer, const size_t length, HeaderType * HeaderData, const char debug); @@ -206,6 +217,154 @@ ISOMessageID getISOMessageType(const char *messageData, const size_t length, con } +ssize_t encodeOSEMMessage(const double * latitude_deg, const double * longitude_deg, const float * altitude_m, const float * maxWayDeviation_m, const float * maxLateralDeviation_m, const float * minimumPositioningAccuracy_m, char * osemDataBuffer, const size_t bufferLength, const char debug) { + + const char SizeDifference64bitTo48bit = 2; + OSEMType OSEMData; + struct timeval currentTime; + struct tm * printableTime; + char *p = osemDataBuffer; + TimeSetToCurrentSystemTime(¤tTime); + printableTime = localtime(¤tTime.tv_sec); + + memset(osemDataBuffer, 0, bufferLength); + + // If buffer too small to hold OSEM data, generate an error + if (bufferLength < sizeof (OSEMData) - 2* SizeDifference64bitTo48bit) { + LogMessage(LOG_LEVEL_ERROR, "Buffer too small to hold necessary OSEM data"); + return -1; + } + + // Build header, and account for the two values which are 48 bit in the message + OSEMData.Header = buildISOHeader(MESSAGE_ID_OSEM, sizeof (OSEMData) + - sizeof (OSEMData.Header) - sizeof (OSEMData.footer) - 2*SizeDifference64bitTo48bit, debug); + + // Fill the OSEM struct with relevant values + OSEMData.LatitudeValueIdU16 = VALUE_ID_OSEM_LATITUDE; + OSEMData.LatitudeContentLengthU16 = sizeof (OSEMData.LatitudeI64) - SizeDifference64bitTo48bit; + OSEMData.LatitudeI64 = (latitude_deg == NULL) ? + LATITUDE_UNAVAILABLE_VALUE : (int64_t) (*latitude_deg * LATITUDE_ONE_DEGREE_VALUE); + + OSEMData.LongitudeValueIdU16 = VALUE_ID_OSEM_LONGITUDE; + OSEMData.LongitudeContentLengthU16 = sizeof (OSEMData.LongitudeI64) - SizeDifference64bitTo48bit; + OSEMData.LongitudeI64 = (longitude_deg == NULL) ? + LONGITUDE_UNAVAILABLE_VALUE : (int64_t) (*longitude_deg * LONGITUDE_ONE_DEGREE_VALUE); + + OSEMData.AltitudeValueIdU16 = VALUE_ID_OSEM_ALTITUDE; + OSEMData.AltitudeContentLengthU16 = sizeof (OSEMData.AltitudeI32); + OSEMData.AltitudeI32 = (altitude_m == NULL) ? + ALTITUDE_UNAVAILABLE_VALUE : (int32_t) (*altitude_m * ALTITUDE_ONE_METER_VALUE); + + OSEMData.DateValueIdU16 = VALUE_ID_OSEM_DATE; + OSEMData.DateContentLengthU16 = sizeof (OSEMData.DateContentLengthU16); + OSEMData.DateU32 = (uint32_t) ((printableTime->tm_year + 1900) * 10000 + (printableTime->tm_mon + 1) * 100 + + (printableTime->tm_mday)); + + OSEMData.GPSWeekValueIdU16 = VALUE_ID_OSEM_GPS_WEEK; + OSEMData.GPSWeekContentLengthU16 = sizeof (OSEMData.GPSWeekContentLengthU16); + OSEMData.GPSWeekU16 = TimeGetAsGPSweek(¤tTime); + + OSEMData.GPSSOWValueIdU16 = VALUE_ID_OSEM_GPS_QUARTER_MILLISECOND_OF_WEEK; + OSEMData.GPSSOWContentLengthU16 = sizeof (OSEMData.GPSQmsOfWeekU32); + OSEMData.GPSQmsOfWeekU32 = TimeGetAsGPSqmsOfWeek(¤tTime); + + OSEMData.MaxWayDeviationValueIdU16 = VALUE_ID_OSEM_MAX_WAY_DEVIATION; + OSEMData.MaxWayDeviationContentLengthU16 = sizeof (OSEMData.MaxWayDeviationU16); + OSEMData.MaxWayDeviationU16 = (maxWayDeviation_m == NULL) ? + MAX_WAY_DEVIATION_UNAVAILABLE_VALUE : (uint16_t) (*maxWayDeviation_m * MAX_WAY_DEVIATION_ONE_METER_VALUE); + + OSEMData.MaxLateralDeviationValueIdU16 = VALUE_ID_OSEM_MAX_LATERAL_DEVIATION; + OSEMData.MaxLateralDeviationContentLengthU16 = sizeof (OSEMData.MaxLateralDeviationU16); + OSEMData.MaxLateralDeviationU16 = (maxLateralDeviation_m == NULL) ? + MAX_LATERAL_DEVIATION_UNAVAILABLE_VALUE : (uint16_t) (*maxLateralDeviation_m * MAX_LATERAL_DEVIATION_ONE_METER_VALUE); + + OSEMData.MinPosAccuracyValueIdU16 = VALUE_ID_OSEM_MIN_POSITIONING_ACCURACY; + OSEMData.MinPosAccuracyContentLengthU16 = sizeof (OSEMData.MinPosAccuracyU16); + OSEMData.MinPosAccuracyU16 = (minimumPositioningAccuracy_m == NULL) ? + MIN_POSITIONING_ACCURACY_NOT_REQUIRED_VALUE : (uint16_t) (*minimumPositioningAccuracy_m * MIN_POSITIONING_ACCURACY_ONE_METER_VALUE); + + if (debug) { + LogPrint("OSEM message:\n\tLatitude value ID: 0x%x\n\tLatitude content length: %u\n\tLatitude: %ld [100 nanodegrees]\n\t" + "Longitude value ID: 0x%x\n\tLongitude content length: %u\n\tLongitude: %ld [100 nanodegrees]\n\t" + "Altitude value ID: 0x%x\n\tAltitude content length: %u\n\tAltitude: %d [cm]" + "Date value ID: 0x%x\n\tDate content length: %u\n\tDate: %u\n\t" + "GPS week value ID: 0x%x\n\tGPS week content length: %u\n\tGPS week: %u\n\t" + "GPS second of week value ID: 0x%x\n\tGPS second of week content length: %u\n\tGPS second of week: %u [¼ ms]\n\t" + "Max way deviation value ID: 0x%x\n\tMax way deviation content length: %u\n\tMax way deviation: %u\n\t" + "Max lateral deviation value ID: 0x%x\n\tMax lateral deviation content length: %u\n\t" + "Min positioning accuracy value ID: 0x%x\n\tMin positioning accuracy content length: %u\n\tMin positioning accuracy: %u", + OSEMData.LatitudeValueIdU16, OSEMData.LatitudeContentLengthU16, OSEMData.LatitudeI64, + OSEMData.LongitudeValueIdU16, OSEMData.LongitudeContentLengthU16, OSEMData.LongitudeI64, + OSEMData.AltitudeValueIdU16, OSEMData.AltitudeContentLengthU16, OSEMData.AltitudeI32, + OSEMData.DateValueIdU16, OSEMData.DateContentLengthU16, OSEMData.DateU32, + OSEMData.GPSWeekValueIdU16, OSEMData.GPSWeekContentLengthU16, OSEMData.GPSWeekU16, + OSEMData.GPSSOWValueIdU16, OSEMData.GPSSOWContentLengthU16, OSEMData.GPSQmsOfWeekU32, + OSEMData.MaxWayDeviationValueIdU16, OSEMData.MaxWayDeviationContentLengthU16, OSEMData.MaxWayDeviationU16, + OSEMData.MaxLateralDeviationValueIdU16, OSEMData.MaxLateralDeviationContentLengthU16, OSEMData.MaxLateralDeviationU16, + OSEMData.MinPosAccuracyValueIdU16, OSEMData.MinPosAccuracyContentLengthU16, OSEMData.MinPosAccuracyU16); + } + + // Switch endianness to little endian for all fields + OSEMData.LatitudeValueIdU16 = htole16(OSEMData.LatitudeValueIdU16); + OSEMData.LatitudeContentLengthU16 = htole16(OSEMData.LatitudeContentLengthU16); + OSEMData.LatitudeI64 = (int64_t) htole48(OSEMData.LatitudeI64); + OSEMData.LongitudeValueIdU16 = htole16(OSEMData.LongitudeValueIdU16); + OSEMData.LongitudeContentLengthU16 = htole16(OSEMData.LongitudeContentLengthU16); + OSEMData.LongitudeI64 = (int64_t) htole48(OSEMData.LongitudeI64); + OSEMData.AltitudeValueIdU16 = htole16(OSEMData.AltitudeValueIdU16); + OSEMData.AltitudeContentLengthU16 = htole16(OSEMData.AltitudeContentLengthU16); + OSEMData.AltitudeI32 = (int32_t) htole32(OSEMData.AltitudeI32); + OSEMData.DateValueIdU16 = htole16(OSEMData.DateValueIdU16); + OSEMData.DateContentLengthU16 = htole16(OSEMData.DateContentLengthU16); + OSEMData.DateU32 = htole32(OSEMData.DateU32); + OSEMData.GPSWeekValueIdU16 = htole16(OSEMData.GPSWeekValueIdU16); + OSEMData.GPSWeekContentLengthU16 = htole16(OSEMData.GPSWeekContentLengthU16); + OSEMData.GPSWeekU16 = htole16(OSEMData.GPSWeekU16); + OSEMData.GPSSOWValueIdU16 = htole16(OSEMData.GPSSOWValueIdU16); + OSEMData.GPSSOWContentLengthU16 = htole16(OSEMData.GPSSOWContentLengthU16); + OSEMData.GPSQmsOfWeekU32 = htole32(OSEMData.GPSQmsOfWeekU32); + OSEMData.MaxWayDeviationValueIdU16 = htole16(OSEMData.MaxWayDeviationValueIdU16); + OSEMData.MaxWayDeviationContentLengthU16 = htole16(OSEMData.MaxWayDeviationContentLengthU16); + OSEMData.MaxWayDeviationU16 = htole16(OSEMData.MaxWayDeviationU16); + OSEMData.MaxLateralDeviationValueIdU16 = htole16(OSEMData.MaxLateralDeviationValueIdU16); + OSEMData.MaxLateralDeviationContentLengthU16 = htole16(OSEMData.MaxLateralDeviationContentLengthU16); + OSEMData.MaxLateralDeviationU16 = htole16(OSEMData.MaxLateralDeviationU16); + OSEMData.MinPosAccuracyValueIdU16 = htole16(OSEMData.MinPosAccuracyValueIdU16); + OSEMData.MinPosAccuracyContentLengthU16 = htole16(OSEMData.MinPosAccuracyContentLengthU16); + OSEMData.MinPosAccuracyU16 = htole16(OSEMData.MinPosAccuracyU16); + + + // Copy data from OSEM struct into the buffer + // Must be done before constructing the footer due to the two 48bit size anomalies + memcpy(p, &OSEMData.Header, sizeof (OSEMData.Header)); + p += sizeof (OSEMData.Header); + + // Special handling of 48 bit value + memcpy(p, &OSEMData.LatitudeValueIdU16, sizeof (OSEMData.LatitudeValueIdU16) + sizeof (OSEMData.LatitudeContentLengthU16)); + p += sizeof (OSEMData.LatitudeValueIdU16) + sizeof (OSEMData.LatitudeContentLengthU16); + memcpy(p, &OSEMData.LatitudeI64, sizeof (OSEMData.LatitudeI64) - SizeDifference64bitTo48bit); + p += sizeof (OSEMData.LatitudeI64) - SizeDifference64bitTo48bit; + + // Special handling of 48 bit value + memcpy(p, &OSEMData.LongitudeValueIdU16, sizeof (OSEMData.LongitudeValueIdU16) + sizeof (OSEMData.LongitudeContentLengthU16)); + p += sizeof (OSEMData.LongitudeValueIdU16) + sizeof (OSEMData.LongitudeContentLengthU16); + memcpy(p, &OSEMData.LongitudeI64, sizeof (OSEMData.LongitudeI64) - SizeDifference64bitTo48bit); + p += sizeof (OSEMData.LongitudeI64) - SizeDifference64bitTo48bit; + + // Copy rest of struct (excluding footer) directly into buffer since no more byte anomalies remain + memcpy(p, &OSEMData.AltitudeI32, sizeof (OSEMData) - sizeof (OSEMData.footer) + - (size_t) (p - osemDataBuffer + 2*SizeDifference64bitTo48bit)); + p += sizeof (OSEMData) - sizeof (OSEMData.footer) - (size_t) (p - osemDataBuffer + 2*SizeDifference64bitTo48bit); + + // Build footer + OSEMData.footer = buildISOFooter(osemDataBuffer, sizeof (OSEMType) - 2*SizeDifference64bitTo48bit, debug); + memcpy(p, &OSEMData.footer, sizeof (OSEMData.footer)); + + return sizeof (OSEMType) - 2*SizeDifference64bitTo48bit; +} + + + /*! * \brief encodeSTRTMessage Constructs an ISO STRT message based on start time parameters * \param startTimeGPSqmsOW Quarter milliseconds of week when recipient of message shall start @@ -247,7 +406,7 @@ ssize_t encodeSTRTMessage(const uint32_t startTimeGPSqmsOW, const uint16_t start if (debug) { LogPrint("STRT message:\n\tGPS second of week value ID: 0x%x\n\t" - "GPS second of week content length: %u\n\tGPS second of week: %u qms\n\t" + "GPS second of week content length: %u\n\tGPS second of week: %u [¼ ms]\n\t" "GPS week value ID: 0x%x\n\tGPS week content length: %u\n\t" "GPS week: %u", STRTData.StartTimeValueIdU16, STRTData.StartTimeContentLengthU16, STRTData.StartTimeU32, STRTData.GPSWeekValueIdU16, STRTData.GPSWeekContentLengthU16, From ced138870c18b26872cfe8a45ab3abdba05ea305 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 24 Jan 2020 09:48:36 +0100 Subject: [PATCH 121/523] Removed build OSEM function from object control --- server/inc/objectcontrol.h | 2 - server/src/objectcontrol.c | 94 -------------------------------------- 2 files changed, 96 deletions(-) diff --git a/server/inc/objectcontrol.h b/server/inc/objectcontrol.h index b5a661c31..abff28fc7 100644 --- a/server/inc/objectcontrol.h +++ b/server/inc/objectcontrol.h @@ -21,7 +21,6 @@ ------------------------------------------------------------*/ void objectcontrol_task(TimeType *GPSTime, GSDType *GSD, LOG_LEVEL logLevel); -I32 ObjectControlBuildOSEMMessage(C8* MessageBuffer, OSEMType *OSEMData, TimeType *GPSTime, C8 *Latitude, C8 *Longitude, C8 *Altitude, U8 debug); I32 ObjectControlBuildOSTMMessage(C8* MessageBuffer, OSTMType *OSTMData, C8 CommandOption, U8 debug); I32 ObjectControlBuildHEABMessage(C8* MessageBuffer, HEABType *HEABData, TimeType *GPSTime, U8 CCStatus, U8 debug); int ObjectControlBuildLLCMMessage(char* MessageBuffer, unsigned short Speed, unsigned short Curvature, unsigned char Mode, char debug); @@ -31,6 +30,5 @@ I32 ObjectControlBuildTRAJMessageHeader(C8* MessageBuffer, I32 *RowCount, Header I32 ObjectControlBuildTRAJMessage(C8* MessageBuffer, FILE *fd, I32 RowCount, DOTMType *DOTMType, U8 debug); I32 ObjectControlBuildVOILMessage(C8* MessageBuffer, VOILType *VOILData, C8* SimData, U8 debug); I32 ObjectControlSendTRAJMessage(C8* Filename, I32 *Socket, I32 RowCount, C8 *IP, U32 Port, DOTMType *DOTMData, U8 debug); -I32 ObjectControlMONRToASCII(MONRType *MONRData, GeoPosition *OriginPosition, I32 Idn, C8 *Id, C8 *Timestamp, C8 *XPosition, C8 *YPosition, C8 *ZPosition, C8 *LongitudinalSpeed, C8 *LateralSpeed, C8 *LongitudinalAcc, C8 *LateralAcc, C8 *Heading, C8 *DriveDirection, C8 *ObjectState, C8 *ReadyToArm, C8* ErrorStatus, C8 debug); int ObjectControlOSEMtoASCII(OSEMType *OSEMData,char *GPSWeek, char *GPSLatitude, char *GPSLongitude, char *GPSAltitude); #endif //__OBJECTCONTROL_H_INCLUDED__ diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 94cdd24eb..332388db0 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -155,8 +155,6 @@ static I32 vCheckRemoteDisconnected(int *sockfd); static void vCreateSafetyChannel(const char *name, const uint32_t port, int *sockfd, struct sockaddr_in *addr); static void vCloseSafetyChannel(int *sockfd); -I32 ObjectControlBuildOSEMMessage(C8 * MessageBuffer, OSEMType * OSEMData, TimeType * GPSTime, C8 * Latitude, - C8 * Longitude, C8 * Altitude, U8 debug); static size_t uiRecvMonitor(int *sockfd, char *buffer, size_t length); static int iGetObjectIndexFromObjectIP(in_addr_t ipAddr, in_addr_t objectIPs[], unsigned int numberOfObjects); static void signalHandler(int signo); @@ -1193,98 +1191,6 @@ I32 ObjectControlBuildVOILMessage(C8 * MessageBuffer, VOILType * VOILData, C8 * } - -I32 ObjectControlBuildOSEMMessage(C8 * MessageBuffer, OSEMType * OSEMData, TimeType * GPSTime, C8 * Latitude, - C8 * Longitude, C8 * Altitude, U8 debug) { - I32 MessageIndex = 0, i = 0; - dbl Data; - U16 Crc = 0; - C8 *p; - U32 ISODate = 0; - - bzero(MessageBuffer, COMMAND_OSEM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); - - OSEMData->Header.SyncWordU16 = ISO_SYNC_WORD; - OSEMData->Header.TransmitterIdU8 = 0; - OSEMData->Header.MessageCounterU8 = 0; - OSEMData->Header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; - OSEMData->Header.MessageIdU16 = COMMAND_OSEM_CODE; - OSEMData->Header.MessageLengthU32 = sizeof (OSEMType) - sizeof (HeaderType) - 4; - OSEMData->LatitudeValueIdU16 = VALUE_ID_LATITUDE; - OSEMData->LatitudeContentLengthU16 = 6; - OSEMData->LatitudeI64 = (I64) ((atof((const char *)Latitude) * 1e10)); - OSEMData->LongitudeValueIdU16 = VALUE_ID_LONGITUDE; - OSEMData->LongitudeContentLengthU16 = 6; - OSEMData->LongitudeI64 = (I64) ((atof((const char *)Longitude) * 1e10)); - OSEMData->AltitudeValueIdU16 = VALUE_ID_ALTITUDE; - OSEMData->AltitudeContentLengthU16 = 4; - OSEMData->AltitudeI32 = (I32) (atof((char *)Altitude) * 1e2); - OSEMData->DateValueIdU16 = VALUE_ID_DATE_ISO8601; - OSEMData->DateContentLengthU16 = 4; - OSEMData->DateU32 = - ((U32) GPSTime->YearU16 * 10000) + ((U32) GPSTime->MonthU8 * 100) + ((U32) GPSTime->DayU8); - OSEMData->GPSWeekValueIdU16 = VALUE_ID_GPS_WEEK; - OSEMData->GPSWeekContentLengthU16 = 2; - OSEMData->GPSWeekU16 = GPSTime->GPSWeekU16; - OSEMData->GPSSOWValueIdU16 = VALUE_ID_GPS_SECOND_OF_WEEK; - OSEMData->GPSSOWContentLengthU16 = 4; - OSEMData->GPSQmsOfWeekU32 = - ((GPSTime->GPSSecondsOfWeekU32 * 1000 + GPSTime->MillisecondU16) << 2) + GPSTime->MicroSecondU16; - OSEMData->MaxWayDeviationValueIdU16 = VALUE_ID_MAX_WAY_DEVIATION; - OSEMData->MaxWayDeviationContentLengthU16 = 2; - OSEMData->MaxWayDeviationU16 = 65535; - OSEMData->MaxLateralDeviationValueIdU16 = VALUE_ID_MAX_LATERAL_DEVIATION; - OSEMData->MaxLateralDeviationContentLengthU16 = 2; - OSEMData->MaxLateralDeviationU16 = 65535; - OSEMData->MinPosAccuracyContentLengthU16 = 2; - OSEMData->MinPosAccuracyValueIdU16 = VALUE_ID_MIN_POS_ACCURACY; - OSEMData->MinPosAccuracyU16 = 65535; - - if (!GPSTime->isGPSenabled) { - OSEMData->DateU32 = UtilgetIntDateFromMS(UtilgetCurrentUTCtimeMS()); - UtilgetCurrentGPStime(&OSEMData->GPSWeekU16, &OSEMData->GPSQmsOfWeekU32); - } - - p = (C8 *) OSEMData; - for (i = 0; i < 21; i++) - *(MessageBuffer + i) = *p++; - *p++; - *p++; - for (; i < 31; i++) - *(MessageBuffer + i) = *p++; - *p++; - *p++; - for (; i < sizeof (OSEMType) - 4; i++) - *(MessageBuffer + i) = *p++; - - Crc = crc_16((const C8 *)MessageBuffer, sizeof (OSEMType) - 4); - Crc = 0; - *(MessageBuffer + i++) = (U8) (Crc); - *(MessageBuffer + i++) = (U8) (Crc >> 8); - - MessageIndex = i; - - if (debug) { - // TODO: Change to log printout when byte thingy has been implemented - printf("OSEM total length = %d bytes (header+message+footer)\n", - (int)(COMMAND_OSEM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH)); - printf("----HEADER----\n"); - for (i = 0; i < sizeof (HeaderType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----MESSAGE----\n"); - for (; i < sizeof (OSEMType) - 4; i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----FOOTER----\n"); - for (; i < MessageIndex; i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n"); - printf("Latitude = %ld\n", OSEMData->LatitudeI64); - printf("Longitude = %ld\n", OSEMData->LongitudeI64); - printf("ISODate = %d\n", OSEMData->DateU32); - } - return MessageIndex; //Total number of bytes -} - int ObjectControlOSEMtoASCII(OSEMType * OSEMData, char *GPSWeek, char *GPSLatitude, char *GPSLongitude, char *GPSAltitude) { // what do i want? in my mq? gps week, origin in lat and long coordinates From e90be3f70d130eced82ec4e7d9ba37d54c57cb0b Mon Sep 17 00:00:00 2001 From: alfaro01 Date: Fri, 24 Jan 2020 10:18:33 +0100 Subject: [PATCH 122/523] Fixed issue with MQBus-system --- modules/Visualization/src/main.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/modules/Visualization/src/main.c b/modules/Visualization/src/main.c index 2418da0e0..1395dc3a0 100644 --- a/modules/Visualization/src/main.c +++ b/modules/Visualization/src/main.c @@ -190,7 +190,9 @@ int main() { } //Return MQBus to "stack" - MQBusDisconnect(); + (void)iCommClose(); + + LogMessage(LOG_LEVEL_INFO, "Visualization exiting..."); return 0; } From 14b96801421a43932531139edc0379ab47204f4f Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 24 Jan 2020 10:19:31 +0100 Subject: [PATCH 123/523] Started using the new OSEM builder function --- server/src/objectcontrol.c | 39 +++++--------------------------------- 1 file changed, 5 insertions(+), 34 deletions(-) diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 332388db0..09e9ca32c 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -752,9 +752,11 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { UtilSetObjectPositionIP(&OP[iIndex], object_address_name[iIndex]); - MessageLength = ObjectControlBuildOSEMMessage(MessageBuffer, &OSEMData, GPSTime, - OriginLatitude, OriginLongitude, - OriginAltitude, 0); + MessageLength = encodeOSEMMessage(&OriginPosition.Latitude, &OriginPosition.Longitude, &OriginPosition.Altitude, + NULL, NULL, NULL, MessageBuffer, sizeof (MessageBuffer), 0); + if (MessageLength < 0) { + util_error("OSEM encoding error"); + } DisconnectU8 = 0; @@ -816,17 +818,6 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { LogMessage(LOG_LEVEL_INFO, "Sending OSEM"); LOG_SEND(LogBuffer, "[ObjectControl] Sending OSEM."); - ObjectControlOSEMtoASCII(&OSEMData, GPSWeek, OriginLatitude, OriginLongitude, - OriginAltitude); - bzero(pcSendBuffer, sizeof (pcSendBuffer)); - strcat(pcSendBuffer, GPSWeek); - strcat(pcSendBuffer, ";"); - strcat(pcSendBuffer, OriginLatitude); - strcat(pcSendBuffer, ";"); - strcat(pcSendBuffer, OriginLongitude); - strcat(pcSendBuffer, ";"); - strcat(pcSendBuffer, OriginAltitude); - //Restore the buffers DataDictionaryGetOriginLatitudeC8(GSD, OriginLatitude, SMALL_BUFFER_SIZE_0); DataDictionaryGetOriginLongitudeC8(GSD, OriginLongitude, SMALL_BUFFER_SIZE_0); @@ -1191,26 +1182,6 @@ I32 ObjectControlBuildVOILMessage(C8 * MessageBuffer, VOILType * VOILData, C8 * } -int ObjectControlOSEMtoASCII(OSEMType * OSEMData, char *GPSWeek, char *GPSLatitude, char *GPSLongitude, - char *GPSAltitude) { - // what do i want? in my mq? gps week, origin in lat and long coordinates - bzero(GPSWeek, SMALL_BUFFER_SIZE_0); - bzero(GPSLatitude, SMALL_BUFFER_SIZE_0); - bzero(GPSLongitude, SMALL_BUFFER_SIZE_0); - bzero(GPSAltitude, SMALL_BUFFER_SIZE_0); - - if (OSEMData->Header.MessageIdU16 == COMMAND_OSEM_CODE) { - sprintf(GPSWeek, "%" PRIu16, OSEMData->GPSWeekU16); - - sprintf(GPSLatitude, "%" PRIi64, OSEMData->LatitudeI64); - - sprintf(GPSLongitude, "%" PRIi64, OSEMData->LongitudeI64); - - sprintf(GPSAltitude, "%" PRIi32, OSEMData->AltitudeI32); - } - return 0; -} - I32 ObjectControlBuildOSTMMessage(C8 * MessageBuffer, OSTMType * OSTMData, C8 CommandOption, U8 debug) { I32 MessageIndex = 0, i; U16 Crc = 0; From 26c044c6456a7d0a00a81c3fa4bc1e937aa63fd4 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 24 Jan 2020 10:20:35 +0100 Subject: [PATCH 124/523] Made MessageLength a ssize_t instead of an int --- server/src/objectcontrol.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 09e9ca32c..1afac7634 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -241,7 +241,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { FILE *fd; C8 Timestamp[SMALL_BUFFER_SIZE_0]; C8 GPSWeek[SMALL_BUFFER_SIZE_0]; - I32 MessageLength; + ssize_t MessageLength; C8 *MiscPtr; C8 MiscText[SMALL_BUFFER_SIZE_0]; U32 StartTimeU32 = 0; From c3a40190dd53f440d3aad149a77a6de4daca5fcc Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 24 Jan 2020 10:31:27 +0100 Subject: [PATCH 125/523] Removed unused struct --- server/src/objectcontrol.c | 1 - 1 file changed, 1 deletion(-) diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 1afac7634..f1502fc2a 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -265,7 +265,6 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { C8 pcSendBuffer[MBUS_MAX_DATALEN]; C8 ObjectPort[SMALL_BUFFER_SIZE_0]; HeaderType HeaderData; - OSEMType OSEMData; OSTMType OSTMData; HEABType HEABData; MONRType MONRData; From fdac5a1737f87fd73d3bec635b1792fd3401670d Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 24 Jan 2020 10:33:31 +0100 Subject: [PATCH 126/523] Refactoring OSEM struct --- server/inc/iso22133.h | 62 ++++++++-------- server/src/iso22133.c | 168 +++++++++++++++++++++--------------------- 2 files changed, 115 insertions(+), 115 deletions(-) diff --git a/server/inc/iso22133.h b/server/inc/iso22133.h index d78e28a20..7107f09e8 100644 --- a/server/inc/iso22133.h +++ b/server/inc/iso22133.h @@ -59,34 +59,34 @@ typedef struct //! *************************** OSEM typedef struct { - HeaderType Header; - uint16_t LatitudeValueIdU16; - uint16_t LatitudeContentLengthU16; - int64_t LatitudeI64; - uint16_t LongitudeValueIdU16; - uint16_t LongitudeContentLengthU16; - int64_t LongitudeI64; - uint16_t AltitudeValueIdU16; - uint16_t AltitudeContentLengthU16; - int32_t AltitudeI32; - uint16_t DateValueIdU16; - uint16_t DateContentLengthU16; - uint32_t DateU32; - uint16_t GPSWeekValueIdU16; - uint16_t GPSWeekContentLengthU16; - uint16_t GPSWeekU16; - uint16_t GPSSOWValueIdU16; - uint16_t GPSSOWContentLengthU16; - uint32_t GPSQmsOfWeekU32; - uint16_t MaxWayDeviationValueIdU16; - uint16_t MaxWayDeviationContentLengthU16; - uint16_t MaxWayDeviationU16; - uint16_t MaxLateralDeviationValueIdU16; - uint16_t MaxLateralDeviationContentLengthU16; - uint16_t MaxLateralDeviationU16; - uint16_t MinPosAccuracyValueIdU16; - uint16_t MinPosAccuracyContentLengthU16; - uint16_t MinPosAccuracyU16; + HeaderType header; + uint16_t latitudeValueID; + uint16_t latitudeContentLength; + int64_t latitude; + uint16_t longitudeValueID; + uint16_t longitudeContentLength; + int64_t longitude; + uint16_t altitudeValueID; + uint16_t altitudeContentLength; + int32_t altitude; + uint16_t dateValueID; + uint16_t dateContentLength; + uint32_t date; + uint16_t GPSWeekValueID; + uint16_t GPSWeekContentLength; + uint16_t GPSWeek; + uint16_t GPSQmsOfWeekValueID; + uint16_t GPSQmsOfWeekContentLength; + uint32_t GPSQmsOfWeek; + uint16_t maxWayDeviationValueID; + uint16_t maxWayDeviationContentLength; + uint16_t maxWayDeviation; + uint16_t maxLateralDeviationValueID; + uint16_t maxLateralDeviationContentLength; + uint16_t maxLateralDeviation; + uint16_t minPosAccuracyValueID; + uint16_t minPosAccuracyContentLength; + uint16_t minPosAccuracy; FooterType footer; } OSEMType; //85 bytes @@ -109,9 +109,9 @@ typedef struct uint16_t StartTimeValueIdU16; uint16_t StartTimeContentLengthU16; uint32_t StartTimeU32; - uint16_t GPSWeekValueIdU16; - uint16_t GPSWeekContentLengthU16; - uint16_t GPSWeekU16; + uint16_t GPSWeekValueID; + uint16_t GPSWeekContentLength; + uint16_t GPSWeek; FooterType footer; } STRTType; //27 bytes diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 8b0862b94..059080e15 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -236,51 +236,51 @@ ssize_t encodeOSEMMessage(const double * latitude_deg, const double * longitude_ } // Build header, and account for the two values which are 48 bit in the message - OSEMData.Header = buildISOHeader(MESSAGE_ID_OSEM, sizeof (OSEMData) - - sizeof (OSEMData.Header) - sizeof (OSEMData.footer) - 2*SizeDifference64bitTo48bit, debug); + OSEMData.header = buildISOHeader(MESSAGE_ID_OSEM, sizeof (OSEMData) + - sizeof (OSEMData.header) - sizeof (OSEMData.footer) - 2*SizeDifference64bitTo48bit, debug); // Fill the OSEM struct with relevant values - OSEMData.LatitudeValueIdU16 = VALUE_ID_OSEM_LATITUDE; - OSEMData.LatitudeContentLengthU16 = sizeof (OSEMData.LatitudeI64) - SizeDifference64bitTo48bit; - OSEMData.LatitudeI64 = (latitude_deg == NULL) ? + OSEMData.latitudeValueID = VALUE_ID_OSEM_LATITUDE; + OSEMData.latitudeContentLength = sizeof (OSEMData.latitude) - SizeDifference64bitTo48bit; + OSEMData.latitude = (latitude_deg == NULL) ? LATITUDE_UNAVAILABLE_VALUE : (int64_t) (*latitude_deg * LATITUDE_ONE_DEGREE_VALUE); - OSEMData.LongitudeValueIdU16 = VALUE_ID_OSEM_LONGITUDE; - OSEMData.LongitudeContentLengthU16 = sizeof (OSEMData.LongitudeI64) - SizeDifference64bitTo48bit; - OSEMData.LongitudeI64 = (longitude_deg == NULL) ? + OSEMData.longitudeValueID = VALUE_ID_OSEM_LONGITUDE; + OSEMData.longitudeContentLength = sizeof (OSEMData.longitude) - SizeDifference64bitTo48bit; + OSEMData.longitude = (longitude_deg == NULL) ? LONGITUDE_UNAVAILABLE_VALUE : (int64_t) (*longitude_deg * LONGITUDE_ONE_DEGREE_VALUE); - OSEMData.AltitudeValueIdU16 = VALUE_ID_OSEM_ALTITUDE; - OSEMData.AltitudeContentLengthU16 = sizeof (OSEMData.AltitudeI32); - OSEMData.AltitudeI32 = (altitude_m == NULL) ? + OSEMData.altitudeValueID = VALUE_ID_OSEM_ALTITUDE; + OSEMData.altitudeContentLength = sizeof (OSEMData.altitude); + OSEMData.altitude = (altitude_m == NULL) ? ALTITUDE_UNAVAILABLE_VALUE : (int32_t) (*altitude_m * ALTITUDE_ONE_METER_VALUE); - OSEMData.DateValueIdU16 = VALUE_ID_OSEM_DATE; - OSEMData.DateContentLengthU16 = sizeof (OSEMData.DateContentLengthU16); - OSEMData.DateU32 = (uint32_t) ((printableTime->tm_year + 1900) * 10000 + (printableTime->tm_mon + 1) * 100 + OSEMData.dateValueID = VALUE_ID_OSEM_DATE; + OSEMData.dateContentLength = sizeof (OSEMData.dateContentLength); + OSEMData.date = (uint32_t) ((printableTime->tm_year + 1900) * 10000 + (printableTime->tm_mon + 1) * 100 + (printableTime->tm_mday)); - OSEMData.GPSWeekValueIdU16 = VALUE_ID_OSEM_GPS_WEEK; - OSEMData.GPSWeekContentLengthU16 = sizeof (OSEMData.GPSWeekContentLengthU16); - OSEMData.GPSWeekU16 = TimeGetAsGPSweek(¤tTime); + OSEMData.GPSWeekValueID = VALUE_ID_OSEM_GPS_WEEK; + OSEMData.GPSWeekContentLength = sizeof (OSEMData.GPSWeekContentLength); + OSEMData.GPSWeek = TimeGetAsGPSweek(¤tTime); - OSEMData.GPSSOWValueIdU16 = VALUE_ID_OSEM_GPS_QUARTER_MILLISECOND_OF_WEEK; - OSEMData.GPSSOWContentLengthU16 = sizeof (OSEMData.GPSQmsOfWeekU32); - OSEMData.GPSQmsOfWeekU32 = TimeGetAsGPSqmsOfWeek(¤tTime); + OSEMData.GPSQmsOfWeekValueID = VALUE_ID_OSEM_GPS_QUARTER_MILLISECOND_OF_WEEK; + OSEMData.GPSQmsOfWeekContentLength = sizeof (OSEMData.GPSQmsOfWeek); + OSEMData.GPSQmsOfWeek = TimeGetAsGPSqmsOfWeek(¤tTime); - OSEMData.MaxWayDeviationValueIdU16 = VALUE_ID_OSEM_MAX_WAY_DEVIATION; - OSEMData.MaxWayDeviationContentLengthU16 = sizeof (OSEMData.MaxWayDeviationU16); - OSEMData.MaxWayDeviationU16 = (maxWayDeviation_m == NULL) ? + OSEMData.maxWayDeviationValueID = VALUE_ID_OSEM_MAX_WAY_DEVIATION; + OSEMData.maxWayDeviationContentLength = sizeof (OSEMData.maxWayDeviation); + OSEMData.maxWayDeviation = (maxWayDeviation_m == NULL) ? MAX_WAY_DEVIATION_UNAVAILABLE_VALUE : (uint16_t) (*maxWayDeviation_m * MAX_WAY_DEVIATION_ONE_METER_VALUE); - OSEMData.MaxLateralDeviationValueIdU16 = VALUE_ID_OSEM_MAX_LATERAL_DEVIATION; - OSEMData.MaxLateralDeviationContentLengthU16 = sizeof (OSEMData.MaxLateralDeviationU16); - OSEMData.MaxLateralDeviationU16 = (maxLateralDeviation_m == NULL) ? + OSEMData.maxLateralDeviationValueID = VALUE_ID_OSEM_MAX_LATERAL_DEVIATION; + OSEMData.maxLateralDeviationContentLength = sizeof (OSEMData.maxLateralDeviation); + OSEMData.maxLateralDeviation = (maxLateralDeviation_m == NULL) ? MAX_LATERAL_DEVIATION_UNAVAILABLE_VALUE : (uint16_t) (*maxLateralDeviation_m * MAX_LATERAL_DEVIATION_ONE_METER_VALUE); - OSEMData.MinPosAccuracyValueIdU16 = VALUE_ID_OSEM_MIN_POSITIONING_ACCURACY; - OSEMData.MinPosAccuracyContentLengthU16 = sizeof (OSEMData.MinPosAccuracyU16); - OSEMData.MinPosAccuracyU16 = (minimumPositioningAccuracy_m == NULL) ? + OSEMData.minPosAccuracyValueID = VALUE_ID_OSEM_MIN_POSITIONING_ACCURACY; + OSEMData.minPosAccuracyContentLength = sizeof (OSEMData.minPosAccuracy); + OSEMData.minPosAccuracy = (minimumPositioningAccuracy_m == NULL) ? MIN_POSITIONING_ACCURACY_NOT_REQUIRED_VALUE : (uint16_t) (*minimumPositioningAccuracy_m * MIN_POSITIONING_ACCURACY_ONE_METER_VALUE); if (debug) { @@ -293,66 +293,66 @@ ssize_t encodeOSEMMessage(const double * latitude_deg, const double * longitude_ "Max way deviation value ID: 0x%x\n\tMax way deviation content length: %u\n\tMax way deviation: %u\n\t" "Max lateral deviation value ID: 0x%x\n\tMax lateral deviation content length: %u\n\t" "Min positioning accuracy value ID: 0x%x\n\tMin positioning accuracy content length: %u\n\tMin positioning accuracy: %u", - OSEMData.LatitudeValueIdU16, OSEMData.LatitudeContentLengthU16, OSEMData.LatitudeI64, - OSEMData.LongitudeValueIdU16, OSEMData.LongitudeContentLengthU16, OSEMData.LongitudeI64, - OSEMData.AltitudeValueIdU16, OSEMData.AltitudeContentLengthU16, OSEMData.AltitudeI32, - OSEMData.DateValueIdU16, OSEMData.DateContentLengthU16, OSEMData.DateU32, - OSEMData.GPSWeekValueIdU16, OSEMData.GPSWeekContentLengthU16, OSEMData.GPSWeekU16, - OSEMData.GPSSOWValueIdU16, OSEMData.GPSSOWContentLengthU16, OSEMData.GPSQmsOfWeekU32, - OSEMData.MaxWayDeviationValueIdU16, OSEMData.MaxWayDeviationContentLengthU16, OSEMData.MaxWayDeviationU16, - OSEMData.MaxLateralDeviationValueIdU16, OSEMData.MaxLateralDeviationContentLengthU16, OSEMData.MaxLateralDeviationU16, - OSEMData.MinPosAccuracyValueIdU16, OSEMData.MinPosAccuracyContentLengthU16, OSEMData.MinPosAccuracyU16); + OSEMData.latitudeValueID, OSEMData.latitudeContentLength, OSEMData.latitude, + OSEMData.longitudeValueID, OSEMData.longitudeContentLength, OSEMData.longitude, + OSEMData.altitudeValueID, OSEMData.altitudeContentLength, OSEMData.altitude, + OSEMData.dateValueID, OSEMData.dateContentLength, OSEMData.date, + OSEMData.GPSWeekValueID, OSEMData.GPSWeekContentLength, OSEMData.GPSWeek, + OSEMData.GPSQmsOfWeekValueID, OSEMData.GPSQmsOfWeekContentLength, OSEMData.GPSQmsOfWeek, + OSEMData.maxWayDeviationValueID, OSEMData.maxWayDeviationContentLength, OSEMData.maxWayDeviation, + OSEMData.maxLateralDeviationValueID, OSEMData.maxLateralDeviationContentLength, OSEMData.maxLateralDeviation, + OSEMData.minPosAccuracyValueID, OSEMData.minPosAccuracyContentLength, OSEMData.minPosAccuracy); } // Switch endianness to little endian for all fields - OSEMData.LatitudeValueIdU16 = htole16(OSEMData.LatitudeValueIdU16); - OSEMData.LatitudeContentLengthU16 = htole16(OSEMData.LatitudeContentLengthU16); - OSEMData.LatitudeI64 = (int64_t) htole48(OSEMData.LatitudeI64); - OSEMData.LongitudeValueIdU16 = htole16(OSEMData.LongitudeValueIdU16); - OSEMData.LongitudeContentLengthU16 = htole16(OSEMData.LongitudeContentLengthU16); - OSEMData.LongitudeI64 = (int64_t) htole48(OSEMData.LongitudeI64); - OSEMData.AltitudeValueIdU16 = htole16(OSEMData.AltitudeValueIdU16); - OSEMData.AltitudeContentLengthU16 = htole16(OSEMData.AltitudeContentLengthU16); - OSEMData.AltitudeI32 = (int32_t) htole32(OSEMData.AltitudeI32); - OSEMData.DateValueIdU16 = htole16(OSEMData.DateValueIdU16); - OSEMData.DateContentLengthU16 = htole16(OSEMData.DateContentLengthU16); - OSEMData.DateU32 = htole32(OSEMData.DateU32); - OSEMData.GPSWeekValueIdU16 = htole16(OSEMData.GPSWeekValueIdU16); - OSEMData.GPSWeekContentLengthU16 = htole16(OSEMData.GPSWeekContentLengthU16); - OSEMData.GPSWeekU16 = htole16(OSEMData.GPSWeekU16); - OSEMData.GPSSOWValueIdU16 = htole16(OSEMData.GPSSOWValueIdU16); - OSEMData.GPSSOWContentLengthU16 = htole16(OSEMData.GPSSOWContentLengthU16); - OSEMData.GPSQmsOfWeekU32 = htole32(OSEMData.GPSQmsOfWeekU32); - OSEMData.MaxWayDeviationValueIdU16 = htole16(OSEMData.MaxWayDeviationValueIdU16); - OSEMData.MaxWayDeviationContentLengthU16 = htole16(OSEMData.MaxWayDeviationContentLengthU16); - OSEMData.MaxWayDeviationU16 = htole16(OSEMData.MaxWayDeviationU16); - OSEMData.MaxLateralDeviationValueIdU16 = htole16(OSEMData.MaxLateralDeviationValueIdU16); - OSEMData.MaxLateralDeviationContentLengthU16 = htole16(OSEMData.MaxLateralDeviationContentLengthU16); - OSEMData.MaxLateralDeviationU16 = htole16(OSEMData.MaxLateralDeviationU16); - OSEMData.MinPosAccuracyValueIdU16 = htole16(OSEMData.MinPosAccuracyValueIdU16); - OSEMData.MinPosAccuracyContentLengthU16 = htole16(OSEMData.MinPosAccuracyContentLengthU16); - OSEMData.MinPosAccuracyU16 = htole16(OSEMData.MinPosAccuracyU16); + OSEMData.latitudeValueID = htole16(OSEMData.latitudeValueID); + OSEMData.latitudeContentLength = htole16(OSEMData.latitudeContentLength); + OSEMData.latitude = (int64_t) htole48(OSEMData.latitude); + OSEMData.longitudeValueID = htole16(OSEMData.longitudeValueID); + OSEMData.longitudeContentLength = htole16(OSEMData.longitudeContentLength); + OSEMData.longitude = (int64_t) htole48(OSEMData.longitude); + OSEMData.altitudeValueID = htole16(OSEMData.altitudeValueID); + OSEMData.altitudeContentLength = htole16(OSEMData.altitudeContentLength); + OSEMData.altitude = (int32_t) htole32(OSEMData.altitude); + OSEMData.dateValueID = htole16(OSEMData.dateValueID); + OSEMData.dateContentLength = htole16(OSEMData.dateContentLength); + OSEMData.date = htole32(OSEMData.date); + OSEMData.GPSWeekValueID = htole16(OSEMData.GPSWeekValueID); + OSEMData.GPSWeekContentLength = htole16(OSEMData.GPSWeekContentLength); + OSEMData.GPSWeek = htole16(OSEMData.GPSWeek); + OSEMData.GPSQmsOfWeekValueID = htole16(OSEMData.GPSQmsOfWeekValueID); + OSEMData.GPSQmsOfWeekContentLength = htole16(OSEMData.GPSQmsOfWeekContentLength); + OSEMData.GPSQmsOfWeek = htole32(OSEMData.GPSQmsOfWeek); + OSEMData.maxWayDeviationValueID = htole16(OSEMData.maxWayDeviationValueID); + OSEMData.maxWayDeviationContentLength = htole16(OSEMData.maxWayDeviationContentLength); + OSEMData.maxWayDeviation = htole16(OSEMData.maxWayDeviation); + OSEMData.maxLateralDeviationValueID = htole16(OSEMData.maxLateralDeviationValueID); + OSEMData.maxLateralDeviationContentLength = htole16(OSEMData.maxLateralDeviationContentLength); + OSEMData.maxLateralDeviation = htole16(OSEMData.maxLateralDeviation); + OSEMData.minPosAccuracyValueID = htole16(OSEMData.minPosAccuracyValueID); + OSEMData.minPosAccuracyContentLength = htole16(OSEMData.minPosAccuracyContentLength); + OSEMData.minPosAccuracy = htole16(OSEMData.minPosAccuracy); // Copy data from OSEM struct into the buffer // Must be done before constructing the footer due to the two 48bit size anomalies - memcpy(p, &OSEMData.Header, sizeof (OSEMData.Header)); - p += sizeof (OSEMData.Header); + memcpy(p, &OSEMData.header, sizeof (OSEMData.header)); + p += sizeof (OSEMData.header); // Special handling of 48 bit value - memcpy(p, &OSEMData.LatitudeValueIdU16, sizeof (OSEMData.LatitudeValueIdU16) + sizeof (OSEMData.LatitudeContentLengthU16)); - p += sizeof (OSEMData.LatitudeValueIdU16) + sizeof (OSEMData.LatitudeContentLengthU16); - memcpy(p, &OSEMData.LatitudeI64, sizeof (OSEMData.LatitudeI64) - SizeDifference64bitTo48bit); - p += sizeof (OSEMData.LatitudeI64) - SizeDifference64bitTo48bit; + memcpy(p, &OSEMData.latitudeValueID, sizeof (OSEMData.latitudeValueID) + sizeof (OSEMData.latitudeContentLength)); + p += sizeof (OSEMData.latitudeValueID) + sizeof (OSEMData.latitudeContentLength); + memcpy(p, &OSEMData.latitude, sizeof (OSEMData.latitude) - SizeDifference64bitTo48bit); + p += sizeof (OSEMData.latitude) - SizeDifference64bitTo48bit; // Special handling of 48 bit value - memcpy(p, &OSEMData.LongitudeValueIdU16, sizeof (OSEMData.LongitudeValueIdU16) + sizeof (OSEMData.LongitudeContentLengthU16)); - p += sizeof (OSEMData.LongitudeValueIdU16) + sizeof (OSEMData.LongitudeContentLengthU16); - memcpy(p, &OSEMData.LongitudeI64, sizeof (OSEMData.LongitudeI64) - SizeDifference64bitTo48bit); - p += sizeof (OSEMData.LongitudeI64) - SizeDifference64bitTo48bit; + memcpy(p, &OSEMData.longitudeValueID, sizeof (OSEMData.longitudeValueID) + sizeof (OSEMData.longitudeContentLength)); + p += sizeof (OSEMData.longitudeValueID) + sizeof (OSEMData.longitudeContentLength); + memcpy(p, &OSEMData.longitude, sizeof (OSEMData.longitude) - SizeDifference64bitTo48bit); + p += sizeof (OSEMData.longitude) - SizeDifference64bitTo48bit; // Copy rest of struct (excluding footer) directly into buffer since no more byte anomalies remain - memcpy(p, &OSEMData.AltitudeI32, sizeof (OSEMData) - sizeof (OSEMData.footer) + memcpy(p, &OSEMData.altitude, sizeof (OSEMData) - sizeof (OSEMData.footer) - (size_t) (p - osemDataBuffer + 2*SizeDifference64bitTo48bit)); p += sizeof (OSEMData) - sizeof (OSEMData.footer) - (size_t) (p - osemDataBuffer + 2*SizeDifference64bitTo48bit); @@ -397,20 +397,20 @@ ssize_t encodeSTRTMessage(const uint32_t startTimeGPSqmsOW, const uint16_t start STRTData.StartTimeContentLengthU16 = htole16(STRTData.StartTimeContentLengthU16); STRTData.StartTimeU32 = startTimeGPSqmsOW; STRTData.StartTimeU32 = htole32(STRTData.StartTimeU32); - STRTData.GPSWeekValueIdU16 = VALUE_ID_STRT_GPS_WEEK; - STRTData.GPSWeekValueIdU16 = htole16(STRTData.GPSWeekValueIdU16); - STRTData.GPSWeekContentLengthU16 = sizeof (STRTData.GPSWeekU16); - STRTData.GPSWeekContentLengthU16 = htole16(STRTData.GPSWeekContentLengthU16); - STRTData.GPSWeekU16 = startGPSWeek; - STRTData.GPSWeekU16 = htole16(STRTData.GPSWeekU16); + STRTData.GPSWeekValueID = VALUE_ID_STRT_GPS_WEEK; + STRTData.GPSWeekValueID = htole16(STRTData.GPSWeekValueID); + STRTData.GPSWeekContentLength = sizeof (STRTData.GPSWeek); + STRTData.GPSWeekContentLength = htole16(STRTData.GPSWeekContentLength); + STRTData.GPSWeek = startGPSWeek; + STRTData.GPSWeek = htole16(STRTData.GPSWeek); if (debug) { LogPrint("STRT message:\n\tGPS second of week value ID: 0x%x\n\t" "GPS second of week content length: %u\n\tGPS second of week: %u [¼ ms]\n\t" "GPS week value ID: 0x%x\n\tGPS week content length: %u\n\t" "GPS week: %u", STRTData.StartTimeValueIdU16, STRTData.StartTimeContentLengthU16, - STRTData.StartTimeU32, STRTData.GPSWeekValueIdU16, STRTData.GPSWeekContentLengthU16, - STRTData.GPSWeekU16); + STRTData.StartTimeU32, STRTData.GPSWeekValueID, STRTData.GPSWeekContentLength, + STRTData.GPSWeek); } STRTData.footer = buildISOFooter(&STRTData, sizeof (STRTType), debug); From 4d976194fb5de15d5f9e149d656e49955a105b82 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 24 Jan 2020 10:58:40 +0100 Subject: [PATCH 127/523] Corrected a few warnings --- server/src/iso22133.c | 52 ++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 51 insertions(+), 1 deletion(-) diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 059080e15..45548aa9c 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -129,6 +129,10 @@ ISOMessageReturnValue decodeISOFooter(const char *MessageBuffer, const size_t le memcpy(&FooterData->Crc, MessageBuffer, sizeof (FooterData->Crc)); FooterData->Crc = le16toh(FooterData->Crc); + if (debug) { + LogPrint("Decoded ISO footer:\n\tCRC: 0x%x", FooterData->Crc); + } + // TODO: check on CRC return MESSAGE_OK; } @@ -585,6 +589,30 @@ ISOMessageReturnValue MONRToASCII(const MONRType * MONRData, char *asciiBuffer, sprintf(asciiBuffer + strlen(asciiBuffer), "%u;%u;%u;%u;", MONRData->driveDirection, MONRData->state, MONRData->readyToArm, MONRData->errorStatus); + if (debug) { + LogPrint("Converted MONR to string:\n\tMONR:\n\t\t" + "Value ID = 0x%x\n\t\t" + "Content length = %u\n\t\t" + "GPS quarter millisecond of week = %u\n\t\t" + "X position = %d\n\t\t" + "Y position = %d\n\t\t" + "Z position = %d\n\t\t" + "Heading = %d\n\t\t" + "Longitudinal speed = %d\n\t\t" + "Lateral speed = %d\n\t\t" + "Longitudinal acceleration = %d\n\t\t" + "Lateral acceleration = %d\n\t\t" + "Drive direction = %d\n\t\t" + "State = %d\n\t\t" + "Ready to arm = %d\n\t\t" + "Error status = %d\n\t" + "String:\n\t\t<%s>", MONRData->monrStructValueID, MONRData->monrStructContentLength, + MONRData->gpsQmsOfWeek, MONRData->xPosition, MONRData->yPosition, MONRData->zPosition, + MONRData->heading, MONRData->longitudinalSpeed, MONRData->lateralSpeed, MONRData->longitudinalAcc, + MONRData->lateralAcc, MONRData->driveDirection, MONRData->state, MONRData->readyToArm, + MONRData->errorStatus, asciiBuffer); + } + return MESSAGE_OK; } @@ -598,7 +626,6 @@ ISOMessageReturnValue MONRToASCII(const MONRType * MONRData, char *asciiBuffer, */ ISOMessageReturnValue ASCIIToMONR(const char *asciiBuffer, MONRType * MONRData, const char debug) { - const size_t bufferLength = strlen(asciiBuffer); const char *token; const char delim[] = ";"; const int NumberBaseDecimal = 10; @@ -647,5 +674,28 @@ ISOMessageReturnValue ASCIIToMONR(const char *asciiBuffer, MONRType * MONRData, token = strtok(NULL, delim); MONRData->errorStatus = (uint8_t) strtoul(token, NULL, NumberBaseDecimal); + if (debug) { + LogPrint("Converted string to MONR:\n\tString:\n\t\t<%s>\n\tMONR:\n\t\t" + "Value ID = 0x%x\n\t\t" + "Content length = %u\n\t\t" + "GPS quarter millisecond of week = %u\n\t\t" + "X position = %d\n\t\t" + "Y position = %d\n\t\t" + "Z position = %d\n\t\t" + "Heading = %d\n\t\t" + "Longitudinal speed = %d\n\t\t" + "Lateral speed = %d\n\t\t" + "Longitudinal acceleration = %d\n\t\t" + "Lateral acceleration = %d\n\t\t" + "Drive direction = %d\n\t\t" + "State = %d\n\t\t" + "Ready to arm = %d\n\t\t" + "Error status = %d", asciiBuffer, MONRData->monrStructValueID, MONRData->monrStructContentLength, + MONRData->gpsQmsOfWeek, MONRData->xPosition, MONRData->yPosition, MONRData->zPosition, + MONRData->heading, MONRData->longitudinalSpeed, MONRData->lateralSpeed, MONRData->longitudinalAcc, + MONRData->lateralAcc, MONRData->driveDirection, MONRData->state, MONRData->readyToArm, + MONRData->errorStatus); + } + return MESSAGE_OK; } From 61011340cc205932ba995c0122f3937f7d2c46dd Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 24 Jan 2020 11:09:08 +0100 Subject: [PATCH 128/523] Added function comments --- server/src/iso22133.c | 22 +++++++++++++++++++++- 1 file changed, 21 insertions(+), 1 deletion(-) diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 45548aa9c..cb1e31acb 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -164,6 +164,13 @@ HeaderType buildISOHeader(ISOMessageID id, uint32_t messageLength, const char de return header; } +/*! + * \brief buildISOFooter Constructs a footer for an ISO message + * \param message Pointer to start of message header + * \param messageSize Size of the entire message including header and footer + * \param debug Flag for enabling debugging + * \return A struct containing ISO footer data + */ FooterType buildISOFooter(const void *message, const size_t messageSize, const char debug) { FooterType footer; @@ -220,7 +227,20 @@ ISOMessageID getISOMessageType(const char *messageData, const size_t length, con } } - +/*! + * \brief encodeOSEMMessage Creates an OSEM message and writes it into a buffer based on supplied values. All values are passed as pointers and + * passing them as NULL causes the OSEM message to contain a default value for that field (a value representing "unavailable" or similar). + * \param latitude_deg Latitude in degrees of the test origin + * \param longitude_deg Longitude in degrees of the test origin + * \param altitude_m Altitude in meters above sea level of the test origin + * \param maxWayDeviation_m Maximum allowed deviation from target trajectory point, in meters + * \param maxLateralDeviation_m Maximum lateral deviation from trajectory allowed, in meters + * \param minimumPositioningAccuracy_m Minimum positioning accuracy required of the object + * \param osemDataBuffer Buffer to which OSEM message is to be written + * \param bufferLength Size of the buffer to which OSEM message is to be written + * \param debug Flag for enabling debugging + * \return Number of bytes written to the buffer, or -1 in case of an error + */ ssize_t encodeOSEMMessage(const double * latitude_deg, const double * longitude_deg, const float * altitude_m, const float * maxWayDeviation_m, const float * maxLateralDeviation_m, const float * minimumPositioningAccuracy_m, char * osemDataBuffer, const size_t bufferLength, const char debug) { const char SizeDifference64bitTo48bit = 2; From 91df6d8f9ef0857d6beeb0d5103f3b9cf8f111a3 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 24 Jan 2020 11:12:03 +0100 Subject: [PATCH 129/523] Made debug printout for STRT work with endianness switching --- server/src/iso22133.c | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/server/src/iso22133.c b/server/src/iso22133.c index cb1e31acb..1d72ca7c4 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -414,19 +414,13 @@ ssize_t encodeSTRTMessage(const uint32_t startTimeGPSqmsOW, const uint16_t start STRTData.header = buildISOHeader(MESSAGE_ID_STRT, sizeof (STRTType) - sizeof (HeaderType) - sizeof (FooterType), debug); - // Fill contents, ensuring to swap from host endianness to little endian + // Fill contents STRTData.StartTimeValueIdU16 = VALUE_ID_STRT_GPS_QMS_OF_WEEK; - STRTData.StartTimeValueIdU16 = htole16(STRTData.StartTimeValueIdU16); STRTData.StartTimeContentLengthU16 = sizeof (STRTData.StartTimeU32); - STRTData.StartTimeContentLengthU16 = htole16(STRTData.StartTimeContentLengthU16); STRTData.StartTimeU32 = startTimeGPSqmsOW; - STRTData.StartTimeU32 = htole32(STRTData.StartTimeU32); STRTData.GPSWeekValueID = VALUE_ID_STRT_GPS_WEEK; - STRTData.GPSWeekValueID = htole16(STRTData.GPSWeekValueID); STRTData.GPSWeekContentLength = sizeof (STRTData.GPSWeek); - STRTData.GPSWeekContentLength = htole16(STRTData.GPSWeekContentLength); STRTData.GPSWeek = startGPSWeek; - STRTData.GPSWeek = htole16(STRTData.GPSWeek); if (debug) { LogPrint("STRT message:\n\tGPS second of week value ID: 0x%x\n\t" @@ -437,6 +431,15 @@ ssize_t encodeSTRTMessage(const uint32_t startTimeGPSqmsOW, const uint16_t start STRTData.GPSWeek); } + // Swap from host endianness to little endian + STRTData.StartTimeValueIdU16 = htole16(STRTData.StartTimeValueIdU16); + STRTData.StartTimeContentLengthU16 = htole16(STRTData.StartTimeContentLengthU16); + STRTData.StartTimeU32 = htole32(STRTData.StartTimeU32); + STRTData.GPSWeekValueID = htole16(STRTData.GPSWeekValueID); + STRTData.GPSWeekContentLength = htole16(STRTData.GPSWeekContentLength); + STRTData.GPSWeek = htole16(STRTData.GPSWeek); + + // Construct footer STRTData.footer = buildISOFooter(&STRTData, sizeof (STRTType), debug); memcpy(strtDataBuffer, &STRTData, sizeof (STRTType)); From f4fcc294ee593356a51cb4d094ca7e0e2d6e3cf9 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 24 Jan 2020 12:35:03 +0100 Subject: [PATCH 130/523] Changed build STRT function to use a struct timeval instead of specific values --- server/inc/iso22133.h | 4 ++-- server/src/iso22133.c | 9 ++++----- server/src/objectcontrol.c | 3 +-- 3 files changed, 7 insertions(+), 9 deletions(-) diff --git a/server/inc/iso22133.h b/server/inc/iso22133.h index 7107f09e8..c1e11312a 100644 --- a/server/inc/iso22133.h +++ b/server/inc/iso22133.h @@ -13,6 +13,7 @@ extern "C" { #include #include #include +#include #define ISO_PROTOCOL_VERSION 2 #define ACK_REQ 0 @@ -380,8 +381,7 @@ typedef enum { } ISOMessageID; ISOMessageReturnValue decodeMONRMessage(const char * MonrData, const size_t length, MONRType * MONRData, const char debug); -ssize_t encodeSTRTMessage(const uint32_t startTimeGPSqmsOW, const uint16_t startGPSWeek, char * strtDataBuffer, - const size_t bufferLength, const char debug); +ssize_t encodeSTRTMessage(const struct timeval timeOfStart, char * strtDataBuffer, const size_t bufferLength, const char debug); ssize_t encodeOSEMMessage(const double * latitude_deg, const double * longitude_deg, const float * altitude_m, const float * maxPositionDeviation_m, const float * maxLateralDeviation_m, const float * minimumPositioningAccuracy_m, char * osemDataBuffer, const size_t bufferLength, const char debug); ISOMessageReturnValue MONRToASCII(const MONRType * MONRData, char * asciiBuffer, const size_t bufferLength, const char debug); ISOMessageReturnValue ASCIIToMONR(const char * asciiBuffer, MONRType * MONRData, const char debug); diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 1d72ca7c4..3894c767f 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -391,14 +391,13 @@ ssize_t encodeOSEMMessage(const double * latitude_deg, const double * longitude_ /*! * \brief encodeSTRTMessage Constructs an ISO STRT message based on start time parameters - * \param startTimeGPSqmsOW Quarter milliseconds of week when recipient of message shall start - * \param startGPSWeek GPS week when recipient shall start + * \param timeOfStart Time when test shall start * \param strtDataBuffer Data buffer in which to place encoded STRT message * \param bufferLength Size of data buffer in which to place encoded STRT message * \param debug Flag for enabling debugging * \return number of bytes written to the data buffer, or -1 if an error occurred */ -ssize_t encodeSTRTMessage(const uint32_t startTimeGPSqmsOW, const uint16_t startGPSWeek, char *strtDataBuffer, +ssize_t encodeSTRTMessage(const struct timeval timeOfStart, char *strtDataBuffer, const size_t bufferLength, const char debug) { STRTType STRTData; @@ -417,10 +416,10 @@ ssize_t encodeSTRTMessage(const uint32_t startTimeGPSqmsOW, const uint16_t start // Fill contents STRTData.StartTimeValueIdU16 = VALUE_ID_STRT_GPS_QMS_OF_WEEK; STRTData.StartTimeContentLengthU16 = sizeof (STRTData.StartTimeU32); - STRTData.StartTimeU32 = startTimeGPSqmsOW; + STRTData.StartTimeU32 = TimeGetAsGPSqmsOfWeek(&timeOfStart); STRTData.GPSWeekValueID = VALUE_ID_STRT_GPS_WEEK; STRTData.GPSWeekContentLength = sizeof (STRTData.GPSWeek); - STRTData.GPSWeek = startGPSWeek; + STRTData.GPSWeek = TimeGetAsGPSweek(&timeOfStart); if (debug) { LogPrint("STRT message:\n\tGPS second of week value ID: 0x%x\n\t" diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index f1502fc2a..3cabf53da 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -620,8 +620,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { TimeSetToUTCms(&startDelay, (int64_t) strtoul(MiscPtr + 1, NULL, 10)); timeradd(&startTime, &startDelay, &startTime); MessageLength = - (int)encodeSTRTMessage(TimeGetAsGPSqmsOfWeek(&startTime), TimeGetAsGPSweek(&startTime), - MessageBuffer, sizeof (MessageBuffer), 0); + (int)encodeSTRTMessage(startTime, MessageBuffer, sizeof (MessageBuffer), 0); ASPData.MTSPU32 = 0; ASPData.TimeToSyncPointDbl = 0; From 23409c919f2d8fa8f030b8be01aced9659c4548d Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Fri, 24 Jan 2020 12:48:45 +0100 Subject: [PATCH 131/523] ran formatting script --- server/src/objectcontrol.c | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 83541bdeb..adcca89e9 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -846,22 +846,22 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { (!(1 & DisconnectU8)) * 3); break; case EADDRINUSE: - util_error("[ObjectControl] Local address/port already in use"); + util_error("[ObjectControl] Local address/port already in use"); break; case EALREADY: - util_error("[ObjectControl] Previous connection attempt still in progress"); + util_error("[ObjectControl] Previous connection attempt still in progress"); break; case EISCONN: - util_error("[ObjectControl] Socket is already connected"); + util_error("[ObjectControl] Socket is already connected"); break; case ENETUNREACH: - util_error("[ObjectControl] Network unreachable"); + util_error("[ObjectControl] Network unreachable"); break; case ETIMEDOUT: - util_error("[ObjectControl] Connection timed out"); + util_error("[ObjectControl] Connection timed out"); break; default: - util_error("ERR: Failed to connect to control socket"); + util_error("ERR: Failed to connect to control socket"); break; } @@ -877,7 +877,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { } - } while (iExit == 0 && iResult < 0 && DisconnectU8 == 0); + } while (iExit == 0 && iResult < 0 && DisconnectU8 == 0); if (iResult >= 0) { /* Send OSEM command in mq so that we get some information like GPSweek, origin (latitude,logitude,altitude in gps coordinates) */ From 8157b62370869733f6c691fe6e1a99e8639db475 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 24 Jan 2020 12:52:30 +0100 Subject: [PATCH 132/523] Made time in STRT message optional --- server/inc/iso22133.h | 2 +- server/src/iso22133.c | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/server/inc/iso22133.h b/server/inc/iso22133.h index c1e11312a..73c4227e0 100644 --- a/server/inc/iso22133.h +++ b/server/inc/iso22133.h @@ -381,7 +381,7 @@ typedef enum { } ISOMessageID; ISOMessageReturnValue decodeMONRMessage(const char * MonrData, const size_t length, MONRType * MONRData, const char debug); -ssize_t encodeSTRTMessage(const struct timeval timeOfStart, char * strtDataBuffer, const size_t bufferLength, const char debug); +ssize_t encodeSTRTMessage(const struct timeval* timeOfStart, char * strtDataBuffer, const size_t bufferLength, const char debug); ssize_t encodeOSEMMessage(const double * latitude_deg, const double * longitude_deg, const float * altitude_m, const float * maxPositionDeviation_m, const float * maxLateralDeviation_m, const float * minimumPositioningAccuracy_m, char * osemDataBuffer, const size_t bufferLength, const char debug); ISOMessageReturnValue MONRToASCII(const MONRType * MONRData, char * asciiBuffer, const size_t bufferLength, const char debug); ISOMessageReturnValue ASCIIToMONR(const char * asciiBuffer, MONRType * MONRData, const char debug); diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 3894c767f..be53996e0 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -391,13 +391,13 @@ ssize_t encodeOSEMMessage(const double * latitude_deg, const double * longitude_ /*! * \brief encodeSTRTMessage Constructs an ISO STRT message based on start time parameters - * \param timeOfStart Time when test shall start + * \param timeOfStart Time when test shall start, a value of NULL indicates that the time is not known * \param strtDataBuffer Data buffer in which to place encoded STRT message * \param bufferLength Size of data buffer in which to place encoded STRT message * \param debug Flag for enabling debugging * \return number of bytes written to the data buffer, or -1 if an error occurred */ -ssize_t encodeSTRTMessage(const struct timeval timeOfStart, char *strtDataBuffer, +ssize_t encodeSTRTMessage(const struct timeval * timeOfStart, char *strtDataBuffer, const size_t bufferLength, const char debug) { STRTType STRTData; @@ -416,10 +416,10 @@ ssize_t encodeSTRTMessage(const struct timeval timeOfStart, char *strtDataBuffer // Fill contents STRTData.StartTimeValueIdU16 = VALUE_ID_STRT_GPS_QMS_OF_WEEK; STRTData.StartTimeContentLengthU16 = sizeof (STRTData.StartTimeU32); - STRTData.StartTimeU32 = TimeGetAsGPSqmsOfWeek(&timeOfStart); + STRTData.StartTimeU32 = timeOfStart == NULL ? GPS_SECOND_OF_WEEK_UNAVAILABLE_VALUE : TimeGetAsGPSqmsOfWeek(&timeOfStart); STRTData.GPSWeekValueID = VALUE_ID_STRT_GPS_WEEK; STRTData.GPSWeekContentLength = sizeof (STRTData.GPSWeek); - STRTData.GPSWeek = TimeGetAsGPSweek(&timeOfStart); + STRTData.GPSWeek = timeOfStart == NULL ? GPS_WEEK_UNAVAILABLE_VALUE : TimeGetAsGPSweek(&timeOfStart); if (debug) { LogPrint("STRT message:\n\tGPS second of week value ID: 0x%x\n\t" From 421e8f8df4c77dd06d74bc57a680d28bda0badfa Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 24 Jan 2020 12:52:50 +0100 Subject: [PATCH 133/523] Ran code formatter --- server/src/iso22133.c | 105 +++++++++++++++++++++---------------- server/src/objectcontrol.c | 9 ++-- 2 files changed, 65 insertions(+), 49 deletions(-) diff --git a/server/src/iso22133.c b/server/src/iso22133.c index be53996e0..d66ec0ae5 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -241,48 +241,53 @@ ISOMessageID getISOMessageType(const char *messageData, const size_t length, con * \param debug Flag for enabling debugging * \return Number of bytes written to the buffer, or -1 in case of an error */ -ssize_t encodeOSEMMessage(const double * latitude_deg, const double * longitude_deg, const float * altitude_m, const float * maxWayDeviation_m, const float * maxLateralDeviation_m, const float * minimumPositioningAccuracy_m, char * osemDataBuffer, const size_t bufferLength, const char debug) { +ssize_t encodeOSEMMessage(const double *latitude_deg, const double *longitude_deg, const float *altitude_m, + const float *maxWayDeviation_m, const float *maxLateralDeviation_m, + const float *minimumPositioningAccuracy_m, char *osemDataBuffer, + const size_t bufferLength, const char debug) { const char SizeDifference64bitTo48bit = 2; OSEMType OSEMData; struct timeval currentTime; - struct tm * printableTime; + struct tm *printableTime; char *p = osemDataBuffer; + TimeSetToCurrentSystemTime(¤tTime); printableTime = localtime(¤tTime.tv_sec); memset(osemDataBuffer, 0, bufferLength); // If buffer too small to hold OSEM data, generate an error - if (bufferLength < sizeof (OSEMData) - 2* SizeDifference64bitTo48bit) { + if (bufferLength < sizeof (OSEMData) - 2 * SizeDifference64bitTo48bit) { LogMessage(LOG_LEVEL_ERROR, "Buffer too small to hold necessary OSEM data"); return -1; } // Build header, and account for the two values which are 48 bit in the message OSEMData.header = buildISOHeader(MESSAGE_ID_OSEM, sizeof (OSEMData) - - sizeof (OSEMData.header) - sizeof (OSEMData.footer) - 2*SizeDifference64bitTo48bit, debug); + - sizeof (OSEMData.header) - sizeof (OSEMData.footer) - + 2 * SizeDifference64bitTo48bit, debug); // Fill the OSEM struct with relevant values OSEMData.latitudeValueID = VALUE_ID_OSEM_LATITUDE; OSEMData.latitudeContentLength = sizeof (OSEMData.latitude) - SizeDifference64bitTo48bit; OSEMData.latitude = (latitude_deg == NULL) ? - LATITUDE_UNAVAILABLE_VALUE : (int64_t) (*latitude_deg * LATITUDE_ONE_DEGREE_VALUE); + LATITUDE_UNAVAILABLE_VALUE : (int64_t) (*latitude_deg * LATITUDE_ONE_DEGREE_VALUE); OSEMData.longitudeValueID = VALUE_ID_OSEM_LONGITUDE; OSEMData.longitudeContentLength = sizeof (OSEMData.longitude) - SizeDifference64bitTo48bit; OSEMData.longitude = (longitude_deg == NULL) ? - LONGITUDE_UNAVAILABLE_VALUE : (int64_t) (*longitude_deg * LONGITUDE_ONE_DEGREE_VALUE); + LONGITUDE_UNAVAILABLE_VALUE : (int64_t) (*longitude_deg * LONGITUDE_ONE_DEGREE_VALUE); OSEMData.altitudeValueID = VALUE_ID_OSEM_ALTITUDE; OSEMData.altitudeContentLength = sizeof (OSEMData.altitude); OSEMData.altitude = (altitude_m == NULL) ? - ALTITUDE_UNAVAILABLE_VALUE : (int32_t) (*altitude_m * ALTITUDE_ONE_METER_VALUE); + ALTITUDE_UNAVAILABLE_VALUE : (int32_t) (*altitude_m * ALTITUDE_ONE_METER_VALUE); OSEMData.dateValueID = VALUE_ID_OSEM_DATE; OSEMData.dateContentLength = sizeof (OSEMData.dateContentLength); OSEMData.date = (uint32_t) ((printableTime->tm_year + 1900) * 10000 + (printableTime->tm_mon + 1) * 100 - + (printableTime->tm_mday)); + + (printableTime->tm_mday)); OSEMData.GPSWeekValueID = VALUE_ID_OSEM_GPS_WEEK; OSEMData.GPSWeekContentLength = sizeof (OSEMData.GPSWeekContentLength); @@ -295,37 +300,42 @@ ssize_t encodeOSEMMessage(const double * latitude_deg, const double * longitude_ OSEMData.maxWayDeviationValueID = VALUE_ID_OSEM_MAX_WAY_DEVIATION; OSEMData.maxWayDeviationContentLength = sizeof (OSEMData.maxWayDeviation); OSEMData.maxWayDeviation = (maxWayDeviation_m == NULL) ? - MAX_WAY_DEVIATION_UNAVAILABLE_VALUE : (uint16_t) (*maxWayDeviation_m * MAX_WAY_DEVIATION_ONE_METER_VALUE); + MAX_WAY_DEVIATION_UNAVAILABLE_VALUE : (uint16_t) (*maxWayDeviation_m * + MAX_WAY_DEVIATION_ONE_METER_VALUE); OSEMData.maxLateralDeviationValueID = VALUE_ID_OSEM_MAX_LATERAL_DEVIATION; OSEMData.maxLateralDeviationContentLength = sizeof (OSEMData.maxLateralDeviation); OSEMData.maxLateralDeviation = (maxLateralDeviation_m == NULL) ? - MAX_LATERAL_DEVIATION_UNAVAILABLE_VALUE : (uint16_t) (*maxLateralDeviation_m * MAX_LATERAL_DEVIATION_ONE_METER_VALUE); + MAX_LATERAL_DEVIATION_UNAVAILABLE_VALUE : (uint16_t) (*maxLateralDeviation_m * + MAX_LATERAL_DEVIATION_ONE_METER_VALUE); OSEMData.minPosAccuracyValueID = VALUE_ID_OSEM_MIN_POSITIONING_ACCURACY; OSEMData.minPosAccuracyContentLength = sizeof (OSEMData.minPosAccuracy); OSEMData.minPosAccuracy = (minimumPositioningAccuracy_m == NULL) ? - MIN_POSITIONING_ACCURACY_NOT_REQUIRED_VALUE : (uint16_t) (*minimumPositioningAccuracy_m * MIN_POSITIONING_ACCURACY_ONE_METER_VALUE); + MIN_POSITIONING_ACCURACY_NOT_REQUIRED_VALUE : (uint16_t) (*minimumPositioningAccuracy_m * + MIN_POSITIONING_ACCURACY_ONE_METER_VALUE); if (debug) { - LogPrint("OSEM message:\n\tLatitude value ID: 0x%x\n\tLatitude content length: %u\n\tLatitude: %ld [100 nanodegrees]\n\t" - "Longitude value ID: 0x%x\n\tLongitude content length: %u\n\tLongitude: %ld [100 nanodegrees]\n\t" - "Altitude value ID: 0x%x\n\tAltitude content length: %u\n\tAltitude: %d [cm]" - "Date value ID: 0x%x\n\tDate content length: %u\n\tDate: %u\n\t" - "GPS week value ID: 0x%x\n\tGPS week content length: %u\n\tGPS week: %u\n\t" - "GPS second of week value ID: 0x%x\n\tGPS second of week content length: %u\n\tGPS second of week: %u [¼ ms]\n\t" - "Max way deviation value ID: 0x%x\n\tMax way deviation content length: %u\n\tMax way deviation: %u\n\t" - "Max lateral deviation value ID: 0x%x\n\tMax lateral deviation content length: %u\n\t" - "Min positioning accuracy value ID: 0x%x\n\tMin positioning accuracy content length: %u\n\tMin positioning accuracy: %u", - OSEMData.latitudeValueID, OSEMData.latitudeContentLength, OSEMData.latitude, - OSEMData.longitudeValueID, OSEMData.longitudeContentLength, OSEMData.longitude, - OSEMData.altitudeValueID, OSEMData.altitudeContentLength, OSEMData.altitude, - OSEMData.dateValueID, OSEMData.dateContentLength, OSEMData.date, - OSEMData.GPSWeekValueID, OSEMData.GPSWeekContentLength, OSEMData.GPSWeek, - OSEMData.GPSQmsOfWeekValueID, OSEMData.GPSQmsOfWeekContentLength, OSEMData.GPSQmsOfWeek, - OSEMData.maxWayDeviationValueID, OSEMData.maxWayDeviationContentLength, OSEMData.maxWayDeviation, - OSEMData.maxLateralDeviationValueID, OSEMData.maxLateralDeviationContentLength, OSEMData.maxLateralDeviation, - OSEMData.minPosAccuracyValueID, OSEMData.minPosAccuracyContentLength, OSEMData.minPosAccuracy); + LogPrint + ("OSEM message:\n\tLatitude value ID: 0x%x\n\tLatitude content length: %u\n\tLatitude: %ld [100 nanodegrees]\n\t" + "Longitude value ID: 0x%x\n\tLongitude content length: %u\n\tLongitude: %ld [100 nanodegrees]\n\t" + "Altitude value ID: 0x%x\n\tAltitude content length: %u\n\tAltitude: %d [cm]" + "Date value ID: 0x%x\n\tDate content length: %u\n\tDate: %u\n\t" + "GPS week value ID: 0x%x\n\tGPS week content length: %u\n\tGPS week: %u\n\t" + "GPS second of week value ID: 0x%x\n\tGPS second of week content length: %u\n\tGPS second of week: %u [¼ ms]\n\t" + "Max way deviation value ID: 0x%x\n\tMax way deviation content length: %u\n\tMax way deviation: %u\n\t" + "Max lateral deviation value ID: 0x%x\n\tMax lateral deviation content length: %u\n\t" + "Min positioning accuracy value ID: 0x%x\n\tMin positioning accuracy content length: %u\n\tMin positioning accuracy: %u", + OSEMData.latitudeValueID, OSEMData.latitudeContentLength, OSEMData.latitude, + OSEMData.longitudeValueID, OSEMData.longitudeContentLength, OSEMData.longitude, + OSEMData.altitudeValueID, OSEMData.altitudeContentLength, OSEMData.altitude, + OSEMData.dateValueID, OSEMData.dateContentLength, OSEMData.date, OSEMData.GPSWeekValueID, + OSEMData.GPSWeekContentLength, OSEMData.GPSWeek, OSEMData.GPSQmsOfWeekValueID, + OSEMData.GPSQmsOfWeekContentLength, OSEMData.GPSQmsOfWeek, OSEMData.maxWayDeviationValueID, + OSEMData.maxWayDeviationContentLength, OSEMData.maxWayDeviation, + OSEMData.maxLateralDeviationValueID, OSEMData.maxLateralDeviationContentLength, + OSEMData.maxLateralDeviation, OSEMData.minPosAccuracyValueID, + OSEMData.minPosAccuracyContentLength, OSEMData.minPosAccuracy); } // Switch endianness to little endian for all fields @@ -364,27 +374,31 @@ ssize_t encodeOSEMMessage(const double * latitude_deg, const double * longitude_ p += sizeof (OSEMData.header); // Special handling of 48 bit value - memcpy(p, &OSEMData.latitudeValueID, sizeof (OSEMData.latitudeValueID) + sizeof (OSEMData.latitudeContentLength)); + memcpy(p, &OSEMData.latitudeValueID, + sizeof (OSEMData.latitudeValueID) + sizeof (OSEMData.latitudeContentLength)); p += sizeof (OSEMData.latitudeValueID) + sizeof (OSEMData.latitudeContentLength); memcpy(p, &OSEMData.latitude, sizeof (OSEMData.latitude) - SizeDifference64bitTo48bit); p += sizeof (OSEMData.latitude) - SizeDifference64bitTo48bit; // Special handling of 48 bit value - memcpy(p, &OSEMData.longitudeValueID, sizeof (OSEMData.longitudeValueID) + sizeof (OSEMData.longitudeContentLength)); + memcpy(p, &OSEMData.longitudeValueID, + sizeof (OSEMData.longitudeValueID) + sizeof (OSEMData.longitudeContentLength)); p += sizeof (OSEMData.longitudeValueID) + sizeof (OSEMData.longitudeContentLength); memcpy(p, &OSEMData.longitude, sizeof (OSEMData.longitude) - SizeDifference64bitTo48bit); p += sizeof (OSEMData.longitude) - SizeDifference64bitTo48bit; // Copy rest of struct (excluding footer) directly into buffer since no more byte anomalies remain memcpy(p, &OSEMData.altitude, sizeof (OSEMData) - sizeof (OSEMData.footer) - - (size_t) (p - osemDataBuffer + 2*SizeDifference64bitTo48bit)); - p += sizeof (OSEMData) - sizeof (OSEMData.footer) - (size_t) (p - osemDataBuffer + 2*SizeDifference64bitTo48bit); + - (size_t) (p - osemDataBuffer + 2 * SizeDifference64bitTo48bit)); + p += sizeof (OSEMData) - sizeof (OSEMData.footer) - (size_t) (p - osemDataBuffer + + 2 * SizeDifference64bitTo48bit); // Build footer - OSEMData.footer = buildISOFooter(osemDataBuffer, sizeof (OSEMType) - 2*SizeDifference64bitTo48bit, debug); + OSEMData.footer = + buildISOFooter(osemDataBuffer, sizeof (OSEMType) - 2 * SizeDifference64bitTo48bit, debug); memcpy(p, &OSEMData.footer, sizeof (OSEMData.footer)); - return sizeof (OSEMType) - 2*SizeDifference64bitTo48bit; + return sizeof (OSEMType) - 2 * SizeDifference64bitTo48bit; } @@ -397,7 +411,7 @@ ssize_t encodeOSEMMessage(const double * latitude_deg, const double * longitude_ * \param debug Flag for enabling debugging * \return number of bytes written to the data buffer, or -1 if an error occurred */ -ssize_t encodeSTRTMessage(const struct timeval * timeOfStart, char *strtDataBuffer, +ssize_t encodeSTRTMessage(const struct timeval *timeOfStart, char *strtDataBuffer, const size_t bufferLength, const char debug) { STRTType STRTData; @@ -416,7 +430,8 @@ ssize_t encodeSTRTMessage(const struct timeval * timeOfStart, char *strtDataBuff // Fill contents STRTData.StartTimeValueIdU16 = VALUE_ID_STRT_GPS_QMS_OF_WEEK; STRTData.StartTimeContentLengthU16 = sizeof (STRTData.StartTimeU32); - STRTData.StartTimeU32 = timeOfStart == NULL ? GPS_SECOND_OF_WEEK_UNAVAILABLE_VALUE : TimeGetAsGPSqmsOfWeek(&timeOfStart); + STRTData.StartTimeU32 = + timeOfStart == NULL ? GPS_SECOND_OF_WEEK_UNAVAILABLE_VALUE : TimeGetAsGPSqmsOfWeek(&timeOfStart); STRTData.GPSWeekValueID = VALUE_ID_STRT_GPS_WEEK; STRTData.GPSWeekContentLength = sizeof (STRTData.GPSWeek); STRTData.GPSWeek = timeOfStart == NULL ? GPS_WEEK_UNAVAILABLE_VALUE : TimeGetAsGPSweek(&timeOfStart); @@ -630,9 +645,9 @@ ISOMessageReturnValue MONRToASCII(const MONRType * MONRData, char *asciiBuffer, "Error status = %d\n\t" "String:\n\t\t<%s>", MONRData->monrStructValueID, MONRData->monrStructContentLength, MONRData->gpsQmsOfWeek, MONRData->xPosition, MONRData->yPosition, MONRData->zPosition, - MONRData->heading, MONRData->longitudinalSpeed, MONRData->lateralSpeed, MONRData->longitudinalAcc, - MONRData->lateralAcc, MONRData->driveDirection, MONRData->state, MONRData->readyToArm, - MONRData->errorStatus, asciiBuffer); + MONRData->heading, MONRData->longitudinalSpeed, MONRData->lateralSpeed, + MONRData->longitudinalAcc, MONRData->lateralAcc, MONRData->driveDirection, MONRData->state, + MONRData->readyToArm, MONRData->errorStatus, asciiBuffer); } return MESSAGE_OK; @@ -712,11 +727,11 @@ ISOMessageReturnValue ASCIIToMONR(const char *asciiBuffer, MONRType * MONRData, "Drive direction = %d\n\t\t" "State = %d\n\t\t" "Ready to arm = %d\n\t\t" - "Error status = %d", asciiBuffer, MONRData->monrStructValueID, MONRData->monrStructContentLength, - MONRData->gpsQmsOfWeek, MONRData->xPosition, MONRData->yPosition, MONRData->zPosition, - MONRData->heading, MONRData->longitudinalSpeed, MONRData->lateralSpeed, MONRData->longitudinalAcc, - MONRData->lateralAcc, MONRData->driveDirection, MONRData->state, MONRData->readyToArm, - MONRData->errorStatus); + "Error status = %d", asciiBuffer, MONRData->monrStructValueID, + MONRData->monrStructContentLength, MONRData->gpsQmsOfWeek, MONRData->xPosition, + MONRData->yPosition, MONRData->zPosition, MONRData->heading, MONRData->longitudinalSpeed, + MONRData->lateralSpeed, MONRData->longitudinalAcc, MONRData->lateralAcc, + MONRData->driveDirection, MONRData->state, MONRData->readyToArm, MONRData->errorStatus); } return MESSAGE_OK; diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index e8001521a..51c7358e3 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -622,8 +622,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { TimeSetToUTCms(&startTime, (int64_t) strtoul(MiscPtr, &MiscPtr, 10)); TimeSetToUTCms(&startDelay, (int64_t) strtoul(MiscPtr + 1, NULL, 10)); timeradd(&startTime, &startDelay, &startTime); - MessageLength = - (int)encodeSTRTMessage(startTime, MessageBuffer, sizeof (MessageBuffer), 0); + MessageLength = (int)encodeSTRTMessage(startTime, MessageBuffer, sizeof (MessageBuffer), 0); ASPData.MTSPU32 = 0; ASPData.TimeToSyncPointDbl = 0; @@ -753,8 +752,10 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { UtilSetObjectPositionIP(&OP[iIndex], object_address_name[iIndex]); - MessageLength = encodeOSEMMessage(&OriginPosition.Latitude, &OriginPosition.Longitude, &OriginPosition.Altitude, - NULL, NULL, NULL, MessageBuffer, sizeof (MessageBuffer), 0); + MessageLength = + encodeOSEMMessage(&OriginPosition.Latitude, &OriginPosition.Longitude, + &OriginPosition.Altitude, NULL, NULL, NULL, MessageBuffer, + sizeof (MessageBuffer), 0); if (MessageLength < 0) { util_error("OSEM encoding error"); } From 63f57fc1d6e91f0cc88b7fa6782ac5617e9d0896 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 24 Jan 2020 13:46:01 +0100 Subject: [PATCH 134/523] Reimplemented OSTM function in iso22133 and added endianness stuff to header build function --- server/inc/iso22133.h | 20 ++++++++++++ server/inc/util.h | 9 ----- server/src/iso22133.c | 67 ++++++++++++++++++++++++++++++++++---- server/src/objectcontrol.c | 50 ---------------------------- 4 files changed, 81 insertions(+), 65 deletions(-) diff --git a/server/inc/iso22133.h b/server/inc/iso22133.h index 73c4227e0..d0ab8176f 100644 --- a/server/inc/iso22133.h +++ b/server/inc/iso22133.h @@ -103,6 +103,25 @@ typedef struct #define VALUE_ID_OSEM_MIN_POSITIONING_ACCURACY 0x0074 +//! *************************** OSTM +typedef struct +{ + HeaderType header; + uint16_t stateValueID; + uint16_t stateContentLength; + uint8_t state; + FooterType footer; +} OSTMType; //16 bytes + +//! OSTM value IDs +#define VALUE_ID_OSTM_STATE_CHANGE_REQUEST 0x0064 +typedef enum { + OBJECT_COMMAND_ARM = 0x02, //!< Request to arm the target object + OBJECT_COMMAND_DISARM = 0x03, //!< Request to disarm the target object + OBJECT_COMMAND_REMOTE_CONTROL = 0x06 //!< Request for remote control of the target object +} ObjectCommandType; + + //! *************************** STRT typedef struct { @@ -383,6 +402,7 @@ typedef enum { ISOMessageReturnValue decodeMONRMessage(const char * MonrData, const size_t length, MONRType * MONRData, const char debug); ssize_t encodeSTRTMessage(const struct timeval* timeOfStart, char * strtDataBuffer, const size_t bufferLength, const char debug); ssize_t encodeOSEMMessage(const double * latitude_deg, const double * longitude_deg, const float * altitude_m, const float * maxPositionDeviation_m, const float * maxLateralDeviation_m, const float * minimumPositioningAccuracy_m, char * osemDataBuffer, const size_t bufferLength, const char debug); +ssize_t encodeOSTMMessage(const ObjectCommandType command, char * ostmDataBuffer, const size_t bufferLength, const char debug); ISOMessageReturnValue MONRToASCII(const MONRType * MONRData, char * asciiBuffer, const size_t bufferLength, const char debug); ISOMessageReturnValue ASCIIToMONR(const char * asciiBuffer, MONRType * MONRData, const char debug); ISOMessageID getISOMessageType(const char * messageData, const size_t length, const char debug); diff --git a/server/inc/util.h b/server/inc/util.h index 3e69dfd80..2335b044e 100644 --- a/server/inc/util.h +++ b/server/inc/util.h @@ -278,15 +278,6 @@ typedef struct } CartesianPosition; -typedef struct -{ - HeaderType Header; - U16 StateValueIdU16; - U16 StateContentLengthU16; - U8 StateU8; -} OSTMType; //16 bytes - - typedef struct { HeaderType Header; diff --git a/server/src/iso22133.c b/server/src/iso22133.c index d66ec0ae5..d194e4452 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -140,7 +140,7 @@ ISOMessageReturnValue decodeISOFooter(const char *MessageBuffer, const size_t le /*! * \brief buildISOHeader Constructs an ISO header based on the supplied message ID and content length * \param id Message ID of the message for which the header is to be used - * \param messageLength Length of the message excluding header and footer + * \param messageLength Length of the message including header and footer * \param debug Flag for enabling debugging * \return A struct containing ISO header data */ @@ -151,8 +151,15 @@ HeaderType buildISOHeader(ISOMessageID id, uint32_t messageLength, const char de header.TransmitterIdU8 = 0; header.MessageCounterU8 = 0; header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; - header.MessageIdU16 = (uint16_t) id; - header.MessageLengthU32 = messageLength; + if (messageLength >= sizeof (HeaderType) + sizeof (FooterType)) { + header.MessageIdU16 = (uint16_t) id; + header.MessageLengthU32 = messageLength - sizeof (HeaderType) - sizeof (FooterType); + } + else { + LogMessage(LOG_LEVEL_ERROR, "Supplied message length too small to hold header and footer"); + header.MessageIdU16 = (uint16_t) MESSAGE_ID_INVALID; + header.MessageLengthU32 = 0; + } if (debug) { LogPrint("Encoded ISO header:\n\tSync word: 0x%x\n\tTransmitter ID: %u\n\tMessage counter: %u\n\t" @@ -161,6 +168,11 @@ HeaderType buildISOHeader(ISOMessageID id, uint32_t messageLength, const char de header.MessageIdU16, header.MessageLengthU32); } + // Convert from host endianness to little endian + header.SyncWordU16 = htole16(header.SyncWordU16); + header.MessageIdU16 = htole16(header.MessageIdU16); + header.MessageLengthU32 = htole32(header.MessageLengthU32); + return header; } @@ -265,8 +277,7 @@ ssize_t encodeOSEMMessage(const double *latitude_deg, const double *longitude_de // Build header, and account for the two values which are 48 bit in the message OSEMData.header = buildISOHeader(MESSAGE_ID_OSEM, sizeof (OSEMData) - - sizeof (OSEMData.header) - sizeof (OSEMData.footer) - - 2 * SizeDifference64bitTo48bit, debug); + - 2 * SizeDifference64bitTo48bit, debug); // Fill the OSEM struct with relevant values OSEMData.latitudeValueID = VALUE_ID_OSEM_LATITUDE; @@ -402,6 +413,50 @@ ssize_t encodeOSEMMessage(const double *latitude_deg, const double *longitude_de } +ssize_t encodeOSTMMessage(const ObjectCommandType command, char * ostmDataBuffer, const size_t bufferLength, const char debug) { + + OSTMType OSTMData; + + memset(ostmDataBuffer, 0, bufferLength); + + // Check so buffer can hold message + if (bufferLength < sizeof (OSTMData)) { + LogMessage(LOG_LEVEL_ERROR, "Buffer too small to hold necessary OSTM data"); + return -1; + } + + // Check vs allowed commands + if (!(command == OBJECT_COMMAND_ARM || command == OBJECT_COMMAND_DISARM || command == OBJECT_COMMAND_REMOTE_CONTROL)) { + LogMessage(LOG_LEVEL_ERROR, "OSTM does not support command %u", (uint8_t) command); + return -1; + } + + // Construct header + OSTMData.header = buildISOHeader(MESSAGE_ID_OSTM, sizeof (OSTMData), debug); + + // Fill contents + OSTMData.stateValueID = VALUE_ID_OSTM_STATE_CHANGE_REQUEST; + OSTMData.stateContentLength = sizeof (OSTMData.state); + OSTMData.state = (uint8_t) command; + + if (debug) { + LogPrint("OSTM message:\n\tState change request value ID: 0x%x\n\t" + "State change request content length: %u\n\tState change request: %u", + OSTMData.stateValueID, OSTMData.stateContentLength, OSTMData.state); + } + + // Convert from host endianness to little endian + OSTMData.stateValueID = htole16(OSTMData.stateValueID); + OSTMData.stateContentLength = htole16(OSTMData.stateContentLength); + + // Construct footer + OSTMData.footer = buildISOFooter(&OSTMData, sizeof (OSTMData), debug); + + memcpy(ostmDataBuffer, &OSTMData, sizeof (OSTMData)); + + return sizeof (OSTMType); +} + /*! * \brief encodeSTRTMessage Constructs an ISO STRT message based on start time parameters @@ -425,7 +480,7 @@ ssize_t encodeSTRTMessage(const struct timeval *timeOfStart, char *strtDataBuffe } STRTData.header = - buildISOHeader(MESSAGE_ID_STRT, sizeof (STRTType) - sizeof (HeaderType) - sizeof (FooterType), debug); + buildISOHeader(MESSAGE_ID_STRT, sizeof (STRTType), debug); // Fill contents STRTData.StartTimeValueIdU16 = VALUE_ID_STRT_GPS_QMS_OF_WEEK; diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 51c7358e3..938be7649 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -75,12 +75,6 @@ #define COMMAND_DOTM_ROWS_IN_TRANSMISSION 40 #define COMMAND_DTM_BYTES_IN_ROW 30 -#define COMMAND_OSTM_CODE 3 -#define COMMAND_OSTM_NOFV 1 -#define COMMAND_OSTM_MESSAGE_LENGTH sizeof(OSTMType) -#define COMMAND_OSTM_OPT_SET_ARMED_STATE 2 -#define COMMAND_OSTM_OPT_SET_DISARMED_STATE 3 - #define COMMAND_HEAB_CODE 5 #define COMMAND_HEAB_NOFV 2 #define COMMAND_HEAB_MESSAGE_LENGTH sizeof(HEABType) @@ -1184,50 +1178,6 @@ I32 ObjectControlBuildVOILMessage(C8 * MessageBuffer, VOILType * VOILData, C8 * } -I32 ObjectControlBuildOSTMMessage(C8 * MessageBuffer, OSTMType * OSTMData, C8 CommandOption, U8 debug) { - I32 MessageIndex = 0, i; - U16 Crc = 0; - C8 *p; - - bzero(MessageBuffer, COMMAND_OSTM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); - - OSTMData->Header.SyncWordU16 = ISO_SYNC_WORD; - OSTMData->Header.TransmitterIdU8 = 0; - OSTMData->Header.MessageCounterU8 = 0; - OSTMData->Header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; - OSTMData->Header.MessageIdU16 = COMMAND_OSTM_CODE; - OSTMData->Header.MessageLengthU32 = sizeof (OSTMType) - sizeof (HeaderType); - OSTMData->StateValueIdU16 = VALUE_ID_STATE_CHANGE_REQUEST; - OSTMData->StateContentLengthU16 = sizeof (OSTMData->StateU8); - OSTMData->StateU8 = (U8) CommandOption; - - p = (C8 *) OSTMData; - for (i = 0; i < sizeof (OSTMType); i++) - *(MessageBuffer + i) = *p++; - Crc = crc_16((const C8 *)MessageBuffer, sizeof (OSTMType)); - Crc = 0; - *(MessageBuffer + i++) = (U8) (Crc >> 8); - *(MessageBuffer + i++) = (U8) (Crc); - MessageIndex = i; - - if (debug) { - // TODO: Change to log printout when byte thingy has been implemented - printf("OSTM total length = %d bytes (header+message+footer)\n", - (int)(COMMAND_OSTM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH)); - printf("----HEADER----\n"); - for (i = 0; i < sizeof (HeaderType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----MESSAGE----\n"); - for (; i < sizeof (OSTMType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----FOOTER----\n"); - for (; i < MessageIndex; i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n"); - } - - return MessageIndex; //Total number of bytes -} I32 ObjectControlBuildHEABMessage(C8 * MessageBuffer, HEABType * HEABData, TimeType * GPSTime, U8 CCStatus, From f33bcdd2dcd0bf61ddd9d0085053e2c01b1f8fbf Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 24 Jan 2020 14:47:23 +0100 Subject: [PATCH 135/523] Ran code formatter --- server/src/iso22133.c | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/server/src/iso22133.c b/server/src/iso22133.c index d194e4452..d91f2322b 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -413,7 +413,8 @@ ssize_t encodeOSEMMessage(const double *latitude_deg, const double *longitude_de } -ssize_t encodeOSTMMessage(const ObjectCommandType command, char * ostmDataBuffer, const size_t bufferLength, const char debug) { +ssize_t encodeOSTMMessage(const ObjectCommandType command, char *ostmDataBuffer, const size_t bufferLength, + const char debug) { OSTMType OSTMData; @@ -426,7 +427,9 @@ ssize_t encodeOSTMMessage(const ObjectCommandType command, char * ostmDataBuffer } // Check vs allowed commands - if (!(command == OBJECT_COMMAND_ARM || command == OBJECT_COMMAND_DISARM || command == OBJECT_COMMAND_REMOTE_CONTROL)) { + if (! + (command == OBJECT_COMMAND_ARM || command == OBJECT_COMMAND_DISARM + || command == OBJECT_COMMAND_REMOTE_CONTROL)) { LogMessage(LOG_LEVEL_ERROR, "OSTM does not support command %u", (uint8_t) command); return -1; } @@ -479,8 +482,7 @@ ssize_t encodeSTRTMessage(const struct timeval *timeOfStart, char *strtDataBuffe return -1; } - STRTData.header = - buildISOHeader(MESSAGE_ID_STRT, sizeof (STRTType), debug); + STRTData.header = buildISOHeader(MESSAGE_ID_STRT, sizeof (STRTType), debug); // Fill contents STRTData.StartTimeValueIdU16 = VALUE_ID_STRT_GPS_QMS_OF_WEEK; From f57a77658371ade23b4678bc9eb8d7e2a7eff07a Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 24 Jan 2020 14:53:01 +0100 Subject: [PATCH 136/523] Fixed missing thingy --- server/src/objectcontrol.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 938be7649..20422826b 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -616,7 +616,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { TimeSetToUTCms(&startTime, (int64_t) strtoul(MiscPtr, &MiscPtr, 10)); TimeSetToUTCms(&startDelay, (int64_t) strtoul(MiscPtr + 1, NULL, 10)); timeradd(&startTime, &startDelay, &startTime); - MessageLength = (int)encodeSTRTMessage(startTime, MessageBuffer, sizeof (MessageBuffer), 0); + MessageLength = (int)encodeSTRTMessage(&startTime, MessageBuffer, sizeof (MessageBuffer), 0); ASPData.MTSPU32 = 0; ASPData.TimeToSyncPointDbl = 0; From d052cc7e37cdfaf8f04a6f19025b4624161ab3c7 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 24 Jan 2020 14:53:51 +0100 Subject: [PATCH 137/523] Fixed missing thingy --- server/src/objectcontrol.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 51c7358e3..a096315f4 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -622,7 +622,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { TimeSetToUTCms(&startTime, (int64_t) strtoul(MiscPtr, &MiscPtr, 10)); TimeSetToUTCms(&startDelay, (int64_t) strtoul(MiscPtr + 1, NULL, 10)); timeradd(&startTime, &startDelay, &startTime); - MessageLength = (int)encodeSTRTMessage(startTime, MessageBuffer, sizeof (MessageBuffer), 0); + MessageLength = (int)encodeSTRTMessage(&startTime, MessageBuffer, sizeof (MessageBuffer), 0); ASPData.MTSPU32 = 0; ASPData.TimeToSyncPointDbl = 0; From e2261ab46fc140a71e821a7483b8c84def889450 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 24 Jan 2020 14:57:41 +0100 Subject: [PATCH 138/523] Removed unused function declarations and switched to using new iso22133 functions --- server/inc/objectcontrol.h | 2 -- server/src/objectcontrol.c | 12 +++++------- 2 files changed, 5 insertions(+), 9 deletions(-) diff --git a/server/inc/objectcontrol.h b/server/inc/objectcontrol.h index abff28fc7..a760dc41c 100644 --- a/server/inc/objectcontrol.h +++ b/server/inc/objectcontrol.h @@ -21,7 +21,6 @@ ------------------------------------------------------------*/ void objectcontrol_task(TimeType *GPSTime, GSDType *GSD, LOG_LEVEL logLevel); -I32 ObjectControlBuildOSTMMessage(C8* MessageBuffer, OSTMType *OSTMData, C8 CommandOption, U8 debug); I32 ObjectControlBuildHEABMessage(C8* MessageBuffer, HEABType *HEABData, TimeType *GPSTime, U8 CCStatus, U8 debug); int ObjectControlBuildLLCMMessage(char* MessageBuffer, unsigned short Speed, unsigned short Curvature, unsigned char Mode, char debug); I32 ObjectControlBuildSYPMMessage(C8* MessageBuffer, SYPMType *SYPMData, U32 SyncPoint, U32 StopTime, U8 debug); @@ -30,5 +29,4 @@ I32 ObjectControlBuildTRAJMessageHeader(C8* MessageBuffer, I32 *RowCount, Header I32 ObjectControlBuildTRAJMessage(C8* MessageBuffer, FILE *fd, I32 RowCount, DOTMType *DOTMType, U8 debug); I32 ObjectControlBuildVOILMessage(C8* MessageBuffer, VOILType *VOILData, C8* SimData, U8 debug); I32 ObjectControlSendTRAJMessage(C8* Filename, I32 *Socket, I32 RowCount, C8 *IP, U32 Port, DOTMType *DOTMData, U8 debug); -int ObjectControlOSEMtoASCII(OSEMType *OSEMData,char *GPSWeek, char *GPSLatitude, char *GPSLongitude, char *GPSAltitude); #endif //__OBJECTCONTROL_H_INCLUDED__ diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 20422826b..cec5898f5 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -152,7 +152,7 @@ static void vCloseSafetyChannel(int *sockfd); static size_t uiRecvMonitor(int *sockfd, char *buffer, size_t length); static int iGetObjectIndexFromObjectIP(in_addr_t ipAddr, in_addr_t objectIPs[], unsigned int numberOfObjects); static void signalHandler(int signo); -I32 ObjectControlBuildOSTMMessage(C8 * MessageBuffer, OSTMType * OSTMData, C8 CommandOption, U8 debug); + I32 ObjectControlBuildHEABMessage(C8 * MessageBuffer, HEABType * HEABData, TimeType * GPSTime, U8 CCStatus, U8 debug); int ObjectControlBuildLLCMMessage(char *MessageBuffer, unsigned short Speed, unsigned short Curvature, @@ -581,9 +581,8 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { LogMessage(LOG_LEVEL_INFO, "Sending ARM"); LOG_SEND(LogBuffer, "[ObjectControl] Sending ARM"); vSetState(OBC_STATE_ARMED, GSD); - MessageLength = - ObjectControlBuildOSTMMessage(MessageBuffer, &OSTMData, COMMAND_OSTM_OPT_SET_ARMED_STATE, - 0); + MessageLength = encodeOSTMMessage(OBJECT_COMMAND_ARM, MessageBuffer, sizeof (MessageBuffer), 0); + for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { /*Send OSTM message */ UtilSendTCPData("[Object Control]", MessageBuffer, MessageLength, &socket_fds[iIndex], 0); @@ -597,9 +596,8 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { LOG_SEND(LogBuffer, "[ObjectControl] Sending DISARM"); vSetState(OBC_STATE_CONNECTED, GSD); - MessageLength = - ObjectControlBuildOSTMMessage(MessageBuffer, &OSTMData, - COMMAND_OSTM_OPT_SET_DISARMED_STATE, 0); + MessageLength = encodeOSTMMessage(OBJECT_COMMAND_DISARM, MessageBuffer, sizeof (MessageBuffer), 0); + for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { /*Send OSTM message */ UtilSendTCPData("[" MODULE_NAME "]", MessageBuffer, MessageLength, &socket_fds[iIndex], From 56665b0ba245eb01e4e0b0f5b3e5202e81586d4b Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Fri, 24 Jan 2020 15:39:08 +0100 Subject: [PATCH 139/523] New init --- server/src/datadictionary.c | 18 ++++++------------ server/src/objectcontrol.c | 6 +++--- server/src/systemcontrol.c | 6 +++--- 3 files changed, 12 insertions(+), 18 deletions(-) diff --git a/server/src/datadictionary.c b/server/src/datadictionary.c index d35a38bfb..c9536d3d0 100644 --- a/server/src/datadictionary.c +++ b/server/src/datadictionary.c @@ -95,6 +95,9 @@ ReadWriteAccess_t DataDictionaryConstructor(GSDType * GSD) { DataDictionarySetOBCStateU8(GSD, OBC_STATE_UNDEFINED); + //Allocate memory in data dict + + return Res; } @@ -1684,14 +1687,14 @@ ReadWriteAccess_t DataDictionaryInitMONR(GSDType * GSD, U8 objectCount){ printf("File size on open: %ld\n", st.st_size); - lseek(fd, (sizeof (MONRType) * objectCount) - 1, SEEK_SET); + lseek(fd, (sizeof (MONRType)) - 1, SEEK_SET); write(fd, "", 1); stat(filePath, &st); printf("File size after writing: %ld\n", st.st_size); // Map memory to created file - GSD->MonrMessages = (MONRType *) mmap(NULL, (sizeof (MONRType) * objectCount), PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0); + GSD->MonrMessages = (MONRType *) mmap(NULL, (sizeof (MONRType)), PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0); LogPrint("Initing"); @@ -1713,7 +1716,6 @@ ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, MONRType * MONR, U8 id) { ReadWriteAccess_t Res; Res = WRITE_OK; pthread_mutex_lock(&MONRMutex); - //GSD->MonrMessages[id].HeadingU16 = MONR->HeadingU16; GSD->MonrMessages[id] = *MONR; @@ -1736,16 +1738,7 @@ ReadWriteAccess_t DataDictionaryGetMONR(GSDType * GSD, MONRType *MONR, U8 id) { int fd; pthread_mutex_lock(&MONRMutex); - sprintf(filePath, "%stest%d.mem", SHARED_MEMORY_PATH, 5); - fd = open(filePath, O_RDWR | O_CREAT, S_IRUSR | S_IWUSR); - stat(filePath, &st); - - // Map memory to created file - GSD->MonrMessages = (MONRType *) mmap(NULL, sizeof (MONRType), PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0); - - *MONR = GSD->MonrMessages[id]; - //GSD->memory = (char *) mmap(NULL, 6, PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0); LogPrint("Getting heading: %d for object[%d]", MONR->HeadingU16, id); pthread_mutex_unlock(&MONRMutex); return READ_OK; @@ -1781,6 +1774,7 @@ ReadWriteAccess_t DataDictionarySetNumberOfObjectsU8(GSDType * GSD, U8 *numberOf Res = WRITE_OK; pthread_mutex_lock(&numberOfObjectsMutex); GSD->numberOfObjects = *numberOfObjects; + DataDictionaryInitMONR(GSD, *numberOfObjects); pthread_mutex_unlock(&numberOfObjectsMutex); return Res; } diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 30ac48638..5e7859784 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -489,7 +489,6 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { //Store MONR in GSD DataDictionarySetMONR(GSD, &MONRData, iIndex); - DataDictionaryGetMONR(GSD, &MONRData, iIndex); //UtilSendUDPData("ObjectControl", &ObjectControlUDPSocketfdI32, &simulator_addr, &MONRData, sizeof(MONRData), 0); @@ -786,6 +785,9 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { //OSEMSentU8 = 0; STRTSentU8 = 0; + + DataDictionaryInitMONR(GSD, 2); + } else { LogMessage(LOG_LEVEL_INFO, @@ -794,8 +796,6 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { iCommSend(COMM_FAILURE, pcSendBuffer, sizeof (iCommand)); } - //Allocate memory in data dict - DataDictionaryInitMONR(GSD, nbr_objects); } else if (iCommand == COMM_ACCM && vGetState(GSD) == OBC_STATE_CONNECTED) { diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index 7ea41557b..983ec61e3 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -565,13 +565,12 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { break; case COMM_MONR: // TODO: Decode - /*if (RVSSChannelSocket != 0 && RVSSConfigU32 & RVSS_MONR_CHANNEL && bytesReceived >= 0) { + if (RVSSChannelSocket != 0 && RVSSConfigU32 & RVSS_MONR_CHANNEL && bytesReceived >= 0) { UtilPopulateMonitorDataStruct(pcRecvBuffer, (size_t) bytesReceived, &monrData, 0); SystemControlBuildRVSSMONRChannelMessage(RVSSData, &RVSSMessageLengthU32, monrData, 0); UtilSendUDPData("SystemControl", &RVSSChannelSocket, &RVSSChannelAddr, RVSSData, RVSSMessageLengthU32, 0); - }*/ - DataDictionaryGetMONR(GSD, &monrData.MONR, 0); + } SystemControlBuildRVSSMONRChannelMessage(RVSSData, &RVSSMessageLengthU32, monrData, 0); UtilSendUDPData("SystemControl", &RVSSChannelSocket, &RVSSChannelAddr, RVSSData, @@ -792,6 +791,7 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { } break; case InitializeScenario_0: + DataDictionaryInitMONR(GSD, 2); if (server_state == SERVER_STATE_IDLE && objectControlState == OBC_STATE_IDLE) { if (iCommSend(COMM_INIT, pcBuffer, strlen(pcBuffer) + 1) < 0) { LogMessage(LOG_LEVEL_ERROR, "Fatal communication fault when sending INIT command"); From e999cb95921228c152a0396777a17f5ef50544a4 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 24 Jan 2020 16:08:11 +0100 Subject: [PATCH 140/523] Implemented HEAB encoder function in iso22133 --- server/inc/iso22133.h | 25 ++++++++++++++++++ server/src/iso22133.c | 59 +++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 84 insertions(+) diff --git a/server/inc/iso22133.h b/server/inc/iso22133.h index d0ab8176f..c57d21418 100644 --- a/server/inc/iso22133.h +++ b/server/inc/iso22133.h @@ -140,7 +140,31 @@ typedef struct #define VALUE_ID_STRT_GPS_WEEK 0x0003 +//! *************************** HEAB +#define HEAB_FREQUENCY_HZ 100 +typedef struct +{ + HeaderType header; + uint16_t HEABStructValueID; + uint16_t HEABStructContentLength; + uint32_t GPSQmsOfWeek; + uint8_t controlCenterStatus; + FooterType footer; +} HEABType; //16 bytes + +//! HEAB value IDs +#define VALUE_ID_HEAB_STRUCT 0x0090 +typedef enum { + CONTROL_CENTER_STATUS_INIT = 0x00, + CONTROL_CENTER_STATUS_READY = 0x01, + CONTROL_CENTER_STATUS_ABORT = 0x02, + CONTROL_CENTER_STATUS_RUNNING = 0x03, + CONTROL_CENTER_STATUS_TEST_DONE = 0x04, + CONTROL_CENTER_STATUS_NORMAL_STOP = 0x05 +} ControlCenterStatusType; + //! *************************** MONR +#define MONR_EXPECTED_FREQUENCY_HZ 100 typedef struct { HeaderType header; @@ -403,6 +427,7 @@ ISOMessageReturnValue decodeMONRMessage(const char * MonrData, const size_t leng ssize_t encodeSTRTMessage(const struct timeval* timeOfStart, char * strtDataBuffer, const size_t bufferLength, const char debug); ssize_t encodeOSEMMessage(const double * latitude_deg, const double * longitude_deg, const float * altitude_m, const float * maxPositionDeviation_m, const float * maxLateralDeviation_m, const float * minimumPositioningAccuracy_m, char * osemDataBuffer, const size_t bufferLength, const char debug); ssize_t encodeOSTMMessage(const ObjectCommandType command, char * ostmDataBuffer, const size_t bufferLength, const char debug); +ssize_t encodeHEABMessage(const ControlCenterStatusType status, char * heabDataBuffer, const size_t bufferLength, const char debug); ISOMessageReturnValue MONRToASCII(const MONRType * MONRData, char * asciiBuffer, const size_t bufferLength, const char debug); ISOMessageReturnValue ASCIIToMONR(const char * asciiBuffer, MONRType * MONRData, const char debug); ISOMessageID getISOMessageType(const char * messageData, const size_t length, const char debug); diff --git a/server/src/iso22133.c b/server/src/iso22133.c index d91f2322b..9769e2ebb 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -518,7 +518,66 @@ ssize_t encodeSTRTMessage(const struct timeval *timeOfStart, char *strtDataBuffe return sizeof (STRTType); } +/*! + * \brief encodeHEABMessage Constructs an ISO HEAB message based on current control center status and system time + * \param status Current control center status according to ::ControlCenterStatusType. Entering an unaccepable value + * makes this parameter default to ABORT + * \param heabDataBuffer Buffer to which HEAB message is to be written + * \param bufferLength Size of buffer to which HEAB message is to be written + * \param debug Flag for enabling debugging + * \return Number of bytes written or -1 in case of an error + */ +ssize_t encodeHEABMessage(const ControlCenterStatusType status, char *heabDataBuffer, + const size_t bufferLength, const char debug) { + + HEABType HEABData; + struct timeval currentTime; + TimeSetToCurrentSystemTime(¤tTime); + + memset(heabDataBuffer, 0, bufferLength); + + // If buffer too small to hold HEAB data, generate an error + if (bufferLength < sizeof (HEABType)) { + LogMessage(LOG_LEVEL_ERROR, "Buffer too small to hold necessary HEAB data"); + return -1; + } + + HEABData.header = buildISOHeader(MESSAGE_ID_HEAB, sizeof (HEABData), debug); + + // Fill contents + HEABData.HEABStructValueID = VALUE_ID_HEAB_STRUCT; + HEABData.HEABStructContentLength = sizeof (HEABType) - sizeof (HeaderType) - sizeof (FooterType) + - sizeof (HEABData.HEABStructValueID) - sizeof (HEABData.HEABStructContentLength); + HEABData.GPSQmsOfWeek = TimeGetAsGPSqmsOfWeek(¤tTime); + if (!(status == CONTROL_CENTER_STATUS_INIT || status == CONTROL_CENTER_STATUS_READY + || status == CONTROL_CENTER_STATUS_ABORT || status == CONTROL_CENTER_STATUS_RUNNING + || status == CONTROL_CENTER_STATUS_TEST_DONE || status == CONTROL_CENTER_STATUS_NORMAL_STOP)) { + LogMessage(LOG_LEVEL_ERROR, "HEAB does not support status ID %u - defaulting to ABORT", (uint8_t) status); + HEABData.controlCenterStatus = (uint8_t) CONTROL_CENTER_STATUS_ABORT; + } + else { + HEABData.controlCenterStatus = (uint8_t) status; + } + + if (debug) { + LogPrint("HEAB message:\n\tHEAB struct value ID: 0x%x\n\t" + "HEAB struct content length: %u\n\tGPS second of week: %u [¼ ms]\n\t" + "Control center status: 0x%x", HEABData.HEABStructValueID, HEABData.HEABStructContentLength, + HEABData.GPSQmsOfWeek, HEABData.controlCenterStatus); + } + + // Switch from host endianness to little endian + HEABData.HEABStructValueID = htole16(HEABData.HEABStructValueID); + HEABData.HEABStructContentLength = htole16(HEABData.HEABStructContentLength); + HEABData.GPSQmsOfWeek = htole32(HEABData.GPSQmsOfWeek); + + HEABData.footer = buildISOFooter(&HEABData, sizeof (HEABData), debug); + memcpy(heabDataBuffer, &HEABData, sizeof (HEABData)); + + return sizeof (HEABType); + +} /*! * \brief buildMONRMessage Fills a MONRType struct from a buffer of raw data From ef59b5a588652443b43a37a4f8f24483599ba955 Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Fri, 24 Jan 2020 16:08:24 +0100 Subject: [PATCH 141/523] Removed some debug prints --- server/inc/datadictionary.h | 2 +- server/src/objectcontrol.c | 2 +- server/src/systemcontrol.c | 3 ++- 3 files changed, 4 insertions(+), 3 deletions(-) diff --git a/server/inc/datadictionary.h b/server/inc/datadictionary.h index f390d627f..f25cc3cc4 100644 --- a/server/inc/datadictionary.h +++ b/server/inc/datadictionary.h @@ -122,7 +122,7 @@ ReadWriteAccess_t DataDictionarySetOBCStateU8(GSDType *GSD, OBCState_t OBCState) OBCState_t DataDictionaryGetOBCStateU8(GSDType *GSD); ReadWriteAccess_t DataDictionaryFreeMONR(GSDType *GSD); -ReadWriteAccess_t DataDictionaryInitMONR(GSDType *GSD, U8 objectCount); +ReadWriteAccess_t DataDictionaryInitMONR(GSDType *GSD); ReadWriteAccess_t DataDictionarySetMONR(GSDType *GSD, MONRType * MONR, U8 index); ReadWriteAccess_t DataDictionaryGetMONR(GSDType *GSD, MONRType * MONR, U8 index); diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 5e7859784..269d1503e 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -786,7 +786,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { //OSEMSentU8 = 0; STRTSentU8 = 0; - DataDictionaryInitMONR(GSD, 2); + DataDictionaryInitMONR(GSD); } else { diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index 983ec61e3..f8e1b2715 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -791,7 +791,6 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { } break; case InitializeScenario_0: - DataDictionaryInitMONR(GSD, 2); if (server_state == SERVER_STATE_IDLE && objectControlState == OBC_STATE_IDLE) { if (iCommSend(COMM_INIT, pcBuffer, strlen(pcBuffer) + 1) < 0) { LogMessage(LOG_LEVEL_ERROR, "Fatal communication fault when sending INIT command"); @@ -801,6 +800,8 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "InitializeScenario:", ControlResponseBuffer, 0, &ClientSocket, 0); + DataDictionaryInitMONR(GSD); + SystemControlSendLog("[SystemControl] Sending INIT.\n", &ClientSocket, 0); } else if (server_state == SERVER_STATE_INWORK && objectControlState == OBC_STATE_INITIALIZED) { From a74d78c2137ed9b9b28f13ba8bf708d20440a302 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 24 Jan 2020 16:08:30 +0100 Subject: [PATCH 142/523] Removed HEAB typedef from util.h --- server/inc/util.h | 11 ----------- 1 file changed, 11 deletions(-) diff --git a/server/inc/util.h b/server/inc/util.h index 2335b044e..c2c4327fb 100644 --- a/server/inc/util.h +++ b/server/inc/util.h @@ -308,17 +308,6 @@ typedef struct } MTSPType; // -typedef struct -{ - HeaderType Header; - U16 HeabStructValueIdU16; - U16 HeabStructContentLengthU16; - U32 GPSQmsOfWeekU32; - U8 CCStatusU8; -} HEABType; //16 bytes - - - typedef struct { MONRType MONR; From b11fb12426e3597f80eb7d78d3f164dc4ac93f32 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 24 Jan 2020 16:09:03 +0100 Subject: [PATCH 143/523] Removed HEAB builder function from object control --- server/inc/objectcontrol.h | 1 - 1 file changed, 1 deletion(-) diff --git a/server/inc/objectcontrol.h b/server/inc/objectcontrol.h index a760dc41c..639c7ffb0 100644 --- a/server/inc/objectcontrol.h +++ b/server/inc/objectcontrol.h @@ -21,7 +21,6 @@ ------------------------------------------------------------*/ void objectcontrol_task(TimeType *GPSTime, GSDType *GSD, LOG_LEVEL logLevel); -I32 ObjectControlBuildHEABMessage(C8* MessageBuffer, HEABType *HEABData, TimeType *GPSTime, U8 CCStatus, U8 debug); int ObjectControlBuildLLCMMessage(char* MessageBuffer, unsigned short Speed, unsigned short Curvature, unsigned char Mode, char debug); I32 ObjectControlBuildSYPMMessage(C8* MessageBuffer, SYPMType *SYPMData, U32 SyncPoint, U32 StopTime, U8 debug); I32 ObjectControlBuildMTSPMessage(C8* MessageBuffer, MTSPType *MTSPData, U32 SyncTimestamp, U8 debug); From 0e366b003fbde6cd9f85b4605e2be1037ebe9ceb Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 24 Jan 2020 16:09:37 +0100 Subject: [PATCH 144/523] Cleaned up object control and started using HEAB encoder function from iso22133 --- server/src/objectcontrol.c | 137 +++++++------------------------------ 1 file changed, 25 insertions(+), 112 deletions(-) diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index cec5898f5..018530019 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -54,9 +54,6 @@ #define OC_SLEEP_TIME_NONEMPTY_MQ_S 0 #define OC_SLEEP_TIME_NONEMPTY_MQ_NS 0 - -#define TASK_PERIOD_MS 1 -#define HEARTBEAT_TIME_MS 10 #define OBJECT_CONTROL_CONTROL_MODE 0 #define OBJECT_CONTROL_REPLAY_MODE 1 #define OBJECT_CONTROL_ABORT_MODE 1 @@ -75,13 +72,6 @@ #define COMMAND_DOTM_ROWS_IN_TRANSMISSION 40 #define COMMAND_DTM_BYTES_IN_ROW 30 -#define COMMAND_HEAB_CODE 5 -#define COMMAND_HEAB_NOFV 2 -#define COMMAND_HEAB_MESSAGE_LENGTH sizeof(HEABType) -#define COMMAND_HEAB_OPT_SERVER_STATUS_BOOTING 0 -#define COMMAND_HEAB_OPT_SERVER_STATUS_OK 1 -#define COMMAND_HEAB_OPT_SERVER_STATUS_ABORT 2 - #define COMMAND_VOIL_CODE 0xA100 //#define COMMAND_VOIL_NOFV 2 #define COMMAND_VOIL_MESSAGE_LENGTH (16 * sizeof(Sim1Type) + sizeof(HeaderType) + 5) @@ -95,8 +85,6 @@ #define COMMAND_MTSP_CODE 0xA104 #define COMMAND_MTSP_MESSAGE_LENGTH sizeof(MTSPType) - - #define ASP_MESSAGE_LENGTH sizeof(ASPType) #define SMALL_BUFFER_SIZE_0 20 @@ -112,12 +100,6 @@ #define TEMP_LOG_FILE "log/temp.log" -typedef enum { - COMMAND_HEARTBEAT_GO, - COMMAND_HEARTBEAT_ABORT -} hearbeatCommand_t; - - typedef enum { TRANSITION_RESULT_UNDEFINED, TRANSITION_OK, @@ -153,8 +135,6 @@ static size_t uiRecvMonitor(int *sockfd, char *buffer, size_t length); static int iGetObjectIndexFromObjectIP(in_addr_t ipAddr, in_addr_t objectIPs[], unsigned int numberOfObjects); static void signalHandler(int signo); -I32 ObjectControlBuildHEABMessage(C8 * MessageBuffer, HEABType * HEABData, TimeType * GPSTime, U8 CCStatus, - U8 debug); int ObjectControlBuildLLCMMessage(char *MessageBuffer, unsigned short Speed, unsigned short Curvature, unsigned char Mode, char debug); I32 ObjectControlBuildSYPMMessage(C8 * MessageBuffer, SYPMType * SYPMData, U32 SyncPoint, U32 StopTime, @@ -229,7 +209,9 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { const struct timespec mqNonEmptyPollPeriod = { OC_SLEEP_TIME_NONEMPTY_MQ_S, OC_SLEEP_TIME_NONEMPTY_MQ_NS }; const struct timeval stateReportPeriod = { OC_STATE_REPORT_PERIOD_S, OC_STATE_REPORT_PERIOD_US }; - struct timeval currentTime, nextStateReportTime; + struct timeval currentTime, nextStateReportTime, nextHeartbeatTime; + const struct timeval heartbeatPeriod = { 1 / HEAB_FREQUENCY_HZ, + (1000000 / HEAB_FREQUENCY_HZ) % 1000000 }; U8 iForceObjectToLocalhostU8 = 0; FILE *fd; @@ -259,8 +241,6 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { C8 pcSendBuffer[MBUS_MAX_DATALEN]; C8 ObjectPort[SMALL_BUFFER_SIZE_0]; HeaderType HeaderData; - OSTMType OSTMData; - HEABType HEABData; MONRType MONRData; DOTMType DOTMData; TRAJInfoType TRAJInfoData; @@ -287,7 +267,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { I32 ASPStepBackCount = 0; char confDirectoryPath[MAX_FILE_PATH]; - U8 ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_BOOTING; + ControlCenterStatusType objectControlServerStatus = CONTROL_CENTER_STATUS_INIT; vInitializeState(OBC_STATE_IDLE, GSD); U8 uiTimeCycle = 0; @@ -328,37 +308,30 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { // Initialize timer for sending state TimeSetToCurrentSystemTime(¤tTime); nextStateReportTime = currentTime; - - // Initialize timer for sending state - TimeSetToCurrentSystemTime(¤tTime); - nextStateReportTime = currentTime; + nextHeartbeatTime = currentTime; while (!iExit) { if (vGetState(GSD) == OBC_STATE_ERROR) { - ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_ABORT; - MessageLength = - ObjectControlBuildHEABMessage(MessageBuffer, &HEABData, GPSTime, ObjectControlServerStatus, - 0); + objectControlServerStatus = CONTROL_CENTER_STATUS_ABORT; + MessageLength = encodeHEABMessage(objectControlServerStatus, MessageBuffer, sizeof (MessageBuffer), 0); UtilSendUDPData("Object Control", &safety_socket_fd[iIndex], &safety_object_addr[iIndex], MessageBuffer, MessageLength, 0); } - if (vGetState(GSD) == OBC_STATE_RUNNING || vGetState(GSD) == OBC_STATE_ARMED - || vGetState(GSD) == OBC_STATE_CONNECTED) { - /*HEAB*/ for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - if (uiTimeCycle == 0) { - //HeartbeatMessageCounter ++; - MessageLength = - ObjectControlBuildHEABMessage(MessageBuffer, &HEABData, GPSTime, - ObjectControlServerStatus, 0); - //ObjectControlSendUDPData(&safety_socket_fd[iIndex], &safety_object_addr[iIndex], MessageBuffer, MessageLength, 1); - UtilSendUDPData("Object Control", &safety_socket_fd[iIndex], &safety_object_addr[iIndex], + // Heartbeat + if ((vGetState(GSD) == OBC_STATE_RUNNING || vGetState(GSD) == OBC_STATE_ARMED + || vGetState(GSD) == OBC_STATE_CONNECTED) && timercmp(¤tTime, &nextHeartbeatTime, >)) { + + timeradd(&nextHeartbeatTime, &heartbeatPeriod, &nextHeartbeatTime); + MessageLength = encodeHEABMessage(objectControlServerStatus, MessageBuffer, sizeof (MessageBuffer), 0); + + // Transmit heartbeat to all objects + for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { + UtilSendUDPData("Object Control", &safety_socket_fd[iIndex], &safety_object_addr[iIndex], MessageBuffer, MessageLength, 0); - } } - // Check if any object has disconnected - if so, disconnect all objects and return to idle DisconnectU8 = 0; for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { @@ -447,7 +420,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { LogMessage(LOG_LEVEL_ERROR, "Fatal communication fault when sending MONR command - entering error state"); vSetState(OBC_STATE_ERROR, GSD); - ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_ABORT; + objectControlServerStatus = CONTROL_CENTER_STATUS_ABORT; } } @@ -588,7 +561,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { UtilSendTCPData("[Object Control]", MessageBuffer, MessageLength, &socket_fds[iIndex], 0); } - ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_OK; //Set server to READY + objectControlServerStatus = CONTROL_CENTER_STATUS_READY; } else if (iCommand == COMM_DISARM && vGetState(GSD) == OBC_STATE_ARMED) { @@ -604,7 +577,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { 0); } - ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_OK; //Set server to READY + objectControlServerStatus = CONTROL_CENTER_STATUS_READY; } else if (iCommand == COMM_STRT && (vGetState(GSD) == OBC_STATE_ARMED) /*|| OBC_STATE_INITIALIZED) */ ) //OBC_STATE_INITIALIZED is temporary! { @@ -621,7 +594,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { SearchStartIndex = -1; ASPData.PrevTimeToSyncPointDbl = 0; OldTimeU32 = CurrentTimeU32; - ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_OK; //Set server to READY + objectControlServerStatus = CONTROL_CENTER_STATUS_READY; for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { UtilSendTCPData("Object Control", MessageBuffer, MessageLength, &socket_fds[iIndex], 0); @@ -648,7 +621,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { } else if (iCommand == COMM_ABORT && vGetState(GSD) == OBC_STATE_RUNNING) { vSetState(OBC_STATE_CONNECTED, GSD); - ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_ABORT; //Set server to ABORT + objectControlServerStatus = CONTROL_CENTER_STATUS_ABORT; LogMessage(LOG_LEVEL_WARNING, "ABORT received"); LOG_SEND(LogBuffer, "[ObjectControl] ABORT received."); } @@ -821,7 +794,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { LogMessage(LOG_LEVEL_ERROR, "Fatal communication fault when sending OSEM command - entering error state"); vSetState(OBC_STATE_ERROR, GSD); - ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_ABORT; + objectControlServerStatus = CONTROL_CENTER_STATUS_ABORT; } UtilSendTCPData("Object Control", MessageBuffer, MessageLength, &socket_fds[iIndex], 0); @@ -927,7 +900,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { ObjectcontrolExecutionMode = OBJECT_CONTROL_CONTROL_MODE; /*Set server status */ - ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_OK; //Set server to READY + objectControlServerStatus = CONTROL_CENTER_STATUS_READY; if (DisconnectU8 == 0) { vSetState(OBC_STATE_CONNECTED, GSD); @@ -997,11 +970,6 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { /* Make call periodic */ sleep_time = iCommand == COMM_INV ? mqEmptyPollPeriod : mqNonEmptyPollPeriod; - ++uiTimeCycle; - if (uiTimeCycle >= HEARTBEAT_TIME_MS / TASK_PERIOD_MS) { - uiTimeCycle = 0; - } - // Periodically send state to signal aliveness TimeSetToCurrentSystemTime(¤tTime); if (timercmp(¤tTime, &nextStateReportTime, >)) { @@ -1013,7 +981,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { LogMessage(LOG_LEVEL_ERROR, "Fatal communication fault when sending OBC_STATE command - entering error state"); vSetState(OBC_STATE_ERROR, GSD); - ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_ABORT; + objectControlServerStatus = CONTROL_CENTER_STATUS_ABORT; } } @@ -1178,61 +1146,6 @@ I32 ObjectControlBuildVOILMessage(C8 * MessageBuffer, VOILType * VOILData, C8 * -I32 ObjectControlBuildHEABMessage(C8 * MessageBuffer, HEABType * HEABData, TimeType * GPSTime, U8 CCStatus, - U8 debug) { - I32 MessageIndex = 0, i; - U16 Crc = 0; - C8 *p; - - bzero(MessageBuffer, COMMAND_HEAB_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); - - HEABData->Header.SyncWordU16 = ISO_SYNC_WORD; - HEABData->Header.TransmitterIdU8 = 0; - HEABData->Header.MessageCounterU8 = 0; - HEABData->Header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; - HEABData->Header.MessageIdU16 = COMMAND_HEAB_CODE; - HEABData->Header.MessageLengthU32 = sizeof (HEABType) - sizeof (HeaderType); - HEABData->HeabStructValueIdU16 = VALUE_ID_HEAB_STRUCT; - HEABData->HeabStructContentLengthU16 = sizeof (HEABType) - sizeof (HeaderType) - - sizeof (HEABData->HeabStructValueIdU16) - sizeof (HEABData->HeabStructContentLengthU16); - HEABData->GPSQmsOfWeekU32 = - ((GPSTime->GPSSecondsOfWeekU32 * 1000 + (U32) TimeControlGetMillisecond(GPSTime)) << 2) + - GPSTime->MicroSecondU16; - HEABData->CCStatusU8 = CCStatus; - - if (!GPSTime->isGPSenabled) { - UtilgetCurrentGPStime(NULL, &HEABData->GPSQmsOfWeekU32); - } - - p = (C8 *) HEABData; - for (i = 0; i < sizeof (HEABType); i++) - *(MessageBuffer + i) = *p++; - Crc = crc_16((const C8 *)MessageBuffer, sizeof (HEABType)); - Crc = 0; - *(MessageBuffer + i++) = (U8) (Crc); - *(MessageBuffer + i++) = (U8) (Crc >> 8); - MessageIndex = i; - - if (debug) { - // TODO: Change to log printout when byte thingy has been implemented - printf("HEAB total length = %d bytes (header+message+footer)\n", - (int)(COMMAND_HEAB_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH)); - printf("----HEADER----\n"); - for (i = 0; i < sizeof (HeaderType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----MESSAGE----\n"); - for (; i < sizeof (HEABType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----FOOTER----\n"); - for (; i < MessageIndex; i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n"); - } - - return MessageIndex; //Total number of bytes - -} - int ObjectControlBuildLLCMMessage(char *MessageBuffer, unsigned short Speed, unsigned short Curvature, unsigned char Mode, char debug) { From c1cc4add84183e5dc97e12e723ec34101ed9e507 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 24 Jan 2020 16:11:02 +0100 Subject: [PATCH 145/523] Removed function duplicate from util.c/h --- server/inc/util.h | 1 - server/src/util.c | 53 ----------------------------------------------- 2 files changed, 54 deletions(-) diff --git a/server/inc/util.h b/server/inc/util.h index c2c4327fb..c67c0c1b5 100644 --- a/server/inc/util.h +++ b/server/inc/util.h @@ -807,7 +807,6 @@ U32 UtilHexTextToBinary(U32 DataLength, C8 *Text, C8 *Binary, U8 Debug); U32 UtilCreateDirContent(C8* DirPath, C8* TempPath); U16 UtilGetMillisecond(TimeType *GPSTime); I32 UtilISOBuildINSUPMessage(C8* MessageBuffer, INSUPType *INSUPData, C8 CommandOption, U8 Debug); -I32 UtilISOBuildHEABMessage(C8* MessageBuffer, HEABType *HEABData, TimeType *GPSTime, U8 CCStatus, U8 Debug); I32 UtilISOBuildTRAJMessageHeader(C8* MessageBuffer, I32 RowCount, HeaderType *HeaderData, TRAJInfoType *TRAJInfoData, U8 Debug); I32 UtilISOBuildTRAJMessage(C8 *MessageBuffer, C8 *DTMData, I32 RowCount, DOTMType *DOTMData, U8 debug); I32 UtilISOBuildTRAJInfo(C8* MessageBuffer, TRAJInfoType *TRAJInfoData, U8 debug); diff --git a/server/src/util.c b/server/src/util.c index 21ad73993..66e00b489 100644 --- a/server/src/util.c +++ b/server/src/util.c @@ -3257,59 +3257,6 @@ I32 UtilISOBuildINSUPMessage(C8 * MessageBuffer, INSUPType * INSUPData, C8 Comma return MessageIndex; //Total number of bytes } -I32 UtilISOBuildHEABMessage(C8 * MessageBuffer, HEABType * HEABData, TimeType * GPSTime, U8 CCStatus, - U8 Debug) { - I32 MessageIndex = 0, i; - U16 Crc = 0; - C8 *p; - - bzero(MessageBuffer, ISO_HEAB_MESSAGE_LENGTH + ISO_MESSAGE_FOOTER_LENGTH); - - HEABData->Header.SyncWordU16 = ISO_SYNC_WORD; - HEABData->Header.TransmitterIdU8 = 0; - HEABData->Header.MessageCounterU8 = 0; - HEABData->Header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; - HEABData->Header.MessageIdU16 = ISO_HEAB_CODE; - HEABData->Header.MessageLengthU32 = sizeof (HEABType) - sizeof (HeaderType); - //HEABData->HeabStructValueIdU16 = 0; - //HEABData->HeabStructContentLengthU16 = sizeof(HEABType) - sizeof(HeaderType) - 4; - HEABData->GPSQmsOfWeekU32 = - ((GPSTime->GPSSecondsOfWeekU32 * 1000 + (U32) UtilGetMillisecond(GPSTime)) << 2) + - GPSTime->MicroSecondU16; - HEABData->CCStatusU8 = CCStatus; - - if (!GPSTime->isGPSenabled) { - UtilgetCurrentGPStime(NULL, &HEABData->GPSQmsOfWeekU32); - } - - p = (C8 *) HEABData; - for (i = 0; i < sizeof (HEABType); i++) - *(MessageBuffer + i) = *p++; - Crc = crc_16((const C8 *)MessageBuffer, sizeof (HEABType)); - Crc = 0; - *(MessageBuffer + i++) = (U8) (Crc); - *(MessageBuffer + i++) = (U8) (Crc >> 8); - MessageIndex = i; - - if (Debug) { - // TODO: Change this when bytes thingy has been implemented in logging - printf("HEAB total length = %d bytes (header+message+footer)\n", - (int)(ISO_HEAB_MESSAGE_LENGTH + ISO_MESSAGE_FOOTER_LENGTH)); - printf("----HEADER----\n"); - for (i = 0; i < sizeof (HeaderType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----MESSAGE----\n"); - for (; i < sizeof (HEABType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----FOOTER----\n"); - for (; i < MessageIndex; i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n"); - } - - return MessageIndex; //Total number of bytes -} - U16 UtilGetMillisecond(TimeType * GPSTime) { struct timeval now; From 4c2070681b8f9561b39c505b016a1fc697db0d79 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 24 Jan 2020 16:13:18 +0100 Subject: [PATCH 146/523] Removed conflicting defines --- server/inc/util.h | 1 - 1 file changed, 1 deletion(-) diff --git a/server/inc/util.h b/server/inc/util.h index c67c0c1b5..c256e5c21 100644 --- a/server/inc/util.h +++ b/server/inc/util.h @@ -155,7 +155,6 @@ extern "C"{ #define VALUE_ID_TRAJECTORY_ID 0x101 #define VALUE_ID_TRAJECTORY_NAME 0x102 #define VALUE_ID_TRAJECTORY_VERSION 0x103 -#define VALUE_ID_HEAB_STRUCT 0x90 #define VALUE_ID_INSUP_MODE 0x200 #define C8 uint8_t From d414377b77369c4a2fd76d6e5c4f76f43b3aa110 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 24 Jan 2020 16:17:04 +0100 Subject: [PATCH 147/523] Function comment for OSTM encoder function --- server/src/iso22133.c | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/server/src/iso22133.c b/server/src/iso22133.c index d91f2322b..e62031556 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -412,7 +412,14 @@ ssize_t encodeOSEMMessage(const double *latitude_deg, const double *longitude_de return sizeof (OSEMType) - 2 * SizeDifference64bitTo48bit; } - +/*! + * \brief encodeOSTMMessage Constructs an ISO OSTM message based on specified command + * \param command Command to send to object according to ::ObjectCommandType + * \param ostmDataBuffer Data buffer to which OSTM is to be written + * \param bufferLength Length of data buffer to which OSTM is to be written + * \param debug Flag for enabling debugging + * \return Number of bytes written to buffer, or -1 in case of error + */ ssize_t encodeOSTMMessage(const ObjectCommandType command, char *ostmDataBuffer, const size_t bufferLength, const char debug) { From 9941e43db1325afc076f4e97a3a1bf6ab619e82b Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 24 Jan 2020 16:17:31 +0100 Subject: [PATCH 148/523] Ran code formatter --- server/src/objectcontrol.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index cec5898f5..27efe00e6 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -581,7 +581,8 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { LogMessage(LOG_LEVEL_INFO, "Sending ARM"); LOG_SEND(LogBuffer, "[ObjectControl] Sending ARM"); vSetState(OBC_STATE_ARMED, GSD); - MessageLength = encodeOSTMMessage(OBJECT_COMMAND_ARM, MessageBuffer, sizeof (MessageBuffer), 0); + MessageLength = + encodeOSTMMessage(OBJECT_COMMAND_ARM, MessageBuffer, sizeof (MessageBuffer), 0); for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { /*Send OSTM message */ @@ -596,7 +597,8 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { LOG_SEND(LogBuffer, "[ObjectControl] Sending DISARM"); vSetState(OBC_STATE_CONNECTED, GSD); - MessageLength = encodeOSTMMessage(OBJECT_COMMAND_DISARM, MessageBuffer, sizeof (MessageBuffer), 0); + MessageLength = + encodeOSTMMessage(OBJECT_COMMAND_DISARM, MessageBuffer, sizeof (MessageBuffer), 0); for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { /*Send OSTM message */ From fa4276300cfc282158be709580d2d2cbd2949234 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 24 Jan 2020 16:18:42 +0100 Subject: [PATCH 149/523] Removed HEAB defines from util.h --- server/inc/util.h | 7 ------- 1 file changed, 7 deletions(-) diff --git a/server/inc/util.h b/server/inc/util.h index c256e5c21..a507f92d1 100644 --- a/server/inc/util.h +++ b/server/inc/util.h @@ -203,13 +203,6 @@ extern "C"{ #define ISO_INSUP_OPT_SET_ARMED_STATE 2 #define ISO_INSUP_OPT_SET_DISARMED_STATE 3 -#define ISO_HEAB_CODE 5 -#define ISO_HEAB_NOFV 2 -#define ISO_HEAB_MESSAGE_LENGTH sizeof(HEABType) -#define ISO_HEAB_OPT_SERVER_STATUS_BOOTING 0 -#define ISO_HEAB_OPT_SERVER_STATUS_OK 1 -#define ISO_HEAB_OPT_SERVER_STATUS_ABORT 2 - #define ISO_TRAJ_CODE 1 #define ISO_DTM_ROWS_IN_TRANSMISSION 40 #define ISO_DTM_ROW_MESSAGE_LENGTH sizeof(DOTMType) From b6d900dd31f203173a368428915398ea60735da2 Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Fri, 24 Jan 2020 16:21:33 +0100 Subject: [PATCH 150/523] Updated with dev merge --- server/src/datadictionary.c | 16 ++++------------ 1 file changed, 4 insertions(+), 12 deletions(-) diff --git a/server/src/datadictionary.c b/server/src/datadictionary.c index c9536d3d0..945b4dc0c 100644 --- a/server/src/datadictionary.c +++ b/server/src/datadictionary.c @@ -1673,7 +1673,7 @@ OBCState_t DataDictionaryGetOBCStateU8(GSDType * GSD) { * \param objects number of objects that will transmitt monr * \return Result according to ::ReadWriteAccess_t */ -ReadWriteAccess_t DataDictionaryInitMONR(GSDType * GSD, U8 objectCount){ +ReadWriteAccess_t DataDictionaryInitMONR(GSDType * GSD){ ReadWriteAccess_t Res; Res = WRITE_OK; char filePath[PATH_MAX]; @@ -1685,13 +1685,13 @@ ReadWriteAccess_t DataDictionaryInitMONR(GSDType * GSD, U8 objectCount){ fd = open(filePath, O_RDWR | O_CREAT, S_IRUSR | S_IWUSR); stat(filePath, &st); - printf("File size on open: %ld\n", st.st_size); + //printf("File size on open: %ld\n", st.st_size); lseek(fd, (sizeof (MONRType)) - 1, SEEK_SET); write(fd, "", 1); stat(filePath, &st); - printf("File size after writing: %ld\n", st.st_size); + //printf("File size after writing: %ld\n", st.st_size); // Map memory to created file GSD->MonrMessages = (MONRType *) mmap(NULL, (sizeof (MONRType)), PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0); @@ -1716,11 +1716,8 @@ ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, MONRType * MONR, U8 id) { ReadWriteAccess_t Res; Res = WRITE_OK; pthread_mutex_lock(&MONRMutex); - //GSD->MonrMessages[id].HeadingU16 = MONR->HeadingU16; - GSD->MonrMessages[id] = *MONR; LogPrint("Setting %d", GSD->MonrMessages[id].HeadingU16); - pthread_mutex_unlock(&MONRMutex); return Res; } @@ -1733,13 +1730,8 @@ ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, MONRType * MONR, U8 id) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetMONR(GSDType * GSD, MONRType *MONR, U8 id) { - struct stat st; - char filePath[PATH_MAX]; - int fd; - pthread_mutex_lock(&MONRMutex); *MONR = GSD->MonrMessages[id]; - LogPrint("Getting heading: %d for object[%d]", MONR->HeadingU16, id); pthread_mutex_unlock(&MONRMutex); return READ_OK; } @@ -1754,6 +1746,7 @@ ReadWriteAccess_t DataDictionaryFreeMONR(GSDType * GSD){ ReadWriteAccess_t Res; Res = WRITE_OK; pthread_mutex_lock(&MONRMutex); + munmap(GSD->MonrMessages, sizeof (MONRType)); free(GSD->MonrMessages); pthread_mutex_unlock(&MONRMutex); return Res; @@ -1774,7 +1767,6 @@ ReadWriteAccess_t DataDictionarySetNumberOfObjectsU8(GSDType * GSD, U8 *numberOf Res = WRITE_OK; pthread_mutex_lock(&numberOfObjectsMutex); GSD->numberOfObjects = *numberOfObjects; - DataDictionaryInitMONR(GSD, *numberOfObjects); pthread_mutex_unlock(&numberOfObjectsMutex); return Res; } From 2eadd73a3ec6776e0da584a2c492a8f82d1e47eb Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Fri, 24 Jan 2020 16:25:53 +0100 Subject: [PATCH 151/523] Ran formatting script --- server/src/datadictionary.c | 1612 +++++----- server/src/objectcontrol.c | 5540 ++++++++++++++++++----------------- server/src/systemcontrol.c | 3722 +++++++++++------------ 3 files changed, 5442 insertions(+), 5432 deletions(-) diff --git a/server/src/datadictionary.c b/server/src/datadictionary.c index 945b4dc0c..ea2acf303 100644 --- a/server/src/datadictionary.c +++ b/server/src/datadictionary.c @@ -67,38 +67,38 @@ Initialization data that is configurable is stored in test.conf. * \return Error code defined by ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryConstructor(GSDType * GSD) { - ReadWriteAccess_t Res = READ_OK; + ReadWriteAccess_t Res = READ_OK; - Res = Res == READ_OK ? DataDictionaryInitOriginLatitudeDbl(GSD) : Res; - Res = Res == READ_OK ? DataDictionaryInitOriginLongitudeDbl(GSD) : Res; - Res = Res == READ_OK ? DataDictionaryInitOriginAltitudeDbl(GSD) : Res; - Res = Res == READ_OK ? DataDictionaryInitVisualizationServerU32(GSD) : Res; - Res = Res == READ_OK ? DataDictionaryInitForceToLocalhostU8(GSD) : Res; - Res = Res == READ_OK ? DataDictionaryInitASPMaxTimeDiffDbl(GSD) : Res; - Res = Res == READ_OK ? DataDictionaryInitASPMaxTrajDiffDbl(GSD) : Res; - Res = Res == READ_OK ? DataDictionaryInitASPStepBackCountU32(GSD) : Res; - Res = Res == READ_OK ? DataDictionaryInitASPFilterLevelDbl(GSD) : Res; - Res = Res == READ_OK ? DataDictionaryInitASPMaxDeltaTimeDbl(GSD) : Res; - Res = Res == READ_OK ? DataDictionaryInitTimeServerIPU32(GSD) : Res; - Res = Res == READ_OK ? DataDictionaryInitTimeServerPortU16(GSD) : Res; - Res = Res == READ_OK ? DataDictionaryInitSimulatorIPU32(GSD) : Res; - Res = Res == READ_OK ? DataDictionaryInitSimulatorTCPPortU16(GSD) : Res; - Res = Res == READ_OK ? DataDictionaryInitSimulatorUDPPortU16(GSD) : Res; - Res = Res == READ_OK ? DataDictionaryInitSimulatorModeU8(GSD) : Res; - Res = Res == READ_OK ? DataDictionaryInitVOILReceiversC8(GSD) : Res; - Res = Res == READ_OK ? DataDictionaryInitDTMReceiversC8(GSD) : Res; - Res = Res == READ_OK ? DataDictionaryInitExternalSupervisorIPU32(GSD) : Res; - Res = Res == READ_OK ? DataDictionaryInitRVSSConfigU32(GSD) : Res; - Res = Res == READ_OK ? DataDictionaryInitRVSSRateU8(GSD) : Res; - Res = Res == READ_OK ? DataDictionaryInitSupervisorTCPPortU16(GSD) : Res; - Res = Res == READ_OK ? DataDictionaryInitMiscDataC8(GSD) : Res; + Res = Res == READ_OK ? DataDictionaryInitOriginLatitudeDbl(GSD) : Res; + Res = Res == READ_OK ? DataDictionaryInitOriginLongitudeDbl(GSD) : Res; + Res = Res == READ_OK ? DataDictionaryInitOriginAltitudeDbl(GSD) : Res; + Res = Res == READ_OK ? DataDictionaryInitVisualizationServerU32(GSD) : Res; + Res = Res == READ_OK ? DataDictionaryInitForceToLocalhostU8(GSD) : Res; + Res = Res == READ_OK ? DataDictionaryInitASPMaxTimeDiffDbl(GSD) : Res; + Res = Res == READ_OK ? DataDictionaryInitASPMaxTrajDiffDbl(GSD) : Res; + Res = Res == READ_OK ? DataDictionaryInitASPStepBackCountU32(GSD) : Res; + Res = Res == READ_OK ? DataDictionaryInitASPFilterLevelDbl(GSD) : Res; + Res = Res == READ_OK ? DataDictionaryInitASPMaxDeltaTimeDbl(GSD) : Res; + Res = Res == READ_OK ? DataDictionaryInitTimeServerIPU32(GSD) : Res; + Res = Res == READ_OK ? DataDictionaryInitTimeServerPortU16(GSD) : Res; + Res = Res == READ_OK ? DataDictionaryInitSimulatorIPU32(GSD) : Res; + Res = Res == READ_OK ? DataDictionaryInitSimulatorTCPPortU16(GSD) : Res; + Res = Res == READ_OK ? DataDictionaryInitSimulatorUDPPortU16(GSD) : Res; + Res = Res == READ_OK ? DataDictionaryInitSimulatorModeU8(GSD) : Res; + Res = Res == READ_OK ? DataDictionaryInitVOILReceiversC8(GSD) : Res; + Res = Res == READ_OK ? DataDictionaryInitDTMReceiversC8(GSD) : Res; + Res = Res == READ_OK ? DataDictionaryInitExternalSupervisorIPU32(GSD) : Res; + Res = Res == READ_OK ? DataDictionaryInitRVSSConfigU32(GSD) : Res; + Res = Res == READ_OK ? DataDictionaryInitRVSSRateU8(GSD) : Res; + Res = Res == READ_OK ? DataDictionaryInitSupervisorTCPPortU16(GSD) : Res; + Res = Res == READ_OK ? DataDictionaryInitMiscDataC8(GSD) : Res; - DataDictionarySetOBCStateU8(GSD, OBC_STATE_UNDEFINED); + DataDictionarySetOBCStateU8(GSD, OBC_STATE_UNDEFINED); - //Allocate memory in data dict + //Allocate memory in data dict - return Res; + return Res; } @@ -108,23 +108,23 @@ ReadWriteAccess_t DataDictionaryConstructor(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitOriginLatitudeDbl(GSDType * GSD) { - ReadWriteAccess_t Res = UNDEFINED; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + ReadWriteAccess_t Res = UNDEFINED; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; - if (DataDictionarySearchParameter("OrigoLatitude=", ResultBufferC8)) { - Res = READ_OK; - pthread_mutex_lock(&OriginLatitudeMutex); - GSD->OriginLatitudeDbl = atof(ResultBufferC8); - bzero(GSD->OriginLatitudeC8, DD_CONTROL_BUFFER_SIZE_20); - strcat(GSD->OriginLatitudeC8, ResultBufferC8); - pthread_mutex_unlock(&OriginLatitudeMutex); - } - else { - Res = PARAMETER_NOTFOUND; - LogMessage(LOG_LEVEL_ERROR, "OriginLatitude not found!"); - } + if (DataDictionarySearchParameter("OrigoLatitude=", ResultBufferC8)) { + Res = READ_OK; + pthread_mutex_lock(&OriginLatitudeMutex); + GSD->OriginLatitudeDbl = atof(ResultBufferC8); + bzero(GSD->OriginLatitudeC8, DD_CONTROL_BUFFER_SIZE_20); + strcat(GSD->OriginLatitudeC8, ResultBufferC8); + pthread_mutex_unlock(&OriginLatitudeMutex); + } + else { + Res = PARAMETER_NOTFOUND; + LogMessage(LOG_LEVEL_ERROR, "OriginLatitude not found!"); + } - return Res; + return Res; } /*! @@ -134,19 +134,19 @@ ReadWriteAccess_t DataDictionaryInitOriginLatitudeDbl(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetOriginLatitudeDbl(GSDType * GSD, C8 * Latitude) { - ReadWriteAccess_t Res; + ReadWriteAccess_t Res; - if (UtilWriteConfigurationParameter("OrigoLatitude", Latitude, 0)) { - Res = WRITE_OK; - pthread_mutex_lock(&OriginLatitudeMutex); - GSD->OriginLatitudeDbl = atof(Latitude); - bzero(GSD->OriginLatitudeC8, DD_CONTROL_BUFFER_SIZE_20); - strcat(GSD->OriginLatitudeC8, Latitude); - pthread_mutex_unlock(&OriginLatitudeMutex); - } - else - Res = PARAMETER_NOTFOUND; - return Res; + if (UtilWriteConfigurationParameter("OrigoLatitude", Latitude, 0)) { + Res = WRITE_OK; + pthread_mutex_lock(&OriginLatitudeMutex); + GSD->OriginLatitudeDbl = atof(Latitude); + bzero(GSD->OriginLatitudeC8, DD_CONTROL_BUFFER_SIZE_20); + strcat(GSD->OriginLatitudeC8, Latitude); + pthread_mutex_unlock(&OriginLatitudeMutex); + } + else + Res = PARAMETER_NOTFOUND; + return Res; } /*! @@ -156,10 +156,10 @@ ReadWriteAccess_t DataDictionarySetOriginLatitudeDbl(GSDType * GSD, C8 * Latitud * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetOriginLatitudeDbl(GSDType * GSD, dbl * Latitude) { - pthread_mutex_lock(&OriginLatitudeMutex); - *Latitude = GSD->OriginLatitudeDbl; - pthread_mutex_unlock(&OriginLatitudeMutex); - return READ_OK; + pthread_mutex_lock(&OriginLatitudeMutex); + *Latitude = GSD->OriginLatitudeDbl; + pthread_mutex_unlock(&OriginLatitudeMutex); + return READ_OK; } /*! @@ -169,11 +169,11 @@ ReadWriteAccess_t DataDictionaryGetOriginLatitudeDbl(GSDType * GSD, dbl * Latitu * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetOriginLatitudeC8(GSDType * GSD, C8 * Latitude, U32 BuffLen) { - pthread_mutex_lock(&OriginLatitudeMutex); - bzero(Latitude, BuffLen); - strcat(Latitude, GSD->OriginLatitudeC8); - pthread_mutex_unlock(&OriginLatitudeMutex); - return READ_OK; + pthread_mutex_lock(&OriginLatitudeMutex); + bzero(Latitude, BuffLen); + strcat(Latitude, GSD->OriginLatitudeC8); + pthread_mutex_unlock(&OriginLatitudeMutex); + return READ_OK; } /*END of Origin Latitude*/ @@ -185,23 +185,23 @@ ReadWriteAccess_t DataDictionaryGetOriginLatitudeC8(GSDType * GSD, C8 * Latitude * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitOriginLongitudeDbl(GSDType * GSD) { - ReadWriteAccess_t Res = UNDEFINED; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + ReadWriteAccess_t Res = UNDEFINED; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; - if (DataDictionarySearchParameter("OrigoLongitude=", ResultBufferC8)) { - Res = READ_OK; - pthread_mutex_lock(&OriginLongitudeMutex); - GSD->OriginLongitudeDbl = atof(ResultBufferC8); - bzero(GSD->OriginLongitudeC8, DD_CONTROL_BUFFER_SIZE_20); - strcat(GSD->OriginLongitudeC8, ResultBufferC8); - pthread_mutex_unlock(&OriginLongitudeMutex); - } - else { - Res = PARAMETER_NOTFOUND; - LogMessage(LOG_LEVEL_ERROR, "OriginLongitude not found!"); - } + if (DataDictionarySearchParameter("OrigoLongitude=", ResultBufferC8)) { + Res = READ_OK; + pthread_mutex_lock(&OriginLongitudeMutex); + GSD->OriginLongitudeDbl = atof(ResultBufferC8); + bzero(GSD->OriginLongitudeC8, DD_CONTROL_BUFFER_SIZE_20); + strcat(GSD->OriginLongitudeC8, ResultBufferC8); + pthread_mutex_unlock(&OriginLongitudeMutex); + } + else { + Res = PARAMETER_NOTFOUND; + LogMessage(LOG_LEVEL_ERROR, "OriginLongitude not found!"); + } - return Res; + return Res; } /*! @@ -211,19 +211,19 @@ ReadWriteAccess_t DataDictionaryInitOriginLongitudeDbl(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetOriginLongitudeDbl(GSDType * GSD, C8 * Longitude) { - ReadWriteAccess_t Res; + ReadWriteAccess_t Res; - if (UtilWriteConfigurationParameter("OrigoLongitude", Longitude, 0)) { - Res = WRITE_OK; - pthread_mutex_lock(&OriginLongitudeMutex); - GSD->OriginLongitudeDbl = atof(Longitude); - bzero(GSD->OriginLongitudeC8, DD_CONTROL_BUFFER_SIZE_20); - strcat(GSD->OriginLongitudeC8, Longitude); - pthread_mutex_unlock(&OriginLongitudeMutex); - } - else - Res = PARAMETER_NOTFOUND; - return Res; + if (UtilWriteConfigurationParameter("OrigoLongitude", Longitude, 0)) { + Res = WRITE_OK; + pthread_mutex_lock(&OriginLongitudeMutex); + GSD->OriginLongitudeDbl = atof(Longitude); + bzero(GSD->OriginLongitudeC8, DD_CONTROL_BUFFER_SIZE_20); + strcat(GSD->OriginLongitudeC8, Longitude); + pthread_mutex_unlock(&OriginLongitudeMutex); + } + else + Res = PARAMETER_NOTFOUND; + return Res; } /*! @@ -233,10 +233,10 @@ ReadWriteAccess_t DataDictionarySetOriginLongitudeDbl(GSDType * GSD, C8 * Longit * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetOriginLongitudeDbl(GSDType * GSD, dbl * Longitude) { - pthread_mutex_lock(&OriginLongitudeMutex); - *Longitude = GSD->OriginLongitudeDbl; - pthread_mutex_unlock(&OriginLongitudeMutex); - return READ_OK; + pthread_mutex_lock(&OriginLongitudeMutex); + *Longitude = GSD->OriginLongitudeDbl; + pthread_mutex_unlock(&OriginLongitudeMutex); + return READ_OK; } /*! @@ -246,11 +246,11 @@ ReadWriteAccess_t DataDictionaryGetOriginLongitudeDbl(GSDType * GSD, dbl * Longi * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetOriginLongitudeC8(GSDType * GSD, C8 * Longitude, U32 BuffLen) { - pthread_mutex_lock(&OriginLongitudeMutex); - bzero(Longitude, BuffLen); - strcat(Longitude, GSD->OriginLongitudeC8); - pthread_mutex_unlock(&OriginLongitudeMutex); - return READ_OK; + pthread_mutex_lock(&OriginLongitudeMutex); + bzero(Longitude, BuffLen); + strcat(Longitude, GSD->OriginLongitudeC8); + pthread_mutex_unlock(&OriginLongitudeMutex); + return READ_OK; } /*END of Origin Longitude*/ @@ -262,23 +262,23 @@ ReadWriteAccess_t DataDictionaryGetOriginLongitudeC8(GSDType * GSD, C8 * Longitu * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitOriginAltitudeDbl(GSDType * GSD) { - ReadWriteAccess_t Res = UNDEFINED; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + ReadWriteAccess_t Res = UNDEFINED; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; - if (DataDictionarySearchParameter("OrigoAltitude=", ResultBufferC8)) { - Res = READ_OK; - pthread_mutex_lock(&OriginAltitudeMutex); - GSD->OriginAltitudeDbl = atof(ResultBufferC8); - bzero(GSD->OriginAltitudeC8, DD_CONTROL_BUFFER_SIZE_20); - strcat(GSD->OriginAltitudeC8, ResultBufferC8); - pthread_mutex_unlock(&OriginAltitudeMutex); - } - else { - Res = PARAMETER_NOTFOUND; - LogMessage(LOG_LEVEL_ERROR, "OriginAltitude not found!"); - } + if (DataDictionarySearchParameter("OrigoAltitude=", ResultBufferC8)) { + Res = READ_OK; + pthread_mutex_lock(&OriginAltitudeMutex); + GSD->OriginAltitudeDbl = atof(ResultBufferC8); + bzero(GSD->OriginAltitudeC8, DD_CONTROL_BUFFER_SIZE_20); + strcat(GSD->OriginAltitudeC8, ResultBufferC8); + pthread_mutex_unlock(&OriginAltitudeMutex); + } + else { + Res = PARAMETER_NOTFOUND; + LogMessage(LOG_LEVEL_ERROR, "OriginAltitude not found!"); + } - return Res; + return Res; } /*! @@ -288,19 +288,19 @@ ReadWriteAccess_t DataDictionaryInitOriginAltitudeDbl(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetOriginAltitudeDbl(GSDType * GSD, C8 * Altitude) { - ReadWriteAccess_t Res; + ReadWriteAccess_t Res; - if (UtilWriteConfigurationParameter("OrigoAltitude", Altitude, 0)) { - Res = WRITE_OK; - pthread_mutex_lock(&OriginAltitudeMutex); - GSD->OriginAltitudeDbl = atof(Altitude); - bzero(GSD->OriginAltitudeC8, DD_CONTROL_BUFFER_SIZE_20); - strcat(GSD->OriginAltitudeC8, Altitude); - pthread_mutex_unlock(&OriginAltitudeMutex); - } - else - Res = PARAMETER_NOTFOUND; - return Res; + if (UtilWriteConfigurationParameter("OrigoAltitude", Altitude, 0)) { + Res = WRITE_OK; + pthread_mutex_lock(&OriginAltitudeMutex); + GSD->OriginAltitudeDbl = atof(Altitude); + bzero(GSD->OriginAltitudeC8, DD_CONTROL_BUFFER_SIZE_20); + strcat(GSD->OriginAltitudeC8, Altitude); + pthread_mutex_unlock(&OriginAltitudeMutex); + } + else + Res = PARAMETER_NOTFOUND; + return Res; } /*! @@ -310,10 +310,10 @@ ReadWriteAccess_t DataDictionarySetOriginAltitudeDbl(GSDType * GSD, C8 * Altitud * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetOriginAltitudeDbl(GSDType * GSD, dbl * Altitude) { - pthread_mutex_lock(&OriginAltitudeMutex); - *Altitude = GSD->OriginAltitudeDbl; - pthread_mutex_unlock(&OriginAltitudeMutex); - return READ_OK; + pthread_mutex_lock(&OriginAltitudeMutex); + *Altitude = GSD->OriginAltitudeDbl; + pthread_mutex_unlock(&OriginAltitudeMutex); + return READ_OK; } /*! @@ -323,11 +323,11 @@ ReadWriteAccess_t DataDictionaryGetOriginAltitudeDbl(GSDType * GSD, dbl * Altitu * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetOriginAltitudeC8(GSDType * GSD, C8 * Altitude, U32 BuffLen) { - pthread_mutex_lock(&OriginAltitudeMutex); - bzero(Altitude, BuffLen); - strcat(Altitude, GSD->OriginAltitudeC8); - pthread_mutex_unlock(&OriginAltitudeMutex); - return READ_OK; + pthread_mutex_lock(&OriginAltitudeMutex); + bzero(Altitude, BuffLen); + strcat(Altitude, GSD->OriginAltitudeC8); + pthread_mutex_unlock(&OriginAltitudeMutex); + return READ_OK; } /*END of Origin Altitude*/ @@ -339,23 +339,23 @@ ReadWriteAccess_t DataDictionaryGetOriginAltitudeC8(GSDType * GSD, C8 * Altitude * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitVisualizationServerU32(GSDType * GSD) { - ReadWriteAccess_t Res = UNDEFINED; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + ReadWriteAccess_t Res = UNDEFINED; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; - if (DataDictionarySearchParameter("VisualizationServerName=", ResultBufferC8)) { - Res = READ_OK; - pthread_mutex_lock(&VisualizationServerMutex); - GSD->VisualizationServerU32 = UtilIPStringToInt(ResultBufferC8); - bzero(GSD->VisualizationServerC8, DD_CONTROL_BUFFER_SIZE_20); - strcat(GSD->VisualizationServerC8, ResultBufferC8); - pthread_mutex_unlock(&VisualizationServerMutex); - } - else { - Res = PARAMETER_NOTFOUND; - LogMessage(LOG_LEVEL_ERROR, "VisualizationServerName not found!"); - } + if (DataDictionarySearchParameter("VisualizationServerName=", ResultBufferC8)) { + Res = READ_OK; + pthread_mutex_lock(&VisualizationServerMutex); + GSD->VisualizationServerU32 = UtilIPStringToInt(ResultBufferC8); + bzero(GSD->VisualizationServerC8, DD_CONTROL_BUFFER_SIZE_20); + strcat(GSD->VisualizationServerC8, ResultBufferC8); + pthread_mutex_unlock(&VisualizationServerMutex); + } + else { + Res = PARAMETER_NOTFOUND; + LogMessage(LOG_LEVEL_ERROR, "VisualizationServerName not found!"); + } - return Res; + return Res; } /*! @@ -365,19 +365,19 @@ ReadWriteAccess_t DataDictionaryInitVisualizationServerU32(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetVisualizationServerU32(GSDType * GSD, C8 * IP) { - ReadWriteAccess_t Res; + ReadWriteAccess_t Res; - if (UtilWriteConfigurationParameter("VisualizationServerName", IP, 0)) { - Res = WRITE_OK; - pthread_mutex_lock(&VisualizationServerMutex); - GSD->VisualizationServerU32 = UtilIPStringToInt(IP); - bzero(GSD->VisualizationServerC8, DD_CONTROL_BUFFER_SIZE_20); - strcat(GSD->VisualizationServerC8, IP); - pthread_mutex_unlock(&VisualizationServerMutex); - } - else - Res = PARAMETER_NOTFOUND; - return Res; + if (UtilWriteConfigurationParameter("VisualizationServerName", IP, 0)) { + Res = WRITE_OK; + pthread_mutex_lock(&VisualizationServerMutex); + GSD->VisualizationServerU32 = UtilIPStringToInt(IP); + bzero(GSD->VisualizationServerC8, DD_CONTROL_BUFFER_SIZE_20); + strcat(GSD->VisualizationServerC8, IP); + pthread_mutex_unlock(&VisualizationServerMutex); + } + else + Res = PARAMETER_NOTFOUND; + return Res; } /*! @@ -387,10 +387,10 @@ ReadWriteAccess_t DataDictionarySetVisualizationServerU32(GSDType * GSD, C8 * IP * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetVisualizationServerU32(GSDType * GSD, U32 * IP) { - pthread_mutex_lock(&VisualizationServerMutex); - *IP = GSD->VisualizationServerU32; - pthread_mutex_unlock(&VisualizationServerMutex); - return READ_OK; + pthread_mutex_lock(&VisualizationServerMutex); + *IP = GSD->VisualizationServerU32; + pthread_mutex_unlock(&VisualizationServerMutex); + return READ_OK; } @@ -401,11 +401,11 @@ ReadWriteAccess_t DataDictionaryGetVisualizationServerU32(GSDType * GSD, U32 * I * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetVisualizationServerC8(GSDType * GSD, C8 * IP, U32 BuffLen) { - pthread_mutex_lock(&VisualizationServerMutex); - bzero(IP, BuffLen); - strcat(IP, GSD->VisualizationServerC8); - pthread_mutex_unlock(&VisualizationServerMutex); - return READ_OK; + pthread_mutex_lock(&VisualizationServerMutex); + bzero(IP, BuffLen); + strcat(IP, GSD->VisualizationServerC8); + pthread_mutex_unlock(&VisualizationServerMutex); + return READ_OK; } /*END of VisualizationServer*/ @@ -418,21 +418,21 @@ ReadWriteAccess_t DataDictionaryGetVisualizationServerC8(GSDType * GSD, C8 * IP, * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitForceToLocalhostU8(GSDType * GSD) { - ReadWriteAccess_t Res = UNDEFINED; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + ReadWriteAccess_t Res = UNDEFINED; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; - if (DataDictionarySearchParameter("ForceObjectToLocalhost=", ResultBufferC8)) { - Res = READ_OK; - pthread_mutex_lock(&ForceObjectToLocalhostMutex); - GSD->ForceObjectToLocalhostU8 = atoi(ResultBufferC8); - pthread_mutex_unlock(&ForceObjectToLocalhostMutex); - } - else { - Res = PARAMETER_NOTFOUND; - LogMessage(LOG_LEVEL_ERROR, "ForceObjectToLocalhost not found!"); - } + if (DataDictionarySearchParameter("ForceObjectToLocalhost=", ResultBufferC8)) { + Res = READ_OK; + pthread_mutex_lock(&ForceObjectToLocalhostMutex); + GSD->ForceObjectToLocalhostU8 = atoi(ResultBufferC8); + pthread_mutex_unlock(&ForceObjectToLocalhostMutex); + } + else { + Res = PARAMETER_NOTFOUND; + LogMessage(LOG_LEVEL_ERROR, "ForceObjectToLocalhost not found!"); + } - return Res; + return Res; } /*! @@ -442,17 +442,17 @@ ReadWriteAccess_t DataDictionaryInitForceToLocalhostU8(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetForceToLocalhostU8(GSDType * GSD, C8 * ForceLocalhost) { - ReadWriteAccess_t Res; + ReadWriteAccess_t Res; - if (UtilWriteConfigurationParameter("ForceObjectToLocalhost", ForceLocalhost, 0)) { - Res = WRITE_OK; - pthread_mutex_lock(&ForceObjectToLocalhostMutex); - GSD->ForceObjectToLocalhostU8 = atoi(ForceLocalhost); - pthread_mutex_unlock(&ForceObjectToLocalhostMutex); - } - else - Res = PARAMETER_NOTFOUND; - return Res; + if (UtilWriteConfigurationParameter("ForceObjectToLocalhost", ForceLocalhost, 0)) { + Res = WRITE_OK; + pthread_mutex_lock(&ForceObjectToLocalhostMutex); + GSD->ForceObjectToLocalhostU8 = atoi(ForceLocalhost); + pthread_mutex_unlock(&ForceObjectToLocalhostMutex); + } + else + Res = PARAMETER_NOTFOUND; + return Res; } /*! @@ -462,10 +462,10 @@ ReadWriteAccess_t DataDictionarySetForceToLocalhostU8(GSDType * GSD, C8 * ForceL * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetForceToLocalhostU8(GSDType * GSD, U8 * ForceLocalhost) { - pthread_mutex_lock(&ForceObjectToLocalhostMutex); - *ForceLocalhost = GSD->ForceObjectToLocalhostU8; - pthread_mutex_unlock(&ForceObjectToLocalhostMutex); - return READ_OK; + pthread_mutex_lock(&ForceObjectToLocalhostMutex); + *ForceLocalhost = GSD->ForceObjectToLocalhostU8; + pthread_mutex_unlock(&ForceObjectToLocalhostMutex); + return READ_OK; } /*END of ForceToLocalhost*/ @@ -477,21 +477,21 @@ ReadWriteAccess_t DataDictionaryGetForceToLocalhostU8(GSDType * GSD, U8 * ForceL * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitASPMaxTimeDiffDbl(GSDType * GSD) { - ReadWriteAccess_t Res = UNDEFINED; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + ReadWriteAccess_t Res = UNDEFINED; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; - if (DataDictionarySearchParameter("ASPMaxTimeDiff=", ResultBufferC8)) { - Res = READ_OK; - pthread_mutex_lock(&ASPMaxTimeDiffMutex); - GSD->ASPMaxTimeDiffDbl = atof(ResultBufferC8); - pthread_mutex_unlock(&ASPMaxTimeDiffMutex); - } - else { - Res = PARAMETER_NOTFOUND; - LogMessage(LOG_LEVEL_ERROR, "ASPMaxTimeDiff not found!"); - } + if (DataDictionarySearchParameter("ASPMaxTimeDiff=", ResultBufferC8)) { + Res = READ_OK; + pthread_mutex_lock(&ASPMaxTimeDiffMutex); + GSD->ASPMaxTimeDiffDbl = atof(ResultBufferC8); + pthread_mutex_unlock(&ASPMaxTimeDiffMutex); + } + else { + Res = PARAMETER_NOTFOUND; + LogMessage(LOG_LEVEL_ERROR, "ASPMaxTimeDiff not found!"); + } - return Res; + return Res; } /*! @@ -501,17 +501,17 @@ ReadWriteAccess_t DataDictionaryInitASPMaxTimeDiffDbl(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetASPMaxTimeDiffDbl(GSDType * GSD, C8 * ASPMaxTimeDiff) { - ReadWriteAccess_t Res; + ReadWriteAccess_t Res; - if (UtilWriteConfigurationParameter("ASPMaxTimeDiff", ASPMaxTimeDiff, 0)) { - Res = WRITE_OK; - pthread_mutex_lock(&ASPMaxTimeDiffMutex); - GSD->ASPMaxTimeDiffDbl = atof(ASPMaxTimeDiff); - pthread_mutex_unlock(&ASPMaxTimeDiffMutex); - } - else - Res = PARAMETER_NOTFOUND; - return Res; + if (UtilWriteConfigurationParameter("ASPMaxTimeDiff", ASPMaxTimeDiff, 0)) { + Res = WRITE_OK; + pthread_mutex_lock(&ASPMaxTimeDiffMutex); + GSD->ASPMaxTimeDiffDbl = atof(ASPMaxTimeDiff); + pthread_mutex_unlock(&ASPMaxTimeDiffMutex); + } + else + Res = PARAMETER_NOTFOUND; + return Res; } /*! @@ -521,10 +521,10 @@ ReadWriteAccess_t DataDictionarySetASPMaxTimeDiffDbl(GSDType * GSD, C8 * ASPMaxT * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetASPMaxTimeDiffDbl(GSDType * GSD, dbl * ASPMaxTimeDiff) { - pthread_mutex_lock(&ASPMaxTimeDiffMutex); - *ASPMaxTimeDiff = GSD->ASPMaxTimeDiffDbl; - pthread_mutex_unlock(&ASPMaxTimeDiffMutex); - return READ_OK; + pthread_mutex_lock(&ASPMaxTimeDiffMutex); + *ASPMaxTimeDiff = GSD->ASPMaxTimeDiffDbl; + pthread_mutex_unlock(&ASPMaxTimeDiffMutex); + return READ_OK; } /*END of ASPMaxTimeDiff*/ @@ -536,21 +536,21 @@ ReadWriteAccess_t DataDictionaryGetASPMaxTimeDiffDbl(GSDType * GSD, dbl * ASPMax * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitASPMaxTrajDiffDbl(GSDType * GSD) { - ReadWriteAccess_t Res = UNDEFINED; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + ReadWriteAccess_t Res = UNDEFINED; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; - if (DataDictionarySearchParameter("ASPMaxTrajDiff=", ResultBufferC8)) { - Res = READ_OK; - pthread_mutex_lock(&ASPMaxTrajDiffMutex); - GSD->ASPMaxTrajDiffDbl = atof(ResultBufferC8); - pthread_mutex_unlock(&ASPMaxTrajDiffMutex); - } - else { - Res = PARAMETER_NOTFOUND; - LogMessage(LOG_LEVEL_ERROR, "ASPMaxTrajDiff not found!"); - } + if (DataDictionarySearchParameter("ASPMaxTrajDiff=", ResultBufferC8)) { + Res = READ_OK; + pthread_mutex_lock(&ASPMaxTrajDiffMutex); + GSD->ASPMaxTrajDiffDbl = atof(ResultBufferC8); + pthread_mutex_unlock(&ASPMaxTrajDiffMutex); + } + else { + Res = PARAMETER_NOTFOUND; + LogMessage(LOG_LEVEL_ERROR, "ASPMaxTrajDiff not found!"); + } - return Res; + return Res; } /*! @@ -560,17 +560,17 @@ ReadWriteAccess_t DataDictionaryInitASPMaxTrajDiffDbl(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetASPMaxTrajDiffDbl(GSDType * GSD, C8 * ASPMaxTrajDiff) { - ReadWriteAccess_t Res; + ReadWriteAccess_t Res; - if (UtilWriteConfigurationParameter("ASPMaxTrajDiff", ASPMaxTrajDiff, 0)) { - Res = WRITE_OK; - pthread_mutex_lock(&ASPMaxTrajDiffMutex); - GSD->ASPMaxTrajDiffDbl = atof(ASPMaxTrajDiff); - pthread_mutex_unlock(&ASPMaxTrajDiffMutex); - } - else - Res = PARAMETER_NOTFOUND; - return Res; + if (UtilWriteConfigurationParameter("ASPMaxTrajDiff", ASPMaxTrajDiff, 0)) { + Res = WRITE_OK; + pthread_mutex_lock(&ASPMaxTrajDiffMutex); + GSD->ASPMaxTrajDiffDbl = atof(ASPMaxTrajDiff); + pthread_mutex_unlock(&ASPMaxTrajDiffMutex); + } + else + Res = PARAMETER_NOTFOUND; + return Res; } /*! @@ -580,10 +580,10 @@ ReadWriteAccess_t DataDictionarySetASPMaxTrajDiffDbl(GSDType * GSD, C8 * ASPMaxT * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetASPMaxTrajDiffDbl(GSDType * GSD, dbl * ASPMaxTrajDiff) { - pthread_mutex_lock(&ASPMaxTrajDiffMutex); - *ASPMaxTrajDiff = GSD->ASPMaxTrajDiffDbl; - pthread_mutex_unlock(&ASPMaxTrajDiffMutex); - return READ_OK; + pthread_mutex_lock(&ASPMaxTrajDiffMutex); + *ASPMaxTrajDiff = GSD->ASPMaxTrajDiffDbl; + pthread_mutex_unlock(&ASPMaxTrajDiffMutex); + return READ_OK; } /*END of ASPMaxTrajDiff*/ @@ -596,21 +596,21 @@ ReadWriteAccess_t DataDictionaryGetASPMaxTrajDiffDbl(GSDType * GSD, dbl * ASPMax * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitASPStepBackCountU32(GSDType * GSD) { - ReadWriteAccess_t Res = UNDEFINED; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + ReadWriteAccess_t Res = UNDEFINED; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; - if (DataDictionarySearchParameter("ASPStepBackCount=", ResultBufferC8)) { - Res = READ_OK; - pthread_mutex_lock(&ASPStepBackCountMutex); - GSD->ASPStepBackCountU32 = atoi(ResultBufferC8); - pthread_mutex_unlock(&ASPStepBackCountMutex); - } - else { - Res = PARAMETER_NOTFOUND; - LogMessage(LOG_LEVEL_ERROR, "ASPStepBackCount not found!"); - } + if (DataDictionarySearchParameter("ASPStepBackCount=", ResultBufferC8)) { + Res = READ_OK; + pthread_mutex_lock(&ASPStepBackCountMutex); + GSD->ASPStepBackCountU32 = atoi(ResultBufferC8); + pthread_mutex_unlock(&ASPStepBackCountMutex); + } + else { + Res = PARAMETER_NOTFOUND; + LogMessage(LOG_LEVEL_ERROR, "ASPStepBackCount not found!"); + } - return Res; + return Res; } /*! @@ -620,17 +620,17 @@ ReadWriteAccess_t DataDictionaryInitASPStepBackCountU32(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetASPStepBackCountU32(GSDType * GSD, C8 * ASPStepBackCount) { - ReadWriteAccess_t Res; + ReadWriteAccess_t Res; - if (UtilWriteConfigurationParameter("ASPStepBackCount", ASPStepBackCount, 0)) { - Res = WRITE_OK; - pthread_mutex_lock(&ASPStepBackCountMutex); - GSD->ASPStepBackCountU32 = atoi(ASPStepBackCount); - pthread_mutex_unlock(&ASPStepBackCountMutex); - } - else - Res = PARAMETER_NOTFOUND; - return Res; + if (UtilWriteConfigurationParameter("ASPStepBackCount", ASPStepBackCount, 0)) { + Res = WRITE_OK; + pthread_mutex_lock(&ASPStepBackCountMutex); + GSD->ASPStepBackCountU32 = atoi(ASPStepBackCount); + pthread_mutex_unlock(&ASPStepBackCountMutex); + } + else + Res = PARAMETER_NOTFOUND; + return Res; } /*! @@ -640,10 +640,10 @@ ReadWriteAccess_t DataDictionarySetASPStepBackCountU32(GSDType * GSD, C8 * ASPSt * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetASPStepBackCountU32(GSDType * GSD, U32 * ASPStepBackCount) { - pthread_mutex_lock(&ASPStepBackCountMutex); - *ASPStepBackCount = GSD->ASPStepBackCountU32; - pthread_mutex_unlock(&ASPStepBackCountMutex); - return READ_OK; + pthread_mutex_lock(&ASPStepBackCountMutex); + *ASPStepBackCount = GSD->ASPStepBackCountU32; + pthread_mutex_unlock(&ASPStepBackCountMutex); + return READ_OK; } /*END of ASPStepBackCount*/ @@ -655,21 +655,21 @@ ReadWriteAccess_t DataDictionaryGetASPStepBackCountU32(GSDType * GSD, U32 * ASPS * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitASPFilterLevelDbl(GSDType * GSD) { - ReadWriteAccess_t Res = UNDEFINED; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + ReadWriteAccess_t Res = UNDEFINED; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; - if (DataDictionarySearchParameter("ASPFilterLevel=", ResultBufferC8)) { - Res = READ_OK; - pthread_mutex_lock(&ASPFilterLevelMutex); - GSD->ASPFilterLevelDbl = atof(ResultBufferC8); - pthread_mutex_unlock(&ASPFilterLevelMutex); - } - else { - Res = PARAMETER_NOTFOUND; - LogMessage(LOG_LEVEL_ERROR, "ASPFilterLevel not found!"); - } + if (DataDictionarySearchParameter("ASPFilterLevel=", ResultBufferC8)) { + Res = READ_OK; + pthread_mutex_lock(&ASPFilterLevelMutex); + GSD->ASPFilterLevelDbl = atof(ResultBufferC8); + pthread_mutex_unlock(&ASPFilterLevelMutex); + } + else { + Res = PARAMETER_NOTFOUND; + LogMessage(LOG_LEVEL_ERROR, "ASPFilterLevel not found!"); + } - return Res; + return Res; } /*! @@ -679,17 +679,17 @@ ReadWriteAccess_t DataDictionaryInitASPFilterLevelDbl(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetASPFilterLevelDbl(GSDType * GSD, C8 * ASPFilterLevel) { - ReadWriteAccess_t Res; + ReadWriteAccess_t Res; - if (UtilWriteConfigurationParameter("ASPFilterLevel", ASPFilterLevel, 0)) { - Res = WRITE_OK; - pthread_mutex_lock(&ASPFilterLevelMutex); - GSD->ASPFilterLevelDbl = atof(ASPFilterLevel); - pthread_mutex_unlock(&ASPFilterLevelMutex); - } - else - Res = PARAMETER_NOTFOUND; - return Res; + if (UtilWriteConfigurationParameter("ASPFilterLevel", ASPFilterLevel, 0)) { + Res = WRITE_OK; + pthread_mutex_lock(&ASPFilterLevelMutex); + GSD->ASPFilterLevelDbl = atof(ASPFilterLevel); + pthread_mutex_unlock(&ASPFilterLevelMutex); + } + else + Res = PARAMETER_NOTFOUND; + return Res; } /*! @@ -699,10 +699,10 @@ ReadWriteAccess_t DataDictionarySetASPFilterLevelDbl(GSDType * GSD, C8 * ASPFilt * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetASPFilterLevelDbl(GSDType * GSD, dbl * ASPFilterLevel) { - pthread_mutex_lock(&ASPFilterLevelMutex); - *ASPFilterLevel = GSD->ASPFilterLevelDbl; - pthread_mutex_unlock(&ASPFilterLevelMutex); - return READ_OK; + pthread_mutex_lock(&ASPFilterLevelMutex); + *ASPFilterLevel = GSD->ASPFilterLevelDbl; + pthread_mutex_unlock(&ASPFilterLevelMutex); + return READ_OK; } /*END of ASPFilterLevel*/ @@ -714,21 +714,21 @@ ReadWriteAccess_t DataDictionaryGetASPFilterLevelDbl(GSDType * GSD, dbl * ASPFil * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitASPMaxDeltaTimeDbl(GSDType * GSD) { - ReadWriteAccess_t Res = UNDEFINED; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + ReadWriteAccess_t Res = UNDEFINED; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; - if (DataDictionarySearchParameter("ASPMaxDeltaTime=", ResultBufferC8)) { - Res = READ_OK; - pthread_mutex_lock(&ASPMaxDeltaTimeMutex); - GSD->ASPMaxDeltaTimeDbl = atof(ResultBufferC8); - pthread_mutex_unlock(&ASPMaxDeltaTimeMutex); - } - else { - Res = PARAMETER_NOTFOUND; - LogMessage(LOG_LEVEL_ERROR, "ASPMaxDeltaTime not found!"); - } + if (DataDictionarySearchParameter("ASPMaxDeltaTime=", ResultBufferC8)) { + Res = READ_OK; + pthread_mutex_lock(&ASPMaxDeltaTimeMutex); + GSD->ASPMaxDeltaTimeDbl = atof(ResultBufferC8); + pthread_mutex_unlock(&ASPMaxDeltaTimeMutex); + } + else { + Res = PARAMETER_NOTFOUND; + LogMessage(LOG_LEVEL_ERROR, "ASPMaxDeltaTime not found!"); + } - return Res; + return Res; } /*! @@ -738,17 +738,17 @@ ReadWriteAccess_t DataDictionaryInitASPMaxDeltaTimeDbl(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetASPMaxDeltaTimeDbl(GSDType * GSD, C8 * ASPMaxDeltaTime) { - ReadWriteAccess_t Res; + ReadWriteAccess_t Res; - if (UtilWriteConfigurationParameter("ASPMaxDeltaTime", ASPMaxDeltaTime, 0)) { - Res = WRITE_OK; - pthread_mutex_lock(&ASPMaxDeltaTimeMutex); - GSD->ASPMaxDeltaTimeDbl = atof(ASPMaxDeltaTime); - pthread_mutex_unlock(&ASPMaxDeltaTimeMutex); - } - else - Res = PARAMETER_NOTFOUND; - return Res; + if (UtilWriteConfigurationParameter("ASPMaxDeltaTime", ASPMaxDeltaTime, 0)) { + Res = WRITE_OK; + pthread_mutex_lock(&ASPMaxDeltaTimeMutex); + GSD->ASPMaxDeltaTimeDbl = atof(ASPMaxDeltaTime); + pthread_mutex_unlock(&ASPMaxDeltaTimeMutex); + } + else + Res = PARAMETER_NOTFOUND; + return Res; } /*! @@ -758,10 +758,10 @@ ReadWriteAccess_t DataDictionarySetASPMaxDeltaTimeDbl(GSDType * GSD, C8 * ASPMax * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetASPMaxDeltaTimeDbl(GSDType * GSD, dbl * ASPMaxDeltaTime) { - pthread_mutex_lock(&ASPMaxDeltaTimeMutex); - *ASPMaxDeltaTime = GSD->ASPMaxDeltaTimeDbl; - pthread_mutex_unlock(&ASPMaxDeltaTimeMutex); - return READ_OK; + pthread_mutex_lock(&ASPMaxDeltaTimeMutex); + *ASPMaxDeltaTime = GSD->ASPMaxDeltaTimeDbl; + pthread_mutex_unlock(&ASPMaxDeltaTimeMutex); + return READ_OK; } /*END of ASPFilterLevel*/ @@ -774,23 +774,23 @@ ReadWriteAccess_t DataDictionaryGetASPMaxDeltaTimeDbl(GSDType * GSD, dbl * ASPMa * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitTimeServerIPU32(GSDType * GSD) { - ReadWriteAccess_t Res = UNDEFINED; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + ReadWriteAccess_t Res = UNDEFINED; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; - if (DataDictionarySearchParameter("TimeServerIP=", ResultBufferC8)) { - Res = READ_OK; - pthread_mutex_lock(&TimeServerIPMutex); - GSD->TimeServerIPU32 = UtilIPStringToInt(ResultBufferC8); - bzero(GSD->TimeServerIPC8, DD_CONTROL_BUFFER_SIZE_20); - strcat(GSD->TimeServerIPC8, ResultBufferC8); - pthread_mutex_unlock(&TimeServerIPMutex); - } - else { - Res = PARAMETER_NOTFOUND; - LogMessage(LOG_LEVEL_ERROR, "TimeServerIP not found!"); - } + if (DataDictionarySearchParameter("TimeServerIP=", ResultBufferC8)) { + Res = READ_OK; + pthread_mutex_lock(&TimeServerIPMutex); + GSD->TimeServerIPU32 = UtilIPStringToInt(ResultBufferC8); + bzero(GSD->TimeServerIPC8, DD_CONTROL_BUFFER_SIZE_20); + strcat(GSD->TimeServerIPC8, ResultBufferC8); + pthread_mutex_unlock(&TimeServerIPMutex); + } + else { + Res = PARAMETER_NOTFOUND; + LogMessage(LOG_LEVEL_ERROR, "TimeServerIP not found!"); + } - return Res; + return Res; } /*! @@ -800,19 +800,19 @@ ReadWriteAccess_t DataDictionaryInitTimeServerIPU32(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetTimeServerIPU32(GSDType * GSD, C8 * TimeServerIP) { - ReadWriteAccess_t Res; + ReadWriteAccess_t Res; - if (UtilWriteConfigurationParameter("TimeServerIP", TimeServerIP, 0)) { - Res = WRITE_OK; - pthread_mutex_lock(&TimeServerIPMutex); - GSD->TimeServerIPU32 = UtilIPStringToInt(TimeServerIP); - bzero(GSD->TimeServerIPC8, DD_CONTROL_BUFFER_SIZE_20); - strcat(GSD->TimeServerIPC8, TimeServerIP); - pthread_mutex_unlock(&TimeServerIPMutex); - } - else - Res = PARAMETER_NOTFOUND; - return Res; + if (UtilWriteConfigurationParameter("TimeServerIP", TimeServerIP, 0)) { + Res = WRITE_OK; + pthread_mutex_lock(&TimeServerIPMutex); + GSD->TimeServerIPU32 = UtilIPStringToInt(TimeServerIP); + bzero(GSD->TimeServerIPC8, DD_CONTROL_BUFFER_SIZE_20); + strcat(GSD->TimeServerIPC8, TimeServerIP); + pthread_mutex_unlock(&TimeServerIPMutex); + } + else + Res = PARAMETER_NOTFOUND; + return Res; } /*! @@ -822,10 +822,10 @@ ReadWriteAccess_t DataDictionarySetTimeServerIPU32(GSDType * GSD, C8 * TimeServe * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetTimeServerIPU32(GSDType * GSD, U32 * TimeServerIP) { - pthread_mutex_lock(&TimeServerIPMutex); - *TimeServerIP = GSD->TimeServerIPU32; - pthread_mutex_unlock(&TimeServerIPMutex); - return READ_OK; + pthread_mutex_lock(&TimeServerIPMutex); + *TimeServerIP = GSD->TimeServerIPU32; + pthread_mutex_unlock(&TimeServerIPMutex); + return READ_OK; } /*! @@ -835,11 +835,11 @@ ReadWriteAccess_t DataDictionaryGetTimeServerIPU32(GSDType * GSD, U32 * TimeServ * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetTimeServerIPC8(GSDType * GSD, C8 * TimeServerIP, U32 BuffLen) { - pthread_mutex_lock(&TimeServerIPMutex); - bzero(TimeServerIP, BuffLen); - strcat(TimeServerIP, GSD->TimeServerIPC8); - pthread_mutex_unlock(&TimeServerIPMutex); - return READ_OK; + pthread_mutex_lock(&TimeServerIPMutex); + bzero(TimeServerIP, BuffLen); + strcat(TimeServerIP, GSD->TimeServerIPC8); + pthread_mutex_unlock(&TimeServerIPMutex); + return READ_OK; } /*END of TimeServerIP*/ @@ -852,21 +852,21 @@ ReadWriteAccess_t DataDictionaryGetTimeServerIPC8(GSDType * GSD, C8 * TimeServer * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitTimeServerPortU16(GSDType * GSD) { - ReadWriteAccess_t Res = UNDEFINED; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + ReadWriteAccess_t Res = UNDEFINED; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; - if (DataDictionarySearchParameter("TimeServerPort=", ResultBufferC8)) { - Res = READ_OK; - pthread_mutex_lock(&TimeServerPortMutex); - GSD->TimeServerPortU16 = atoi(ResultBufferC8); - pthread_mutex_unlock(&TimeServerPortMutex); - } - else { - Res = PARAMETER_NOTFOUND; - LogMessage(LOG_LEVEL_ERROR, "TimeServerPort not found!"); - } + if (DataDictionarySearchParameter("TimeServerPort=", ResultBufferC8)) { + Res = READ_OK; + pthread_mutex_lock(&TimeServerPortMutex); + GSD->TimeServerPortU16 = atoi(ResultBufferC8); + pthread_mutex_unlock(&TimeServerPortMutex); + } + else { + Res = PARAMETER_NOTFOUND; + LogMessage(LOG_LEVEL_ERROR, "TimeServerPort not found!"); + } - return Res; + return Res; } /*! @@ -876,17 +876,17 @@ ReadWriteAccess_t DataDictionaryInitTimeServerPortU16(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetTimeServerPortU16(GSDType * GSD, C8 * TimeServerPort) { - ReadWriteAccess_t Res; + ReadWriteAccess_t Res; - if (UtilWriteConfigurationParameter("TimeServerPort", TimeServerPort, 0)) { - Res = WRITE_OK; - pthread_mutex_lock(&TimeServerPortMutex); - GSD->TimeServerPortU16 = atoi(TimeServerPort); - pthread_mutex_unlock(&TimeServerPortMutex); - } - else - Res = PARAMETER_NOTFOUND; - return Res; + if (UtilWriteConfigurationParameter("TimeServerPort", TimeServerPort, 0)) { + Res = WRITE_OK; + pthread_mutex_lock(&TimeServerPortMutex); + GSD->TimeServerPortU16 = atoi(TimeServerPort); + pthread_mutex_unlock(&TimeServerPortMutex); + } + else + Res = PARAMETER_NOTFOUND; + return Res; } /*! @@ -896,10 +896,10 @@ ReadWriteAccess_t DataDictionarySetTimeServerPortU16(GSDType * GSD, C8 * TimeSer * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetTimeServerPortU16(GSDType * GSD, U16 * TimeServerPort) { - pthread_mutex_lock(&TimeServerPortMutex); - *TimeServerPort = GSD->TimeServerPortU16; - pthread_mutex_unlock(&TimeServerPortMutex); - return READ_OK; + pthread_mutex_lock(&TimeServerPortMutex); + *TimeServerPort = GSD->TimeServerPortU16; + pthread_mutex_unlock(&TimeServerPortMutex); + return READ_OK; } /*END of TimeServerPort*/ @@ -912,23 +912,23 @@ ReadWriteAccess_t DataDictionaryGetTimeServerPortU16(GSDType * GSD, U16 * TimeSe * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitSimulatorIPU32(GSDType * GSD) { - ReadWriteAccess_t Res = UNDEFINED; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + ReadWriteAccess_t Res = UNDEFINED; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; - if (DataDictionarySearchParameter("SimulatorIP=", ResultBufferC8)) { - Res = READ_OK; - pthread_mutex_lock(&SimulatorIPMutex); - GSD->SimulatorIPU32 = UtilIPStringToInt(ResultBufferC8); - bzero(GSD->SimulatorIPC8, DD_CONTROL_BUFFER_SIZE_20); - strcat(GSD->SimulatorIPC8, ResultBufferC8); - pthread_mutex_unlock(&SimulatorIPMutex); - } - else { - Res = PARAMETER_NOTFOUND; - LogMessage(LOG_LEVEL_ERROR, "SimulatorIP not found!"); - } + if (DataDictionarySearchParameter("SimulatorIP=", ResultBufferC8)) { + Res = READ_OK; + pthread_mutex_lock(&SimulatorIPMutex); + GSD->SimulatorIPU32 = UtilIPStringToInt(ResultBufferC8); + bzero(GSD->SimulatorIPC8, DD_CONTROL_BUFFER_SIZE_20); + strcat(GSD->SimulatorIPC8, ResultBufferC8); + pthread_mutex_unlock(&SimulatorIPMutex); + } + else { + Res = PARAMETER_NOTFOUND; + LogMessage(LOG_LEVEL_ERROR, "SimulatorIP not found!"); + } - return Res; + return Res; } /*! @@ -938,19 +938,19 @@ ReadWriteAccess_t DataDictionaryInitSimulatorIPU32(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetSimulatorIPU32(GSDType * GSD, C8 * SimulatorIP) { - ReadWriteAccess_t Res; + ReadWriteAccess_t Res; - if (UtilWriteConfigurationParameter("SimulatorIP", SimulatorIP, 0)) { - Res = WRITE_OK; - pthread_mutex_lock(&SimulatorIPMutex); - GSD->SimulatorIPU32 = UtilIPStringToInt(SimulatorIP); - bzero(GSD->SimulatorIPC8, DD_CONTROL_BUFFER_SIZE_20); - strcat(GSD->SimulatorIPC8, SimulatorIP); - pthread_mutex_unlock(&SimulatorIPMutex); - } - else - Res = PARAMETER_NOTFOUND; - return Res; + if (UtilWriteConfigurationParameter("SimulatorIP", SimulatorIP, 0)) { + Res = WRITE_OK; + pthread_mutex_lock(&SimulatorIPMutex); + GSD->SimulatorIPU32 = UtilIPStringToInt(SimulatorIP); + bzero(GSD->SimulatorIPC8, DD_CONTROL_BUFFER_SIZE_20); + strcat(GSD->SimulatorIPC8, SimulatorIP); + pthread_mutex_unlock(&SimulatorIPMutex); + } + else + Res = PARAMETER_NOTFOUND; + return Res; } /*! @@ -960,10 +960,10 @@ ReadWriteAccess_t DataDictionarySetSimulatorIPU32(GSDType * GSD, C8 * SimulatorI * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetSimulatorIPU32(GSDType * GSD, U32 * SimulatorIP) { - pthread_mutex_lock(&SimulatorIPMutex); - *SimulatorIP = GSD->SimulatorIPU32; - pthread_mutex_unlock(&SimulatorIPMutex); - return READ_OK; + pthread_mutex_lock(&SimulatorIPMutex); + *SimulatorIP = GSD->SimulatorIPU32; + pthread_mutex_unlock(&SimulatorIPMutex); + return READ_OK; } /*! @@ -973,11 +973,11 @@ ReadWriteAccess_t DataDictionaryGetSimulatorIPU32(GSDType * GSD, U32 * Simulator * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetSimulatorIPC8(GSDType * GSD, C8 * SimulatorIP, U32 BuffLen) { - pthread_mutex_lock(&SimulatorIPMutex); - bzero(SimulatorIP, BuffLen); - strcat(SimulatorIP, GSD->SimulatorIPC8); - pthread_mutex_unlock(&SimulatorIPMutex); - return READ_OK; + pthread_mutex_lock(&SimulatorIPMutex); + bzero(SimulatorIP, BuffLen); + strcat(SimulatorIP, GSD->SimulatorIPC8); + pthread_mutex_unlock(&SimulatorIPMutex); + return READ_OK; } /*END of SimulatorIP*/ @@ -989,21 +989,21 @@ ReadWriteAccess_t DataDictionaryGetSimulatorIPC8(GSDType * GSD, C8 * SimulatorIP * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitSimulatorTCPPortU16(GSDType * GSD) { - ReadWriteAccess_t Res = UNDEFINED; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + ReadWriteAccess_t Res = UNDEFINED; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; - if (DataDictionarySearchParameter("SimulatorTCPPort=", ResultBufferC8)) { - Res = READ_OK; - pthread_mutex_lock(&SimulatorTCPPortMutex); - GSD->SimulatorTCPPortU16 = atoi(ResultBufferC8); - pthread_mutex_unlock(&SimulatorTCPPortMutex); - } - else { - Res = PARAMETER_NOTFOUND; - LogMessage(LOG_LEVEL_ERROR, "SimulatorTCPPort not found!"); - } + if (DataDictionarySearchParameter("SimulatorTCPPort=", ResultBufferC8)) { + Res = READ_OK; + pthread_mutex_lock(&SimulatorTCPPortMutex); + GSD->SimulatorTCPPortU16 = atoi(ResultBufferC8); + pthread_mutex_unlock(&SimulatorTCPPortMutex); + } + else { + Res = PARAMETER_NOTFOUND; + LogMessage(LOG_LEVEL_ERROR, "SimulatorTCPPort not found!"); + } - return Res; + return Res; } /*! @@ -1013,17 +1013,17 @@ ReadWriteAccess_t DataDictionaryInitSimulatorTCPPortU16(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetSimulatorTCPPortU16(GSDType * GSD, C8 * SimulatorTCPPort) { - ReadWriteAccess_t Res; + ReadWriteAccess_t Res; - if (UtilWriteConfigurationParameter("SimulatorTCPPort", SimulatorTCPPort, 0)) { - Res = WRITE_OK; - pthread_mutex_lock(&SimulatorTCPPortMutex); - GSD->SimulatorTCPPortU16 = atoi(SimulatorTCPPort); - pthread_mutex_unlock(&SimulatorTCPPortMutex); - } - else - Res = PARAMETER_NOTFOUND; - return Res; + if (UtilWriteConfigurationParameter("SimulatorTCPPort", SimulatorTCPPort, 0)) { + Res = WRITE_OK; + pthread_mutex_lock(&SimulatorTCPPortMutex); + GSD->SimulatorTCPPortU16 = atoi(SimulatorTCPPort); + pthread_mutex_unlock(&SimulatorTCPPortMutex); + } + else + Res = PARAMETER_NOTFOUND; + return Res; } /*! @@ -1033,10 +1033,10 @@ ReadWriteAccess_t DataDictionarySetSimulatorTCPPortU16(GSDType * GSD, C8 * Simul * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetSimulatorTCPPortU16(GSDType * GSD, U16 * SimulatorTCPPort) { - pthread_mutex_lock(&SimulatorTCPPortMutex); - *SimulatorTCPPort = GSD->SimulatorTCPPortU16; - pthread_mutex_unlock(&SimulatorTCPPortMutex); - return READ_OK; + pthread_mutex_lock(&SimulatorTCPPortMutex); + *SimulatorTCPPort = GSD->SimulatorTCPPortU16; + pthread_mutex_unlock(&SimulatorTCPPortMutex); + return READ_OK; } /*END of SimulatorTCPPort*/ @@ -1048,21 +1048,21 @@ ReadWriteAccess_t DataDictionaryGetSimulatorTCPPortU16(GSDType * GSD, U16 * Simu * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitSimulatorUDPPortU16(GSDType * GSD) { - ReadWriteAccess_t Res = UNDEFINED; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + ReadWriteAccess_t Res = UNDEFINED; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; - if (DataDictionarySearchParameter("SimulatorUDPPort=", ResultBufferC8)) { - Res = READ_OK; - pthread_mutex_lock(&SimulatorUDPPortMutex); - GSD->SimulatorUDPPortU16 = atoi(ResultBufferC8); - pthread_mutex_unlock(&SimulatorUDPPortMutex); - } - else { - Res = PARAMETER_NOTFOUND; - LogMessage(LOG_LEVEL_ERROR, "SimulatorUDPPort not found!"); - } + if (DataDictionarySearchParameter("SimulatorUDPPort=", ResultBufferC8)) { + Res = READ_OK; + pthread_mutex_lock(&SimulatorUDPPortMutex); + GSD->SimulatorUDPPortU16 = atoi(ResultBufferC8); + pthread_mutex_unlock(&SimulatorUDPPortMutex); + } + else { + Res = PARAMETER_NOTFOUND; + LogMessage(LOG_LEVEL_ERROR, "SimulatorUDPPort not found!"); + } - return Res; + return Res; } /*! @@ -1072,17 +1072,17 @@ ReadWriteAccess_t DataDictionaryInitSimulatorUDPPortU16(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetSimulatorUDPPortU16(GSDType * GSD, C8 * SimulatorUDPPort) { - ReadWriteAccess_t Res; + ReadWriteAccess_t Res; - if (UtilWriteConfigurationParameter("SimulatorUDPPort", SimulatorUDPPort, 0)) { - Res = WRITE_OK; - pthread_mutex_lock(&SimulatorUDPPortMutex); - GSD->SimulatorUDPPortU16 = atoi(SimulatorUDPPort); - pthread_mutex_unlock(&SimulatorUDPPortMutex); - } - else - Res = PARAMETER_NOTFOUND; - return Res; + if (UtilWriteConfigurationParameter("SimulatorUDPPort", SimulatorUDPPort, 0)) { + Res = WRITE_OK; + pthread_mutex_lock(&SimulatorUDPPortMutex); + GSD->SimulatorUDPPortU16 = atoi(SimulatorUDPPort); + pthread_mutex_unlock(&SimulatorUDPPortMutex); + } + else + Res = PARAMETER_NOTFOUND; + return Res; } /*! @@ -1092,10 +1092,10 @@ ReadWriteAccess_t DataDictionarySetSimulatorUDPPortU16(GSDType * GSD, C8 * Simul * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetSimulatorUDPPortU16(GSDType * GSD, U16 * SimulatorUDPPort) { - pthread_mutex_lock(&SimulatorUDPPortMutex); - *SimulatorUDPPort = GSD->SimulatorUDPPortU16; - pthread_mutex_unlock(&SimulatorUDPPortMutex); - return READ_OK; + pthread_mutex_lock(&SimulatorUDPPortMutex); + *SimulatorUDPPort = GSD->SimulatorUDPPortU16; + pthread_mutex_unlock(&SimulatorUDPPortMutex); + return READ_OK; } /*END of SimulatorUDPPort*/ @@ -1107,21 +1107,21 @@ ReadWriteAccess_t DataDictionaryGetSimulatorUDPPortU16(GSDType * GSD, U16 * Simu * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitSimulatorModeU8(GSDType * GSD) { - ReadWriteAccess_t Res = UNDEFINED; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + ReadWriteAccess_t Res = UNDEFINED; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; - if (DataDictionarySearchParameter("SimulatorMode=", ResultBufferC8)) { - Res = READ_OK; - pthread_mutex_lock(&SimulatorModeMutex); - GSD->SimulatorModeU8 = atoi(ResultBufferC8); - pthread_mutex_unlock(&SimulatorModeMutex); - } - else { - Res = PARAMETER_NOTFOUND; - LogMessage(LOG_LEVEL_ERROR, "SimulatorMode not found!"); - } + if (DataDictionarySearchParameter("SimulatorMode=", ResultBufferC8)) { + Res = READ_OK; + pthread_mutex_lock(&SimulatorModeMutex); + GSD->SimulatorModeU8 = atoi(ResultBufferC8); + pthread_mutex_unlock(&SimulatorModeMutex); + } + else { + Res = PARAMETER_NOTFOUND; + LogMessage(LOG_LEVEL_ERROR, "SimulatorMode not found!"); + } - return Res; + return Res; } /*! @@ -1131,17 +1131,17 @@ ReadWriteAccess_t DataDictionaryInitSimulatorModeU8(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetSimulatorModeU8(GSDType * GSD, C8 * SimulatorMode) { - ReadWriteAccess_t Res; + ReadWriteAccess_t Res; - if (UtilWriteConfigurationParameter("SimulatorMode", SimulatorMode, 0)) { - Res = WRITE_OK; - pthread_mutex_lock(&SimulatorModeMutex); - GSD->SimulatorModeU8 = atoi(SimulatorMode); - pthread_mutex_unlock(&SimulatorModeMutex); - } - else - Res = PARAMETER_NOTFOUND; - return Res; + if (UtilWriteConfigurationParameter("SimulatorMode", SimulatorMode, 0)) { + Res = WRITE_OK; + pthread_mutex_lock(&SimulatorModeMutex); + GSD->SimulatorModeU8 = atoi(SimulatorMode); + pthread_mutex_unlock(&SimulatorModeMutex); + } + else + Res = PARAMETER_NOTFOUND; + return Res; } /*! @@ -1151,10 +1151,10 @@ ReadWriteAccess_t DataDictionarySetSimulatorModeU8(GSDType * GSD, C8 * Simulator * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetSimulatorModeU8(GSDType * GSD, U8 * SimulatorMode) { - pthread_mutex_lock(&SimulatorModeMutex); - *SimulatorMode = GSD->SimulatorModeU8; - pthread_mutex_unlock(&SimulatorModeMutex); - return READ_OK; + pthread_mutex_lock(&SimulatorModeMutex); + *SimulatorMode = GSD->SimulatorModeU8; + pthread_mutex_unlock(&SimulatorModeMutex); + return READ_OK; } /*END of SimulatorMode*/ @@ -1166,21 +1166,21 @@ ReadWriteAccess_t DataDictionaryGetSimulatorModeU8(GSDType * GSD, U8 * Simulator * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitVOILReceiversC8(GSDType * GSD) { - ReadWriteAccess_t Res = UNDEFINED; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_1024]; + ReadWriteAccess_t Res = UNDEFINED; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_1024]; - if (DataDictionarySearchParameter("VOILReceivers=", ResultBufferC8)) { - Res = READ_OK; - pthread_mutex_lock(&VOILReceiversMutex); - strcpy(GSD->VOILReceiversC8, ResultBufferC8); - pthread_mutex_unlock(&VOILReceiversMutex); - } - else { - Res = PARAMETER_NOTFOUND; - LogMessage(LOG_LEVEL_ERROR, "VOILReceivers not found!"); - } + if (DataDictionarySearchParameter("VOILReceivers=", ResultBufferC8)) { + Res = READ_OK; + pthread_mutex_lock(&VOILReceiversMutex); + strcpy(GSD->VOILReceiversC8, ResultBufferC8); + pthread_mutex_unlock(&VOILReceiversMutex); + } + else { + Res = PARAMETER_NOTFOUND; + LogMessage(LOG_LEVEL_ERROR, "VOILReceivers not found!"); + } - return Res; + return Res; } /*! @@ -1190,17 +1190,17 @@ ReadWriteAccess_t DataDictionaryInitVOILReceiversC8(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetVOILReceiversC8(GSDType * GSD, C8 * VOILReceivers) { - ReadWriteAccess_t Res; + ReadWriteAccess_t Res; - if (UtilWriteConfigurationParameter("VOILReceivers", VOILReceivers, 0)) { - Res = WRITE_OK; - pthread_mutex_lock(&VOILReceiversMutex); - strcpy(GSD->VOILReceiversC8, VOILReceivers); - pthread_mutex_unlock(&VOILReceiversMutex); - } - else - Res = PARAMETER_NOTFOUND; - return Res; + if (UtilWriteConfigurationParameter("VOILReceivers", VOILReceivers, 0)) { + Res = WRITE_OK; + pthread_mutex_lock(&VOILReceiversMutex); + strcpy(GSD->VOILReceiversC8, VOILReceivers); + pthread_mutex_unlock(&VOILReceiversMutex); + } + else + Res = PARAMETER_NOTFOUND; + return Res; } /*! @@ -1210,11 +1210,11 @@ ReadWriteAccess_t DataDictionarySetVOILReceiversC8(GSDType * GSD, C8 * VOILRecei * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetVOILReceiversC8(GSDType * GSD, U8 * VOILReceivers, U32 BuffLen) { - pthread_mutex_lock(&VOILReceiversMutex); - bzero(VOILReceivers, BuffLen); - strcpy(VOILReceivers, GSD->VOILReceiversC8); - pthread_mutex_unlock(&VOILReceiversMutex); - return READ_OK; + pthread_mutex_lock(&VOILReceiversMutex); + bzero(VOILReceivers, BuffLen); + strcpy(VOILReceivers, GSD->VOILReceiversC8); + pthread_mutex_unlock(&VOILReceiversMutex); + return READ_OK; } /*END of VOILReceivers*/ @@ -1226,21 +1226,21 @@ ReadWriteAccess_t DataDictionaryGetVOILReceiversC8(GSDType * GSD, U8 * VOILRecei * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitDTMReceiversC8(GSDType * GSD) { - ReadWriteAccess_t Res = UNDEFINED; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_1024]; + ReadWriteAccess_t Res = UNDEFINED; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_1024]; - if (DataDictionarySearchParameter("DTMReceivers=", ResultBufferC8)) { - Res = READ_OK; - pthread_mutex_lock(&DTMReceiversMutex); - strcpy(GSD->DTMReceiversC8, ResultBufferC8); - pthread_mutex_unlock(&DTMReceiversMutex); - } - else { - Res = PARAMETER_NOTFOUND; - LogMessage(LOG_LEVEL_ERROR, "DTMReceivers not found!"); - } + if (DataDictionarySearchParameter("DTMReceivers=", ResultBufferC8)) { + Res = READ_OK; + pthread_mutex_lock(&DTMReceiversMutex); + strcpy(GSD->DTMReceiversC8, ResultBufferC8); + pthread_mutex_unlock(&DTMReceiversMutex); + } + else { + Res = PARAMETER_NOTFOUND; + LogMessage(LOG_LEVEL_ERROR, "DTMReceivers not found!"); + } - return Res; + return Res; } /*! @@ -1250,17 +1250,17 @@ ReadWriteAccess_t DataDictionaryInitDTMReceiversC8(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetDTMReceiversC8(GSDType * GSD, C8 * DTMReceivers) { - ReadWriteAccess_t Res; + ReadWriteAccess_t Res; - if (UtilWriteConfigurationParameter("DTMReceivers", DTMReceivers, 0)) { - Res = WRITE_OK; - pthread_mutex_lock(&DTMReceiversMutex); - strcpy(GSD->DTMReceiversC8, DTMReceivers); - pthread_mutex_unlock(&DTMReceiversMutex); - } - else - Res = PARAMETER_NOTFOUND; - return Res; + if (UtilWriteConfigurationParameter("DTMReceivers", DTMReceivers, 0)) { + Res = WRITE_OK; + pthread_mutex_lock(&DTMReceiversMutex); + strcpy(GSD->DTMReceiversC8, DTMReceivers); + pthread_mutex_unlock(&DTMReceiversMutex); + } + else + Res = PARAMETER_NOTFOUND; + return Res; } /*! @@ -1270,11 +1270,11 @@ ReadWriteAccess_t DataDictionarySetDTMReceiversC8(GSDType * GSD, C8 * DTMReceive * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetDTMReceiversC8(GSDType * GSD, U8 * DTMReceivers, U32 BuffLen) { - pthread_mutex_lock(&DTMReceiversMutex); - bzero(DTMReceivers, BuffLen); - strcpy(DTMReceivers, GSD->DTMReceiversC8); - pthread_mutex_unlock(&DTMReceiversMutex); - return READ_OK; + pthread_mutex_lock(&DTMReceiversMutex); + bzero(DTMReceivers, BuffLen); + strcpy(DTMReceivers, GSD->DTMReceiversC8); + pthread_mutex_unlock(&DTMReceiversMutex); + return READ_OK; } /*END of DTMReceivers*/ @@ -1286,24 +1286,24 @@ ReadWriteAccess_t DataDictionaryGetDTMReceiversC8(GSDType * GSD, U8 * DTMReceive * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitExternalSupervisorIPU32(GSDType * GSD) { - ReadWriteAccess_t Res = UNDEFINED; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + ReadWriteAccess_t Res = UNDEFINED; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; - if (DataDictionarySearchParameter("SupervisorIP=", ResultBufferC8)) { - Res = READ_OK; - pthread_mutex_lock(&ExternalSupervisorIPMutex); - GSD->ExternalSupervisorIPU32 = UtilIPStringToInt(ResultBufferC8); - bzero(GSD->ExternalSupervisorIPC8, DD_CONTROL_BUFFER_SIZE_20); - strcat(GSD->ExternalSupervisorIPC8, ResultBufferC8); - pthread_mutex_unlock(&ExternalSupervisorIPMutex); - //LogMessage(LOG_LEVEL_ERROR,"Supervisor IP: %s", ResultBufferC8); - } - else { - Res = PARAMETER_NOTFOUND; - LogMessage(LOG_LEVEL_ERROR, "Supervisor IP not found!"); - } + if (DataDictionarySearchParameter("SupervisorIP=", ResultBufferC8)) { + Res = READ_OK; + pthread_mutex_lock(&ExternalSupervisorIPMutex); + GSD->ExternalSupervisorIPU32 = UtilIPStringToInt(ResultBufferC8); + bzero(GSD->ExternalSupervisorIPC8, DD_CONTROL_BUFFER_SIZE_20); + strcat(GSD->ExternalSupervisorIPC8, ResultBufferC8); + pthread_mutex_unlock(&ExternalSupervisorIPMutex); + //LogMessage(LOG_LEVEL_ERROR,"Supervisor IP: %s", ResultBufferC8); + } + else { + Res = PARAMETER_NOTFOUND; + LogMessage(LOG_LEVEL_ERROR, "Supervisor IP not found!"); + } - return Res; + return Res; } /*! @@ -1313,19 +1313,19 @@ ReadWriteAccess_t DataDictionaryInitExternalSupervisorIPU32(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetExternalSupervisorIPU32(GSDType * GSD, C8 * IP) { - ReadWriteAccess_t Res; + ReadWriteAccess_t Res; - if (UtilWriteConfigurationParameter("SupervisorIP", IP, 0)) { - Res = WRITE_OK; - pthread_mutex_lock(&ExternalSupervisorIPMutex); - GSD->ExternalSupervisorIPU32 = UtilIPStringToInt(IP); - bzero(GSD->ExternalSupervisorIPC8, DD_CONTROL_BUFFER_SIZE_20); - strcat(GSD->ExternalSupervisorIPC8, IP); - pthread_mutex_unlock(&ExternalSupervisorIPMutex); - } - else - Res = PARAMETER_NOTFOUND; - return Res; + if (UtilWriteConfigurationParameter("SupervisorIP", IP, 0)) { + Res = WRITE_OK; + pthread_mutex_lock(&ExternalSupervisorIPMutex); + GSD->ExternalSupervisorIPU32 = UtilIPStringToInt(IP); + bzero(GSD->ExternalSupervisorIPC8, DD_CONTROL_BUFFER_SIZE_20); + strcat(GSD->ExternalSupervisorIPC8, IP); + pthread_mutex_unlock(&ExternalSupervisorIPMutex); + } + else + Res = PARAMETER_NOTFOUND; + return Res; } /*! @@ -1335,10 +1335,10 @@ ReadWriteAccess_t DataDictionarySetExternalSupervisorIPU32(GSDType * GSD, C8 * I * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetExternalSupervisorIPU32(GSDType * GSD, U32 * IP) { - pthread_mutex_lock(&ExternalSupervisorIPMutex); - *IP = GSD->ExternalSupervisorIPU32; - pthread_mutex_unlock(&ExternalSupervisorIPMutex); - return READ_OK; + pthread_mutex_lock(&ExternalSupervisorIPMutex); + *IP = GSD->ExternalSupervisorIPU32; + pthread_mutex_unlock(&ExternalSupervisorIPMutex); + return READ_OK; } /*! @@ -1348,11 +1348,11 @@ ReadWriteAccess_t DataDictionaryGetExternalSupervisorIPU32(GSDType * GSD, U32 * * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetExternalSupervisorIPC8(GSDType * GSD, C8 * IP, U32 BuffLen) { - pthread_mutex_lock(&ExternalSupervisorIPMutex); - bzero(IP, BuffLen); - strcat(IP, GSD->ExternalSupervisorIPC8); - pthread_mutex_unlock(&ExternalSupervisorIPMutex); - return READ_OK; + pthread_mutex_lock(&ExternalSupervisorIPMutex); + bzero(IP, BuffLen); + strcat(IP, GSD->ExternalSupervisorIPC8); + pthread_mutex_unlock(&ExternalSupervisorIPMutex); + return READ_OK; } /*END of External Supervisor IP*/ @@ -1364,21 +1364,21 @@ ReadWriteAccess_t DataDictionaryGetExternalSupervisorIPC8(GSDType * GSD, C8 * IP * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitSupervisorTCPPortU16(GSDType * GSD) { - ReadWriteAccess_t Res = UNDEFINED; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + ReadWriteAccess_t Res = UNDEFINED; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; - if (DataDictionarySearchParameter("SupervisorTCPPort=", ResultBufferC8)) { - Res = READ_OK; - pthread_mutex_lock(&SupervisorTCPPortMutex); - GSD->SupervisorTCPPortU16 = atoi(ResultBufferC8); - pthread_mutex_unlock(&SupervisorTCPPortMutex); - } - else { - Res = PARAMETER_NOTFOUND; - LogMessage(LOG_LEVEL_ERROR, "SupervisorTCPPort not found!"); - } + if (DataDictionarySearchParameter("SupervisorTCPPort=", ResultBufferC8)) { + Res = READ_OK; + pthread_mutex_lock(&SupervisorTCPPortMutex); + GSD->SupervisorTCPPortU16 = atoi(ResultBufferC8); + pthread_mutex_unlock(&SupervisorTCPPortMutex); + } + else { + Res = PARAMETER_NOTFOUND; + LogMessage(LOG_LEVEL_ERROR, "SupervisorTCPPort not found!"); + } - return Res; + return Res; } /*! @@ -1388,17 +1388,17 @@ ReadWriteAccess_t DataDictionaryInitSupervisorTCPPortU16(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetSupervisorTCPPortU16(GSDType * GSD, C8 * SupervisorTCPPort) { - ReadWriteAccess_t Res; + ReadWriteAccess_t Res; - if (UtilWriteConfigurationParameter("SupervisorTCPPort", SupervisorTCPPort, 0)) { - Res = WRITE_OK; - pthread_mutex_lock(&SupervisorTCPPortMutex); - GSD->SupervisorTCPPortU16 = atoi(SupervisorTCPPort); - pthread_mutex_unlock(&SupervisorTCPPortMutex); - } - else - Res = PARAMETER_NOTFOUND; - return Res; + if (UtilWriteConfigurationParameter("SupervisorTCPPort", SupervisorTCPPort, 0)) { + Res = WRITE_OK; + pthread_mutex_lock(&SupervisorTCPPortMutex); + GSD->SupervisorTCPPortU16 = atoi(SupervisorTCPPort); + pthread_mutex_unlock(&SupervisorTCPPortMutex); + } + else + Res = PARAMETER_NOTFOUND; + return Res; } /*! @@ -1408,10 +1408,10 @@ ReadWriteAccess_t DataDictionarySetSupervisorTCPPortU16(GSDType * GSD, C8 * Supe * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetSupervisorTCPPortU16(GSDType * GSD, U16 * SupervisorTCPPort) { - pthread_mutex_lock(&SupervisorTCPPortMutex); - *SupervisorTCPPort = GSD->SupervisorTCPPortU16; - pthread_mutex_unlock(&SupervisorTCPPortMutex); - return READ_OK; + pthread_mutex_lock(&SupervisorTCPPortMutex); + *SupervisorTCPPort = GSD->SupervisorTCPPortU16; + pthread_mutex_unlock(&SupervisorTCPPortMutex); + return READ_OK; } /*END of External SupervisorTCPPort*/ @@ -1423,22 +1423,22 @@ ReadWriteAccess_t DataDictionaryGetSupervisorTCPPortU16(GSDType * GSD, U16 * Sup * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitRVSSConfigU32(GSDType * GSD) { - ReadWriteAccess_t Res = UNDEFINED; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + ReadWriteAccess_t Res = UNDEFINED; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; - if (DataDictionarySearchParameter("RVSSConfig=", ResultBufferC8)) { - Res = READ_OK; - pthread_mutex_lock(&DataDictionaryRVSSConfigMutex); - GSD->DataDictionaryRVSSConfigU32 = atoi(ResultBufferC8); - pthread_mutex_unlock(&DataDictionaryRVSSConfigMutex); - //LogMessage(LOG_LEVEL_ERROR,"RVSSConfig: %s", ResultBufferC8); - } - else { - Res = PARAMETER_NOTFOUND; - LogMessage(LOG_LEVEL_ERROR, "RVSSConfig not found!"); - } + if (DataDictionarySearchParameter("RVSSConfig=", ResultBufferC8)) { + Res = READ_OK; + pthread_mutex_lock(&DataDictionaryRVSSConfigMutex); + GSD->DataDictionaryRVSSConfigU32 = atoi(ResultBufferC8); + pthread_mutex_unlock(&DataDictionaryRVSSConfigMutex); + //LogMessage(LOG_LEVEL_ERROR,"RVSSConfig: %s", ResultBufferC8); + } + else { + Res = PARAMETER_NOTFOUND; + LogMessage(LOG_LEVEL_ERROR, "RVSSConfig not found!"); + } - return Res; + return Res; } /*! @@ -1448,21 +1448,21 @@ ReadWriteAccess_t DataDictionaryInitRVSSConfigU32(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetRVSSConfigU32(GSDType * GSD, U32 RVSSConfig) { - ReadWriteAccess_t Res; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + ReadWriteAccess_t Res; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; - bzero(ResultBufferC8, DD_CONTROL_BUFFER_SIZE_20); - sprintf(ResultBufferC8, "%" PRIu32, RVSSConfig); + bzero(ResultBufferC8, DD_CONTROL_BUFFER_SIZE_20); + sprintf(ResultBufferC8, "%" PRIu32, RVSSConfig); - if (UtilWriteConfigurationParameter("RVSSConfig", ResultBufferC8, 0)) { - Res = WRITE_OK; - pthread_mutex_lock(&DataDictionaryRVSSConfigMutex); - GSD->DataDictionaryRVSSConfigU32 = RVSSConfig; - pthread_mutex_unlock(&DataDictionaryRVSSConfigMutex); - } - else - Res = PARAMETER_NOTFOUND; - return Res; + if (UtilWriteConfigurationParameter("RVSSConfig", ResultBufferC8, 0)) { + Res = WRITE_OK; + pthread_mutex_lock(&DataDictionaryRVSSConfigMutex); + GSD->DataDictionaryRVSSConfigU32 = RVSSConfig; + pthread_mutex_unlock(&DataDictionaryRVSSConfigMutex); + } + else + Res = PARAMETER_NOTFOUND; + return Res; } /*! @@ -1472,10 +1472,10 @@ ReadWriteAccess_t DataDictionarySetRVSSConfigU32(GSDType * GSD, U32 RVSSConfig) * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetRVSSConfigU32(GSDType * GSD, U32 * RVSSConfig) { - pthread_mutex_lock(&DataDictionaryRVSSConfigMutex); - *RVSSConfig = GSD->DataDictionaryRVSSConfigU32; - pthread_mutex_unlock(&DataDictionaryRVSSConfigMutex); - return READ_OK; + pthread_mutex_lock(&DataDictionaryRVSSConfigMutex); + *RVSSConfig = GSD->DataDictionaryRVSSConfigU32; + pthread_mutex_unlock(&DataDictionaryRVSSConfigMutex); + return READ_OK; } /*END of Runtime Variable Subscription Service (RVSS) Configuration**/ @@ -1488,22 +1488,22 @@ ReadWriteAccess_t DataDictionaryGetRVSSConfigU32(GSDType * GSD, U32 * RVSSConfig * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitRVSSRateU8(GSDType * GSD) { - ReadWriteAccess_t Res = UNDEFINED; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + ReadWriteAccess_t Res = UNDEFINED; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; - if (DataDictionarySearchParameter("RVSSRate=", ResultBufferC8)) { - Res = READ_OK; - pthread_mutex_lock(&DataDictionaryRVSSRateMutex); - GSD->DataDictionaryRVSSRateU8 = (U8) atoi(ResultBufferC8); - pthread_mutex_unlock(&DataDictionaryRVSSRateMutex); - //LogMessage(LOG_LEVEL_ERROR,"RVSSRate: %s", ResultBufferC8); - } - else { - Res = PARAMETER_NOTFOUND; - LogMessage(LOG_LEVEL_ERROR, "RVSSRate not found!"); - } + if (DataDictionarySearchParameter("RVSSRate=", ResultBufferC8)) { + Res = READ_OK; + pthread_mutex_lock(&DataDictionaryRVSSRateMutex); + GSD->DataDictionaryRVSSRateU8 = (U8) atoi(ResultBufferC8); + pthread_mutex_unlock(&DataDictionaryRVSSRateMutex); + //LogMessage(LOG_LEVEL_ERROR,"RVSSRate: %s", ResultBufferC8); + } + else { + Res = PARAMETER_NOTFOUND; + LogMessage(LOG_LEVEL_ERROR, "RVSSRate not found!"); + } - return Res; + return Res; } /*! @@ -1513,21 +1513,21 @@ ReadWriteAccess_t DataDictionaryInitRVSSRateU8(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetRVSSRateU8(GSDType * GSD, U8 RVSSRate) { - ReadWriteAccess_t Res; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + ReadWriteAccess_t Res; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; - bzero(ResultBufferC8, DD_CONTROL_BUFFER_SIZE_20); - sprintf(ResultBufferC8, "%" PRIu8, RVSSRate); + bzero(ResultBufferC8, DD_CONTROL_BUFFER_SIZE_20); + sprintf(ResultBufferC8, "%" PRIu8, RVSSRate); - if (UtilWriteConfigurationParameter("RVSSRate", ResultBufferC8, 0)) { - Res = WRITE_OK; - pthread_mutex_lock(&DataDictionaryRVSSRateMutex); - GSD->DataDictionaryRVSSRateU8 = RVSSRate; - pthread_mutex_unlock(&DataDictionaryRVSSRateMutex); - } - else - Res = PARAMETER_NOTFOUND; - return Res; + if (UtilWriteConfigurationParameter("RVSSRate", ResultBufferC8, 0)) { + Res = WRITE_OK; + pthread_mutex_lock(&DataDictionaryRVSSRateMutex); + GSD->DataDictionaryRVSSRateU8 = RVSSRate; + pthread_mutex_unlock(&DataDictionaryRVSSRateMutex); + } + else + Res = PARAMETER_NOTFOUND; + return Res; } /*! @@ -1537,10 +1537,10 @@ ReadWriteAccess_t DataDictionarySetRVSSRateU8(GSDType * GSD, U8 RVSSRate) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetRVSSRateU8(GSDType * GSD, U8 * RVSSRate) { - pthread_mutex_lock(&DataDictionaryRVSSRateMutex); - *RVSSRate = GSD->DataDictionaryRVSSRateU8; - pthread_mutex_unlock(&DataDictionaryRVSSRateMutex); - return READ_OK; + pthread_mutex_lock(&DataDictionaryRVSSRateMutex); + *RVSSRate = GSD->DataDictionaryRVSSRateU8; + pthread_mutex_unlock(&DataDictionaryRVSSRateMutex); + return READ_OK; } /*END of Runtime Variable Subscription Service (RVSS) Rate**/ @@ -1554,10 +1554,10 @@ ReadWriteAccess_t DataDictionaryGetRVSSRateU8(GSDType * GSD, U8 * RVSSRate) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetRVSSAsp(GSDType * GSD, ASPType * ASPD) { - pthread_mutex_lock(&ASPDataMutex); - GSD->ASPData = *ASPD; - pthread_mutex_unlock(&ASPDataMutex); - return WRITE_OK; + pthread_mutex_lock(&ASPDataMutex); + GSD->ASPData = *ASPD; + pthread_mutex_unlock(&ASPDataMutex); + return WRITE_OK; } /*! @@ -1567,10 +1567,10 @@ ReadWriteAccess_t DataDictionarySetRVSSAsp(GSDType * GSD, ASPType * ASPD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetRVSSAsp(GSDType * GSD, ASPType * ASPD) { - pthread_mutex_lock(&ASPDataMutex); - *ASPD = GSD->ASPData; - pthread_mutex_unlock(&ASPDataMutex); - return READ_OK; + pthread_mutex_lock(&ASPDataMutex); + *ASPD = GSD->ASPData; + pthread_mutex_unlock(&ASPDataMutex); + return READ_OK; } /*END ASPDebug*/ @@ -1582,21 +1582,21 @@ ReadWriteAccess_t DataDictionaryGetRVSSAsp(GSDType * GSD, ASPType * ASPD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitMiscDataC8(GSDType * GSD) { - ReadWriteAccess_t Res = UNDEFINED; - C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; + ReadWriteAccess_t Res = UNDEFINED; + C8 ResultBufferC8[DD_CONTROL_BUFFER_SIZE_20]; - if (DataDictionarySearchParameter("MiscData=", ResultBufferC8)) { - Res = READ_OK; - pthread_mutex_lock(&MiscDataMutex); - strcpy(GSD->MiscDataC8, ResultBufferC8); - pthread_mutex_unlock(&MiscDataMutex); - } - else { - Res = PARAMETER_NOTFOUND; - LogMessage(LOG_LEVEL_ERROR, "MiscData not found!"); - } + if (DataDictionarySearchParameter("MiscData=", ResultBufferC8)) { + Res = READ_OK; + pthread_mutex_lock(&MiscDataMutex); + strcpy(GSD->MiscDataC8, ResultBufferC8); + pthread_mutex_unlock(&MiscDataMutex); + } + else { + Res = PARAMETER_NOTFOUND; + LogMessage(LOG_LEVEL_ERROR, "MiscData not found!"); + } - return Res; + return Res; } /*! @@ -1606,17 +1606,18 @@ ReadWriteAccess_t DataDictionaryInitMiscDataC8(GSDType * GSD) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetMiscDataC8(GSDType * GSD, C8 * MiscData) { - ReadWriteAccess_t Res; - if (UtilWriteConfigurationParameter("MiscData", MiscData, 0)) { - Res = WRITE_OK; - pthread_mutex_lock(&MiscDataMutex); - bzero(GSD->MiscDataC8, DD_CONTROL_BUFFER_SIZE_1024); - strcpy(GSD->MiscDataC8, MiscData); - pthread_mutex_unlock(&MiscDataMutex); - } - else - Res = PARAMETER_NOTFOUND; - return Res; + ReadWriteAccess_t Res; + + if (UtilWriteConfigurationParameter("MiscData", MiscData, 0)) { + Res = WRITE_OK; + pthread_mutex_lock(&MiscDataMutex); + bzero(GSD->MiscDataC8, DD_CONTROL_BUFFER_SIZE_1024); + strcpy(GSD->MiscDataC8, MiscData); + pthread_mutex_unlock(&MiscDataMutex); + } + else + Res = PARAMETER_NOTFOUND; + return Res; } /*! @@ -1626,11 +1627,11 @@ ReadWriteAccess_t DataDictionarySetMiscDataC8(GSDType * GSD, C8 * MiscData) { * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetMiscDataC8(GSDType * GSD, U8 * MiscData, U32 BuffLen) { - pthread_mutex_lock(&MiscDataMutex); - bzero(MiscData, BuffLen); - strcpy(MiscData, GSD->MiscDataC8); - pthread_mutex_unlock(&MiscDataMutex); - return READ_OK; + pthread_mutex_lock(&MiscDataMutex); + bzero(MiscData, BuffLen); + strcpy(MiscData, GSD->MiscDataC8); + pthread_mutex_unlock(&MiscDataMutex); + return READ_OK; } /*END of MiscData*/ @@ -1644,12 +1645,13 @@ ReadWriteAccess_t DataDictionaryGetMiscDataC8(GSDType * GSD, U8 * MiscData, U32 * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetOBCStateU8(GSDType * GSD, OBCState_t OBCState) { - ReadWriteAccess_t Res; - Res = WRITE_OK; - pthread_mutex_lock(&OBCStateMutex); - GSD->OBCStateU8 = OBCState; - pthread_mutex_unlock(&OBCStateMutex); - return Res; + ReadWriteAccess_t Res; + + Res = WRITE_OK; + pthread_mutex_lock(&OBCStateMutex); + GSD->OBCStateU8 = OBCState; + pthread_mutex_unlock(&OBCStateMutex); + return Res; } /*! @@ -1658,11 +1660,12 @@ ReadWriteAccess_t DataDictionarySetOBCStateU8(GSDType * GSD, OBCState_t OBCState * \return Current object control state according to ::OBCState_t */ OBCState_t DataDictionaryGetOBCStateU8(GSDType * GSD) { - OBCState_t Ret; - pthread_mutex_lock(&OBCStateMutex); - Ret = GSD->OBCStateU8; - pthread_mutex_unlock(&OBCStateMutex); - return Ret; + OBCState_t Ret; + + pthread_mutex_lock(&OBCStateMutex); + Ret = GSD->OBCStateU8; + pthread_mutex_unlock(&OBCStateMutex); + return Ret; } /*END OBCState*/ @@ -1673,36 +1676,38 @@ OBCState_t DataDictionaryGetOBCStateU8(GSDType * GSD) { * \param objects number of objects that will transmitt monr * \return Result according to ::ReadWriteAccess_t */ -ReadWriteAccess_t DataDictionaryInitMONR(GSDType * GSD){ - ReadWriteAccess_t Res; - Res = WRITE_OK; - char filePath[PATH_MAX]; - int fd; - struct stat st; +ReadWriteAccess_t DataDictionaryInitMONR(GSDType * GSD) { + ReadWriteAccess_t Res; + + Res = WRITE_OK; + char filePath[PATH_MAX]; + int fd; + struct stat st; - pthread_mutex_lock(&MONRMutex); - sprintf(filePath, "%stest%d.mem", SHARED_MEMORY_PATH, 5); - fd = open(filePath, O_RDWR | O_CREAT, S_IRUSR | S_IWUSR); - stat(filePath, &st); + pthread_mutex_lock(&MONRMutex); + sprintf(filePath, "%stest%d.mem", SHARED_MEMORY_PATH, 5); + fd = open(filePath, O_RDWR | O_CREAT, S_IRUSR | S_IWUSR); + stat(filePath, &st); - //printf("File size on open: %ld\n", st.st_size); + //printf("File size on open: %ld\n", st.st_size); - lseek(fd, (sizeof (MONRType)) - 1, SEEK_SET); - write(fd, "", 1); + lseek(fd, (sizeof (MONRType)) - 1, SEEK_SET); + write(fd, "", 1); - stat(filePath, &st); - //printf("File size after writing: %ld\n", st.st_size); + stat(filePath, &st); + //printf("File size after writing: %ld\n", st.st_size); - // Map memory to created file - GSD->MonrMessages = (MONRType *) mmap(NULL, (sizeof (MONRType)), PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0); + // Map memory to created file + GSD->MonrMessages = + (MONRType *) mmap(NULL, (sizeof (MONRType)), PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0); - LogPrint("Initing"); + LogPrint("Initing"); - //munmap(memory, 6); + //munmap(memory, 6); - pthread_mutex_unlock(&MONRMutex); - return Res; + pthread_mutex_unlock(&MONRMutex); + return Res; } /*! @@ -1713,13 +1718,14 @@ ReadWriteAccess_t DataDictionaryInitMONR(GSDType * GSD){ * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, MONRType * MONR, U8 id) { - ReadWriteAccess_t Res; - Res = WRITE_OK; - pthread_mutex_lock(&MONRMutex); - GSD->MonrMessages[id] = *MONR; - LogPrint("Setting %d", GSD->MonrMessages[id].HeadingU16); - pthread_mutex_unlock(&MONRMutex); - return Res; + ReadWriteAccess_t Res; + + Res = WRITE_OK; + pthread_mutex_lock(&MONRMutex); + GSD->MonrMessages[id] = *MONR; + LogPrint("Setting %d", GSD->MonrMessages[id].HeadingU16); + pthread_mutex_unlock(&MONRMutex); + return Res; } /*! @@ -1729,11 +1735,11 @@ ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, MONRType * MONR, U8 id) { * \param id requesed object id * \return Result according to ::ReadWriteAccess_t */ -ReadWriteAccess_t DataDictionaryGetMONR(GSDType * GSD, MONRType *MONR, U8 id) { - pthread_mutex_lock(&MONRMutex); - *MONR = GSD->MonrMessages[id]; - pthread_mutex_unlock(&MONRMutex); - return READ_OK; +ReadWriteAccess_t DataDictionaryGetMONR(GSDType * GSD, MONRType * MONR, U8 id) { + pthread_mutex_lock(&MONRMutex); + *MONR = GSD->MonrMessages[id]; + pthread_mutex_unlock(&MONRMutex); + return READ_OK; } /*! @@ -1742,14 +1748,15 @@ ReadWriteAccess_t DataDictionaryGetMONR(GSDType * GSD, MONRType *MONR, U8 id) { * \param objects number of objects that will transmitt monr * \return Result according to ::ReadWriteAccess_t */ -ReadWriteAccess_t DataDictionaryFreeMONR(GSDType * GSD){ - ReadWriteAccess_t Res; - Res = WRITE_OK; - pthread_mutex_lock(&MONRMutex); - munmap(GSD->MonrMessages, sizeof (MONRType)); - free(GSD->MonrMessages); - pthread_mutex_unlock(&MONRMutex); - return Res; +ReadWriteAccess_t DataDictionaryFreeMONR(GSDType * GSD) { + ReadWriteAccess_t Res; + + Res = WRITE_OK; + pthread_mutex_lock(&MONRMutex); + munmap(GSD->MonrMessages, sizeof (MONRType)); + free(GSD->MonrMessages); + pthread_mutex_unlock(&MONRMutex); + return Res; } /*END of MONR*/ @@ -1762,13 +1769,14 @@ ReadWriteAccess_t DataDictionaryFreeMONR(GSDType * GSD){ * \param OBCState * \return Result according to ::ReadWriteAccess_t */ -ReadWriteAccess_t DataDictionarySetNumberOfObjectsU8(GSDType * GSD, U8 *numberOfObjects) { - ReadWriteAccess_t Res; - Res = WRITE_OK; - pthread_mutex_lock(&numberOfObjectsMutex); - GSD->numberOfObjects = *numberOfObjects; - pthread_mutex_unlock(&numberOfObjectsMutex); - return Res; +ReadWriteAccess_t DataDictionarySetNumberOfObjectsU8(GSDType * GSD, U8 * numberOfObjects) { + ReadWriteAccess_t Res; + + Res = WRITE_OK; + pthread_mutex_lock(&numberOfObjectsMutex); + GSD->numberOfObjects = *numberOfObjects; + pthread_mutex_unlock(&numberOfObjectsMutex); + return Res; } /*! @@ -1776,11 +1784,11 @@ ReadWriteAccess_t DataDictionarySetNumberOfObjectsU8(GSDType * GSD, U8 *numberOf * \param GSD Pointer to shared allocated memory * \return Current object control state according to ::OBCState_t */ -ReadWriteAccess_t DataDictionaryGetNumberOfObjectsU8(GSDType * GSD, U8 *numberOfObjects) { - pthread_mutex_lock(&numberOfObjectsMutex); - *numberOfObjects = GSD->numberOfObjects; - pthread_mutex_unlock(&numberOfObjectsMutex); - return READ_OK; +ReadWriteAccess_t DataDictionaryGetNumberOfObjectsU8(GSDType * GSD, U8 * numberOfObjects) { + pthread_mutex_lock(&numberOfObjectsMutex); + *numberOfObjects = GSD->numberOfObjects; + pthread_mutex_unlock(&numberOfObjectsMutex); + return READ_OK; } /*END of NbrOfObjects*/ @@ -1794,11 +1802,11 @@ the parameter value. * \return Length of read parameter string */ U64 DataDictionarySearchParameter(C8 * ParameterName, C8 * ResultBuffer) { - char confPathDir[MAX_FILE_PATH]; + char confPathDir[MAX_FILE_PATH]; - UtilGetConfDirectoryPath(confPathDir, sizeof (confPathDir)); - strcat(confPathDir, CONF_FILE_NAME); - bzero(ResultBuffer, DD_CONTROL_BUFFER_SIZE_20); - UtilSearchTextFile(confPathDir, ParameterName, "", ResultBuffer); - return strlen(ResultBuffer); + UtilGetConfDirectoryPath(confPathDir, sizeof (confPathDir)); + strcat(confPathDir, CONF_FILE_NAME); + bzero(ResultBuffer, DD_CONTROL_BUFFER_SIZE_20); + UtilSearchTextFile(confPathDir, ParameterName, "", ResultBuffer); + return strlen(ResultBuffer); } diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 269d1503e..a32cb058b 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -134,16 +134,16 @@ typedef enum { - COMMAND_HEARTBEAT_GO, - COMMAND_HEARTBEAT_ABORT + COMMAND_HEARTBEAT_GO, + COMMAND_HEARTBEAT_ABORT } hearbeatCommand_t; typedef enum { - TRANSITION_RESULT_UNDEFINED, - TRANSITION_OK, - TRANSITION_INVALID, - TRANSITION_MEMORY_ERROR + TRANSITION_RESULT_UNDEFINED, + TRANSITION_OK, + TRANSITION_INVALID, + TRANSITION_MEMORY_ERROR } StateTransitionResult; /* Small note: syntax for declaring a function pointer is (example for a function taking an int and a float, @@ -158,7 +158,7 @@ typedef StateTransitionResult(*StateTransition) (OBCState_t * currentState, OBCS C8 TrajBuffer[COMMAND_DOTM_ROWS_IN_TRANSMISSION * COMMAND_DOTM_ROW_MESSAGE_LENGTH + - COMMAND_MESSAGE_HEADER_LENGTH + COMMAND_TRAJ_INFO_ROW_MESSAGE_LENGTH]; + COMMAND_MESSAGE_HEADER_LENGTH + COMMAND_TRAJ_INFO_ROW_MESSAGE_LENGTH]; /*------------------------------------------------------------ @@ -169,40 +169,40 @@ static void vDisconnectObject(int *sockfd); static I32 vCheckRemoteDisconnected(int *sockfd); static void vCreateSafetyChannel(const char *name, const uint32_t port, int *sockfd, - struct sockaddr_in *addr); + struct sockaddr_in *addr); static void vCloseSafetyChannel(int *sockfd); I32 ObjectControlBuildOSEMMessage(C8 * MessageBuffer, OSEMType * OSEMData, TimeType * GPSTime, C8 * Latitude, - C8 * Longitude, C8 * Altitude, U8 debug); + C8 * Longitude, C8 * Altitude, U8 debug); static size_t uiRecvMonitor(int *sockfd, char *buffer, size_t length); static int iGetObjectIndexFromObjectIP(in_addr_t ipAddr, in_addr_t objectIPs[], unsigned int numberOfObjects); static void signalHandler(int signo); I32 ObjectControlBuildSTRTMessage(C8 * MessageBuffer, STRTType * STRTData, TimeType * GPSTime, - U32 ScenarioStartTime, U32 DelayStart, U32 * OutgoingStartTime, U8 debug); + U32 ScenarioStartTime, U32 DelayStart, U32 * OutgoingStartTime, U8 debug); I32 ObjectControlBuildOSTMMessage(C8 * MessageBuffer, OSTMType * OSTMData, C8 CommandOption, U8 debug); I32 ObjectControlBuildHEABMessage(C8 * MessageBuffer, HEABType * HEABData, TimeType * GPSTime, U8 CCStatus, - U8 debug); + U8 debug); int ObjectControlBuildLLCMMessage(char *MessageBuffer, unsigned short Speed, unsigned short Curvature, - unsigned char Mode, char debug); + unsigned char Mode, char debug); I32 ObjectControlBuildSYPMMessage(C8 * MessageBuffer, SYPMType * SYPMData, U32 SyncPoint, U32 StopTime, - U8 debug); + U8 debug); I32 ObjectControlBuildMTSPMessage(C8 * MessageBuffer, MTSPType * MTSPData, U32 SyncTimestamp, U8 debug); I32 ObjectControlBuildTRAJMessageHeader(C8 * MessageBuffer, I32 * RowCount, HeaderType * HeaderData, - TRAJInfoType * TRAJInfoData, C8 * TrajFileHeader, U8 debug); + TRAJInfoType * TRAJInfoData, C8 * TrajFileHeader, U8 debug); I32 ObjectControlBuildTRAJMessage(C8 * MessageBuffer, FILE * fd, I32 RowCount, DOTMType * DOTMData, U8 debug); I32 ObjectControlSendTRAJMessage(C8 * Filename, I32 * Socket, I32 RowCount, C8 * IP, U32 Port, - DOTMType * DOTMData, U8 debug); + DOTMType * DOTMData, U8 debug); int ObjectControlSendUDPData(int *sockfd, struct sockaddr_in *addr, char *SendData, int Length, char debug); I32 ObjectControlMONRToASCII(MONRType * MONRData, GeoPosition * OriginPosition, I32 Idn, C8 * Id, - C8 * Timestamp, C8 * XPosition, C8 * YPosition, C8 * ZPosition, - C8 * LongitudinalSpeed, C8 * LateralSpeed, C8 * LongitudinalAcc, C8 * LateralAcc, - C8 * Heading, C8 * DriveDirection, C8 * ObjectState, C8 * ReadyToArm, - C8 * ErrorStatus, C8 debug); + C8 * Timestamp, C8 * XPosition, C8 * YPosition, C8 * ZPosition, + C8 * LongitudinalSpeed, C8 * LateralSpeed, C8 * LongitudinalAcc, C8 * LateralAcc, + C8 * Heading, C8 * DriveDirection, C8 * ObjectState, C8 * ReadyToArm, + C8 * ErrorStatus, C8 debug); I32 ObjectControlBuildMONRMessage(C8 * MonrData, MONRType * MONRData, U8 debug); I32 ObjectControlBuildVOILMessage(C8 * MessageBuffer, VOILType * VOILData, C8 * SimData, U8 debug); I32 ObjectControlSendDTMMessage(C8 * DTMData, I32 * Socket, I32 RowCount, C8 * IP, U32 Port, - DOTMType * DOTMData, U8 debug); + DOTMType * DOTMData, U8 debug); I32 ObjectControlBuildDTMMessage(C8 * MessageBuffer, C8 * DTMData, I32 RowCount, DOTMType * DOTMData, - U8 debug); + U8 debug); I32 ObjectControlBuildASPMessage(C8 * MessageBuffer, ASPType * ASPData, U8 debug); I32 ObjectControlBuildACCMMessage(ACCMData * mqACCMData, ACCMType * ACCM, U8 debug); I32 ObjectControlBuildEXACMessage(EXACData * mqEXACData, EXACType * EXAC, U8 debug); @@ -212,8 +212,8 @@ I32 ObjectControlSendTRCMMessage(TRCMData * TRCM, I32 * socket, U8 debug); I32 ObjectControlSendEXACMessage(EXACData * EXAC, I32 * socket, U8 debug); static int iFindObjectsInfo(C8 object_traj_file[MAX_OBJECTS][MAX_FILE_PATH], - C8 object_address_name[MAX_OBJECTS][MAX_FILE_PATH], - in_addr_t objectIPs[MAX_OBJECTS], I32 * nbr_objects); + C8 object_address_name[MAX_OBJECTS][MAX_FILE_PATH], + in_addr_t objectIPs[MAX_OBJECTS], I32 * nbr_objects); OBCState_t vInitializeState(OBCState_t firstState, GSDType * GSD); inline OBCState_t vGetState(GSDType * GSD); @@ -239,881 +239,883 @@ static volatile int iExit = 0; ------------------------------------------------------------*/ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { - I32 safety_socket_fd[MAX_OBJECTS]; - struct sockaddr_in safety_object_addr[MAX_OBJECTS]; - I32 socket_fds[MAX_OBJECTS]; - I32 socket_fd = 0; - C8 object_traj_file[MAX_OBJECTS][MAX_FILE_PATH]; - C8 object_address_name[MAX_OBJECTS][MAX_FILE_PATH]; - in_addr_t objectIPs[MAX_OBJECTS]; - U32 object_udp_port[MAX_OBJECTS]; - U32 object_tcp_port[MAX_OBJECTS]; - I32 nbr_objects = 0; - enum COMMAND iCommand; - U8 pcRecvBuffer[RECV_MESSAGE_BUFFER]; - C8 pcTempBuffer[512]; - C8 MessageBuffer[BUFFER_SIZE_3100]; - I32 iIndex = 0, i = 0; - struct timespec sleep_time, ref_time; - - /*! Timers for reporting state over message bus */ - const struct timespec mqEmptyPollPeriod = { OC_SLEEP_TIME_EMPTY_MQ_S, OC_SLEEP_TIME_EMPTY_MQ_NS }; - const struct timespec mqNonEmptyPollPeriod = - { OC_SLEEP_TIME_NONEMPTY_MQ_S, OC_SLEEP_TIME_NONEMPTY_MQ_NS }; - const struct timeval stateReportPeriod = { OC_STATE_REPORT_PERIOD_S, OC_STATE_REPORT_PERIOD_US }; - struct timeval currentTime, nextStateReportTime; - U8 iForceObjectToLocalhostU8 = 0; - - FILE *fd; - C8 Id[SMALL_BUFFER_SIZE_0]; - C8 GPSWeek[SMALL_BUFFER_SIZE_0], Timestamp[SMALL_BUFFER_SIZE_0], XPosition[SMALL_BUFFER_SIZE_0], - YPosition[SMALL_BUFFER_SIZE_0], ZPosition[SMALL_BUFFER_SIZE_0]; - C8 Speed[SMALL_BUFFER_SIZE_0], LongitudinalSpeed[SMALL_BUFFER_SIZE_0], LateralSpeed[SMALL_BUFFER_SIZE_0], - LongitudinalAcc[SMALL_BUFFER_SIZE_0], LateralAcc[SMALL_BUFFER_SIZE_0]; - C8 Heading[SMALL_BUFFER_SIZE_0], DriveDirection[SMALL_BUFFER_SIZE_1], ObjectState[SMALL_BUFFER_SIZE_1], - ReadyToArm[SMALL_BUFFER_SIZE_1], MTSP[SMALL_BUFFER_SIZE_0], ErrorStatus[SMALL_BUFFER_SIZE_0]; - I32 MessageLength; - C8 *MiscPtr; - C8 MiscText[SMALL_BUFFER_SIZE_0]; - U32 StartTimeU32 = 0; - U32 OutgoingStartTimeU32 = 0; - U32 DelayedStartU32 = 0; - U32 CurrentTimeU32 = 0; - U32 OldTimeU32 = 0; - U64 TimeCap1, TimeCap2; - struct timeval CurrentTimeStruct; - I32 HeartbeatMessageCounter = 0; - - ObjectPosition OP[MAX_OBJECTS]; - flt SpaceArr[MAX_OBJECTS][TRAJECTORY_FILE_MAX_ROWS]; - flt TimeArr[MAX_OBJECTS][TRAJECTORY_FILE_MAX_ROWS]; - SpaceTime SpaceTimeArr[MAX_OBJECTS][TRAJECTORY_FILE_MAX_ROWS]; - C8 OriginLatitude[SMALL_BUFFER_SIZE_0], OriginLongitude[SMALL_BUFFER_SIZE_0], - OriginAltitude[SMALL_BUFFER_SIZE_0], OriginHeading[SMALL_BUFFER_SIZE_0]; - C8 TextBuffer[SMALL_BUFFER_SIZE_0]; - dbl OriginLatitudeDbl; - dbl OriginLongitudeDbl; - dbl OriginAltitudeDbl; - dbl OriginHeadingDbl; - C8 pcSendBuffer[MBUS_MAX_DATALEN]; - C8 ObjectPort[SMALL_BUFFER_SIZE_0]; - HeaderType HeaderData; - OSEMType OSEMData; - STRTType STRTData; - OSTMType OSTMData; - HEABType HEABData; - MONRType MONRData; - DOTMType DOTMData; - TRAJInfoType TRAJInfoData; - VOILType VOILData; - SYPMType SYPMData; - MTSPType MTSPData; - ACCMData mqACCMData; - EXACData mqEXACData; - GeoPosition OriginPosition; - ASPType ASPData; - - ASPData.MTSPU32 = 0; - ASPData.TimeToSyncPointDbl = 0; - ASPData.PrevTimeToSyncPointDbl = 0; - ASPData.CurrentTimeDbl = 0; - AdaptiveSyncPoint ASP[MAX_ADAPTIVE_SYNC_POINTS]; - I32 SyncPointCount = 0; - I32 SearchStartIndex = 0; - dbl ASPMaxTimeDiffDbl = 0; - dbl ASPMaxTrajDiffDbl = 0; - dbl ASPFilterLevelDbl = 0; - dbl ASPMaxDeltaTimeDbl = 0; - I32 ASPDebugRate = 1; - I32 ASPStepBackCount = 0; - char confDirectoryPath[MAX_FILE_PATH]; - - U8 ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_BOOTING; - - vInitializeState(OBC_STATE_IDLE, GSD); - U8 uiTimeCycle = 0; - I32 ObjectcontrolExecutionMode = OBJECT_CONTROL_CONTROL_MODE; - - C8 Buffer2[SMALL_BUFFER_SIZE_1]; - C8 LogBuffer[LOG_BUFFER_LENGTH]; - C8 VOILReceivers[SMALL_BUFFER_SIZE_254]; - C8 DTMReceivers[SMALL_BUFFER_SIZE_254]; - U32 RowCount; - U32 DTMIpU32; - U32 DTMLengthU32; - - U8 DisconnectU8 = 0; - I32 iResult; - - FILE *TempFd; - U16 MiscU16; - I32 j = 0; - - U8 STRTSentU8 = 0; - C8 FileHeaderBufferC8[TRAJ_FILE_HEADER_ROW]; - - - // Create log - LogInit(MODULE_NAME, logLevel); - LogMessage(LOG_LEVEL_INFO, "Object control task running with PID: %i", getpid()); - - - // Set up signal handlers - if (signal(SIGINT, signalHandler) == SIG_ERR) - util_error("Unable to initialize signal handler"); - - // Set up message bus connection - if (iCommInit()) - util_error("Unable to connect to message queue bus"); - - // Initialize timer for sending state - TimeSetToCurrentSystemTime(¤tTime); - nextStateReportTime = currentTime; - - // Initialize timer for sending state - TimeSetToCurrentSystemTime(¤tTime); - nextStateReportTime = currentTime; - - while (!iExit) { - - if (vGetState(GSD) == OBC_STATE_ERROR) { - ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_ABORT; - MessageLength = - ObjectControlBuildHEABMessage(MessageBuffer, &HEABData, GPSTime, ObjectControlServerStatus, - 0); - UtilSendUDPData("Object Control", &safety_socket_fd[iIndex], &safety_object_addr[iIndex], - MessageBuffer, MessageLength, 0); - } - - if (vGetState(GSD) == OBC_STATE_RUNNING || vGetState(GSD) == OBC_STATE_ARMED - || vGetState(GSD) == OBC_STATE_CONNECTED) { - /*HEAB*/ for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - if (uiTimeCycle == 0) { - //HeartbeatMessageCounter ++; - MessageLength = - ObjectControlBuildHEABMessage(MessageBuffer, &HEABData, GPSTime, - ObjectControlServerStatus, 0); - //ObjectControlSendUDPData(&safety_socket_fd[iIndex], &safety_object_addr[iIndex], MessageBuffer, MessageLength, 1); - UtilSendUDPData("Object Control", &safety_socket_fd[iIndex], &safety_object_addr[iIndex], - MessageBuffer, MessageLength, 0); - } - } - - - // Check if any object has disconnected - if so, disconnect all objects and return to idle - DisconnectU8 = 0; - for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - DisconnectU8 |= vCheckRemoteDisconnected(&socket_fds[iIndex]); - if (DisconnectU8) { - LogMessage(LOG_LEVEL_WARNING, "Lost connection to IP %s - returning to IDLE", - object_address_name[iIndex]); - - for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - vDisconnectObject(&socket_fds[iIndex]); - } - - /* Close safety socket */ - for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - vCloseSafetyChannel(&safety_socket_fd[iIndex]); - } - vSetState(OBC_STATE_IDLE, GSD); - break; - } - } - } - - if (vGetState(GSD) == OBC_STATE_RUNNING || vGetState(GSD) == OBC_STATE_CONNECTED - || vGetState(GSD) == OBC_STATE_ARMED) { - char buffer[RECV_MESSAGE_BUFFER]; - size_t receivedMONRData = 0; - - // this is etsi time lets remov it ans use utc instead - //gettimeofday(&CurrentTimeStruct, NULL); - - CurrentTimeU32 = - ((GPSTime->GPSSecondsOfWeekU32 * 1000 + (U32) TimeControlGetMillisecond(GPSTime)) << 2) + - GPSTime->MicroSecondU16; - - - /*MTSP*/ if (HeartbeatMessageCounter == 0) { - HeartbeatMessageCounter = 0; - for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - for (i = 0; i < SyncPointCount; i++) { - if (TEST_SYNC_POINTS == 0 - && strstr(object_address_name[iIndex], ASP[i].SlaveIP) != NULL - && ASPData.MTSPU32 > 0 && ASPData.TimeToSyncPointDbl > -1) { - /*Send Master time to adaptive sync point */ - MessageLength = - ObjectControlBuildMTSPMessage(MessageBuffer, &MTSPData, ASPData.MTSPU32, 0); - //ObjectControlSendUDPData(&safety_socket_fd[iIndex], &safety_object_addr[iIndex], MessageBuffer, MessageLength, 0); - UtilSendUDPData("Object Control", &safety_socket_fd[iIndex], - &safety_object_addr[iIndex], MessageBuffer, MessageLength, 0); - } - /*else if(TEST_SYNC_POINTS == 1 && iIndex == 1 && ASPData.MTSPU32 > 0 && ASPData.TimeToSyncPointDbl > -1 ) - { - Send Master time to adaptive sync point - MessageLength =ObjectControlBuildMTSPMessage(MessageBuffer, &MTSPData, (U32)ASPData.MTSPU32, 0); - ObjectControlSendUDPData(&safety_socket_fd[iIndex], &safety_object_addr[iIndex], MessageBuffer, MessageLength, 0); - } */ - } - } - } - - - for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - bzero(buffer, RECV_MESSAGE_BUFFER); - receivedMONRData = uiRecvMonitor(&safety_socket_fd[iIndex], buffer, RECV_MESSAGE_BUFFER); - - if (receivedMONRData == sizeof (MONRType)) { - LogMessage(LOG_LEVEL_DEBUG, "Recieved new data from %s %d %d: %s", - object_address_name[iIndex], object_udp_port[iIndex], receivedMONRData, - buffer); - - if (ObjectcontrolExecutionMode == OBJECT_CONTROL_CONTROL_MODE) { - // Append IP to buffer - memcpy(&buffer[receivedMONRData], &safety_object_addr[iIndex].sin_addr.s_addr, - sizeof (in_addr_t)); - // Send MONR message as bytes - - if (iCommSend(COMM_MONR, buffer, (size_t) (receivedMONRData) + sizeof (in_addr_t)) < - 0) { - LogMessage(LOG_LEVEL_ERROR, - "Fatal communication fault when sending MONR command - entering error state"); - vSetState(OBC_STATE_ERROR, GSD); - ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_ABORT; - } - } - - ObjectControlBuildMONRMessage(buffer, &MONRData, 0); - - //Store MONR in GSD - DataDictionarySetMONR(GSD, &MONRData, iIndex); - - - //UtilSendUDPData("ObjectControl", &ObjectControlUDPSocketfdI32, &simulator_addr, &MONRData, sizeof(MONRData), 0); - - /* - for (i = 0; - i < - (MONRData.Header.MessageLengthU32 + COMMAND_MESSAGE_HEADER_LENGTH + - COMMAND_MESSAGE_FOOTER_LENGTH); i++) - GSD->MONRData[i] = buffer[i]; - GSD->MONRSizeU8 = - MONRData.Header.MessageLengthU32 + COMMAND_MESSAGE_HEADER_LENGTH + - COMMAND_MESSAGE_FOOTER_LENGTH; */ - - ObjectControlMONRToASCII(&MONRData, &OriginPosition, iIndex, Id, Timestamp, XPosition, - YPosition, ZPosition, LongitudinalSpeed, LateralSpeed, - LongitudinalAcc, LateralAcc, Heading, DriveDirection, - ObjectState, ReadyToArm, ErrorStatus, 1); - bzero(buffer, OBJECT_MESS_BUFFER_SIZE); - strcat(buffer, object_address_name[iIndex]); - strcat(buffer, ";"); - strcat(buffer, "0"); - strcat(buffer, ";"); - strcat(buffer, Timestamp); - strcat(buffer, ";"); - strcat(buffer, XPosition); - strcat(buffer, ";"); - strcat(buffer, YPosition); - strcat(buffer, ";"); - strcat(buffer, ZPosition); - strcat(buffer, ";"); - strcat(buffer, Heading); - strcat(buffer, ";"); - strcat(buffer, LongitudinalSpeed); - strcat(buffer, ";"); - strcat(buffer, LateralSpeed); - strcat(buffer, ";"); - strcat(buffer, LongitudinalAcc); - strcat(buffer, ";"); - strcat(buffer, LateralAcc); - strcat(buffer, ";"); - strcat(buffer, DriveDirection); - strcat(buffer, ";"); - strcat(buffer, ObjectState); - strcat(buffer, ";"); - strcat(buffer, ReadyToArm); - strcat(buffer, ";"); - strcat(buffer, ErrorStatus); - strcat(buffer, ";"); - - - if (ASPData.MTSPU32 != 0) { - //Add MTSP to MONR if not 0 - bzero(MTSP, SMALL_BUFFER_SIZE_0); - sprintf(MTSP, "%" PRIu32, ASPData.MTSPU32); - strcat(buffer, MTSP); - strcat(buffer, ";"); - } - - //Ok, let's do the ASP - for (i = 0; i < SyncPointCount; i++) { - if (TEST_SYNC_POINTS == 0 - && strstr(object_address_name[iIndex], ASP[i].MasterIP) != NULL - && CurrentTimeU32 > StartTimeU32 && StartTimeU32 > 0 - && ASPData.TimeToSyncPointDbl > -1 - /*|| TEST_SYNC_POINTS == 1 && ASP[0].TestPort == object_udp_port[iIndex] && StartTimeU32 > 0 && iIndex == 0 && TimeToSyncPoint > -1 */ - ) { - // Use the util.c function for time here but it soent mather - gettimeofday(&CurrentTimeStruct, NULL); //Capture time - - TimeCap1 = (uint64_t) CurrentTimeStruct.tv_sec * 1000 + (uint64_t) CurrentTimeStruct.tv_usec / 1000; //Calculate initial timestamp - - OP[iIndex].x = ((dbl) atoi(XPosition)) / 1000; //Set x and y on OP (ObjectPosition) - OP[iIndex].y = ((dbl) atoi(YPosition)) / 1000; - - //OP[iIndex].OrigoDistance = sqrt(pow(OP[iIndex].x,2) + pow(OP[iIndex].y,2)); //Calculate hypotenuse - - UtilCalcPositionDelta(OriginLatitudeDbl, OriginLongitudeDbl, - atof(XPosition) / 1e7, atof(YPosition) / 1e7, &OP[iIndex]); - - if (OP[iIndex].BestFoundTrajectoryIndex <= OP[iIndex].SyncIndex) { - ASPData.CurrentTimeU32 = CurrentTimeU32; - ASPData.CurrentTimeDbl = - (((double)CurrentTimeU32 - (double)StartTimeU32) / 1000); - SearchStartIndex = OP[iIndex].BestFoundTrajectoryIndex - ASPStepBackCount; - UtilFindCurrentTrajectoryPosition(&OP[iIndex], SearchStartIndex, - ASPData.CurrentTimeDbl, ASPMaxTrajDiffDbl, - ASPMaxTimeDiffDbl, 0); - ASPData.BestFoundIndexI32 = OP[iIndex].BestFoundTrajectoryIndex; - - if (OP[iIndex].BestFoundTrajectoryIndex != TRAJ_POSITION_NOT_FOUND) { - ASPData.TimeToSyncPointDbl = UtilCalculateTimeToSync(&OP[iIndex]); - if (ASPData.TimeToSyncPointDbl > 0) { - if (ASPData.PrevTimeToSyncPointDbl != 0 && ASPFilterLevelDbl > 0) { - if (ASPData.TimeToSyncPointDbl / ASPData.PrevTimeToSyncPointDbl > - (1 + ASPFilterLevelDbl / 100)) - ASPData.TimeToSyncPointDbl = ASPData.PrevTimeToSyncPointDbl + ASPMaxDeltaTimeDbl; //TimeToSyncPoint*ASPFilterLevelDbl/500; - else if (ASPData.TimeToSyncPointDbl / - ASPData.PrevTimeToSyncPointDbl < - (1 - ASPFilterLevelDbl / 100)) - ASPData.TimeToSyncPointDbl = ASPData.PrevTimeToSyncPointDbl - ASPMaxDeltaTimeDbl; //TimeToSyncPoint*ASPFilterLevelDbl/500; - } - ASPData.MTSPU32 = - CurrentTimeU32 + (U32) (ASPData.TimeToSyncPointDbl * 4000); - - ASPData.PrevTimeToSyncPointDbl = ASPData.TimeToSyncPointDbl; - OldTimeU32 = CurrentTimeU32; - } - else { - CurrentTimeU32 = 0; - ASPData.TimeToSyncPointDbl = -1; - } - - } - - gettimeofday(&CurrentTimeStruct, NULL); - TimeCap2 = - (uint64_t) CurrentTimeStruct.tv_sec * 1000 + - (uint64_t) CurrentTimeStruct.tv_usec / 1000; - - ASPData.SyncPointIndexI32 = OP[iIndex].SyncIndex; - ASPData.IterationTimeU16 = (U16) (TimeCap2 - TimeCap1); - //Build ASP debug data and set to GSD - //bzero(buffer,OBJECT_MESS_BUFFER_SIZE); - //ObjectControlBuildASPMessage(buffer, &ASPData, 0); - DataDictionarySetRVSSAsp(GSD, &ASPData); - - if (atoi(Timestamp) % ASPDebugRate == 0) { - printf("%d, %d, %3.3f, %s, %s\n", CurrentTimeU32, StartTimeU32, - ASPData.TimeToSyncPointDbl, object_address_name[iIndex], - ASP[i].MasterIP); - printf - ("TtS=%3.3f, BestIndex=%d, MTSP=%d, iIndex=%d, IterationTime=%3.0f ms\n", - ASPData.TimeToSyncPointDbl, OP[iIndex].BestFoundTrajectoryIndex, - ASPData.MTSPU32, iIndex, ((double)(TimeCap2) - (double)TimeCap1)); - printf("CurrentTime=%3.3f, x=%3.3f mm, y=%3.3f\n\n", - ASPData.CurrentTimeDbl, OP[iIndex].x, OP[iIndex].y); - - //Build and send ASP on message queue - //(void)iCommSend(COMM_ASP,buffer); - } - } - } - } - OP[iIndex].Speed = atof(Speed); - } - else if (receivedMONRData > 0) - LogMessage(LOG_LEVEL_INFO, "MONR length error (should be %d but is %ld) from %s.", - sizeof (MONRType), object_address_name[iIndex]); - } - } - - - bzero(pcRecvBuffer, RECV_MESSAGE_BUFFER); - //Have we recieved a command? - if (iCommRecv(&iCommand, pcRecvBuffer, RECV_MESSAGE_BUFFER, NULL)) { - LogMessage(LOG_LEVEL_INFO, "Received command %d", iCommand); - - - if (iCommand == COMM_ARM && vGetState(GSD) == OBC_STATE_CONNECTED) { - - LogMessage(LOG_LEVEL_INFO, "Sending ARM"); - LOG_SEND(LogBuffer, "[ObjectControl] Sending ARM"); - vSetState(OBC_STATE_ARMED, GSD); - MessageLength = - ObjectControlBuildOSTMMessage(MessageBuffer, &OSTMData, COMMAND_OSTM_OPT_SET_ARMED_STATE, - 0); - for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - /*Send OSTM message */ - UtilSendTCPData("[Object Control]", MessageBuffer, MessageLength, &socket_fds[iIndex], 0); - } - - ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_OK; //Set server to READY - } - else if (iCommand == COMM_DISARM && vGetState(GSD) == OBC_STATE_ARMED) { - - LogMessage(LOG_LEVEL_INFO, "Sending DISARM"); - LOG_SEND(LogBuffer, "[ObjectControl] Sending DISARM"); - vSetState(OBC_STATE_CONNECTED, GSD); - - MessageLength = - ObjectControlBuildOSTMMessage(MessageBuffer, &OSTMData, - COMMAND_OSTM_OPT_SET_DISARMED_STATE, 0); - for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - /*Send OSTM message */ - UtilSendTCPData("[" MODULE_NAME "]", MessageBuffer, MessageLength, &socket_fds[iIndex], - 0); - } - - ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_OK; //Set server to READY - } - else if (iCommand == COMM_STRT && (vGetState(GSD) == OBC_STATE_ARMED) /*|| OBC_STATE_INITIALIZED) */ ) //OBC_STATE_INITIALIZED is temporary! - { - bzero(Timestamp, SMALL_BUFFER_SIZE_0); - MiscPtr = strchr(pcRecvBuffer, ';'); - strncpy(Timestamp, MiscPtr + 1, (uint64_t) strchr(MiscPtr + 1, ';') - (uint64_t) MiscPtr - 1); - StartTimeU32 = atol(Timestamp); - bzero(Timestamp, SMALL_BUFFER_SIZE_0); - MiscPtr += 1; - MiscPtr = strchr(pcRecvBuffer, ';'); - strncpy(Timestamp, MiscPtr + 1, (uint64_t) strchr(MiscPtr + 1, ';') - (uint64_t) MiscPtr - 1); - DelayedStartU32 = atoi(Timestamp); - ASPData.MTSPU32 = 0; - ASPData.TimeToSyncPointDbl = 0; - SearchStartIndex = -1; - ASPData.PrevTimeToSyncPointDbl = 0; - OldTimeU32 = CurrentTimeU32; - ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_OK; //Set server to READY - MessageLength = - ObjectControlBuildSTRTMessage(MessageBuffer, &STRTData, GPSTime, (U32) StartTimeU32, - DelayedStartU32, &OutgoingStartTimeU32, 0); - for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - UtilSendTCPData("Object Control", MessageBuffer, MessageLength, &socket_fds[iIndex], 0); - } - vSetState(OBC_STATE_RUNNING, GSD); - - //Store STRT in GSD - if (STRTSentU8 == 0) { - //for(i = 0; i < MessageLength; i++) GSD->STRTData[i] = MessageBuffer[i]; - //GSD->STRTSizeU8 = (U8)MessageLength; - STRTSentU8 = 1; - } - //OBCState = OBC_STATE_INITIALIZED; //This is temporary - //printf("OutgoingStartTimeU32 = %d\n", OutgoingStartTimeU32); - - - GSD->ScenarioStartTimeU32 = OutgoingStartTimeU32; - bzero(MiscText, SMALL_BUFFER_SIZE_0); - sprintf(MiscText, "%" PRIu32, GSD->ScenarioStartTimeU32 << 2); - LOG_SEND(LogBuffer, "[ObjectControl] START received <%s>, GPS time <%s>", pcRecvBuffer, - MiscText); - } - else if (iCommand == COMM_REPLAY) { - ObjectcontrolExecutionMode = OBJECT_CONTROL_REPLAY_MODE; - LogMessage(LOG_LEVEL_INFO, "Entering REPLAY mode <%s>", pcRecvBuffer); - } - else if (iCommand == COMM_ABORT && vGetState(GSD) == OBC_STATE_RUNNING) { - vSetState(OBC_STATE_CONNECTED, GSD); - ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_ABORT; //Set server to ABORT - LogMessage(LOG_LEVEL_WARNING, "ABORT received"); - LOG_SEND(LogBuffer, "[ObjectControl] ABORT received."); - } - else if (iCommand == COMM_CONTROL) { - ObjectcontrolExecutionMode = OBJECT_CONTROL_CONTROL_MODE; - printf("[ObjectControl] Object control in CONTROL mode\n"); - } - else if (iCommand == COMM_INIT) { - LogMessage(LOG_LEVEL_INFO, "INIT received"); - - LOG_SEND(LogBuffer, "[ObjectControl] INIT received."); - nbr_objects = 0; - if (iFindObjectsInfo(object_traj_file, object_address_name, objectIPs, &nbr_objects) == 0) { - // Get objects; name and drive file - DataDictionaryGetForceToLocalhostU8(GSD, &iForceObjectToLocalhostU8); - - for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - if (0 == iForceObjectToLocalhostU8) { - object_udp_port[iIndex] = SAFETY_CHANNEL_PORT; - object_tcp_port[iIndex] = CONTROL_CHANNEL_PORT; - } - else { - object_udp_port[iIndex] = SAFETY_CHANNEL_PORT + iIndex * 2; - object_tcp_port[iIndex] = CONTROL_CHANNEL_PORT + iIndex * 2; - } - } - - /*Setup Adaptive Sync Points (ASP) */ - UtilGetConfDirectoryPath(confDirectoryPath, sizeof (confDirectoryPath)); - strcat(confDirectoryPath, ADAPTIVE_SYNC_FILE_NAME); - fd = fopen(confDirectoryPath, "r"); - if (fd) { - SyncPointCount = UtilCountFileRows(fd) - 1; - fclose(fd); - fd = fopen(confDirectoryPath, "r"); - UtilReadLineCntSpecChars(fd, pcTempBuffer); //Read header - - for (i = 0; i < SyncPointCount; i++) { - UtilSetAdaptiveSyncPoint(&ASP[i], fd, 0); - if (TEST_SYNC_POINTS == 1) - ASP[i].TestPort = SAFETY_CHANNEL_PORT; - } - fclose(fd); - } - - vSetState(OBC_STATE_INITIALIZED, GSD); - LogMessage(LOG_LEVEL_INFO, "ObjectControl is initialized"); - LOG_SEND(LogBuffer, "[ObjectControl] ObjectControl is initialized."); - - //Remove temporary file - remove(TEMP_LOG_FILE); - if (USE_TEMP_LOGFILE) { - //Create temporary file - TempFd = fopen(TEMP_LOG_FILE, "w+"); - } - - //OSEMSentU8 = 0; - STRTSentU8 = 0; - - DataDictionaryInitMONR(GSD); - - } - else { - LogMessage(LOG_LEVEL_INFO, - "Could not initialize: object info was not processed successfully"); - pcSendBuffer[0] = (uint8_t) iCommand; - iCommSend(COMM_FAILURE, pcSendBuffer, sizeof (iCommand)); - } - - - } - else if (iCommand == COMM_ACCM && vGetState(GSD) == OBC_STATE_CONNECTED) { - UtilPopulateACCMDataStructFromMQ(pcRecvBuffer, sizeof (pcRecvBuffer), &mqACCMData); - iIndex = - iGetObjectIndexFromObjectIP(mqACCMData.ip, objectIPs, - sizeof (objectIPs) / sizeof (objectIPs[0])); - if (iIndex != -1) { - ObjectControlSendACCMMessage(&mqACCMData, &(socket_fds[iIndex]), 0); - } - else - LogMessage(LOG_LEVEL_WARNING, "Unable to send ACCM: no valid socket found"); - } - else if (iCommand == COMM_EXAC && vGetState(GSD) == OBC_STATE_RUNNING) { - UtilPopulateEXACDataStructFromMQ(pcRecvBuffer, sizeof (pcRecvBuffer), &mqEXACData); - iIndex = - iGetObjectIndexFromObjectIP(mqEXACData.ip, objectIPs, - sizeof (objectIPs) / sizeof (objectIPs[0])); - if (iIndex != -1) - ObjectControlSendEXACMessage(&mqEXACData, &(socket_fds[iIndex]), 0); - else - LogMessage(LOG_LEVEL_WARNING, "Unable to send EXAC: no valid socket found"); - } - else if (iCommand == COMM_CONNECT && vGetState(GSD) == OBC_STATE_INITIALIZED) { - LogMessage(LOG_LEVEL_INFO, "CONNECT received"); - LOG_SEND(LogBuffer, "[ObjectControl] CONNECT received."); - - /* Connect and send drive files */ - for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - - UtilSetObjectPositionIP(&OP[iIndex], object_address_name[iIndex]); - - MessageLength = ObjectControlBuildOSEMMessage(MessageBuffer, &OSEMData, GPSTime, - OriginLatitude, OriginLongitude, - OriginAltitude, 0); - - DisconnectU8 = 0; - - do { - - iResult = - vConnectObject(&socket_fds[iIndex], object_address_name[iIndex], - object_tcp_port[iIndex], &DisconnectU8); - - if (iResult < 0) { - switch (errno) { - case ECONNREFUSED: - LogMessage(LOG_LEVEL_INFO, - "Unable to connect to object %s:%d, retry in %d sec...", - object_address_name[iIndex], object_tcp_port[iIndex], - (!(1 & DisconnectU8)) * 3); - LOG_SEND(LogBuffer, - "[ObjectControl] Was not able to connect to object, [IP: %s] [PORT: %d], retry in %d sec...", - object_address_name[iIndex], object_tcp_port[iIndex], - (!(1 & DisconnectU8)) * 3); - (void)sleep(3); // TODO: Move this to the rest of the sleep operations? Also, remove the hardcoded 3 - break; - case EADDRINUSE: - LogMessage(LOG_LEVEL_ERROR, "[ObjectControl] Local address/port already in use"); - break; - case EALREADY: - LogMessage(LOG_LEVEL_ERROR, "[ObjectControl] Previous connection attempt still in progress"); - break; - case EISCONN: - LogMessage(LOG_LEVEL_ERROR, "[ObjectControl] Socket is already connected"); - break; - case ENETUNREACH: - LogMessage(LOG_LEVEL_ERROR, "[ObjectControl] Network unreachable"); - break; - case ETIMEDOUT: - LogMessage(LOG_LEVEL_ERROR, "[ObjectControl] Connection timed out"); - break; - default: - LogMessage(LOG_LEVEL_ERROR, "ERR: Failed to connect to control socket"); - break; - } - - } - - bzero(pcRecvBuffer, RECV_MESSAGE_BUFFER); - //Have we received a command? - if (iCommRecv(&iCommand, pcRecvBuffer, RECV_MESSAGE_BUFFER, NULL)) { - if (iCommand == COMM_DISCONNECT) { - DisconnectU8 = 1; - LOG_SEND(LogBuffer, "[ObjectControl] DISCONNECT received."); - } - - } - - } while (iExit == 0 && iResult < 0 && DisconnectU8 == 0); - - if (iResult >= 0) { - /* Send OSEM command in mq so that we get some information like GPSweek, origin (latitude,logitude,altitude in gps coordinates) */ - LogMessage(LOG_LEVEL_INFO, "Sending OSEM"); - LOG_SEND(LogBuffer, "[ObjectControl] Sending OSEM."); - - ObjectControlOSEMtoASCII(&OSEMData, GPSWeek, OriginLatitude, OriginLongitude, - OriginAltitude); - bzero(pcSendBuffer, sizeof (pcSendBuffer)); - strcat(pcSendBuffer, GPSWeek); - strcat(pcSendBuffer, ";"); - strcat(pcSendBuffer, OriginLatitude); - strcat(pcSendBuffer, ";"); - strcat(pcSendBuffer, OriginLongitude); - strcat(pcSendBuffer, ";"); - strcat(pcSendBuffer, OriginAltitude); - - //Restore the buffers - DataDictionaryGetOriginLatitudeC8(GSD, OriginLatitude, SMALL_BUFFER_SIZE_0); - DataDictionaryGetOriginLongitudeC8(GSD, OriginLongitude, SMALL_BUFFER_SIZE_0); - DataDictionaryGetOriginAltitudeC8(GSD, OriginAltitude, SMALL_BUFFER_SIZE_0); - - if (iCommSend(COMM_OSEM, pcSendBuffer, strlen(pcSendBuffer) + 1) < 0) { - LogMessage(LOG_LEVEL_ERROR, - "Fatal communication fault when sending OSEM command - entering error state"); - vSetState(OBC_STATE_ERROR, GSD); - ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_ABORT; - } - UtilSendTCPData("Object Control", MessageBuffer, MessageLength, &socket_fds[iIndex], - 0); - - /*Here we send TRAJ, if the IP-address not is found */ - if (strstr(DTMReceivers, object_address_name[iIndex]) == NULL) { - - fd = fopen(object_traj_file[iIndex], "r"); - - if (fd != NULL) { - //RowCount = UtilCountFileRows(fd); - //printf("RowCount: %d\n", RowCount); - //fclose(fd); - - //fd = fopen(object_traj_file[iIndex], "r"); - //printf("Open file: %s\n", object_traj_file[iIndex]); - UtilReadLineCntSpecChars(fd, FileHeaderBufferC8); - fclose(fd); - - /*TRAJ*/ - MessageLength = ObjectControlBuildTRAJMessageHeader(TrajBuffer, - &RowCount, - &HeaderData, - &TRAJInfoData, - FileHeaderBufferC8, - 0); - - //printf("RowCount: %d\n", RowCount); - - /*Send TRAJ header */ - UtilSendTCPData("Object Control", TrajBuffer, MessageLength, - &socket_fds[iIndex], 0); - - /*Send TRAJ data */ - ObjectControlSendTRAJMessage(object_traj_file[iIndex], &socket_fds[iIndex], - RowCount, - (char *)&object_address_name[iIndex], - object_tcp_port[iIndex], &DOTMData, 0); - - } - else - LogMessage(LOG_LEVEL_WARNING, "Could not open file <%s>", - object_traj_file[iIndex]); - } - - - /* Adaptive Sync Points object configuration start... */ - if (TEST_SYNC_POINTS == 1) - printf("Trajfile: %s\n", object_traj_file[iIndex]); - OP[iIndex].TrajectoryPositionCount = RowCount; - OP[iIndex].SpaceArr = SpaceArr[iIndex]; - OP[iIndex].TimeArr = TimeArr[iIndex]; - OP[iIndex].SpaceTimeArr = SpaceTimeArr[iIndex]; - UtilPopulateSpaceTimeArr(&OP[iIndex], object_traj_file[iIndex]); - - LogMessage(LOG_LEVEL_INFO, "Sync point counts: %d", SyncPointCount); - for (i = 0; i < SyncPointCount; i++) { - if (TEST_SYNC_POINTS == 1 && iIndex == 1) { - /*Send SYPM to slave */ - MessageLength = - ObjectControlBuildSYPMMessage(MessageBuffer, &SYPMData, - ASP[i].SlaveTrajSyncTime * 1000, - ASP[i].SlaveSyncStopTime * 1000, 1); - UtilSendTCPData("Object Control", MessageBuffer, MessageLength, - &socket_fds[iIndex], 0); - } - else if (TEST_SYNC_POINTS == 0 - && strstr(object_address_name[iIndex], ASP[i].SlaveIP) != NULL) { - /*Send SYPM to slave */ - MessageLength = - ObjectControlBuildSYPMMessage(MessageBuffer, &SYPMData, - ASP[i].SlaveTrajSyncTime * 1000, - ASP[i].SlaveSyncStopTime * 1000, 1); - UtilSendTCPData("Object Control", MessageBuffer, MessageLength, - &socket_fds[iIndex], 0); - } - } - - /*Set Sync point in OP */ - for (i = 0; i < SyncPointCount; i++) { - if (TEST_SYNC_POINTS == 1 && iIndex == 0) - UtilSetSyncPoint(&OP[iIndex], 0, 0, 0, ASP[i].MasterTrajSyncTime); - else if (TEST_SYNC_POINTS == 0 - && strstr(object_address_name[iIndex], ASP[i].MasterIP) != NULL) - UtilSetSyncPoint(&OP[iIndex], 0, 0, 0, ASP[i].MasterTrajSyncTime); - } - /* ...end */ - } - - } - for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - if (USE_TEST_HOST == 0) - vCreateSafetyChannel(object_address_name[iIndex], object_udp_port[iIndex], - &safety_socket_fd[iIndex], &safety_object_addr[iIndex]); - else if (USE_TEST_HOST == 1) - vCreateSafetyChannel(TESTSERVER_IP, object_udp_port[iIndex], - &safety_socket_fd[iIndex], &safety_object_addr[iIndex]); - } - - uiTimeCycle = 0; - - /* Execution mode */ - ObjectcontrolExecutionMode = OBJECT_CONTROL_CONTROL_MODE; - - /*Set server status */ - ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_OK; //Set server to READY - - if (DisconnectU8 == 0) { - vSetState(OBC_STATE_CONNECTED, GSD); - iCommSend(COMM_OBJECTS_CONNECTED, NULL, 0); - } - else if (DisconnectU8 == 1) - vSetState(OBC_STATE_IDLE, GSD); - } - else if (iCommand == COMM_DATA_DICT) { - - LogMessage(LOG_LEVEL_INFO, "Updating variables from DataDictionary."); - DataDictionaryGetOriginLatitudeC8(GSD, OriginLatitude, SMALL_BUFFER_SIZE_0); - DataDictionaryGetOriginLongitudeC8(GSD, OriginLongitude, SMALL_BUFFER_SIZE_0); - DataDictionaryGetOriginAltitudeC8(GSD, OriginAltitude, SMALL_BUFFER_SIZE_0); - - DataDictionaryGetOriginLatitudeDbl(GSD, &OriginLatitudeDbl); - DataDictionaryGetOriginLongitudeDbl(GSD, &OriginLongitudeDbl); - DataDictionaryGetOriginAltitudeDbl(GSD, &OriginAltitudeDbl); - - OriginLatitudeDbl = atof(OriginLatitude); - OriginLongitudeDbl = atof(OriginLongitude); - OriginAltitudeDbl = atof(OriginAltitude); - OriginHeadingDbl = atof(OriginHeading); - OriginPosition.Latitude = OriginLatitudeDbl; - OriginPosition.Longitude = OriginLongitudeDbl; - OriginPosition.Altitude = OriginAltitudeDbl; - OriginPosition.Heading = OriginHeadingDbl; - - DataDictionaryGetForceToLocalhostU8(GSD, &iForceObjectToLocalhostU8); - LogMessage(LOG_LEVEL_INFO, "ForceObjectToLocalhost = %d", iForceObjectToLocalhostU8); - LOG_SEND(LogBuffer, "[ObjectControl] ForceObjectToLocalhost = %d", iForceObjectToLocalhostU8); - - DataDictionaryGetASPMaxTimeDiffDbl(GSD, &ASPMaxTimeDiffDbl); - DataDictionaryGetASPMaxTrajDiffDbl(GSD, &ASPMaxTrajDiffDbl); - DataDictionaryGetASPStepBackCountU32(GSD, &ASPStepBackCount); - DataDictionaryGetASPFilterLevelDbl(GSD, &ASPFilterLevelDbl); - DataDictionaryGetASPMaxDeltaTimeDbl(GSD, &ASPMaxDeltaTimeDbl); - ASPDebugRate = 1; - DataDictionaryGetVOILReceiversC8(GSD, VOILReceivers, SMALL_BUFFER_SIZE_254); - DataDictionaryGetDTMReceiversC8(GSD, DTMReceivers, SMALL_BUFFER_SIZE_254); - } - else if (iCommand == COMM_DISCONNECT) { - //#ifndef NOTCP - for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - vDisconnectObject(&socket_fds[iIndex]); - } - DataDictionaryFreeMONR(GSD); - //#endif //NOTCP - - LogMessage(LOG_LEVEL_INFO, "DISCONNECT received"); - LOG_SEND(LogBuffer, "[ObjectControl] DISCONNECT received."); - /* Close safety socket */ - for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - vCloseSafetyChannel(&safety_socket_fd[iIndex]); - } - vSetState(OBC_STATE_IDLE, GSD); - } - else if (iCommand == COMM_EXIT) { - iExit = 1; - iCommClose(); - } - else { - LogMessage(LOG_LEVEL_WARNING, "Unhandled command in object control: %d", iCommand); - } - } - - if (!iExit) { - /* Make call periodic */ - sleep_time = iCommand == COMM_INV ? mqEmptyPollPeriod : mqNonEmptyPollPeriod; - - ++uiTimeCycle; - if (uiTimeCycle >= HEARTBEAT_TIME_MS / TASK_PERIOD_MS) { - uiTimeCycle = 0; - } - - // Periodically send state to signal aliveness - TimeSetToCurrentSystemTime(¤tTime); - if (timercmp(¤tTime, &nextStateReportTime, >)) { - timeradd(&nextStateReportTime, &stateReportPeriod, &nextStateReportTime); - - bzero(Buffer2, sizeof (Buffer2)); - Buffer2[0] = (uint8_t) (DataDictionaryGetOBCStateU8(GSD)); - if (iCommSend(COMM_OBC_STATE, Buffer2, sizeof (Buffer2)) < 0) { - LogMessage(LOG_LEVEL_ERROR, - "Fatal communication fault when sending OBC_STATE command - entering error state"); - vSetState(OBC_STATE_ERROR, GSD); - ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_ABORT; - } - } - - (void)nanosleep(&sleep_time, &ref_time); - } - } - - LogMessage(LOG_LEVEL_INFO, "Object control exiting"); + I32 safety_socket_fd[MAX_OBJECTS]; + struct sockaddr_in safety_object_addr[MAX_OBJECTS]; + I32 socket_fds[MAX_OBJECTS]; + I32 socket_fd = 0; + C8 object_traj_file[MAX_OBJECTS][MAX_FILE_PATH]; + C8 object_address_name[MAX_OBJECTS][MAX_FILE_PATH]; + in_addr_t objectIPs[MAX_OBJECTS]; + U32 object_udp_port[MAX_OBJECTS]; + U32 object_tcp_port[MAX_OBJECTS]; + I32 nbr_objects = 0; + enum COMMAND iCommand; + U8 pcRecvBuffer[RECV_MESSAGE_BUFFER]; + C8 pcTempBuffer[512]; + C8 MessageBuffer[BUFFER_SIZE_3100]; + I32 iIndex = 0, i = 0; + struct timespec sleep_time, ref_time; + + /*! Timers for reporting state over message bus */ + const struct timespec mqEmptyPollPeriod = { OC_SLEEP_TIME_EMPTY_MQ_S, OC_SLEEP_TIME_EMPTY_MQ_NS }; + const struct timespec mqNonEmptyPollPeriod = + { OC_SLEEP_TIME_NONEMPTY_MQ_S, OC_SLEEP_TIME_NONEMPTY_MQ_NS }; + const struct timeval stateReportPeriod = { OC_STATE_REPORT_PERIOD_S, OC_STATE_REPORT_PERIOD_US }; + struct timeval currentTime, nextStateReportTime; + U8 iForceObjectToLocalhostU8 = 0; + + FILE *fd; + C8 Id[SMALL_BUFFER_SIZE_0]; + C8 GPSWeek[SMALL_BUFFER_SIZE_0], Timestamp[SMALL_BUFFER_SIZE_0], XPosition[SMALL_BUFFER_SIZE_0], + YPosition[SMALL_BUFFER_SIZE_0], ZPosition[SMALL_BUFFER_SIZE_0]; + C8 Speed[SMALL_BUFFER_SIZE_0], LongitudinalSpeed[SMALL_BUFFER_SIZE_0], LateralSpeed[SMALL_BUFFER_SIZE_0], + LongitudinalAcc[SMALL_BUFFER_SIZE_0], LateralAcc[SMALL_BUFFER_SIZE_0]; + C8 Heading[SMALL_BUFFER_SIZE_0], DriveDirection[SMALL_BUFFER_SIZE_1], ObjectState[SMALL_BUFFER_SIZE_1], + ReadyToArm[SMALL_BUFFER_SIZE_1], MTSP[SMALL_BUFFER_SIZE_0], ErrorStatus[SMALL_BUFFER_SIZE_0]; + I32 MessageLength; + C8 *MiscPtr; + C8 MiscText[SMALL_BUFFER_SIZE_0]; + U32 StartTimeU32 = 0; + U32 OutgoingStartTimeU32 = 0; + U32 DelayedStartU32 = 0; + U32 CurrentTimeU32 = 0; + U32 OldTimeU32 = 0; + U64 TimeCap1, TimeCap2; + struct timeval CurrentTimeStruct; + I32 HeartbeatMessageCounter = 0; + + ObjectPosition OP[MAX_OBJECTS]; + flt SpaceArr[MAX_OBJECTS][TRAJECTORY_FILE_MAX_ROWS]; + flt TimeArr[MAX_OBJECTS][TRAJECTORY_FILE_MAX_ROWS]; + SpaceTime SpaceTimeArr[MAX_OBJECTS][TRAJECTORY_FILE_MAX_ROWS]; + C8 OriginLatitude[SMALL_BUFFER_SIZE_0], OriginLongitude[SMALL_BUFFER_SIZE_0], + OriginAltitude[SMALL_BUFFER_SIZE_0], OriginHeading[SMALL_BUFFER_SIZE_0]; + C8 TextBuffer[SMALL_BUFFER_SIZE_0]; + dbl OriginLatitudeDbl; + dbl OriginLongitudeDbl; + dbl OriginAltitudeDbl; + dbl OriginHeadingDbl; + C8 pcSendBuffer[MBUS_MAX_DATALEN]; + C8 ObjectPort[SMALL_BUFFER_SIZE_0]; + HeaderType HeaderData; + OSEMType OSEMData; + STRTType STRTData; + OSTMType OSTMData; + HEABType HEABData; + MONRType MONRData; + DOTMType DOTMData; + TRAJInfoType TRAJInfoData; + VOILType VOILData; + SYPMType SYPMData; + MTSPType MTSPData; + ACCMData mqACCMData; + EXACData mqEXACData; + GeoPosition OriginPosition; + ASPType ASPData; + + ASPData.MTSPU32 = 0; + ASPData.TimeToSyncPointDbl = 0; + ASPData.PrevTimeToSyncPointDbl = 0; + ASPData.CurrentTimeDbl = 0; + AdaptiveSyncPoint ASP[MAX_ADAPTIVE_SYNC_POINTS]; + I32 SyncPointCount = 0; + I32 SearchStartIndex = 0; + dbl ASPMaxTimeDiffDbl = 0; + dbl ASPMaxTrajDiffDbl = 0; + dbl ASPFilterLevelDbl = 0; + dbl ASPMaxDeltaTimeDbl = 0; + I32 ASPDebugRate = 1; + I32 ASPStepBackCount = 0; + char confDirectoryPath[MAX_FILE_PATH]; + + U8 ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_BOOTING; + + vInitializeState(OBC_STATE_IDLE, GSD); + U8 uiTimeCycle = 0; + I32 ObjectcontrolExecutionMode = OBJECT_CONTROL_CONTROL_MODE; + + C8 Buffer2[SMALL_BUFFER_SIZE_1]; + C8 LogBuffer[LOG_BUFFER_LENGTH]; + C8 VOILReceivers[SMALL_BUFFER_SIZE_254]; + C8 DTMReceivers[SMALL_BUFFER_SIZE_254]; + U32 RowCount; + U32 DTMIpU32; + U32 DTMLengthU32; + + U8 DisconnectU8 = 0; + I32 iResult; + + FILE *TempFd; + U16 MiscU16; + I32 j = 0; + + U8 STRTSentU8 = 0; + C8 FileHeaderBufferC8[TRAJ_FILE_HEADER_ROW]; + + + // Create log + LogInit(MODULE_NAME, logLevel); + LogMessage(LOG_LEVEL_INFO, "Object control task running with PID: %i", getpid()); + + + // Set up signal handlers + if (signal(SIGINT, signalHandler) == SIG_ERR) + util_error("Unable to initialize signal handler"); + + // Set up message bus connection + if (iCommInit()) + util_error("Unable to connect to message queue bus"); + + // Initialize timer for sending state + TimeSetToCurrentSystemTime(¤tTime); + nextStateReportTime = currentTime; + + // Initialize timer for sending state + TimeSetToCurrentSystemTime(¤tTime); + nextStateReportTime = currentTime; + + while (!iExit) { + + if (vGetState(GSD) == OBC_STATE_ERROR) { + ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_ABORT; + MessageLength = + ObjectControlBuildHEABMessage(MessageBuffer, &HEABData, GPSTime, ObjectControlServerStatus, + 0); + UtilSendUDPData("Object Control", &safety_socket_fd[iIndex], &safety_object_addr[iIndex], + MessageBuffer, MessageLength, 0); + } + + if (vGetState(GSD) == OBC_STATE_RUNNING || vGetState(GSD) == OBC_STATE_ARMED + || vGetState(GSD) == OBC_STATE_CONNECTED) { + /*HEAB*/ for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { + if (uiTimeCycle == 0) { + //HeartbeatMessageCounter ++; + MessageLength = + ObjectControlBuildHEABMessage(MessageBuffer, &HEABData, GPSTime, + ObjectControlServerStatus, 0); + //ObjectControlSendUDPData(&safety_socket_fd[iIndex], &safety_object_addr[iIndex], MessageBuffer, MessageLength, 1); + UtilSendUDPData("Object Control", &safety_socket_fd[iIndex], &safety_object_addr[iIndex], + MessageBuffer, MessageLength, 0); + } + } + + + // Check if any object has disconnected - if so, disconnect all objects and return to idle + DisconnectU8 = 0; + for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { + DisconnectU8 |= vCheckRemoteDisconnected(&socket_fds[iIndex]); + if (DisconnectU8) { + LogMessage(LOG_LEVEL_WARNING, "Lost connection to IP %s - returning to IDLE", + object_address_name[iIndex]); + + for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { + vDisconnectObject(&socket_fds[iIndex]); + } + + /* Close safety socket */ + for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { + vCloseSafetyChannel(&safety_socket_fd[iIndex]); + } + vSetState(OBC_STATE_IDLE, GSD); + break; + } + } + } + + if (vGetState(GSD) == OBC_STATE_RUNNING || vGetState(GSD) == OBC_STATE_CONNECTED + || vGetState(GSD) == OBC_STATE_ARMED) { + char buffer[RECV_MESSAGE_BUFFER]; + size_t receivedMONRData = 0; + + // this is etsi time lets remov it ans use utc instead + //gettimeofday(&CurrentTimeStruct, NULL); + + CurrentTimeU32 = + ((GPSTime->GPSSecondsOfWeekU32 * 1000 + (U32) TimeControlGetMillisecond(GPSTime)) << 2) + + GPSTime->MicroSecondU16; + + + /*MTSP*/ if (HeartbeatMessageCounter == 0) { + HeartbeatMessageCounter = 0; + for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { + for (i = 0; i < SyncPointCount; i++) { + if (TEST_SYNC_POINTS == 0 + && strstr(object_address_name[iIndex], ASP[i].SlaveIP) != NULL + && ASPData.MTSPU32 > 0 && ASPData.TimeToSyncPointDbl > -1) { + /*Send Master time to adaptive sync point */ + MessageLength = + ObjectControlBuildMTSPMessage(MessageBuffer, &MTSPData, ASPData.MTSPU32, 0); + //ObjectControlSendUDPData(&safety_socket_fd[iIndex], &safety_object_addr[iIndex], MessageBuffer, MessageLength, 0); + UtilSendUDPData("Object Control", &safety_socket_fd[iIndex], + &safety_object_addr[iIndex], MessageBuffer, MessageLength, 0); + } + /*else if(TEST_SYNC_POINTS == 1 && iIndex == 1 && ASPData.MTSPU32 > 0 && ASPData.TimeToSyncPointDbl > -1 ) + { + Send Master time to adaptive sync point + MessageLength =ObjectControlBuildMTSPMessage(MessageBuffer, &MTSPData, (U32)ASPData.MTSPU32, 0); + ObjectControlSendUDPData(&safety_socket_fd[iIndex], &safety_object_addr[iIndex], MessageBuffer, MessageLength, 0); + } */ + } + } + } + + + for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { + bzero(buffer, RECV_MESSAGE_BUFFER); + receivedMONRData = uiRecvMonitor(&safety_socket_fd[iIndex], buffer, RECV_MESSAGE_BUFFER); + + if (receivedMONRData == sizeof (MONRType)) { + LogMessage(LOG_LEVEL_DEBUG, "Recieved new data from %s %d %d: %s", + object_address_name[iIndex], object_udp_port[iIndex], receivedMONRData, + buffer); + + if (ObjectcontrolExecutionMode == OBJECT_CONTROL_CONTROL_MODE) { + // Append IP to buffer + memcpy(&buffer[receivedMONRData], &safety_object_addr[iIndex].sin_addr.s_addr, + sizeof (in_addr_t)); + // Send MONR message as bytes + + if (iCommSend(COMM_MONR, buffer, (size_t) (receivedMONRData) + sizeof (in_addr_t)) < + 0) { + LogMessage(LOG_LEVEL_ERROR, + "Fatal communication fault when sending MONR command - entering error state"); + vSetState(OBC_STATE_ERROR, GSD); + ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_ABORT; + } + } + + ObjectControlBuildMONRMessage(buffer, &MONRData, 0); + + //Store MONR in GSD + DataDictionarySetMONR(GSD, &MONRData, iIndex); + + + //UtilSendUDPData("ObjectControl", &ObjectControlUDPSocketfdI32, &simulator_addr, &MONRData, sizeof(MONRData), 0); + + /* + for (i = 0; + i < + (MONRData.Header.MessageLengthU32 + COMMAND_MESSAGE_HEADER_LENGTH + + COMMAND_MESSAGE_FOOTER_LENGTH); i++) + GSD->MONRData[i] = buffer[i]; + GSD->MONRSizeU8 = + MONRData.Header.MessageLengthU32 + COMMAND_MESSAGE_HEADER_LENGTH + + COMMAND_MESSAGE_FOOTER_LENGTH; */ + + ObjectControlMONRToASCII(&MONRData, &OriginPosition, iIndex, Id, Timestamp, XPosition, + YPosition, ZPosition, LongitudinalSpeed, LateralSpeed, + LongitudinalAcc, LateralAcc, Heading, DriveDirection, + ObjectState, ReadyToArm, ErrorStatus, 1); + bzero(buffer, OBJECT_MESS_BUFFER_SIZE); + strcat(buffer, object_address_name[iIndex]); + strcat(buffer, ";"); + strcat(buffer, "0"); + strcat(buffer, ";"); + strcat(buffer, Timestamp); + strcat(buffer, ";"); + strcat(buffer, XPosition); + strcat(buffer, ";"); + strcat(buffer, YPosition); + strcat(buffer, ";"); + strcat(buffer, ZPosition); + strcat(buffer, ";"); + strcat(buffer, Heading); + strcat(buffer, ";"); + strcat(buffer, LongitudinalSpeed); + strcat(buffer, ";"); + strcat(buffer, LateralSpeed); + strcat(buffer, ";"); + strcat(buffer, LongitudinalAcc); + strcat(buffer, ";"); + strcat(buffer, LateralAcc); + strcat(buffer, ";"); + strcat(buffer, DriveDirection); + strcat(buffer, ";"); + strcat(buffer, ObjectState); + strcat(buffer, ";"); + strcat(buffer, ReadyToArm); + strcat(buffer, ";"); + strcat(buffer, ErrorStatus); + strcat(buffer, ";"); + + + if (ASPData.MTSPU32 != 0) { + //Add MTSP to MONR if not 0 + bzero(MTSP, SMALL_BUFFER_SIZE_0); + sprintf(MTSP, "%" PRIu32, ASPData.MTSPU32); + strcat(buffer, MTSP); + strcat(buffer, ";"); + } + + //Ok, let's do the ASP + for (i = 0; i < SyncPointCount; i++) { + if (TEST_SYNC_POINTS == 0 + && strstr(object_address_name[iIndex], ASP[i].MasterIP) != NULL + && CurrentTimeU32 > StartTimeU32 && StartTimeU32 > 0 + && ASPData.TimeToSyncPointDbl > -1 + /*|| TEST_SYNC_POINTS == 1 && ASP[0].TestPort == object_udp_port[iIndex] && StartTimeU32 > 0 && iIndex == 0 && TimeToSyncPoint > -1 */ + ) { + // Use the util.c function for time here but it soent mather + gettimeofday(&CurrentTimeStruct, NULL); //Capture time + + TimeCap1 = (uint64_t) CurrentTimeStruct.tv_sec * 1000 + (uint64_t) CurrentTimeStruct.tv_usec / 1000; //Calculate initial timestamp + + OP[iIndex].x = ((dbl) atoi(XPosition)) / 1000; //Set x and y on OP (ObjectPosition) + OP[iIndex].y = ((dbl) atoi(YPosition)) / 1000; + + //OP[iIndex].OrigoDistance = sqrt(pow(OP[iIndex].x,2) + pow(OP[iIndex].y,2)); //Calculate hypotenuse + + UtilCalcPositionDelta(OriginLatitudeDbl, OriginLongitudeDbl, + atof(XPosition) / 1e7, atof(YPosition) / 1e7, &OP[iIndex]); + + if (OP[iIndex].BestFoundTrajectoryIndex <= OP[iIndex].SyncIndex) { + ASPData.CurrentTimeU32 = CurrentTimeU32; + ASPData.CurrentTimeDbl = + (((double)CurrentTimeU32 - (double)StartTimeU32) / 1000); + SearchStartIndex = OP[iIndex].BestFoundTrajectoryIndex - ASPStepBackCount; + UtilFindCurrentTrajectoryPosition(&OP[iIndex], SearchStartIndex, + ASPData.CurrentTimeDbl, ASPMaxTrajDiffDbl, + ASPMaxTimeDiffDbl, 0); + ASPData.BestFoundIndexI32 = OP[iIndex].BestFoundTrajectoryIndex; + + if (OP[iIndex].BestFoundTrajectoryIndex != TRAJ_POSITION_NOT_FOUND) { + ASPData.TimeToSyncPointDbl = UtilCalculateTimeToSync(&OP[iIndex]); + if (ASPData.TimeToSyncPointDbl > 0) { + if (ASPData.PrevTimeToSyncPointDbl != 0 && ASPFilterLevelDbl > 0) { + if (ASPData.TimeToSyncPointDbl / ASPData.PrevTimeToSyncPointDbl > + (1 + ASPFilterLevelDbl / 100)) + ASPData.TimeToSyncPointDbl = ASPData.PrevTimeToSyncPointDbl + ASPMaxDeltaTimeDbl; //TimeToSyncPoint*ASPFilterLevelDbl/500; + else if (ASPData.TimeToSyncPointDbl / + ASPData.PrevTimeToSyncPointDbl < + (1 - ASPFilterLevelDbl / 100)) + ASPData.TimeToSyncPointDbl = ASPData.PrevTimeToSyncPointDbl - ASPMaxDeltaTimeDbl; //TimeToSyncPoint*ASPFilterLevelDbl/500; + } + ASPData.MTSPU32 = + CurrentTimeU32 + (U32) (ASPData.TimeToSyncPointDbl * 4000); + + ASPData.PrevTimeToSyncPointDbl = ASPData.TimeToSyncPointDbl; + OldTimeU32 = CurrentTimeU32; + } + else { + CurrentTimeU32 = 0; + ASPData.TimeToSyncPointDbl = -1; + } + + } + + gettimeofday(&CurrentTimeStruct, NULL); + TimeCap2 = + (uint64_t) CurrentTimeStruct.tv_sec * 1000 + + (uint64_t) CurrentTimeStruct.tv_usec / 1000; + + ASPData.SyncPointIndexI32 = OP[iIndex].SyncIndex; + ASPData.IterationTimeU16 = (U16) (TimeCap2 - TimeCap1); + //Build ASP debug data and set to GSD + //bzero(buffer,OBJECT_MESS_BUFFER_SIZE); + //ObjectControlBuildASPMessage(buffer, &ASPData, 0); + DataDictionarySetRVSSAsp(GSD, &ASPData); + + if (atoi(Timestamp) % ASPDebugRate == 0) { + printf("%d, %d, %3.3f, %s, %s\n", CurrentTimeU32, StartTimeU32, + ASPData.TimeToSyncPointDbl, object_address_name[iIndex], + ASP[i].MasterIP); + printf + ("TtS=%3.3f, BestIndex=%d, MTSP=%d, iIndex=%d, IterationTime=%3.0f ms\n", + ASPData.TimeToSyncPointDbl, OP[iIndex].BestFoundTrajectoryIndex, + ASPData.MTSPU32, iIndex, ((double)(TimeCap2) - (double)TimeCap1)); + printf("CurrentTime=%3.3f, x=%3.3f mm, y=%3.3f\n\n", + ASPData.CurrentTimeDbl, OP[iIndex].x, OP[iIndex].y); + + //Build and send ASP on message queue + //(void)iCommSend(COMM_ASP,buffer); + } + } + } + } + OP[iIndex].Speed = atof(Speed); + } + else if (receivedMONRData > 0) + LogMessage(LOG_LEVEL_INFO, "MONR length error (should be %d but is %ld) from %s.", + sizeof (MONRType), object_address_name[iIndex]); + } + } + + + bzero(pcRecvBuffer, RECV_MESSAGE_BUFFER); + //Have we recieved a command? + if (iCommRecv(&iCommand, pcRecvBuffer, RECV_MESSAGE_BUFFER, NULL)) { + LogMessage(LOG_LEVEL_INFO, "Received command %d", iCommand); + + + if (iCommand == COMM_ARM && vGetState(GSD) == OBC_STATE_CONNECTED) { + + LogMessage(LOG_LEVEL_INFO, "Sending ARM"); + LOG_SEND(LogBuffer, "[ObjectControl] Sending ARM"); + vSetState(OBC_STATE_ARMED, GSD); + MessageLength = + ObjectControlBuildOSTMMessage(MessageBuffer, &OSTMData, COMMAND_OSTM_OPT_SET_ARMED_STATE, + 0); + for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { + /*Send OSTM message */ + UtilSendTCPData("[Object Control]", MessageBuffer, MessageLength, &socket_fds[iIndex], 0); + } + + ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_OK; //Set server to READY + } + else if (iCommand == COMM_DISARM && vGetState(GSD) == OBC_STATE_ARMED) { + + LogMessage(LOG_LEVEL_INFO, "Sending DISARM"); + LOG_SEND(LogBuffer, "[ObjectControl] Sending DISARM"); + vSetState(OBC_STATE_CONNECTED, GSD); + + MessageLength = + ObjectControlBuildOSTMMessage(MessageBuffer, &OSTMData, + COMMAND_OSTM_OPT_SET_DISARMED_STATE, 0); + for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { + /*Send OSTM message */ + UtilSendTCPData("[" MODULE_NAME "]", MessageBuffer, MessageLength, &socket_fds[iIndex], + 0); + } + + ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_OK; //Set server to READY + } + else if (iCommand == COMM_STRT && (vGetState(GSD) == OBC_STATE_ARMED) /*|| OBC_STATE_INITIALIZED) */ ) //OBC_STATE_INITIALIZED is temporary! + { + bzero(Timestamp, SMALL_BUFFER_SIZE_0); + MiscPtr = strchr(pcRecvBuffer, ';'); + strncpy(Timestamp, MiscPtr + 1, (uint64_t) strchr(MiscPtr + 1, ';') - (uint64_t) MiscPtr - 1); + StartTimeU32 = atol(Timestamp); + bzero(Timestamp, SMALL_BUFFER_SIZE_0); + MiscPtr += 1; + MiscPtr = strchr(pcRecvBuffer, ';'); + strncpy(Timestamp, MiscPtr + 1, (uint64_t) strchr(MiscPtr + 1, ';') - (uint64_t) MiscPtr - 1); + DelayedStartU32 = atoi(Timestamp); + ASPData.MTSPU32 = 0; + ASPData.TimeToSyncPointDbl = 0; + SearchStartIndex = -1; + ASPData.PrevTimeToSyncPointDbl = 0; + OldTimeU32 = CurrentTimeU32; + ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_OK; //Set server to READY + MessageLength = + ObjectControlBuildSTRTMessage(MessageBuffer, &STRTData, GPSTime, (U32) StartTimeU32, + DelayedStartU32, &OutgoingStartTimeU32, 0); + for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { + UtilSendTCPData("Object Control", MessageBuffer, MessageLength, &socket_fds[iIndex], 0); + } + vSetState(OBC_STATE_RUNNING, GSD); + + //Store STRT in GSD + if (STRTSentU8 == 0) { + //for(i = 0; i < MessageLength; i++) GSD->STRTData[i] = MessageBuffer[i]; + //GSD->STRTSizeU8 = (U8)MessageLength; + STRTSentU8 = 1; + } + //OBCState = OBC_STATE_INITIALIZED; //This is temporary + //printf("OutgoingStartTimeU32 = %d\n", OutgoingStartTimeU32); + + + GSD->ScenarioStartTimeU32 = OutgoingStartTimeU32; + bzero(MiscText, SMALL_BUFFER_SIZE_0); + sprintf(MiscText, "%" PRIu32, GSD->ScenarioStartTimeU32 << 2); + LOG_SEND(LogBuffer, "[ObjectControl] START received <%s>, GPS time <%s>", pcRecvBuffer, + MiscText); + } + else if (iCommand == COMM_REPLAY) { + ObjectcontrolExecutionMode = OBJECT_CONTROL_REPLAY_MODE; + LogMessage(LOG_LEVEL_INFO, "Entering REPLAY mode <%s>", pcRecvBuffer); + } + else if (iCommand == COMM_ABORT && vGetState(GSD) == OBC_STATE_RUNNING) { + vSetState(OBC_STATE_CONNECTED, GSD); + ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_ABORT; //Set server to ABORT + LogMessage(LOG_LEVEL_WARNING, "ABORT received"); + LOG_SEND(LogBuffer, "[ObjectControl] ABORT received."); + } + else if (iCommand == COMM_CONTROL) { + ObjectcontrolExecutionMode = OBJECT_CONTROL_CONTROL_MODE; + printf("[ObjectControl] Object control in CONTROL mode\n"); + } + else if (iCommand == COMM_INIT) { + LogMessage(LOG_LEVEL_INFO, "INIT received"); + + LOG_SEND(LogBuffer, "[ObjectControl] INIT received."); + nbr_objects = 0; + if (iFindObjectsInfo(object_traj_file, object_address_name, objectIPs, &nbr_objects) == 0) { + // Get objects; name and drive file + DataDictionaryGetForceToLocalhostU8(GSD, &iForceObjectToLocalhostU8); + + for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { + if (0 == iForceObjectToLocalhostU8) { + object_udp_port[iIndex] = SAFETY_CHANNEL_PORT; + object_tcp_port[iIndex] = CONTROL_CHANNEL_PORT; + } + else { + object_udp_port[iIndex] = SAFETY_CHANNEL_PORT + iIndex * 2; + object_tcp_port[iIndex] = CONTROL_CHANNEL_PORT + iIndex * 2; + } + } + + /*Setup Adaptive Sync Points (ASP) */ + UtilGetConfDirectoryPath(confDirectoryPath, sizeof (confDirectoryPath)); + strcat(confDirectoryPath, ADAPTIVE_SYNC_FILE_NAME); + fd = fopen(confDirectoryPath, "r"); + if (fd) { + SyncPointCount = UtilCountFileRows(fd) - 1; + fclose(fd); + fd = fopen(confDirectoryPath, "r"); + UtilReadLineCntSpecChars(fd, pcTempBuffer); //Read header + + for (i = 0; i < SyncPointCount; i++) { + UtilSetAdaptiveSyncPoint(&ASP[i], fd, 0); + if (TEST_SYNC_POINTS == 1) + ASP[i].TestPort = SAFETY_CHANNEL_PORT; + } + fclose(fd); + } + + vSetState(OBC_STATE_INITIALIZED, GSD); + LogMessage(LOG_LEVEL_INFO, "ObjectControl is initialized"); + LOG_SEND(LogBuffer, "[ObjectControl] ObjectControl is initialized."); + + //Remove temporary file + remove(TEMP_LOG_FILE); + if (USE_TEMP_LOGFILE) { + //Create temporary file + TempFd = fopen(TEMP_LOG_FILE, "w+"); + } + + //OSEMSentU8 = 0; + STRTSentU8 = 0; + + DataDictionaryInitMONR(GSD); + + } + else { + LogMessage(LOG_LEVEL_INFO, + "Could not initialize: object info was not processed successfully"); + pcSendBuffer[0] = (uint8_t) iCommand; + iCommSend(COMM_FAILURE, pcSendBuffer, sizeof (iCommand)); + } + + + } + else if (iCommand == COMM_ACCM && vGetState(GSD) == OBC_STATE_CONNECTED) { + UtilPopulateACCMDataStructFromMQ(pcRecvBuffer, sizeof (pcRecvBuffer), &mqACCMData); + iIndex = + iGetObjectIndexFromObjectIP(mqACCMData.ip, objectIPs, + sizeof (objectIPs) / sizeof (objectIPs[0])); + if (iIndex != -1) { + ObjectControlSendACCMMessage(&mqACCMData, &(socket_fds[iIndex]), 0); + } + else + LogMessage(LOG_LEVEL_WARNING, "Unable to send ACCM: no valid socket found"); + } + else if (iCommand == COMM_EXAC && vGetState(GSD) == OBC_STATE_RUNNING) { + UtilPopulateEXACDataStructFromMQ(pcRecvBuffer, sizeof (pcRecvBuffer), &mqEXACData); + iIndex = + iGetObjectIndexFromObjectIP(mqEXACData.ip, objectIPs, + sizeof (objectIPs) / sizeof (objectIPs[0])); + if (iIndex != -1) + ObjectControlSendEXACMessage(&mqEXACData, &(socket_fds[iIndex]), 0); + else + LogMessage(LOG_LEVEL_WARNING, "Unable to send EXAC: no valid socket found"); + } + else if (iCommand == COMM_CONNECT && vGetState(GSD) == OBC_STATE_INITIALIZED) { + LogMessage(LOG_LEVEL_INFO, "CONNECT received"); + LOG_SEND(LogBuffer, "[ObjectControl] CONNECT received."); + + /* Connect and send drive files */ + for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { + + UtilSetObjectPositionIP(&OP[iIndex], object_address_name[iIndex]); + + MessageLength = ObjectControlBuildOSEMMessage(MessageBuffer, &OSEMData, GPSTime, + OriginLatitude, OriginLongitude, + OriginAltitude, 0); + + DisconnectU8 = 0; + + do { + + iResult = + vConnectObject(&socket_fds[iIndex], object_address_name[iIndex], + object_tcp_port[iIndex], &DisconnectU8); + + if (iResult < 0) { + switch (errno) { + case ECONNREFUSED: + LogMessage(LOG_LEVEL_INFO, + "Unable to connect to object %s:%d, retry in %d sec...", + object_address_name[iIndex], object_tcp_port[iIndex], + (!(1 & DisconnectU8)) * 3); + LOG_SEND(LogBuffer, + "[ObjectControl] Was not able to connect to object, [IP: %s] [PORT: %d], retry in %d sec...", + object_address_name[iIndex], object_tcp_port[iIndex], + (!(1 & DisconnectU8)) * 3); + (void)sleep(3); // TODO: Move this to the rest of the sleep operations? Also, remove the hardcoded 3 + break; + case EADDRINUSE: + LogMessage(LOG_LEVEL_ERROR, + "[ObjectControl] Local address/port already in use"); + break; + case EALREADY: + LogMessage(LOG_LEVEL_ERROR, + "[ObjectControl] Previous connection attempt still in progress"); + break; + case EISCONN: + LogMessage(LOG_LEVEL_ERROR, "[ObjectControl] Socket is already connected"); + break; + case ENETUNREACH: + LogMessage(LOG_LEVEL_ERROR, "[ObjectControl] Network unreachable"); + break; + case ETIMEDOUT: + LogMessage(LOG_LEVEL_ERROR, "[ObjectControl] Connection timed out"); + break; + default: + LogMessage(LOG_LEVEL_ERROR, "ERR: Failed to connect to control socket"); + break; + } + + } + + bzero(pcRecvBuffer, RECV_MESSAGE_BUFFER); + //Have we received a command? + if (iCommRecv(&iCommand, pcRecvBuffer, RECV_MESSAGE_BUFFER, NULL)) { + if (iCommand == COMM_DISCONNECT) { + DisconnectU8 = 1; + LOG_SEND(LogBuffer, "[ObjectControl] DISCONNECT received."); + } + + } + + } while (iExit == 0 && iResult < 0 && DisconnectU8 == 0); + + if (iResult >= 0) { + /* Send OSEM command in mq so that we get some information like GPSweek, origin (latitude,logitude,altitude in gps coordinates) */ + LogMessage(LOG_LEVEL_INFO, "Sending OSEM"); + LOG_SEND(LogBuffer, "[ObjectControl] Sending OSEM."); + + ObjectControlOSEMtoASCII(&OSEMData, GPSWeek, OriginLatitude, OriginLongitude, + OriginAltitude); + bzero(pcSendBuffer, sizeof (pcSendBuffer)); + strcat(pcSendBuffer, GPSWeek); + strcat(pcSendBuffer, ";"); + strcat(pcSendBuffer, OriginLatitude); + strcat(pcSendBuffer, ";"); + strcat(pcSendBuffer, OriginLongitude); + strcat(pcSendBuffer, ";"); + strcat(pcSendBuffer, OriginAltitude); + + //Restore the buffers + DataDictionaryGetOriginLatitudeC8(GSD, OriginLatitude, SMALL_BUFFER_SIZE_0); + DataDictionaryGetOriginLongitudeC8(GSD, OriginLongitude, SMALL_BUFFER_SIZE_0); + DataDictionaryGetOriginAltitudeC8(GSD, OriginAltitude, SMALL_BUFFER_SIZE_0); + + if (iCommSend(COMM_OSEM, pcSendBuffer, strlen(pcSendBuffer) + 1) < 0) { + LogMessage(LOG_LEVEL_ERROR, + "Fatal communication fault when sending OSEM command - entering error state"); + vSetState(OBC_STATE_ERROR, GSD); + ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_ABORT; + } + UtilSendTCPData("Object Control", MessageBuffer, MessageLength, &socket_fds[iIndex], + 0); + + /*Here we send TRAJ, if the IP-address not is found */ + if (strstr(DTMReceivers, object_address_name[iIndex]) == NULL) { + + fd = fopen(object_traj_file[iIndex], "r"); + + if (fd != NULL) { + //RowCount = UtilCountFileRows(fd); + //printf("RowCount: %d\n", RowCount); + //fclose(fd); + + //fd = fopen(object_traj_file[iIndex], "r"); + //printf("Open file: %s\n", object_traj_file[iIndex]); + UtilReadLineCntSpecChars(fd, FileHeaderBufferC8); + fclose(fd); + + /*TRAJ*/ + MessageLength = ObjectControlBuildTRAJMessageHeader(TrajBuffer, + &RowCount, + &HeaderData, + &TRAJInfoData, + FileHeaderBufferC8, + 0); + + //printf("RowCount: %d\n", RowCount); + + /*Send TRAJ header */ + UtilSendTCPData("Object Control", TrajBuffer, MessageLength, + &socket_fds[iIndex], 0); + + /*Send TRAJ data */ + ObjectControlSendTRAJMessage(object_traj_file[iIndex], &socket_fds[iIndex], + RowCount, + (char *)&object_address_name[iIndex], + object_tcp_port[iIndex], &DOTMData, 0); + + } + else + LogMessage(LOG_LEVEL_WARNING, "Could not open file <%s>", + object_traj_file[iIndex]); + } + + + /* Adaptive Sync Points object configuration start... */ + if (TEST_SYNC_POINTS == 1) + printf("Trajfile: %s\n", object_traj_file[iIndex]); + OP[iIndex].TrajectoryPositionCount = RowCount; + OP[iIndex].SpaceArr = SpaceArr[iIndex]; + OP[iIndex].TimeArr = TimeArr[iIndex]; + OP[iIndex].SpaceTimeArr = SpaceTimeArr[iIndex]; + UtilPopulateSpaceTimeArr(&OP[iIndex], object_traj_file[iIndex]); + + LogMessage(LOG_LEVEL_INFO, "Sync point counts: %d", SyncPointCount); + for (i = 0; i < SyncPointCount; i++) { + if (TEST_SYNC_POINTS == 1 && iIndex == 1) { + /*Send SYPM to slave */ + MessageLength = + ObjectControlBuildSYPMMessage(MessageBuffer, &SYPMData, + ASP[i].SlaveTrajSyncTime * 1000, + ASP[i].SlaveSyncStopTime * 1000, 1); + UtilSendTCPData("Object Control", MessageBuffer, MessageLength, + &socket_fds[iIndex], 0); + } + else if (TEST_SYNC_POINTS == 0 + && strstr(object_address_name[iIndex], ASP[i].SlaveIP) != NULL) { + /*Send SYPM to slave */ + MessageLength = + ObjectControlBuildSYPMMessage(MessageBuffer, &SYPMData, + ASP[i].SlaveTrajSyncTime * 1000, + ASP[i].SlaveSyncStopTime * 1000, 1); + UtilSendTCPData("Object Control", MessageBuffer, MessageLength, + &socket_fds[iIndex], 0); + } + } + + /*Set Sync point in OP */ + for (i = 0; i < SyncPointCount; i++) { + if (TEST_SYNC_POINTS == 1 && iIndex == 0) + UtilSetSyncPoint(&OP[iIndex], 0, 0, 0, ASP[i].MasterTrajSyncTime); + else if (TEST_SYNC_POINTS == 0 + && strstr(object_address_name[iIndex], ASP[i].MasterIP) != NULL) + UtilSetSyncPoint(&OP[iIndex], 0, 0, 0, ASP[i].MasterTrajSyncTime); + } + /* ...end */ + } + + } + for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { + if (USE_TEST_HOST == 0) + vCreateSafetyChannel(object_address_name[iIndex], object_udp_port[iIndex], + &safety_socket_fd[iIndex], &safety_object_addr[iIndex]); + else if (USE_TEST_HOST == 1) + vCreateSafetyChannel(TESTSERVER_IP, object_udp_port[iIndex], + &safety_socket_fd[iIndex], &safety_object_addr[iIndex]); + } + + uiTimeCycle = 0; + + /* Execution mode */ + ObjectcontrolExecutionMode = OBJECT_CONTROL_CONTROL_MODE; + + /*Set server status */ + ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_OK; //Set server to READY + + if (DisconnectU8 == 0) { + vSetState(OBC_STATE_CONNECTED, GSD); + iCommSend(COMM_OBJECTS_CONNECTED, NULL, 0); + } + else if (DisconnectU8 == 1) + vSetState(OBC_STATE_IDLE, GSD); + } + else if (iCommand == COMM_DATA_DICT) { + + LogMessage(LOG_LEVEL_INFO, "Updating variables from DataDictionary."); + DataDictionaryGetOriginLatitudeC8(GSD, OriginLatitude, SMALL_BUFFER_SIZE_0); + DataDictionaryGetOriginLongitudeC8(GSD, OriginLongitude, SMALL_BUFFER_SIZE_0); + DataDictionaryGetOriginAltitudeC8(GSD, OriginAltitude, SMALL_BUFFER_SIZE_0); + + DataDictionaryGetOriginLatitudeDbl(GSD, &OriginLatitudeDbl); + DataDictionaryGetOriginLongitudeDbl(GSD, &OriginLongitudeDbl); + DataDictionaryGetOriginAltitudeDbl(GSD, &OriginAltitudeDbl); + + OriginLatitudeDbl = atof(OriginLatitude); + OriginLongitudeDbl = atof(OriginLongitude); + OriginAltitudeDbl = atof(OriginAltitude); + OriginHeadingDbl = atof(OriginHeading); + OriginPosition.Latitude = OriginLatitudeDbl; + OriginPosition.Longitude = OriginLongitudeDbl; + OriginPosition.Altitude = OriginAltitudeDbl; + OriginPosition.Heading = OriginHeadingDbl; + + DataDictionaryGetForceToLocalhostU8(GSD, &iForceObjectToLocalhostU8); + LogMessage(LOG_LEVEL_INFO, "ForceObjectToLocalhost = %d", iForceObjectToLocalhostU8); + LOG_SEND(LogBuffer, "[ObjectControl] ForceObjectToLocalhost = %d", iForceObjectToLocalhostU8); + + DataDictionaryGetASPMaxTimeDiffDbl(GSD, &ASPMaxTimeDiffDbl); + DataDictionaryGetASPMaxTrajDiffDbl(GSD, &ASPMaxTrajDiffDbl); + DataDictionaryGetASPStepBackCountU32(GSD, &ASPStepBackCount); + DataDictionaryGetASPFilterLevelDbl(GSD, &ASPFilterLevelDbl); + DataDictionaryGetASPMaxDeltaTimeDbl(GSD, &ASPMaxDeltaTimeDbl); + ASPDebugRate = 1; + DataDictionaryGetVOILReceiversC8(GSD, VOILReceivers, SMALL_BUFFER_SIZE_254); + DataDictionaryGetDTMReceiversC8(GSD, DTMReceivers, SMALL_BUFFER_SIZE_254); + } + else if (iCommand == COMM_DISCONNECT) { + //#ifndef NOTCP + for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { + vDisconnectObject(&socket_fds[iIndex]); + } + DataDictionaryFreeMONR(GSD); + //#endif //NOTCP + + LogMessage(LOG_LEVEL_INFO, "DISCONNECT received"); + LOG_SEND(LogBuffer, "[ObjectControl] DISCONNECT received."); + /* Close safety socket */ + for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { + vCloseSafetyChannel(&safety_socket_fd[iIndex]); + } + vSetState(OBC_STATE_IDLE, GSD); + } + else if (iCommand == COMM_EXIT) { + iExit = 1; + iCommClose(); + } + else { + LogMessage(LOG_LEVEL_WARNING, "Unhandled command in object control: %d", iCommand); + } + } + + if (!iExit) { + /* Make call periodic */ + sleep_time = iCommand == COMM_INV ? mqEmptyPollPeriod : mqNonEmptyPollPeriod; + + ++uiTimeCycle; + if (uiTimeCycle >= HEARTBEAT_TIME_MS / TASK_PERIOD_MS) { + uiTimeCycle = 0; + } + + // Periodically send state to signal aliveness + TimeSetToCurrentSystemTime(¤tTime); + if (timercmp(¤tTime, &nextStateReportTime, >)) { + timeradd(&nextStateReportTime, &stateReportPeriod, &nextStateReportTime); + + bzero(Buffer2, sizeof (Buffer2)); + Buffer2[0] = (uint8_t) (DataDictionaryGetOBCStateU8(GSD)); + if (iCommSend(COMM_OBC_STATE, Buffer2, sizeof (Buffer2)) < 0) { + LogMessage(LOG_LEVEL_ERROR, + "Fatal communication fault when sending OBC_STATE command - entering error state"); + vSetState(OBC_STATE_ERROR, GSD); + ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_ABORT; + } + } + + (void)nanosleep(&sleep_time, &ref_time); + } + } + + LogMessage(LOG_LEVEL_INFO, "Object control exiting"); } @@ -1122,1162 +1124,1162 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { ------------------------------------------------------------*/ void signalHandler(int signo) { - if (signo == SIGINT) { - LogMessage(LOG_LEVEL_WARNING, "Caught keyboard interrupt"); - iExit = 1; - } - else { - LogMessage(LOG_LEVEL_ERROR, "Caught unhandled signal"); - } + if (signo == SIGINT) { + LogMessage(LOG_LEVEL_WARNING, "Caught keyboard interrupt"); + iExit = 1; + } + else { + LogMessage(LOG_LEVEL_ERROR, "Caught unhandled signal"); + } } I32 ObjectControlBuildVOILMessage(C8 * MessageBuffer, VOILType * VOILData, C8 * SimData, U8 debug) { - I32 MessageIndex = 0, i; - U16 Crc = 0; - C8 *p; - U16 U16Data = 0; - I16 I16Data = 0; - U32 U32Data = 0; - I32 I32Data = 0; - - if (debug) { - printf("Length: %d\n", *(SimData + 3)); - for (i = 0; i < *(SimData + 3) + 4; i++) - printf("%x-", *(SimData + i)); - printf("\n"); - } - - U16Data = (U16Data | *(SimData + 5)) << 8; - U16Data = (U16Data | *(SimData + 4)); - U16 MessageId = U16Data; - - //printf("MessageId = %x\n", MessageId); - - U32Data = (U32Data | *(SimData + 6)) << 8; - U32Data = (U32Data | *(SimData + 7)) << 8; - U32Data = (U32Data | *(SimData + 8)) << 8; - U32Data = (U32Data | *(SimData + 9)); - U32 GPSSOW = U32Data; - - //printf("GPSSOW = %x\n", GPSSOW); - U8 DynamicWorldState = *(SimData + 10); - U8 ObjectCount = *(SimData + 11); - - //printf("ObjectCount = %d\n", ObjectCount); - - U8 ObjectId = *(SimData + 12); - U8 ObjectState = *(SimData + 13); - - I32Data = (I32Data | *(SimData + 14)) << 8; - I32Data = (I32Data | *(SimData + 15)) << 8; - I32Data = (I32Data | *(SimData + 16)) << 8; - I32Data = (I32Data | *(SimData + 17)); - I32 XPosition = I32Data; - - I32Data = 0; - I32Data = (I32Data | *(SimData + 18)) << 8; - I32Data = (I32Data | *(SimData + 19)) << 8; - I32Data = (I32Data | *(SimData + 20)) << 8; - I32Data = (I32Data | *(SimData + 21)); - I32 YPosition = I32Data; - - I32Data = 0; - I32Data = (I32Data | *(SimData + 22)) << 8; - I32Data = (I32Data | *(SimData + 23)) << 8; - I32Data = (I32Data | *(SimData + 24)) << 8; - I32Data = (I32Data | *(SimData + 25)); - I32 ZPosition = I32Data; - - U16Data = 0; - U16Data = (U16Data | *(SimData + 26)) << 8; - U16Data = (U16Data | *(SimData + 27)); - U16 Heading = U16Data; - - U16Data = 0; - U16Data = (U16Data | *(SimData + 28)) << 8; - U16Data = (U16Data | *(SimData + 29)); - U16 Pitch = U16Data; - - U16Data = 0; - U16Data = (U16Data | *(SimData + 30)) << 8; - U16Data = (U16Data | *(SimData + 31)); - U16 Roll = U16Data; - - //printf("Roll = %d\n", Roll); - I16Data = 0; - I16Data = (I16Data | *(SimData + 32)) << 8; - I16Data = (I16Data | *(SimData + 33)); - I16 Speed = I16Data; - - //printf("Speed = %d\n", Speed); - - - bzero(MessageBuffer, - ObjectCount * sizeof (Sim1Type) + 6 + COMMAND_MESSAGE_FOOTER_LENGTH + - COMMAND_MESSAGE_HEADER_LENGTH); - - - VOILData->Header.SyncWordU16 = SYNC_WORD; - VOILData->Header.TransmitterIdU8 = 0; - VOILData->Header.MessageCounterU8 = 0; - VOILData->Header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; - VOILData->Header.MessageIdU16 = COMMAND_VOIL_CODE; - VOILData->Header.MessageLengthU32 = ObjectCount * sizeof (Sim1Type) + 6 - COMMAND_MESSAGE_HEADER_LENGTH; - VOILData->GPSQmsOfWeekU32 = GPSSOW; - VOILData->WorldStateU8 = DynamicWorldState; - VOILData->ObjectCountU8 = ObjectCount; - VOILData->SimObjects[0].ObjectIdU8 = ObjectId; - VOILData->SimObjects[0].ObjectStateU8 = ObjectState; - VOILData->SimObjects[0].XPositionI32 = XPosition; - VOILData->SimObjects[0].YPositionI32 = YPosition; - VOILData->SimObjects[0].ZPositionI32 = ZPosition; - VOILData->SimObjects[0].HeadingU16 = Heading; - VOILData->SimObjects[0].PitchU16 = Pitch; - VOILData->SimObjects[0].RollU16 = Roll; - VOILData->SimObjects[0].SpeedI16 = Speed; - - - p = (C8 *) VOILData; - for (i = 0; i < ObjectCount * sizeof (Sim1Type) + 6 + COMMAND_MESSAGE_HEADER_LENGTH; i++) - *(MessageBuffer + i) = *p++; - //Crc = crc_16((const C8*)MessageBuffer, sizeof(VOILData)); - Crc = 0; - *(MessageBuffer + i++) = (U8) (Crc); - *(MessageBuffer + i++) = (U8) (Crc >> 8); - MessageIndex = i; - - if (debug) { - // TODO: use byte printout from logging when it has been implemented - printf("VOILData total length = %d bytes (header+message+footer)\n", - (int)(ObjectCount * sizeof (Sim1Type) + 6 + COMMAND_MESSAGE_FOOTER_LENGTH + - COMMAND_MESSAGE_HEADER_LENGTH)); - printf("----HEADER----\n"); - for (i = 0; i < sizeof (HeaderType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----MESSAGE----\n"); - for (; i < sizeof (Sim1Type) * ObjectCount + 6 + COMMAND_MESSAGE_HEADER_LENGTH; i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----FOOTER----\n"); - for (; i < MessageIndex; i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n"); - } - - - return ObjectCount * sizeof (Sim1Type) + 6 + COMMAND_MESSAGE_HEADER_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH; //Total number of bytes + I32 MessageIndex = 0, i; + U16 Crc = 0; + C8 *p; + U16 U16Data = 0; + I16 I16Data = 0; + U32 U32Data = 0; + I32 I32Data = 0; + + if (debug) { + printf("Length: %d\n", *(SimData + 3)); + for (i = 0; i < *(SimData + 3) + 4; i++) + printf("%x-", *(SimData + i)); + printf("\n"); + } + + U16Data = (U16Data | *(SimData + 5)) << 8; + U16Data = (U16Data | *(SimData + 4)); + U16 MessageId = U16Data; + + //printf("MessageId = %x\n", MessageId); + + U32Data = (U32Data | *(SimData + 6)) << 8; + U32Data = (U32Data | *(SimData + 7)) << 8; + U32Data = (U32Data | *(SimData + 8)) << 8; + U32Data = (U32Data | *(SimData + 9)); + U32 GPSSOW = U32Data; + + //printf("GPSSOW = %x\n", GPSSOW); + U8 DynamicWorldState = *(SimData + 10); + U8 ObjectCount = *(SimData + 11); + + //printf("ObjectCount = %d\n", ObjectCount); + + U8 ObjectId = *(SimData + 12); + U8 ObjectState = *(SimData + 13); + + I32Data = (I32Data | *(SimData + 14)) << 8; + I32Data = (I32Data | *(SimData + 15)) << 8; + I32Data = (I32Data | *(SimData + 16)) << 8; + I32Data = (I32Data | *(SimData + 17)); + I32 XPosition = I32Data; + + I32Data = 0; + I32Data = (I32Data | *(SimData + 18)) << 8; + I32Data = (I32Data | *(SimData + 19)) << 8; + I32Data = (I32Data | *(SimData + 20)) << 8; + I32Data = (I32Data | *(SimData + 21)); + I32 YPosition = I32Data; + + I32Data = 0; + I32Data = (I32Data | *(SimData + 22)) << 8; + I32Data = (I32Data | *(SimData + 23)) << 8; + I32Data = (I32Data | *(SimData + 24)) << 8; + I32Data = (I32Data | *(SimData + 25)); + I32 ZPosition = I32Data; + + U16Data = 0; + U16Data = (U16Data | *(SimData + 26)) << 8; + U16Data = (U16Data | *(SimData + 27)); + U16 Heading = U16Data; + + U16Data = 0; + U16Data = (U16Data | *(SimData + 28)) << 8; + U16Data = (U16Data | *(SimData + 29)); + U16 Pitch = U16Data; + + U16Data = 0; + U16Data = (U16Data | *(SimData + 30)) << 8; + U16Data = (U16Data | *(SimData + 31)); + U16 Roll = U16Data; + + //printf("Roll = %d\n", Roll); + I16Data = 0; + I16Data = (I16Data | *(SimData + 32)) << 8; + I16Data = (I16Data | *(SimData + 33)); + I16 Speed = I16Data; + + //printf("Speed = %d\n", Speed); + + + bzero(MessageBuffer, + ObjectCount * sizeof (Sim1Type) + 6 + COMMAND_MESSAGE_FOOTER_LENGTH + + COMMAND_MESSAGE_HEADER_LENGTH); + + + VOILData->Header.SyncWordU16 = SYNC_WORD; + VOILData->Header.TransmitterIdU8 = 0; + VOILData->Header.MessageCounterU8 = 0; + VOILData->Header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; + VOILData->Header.MessageIdU16 = COMMAND_VOIL_CODE; + VOILData->Header.MessageLengthU32 = ObjectCount * sizeof (Sim1Type) + 6 - COMMAND_MESSAGE_HEADER_LENGTH; + VOILData->GPSQmsOfWeekU32 = GPSSOW; + VOILData->WorldStateU8 = DynamicWorldState; + VOILData->ObjectCountU8 = ObjectCount; + VOILData->SimObjects[0].ObjectIdU8 = ObjectId; + VOILData->SimObjects[0].ObjectStateU8 = ObjectState; + VOILData->SimObjects[0].XPositionI32 = XPosition; + VOILData->SimObjects[0].YPositionI32 = YPosition; + VOILData->SimObjects[0].ZPositionI32 = ZPosition; + VOILData->SimObjects[0].HeadingU16 = Heading; + VOILData->SimObjects[0].PitchU16 = Pitch; + VOILData->SimObjects[0].RollU16 = Roll; + VOILData->SimObjects[0].SpeedI16 = Speed; + + + p = (C8 *) VOILData; + for (i = 0; i < ObjectCount * sizeof (Sim1Type) + 6 + COMMAND_MESSAGE_HEADER_LENGTH; i++) + *(MessageBuffer + i) = *p++; + //Crc = crc_16((const C8*)MessageBuffer, sizeof(VOILData)); + Crc = 0; + *(MessageBuffer + i++) = (U8) (Crc); + *(MessageBuffer + i++) = (U8) (Crc >> 8); + MessageIndex = i; + + if (debug) { + // TODO: use byte printout from logging when it has been implemented + printf("VOILData total length = %d bytes (header+message+footer)\n", + (int)(ObjectCount * sizeof (Sim1Type) + 6 + COMMAND_MESSAGE_FOOTER_LENGTH + + COMMAND_MESSAGE_HEADER_LENGTH)); + printf("----HEADER----\n"); + for (i = 0; i < sizeof (HeaderType); i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n----MESSAGE----\n"); + for (; i < sizeof (Sim1Type) * ObjectCount + 6 + COMMAND_MESSAGE_HEADER_LENGTH; i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n----FOOTER----\n"); + for (; i < MessageIndex; i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n"); + } + + + return ObjectCount * sizeof (Sim1Type) + 6 + COMMAND_MESSAGE_HEADER_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH; //Total number of bytes } I32 ObjectControlBuildMONRMessage(C8 * MonrData, MONRType * MONRData, U8 debug) { - I32 MessageIndex = 0, i = 0; - dbl Data; - U16 Crc = 0, U16Data = 0; - I16 I16Data = 0; - U32 U32Data = 0; - I32 I32Data = 0; - U64 U64Data = 0; - U16 contentLength = 0; - U16 valueID = 0; - C8 *p = MonrData; - - // Decode ISO header - memcpy(&MONRData->Header.SyncWordU16, p, sizeof (MONRData->Header.SyncWordU16)); - p += sizeof (MONRData->Header.SyncWordU16); - - memcpy(&MONRData->Header.TransmitterIdU8, p, sizeof (MONRData->Header.TransmitterIdU8)); - p += sizeof (MONRData->Header.TransmitterIdU8); - - memcpy(&MONRData->Header.MessageCounterU8, p, sizeof (MONRData->Header.MessageCounterU8)); - p += sizeof (MONRData->Header.MessageCounterU8); - - memcpy(&MONRData->Header.AckReqProtVerU8, p, sizeof (MONRData->Header.AckReqProtVerU8)); - p += sizeof (MONRData->Header.AckReqProtVerU8); - - memcpy(&MONRData->Header.MessageIdU16, p, sizeof (MONRData->Header.MessageIdU16)); - p += sizeof (MONRData->Header.MessageIdU16); - - memcpy(&MONRData->Header.MessageLengthU32, p, sizeof (MONRData->Header.MessageLengthU32)); - p += sizeof (MONRData->Header.MessageLengthU32); - - // Decode content header - memcpy(&valueID, p, sizeof (valueID)); - if (valueID == VALUE_ID_MONR_STRUCT) { - memcpy(&MONRData->MonrStructValueIdU16, p, sizeof (MONRData->MonrStructValueIdU16)); - p += sizeof (MONRData->MonrStructValueIdU16); - - //memcpy(&contentLength, p, sizeof (contentLength)); - memcpy(&MONRData->MonrStructContentLengthU16, p, sizeof (MONRData->MonrStructContentLengthU16)); - p += sizeof (MONRData->MonrStructContentLengthU16); - - // TODO: check on content length - } - else if (debug) { - LogPrint("MONR message did not contain a content header"); - } - - // Decode content - memcpy(&MONRData->GPSQmsOfWeekU32, p, sizeof (MONRData->GPSQmsOfWeekU32)); - p += sizeof (MONRData->GPSQmsOfWeekU32); - - memcpy(&MONRData->XPositionI32, p, sizeof (MONRData->XPositionI32)); - p += sizeof (MONRData->XPositionI32); - - memcpy(&MONRData->YPositionI32, p, sizeof (MONRData->YPositionI32)); - p += sizeof (MONRData->YPositionI32); - - memcpy(&MONRData->ZPositionI32, p, sizeof (MONRData->ZPositionI32)); - p += sizeof (MONRData->ZPositionI32); - - memcpy(&MONRData->HeadingU16, p, sizeof (MONRData->HeadingU16)); - p += sizeof (MONRData->HeadingU16); - - memcpy(&MONRData->LongitudinalSpeedI16, p, sizeof (MONRData->LongitudinalSpeedI16)); - p += sizeof (MONRData->LongitudinalSpeedI16); - - memcpy(&MONRData->LateralSpeedI16, p, sizeof (MONRData->LateralSpeedI16)); - p += sizeof (MONRData->LateralSpeedI16); - - memcpy(&MONRData->LongitudinalAccI16, p, sizeof (MONRData->LongitudinalAccI16)); - p += sizeof (MONRData->LongitudinalAccI16); - - memcpy(&MONRData->LateralAccI16, p, sizeof (MONRData->LateralAccI16)); - p += sizeof (MONRData->LateralAccI16); - - memcpy(&MONRData->DriveDirectionU8, p, sizeof (MONRData->DriveDirectionU8)); - p += sizeof (MONRData->DriveDirectionU8); - - memcpy(&MONRData->StateU8, p, sizeof (MONRData->StateU8)); - p += sizeof (MONRData->StateU8); - - memcpy(&MONRData->ReadyToArmU8, p, sizeof (MONRData->ReadyToArmU8)); - p += sizeof (MONRData->ReadyToArmU8); - - memcpy(&MONRData->ErrorStatusU8, p, sizeof (MONRData->ErrorStatusU8)); - p += sizeof (MONRData->ErrorStatusU8); - - // Footer - memcpy(&MONRData->CRC, p, sizeof (MONRData->CRC)); - p += sizeof (MONRData->CRC); - - // TODO: check on CRC - - if (debug == 1) { - LogPrint("MONR:"); - LogPrint("SyncWord = %x", MONRData->Header.SyncWordU16); - LogPrint("TransmitterId = %d", MONRData->Header.TransmitterIdU8); - LogPrint("PackageCounter = %d", MONRData->Header.MessageCounterU8); - LogPrint("AckReq = %d", MONRData->Header.AckReqProtVerU8); - LogPrint("MessageId = %d", MONRData->Header.MessageIdU16); - LogPrint("MessageLength = %d", MONRData->Header.MessageLengthU32); - LogPrint("ValueId = %d", MONRData->MonrStructValueIdU16); - LogPrint("ContentLength = %d", MONRData->MonrStructContentLengthU16); - LogPrint("GPSSOW = %d", MONRData->GPSQmsOfWeekU32); - LogPrint("XPosition = %d", MONRData->XPositionI32); - LogPrint("YPosition = %d", MONRData->YPositionI32); - LogPrint("ZPosition = %d", MONRData->ZPositionI32); - LogPrint("Heading = %d", MONRData->HeadingU16); - LogPrint("LongitudinalSpeed = %d", MONRData->LongitudinalSpeedI16); - LogPrint("LateralSpeed = %d", MONRData->LateralSpeedI16); - LogPrint("LongitudinalAcc = %d", MONRData->LongitudinalAccI16); - LogPrint("LateralAcc = %d", MONRData->LateralAccI16); - LogPrint("DriveDirection = %d", MONRData->DriveDirectionU8); - LogPrint("State = %d", MONRData->StateU8); - LogPrint("ReadyToArm = %d", MONRData->ReadyToArmU8); - LogPrint("ErrorStatus = %d", MONRData->ErrorStatusU8); - } - - return 0; + I32 MessageIndex = 0, i = 0; + dbl Data; + U16 Crc = 0, U16Data = 0; + I16 I16Data = 0; + U32 U32Data = 0; + I32 I32Data = 0; + U64 U64Data = 0; + U16 contentLength = 0; + U16 valueID = 0; + C8 *p = MonrData; + + // Decode ISO header + memcpy(&MONRData->Header.SyncWordU16, p, sizeof (MONRData->Header.SyncWordU16)); + p += sizeof (MONRData->Header.SyncWordU16); + + memcpy(&MONRData->Header.TransmitterIdU8, p, sizeof (MONRData->Header.TransmitterIdU8)); + p += sizeof (MONRData->Header.TransmitterIdU8); + + memcpy(&MONRData->Header.MessageCounterU8, p, sizeof (MONRData->Header.MessageCounterU8)); + p += sizeof (MONRData->Header.MessageCounterU8); + + memcpy(&MONRData->Header.AckReqProtVerU8, p, sizeof (MONRData->Header.AckReqProtVerU8)); + p += sizeof (MONRData->Header.AckReqProtVerU8); + + memcpy(&MONRData->Header.MessageIdU16, p, sizeof (MONRData->Header.MessageIdU16)); + p += sizeof (MONRData->Header.MessageIdU16); + + memcpy(&MONRData->Header.MessageLengthU32, p, sizeof (MONRData->Header.MessageLengthU32)); + p += sizeof (MONRData->Header.MessageLengthU32); + + // Decode content header + memcpy(&valueID, p, sizeof (valueID)); + if (valueID == VALUE_ID_MONR_STRUCT) { + memcpy(&MONRData->MonrStructValueIdU16, p, sizeof (MONRData->MonrStructValueIdU16)); + p += sizeof (MONRData->MonrStructValueIdU16); + + //memcpy(&contentLength, p, sizeof (contentLength)); + memcpy(&MONRData->MonrStructContentLengthU16, p, sizeof (MONRData->MonrStructContentLengthU16)); + p += sizeof (MONRData->MonrStructContentLengthU16); + + // TODO: check on content length + } + else if (debug) { + LogPrint("MONR message did not contain a content header"); + } + + // Decode content + memcpy(&MONRData->GPSQmsOfWeekU32, p, sizeof (MONRData->GPSQmsOfWeekU32)); + p += sizeof (MONRData->GPSQmsOfWeekU32); + + memcpy(&MONRData->XPositionI32, p, sizeof (MONRData->XPositionI32)); + p += sizeof (MONRData->XPositionI32); + + memcpy(&MONRData->YPositionI32, p, sizeof (MONRData->YPositionI32)); + p += sizeof (MONRData->YPositionI32); + + memcpy(&MONRData->ZPositionI32, p, sizeof (MONRData->ZPositionI32)); + p += sizeof (MONRData->ZPositionI32); + + memcpy(&MONRData->HeadingU16, p, sizeof (MONRData->HeadingU16)); + p += sizeof (MONRData->HeadingU16); + + memcpy(&MONRData->LongitudinalSpeedI16, p, sizeof (MONRData->LongitudinalSpeedI16)); + p += sizeof (MONRData->LongitudinalSpeedI16); + + memcpy(&MONRData->LateralSpeedI16, p, sizeof (MONRData->LateralSpeedI16)); + p += sizeof (MONRData->LateralSpeedI16); + + memcpy(&MONRData->LongitudinalAccI16, p, sizeof (MONRData->LongitudinalAccI16)); + p += sizeof (MONRData->LongitudinalAccI16); + + memcpy(&MONRData->LateralAccI16, p, sizeof (MONRData->LateralAccI16)); + p += sizeof (MONRData->LateralAccI16); + + memcpy(&MONRData->DriveDirectionU8, p, sizeof (MONRData->DriveDirectionU8)); + p += sizeof (MONRData->DriveDirectionU8); + + memcpy(&MONRData->StateU8, p, sizeof (MONRData->StateU8)); + p += sizeof (MONRData->StateU8); + + memcpy(&MONRData->ReadyToArmU8, p, sizeof (MONRData->ReadyToArmU8)); + p += sizeof (MONRData->ReadyToArmU8); + + memcpy(&MONRData->ErrorStatusU8, p, sizeof (MONRData->ErrorStatusU8)); + p += sizeof (MONRData->ErrorStatusU8); + + // Footer + memcpy(&MONRData->CRC, p, sizeof (MONRData->CRC)); + p += sizeof (MONRData->CRC); + + // TODO: check on CRC + + if (debug == 1) { + LogPrint("MONR:"); + LogPrint("SyncWord = %x", MONRData->Header.SyncWordU16); + LogPrint("TransmitterId = %d", MONRData->Header.TransmitterIdU8); + LogPrint("PackageCounter = %d", MONRData->Header.MessageCounterU8); + LogPrint("AckReq = %d", MONRData->Header.AckReqProtVerU8); + LogPrint("MessageId = %d", MONRData->Header.MessageIdU16); + LogPrint("MessageLength = %d", MONRData->Header.MessageLengthU32); + LogPrint("ValueId = %d", MONRData->MonrStructValueIdU16); + LogPrint("ContentLength = %d", MONRData->MonrStructContentLengthU16); + LogPrint("GPSSOW = %d", MONRData->GPSQmsOfWeekU32); + LogPrint("XPosition = %d", MONRData->XPositionI32); + LogPrint("YPosition = %d", MONRData->YPositionI32); + LogPrint("ZPosition = %d", MONRData->ZPositionI32); + LogPrint("Heading = %d", MONRData->HeadingU16); + LogPrint("LongitudinalSpeed = %d", MONRData->LongitudinalSpeedI16); + LogPrint("LateralSpeed = %d", MONRData->LateralSpeedI16); + LogPrint("LongitudinalAcc = %d", MONRData->LongitudinalAccI16); + LogPrint("LateralAcc = %d", MONRData->LateralAccI16); + LogPrint("DriveDirection = %d", MONRData->DriveDirectionU8); + LogPrint("State = %d", MONRData->StateU8); + LogPrint("ReadyToArm = %d", MONRData->ReadyToArmU8); + LogPrint("ErrorStatus = %d", MONRData->ErrorStatusU8); + } + + return 0; } I32 ObjectControlMONRToASCII(MONRType * MONRData, GeoPosition * OriginPosition, I32 Idn, C8 * Id, - C8 * Timestamp, C8 * XPosition, C8 * YPosition, C8 * ZPosition, - C8 * LongitudinalSpeed, C8 * LateralSpeed, C8 * LongitudinalAcc, C8 * LateralAcc, - C8 * Heading, C8 * DriveDirection, C8 * ObjectState, C8 * ReadyToArm, - C8 * ErrorStatus, C8 debug) { - char Buffer[6]; - long unsigned int MonrValueU64; - unsigned int MonrValueU32; - unsigned short MonrValueU16; - unsigned char MonrValueU8; - double iLlh[3] = { 0, 0, 0 }; - double xyz[3] = { 0, 0, 0 }; - double Llh[3] = { 0, 0, 0 }; - uint64_t ConvertGPStoUTC; - - bzero(Id, SMALL_BUFFER_SIZE_1); - bzero(Timestamp, SMALL_BUFFER_SIZE_0); - bzero(XPosition, SMALL_BUFFER_SIZE_0); - bzero(YPosition, SMALL_BUFFER_SIZE_0); - bzero(ZPosition, SMALL_BUFFER_SIZE_0); - bzero(LongitudinalSpeed, SMALL_BUFFER_SIZE_0); - bzero(LateralSpeed, SMALL_BUFFER_SIZE_0); - bzero(LongitudinalAcc, SMALL_BUFFER_SIZE_0); - bzero(LateralAcc, SMALL_BUFFER_SIZE_0); - bzero(Heading, SMALL_BUFFER_SIZE_0); - bzero(DriveDirection, SMALL_BUFFER_SIZE_1); - bzero(ObjectState, SMALL_BUFFER_SIZE_1); - bzero(ReadyToArm, SMALL_BUFFER_SIZE_1); - - - if (MONRData->Header.MessageIdU16 == COMMAND_MONR_CODE) { - //Index - sprintf(Id, "%" PRIu8, (C8) Idn); - - //Timestamp - MonrValueU64 = 0; - //for(i = 0; i <= 5; i++, j++) MonrValueU64 = *(MonrData+j) | (MonrValueU64 << 8); - ConvertGPStoUTC = sprintf(Timestamp, "%" PRIu32, MONRData->GPSQmsOfWeekU32); - - if (debug && MONRData->GPSQmsOfWeekU32 % 400 == 0) { - LogMessage(LOG_LEVEL_DEBUG, "MONR = %x-%x-%x-%x-%x-%x-%d-%d-%d-%d-%d-%d-%d-%d-%d-%d-%d-%d", - MONRData->Header.MessageIdU16, - MONRData->Header.SyncWordU16, - MONRData->Header.TransmitterIdU8, - MONRData->Header.MessageCounterU8, - MONRData->Header.AckReqProtVerU8, - MONRData->Header.MessageLengthU32, - MONRData->MonrStructValueIdU16, - MONRData->MonrStructContentLengthU16, - MONRData->GPSQmsOfWeekU32, - MONRData->XPositionI32, - MONRData->YPositionI32, - MONRData->ZPositionI32, - MONRData->LongitudinalSpeedI16, - MONRData->HeadingU16, - MONRData->DriveDirectionU8, - MONRData->StateU8, MONRData->ReadyToArmU8, MONRData->ErrorStatusU8); - } - - iLlh[0] = OriginPosition->Latitude; - iLlh[1] = OriginPosition->Longitude; - iLlh[2] = OriginPosition->Altitude; - - xyz[0] = ((dbl) MONRData->XPositionI32) / 1000; - xyz[1] = ((dbl) MONRData->YPositionI32) / 1000; - xyz[2] = ((dbl) MONRData->ZPositionI32) / 1000; - - enuToLlh(iLlh, xyz, Llh); - - //XPosition - //MonrValueU32 = 0; - //for(i = 0; i <= 3; i++, j++) MonrValueU32 = *(MonrData+j) | (MonrValueU32 << 8); - //sprintf(Latitude, "%" PRIi32, (I32)(Llh[0]*1e7)); - sprintf(XPosition, "%" PRIi32, MONRData->XPositionI32); - - //YPosition - //MonrValueU32 = 0; - //for(i = 0; i <= 3; i++, j++) MonrValueU32 = *(MonrData+j) | (MonrValueU32 << 8); - //sprintf(Longitude, "%" PRIi32, (I32)(Llh[1]*1e7)); - sprintf(YPosition, "%" PRIi32, MONRData->YPositionI32); - - //ZPosition - //MonrValueU32 = 0; - //for(i = 0; i <= 3; i++, j++) MonrValueU32 = *(MonrData+j) | (MonrValueU32 << 8); - //sprintf(Altitude, "%" PRIi32, (I32)(Llh[2])); - sprintf(ZPosition, "%" PRIi32, MONRData->ZPositionI32); - - //Speed - //MonrValueU16 = 0; - //for(i = 0; i <= 1; i++, j++) MonrValueU16 = *(MonrData+j) | (MonrValueU16 << 8); - sprintf(LongitudinalSpeed, "%" PRIi16, MONRData->LongitudinalSpeedI16); - - //LatSpeed - //MonrValueU16 = 0; - //for(i = 0; i <= 1; i++, j++) MonrValueU16 = *(MonrData+j) | (MonrValueU16 << 8); - sprintf(LateralSpeed, "%" PRIi16, MONRData->LateralSpeedI16); - - //LongAcc - //MonrValueU16 = 0; - //for(i = 0; i <= 1; i++, j++) MonrValueU16 = *(MonrData+j) | (MonrValueU16 << 8); - sprintf(LongitudinalAcc, "%" PRIi16, MONRData->LongitudinalAccI16); - - //LatAcc - //MonrValueU16 = 0; - //for(i = 0; i <= 1; i++, j++) MonrValueU16 = *(MonrData+j) | (MonrValueU16 << 8); - sprintf(LateralAcc, "%" PRIi16, MONRData->LateralAccI16); - - //Heading - //MonrValueU16 = 0; - //for(i = 0; i <= 1; i++, j++) MonrValueU16 = *(MonrData+j) | (MonrValueU16 << 8); - sprintf(Heading, "%" PRIu16, MONRData->HeadingU16); - - //Driving direction - //MonrValueU8 = (unsigned char)*(MonrData+j); - //printf("D: %d\n", MonrValueU8 ); - - sprintf(DriveDirection, "%" PRIu8, MONRData->DriveDirectionU8); - - //State - //MonrValueU8 = (unsigned char)*(MonrData+j); - sprintf(ObjectState, "%" PRIu8, MONRData->StateU8); - - //ReadyToArmU8 - //MonrValueU8 = (unsigned char)*(MonrData+j); - sprintf(ReadyToArm, "%" PRIu8, MONRData->ReadyToArmU8); - - //ErrorStatusU8 - //MonrValueU8 = (unsigned char)*(MonrData+j); - sprintf(ErrorStatus, "%" PRIu8, MONRData->ErrorStatusU8); - - } - - return 0; + C8 * Timestamp, C8 * XPosition, C8 * YPosition, C8 * ZPosition, + C8 * LongitudinalSpeed, C8 * LateralSpeed, C8 * LongitudinalAcc, C8 * LateralAcc, + C8 * Heading, C8 * DriveDirection, C8 * ObjectState, C8 * ReadyToArm, + C8 * ErrorStatus, C8 debug) { + char Buffer[6]; + long unsigned int MonrValueU64; + unsigned int MonrValueU32; + unsigned short MonrValueU16; + unsigned char MonrValueU8; + double iLlh[3] = { 0, 0, 0 }; + double xyz[3] = { 0, 0, 0 }; + double Llh[3] = { 0, 0, 0 }; + uint64_t ConvertGPStoUTC; + + bzero(Id, SMALL_BUFFER_SIZE_1); + bzero(Timestamp, SMALL_BUFFER_SIZE_0); + bzero(XPosition, SMALL_BUFFER_SIZE_0); + bzero(YPosition, SMALL_BUFFER_SIZE_0); + bzero(ZPosition, SMALL_BUFFER_SIZE_0); + bzero(LongitudinalSpeed, SMALL_BUFFER_SIZE_0); + bzero(LateralSpeed, SMALL_BUFFER_SIZE_0); + bzero(LongitudinalAcc, SMALL_BUFFER_SIZE_0); + bzero(LateralAcc, SMALL_BUFFER_SIZE_0); + bzero(Heading, SMALL_BUFFER_SIZE_0); + bzero(DriveDirection, SMALL_BUFFER_SIZE_1); + bzero(ObjectState, SMALL_BUFFER_SIZE_1); + bzero(ReadyToArm, SMALL_BUFFER_SIZE_1); + + + if (MONRData->Header.MessageIdU16 == COMMAND_MONR_CODE) { + //Index + sprintf(Id, "%" PRIu8, (C8) Idn); + + //Timestamp + MonrValueU64 = 0; + //for(i = 0; i <= 5; i++, j++) MonrValueU64 = *(MonrData+j) | (MonrValueU64 << 8); + ConvertGPStoUTC = sprintf(Timestamp, "%" PRIu32, MONRData->GPSQmsOfWeekU32); + + if (debug && MONRData->GPSQmsOfWeekU32 % 400 == 0) { + LogMessage(LOG_LEVEL_DEBUG, "MONR = %x-%x-%x-%x-%x-%x-%d-%d-%d-%d-%d-%d-%d-%d-%d-%d-%d-%d", + MONRData->Header.MessageIdU16, + MONRData->Header.SyncWordU16, + MONRData->Header.TransmitterIdU8, + MONRData->Header.MessageCounterU8, + MONRData->Header.AckReqProtVerU8, + MONRData->Header.MessageLengthU32, + MONRData->MonrStructValueIdU16, + MONRData->MonrStructContentLengthU16, + MONRData->GPSQmsOfWeekU32, + MONRData->XPositionI32, + MONRData->YPositionI32, + MONRData->ZPositionI32, + MONRData->LongitudinalSpeedI16, + MONRData->HeadingU16, + MONRData->DriveDirectionU8, + MONRData->StateU8, MONRData->ReadyToArmU8, MONRData->ErrorStatusU8); + } + + iLlh[0] = OriginPosition->Latitude; + iLlh[1] = OriginPosition->Longitude; + iLlh[2] = OriginPosition->Altitude; + + xyz[0] = ((dbl) MONRData->XPositionI32) / 1000; + xyz[1] = ((dbl) MONRData->YPositionI32) / 1000; + xyz[2] = ((dbl) MONRData->ZPositionI32) / 1000; + + enuToLlh(iLlh, xyz, Llh); + + //XPosition + //MonrValueU32 = 0; + //for(i = 0; i <= 3; i++, j++) MonrValueU32 = *(MonrData+j) | (MonrValueU32 << 8); + //sprintf(Latitude, "%" PRIi32, (I32)(Llh[0]*1e7)); + sprintf(XPosition, "%" PRIi32, MONRData->XPositionI32); + + //YPosition + //MonrValueU32 = 0; + //for(i = 0; i <= 3; i++, j++) MonrValueU32 = *(MonrData+j) | (MonrValueU32 << 8); + //sprintf(Longitude, "%" PRIi32, (I32)(Llh[1]*1e7)); + sprintf(YPosition, "%" PRIi32, MONRData->YPositionI32); + + //ZPosition + //MonrValueU32 = 0; + //for(i = 0; i <= 3; i++, j++) MonrValueU32 = *(MonrData+j) | (MonrValueU32 << 8); + //sprintf(Altitude, "%" PRIi32, (I32)(Llh[2])); + sprintf(ZPosition, "%" PRIi32, MONRData->ZPositionI32); + + //Speed + //MonrValueU16 = 0; + //for(i = 0; i <= 1; i++, j++) MonrValueU16 = *(MonrData+j) | (MonrValueU16 << 8); + sprintf(LongitudinalSpeed, "%" PRIi16, MONRData->LongitudinalSpeedI16); + + //LatSpeed + //MonrValueU16 = 0; + //for(i = 0; i <= 1; i++, j++) MonrValueU16 = *(MonrData+j) | (MonrValueU16 << 8); + sprintf(LateralSpeed, "%" PRIi16, MONRData->LateralSpeedI16); + + //LongAcc + //MonrValueU16 = 0; + //for(i = 0; i <= 1; i++, j++) MonrValueU16 = *(MonrData+j) | (MonrValueU16 << 8); + sprintf(LongitudinalAcc, "%" PRIi16, MONRData->LongitudinalAccI16); + + //LatAcc + //MonrValueU16 = 0; + //for(i = 0; i <= 1; i++, j++) MonrValueU16 = *(MonrData+j) | (MonrValueU16 << 8); + sprintf(LateralAcc, "%" PRIi16, MONRData->LateralAccI16); + + //Heading + //MonrValueU16 = 0; + //for(i = 0; i <= 1; i++, j++) MonrValueU16 = *(MonrData+j) | (MonrValueU16 << 8); + sprintf(Heading, "%" PRIu16, MONRData->HeadingU16); + + //Driving direction + //MonrValueU8 = (unsigned char)*(MonrData+j); + //printf("D: %d\n", MonrValueU8 ); + + sprintf(DriveDirection, "%" PRIu8, MONRData->DriveDirectionU8); + + //State + //MonrValueU8 = (unsigned char)*(MonrData+j); + sprintf(ObjectState, "%" PRIu8, MONRData->StateU8); + + //ReadyToArmU8 + //MonrValueU8 = (unsigned char)*(MonrData+j); + sprintf(ReadyToArm, "%" PRIu8, MONRData->ReadyToArmU8); + + //ErrorStatusU8 + //MonrValueU8 = (unsigned char)*(MonrData+j); + sprintf(ErrorStatus, "%" PRIu8, MONRData->ErrorStatusU8); + + } + + return 0; } I32 ObjectControlBuildOSEMMessage(C8 * MessageBuffer, OSEMType * OSEMData, TimeType * GPSTime, C8 * Latitude, - C8 * Longitude, C8 * Altitude, U8 debug) { - I32 MessageIndex = 0, i = 0; - dbl Data; - U16 Crc = 0; - C8 *p; - U32 ISODate = 0; - - bzero(MessageBuffer, COMMAND_OSEM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); - - OSEMData->Header.SyncWordU16 = SYNC_WORD; - OSEMData->Header.TransmitterIdU8 = 0; - OSEMData->Header.MessageCounterU8 = 0; - OSEMData->Header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; - OSEMData->Header.MessageIdU16 = COMMAND_OSEM_CODE; - OSEMData->Header.MessageLengthU32 = sizeof (OSEMType) - sizeof (HeaderType) - 4; - OSEMData->LatitudeValueIdU16 = VALUE_ID_LATITUDE; - OSEMData->LatitudeContentLengthU16 = 6; - OSEMData->LatitudeI64 = (I64) ((atof((const char *)Latitude) * 1e10)); - OSEMData->LongitudeValueIdU16 = VALUE_ID_LONGITUDE; - OSEMData->LongitudeContentLengthU16 = 6; - OSEMData->LongitudeI64 = (I64) ((atof((const char *)Longitude) * 1e10)); - OSEMData->AltitudeValueIdU16 = VALUE_ID_ALTITUDE; - OSEMData->AltitudeContentLengthU16 = 4; - OSEMData->AltitudeI32 = (I32) (atof((char *)Altitude) * 1e2); - OSEMData->DateValueIdU16 = VALUE_ID_DATE_ISO8601; - OSEMData->DateContentLengthU16 = 4; - OSEMData->DateU32 = - ((U32) GPSTime->YearU16 * 10000) + ((U32) GPSTime->MonthU8 * 100) + ((U32) GPSTime->DayU8); - OSEMData->GPSWeekValueIdU16 = VALUE_ID_GPS_WEEK; - OSEMData->GPSWeekContentLengthU16 = 2; - OSEMData->GPSWeekU16 = GPSTime->GPSWeekU16; - OSEMData->GPSSOWValueIdU16 = VALUE_ID_GPS_SECOND_OF_WEEK; - OSEMData->GPSSOWContentLengthU16 = 4; - OSEMData->GPSQmsOfWeekU32 = - ((GPSTime->GPSSecondsOfWeekU32 * 1000 + GPSTime->MillisecondU16) << 2) + GPSTime->MicroSecondU16; - OSEMData->MaxWayDeviationValueIdU16 = VALUE_ID_MAX_WAY_DEVIATION; - OSEMData->MaxWayDeviationContentLengthU16 = 2; - OSEMData->MaxWayDeviationU16 = 65535; - OSEMData->MaxLateralDeviationValueIdU16 = VALUE_ID_MAX_LATERAL_DEVIATION; - OSEMData->MaxLateralDeviationContentLengthU16 = 2; - OSEMData->MaxLateralDeviationU16 = 65535; - OSEMData->MinPosAccuracyContentLengthU16 = 2; - OSEMData->MinPosAccuracyValueIdU16 = VALUE_ID_MIN_POS_ACCURACY; - OSEMData->MinPosAccuracyU16 = 65535; - - if (!GPSTime->isGPSenabled) { - OSEMData->DateU32 = UtilgetIntDateFromMS(UtilgetCurrentUTCtimeMS()); - UtilgetCurrentGPStime(&OSEMData->GPSWeekU16, &OSEMData->GPSQmsOfWeekU32); - } - - p = (C8 *) OSEMData; - for (i = 0; i < 21; i++) - *(MessageBuffer + i) = *p++; - *p++; - *p++; - for (; i < 31; i++) - *(MessageBuffer + i) = *p++; - *p++; - *p++; - for (; i < sizeof (OSEMType) - 4; i++) - *(MessageBuffer + i) = *p++; - - Crc = crc_16((const C8 *)MessageBuffer, sizeof (OSEMType) - 4); - Crc = 0; - *(MessageBuffer + i++) = (U8) (Crc); - *(MessageBuffer + i++) = (U8) (Crc >> 8); - - MessageIndex = i; - - if (debug) { - // TODO: Change to log printout when byte thingy has been implemented - printf("OSEM total length = %d bytes (header+message+footer)\n", - (int)(COMMAND_OSEM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH)); - printf("----HEADER----\n"); - for (i = 0; i < sizeof (HeaderType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----MESSAGE----\n"); - for (; i < sizeof (OSEMType) - 4; i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----FOOTER----\n"); - for (; i < MessageIndex; i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n"); - printf("Latitude = %ld\n", OSEMData->LatitudeI64); - printf("Longitude = %ld\n", OSEMData->LongitudeI64); - printf("ISODate = %d\n", OSEMData->DateU32); - } - return MessageIndex; //Total number of bytes + C8 * Longitude, C8 * Altitude, U8 debug) { + I32 MessageIndex = 0, i = 0; + dbl Data; + U16 Crc = 0; + C8 *p; + U32 ISODate = 0; + + bzero(MessageBuffer, COMMAND_OSEM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); + + OSEMData->Header.SyncWordU16 = SYNC_WORD; + OSEMData->Header.TransmitterIdU8 = 0; + OSEMData->Header.MessageCounterU8 = 0; + OSEMData->Header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; + OSEMData->Header.MessageIdU16 = COMMAND_OSEM_CODE; + OSEMData->Header.MessageLengthU32 = sizeof (OSEMType) - sizeof (HeaderType) - 4; + OSEMData->LatitudeValueIdU16 = VALUE_ID_LATITUDE; + OSEMData->LatitudeContentLengthU16 = 6; + OSEMData->LatitudeI64 = (I64) ((atof((const char *)Latitude) * 1e10)); + OSEMData->LongitudeValueIdU16 = VALUE_ID_LONGITUDE; + OSEMData->LongitudeContentLengthU16 = 6; + OSEMData->LongitudeI64 = (I64) ((atof((const char *)Longitude) * 1e10)); + OSEMData->AltitudeValueIdU16 = VALUE_ID_ALTITUDE; + OSEMData->AltitudeContentLengthU16 = 4; + OSEMData->AltitudeI32 = (I32) (atof((char *)Altitude) * 1e2); + OSEMData->DateValueIdU16 = VALUE_ID_DATE_ISO8601; + OSEMData->DateContentLengthU16 = 4; + OSEMData->DateU32 = + ((U32) GPSTime->YearU16 * 10000) + ((U32) GPSTime->MonthU8 * 100) + ((U32) GPSTime->DayU8); + OSEMData->GPSWeekValueIdU16 = VALUE_ID_GPS_WEEK; + OSEMData->GPSWeekContentLengthU16 = 2; + OSEMData->GPSWeekU16 = GPSTime->GPSWeekU16; + OSEMData->GPSSOWValueIdU16 = VALUE_ID_GPS_SECOND_OF_WEEK; + OSEMData->GPSSOWContentLengthU16 = 4; + OSEMData->GPSQmsOfWeekU32 = + ((GPSTime->GPSSecondsOfWeekU32 * 1000 + GPSTime->MillisecondU16) << 2) + GPSTime->MicroSecondU16; + OSEMData->MaxWayDeviationValueIdU16 = VALUE_ID_MAX_WAY_DEVIATION; + OSEMData->MaxWayDeviationContentLengthU16 = 2; + OSEMData->MaxWayDeviationU16 = 65535; + OSEMData->MaxLateralDeviationValueIdU16 = VALUE_ID_MAX_LATERAL_DEVIATION; + OSEMData->MaxLateralDeviationContentLengthU16 = 2; + OSEMData->MaxLateralDeviationU16 = 65535; + OSEMData->MinPosAccuracyContentLengthU16 = 2; + OSEMData->MinPosAccuracyValueIdU16 = VALUE_ID_MIN_POS_ACCURACY; + OSEMData->MinPosAccuracyU16 = 65535; + + if (!GPSTime->isGPSenabled) { + OSEMData->DateU32 = UtilgetIntDateFromMS(UtilgetCurrentUTCtimeMS()); + UtilgetCurrentGPStime(&OSEMData->GPSWeekU16, &OSEMData->GPSQmsOfWeekU32); + } + + p = (C8 *) OSEMData; + for (i = 0; i < 21; i++) + *(MessageBuffer + i) = *p++; + *p++; + *p++; + for (; i < 31; i++) + *(MessageBuffer + i) = *p++; + *p++; + *p++; + for (; i < sizeof (OSEMType) - 4; i++) + *(MessageBuffer + i) = *p++; + + Crc = crc_16((const C8 *)MessageBuffer, sizeof (OSEMType) - 4); + Crc = 0; + *(MessageBuffer + i++) = (U8) (Crc); + *(MessageBuffer + i++) = (U8) (Crc >> 8); + + MessageIndex = i; + + if (debug) { + // TODO: Change to log printout when byte thingy has been implemented + printf("OSEM total length = %d bytes (header+message+footer)\n", + (int)(COMMAND_OSEM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH)); + printf("----HEADER----\n"); + for (i = 0; i < sizeof (HeaderType); i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n----MESSAGE----\n"); + for (; i < sizeof (OSEMType) - 4; i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n----FOOTER----\n"); + for (; i < MessageIndex; i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n"); + printf("Latitude = %ld\n", OSEMData->LatitudeI64); + printf("Longitude = %ld\n", OSEMData->LongitudeI64); + printf("ISODate = %d\n", OSEMData->DateU32); + } + return MessageIndex; //Total number of bytes } int ObjectControlOSEMtoASCII(OSEMType * OSEMData, char *GPSWeek, char *GPSLatitude, char *GPSLongitude, - char *GPSAltitude) { - // what do i want? in my mq? gps week, origin in lat and long coordinates - bzero(GPSWeek, SMALL_BUFFER_SIZE_0); - bzero(GPSLatitude, SMALL_BUFFER_SIZE_0); - bzero(GPSLongitude, SMALL_BUFFER_SIZE_0); - bzero(GPSAltitude, SMALL_BUFFER_SIZE_0); + char *GPSAltitude) { + // what do i want? in my mq? gps week, origin in lat and long coordinates + bzero(GPSWeek, SMALL_BUFFER_SIZE_0); + bzero(GPSLatitude, SMALL_BUFFER_SIZE_0); + bzero(GPSLongitude, SMALL_BUFFER_SIZE_0); + bzero(GPSAltitude, SMALL_BUFFER_SIZE_0); - if (OSEMData->Header.MessageIdU16 == COMMAND_OSEM_CODE) { - sprintf(GPSWeek, "%" PRIu16, OSEMData->GPSWeekU16); + if (OSEMData->Header.MessageIdU16 == COMMAND_OSEM_CODE) { + sprintf(GPSWeek, "%" PRIu16, OSEMData->GPSWeekU16); - sprintf(GPSLatitude, "%" PRIi64, OSEMData->LatitudeI64); + sprintf(GPSLatitude, "%" PRIi64, OSEMData->LatitudeI64); - sprintf(GPSLongitude, "%" PRIi64, OSEMData->LongitudeI64); + sprintf(GPSLongitude, "%" PRIi64, OSEMData->LongitudeI64); - sprintf(GPSAltitude, "%" PRIi32, OSEMData->AltitudeI32); - } - return 0; + sprintf(GPSAltitude, "%" PRIi32, OSEMData->AltitudeI32); + } + return 0; } int ObjectControlBuildSTRTMessage(C8 * MessageBuffer, STRTType * STRTData, TimeType * GPSTime, - U32 ScenarioStartTime, U32 DelayStart, U32 * OutgoingStartTime, U8 debug) { - I32 MessageIndex = 0, i = 0; - U16 Crc = 0; - C8 *p; - - bzero(MessageBuffer, COMMAND_STRT_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); - - STRTData->Header.SyncWordU16 = SYNC_WORD; - STRTData->Header.TransmitterIdU8 = 0; - STRTData->Header.MessageCounterU8 = 0; - STRTData->Header.AckReqProtVerU8 = 0; - STRTData->Header.MessageIdU16 = COMMAND_STRT_CODE; - STRTData->Header.MessageLengthU32 = sizeof (STRTType) - sizeof (HeaderType); - STRTData->StartTimeValueIdU16 = VALUE_ID_GPS_SECOND_OF_WEEK; - STRTData->StartTimeContentLengthU16 = sizeof (STRTData->StartTimeU32); - STRTData->StartTimeU32 = - ((GPSTime->GPSSecondsOfWeekU32 * 1000 + (U32) TimeControlGetMillisecond(GPSTime) + - ScenarioStartTime) << 2) + GPSTime->MicroSecondU16; - STRTData->GPSWeekValueIdU16 = VALUE_ID_GPS_WEEK; - STRTData->GPSWeekContentLengthU16 = sizeof (STRTData->GPSWeekU16); - STRTData->GPSWeekU16 = GPSTime->GPSWeekU16; - // STRTData->DelayStartValueIdU16 = VALUE_ID_RELATIVE_TIME; - // STRTData->DelayStartContentLengthU16 = 4; - // STRTData->DelayStartU32 = DelayStart; - - *OutgoingStartTime = (STRTData->StartTimeU32) >> 2; - - if (!GPSTime->isGPSenabled) { - UtilgetCurrentGPStime(NULL, &STRTData->StartTimeU32); - } - - p = (char *)STRTData; - for (i = 0; i < sizeof (STRTType); i++) - *(MessageBuffer + i) = *p++; - Crc = crc_16((const unsigned char *)MessageBuffer, sizeof (STRTType)); - Crc = 0; - *(MessageBuffer + i++) = (U8) (Crc); - *(MessageBuffer + i++) = (U8) (Crc >> 8); - MessageIndex = i; - - if (debug) { - // TODO: Change to log printout when byte thingy has been implemented - printf("STRT total length = %d bytes (header+message+footer)\n", - (int)(COMMAND_STRT_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH)); - printf("----HEADER----\n"); - for (i = 0; i < sizeof (HeaderType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----MESSAGE----\n"); - for (; i < sizeof (STRTType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----FOOTER----\n"); - for (; i < MessageIndex; i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n"); - } - - return MessageIndex; //Total number of bytes + U32 ScenarioStartTime, U32 DelayStart, U32 * OutgoingStartTime, U8 debug) { + I32 MessageIndex = 0, i = 0; + U16 Crc = 0; + C8 *p; + + bzero(MessageBuffer, COMMAND_STRT_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); + + STRTData->Header.SyncWordU16 = SYNC_WORD; + STRTData->Header.TransmitterIdU8 = 0; + STRTData->Header.MessageCounterU8 = 0; + STRTData->Header.AckReqProtVerU8 = 0; + STRTData->Header.MessageIdU16 = COMMAND_STRT_CODE; + STRTData->Header.MessageLengthU32 = sizeof (STRTType) - sizeof (HeaderType); + STRTData->StartTimeValueIdU16 = VALUE_ID_GPS_SECOND_OF_WEEK; + STRTData->StartTimeContentLengthU16 = sizeof (STRTData->StartTimeU32); + STRTData->StartTimeU32 = + ((GPSTime->GPSSecondsOfWeekU32 * 1000 + (U32) TimeControlGetMillisecond(GPSTime) + + ScenarioStartTime) << 2) + GPSTime->MicroSecondU16; + STRTData->GPSWeekValueIdU16 = VALUE_ID_GPS_WEEK; + STRTData->GPSWeekContentLengthU16 = sizeof (STRTData->GPSWeekU16); + STRTData->GPSWeekU16 = GPSTime->GPSWeekU16; + // STRTData->DelayStartValueIdU16 = VALUE_ID_RELATIVE_TIME; + // STRTData->DelayStartContentLengthU16 = 4; + // STRTData->DelayStartU32 = DelayStart; + + *OutgoingStartTime = (STRTData->StartTimeU32) >> 2; + + if (!GPSTime->isGPSenabled) { + UtilgetCurrentGPStime(NULL, &STRTData->StartTimeU32); + } + + p = (char *)STRTData; + for (i = 0; i < sizeof (STRTType); i++) + *(MessageBuffer + i) = *p++; + Crc = crc_16((const unsigned char *)MessageBuffer, sizeof (STRTType)); + Crc = 0; + *(MessageBuffer + i++) = (U8) (Crc); + *(MessageBuffer + i++) = (U8) (Crc >> 8); + MessageIndex = i; + + if (debug) { + // TODO: Change to log printout when byte thingy has been implemented + printf("STRT total length = %d bytes (header+message+footer)\n", + (int)(COMMAND_STRT_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH)); + printf("----HEADER----\n"); + for (i = 0; i < sizeof (HeaderType); i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n----MESSAGE----\n"); + for (; i < sizeof (STRTType); i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n----FOOTER----\n"); + for (; i < MessageIndex; i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n"); + } + + return MessageIndex; //Total number of bytes } I32 ObjectControlBuildOSTMMessage(C8 * MessageBuffer, OSTMType * OSTMData, C8 CommandOption, U8 debug) { - I32 MessageIndex = 0, i; - U16 Crc = 0; - C8 *p; - - bzero(MessageBuffer, COMMAND_OSTM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); - - OSTMData->Header.SyncWordU16 = SYNC_WORD; - OSTMData->Header.TransmitterIdU8 = 0; - OSTMData->Header.MessageCounterU8 = 0; - OSTMData->Header.AckReqProtVerU8 = 0; - OSTMData->Header.MessageIdU16 = COMMAND_OSTM_CODE; - OSTMData->Header.MessageLengthU32 = sizeof (OSTMType) - sizeof (HeaderType); - OSTMData->StateValueIdU16 = VALUE_ID_STATE_CHANGE_REQUEST; - OSTMData->StateContentLengthU16 = sizeof (OSTMData->StateU8); - OSTMData->StateU8 = (U8) CommandOption; - - p = (C8 *) OSTMData; - for (i = 0; i < sizeof (OSTMType); i++) - *(MessageBuffer + i) = *p++; - Crc = crc_16((const C8 *)MessageBuffer, sizeof (OSTMType)); - Crc = 0; - *(MessageBuffer + i++) = (U8) (Crc >> 8); - *(MessageBuffer + i++) = (U8) (Crc); - MessageIndex = i; - - if (debug) { - // TODO: Change to log printout when byte thingy has been implemented - printf("OSTM total length = %d bytes (header+message+footer)\n", - (int)(COMMAND_OSTM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH)); - printf("----HEADER----\n"); - for (i = 0; i < sizeof (HeaderType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----MESSAGE----\n"); - for (; i < sizeof (OSTMType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----FOOTER----\n"); - for (; i < MessageIndex; i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n"); - } - - return MessageIndex; //Total number of bytes + I32 MessageIndex = 0, i; + U16 Crc = 0; + C8 *p; + + bzero(MessageBuffer, COMMAND_OSTM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); + + OSTMData->Header.SyncWordU16 = SYNC_WORD; + OSTMData->Header.TransmitterIdU8 = 0; + OSTMData->Header.MessageCounterU8 = 0; + OSTMData->Header.AckReqProtVerU8 = 0; + OSTMData->Header.MessageIdU16 = COMMAND_OSTM_CODE; + OSTMData->Header.MessageLengthU32 = sizeof (OSTMType) - sizeof (HeaderType); + OSTMData->StateValueIdU16 = VALUE_ID_STATE_CHANGE_REQUEST; + OSTMData->StateContentLengthU16 = sizeof (OSTMData->StateU8); + OSTMData->StateU8 = (U8) CommandOption; + + p = (C8 *) OSTMData; + for (i = 0; i < sizeof (OSTMType); i++) + *(MessageBuffer + i) = *p++; + Crc = crc_16((const C8 *)MessageBuffer, sizeof (OSTMType)); + Crc = 0; + *(MessageBuffer + i++) = (U8) (Crc >> 8); + *(MessageBuffer + i++) = (U8) (Crc); + MessageIndex = i; + + if (debug) { + // TODO: Change to log printout when byte thingy has been implemented + printf("OSTM total length = %d bytes (header+message+footer)\n", + (int)(COMMAND_OSTM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH)); + printf("----HEADER----\n"); + for (i = 0; i < sizeof (HeaderType); i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n----MESSAGE----\n"); + for (; i < sizeof (OSTMType); i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n----FOOTER----\n"); + for (; i < MessageIndex; i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n"); + } + + return MessageIndex; //Total number of bytes } I32 ObjectControlBuildHEABMessage(C8 * MessageBuffer, HEABType * HEABData, TimeType * GPSTime, U8 CCStatus, - U8 debug) { - I32 MessageIndex = 0, i; - U16 Crc = 0; - C8 *p; - - bzero(MessageBuffer, COMMAND_HEAB_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); - - HEABData->Header.SyncWordU16 = SYNC_WORD; - HEABData->Header.TransmitterIdU8 = 0; - HEABData->Header.MessageCounterU8 = 0; - HEABData->Header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; - HEABData->Header.MessageIdU16 = COMMAND_HEAB_CODE; - HEABData->Header.MessageLengthU32 = sizeof (HEABType) - sizeof (HeaderType); - HEABData->HeabStructValueIdU16 = VALUE_ID_HEAB_STRUCT; - HEABData->HeabStructContentLengthU16 = sizeof (HEABType) - sizeof (HeaderType) - - sizeof (HEABData->HeabStructValueIdU16) - sizeof (HEABData->HeabStructContentLengthU16); - HEABData->GPSQmsOfWeekU32 = - ((GPSTime->GPSSecondsOfWeekU32 * 1000 + (U32) TimeControlGetMillisecond(GPSTime)) << 2) + - GPSTime->MicroSecondU16; - HEABData->CCStatusU8 = CCStatus; - - if (!GPSTime->isGPSenabled) { - UtilgetCurrentGPStime(NULL, &HEABData->GPSQmsOfWeekU32); - } - - p = (C8 *) HEABData; - for (i = 0; i < sizeof (HEABType); i++) - *(MessageBuffer + i) = *p++; - Crc = crc_16((const C8 *)MessageBuffer, sizeof (HEABType)); - Crc = 0; - *(MessageBuffer + i++) = (U8) (Crc); - *(MessageBuffer + i++) = (U8) (Crc >> 8); - MessageIndex = i; - - if (debug) { - // TODO: Change to log printout when byte thingy has been implemented - printf("HEAB total length = %d bytes (header+message+footer)\n", - (int)(COMMAND_HEAB_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH)); - printf("----HEADER----\n"); - for (i = 0; i < sizeof (HeaderType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----MESSAGE----\n"); - for (; i < sizeof (HEABType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----FOOTER----\n"); - for (; i < MessageIndex; i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n"); - } - - return MessageIndex; //Total number of bytes + U8 debug) { + I32 MessageIndex = 0, i; + U16 Crc = 0; + C8 *p; + + bzero(MessageBuffer, COMMAND_HEAB_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); + + HEABData->Header.SyncWordU16 = SYNC_WORD; + HEABData->Header.TransmitterIdU8 = 0; + HEABData->Header.MessageCounterU8 = 0; + HEABData->Header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; + HEABData->Header.MessageIdU16 = COMMAND_HEAB_CODE; + HEABData->Header.MessageLengthU32 = sizeof (HEABType) - sizeof (HeaderType); + HEABData->HeabStructValueIdU16 = VALUE_ID_HEAB_STRUCT; + HEABData->HeabStructContentLengthU16 = sizeof (HEABType) - sizeof (HeaderType) + - sizeof (HEABData->HeabStructValueIdU16) - sizeof (HEABData->HeabStructContentLengthU16); + HEABData->GPSQmsOfWeekU32 = + ((GPSTime->GPSSecondsOfWeekU32 * 1000 + (U32) TimeControlGetMillisecond(GPSTime)) << 2) + + GPSTime->MicroSecondU16; + HEABData->CCStatusU8 = CCStatus; + + if (!GPSTime->isGPSenabled) { + UtilgetCurrentGPStime(NULL, &HEABData->GPSQmsOfWeekU32); + } + + p = (C8 *) HEABData; + for (i = 0; i < sizeof (HEABType); i++) + *(MessageBuffer + i) = *p++; + Crc = crc_16((const C8 *)MessageBuffer, sizeof (HEABType)); + Crc = 0; + *(MessageBuffer + i++) = (U8) (Crc); + *(MessageBuffer + i++) = (U8) (Crc >> 8); + MessageIndex = i; + + if (debug) { + // TODO: Change to log printout when byte thingy has been implemented + printf("HEAB total length = %d bytes (header+message+footer)\n", + (int)(COMMAND_HEAB_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH)); + printf("----HEADER----\n"); + for (i = 0; i < sizeof (HeaderType); i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n----MESSAGE----\n"); + for (; i < sizeof (HEABType); i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n----FOOTER----\n"); + for (; i < MessageIndex; i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n"); + } + + return MessageIndex; //Total number of bytes } int ObjectControlBuildLLCMMessage(char *MessageBuffer, unsigned short Speed, unsigned short Curvature, - unsigned char Mode, char debug) { - int MessageIndex = 0; + unsigned char Mode, char debug) { + int MessageIndex = 0; - bzero(MessageBuffer, COMMAND_LLCM_MESSAGE_LENGTH + COMMAND_MESSAGE_HEADER_LENGTH); + bzero(MessageBuffer, COMMAND_LLCM_MESSAGE_LENGTH + COMMAND_MESSAGE_HEADER_LENGTH); - UtilAddOneByteMessageData(MessageBuffer, COMMAND_CODE_INDEX, COMMAND_LLCM_CODE); + UtilAddOneByteMessageData(MessageBuffer, COMMAND_CODE_INDEX, COMMAND_LLCM_CODE); - MessageIndex = - UtilAddTwoBytesMessageData(MessageBuffer, MessageIndex + COMMAND_MESSAGE_HEADER_LENGTH, Speed); + MessageIndex = + UtilAddTwoBytesMessageData(MessageBuffer, MessageIndex + COMMAND_MESSAGE_HEADER_LENGTH, Speed); - MessageIndex = UtilAddTwoBytesMessageData(MessageBuffer, MessageIndex, Curvature); + MessageIndex = UtilAddTwoBytesMessageData(MessageBuffer, MessageIndex, Curvature); - MessageIndex = UtilAddOneByteMessageData(MessageBuffer, MessageIndex, Mode); + MessageIndex = UtilAddOneByteMessageData(MessageBuffer, MessageIndex, Mode); - UtilAddFourBytesMessageData(MessageBuffer, COMMAND_MESSAGE_LENGTH_INDEX, - (unsigned int)MessageIndex - COMMAND_MESSAGE_HEADER_LENGTH); + UtilAddFourBytesMessageData(MessageBuffer, COMMAND_MESSAGE_LENGTH_INDEX, + (unsigned int)MessageIndex - COMMAND_MESSAGE_HEADER_LENGTH); - if (debug) { - int i = 0; + if (debug) { + int i = 0; - LogMessage(LOG_LEVEL_DEBUG, "LLCM:"); - for (i = 0; i < MessageIndex; i++) - LogMessage(LOG_LEVEL_DEBUG, "[%d]= %x", i, (unsigned char)MessageBuffer[i]); - } + LogMessage(LOG_LEVEL_DEBUG, "LLCM:"); + for (i = 0; i < MessageIndex; i++) + LogMessage(LOG_LEVEL_DEBUG, "[%d]= %x", i, (unsigned char)MessageBuffer[i]); + } - return MessageIndex; //Total number of bytes = COMMAND_MESSAGE_HEADER_LENGTH + message data count + return MessageIndex; //Total number of bytes = COMMAND_MESSAGE_HEADER_LENGTH + message data count } I32 ObjectControlBuildSYPMMessage(C8 * MessageBuffer, SYPMType * SYPMData, U32 SyncPoint, U32 StopTime, - U8 debug) { - - I32 MessageIndex = 0, i; - U16 Crc = 0; - C8 *p; - - bzero(MessageBuffer, COMMAND_SYPM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); - - SYPMData->Header.SyncWordU16 = SYNC_WORD; - SYPMData->Header.TransmitterIdU8 = 0; - SYPMData->Header.MessageCounterU8 = 0; - SYPMData->Header.AckReqProtVerU8 = 0; - SYPMData->Header.MessageIdU16 = COMMAND_SYPM_CODE; - SYPMData->Header.MessageLengthU32 = sizeof (SYPMType) - sizeof (HeaderType); - SYPMData->SyncPointTimeValueIdU16 = 1; - SYPMData->SyncPointTimeContentLengthU16 = 4; - SYPMData->SyncPointTimeU32 = SyncPoint; - SYPMData->FreezeTimeValueIdU16 = 2; - SYPMData->FreezeTimeContentLengthU16 = 4; - SYPMData->FreezeTimeU32 = StopTime; - - - p = (C8 *) SYPMData; - for (i = 0; i < sizeof (SYPMType); i++) - *(MessageBuffer + i) = *p++; - Crc = crc_16((const C8 *)MessageBuffer, sizeof (SYPMType)); - Crc = 0; - *(MessageBuffer + i++) = (U8) (Crc >> 8); - *(MessageBuffer + i++) = (U8) (Crc); - MessageIndex = i; - - if (debug) { - // TODO: Change to log printout when byte thingy has been implemented - printf("SYPM total length = %d bytes (header+message+footer)\n", - (int)(COMMAND_SYPM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH)); - printf("----HEADER----\n"); - for (i = 0; i < sizeof (HeaderType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----MESSAGE----\n"); - for (; i < sizeof (SYPMType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----FOOTER----\n"); - for (; i < MessageIndex; i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n"); - } - - return MessageIndex; //Total number of bytes + U8 debug) { + + I32 MessageIndex = 0, i; + U16 Crc = 0; + C8 *p; + + bzero(MessageBuffer, COMMAND_SYPM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); + + SYPMData->Header.SyncWordU16 = SYNC_WORD; + SYPMData->Header.TransmitterIdU8 = 0; + SYPMData->Header.MessageCounterU8 = 0; + SYPMData->Header.AckReqProtVerU8 = 0; + SYPMData->Header.MessageIdU16 = COMMAND_SYPM_CODE; + SYPMData->Header.MessageLengthU32 = sizeof (SYPMType) - sizeof (HeaderType); + SYPMData->SyncPointTimeValueIdU16 = 1; + SYPMData->SyncPointTimeContentLengthU16 = 4; + SYPMData->SyncPointTimeU32 = SyncPoint; + SYPMData->FreezeTimeValueIdU16 = 2; + SYPMData->FreezeTimeContentLengthU16 = 4; + SYPMData->FreezeTimeU32 = StopTime; + + + p = (C8 *) SYPMData; + for (i = 0; i < sizeof (SYPMType); i++) + *(MessageBuffer + i) = *p++; + Crc = crc_16((const C8 *)MessageBuffer, sizeof (SYPMType)); + Crc = 0; + *(MessageBuffer + i++) = (U8) (Crc >> 8); + *(MessageBuffer + i++) = (U8) (Crc); + MessageIndex = i; + + if (debug) { + // TODO: Change to log printout when byte thingy has been implemented + printf("SYPM total length = %d bytes (header+message+footer)\n", + (int)(COMMAND_SYPM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH)); + printf("----HEADER----\n"); + for (i = 0; i < sizeof (HeaderType); i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n----MESSAGE----\n"); + for (; i < sizeof (SYPMType); i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n----FOOTER----\n"); + for (; i < MessageIndex; i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n"); + } + + return MessageIndex; //Total number of bytes } I32 ObjectControlBuildMTSPMessage(C8 * MessageBuffer, MTSPType * MTSPData, U32 SyncTimestamp, U8 debug) { - I32 MessageIndex = 0, i; - U16 Crc = 0; - C8 *p; - - bzero(MessageBuffer, COMMAND_MTSP_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); - - MTSPData->Header.SyncWordU16 = SYNC_WORD; - MTSPData->Header.TransmitterIdU8 = 0; - MTSPData->Header.MessageCounterU8 = 0; - MTSPData->Header.AckReqProtVerU8 = 0; - MTSPData->Header.MessageIdU16 = COMMAND_MTSP_CODE; - MTSPData->Header.MessageLengthU32 = sizeof (MTSPType) - sizeof (HeaderType); - MTSPData->EstSyncPointTimeValueIdU16 = 1; - MTSPData->EstSyncPointTimeContentLengthU16 = 4; - MTSPData->EstSyncPointTimeU32 = SyncTimestamp; - - - p = (C8 *) MTSPData; - for (i = 0; i < sizeof (MTSPType); i++) - *(MessageBuffer + i) = *p++; - Crc = crc_16((const C8 *)MessageBuffer, sizeof (MTSPType)); - Crc = 0; - *(MessageBuffer + i++) = (U8) (Crc >> 8); - *(MessageBuffer + i++) = (U8) (Crc); - MessageIndex = i; - - if (debug) { - // TODO: Change to log printout when byte thingy has been implemented - printf("MTSPData total length = %d bytes (header+message+footer)\n", - (int)(COMMAND_MTSP_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH)); - printf("----HEADER----\n"); - for (i = 0; i < sizeof (HeaderType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----MESSAGE----\n"); - for (; i < sizeof (MTSPType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----FOOTER----\n"); - for (; i < MessageIndex; i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n"); - } - - return MessageIndex; //Total number of bytes + I32 MessageIndex = 0, i; + U16 Crc = 0; + C8 *p; + + bzero(MessageBuffer, COMMAND_MTSP_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); + + MTSPData->Header.SyncWordU16 = SYNC_WORD; + MTSPData->Header.TransmitterIdU8 = 0; + MTSPData->Header.MessageCounterU8 = 0; + MTSPData->Header.AckReqProtVerU8 = 0; + MTSPData->Header.MessageIdU16 = COMMAND_MTSP_CODE; + MTSPData->Header.MessageLengthU32 = sizeof (MTSPType) - sizeof (HeaderType); + MTSPData->EstSyncPointTimeValueIdU16 = 1; + MTSPData->EstSyncPointTimeContentLengthU16 = 4; + MTSPData->EstSyncPointTimeU32 = SyncTimestamp; + + + p = (C8 *) MTSPData; + for (i = 0; i < sizeof (MTSPType); i++) + *(MessageBuffer + i) = *p++; + Crc = crc_16((const C8 *)MessageBuffer, sizeof (MTSPType)); + Crc = 0; + *(MessageBuffer + i++) = (U8) (Crc >> 8); + *(MessageBuffer + i++) = (U8) (Crc); + MessageIndex = i; + + if (debug) { + // TODO: Change to log printout when byte thingy has been implemented + printf("MTSPData total length = %d bytes (header+message+footer)\n", + (int)(COMMAND_MTSP_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH)); + printf("----HEADER----\n"); + for (i = 0; i < sizeof (HeaderType); i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n----MESSAGE----\n"); + for (; i < sizeof (MTSPType); i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n----FOOTER----\n"); + for (; i < MessageIndex; i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n"); + } + + return MessageIndex; //Total number of bytes } I32 ObjectControlBuildTRAJMessageHeader(C8 * MessageBuffer, I32 * RowCount, HeaderType * HeaderData, - TRAJInfoType * TRAJInfoData, C8 * TrajFileHeader, U8 debug) { - I32 MessageIndex = 0, i, j; - U16 Crc = 0; - C8 *p; - C8 *token; - - - if (strlen(TrajFileHeader) >= 1) { - j = 0; - token = strtok(TrajFileHeader, ";"); - while (token != NULL) { - if (j == 1) { - TRAJInfoData->TrajectoryIDValueIdU16 = VALUE_ID_TRAJECTORY_ID; - TRAJInfoData->TrajectoryIDContentLengthU16 = 2; - TRAJInfoData->TrajectoryIDU16 = atoi(token); - } - else if (j == 2) { - TRAJInfoData->TrajectoryNameValueIdU16 = VALUE_ID_TRAJECTORY_NAME; - TRAJInfoData->TrajectoryNameContentLengthU16 = 64; - bzero(TRAJInfoData->TrajectoryNameC8, 64); - strncpy(TRAJInfoData->TrajectoryNameC8, token, strlen(token)); - } - else if (j == 3) { - TRAJInfoData->TrajectoryVersionValueIdU16 = VALUE_ID_TRAJECTORY_VERSION; - TRAJInfoData->TrajectoryVersionContentLengthU16 = 2; - TRAJInfoData->TrajectoryVersionU16 = atoi(token); - } - else if (j == 4) { - *RowCount = atoi(token); - } - - j++; - token = strtok(NULL, ";"); - } - } - - TRAJInfoData->IpAddressValueIdU16 = 0xA000; - TRAJInfoData->IpAddressContentLengthU16 = 4; - TRAJInfoData->IpAddressU32 = 0; - - bzero(MessageBuffer, COMMAND_MESSAGE_HEADER_LENGTH + COMMAND_TRAJ_INFO_ROW_MESSAGE_LENGTH); - - HeaderData->SyncWordU16 = SYNC_WORD; - HeaderData->TransmitterIdU8 = 0; - HeaderData->MessageCounterU8 = 0; - HeaderData->AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; - HeaderData->MessageIdU16 = COMMAND_DOTM_CODE; - HeaderData->MessageLengthU32 = - *RowCount * COMMAND_DOTM_ROW_MESSAGE_LENGTH + COMMAND_TRAJ_INFO_ROW_MESSAGE_LENGTH; - - p = (C8 *) HeaderData; - for (i = 0; i < COMMAND_MESSAGE_HEADER_LENGTH; i++) - *(MessageBuffer + i) = *p++; - - p = (C8 *) TRAJInfoData; - for (; i < COMMAND_MESSAGE_HEADER_LENGTH + COMMAND_TRAJ_INFO_ROW_MESSAGE_LENGTH; i++) - *(MessageBuffer + i) = *p++; - - MessageIndex = i; - - - if (debug) { - // TODO: Change to log printout when byte thingy has been implemented - printf("Header + TRAJInfo total length = %d bytes\n", - (int)(COMMAND_MESSAGE_HEADER_LENGTH + COMMAND_TRAJ_INFO_ROW_MESSAGE_LENGTH)); - printf("----HEADER + TRAJInfo----\n"); - for (i = 0; i < sizeof (HeaderType) + sizeof (TRAJInfoType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n"); - printf("DOTM message total length = %d bytes.\n", (int)HeaderData->MessageLengthU32); - printf("Traj file header = %s\n", TrajFileHeader); - printf("TrajectoryID = %d\n", TRAJInfoData->TrajectoryIDU16); - printf("TrajectoryName = %s\n", TRAJInfoData->TrajectoryNameC8); - printf("TrajectoryVersion = %d\n", TRAJInfoData->TrajectoryVersionU16); - printf("RowCount = %d\n", *RowCount); - printf("IpAddress = %d\n", TRAJInfoData->IpAddressU32); - - printf("\n----MESSAGE----\n"); - } - - return MessageIndex; //Total number of bytes = COMMAND_MESSAGE_HEADER_LENGTH + TRAJInfoType * TRAJInfoData, C8 * TrajFileHeader, U8 debug) { + I32 MessageIndex = 0, i, j; + U16 Crc = 0; + C8 *p; + C8 *token; + + + if (strlen(TrajFileHeader) >= 1) { + j = 0; + token = strtok(TrajFileHeader, ";"); + while (token != NULL) { + if (j == 1) { + TRAJInfoData->TrajectoryIDValueIdU16 = VALUE_ID_TRAJECTORY_ID; + TRAJInfoData->TrajectoryIDContentLengthU16 = 2; + TRAJInfoData->TrajectoryIDU16 = atoi(token); + } + else if (j == 2) { + TRAJInfoData->TrajectoryNameValueIdU16 = VALUE_ID_TRAJECTORY_NAME; + TRAJInfoData->TrajectoryNameContentLengthU16 = 64; + bzero(TRAJInfoData->TrajectoryNameC8, 64); + strncpy(TRAJInfoData->TrajectoryNameC8, token, strlen(token)); + } + else if (j == 3) { + TRAJInfoData->TrajectoryVersionValueIdU16 = VALUE_ID_TRAJECTORY_VERSION; + TRAJInfoData->TrajectoryVersionContentLengthU16 = 2; + TRAJInfoData->TrajectoryVersionU16 = atoi(token); + } + else if (j == 4) { + *RowCount = atoi(token); + } + + j++; + token = strtok(NULL, ";"); + } + } + + TRAJInfoData->IpAddressValueIdU16 = 0xA000; + TRAJInfoData->IpAddressContentLengthU16 = 4; + TRAJInfoData->IpAddressU32 = 0; + + bzero(MessageBuffer, COMMAND_MESSAGE_HEADER_LENGTH + COMMAND_TRAJ_INFO_ROW_MESSAGE_LENGTH); + + HeaderData->SyncWordU16 = SYNC_WORD; + HeaderData->TransmitterIdU8 = 0; + HeaderData->MessageCounterU8 = 0; + HeaderData->AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; + HeaderData->MessageIdU16 = COMMAND_DOTM_CODE; + HeaderData->MessageLengthU32 = + *RowCount * COMMAND_DOTM_ROW_MESSAGE_LENGTH + COMMAND_TRAJ_INFO_ROW_MESSAGE_LENGTH; + + p = (C8 *) HeaderData; + for (i = 0; i < COMMAND_MESSAGE_HEADER_LENGTH; i++) + *(MessageBuffer + i) = *p++; + + p = (C8 *) TRAJInfoData; + for (; i < COMMAND_MESSAGE_HEADER_LENGTH + COMMAND_TRAJ_INFO_ROW_MESSAGE_LENGTH; i++) + *(MessageBuffer + i) = *p++; + + MessageIndex = i; + + + if (debug) { + // TODO: Change to log printout when byte thingy has been implemented + printf("Header + TRAJInfo total length = %d bytes\n", + (int)(COMMAND_MESSAGE_HEADER_LENGTH + COMMAND_TRAJ_INFO_ROW_MESSAGE_LENGTH)); + printf("----HEADER + TRAJInfo----\n"); + for (i = 0; i < sizeof (HeaderType) + sizeof (TRAJInfoType); i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n"); + printf("DOTM message total length = %d bytes.\n", (int)HeaderData->MessageLengthU32); + printf("Traj file header = %s\n", TrajFileHeader); + printf("TrajectoryID = %d\n", TRAJInfoData->TrajectoryIDU16); + printf("TrajectoryName = %s\n", TRAJInfoData->TrajectoryNameC8); + printf("TrajectoryVersion = %d\n", TRAJInfoData->TrajectoryVersionU16); + printf("RowCount = %d\n", *RowCount); + printf("IpAddress = %d\n", TRAJInfoData->IpAddressU32); + + printf("\n----MESSAGE----\n"); + } + + return MessageIndex; //Total number of bytes = COMMAND_MESSAGE_HEADER_LENGTH } I32 ObjectControlSendTRAJMessage(C8 * Filename, I32 * Socket, I32 RowCount, C8 * IP, U32 Port, - DOTMType * DOTMData, U8 debug) { - FILE *fd; - - // Save socket settings and set it to blocking - int retval = fcntl(*Socket, F_GETFL); - - if (retval < 0) { - LogMessage(LOG_LEVEL_ERROR, "Error getting socket options with fcntl"); - return -1; - } - int socketOptions = retval; - - retval = fcntl(*Socket, F_SETFL, socketOptions & ~O_NONBLOCK); - if (retval < 0) { - LogMessage(LOG_LEVEL_ERROR, "Error setting socket options with fcntl"); - return -1; - } - - - fd = fopen(Filename, "r"); - if (fd == NULL) { - LogMessage(LOG_LEVEL_ERROR, "Unable to open file <%s>", Filename); - return -1; - } - - UtilReadLineCntSpecChars(fd, TrajBuffer); //Read first line - int Rest = 0, i = 0, MessageLength = 0, SumMessageLength = 0, Modulo = 0, Transmissions = 0; - - Transmissions = RowCount / COMMAND_DOTM_ROWS_IN_TRANSMISSION; - Rest = RowCount % COMMAND_DOTM_ROWS_IN_TRANSMISSION; - U16 CrcU16 = 0; - - - for (i = 0; i < Transmissions; i++) { - MessageLength = - ObjectControlBuildTRAJMessage(TrajBuffer, fd, COMMAND_DOTM_ROWS_IN_TRANSMISSION, DOTMData, debug); - - if (i == Transmissions && Rest == 0) { - TrajBuffer[MessageLength] = (U8) (CrcU16); - TrajBuffer[MessageLength + 1] = (U8) (CrcU16 >> 8); - MessageLength = MessageLength + 2; - UtilSendTCPData("Object Control", TrajBuffer, MessageLength, Socket, 0); - SumMessageLength = SumMessageLength + MessageLength; - } - else { - UtilSendTCPData("Object Control", TrajBuffer, MessageLength, Socket, 0); - SumMessageLength = SumMessageLength + MessageLength; - } - - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "Transmission %d: %d bytes sent", i, MessageLength); - } - - if (Rest > 0) { - MessageLength = ObjectControlBuildTRAJMessage(TrajBuffer, fd, Rest, DOTMData, debug); - TrajBuffer[MessageLength] = (U8) (CrcU16); - TrajBuffer[MessageLength + 1] = (U8) (CrcU16 >> 8); - MessageLength = MessageLength + 2; - UtilSendTCPData("Object Control", TrajBuffer, MessageLength, Socket, 0); - SumMessageLength = SumMessageLength + MessageLength; - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "Transmission %d: %d bytes sent.\n", i, MessageLength); - } - - LogMessage(LOG_LEVEL_INFO, "%d DOTM bytes sent to %s:%d", SumMessageLength, IP, Port); - fclose(fd); - - // Reset socket settings - retval = fcntl(*Socket, F_SETFL, socketOptions); - if (retval < 0) { - LogMessage(LOG_LEVEL_ERROR, "Error setting socket options with fcntl"); - return -1; - } - - - return 0; + DOTMType * DOTMData, U8 debug) { + FILE *fd; + + // Save socket settings and set it to blocking + int retval = fcntl(*Socket, F_GETFL); + + if (retval < 0) { + LogMessage(LOG_LEVEL_ERROR, "Error getting socket options with fcntl"); + return -1; + } + int socketOptions = retval; + + retval = fcntl(*Socket, F_SETFL, socketOptions & ~O_NONBLOCK); + if (retval < 0) { + LogMessage(LOG_LEVEL_ERROR, "Error setting socket options with fcntl"); + return -1; + } + + + fd = fopen(Filename, "r"); + if (fd == NULL) { + LogMessage(LOG_LEVEL_ERROR, "Unable to open file <%s>", Filename); + return -1; + } + + UtilReadLineCntSpecChars(fd, TrajBuffer); //Read first line + int Rest = 0, i = 0, MessageLength = 0, SumMessageLength = 0, Modulo = 0, Transmissions = 0; + + Transmissions = RowCount / COMMAND_DOTM_ROWS_IN_TRANSMISSION; + Rest = RowCount % COMMAND_DOTM_ROWS_IN_TRANSMISSION; + U16 CrcU16 = 0; + + + for (i = 0; i < Transmissions; i++) { + MessageLength = + ObjectControlBuildTRAJMessage(TrajBuffer, fd, COMMAND_DOTM_ROWS_IN_TRANSMISSION, DOTMData, debug); + + if (i == Transmissions && Rest == 0) { + TrajBuffer[MessageLength] = (U8) (CrcU16); + TrajBuffer[MessageLength + 1] = (U8) (CrcU16 >> 8); + MessageLength = MessageLength + 2; + UtilSendTCPData("Object Control", TrajBuffer, MessageLength, Socket, 0); + SumMessageLength = SumMessageLength + MessageLength; + } + else { + UtilSendTCPData("Object Control", TrajBuffer, MessageLength, Socket, 0); + SumMessageLength = SumMessageLength + MessageLength; + } + + if (debug) + LogMessage(LOG_LEVEL_DEBUG, "Transmission %d: %d bytes sent", i, MessageLength); + } + + if (Rest > 0) { + MessageLength = ObjectControlBuildTRAJMessage(TrajBuffer, fd, Rest, DOTMData, debug); + TrajBuffer[MessageLength] = (U8) (CrcU16); + TrajBuffer[MessageLength + 1] = (U8) (CrcU16 >> 8); + MessageLength = MessageLength + 2; + UtilSendTCPData("Object Control", TrajBuffer, MessageLength, Socket, 0); + SumMessageLength = SumMessageLength + MessageLength; + if (debug) + LogMessage(LOG_LEVEL_DEBUG, "Transmission %d: %d bytes sent.\n", i, MessageLength); + } + + LogMessage(LOG_LEVEL_INFO, "%d DOTM bytes sent to %s:%d", SumMessageLength, IP, Port); + fclose(fd); + + // Reset socket settings + retval = fcntl(*Socket, F_SETFL, socketOptions); + if (retval < 0) { + LogMessage(LOG_LEVEL_ERROR, "Error setting socket options with fcntl"); + return -1; + } + + + return 0; } I32 ObjectControlBuildTRAJMessage(C8 * MessageBuffer, FILE * fd, I32 RowCount, DOTMType * DOTMData, U8 debug) { - I32 MessageIndex = 0; - C8 RowBuffer[100]; - C8 DataBuffer[20]; - dbl Data; - C8 *src, *p; - U16 Crc = 0; - flt curv = 0; - C8 *pc; - - bzero(MessageBuffer, COMMAND_DOTM_ROW_MESSAGE_LENGTH * RowCount); - - I32 i = 0, j = 0, n = 0; - - for (i = 0; i < RowCount; i++) { - bzero(RowBuffer, 100); - UtilReadLineCntSpecChars(fd, RowBuffer); - - //Read to ';' in row = LINE;0.00;21.239000;39.045000;0.000000;-1.240001;0.029103;0.004005;0.000000;3;ENDLINE; - //Time - src = strchr(RowBuffer, ';'); - bzero(DataBuffer, 20); - strncpy(DataBuffer, src + 1, (U64) strchr(src + 1, ';') - (U64) src - 1); - Data = atof(DataBuffer) * 1e3; - DOTMData->RelativeTimeValueIdU16 = VALUE_ID_RELATIVE_TIME; - DOTMData->RelativeTimeContentLengthU16 = 4; - DOTMData->RelativeTimeU32 = (U32) Data; - if (debug) - printf("Time DataBuffer=%s float=%3.6f\n", DataBuffer, Data); - - //x - src = strchr(src + 1, ';'); - bzero(DataBuffer, 20); - strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); - Data = atof(DataBuffer) * 1e3; - DOTMData->XPositionValueIdU16 = VALUE_ID_X_POSITION; - DOTMData->XPositionContentLengthU16 = 4; - DOTMData->XPositionI32 = (I32) Data; - if (debug) - printf("X DataBuffer=%s float=%3.6f\n", DataBuffer, Data); - - //y - src = strchr(src + 1, ';'); - bzero(DataBuffer, 20); - strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); - Data = atof(DataBuffer) * 1e3; - DOTMData->YPositionValueIdU16 = VALUE_ID_Y_POSITION; - DOTMData->YPositionContentLengthU16 = 4; - DOTMData->YPositionI32 = (I32) Data; - if (debug) - printf("Y DataBuffer=%s float=%3.6f\n", DataBuffer, Data); - - //z - src = strchr(src + 1, ';'); - bzero(DataBuffer, 20); - strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); - Data = atof(DataBuffer) * 1e3; - DOTMData->ZPositionValueIdU16 = VALUE_ID_Z_POSITION; - DOTMData->ZPositionContentLengthU16 = 4; - DOTMData->ZPositionI32 = (I32) Data; - if (debug) - printf("Z DataBuffer=%s float=%3.6f\n", DataBuffer, Data); - - //Heading - src = strchr(src + 1, ';'); - bzero(DataBuffer, 20); - strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); - Data = UtilRadToDeg(atof(DataBuffer)); - Data = 450 - Data; //Turn heading back pi/2 - while (Data < 0) - Data += 360.0; - while (Data > 360) - Data -= 360.0; - DOTMData->HeadingValueIdU16 = VALUE_ID_HEADING; - DOTMData->HeadingContentLengthU16 = 2; - DOTMData->HeadingU16 = (U16) (Data * 1e2); - if (debug) - printf("Heading DataBuffer=%s float=%3.6f\n", DataBuffer, Data); - - //Longitudinal speed - src = strchr(src + 1, ';'); - bzero(DataBuffer, 20); - strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); - Data = atof(DataBuffer) * 1e2; - DOTMData->LongitudinalSpeedValueIdU16 = VALUE_ID_LONGITUDINAL_SPEED; - DOTMData->LongitudinalSpeedContentLengthU16 = 2; - DOTMData->LongitudinalSpeedI16 = (I16) Data; - if (debug) - printf("Long speed DataBuffer=%s float=%3.6f\n", DataBuffer, Data); - - //Lateral speed - src = strchr(src + 1, ';'); - bzero(DataBuffer, 20); - strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); - Data = atof(DataBuffer) * 1e2; - DOTMData->LateralSpeedValueIdU16 = VALUE_ID_LATERAL_SPEED; - DOTMData->LateralSpeedContentLengthU16 = 2; - DOTMData->LateralSpeedI16 = (I16) Data; - if (debug) - printf("Lat speed DataBuffer=%s float=%3.6f\n", DataBuffer, Data); - - //Longitudinal acceleration - src = strchr(src + 1, ';'); - bzero(DataBuffer, 20); - strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); - Data = atof(DataBuffer) * 1e3; - DOTMData->LongitudinalAccValueIdU16 = VALUE_ID_LONGITUDINAL_ACCELERATION; - DOTMData->LongitudinalAccContentLengthU16 = 2; - DOTMData->LongitudinalAccI16 = (I16) Data; - if (debug) - printf("Long acc DataBuffer=%s float=%3.6f\n", DataBuffer, Data); - - //Lateral acceleration - src = strchr(src + 1, ';'); - bzero(DataBuffer, 20); - strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); - Data = atof(DataBuffer) * 1e3; - DOTMData->LateralAccValueIdU16 = VALUE_ID_LATERAL_ACCELERATION; - DOTMData->LateralAccContentLengthU16 = 2; - DOTMData->LateralAccI16 = (I16) Data; - if (debug) - printf("Lat accDataBuffer=%s float=%3.6f\n", DataBuffer, Data); - - //Curvature - src = strchr(src + 1, ';'); - bzero(DataBuffer, 20); - strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); - //Data = atof(DataBuffer) * 3e4; - curv = atof(DataBuffer); - pc = (C8 *) & curv; - DOTMData->CurvatureValueIdU16 = VALUE_ID_CURVATURE; - DOTMData->CurvatureContentLengthU16 = 4; - //DOTMData->CurvatureI32 = (I32) Data; - DOTMData->CurvatureI32 = pc[0]; - DOTMData->CurvatureI32 = DOTMData->CurvatureI32 | ((I32) pc[1]) << 8; - DOTMData->CurvatureI32 = DOTMData->CurvatureI32 | ((I32) pc[2]) << 16; - DOTMData->CurvatureI32 = DOTMData->CurvatureI32 | ((I32) pc[3]) << 24; - - if (debug) - printf("Curv DataBuffer=%s float=%3.6f\n", DataBuffer, Data); - - p = (C8 *) DOTMData; - for (j = 0; j < sizeof (DOTMType); j++, n++) - *(MessageBuffer + n) = *p++; - MessageIndex = n; - } - - - if (debug) { - int i = 0; - - for (i = 0; i < MessageIndex; i++) { - // TODO: Write to log when bytes thingy has been implemented - if ((unsigned char)MessageBuffer[i] >= 0 && (unsigned char)MessageBuffer[i] <= 15) - printf("0"); - printf("%x-", (unsigned char)MessageBuffer[i]); - } - printf("\n"); - } - - return MessageIndex; //Total number of bytes + I32 MessageIndex = 0; + C8 RowBuffer[100]; + C8 DataBuffer[20]; + dbl Data; + C8 *src, *p; + U16 Crc = 0; + flt curv = 0; + C8 *pc; + + bzero(MessageBuffer, COMMAND_DOTM_ROW_MESSAGE_LENGTH * RowCount); + + I32 i = 0, j = 0, n = 0; + + for (i = 0; i < RowCount; i++) { + bzero(RowBuffer, 100); + UtilReadLineCntSpecChars(fd, RowBuffer); + + //Read to ';' in row = LINE;0.00;21.239000;39.045000;0.000000;-1.240001;0.029103;0.004005;0.000000;3;ENDLINE; + //Time + src = strchr(RowBuffer, ';'); + bzero(DataBuffer, 20); + strncpy(DataBuffer, src + 1, (U64) strchr(src + 1, ';') - (U64) src - 1); + Data = atof(DataBuffer) * 1e3; + DOTMData->RelativeTimeValueIdU16 = VALUE_ID_RELATIVE_TIME; + DOTMData->RelativeTimeContentLengthU16 = 4; + DOTMData->RelativeTimeU32 = (U32) Data; + if (debug) + printf("Time DataBuffer=%s float=%3.6f\n", DataBuffer, Data); + + //x + src = strchr(src + 1, ';'); + bzero(DataBuffer, 20); + strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); + Data = atof(DataBuffer) * 1e3; + DOTMData->XPositionValueIdU16 = VALUE_ID_X_POSITION; + DOTMData->XPositionContentLengthU16 = 4; + DOTMData->XPositionI32 = (I32) Data; + if (debug) + printf("X DataBuffer=%s float=%3.6f\n", DataBuffer, Data); + + //y + src = strchr(src + 1, ';'); + bzero(DataBuffer, 20); + strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); + Data = atof(DataBuffer) * 1e3; + DOTMData->YPositionValueIdU16 = VALUE_ID_Y_POSITION; + DOTMData->YPositionContentLengthU16 = 4; + DOTMData->YPositionI32 = (I32) Data; + if (debug) + printf("Y DataBuffer=%s float=%3.6f\n", DataBuffer, Data); + + //z + src = strchr(src + 1, ';'); + bzero(DataBuffer, 20); + strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); + Data = atof(DataBuffer) * 1e3; + DOTMData->ZPositionValueIdU16 = VALUE_ID_Z_POSITION; + DOTMData->ZPositionContentLengthU16 = 4; + DOTMData->ZPositionI32 = (I32) Data; + if (debug) + printf("Z DataBuffer=%s float=%3.6f\n", DataBuffer, Data); + + //Heading + src = strchr(src + 1, ';'); + bzero(DataBuffer, 20); + strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); + Data = UtilRadToDeg(atof(DataBuffer)); + Data = 450 - Data; //Turn heading back pi/2 + while (Data < 0) + Data += 360.0; + while (Data > 360) + Data -= 360.0; + DOTMData->HeadingValueIdU16 = VALUE_ID_HEADING; + DOTMData->HeadingContentLengthU16 = 2; + DOTMData->HeadingU16 = (U16) (Data * 1e2); + if (debug) + printf("Heading DataBuffer=%s float=%3.6f\n", DataBuffer, Data); + + //Longitudinal speed + src = strchr(src + 1, ';'); + bzero(DataBuffer, 20); + strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); + Data = atof(DataBuffer) * 1e2; + DOTMData->LongitudinalSpeedValueIdU16 = VALUE_ID_LONGITUDINAL_SPEED; + DOTMData->LongitudinalSpeedContentLengthU16 = 2; + DOTMData->LongitudinalSpeedI16 = (I16) Data; + if (debug) + printf("Long speed DataBuffer=%s float=%3.6f\n", DataBuffer, Data); + + //Lateral speed + src = strchr(src + 1, ';'); + bzero(DataBuffer, 20); + strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); + Data = atof(DataBuffer) * 1e2; + DOTMData->LateralSpeedValueIdU16 = VALUE_ID_LATERAL_SPEED; + DOTMData->LateralSpeedContentLengthU16 = 2; + DOTMData->LateralSpeedI16 = (I16) Data; + if (debug) + printf("Lat speed DataBuffer=%s float=%3.6f\n", DataBuffer, Data); + + //Longitudinal acceleration + src = strchr(src + 1, ';'); + bzero(DataBuffer, 20); + strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); + Data = atof(DataBuffer) * 1e3; + DOTMData->LongitudinalAccValueIdU16 = VALUE_ID_LONGITUDINAL_ACCELERATION; + DOTMData->LongitudinalAccContentLengthU16 = 2; + DOTMData->LongitudinalAccI16 = (I16) Data; + if (debug) + printf("Long acc DataBuffer=%s float=%3.6f\n", DataBuffer, Data); + + //Lateral acceleration + src = strchr(src + 1, ';'); + bzero(DataBuffer, 20); + strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); + Data = atof(DataBuffer) * 1e3; + DOTMData->LateralAccValueIdU16 = VALUE_ID_LATERAL_ACCELERATION; + DOTMData->LateralAccContentLengthU16 = 2; + DOTMData->LateralAccI16 = (I16) Data; + if (debug) + printf("Lat accDataBuffer=%s float=%3.6f\n", DataBuffer, Data); + + //Curvature + src = strchr(src + 1, ';'); + bzero(DataBuffer, 20); + strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); + //Data = atof(DataBuffer) * 3e4; + curv = atof(DataBuffer); + pc = (C8 *) & curv; + DOTMData->CurvatureValueIdU16 = VALUE_ID_CURVATURE; + DOTMData->CurvatureContentLengthU16 = 4; + //DOTMData->CurvatureI32 = (I32) Data; + DOTMData->CurvatureI32 = pc[0]; + DOTMData->CurvatureI32 = DOTMData->CurvatureI32 | ((I32) pc[1]) << 8; + DOTMData->CurvatureI32 = DOTMData->CurvatureI32 | ((I32) pc[2]) << 16; + DOTMData->CurvatureI32 = DOTMData->CurvatureI32 | ((I32) pc[3]) << 24; + + if (debug) + printf("Curv DataBuffer=%s float=%3.6f\n", DataBuffer, Data); + + p = (C8 *) DOTMData; + for (j = 0; j < sizeof (DOTMType); j++, n++) + *(MessageBuffer + n) = *p++; + MessageIndex = n; + } + + + if (debug) { + int i = 0; + + for (i = 0; i < MessageIndex; i++) { + // TODO: Write to log when bytes thingy has been implemented + if ((unsigned char)MessageBuffer[i] >= 0 && (unsigned char)MessageBuffer[i] <= 15) + printf("0"); + printf("%x-", (unsigned char)MessageBuffer[i]); + } + printf("\n"); + } + + return MessageIndex; //Total number of bytes } I32 ObjectControlBuildASPMessage(C8 * MessageBuffer, ASPType * ASPData, U8 debug) { - I32 MessageIndex = 0, i; - C8 *p; - - bzero(MessageBuffer, ASP_MESSAGE_LENGTH); - p = (C8 *) ASPData; - for (i = 0; i < sizeof (ASPType); i++) - *(MessageBuffer + i) = *p++; - MessageIndex = i; - - if (debug) { - // TODO: Write to log when bytes thingy has been implemented - printf("ASP total length = %d bytes \n", (int)(ASP_MESSAGE_LENGTH)); - printf("\n----MESSAGE----\n"); - for (i = 0; i < sizeof (ASPType); i++) - printf("%x ", (C8) MessageBuffer[i]); - printf("\n"); - } - - return MessageIndex; //Total number of bytes + I32 MessageIndex = 0, i; + C8 *p; + + bzero(MessageBuffer, ASP_MESSAGE_LENGTH); + p = (C8 *) ASPData; + for (i = 0; i < sizeof (ASPType); i++) + *(MessageBuffer + i) = *p++; + MessageIndex = i; + + if (debug) { + // TODO: Write to log when bytes thingy has been implemented + printf("ASP total length = %d bytes \n", (int)(ASP_MESSAGE_LENGTH)); + printf("\n----MESSAGE----\n"); + for (i = 0; i < sizeof (ASPType); i++) + printf("%x ", (C8) MessageBuffer[i]); + printf("\n"); + } + + return MessageIndex; //Total number of bytes } @@ -2289,100 +2291,100 @@ I32 ObjectControlBuildASPMessage(C8 * MessageBuffer, ASPType * ASPData, U8 debug * \return Length of sent message */ I32 ObjectControlSendACCMMessage(ACCMData * ACCM, I32 * socket, U8 debug) { - ACCMType isoACCM; - C8 messageBuffer[sizeof (isoACCM)]; - C8 *ptr = messageBuffer; - U32 messageSize = 0; + ACCMType isoACCM; + C8 messageBuffer[sizeof (isoACCM)]; + C8 *ptr = messageBuffer; + U32 messageSize = 0; - ObjectControlBuildACCMMessage(ACCM, &isoACCM, debug); + ObjectControlBuildACCMMessage(ACCM, &isoACCM, debug); - // Copy ACCM header to send buffer - memcpy(ptr, &isoACCM.header.SyncWordU16, sizeof (isoACCM.header.SyncWordU16)); - ptr += sizeof (isoACCM.header.SyncWordU16); + // Copy ACCM header to send buffer + memcpy(ptr, &isoACCM.header.SyncWordU16, sizeof (isoACCM.header.SyncWordU16)); + ptr += sizeof (isoACCM.header.SyncWordU16); - memcpy(ptr, &isoACCM.header.TransmitterIdU8, sizeof (isoACCM.header.TransmitterIdU8)); - ptr += sizeof (isoACCM.header.TransmitterIdU8); + memcpy(ptr, &isoACCM.header.TransmitterIdU8, sizeof (isoACCM.header.TransmitterIdU8)); + ptr += sizeof (isoACCM.header.TransmitterIdU8); - memcpy(ptr, &isoACCM.header.MessageCounterU8, sizeof (isoACCM.header.MessageCounterU8)); - ptr += sizeof (isoACCM.header.MessageCounterU8); + memcpy(ptr, &isoACCM.header.MessageCounterU8, sizeof (isoACCM.header.MessageCounterU8)); + ptr += sizeof (isoACCM.header.MessageCounterU8); - memcpy(ptr, &isoACCM.header.AckReqProtVerU8, sizeof (isoACCM.header.AckReqProtVerU8)); - ptr += sizeof (isoACCM.header.AckReqProtVerU8); + memcpy(ptr, &isoACCM.header.AckReqProtVerU8, sizeof (isoACCM.header.AckReqProtVerU8)); + ptr += sizeof (isoACCM.header.AckReqProtVerU8); - memcpy(ptr, &isoACCM.header.MessageIdU16, sizeof (isoACCM.header.MessageIdU16)); - ptr += sizeof (isoACCM.header.MessageIdU16); + memcpy(ptr, &isoACCM.header.MessageIdU16, sizeof (isoACCM.header.MessageIdU16)); + ptr += sizeof (isoACCM.header.MessageIdU16); - memcpy(ptr, &isoACCM.header.MessageLengthU32, sizeof (isoACCM.header.MessageLengthU32)); - ptr += sizeof (isoACCM.header.MessageLengthU32); + memcpy(ptr, &isoACCM.header.MessageLengthU32, sizeof (isoACCM.header.MessageLengthU32)); + ptr += sizeof (isoACCM.header.MessageLengthU32); - // Copy ACCM action ID to send buffer - memcpy(ptr, &isoACCM.actionIDValueID, sizeof (isoACCM.actionIDValueID)); - ptr += sizeof (isoACCM.actionIDValueID); + // Copy ACCM action ID to send buffer + memcpy(ptr, &isoACCM.actionIDValueID, sizeof (isoACCM.actionIDValueID)); + ptr += sizeof (isoACCM.actionIDValueID); - memcpy(ptr, &isoACCM.actionIDContentLength, sizeof (isoACCM.actionIDContentLength)); - ptr += sizeof (isoACCM.actionIDContentLength); + memcpy(ptr, &isoACCM.actionIDContentLength, sizeof (isoACCM.actionIDContentLength)); + ptr += sizeof (isoACCM.actionIDContentLength); - memcpy(ptr, &isoACCM.actionID, sizeof (isoACCM.actionID)); - ptr += sizeof (isoACCM.actionID); + memcpy(ptr, &isoACCM.actionID, sizeof (isoACCM.actionID)); + ptr += sizeof (isoACCM.actionID); - // Copy ACCM action type to send buffer - memcpy(ptr, &isoACCM.actionTypeValueID, sizeof (isoACCM.actionTypeValueID)); - ptr += sizeof (isoACCM.actionTypeValueID); + // Copy ACCM action type to send buffer + memcpy(ptr, &isoACCM.actionTypeValueID, sizeof (isoACCM.actionTypeValueID)); + ptr += sizeof (isoACCM.actionTypeValueID); - memcpy(ptr, &isoACCM.actionTypeContentLength, sizeof (isoACCM.actionTypeContentLength)); - ptr += sizeof (isoACCM.actionTypeContentLength); + memcpy(ptr, &isoACCM.actionTypeContentLength, sizeof (isoACCM.actionTypeContentLength)); + ptr += sizeof (isoACCM.actionTypeContentLength); - memcpy(ptr, &isoACCM.actionType, sizeof (isoACCM.actionType)); - ptr += sizeof (isoACCM.actionType); + memcpy(ptr, &isoACCM.actionType, sizeof (isoACCM.actionType)); + ptr += sizeof (isoACCM.actionType); - // Copy ACCM action parameter 1 to send buffer - memcpy(ptr, &isoACCM.actionTypeParameter1ValueID, sizeof (isoACCM.actionTypeParameter1ValueID)); - ptr += sizeof (isoACCM.actionTypeParameter1ValueID); + // Copy ACCM action parameter 1 to send buffer + memcpy(ptr, &isoACCM.actionTypeParameter1ValueID, sizeof (isoACCM.actionTypeParameter1ValueID)); + ptr += sizeof (isoACCM.actionTypeParameter1ValueID); - memcpy(ptr, &isoACCM.actionTypeParameter1ContentLength, - sizeof (isoACCM.actionTypeParameter1ContentLength)); - ptr += sizeof (isoACCM.actionTypeParameter1ContentLength); + memcpy(ptr, &isoACCM.actionTypeParameter1ContentLength, + sizeof (isoACCM.actionTypeParameter1ContentLength)); + ptr += sizeof (isoACCM.actionTypeParameter1ContentLength); - memcpy(ptr, &isoACCM.actionTypeParameter1, sizeof (isoACCM.actionTypeParameter1)); - ptr += sizeof (isoACCM.actionTypeParameter1); + memcpy(ptr, &isoACCM.actionTypeParameter1, sizeof (isoACCM.actionTypeParameter1)); + ptr += sizeof (isoACCM.actionTypeParameter1); - // Copy ACCM action parameter 2 to send buffer - memcpy(ptr, &isoACCM.actionTypeParameter2ValueID, sizeof (isoACCM.actionTypeParameter2ValueID)); - ptr += sizeof (isoACCM.actionTypeParameter2ValueID); + // Copy ACCM action parameter 2 to send buffer + memcpy(ptr, &isoACCM.actionTypeParameter2ValueID, sizeof (isoACCM.actionTypeParameter2ValueID)); + ptr += sizeof (isoACCM.actionTypeParameter2ValueID); - memcpy(ptr, &isoACCM.actionTypeParameter2ContentLength, - sizeof (isoACCM.actionTypeParameter2ContentLength)); - ptr += sizeof (isoACCM.actionTypeParameter2ContentLength); + memcpy(ptr, &isoACCM.actionTypeParameter2ContentLength, + sizeof (isoACCM.actionTypeParameter2ContentLength)); + ptr += sizeof (isoACCM.actionTypeParameter2ContentLength); - memcpy(ptr, &isoACCM.actionTypeParameter2, sizeof (isoACCM.actionTypeParameter2)); - ptr += sizeof (isoACCM.actionTypeParameter2); + memcpy(ptr, &isoACCM.actionTypeParameter2, sizeof (isoACCM.actionTypeParameter2)); + ptr += sizeof (isoACCM.actionTypeParameter2); - // Copy ACCM action parameter 3 to send buffer - memcpy(ptr, &isoACCM.actionTypeParameter3ValueID, sizeof (isoACCM.actionTypeParameter3ValueID)); - ptr += sizeof (isoACCM.actionTypeParameter3ValueID); + // Copy ACCM action parameter 3 to send buffer + memcpy(ptr, &isoACCM.actionTypeParameter3ValueID, sizeof (isoACCM.actionTypeParameter3ValueID)); + ptr += sizeof (isoACCM.actionTypeParameter3ValueID); - memcpy(ptr, &isoACCM.actionTypeParameter3ContentLength, - sizeof (isoACCM.actionTypeParameter3ContentLength)); - ptr += sizeof (isoACCM.actionTypeParameter3ContentLength); + memcpy(ptr, &isoACCM.actionTypeParameter3ContentLength, + sizeof (isoACCM.actionTypeParameter3ContentLength)); + ptr += sizeof (isoACCM.actionTypeParameter3ContentLength); - memcpy(ptr, &isoACCM.actionTypeParameter3, sizeof (isoACCM.actionTypeParameter3)); - ptr += sizeof (isoACCM.actionTypeParameter3); + memcpy(ptr, &isoACCM.actionTypeParameter3, sizeof (isoACCM.actionTypeParameter3)); + ptr += sizeof (isoACCM.actionTypeParameter3); - // Copy ACCM footer to send buffer - memcpy(ptr, &isoACCM.footer.Crc, sizeof (isoACCM.footer.Crc)); - ptr += sizeof (isoACCM.footer.Crc); + // Copy ACCM footer to send buffer + memcpy(ptr, &isoACCM.footer.Crc, sizeof (isoACCM.footer.Crc)); + ptr += sizeof (isoACCM.footer.Crc); - if (ptr > messageBuffer) - messageSize = (U32) (ptr - messageBuffer); + if (ptr > messageBuffer) + messageSize = (U32) (ptr - messageBuffer); - if (messageSize - sizeof (isoACCM.header) - sizeof (isoACCM.footer) != isoACCM.header.MessageLengthU32) - LogMessage(LOG_LEVEL_WARNING, "ACCM message sent with invalid message length"); + if (messageSize - sizeof (isoACCM.header) - sizeof (isoACCM.footer) != isoACCM.header.MessageLengthU32) + LogMessage(LOG_LEVEL_WARNING, "ACCM message sent with invalid message length"); - UtilSendTCPData(MODULE_NAME, messageBuffer, (I32) messageSize, socket, debug); + UtilSendTCPData(MODULE_NAME, messageBuffer, (I32) messageSize, socket, debug); - return (I32) messageSize; + return (I32) messageSize; } /*! @@ -2393,100 +2395,100 @@ I32 ObjectControlSendACCMMessage(ACCMData * ACCM, I32 * socket, U8 debug) { * \return Length of sent message */ I32 ObjectControlSendTRCMMessage(TRCMData * TRCM, I32 * socket, U8 debug) { - TRCMType isoTRCM; - C8 messageBuffer[sizeof (isoTRCM)]; - C8 *ptr = messageBuffer; - U32 messageSize = 0; + TRCMType isoTRCM; + C8 messageBuffer[sizeof (isoTRCM)]; + C8 *ptr = messageBuffer; + U32 messageSize = 0; - ObjectControlBuildTRCMMessage(TRCM, &isoTRCM, debug); + ObjectControlBuildTRCMMessage(TRCM, &isoTRCM, debug); - // Copy TRCM header to send buffer - memcpy(ptr, &isoTRCM.header.SyncWordU16, sizeof (isoTRCM.header.SyncWordU16)); - ptr += sizeof (isoTRCM.header.SyncWordU16); + // Copy TRCM header to send buffer + memcpy(ptr, &isoTRCM.header.SyncWordU16, sizeof (isoTRCM.header.SyncWordU16)); + ptr += sizeof (isoTRCM.header.SyncWordU16); - memcpy(ptr, &isoTRCM.header.TransmitterIdU8, sizeof (isoTRCM.header.TransmitterIdU8)); - ptr += sizeof (isoTRCM.header.TransmitterIdU8); + memcpy(ptr, &isoTRCM.header.TransmitterIdU8, sizeof (isoTRCM.header.TransmitterIdU8)); + ptr += sizeof (isoTRCM.header.TransmitterIdU8); - memcpy(ptr, &isoTRCM.header.MessageCounterU8, sizeof (isoTRCM.header.MessageCounterU8)); - ptr += sizeof (isoTRCM.header.MessageCounterU8); + memcpy(ptr, &isoTRCM.header.MessageCounterU8, sizeof (isoTRCM.header.MessageCounterU8)); + ptr += sizeof (isoTRCM.header.MessageCounterU8); - memcpy(ptr, &isoTRCM.header.AckReqProtVerU8, sizeof (isoTRCM.header.AckReqProtVerU8)); - ptr += sizeof (isoTRCM.header.AckReqProtVerU8); + memcpy(ptr, &isoTRCM.header.AckReqProtVerU8, sizeof (isoTRCM.header.AckReqProtVerU8)); + ptr += sizeof (isoTRCM.header.AckReqProtVerU8); - memcpy(ptr, &isoTRCM.header.MessageIdU16, sizeof (isoTRCM.header.MessageIdU16)); - ptr += sizeof (isoTRCM.header.MessageIdU16); + memcpy(ptr, &isoTRCM.header.MessageIdU16, sizeof (isoTRCM.header.MessageIdU16)); + ptr += sizeof (isoTRCM.header.MessageIdU16); - memcpy(ptr, &isoTRCM.header.MessageLengthU32, sizeof (isoTRCM.header.MessageLengthU32)); - ptr += sizeof (isoTRCM.header.MessageLengthU32); + memcpy(ptr, &isoTRCM.header.MessageLengthU32, sizeof (isoTRCM.header.MessageLengthU32)); + ptr += sizeof (isoTRCM.header.MessageLengthU32); - // Copy TRCM trigger ID to send buffer - memcpy(ptr, &isoTRCM.triggerIDValueID, sizeof (isoTRCM.triggerIDValueID)); - ptr += sizeof (isoTRCM.triggerIDValueID); + // Copy TRCM trigger ID to send buffer + memcpy(ptr, &isoTRCM.triggerIDValueID, sizeof (isoTRCM.triggerIDValueID)); + ptr += sizeof (isoTRCM.triggerIDValueID); - memcpy(ptr, &isoTRCM.triggerIDContentLength, sizeof (isoTRCM.triggerIDContentLength)); - ptr += sizeof (isoTRCM.triggerIDContentLength); + memcpy(ptr, &isoTRCM.triggerIDContentLength, sizeof (isoTRCM.triggerIDContentLength)); + ptr += sizeof (isoTRCM.triggerIDContentLength); - memcpy(ptr, &isoTRCM.triggerID, sizeof (isoTRCM.triggerID)); - ptr += sizeof (isoTRCM.triggerID); + memcpy(ptr, &isoTRCM.triggerID, sizeof (isoTRCM.triggerID)); + ptr += sizeof (isoTRCM.triggerID); - // Copy TRCM trigger type to send buffer - memcpy(ptr, &isoTRCM.triggerTypeValueID, sizeof (isoTRCM.triggerTypeValueID)); - ptr += sizeof (isoTRCM.triggerTypeValueID); + // Copy TRCM trigger type to send buffer + memcpy(ptr, &isoTRCM.triggerTypeValueID, sizeof (isoTRCM.triggerTypeValueID)); + ptr += sizeof (isoTRCM.triggerTypeValueID); - memcpy(ptr, &isoTRCM.triggerTypeContentLength, sizeof (isoTRCM.triggerTypeContentLength)); - ptr += sizeof (isoTRCM.triggerTypeContentLength); + memcpy(ptr, &isoTRCM.triggerTypeContentLength, sizeof (isoTRCM.triggerTypeContentLength)); + ptr += sizeof (isoTRCM.triggerTypeContentLength); - memcpy(ptr, &isoTRCM.triggerType, sizeof (isoTRCM.triggerType)); - ptr += sizeof (isoTRCM.triggerType); + memcpy(ptr, &isoTRCM.triggerType, sizeof (isoTRCM.triggerType)); + ptr += sizeof (isoTRCM.triggerType); - // Copy TRCM trigger parameter 1 to send buffer - memcpy(ptr, &isoTRCM.triggerTypeParameter1ValueID, sizeof (isoTRCM.triggerTypeParameter1ValueID)); - ptr += sizeof (isoTRCM.triggerTypeParameter1ValueID); + // Copy TRCM trigger parameter 1 to send buffer + memcpy(ptr, &isoTRCM.triggerTypeParameter1ValueID, sizeof (isoTRCM.triggerTypeParameter1ValueID)); + ptr += sizeof (isoTRCM.triggerTypeParameter1ValueID); - memcpy(ptr, &isoTRCM.triggerTypeParameter1ContentLength, - sizeof (isoTRCM.triggerTypeParameter1ContentLength)); - ptr += sizeof (isoTRCM.triggerTypeParameter1ContentLength); + memcpy(ptr, &isoTRCM.triggerTypeParameter1ContentLength, + sizeof (isoTRCM.triggerTypeParameter1ContentLength)); + ptr += sizeof (isoTRCM.triggerTypeParameter1ContentLength); - memcpy(ptr, &isoTRCM.triggerTypeParameter1, sizeof (isoTRCM.triggerTypeParameter1)); - ptr += sizeof (isoTRCM.triggerTypeParameter1); + memcpy(ptr, &isoTRCM.triggerTypeParameter1, sizeof (isoTRCM.triggerTypeParameter1)); + ptr += sizeof (isoTRCM.triggerTypeParameter1); - // Copy TRCM trigger parameter 2 to send buffer - memcpy(ptr, &isoTRCM.triggerTypeParameter2ValueID, sizeof (isoTRCM.triggerTypeParameter2ValueID)); - ptr += sizeof (isoTRCM.triggerTypeParameter2ValueID); + // Copy TRCM trigger parameter 2 to send buffer + memcpy(ptr, &isoTRCM.triggerTypeParameter2ValueID, sizeof (isoTRCM.triggerTypeParameter2ValueID)); + ptr += sizeof (isoTRCM.triggerTypeParameter2ValueID); - memcpy(ptr, &isoTRCM.triggerTypeParameter2ContentLength, - sizeof (isoTRCM.triggerTypeParameter2ContentLength)); - ptr += sizeof (isoTRCM.triggerTypeParameter2ContentLength); + memcpy(ptr, &isoTRCM.triggerTypeParameter2ContentLength, + sizeof (isoTRCM.triggerTypeParameter2ContentLength)); + ptr += sizeof (isoTRCM.triggerTypeParameter2ContentLength); - memcpy(ptr, &isoTRCM.triggerTypeParameter2, sizeof (isoTRCM.triggerTypeParameter2)); - ptr += sizeof (isoTRCM.triggerTypeParameter2); + memcpy(ptr, &isoTRCM.triggerTypeParameter2, sizeof (isoTRCM.triggerTypeParameter2)); + ptr += sizeof (isoTRCM.triggerTypeParameter2); - // Copy TRCM trigger parameter 3 to send buffer - memcpy(ptr, &isoTRCM.triggerTypeParameter3ValueID, sizeof (isoTRCM.triggerTypeParameter3ValueID)); - ptr += sizeof (isoTRCM.triggerTypeParameter3ValueID); + // Copy TRCM trigger parameter 3 to send buffer + memcpy(ptr, &isoTRCM.triggerTypeParameter3ValueID, sizeof (isoTRCM.triggerTypeParameter3ValueID)); + ptr += sizeof (isoTRCM.triggerTypeParameter3ValueID); - memcpy(ptr, &isoTRCM.triggerTypeParameter3ContentLength, - sizeof (isoTRCM.triggerTypeParameter3ContentLength)); - ptr += sizeof (isoTRCM.triggerTypeParameter3ContentLength); + memcpy(ptr, &isoTRCM.triggerTypeParameter3ContentLength, + sizeof (isoTRCM.triggerTypeParameter3ContentLength)); + ptr += sizeof (isoTRCM.triggerTypeParameter3ContentLength); - memcpy(ptr, &isoTRCM.triggerTypeParameter3, sizeof (isoTRCM.triggerTypeParameter3)); - ptr += sizeof (isoTRCM.triggerTypeParameter3); + memcpy(ptr, &isoTRCM.triggerTypeParameter3, sizeof (isoTRCM.triggerTypeParameter3)); + ptr += sizeof (isoTRCM.triggerTypeParameter3); - // Copy TRCM footer to send buffer - memcpy(ptr, &isoTRCM.footer.Crc, sizeof (isoTRCM.footer.Crc)); - ptr += sizeof (isoTRCM.footer.Crc); + // Copy TRCM footer to send buffer + memcpy(ptr, &isoTRCM.footer.Crc, sizeof (isoTRCM.footer.Crc)); + ptr += sizeof (isoTRCM.footer.Crc); - if (ptr > messageBuffer) - messageSize = (U32) (ptr - messageBuffer); + if (ptr > messageBuffer) + messageSize = (U32) (ptr - messageBuffer); - if (messageSize - sizeof (isoTRCM.header) - sizeof (isoTRCM.footer) != isoTRCM.header.MessageLengthU32) - LogMessage(LOG_LEVEL_WARNING, "TRCM message sent with invalid message length"); + if (messageSize - sizeof (isoTRCM.header) - sizeof (isoTRCM.footer) != isoTRCM.header.MessageLengthU32) + LogMessage(LOG_LEVEL_WARNING, "TRCM message sent with invalid message length"); - UtilSendTCPData(MODULE_NAME, messageBuffer, (I32) messageSize, socket, 0); + UtilSendTCPData(MODULE_NAME, messageBuffer, (I32) messageSize, socket, 0); - return (I32) messageSize; + return (I32) messageSize; } /*! @@ -2497,67 +2499,67 @@ I32 ObjectControlSendTRCMMessage(TRCMData * TRCM, I32 * socket, U8 debug) { * \return Length of sent message */ I32 ObjectControlSendEXACMessage(EXACData * EXAC, I32 * socket, U8 debug) { - EXACType isoEXAC; - C8 messageBuffer[sizeof (isoEXAC)]; - C8 *ptr = messageBuffer; - U32 messageSize = 0; + EXACType isoEXAC; + C8 messageBuffer[sizeof (isoEXAC)]; + C8 *ptr = messageBuffer; + U32 messageSize = 0; - ObjectControlBuildEXACMessage(EXAC, &isoEXAC, debug); + ObjectControlBuildEXACMessage(EXAC, &isoEXAC, debug); - // Copy EXAC header to send buffer - memcpy(ptr, &isoEXAC.header.SyncWordU16, sizeof (isoEXAC.header.SyncWordU16)); - ptr += sizeof (isoEXAC.header.SyncWordU16); + // Copy EXAC header to send buffer + memcpy(ptr, &isoEXAC.header.SyncWordU16, sizeof (isoEXAC.header.SyncWordU16)); + ptr += sizeof (isoEXAC.header.SyncWordU16); - memcpy(ptr, &isoEXAC.header.TransmitterIdU8, sizeof (isoEXAC.header.TransmitterIdU8)); - ptr += sizeof (isoEXAC.header.TransmitterIdU8); + memcpy(ptr, &isoEXAC.header.TransmitterIdU8, sizeof (isoEXAC.header.TransmitterIdU8)); + ptr += sizeof (isoEXAC.header.TransmitterIdU8); - memcpy(ptr, &isoEXAC.header.MessageCounterU8, sizeof (isoEXAC.header.MessageCounterU8)); - ptr += sizeof (isoEXAC.header.MessageCounterU8); + memcpy(ptr, &isoEXAC.header.MessageCounterU8, sizeof (isoEXAC.header.MessageCounterU8)); + ptr += sizeof (isoEXAC.header.MessageCounterU8); - memcpy(ptr, &isoEXAC.header.AckReqProtVerU8, sizeof (isoEXAC.header.AckReqProtVerU8)); - ptr += sizeof (isoEXAC.header.AckReqProtVerU8); + memcpy(ptr, &isoEXAC.header.AckReqProtVerU8, sizeof (isoEXAC.header.AckReqProtVerU8)); + ptr += sizeof (isoEXAC.header.AckReqProtVerU8); - memcpy(ptr, &isoEXAC.header.MessageIdU16, sizeof (isoEXAC.header.MessageIdU16)); - ptr += sizeof (isoEXAC.header.MessageIdU16); + memcpy(ptr, &isoEXAC.header.MessageIdU16, sizeof (isoEXAC.header.MessageIdU16)); + ptr += sizeof (isoEXAC.header.MessageIdU16); - memcpy(ptr, &isoEXAC.header.MessageLengthU32, sizeof (isoEXAC.header.MessageLengthU32)); - ptr += sizeof (isoEXAC.header.MessageLengthU32); + memcpy(ptr, &isoEXAC.header.MessageLengthU32, sizeof (isoEXAC.header.MessageLengthU32)); + ptr += sizeof (isoEXAC.header.MessageLengthU32); - // Copy EXAC action ID to send buffer - memcpy(ptr, &isoEXAC.actionIDValueID, sizeof (isoEXAC.actionIDValueID)); - ptr += sizeof (isoEXAC.actionIDValueID); + // Copy EXAC action ID to send buffer + memcpy(ptr, &isoEXAC.actionIDValueID, sizeof (isoEXAC.actionIDValueID)); + ptr += sizeof (isoEXAC.actionIDValueID); - memcpy(ptr, &isoEXAC.actionIDContentLength, sizeof (isoEXAC.actionIDContentLength)); - ptr += sizeof (isoEXAC.actionIDContentLength); + memcpy(ptr, &isoEXAC.actionIDContentLength, sizeof (isoEXAC.actionIDContentLength)); + ptr += sizeof (isoEXAC.actionIDContentLength); - memcpy(ptr, &isoEXAC.actionID, sizeof (isoEXAC.actionID)); - ptr += sizeof (isoEXAC.actionID); + memcpy(ptr, &isoEXAC.actionID, sizeof (isoEXAC.actionID)); + ptr += sizeof (isoEXAC.actionID); - // Copy EXAC action execution time to send buffer - memcpy(ptr, &isoEXAC.executionTime_qmsoWValueID, sizeof (isoEXAC.executionTime_qmsoWValueID)); - ptr += sizeof (isoEXAC.executionTime_qmsoWValueID); + // Copy EXAC action execution time to send buffer + memcpy(ptr, &isoEXAC.executionTime_qmsoWValueID, sizeof (isoEXAC.executionTime_qmsoWValueID)); + ptr += sizeof (isoEXAC.executionTime_qmsoWValueID); - memcpy(ptr, &isoEXAC.executionTime_qmsoWContentLength, sizeof (isoEXAC.executionTime_qmsoWContentLength)); - ptr += sizeof (isoEXAC.executionTime_qmsoWContentLength); + memcpy(ptr, &isoEXAC.executionTime_qmsoWContentLength, sizeof (isoEXAC.executionTime_qmsoWContentLength)); + ptr += sizeof (isoEXAC.executionTime_qmsoWContentLength); - memcpy(ptr, &isoEXAC.executionTime_qmsoW, sizeof (isoEXAC.executionTime_qmsoW)); - ptr += sizeof (isoEXAC.executionTime_qmsoW); + memcpy(ptr, &isoEXAC.executionTime_qmsoW, sizeof (isoEXAC.executionTime_qmsoW)); + ptr += sizeof (isoEXAC.executionTime_qmsoW); - // Copy EXAC footer to send buffer - memcpy(ptr, &isoEXAC.footer.Crc, sizeof (isoEXAC.footer.Crc)); - ptr += sizeof (isoEXAC.footer.Crc); + // Copy EXAC footer to send buffer + memcpy(ptr, &isoEXAC.footer.Crc, sizeof (isoEXAC.footer.Crc)); + ptr += sizeof (isoEXAC.footer.Crc); - if (ptr > messageBuffer) - messageSize = (U32) (ptr - messageBuffer); + if (ptr > messageBuffer) + messageSize = (U32) (ptr - messageBuffer); - if (messageSize - sizeof (isoEXAC.header) - sizeof (isoEXAC.footer) != isoEXAC.header.MessageLengthU32) - LogMessage(LOG_LEVEL_WARNING, "EXAC message sent with invalid message length"); + if (messageSize - sizeof (isoEXAC.header) - sizeof (isoEXAC.footer) != isoEXAC.header.MessageLengthU32) + LogMessage(LOG_LEVEL_WARNING, "EXAC message sent with invalid message length"); - UtilSendTCPData(MODULE_NAME, messageBuffer, (I32) messageSize, socket, 0); + UtilSendTCPData(MODULE_NAME, messageBuffer, (I32) messageSize, socket, 0); - return (I32) messageSize; + return (I32) messageSize; } @@ -2569,69 +2571,69 @@ I32 ObjectControlSendEXACMessage(EXACData * EXAC, I32 * socket, U8 debug) { * \return Byte size of ACCM struct */ I32 ObjectControlBuildACCMMessage(ACCMData * mqACCMData, ACCMType * ACCM, U8 debug) { - // Header - ACCM->header.SyncWordU16 = SYNC_WORD; - ACCM->header.TransmitterIdU8 = 0; - ACCM->header.MessageCounterU8 = 0; - ACCM->header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; - ACCM->header.MessageIdU16 = COMMAND_ACCM_CODE; - ACCM->header.MessageLengthU32 = sizeof (ACCMType) - sizeof (HeaderType) - sizeof (FooterType); - - // Data fields - ACCM->actionID = mqACCMData->actionID; - ACCM->actionType = mqACCMData->actionType; - ACCM->actionTypeParameter1 = mqACCMData->actionTypeParameter1; - ACCM->actionTypeParameter2 = mqACCMData->actionTypeParameter2; - ACCM->actionTypeParameter3 = mqACCMData->actionTypeParameter3; - - // Value ID fields - ACCM->actionIDValueID = VALUE_ID_ACTION_ID; - ACCM->actionTypeValueID = VALUE_ID_ACTION_TYPE; - ACCM->actionTypeParameter1ValueID = VALUE_ID_ACTION_TYPE_PARAM1; - ACCM->actionTypeParameter2ValueID = VALUE_ID_ACTION_TYPE_PARAM2; - ACCM->actionTypeParameter3ValueID = VALUE_ID_ACTION_TYPE_PARAM3; - - // Content length fields - ACCM->actionIDContentLength = sizeof (ACCM->actionID); - ACCM->actionTypeContentLength = sizeof (ACCM->actionType); - ACCM->actionTypeParameter1ContentLength = sizeof (ACCM->actionTypeParameter1); - ACCM->actionTypeParameter2ContentLength = sizeof (ACCM->actionTypeParameter2); - ACCM->actionTypeParameter3ContentLength = sizeof (ACCM->actionTypeParameter3); - - // Header content length - ACCM->header.MessageLengthU32 = sizeof (ACCM->actionID) + sizeof (ACCM->actionType) - + sizeof (ACCM->actionTypeParameter1) + sizeof (ACCM->actionTypeParameter2) + - sizeof (ACCM->actionTypeParameter3) - + sizeof (ACCM->actionIDValueID) + sizeof (ACCM->actionTypeValueID) - + sizeof (ACCM->actionTypeParameter1ValueID) + sizeof (ACCM->actionTypeParameter1ValueID) + - sizeof (ACCM->actionTypeParameter3ValueID) - + sizeof (ACCM->actionIDContentLength) + sizeof (ACCM->actionTypeContentLength) - + sizeof (ACCM->actionTypeParameter1ContentLength) + - sizeof (ACCM->actionTypeParameter1ContentLength) + sizeof (ACCM->actionTypeParameter3ContentLength); - - - // Footer (TODO) - ACCM->footer.Crc = 0; - - U32 messageLen = - ACCM->header.MessageLengthU32 + sizeof (ACCM->footer.Crc) + sizeof (ACCM->header.SyncWordU16) + - sizeof (ACCM->header.MessageIdU16) + sizeof (ACCM->header.AckReqProtVerU8) + - sizeof (ACCM->header.TransmitterIdU8) + sizeof (ACCM->header.MessageCounterU8) + - sizeof (ACCM->header.MessageLengthU32); - - if (debug) { - LogPrint - ("ACCM (%u bytes):\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x", - messageLen, ACCM->actionIDValueID, ACCM->actionIDContentLength, ACCM->actionID, - ACCM->actionTypeValueID, ACCM->actionTypeContentLength, ACCM->actionType, - ACCM->actionTypeParameter1ValueID, ACCM->actionTypeParameter1ContentLength, - ACCM->actionTypeParameter1, ACCM->actionTypeParameter2ValueID, - ACCM->actionTypeParameter2ContentLength, ACCM->actionTypeParameter2, - ACCM->actionTypeParameter3ValueID, ACCM->actionTypeParameter3ContentLength, - ACCM->actionTypeParameter3); - } - - return (I32) messageLen; + // Header + ACCM->header.SyncWordU16 = SYNC_WORD; + ACCM->header.TransmitterIdU8 = 0; + ACCM->header.MessageCounterU8 = 0; + ACCM->header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; + ACCM->header.MessageIdU16 = COMMAND_ACCM_CODE; + ACCM->header.MessageLengthU32 = sizeof (ACCMType) - sizeof (HeaderType) - sizeof (FooterType); + + // Data fields + ACCM->actionID = mqACCMData->actionID; + ACCM->actionType = mqACCMData->actionType; + ACCM->actionTypeParameter1 = mqACCMData->actionTypeParameter1; + ACCM->actionTypeParameter2 = mqACCMData->actionTypeParameter2; + ACCM->actionTypeParameter3 = mqACCMData->actionTypeParameter3; + + // Value ID fields + ACCM->actionIDValueID = VALUE_ID_ACTION_ID; + ACCM->actionTypeValueID = VALUE_ID_ACTION_TYPE; + ACCM->actionTypeParameter1ValueID = VALUE_ID_ACTION_TYPE_PARAM1; + ACCM->actionTypeParameter2ValueID = VALUE_ID_ACTION_TYPE_PARAM2; + ACCM->actionTypeParameter3ValueID = VALUE_ID_ACTION_TYPE_PARAM3; + + // Content length fields + ACCM->actionIDContentLength = sizeof (ACCM->actionID); + ACCM->actionTypeContentLength = sizeof (ACCM->actionType); + ACCM->actionTypeParameter1ContentLength = sizeof (ACCM->actionTypeParameter1); + ACCM->actionTypeParameter2ContentLength = sizeof (ACCM->actionTypeParameter2); + ACCM->actionTypeParameter3ContentLength = sizeof (ACCM->actionTypeParameter3); + + // Header content length + ACCM->header.MessageLengthU32 = sizeof (ACCM->actionID) + sizeof (ACCM->actionType) + + sizeof (ACCM->actionTypeParameter1) + sizeof (ACCM->actionTypeParameter2) + + sizeof (ACCM->actionTypeParameter3) + + sizeof (ACCM->actionIDValueID) + sizeof (ACCM->actionTypeValueID) + + sizeof (ACCM->actionTypeParameter1ValueID) + sizeof (ACCM->actionTypeParameter1ValueID) + + sizeof (ACCM->actionTypeParameter3ValueID) + + sizeof (ACCM->actionIDContentLength) + sizeof (ACCM->actionTypeContentLength) + + sizeof (ACCM->actionTypeParameter1ContentLength) + + sizeof (ACCM->actionTypeParameter1ContentLength) + sizeof (ACCM->actionTypeParameter3ContentLength); + + + // Footer (TODO) + ACCM->footer.Crc = 0; + + U32 messageLen = + ACCM->header.MessageLengthU32 + sizeof (ACCM->footer.Crc) + sizeof (ACCM->header.SyncWordU16) + + sizeof (ACCM->header.MessageIdU16) + sizeof (ACCM->header.AckReqProtVerU8) + + sizeof (ACCM->header.TransmitterIdU8) + sizeof (ACCM->header.MessageCounterU8) + + sizeof (ACCM->header.MessageLengthU32); + + if (debug) { + LogPrint + ("ACCM (%u bytes):\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x", + messageLen, ACCM->actionIDValueID, ACCM->actionIDContentLength, ACCM->actionID, + ACCM->actionTypeValueID, ACCM->actionTypeContentLength, ACCM->actionType, + ACCM->actionTypeParameter1ValueID, ACCM->actionTypeParameter1ContentLength, + ACCM->actionTypeParameter1, ACCM->actionTypeParameter2ValueID, + ACCM->actionTypeParameter2ContentLength, ACCM->actionTypeParameter2, + ACCM->actionTypeParameter3ValueID, ACCM->actionTypeParameter3ContentLength, + ACCM->actionTypeParameter3); + } + + return (I32) messageLen; } /*! @@ -2642,53 +2644,53 @@ I32 ObjectControlBuildACCMMessage(ACCMData * mqACCMData, ACCMType * ACCM, U8 deb * \return Byte size of EXAC struct */ I32 ObjectControlBuildEXACMessage(EXACData * mqEXACData, EXACType * EXAC, U8 debug) { - // TODO: Make system time follow GPS time (better) or pass the GSD pointer into here somehow - struct timeval systemTime; + // TODO: Make system time follow GPS time (better) or pass the GSD pointer into here somehow + struct timeval systemTime; - TimeSetToCurrentSystemTime(&systemTime); + TimeSetToCurrentSystemTime(&systemTime); - // Header - EXAC->header.SyncWordU16 = SYNC_WORD; - EXAC->header.TransmitterIdU8 = 0; - EXAC->header.MessageCounterU8 = 0; - EXAC->header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; - EXAC->header.MessageIdU16 = COMMAND_EXAC_CODE; + // Header + EXAC->header.SyncWordU16 = SYNC_WORD; + EXAC->header.TransmitterIdU8 = 0; + EXAC->header.MessageCounterU8 = 0; + EXAC->header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; + EXAC->header.MessageIdU16 = COMMAND_EXAC_CODE; - // Data fields - EXAC->actionID = mqEXACData->actionID; - EXAC->executionTime_qmsoW = mqEXACData->executionTime_qmsoW; + // Data fields + EXAC->actionID = mqEXACData->actionID; + EXAC->executionTime_qmsoW = mqEXACData->executionTime_qmsoW; - // Value ID fields - EXAC->actionIDValueID = VALUE_ID_ACTION_ID; - EXAC->executionTime_qmsoWValueID = VALUE_ID_ACTION_EXECUTE_TIME; + // Value ID fields + EXAC->actionIDValueID = VALUE_ID_ACTION_ID; + EXAC->executionTime_qmsoWValueID = VALUE_ID_ACTION_EXECUTE_TIME; - // Content length fields - EXAC->actionIDContentLength = sizeof (EXAC->actionID); - EXAC->executionTime_qmsoWContentLength = sizeof (EXAC->executionTime_qmsoW); + // Content length fields + EXAC->actionIDContentLength = sizeof (EXAC->actionID); + EXAC->executionTime_qmsoWContentLength = sizeof (EXAC->executionTime_qmsoW); - // Header message length - EXAC->header.MessageLengthU32 = sizeof (EXAC->actionID) + sizeof (EXAC->executionTime_qmsoW) - + sizeof (EXAC->actionIDValueID) + sizeof (EXAC->executionTime_qmsoWValueID) - + sizeof (EXAC->actionIDContentLength) + sizeof (EXAC->executionTime_qmsoWContentLength); + // Header message length + EXAC->header.MessageLengthU32 = sizeof (EXAC->actionID) + sizeof (EXAC->executionTime_qmsoW) + + sizeof (EXAC->actionIDValueID) + sizeof (EXAC->executionTime_qmsoWValueID) + + sizeof (EXAC->actionIDContentLength) + sizeof (EXAC->executionTime_qmsoWContentLength); - // Footer (TODO) - EXAC->footer.Crc = 0; + // Footer (TODO) + EXAC->footer.Crc = 0; - U32 messageLen = - EXAC->header.MessageLengthU32 + sizeof (EXAC->footer.Crc) + sizeof (EXAC->header.SyncWordU16) + - sizeof (EXAC->header.MessageIdU16) + sizeof (EXAC->header.AckReqProtVerU8) + - sizeof (EXAC->header.TransmitterIdU8) + sizeof (EXAC->header.MessageCounterU8) + - sizeof (EXAC->header.MessageLengthU32); + U32 messageLen = + EXAC->header.MessageLengthU32 + sizeof (EXAC->footer.Crc) + sizeof (EXAC->header.SyncWordU16) + + sizeof (EXAC->header.MessageIdU16) + sizeof (EXAC->header.AckReqProtVerU8) + + sizeof (EXAC->header.TransmitterIdU8) + sizeof (EXAC->header.MessageCounterU8) + + sizeof (EXAC->header.MessageLengthU32); - if (debug) { - LogPrint("EXAC (%u bytes):\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x", messageLen, - EXAC->actionIDValueID, EXAC->actionIDContentLength, EXAC->actionID, - EXAC->executionTime_qmsoWValueID, EXAC->executionTime_qmsoWContentLength, - EXAC->executionTime_qmsoW); - } + if (debug) { + LogPrint("EXAC (%u bytes):\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x", messageLen, + EXAC->actionIDValueID, EXAC->actionIDContentLength, EXAC->actionID, + EXAC->executionTime_qmsoWValueID, EXAC->executionTime_qmsoWContentLength, + EXAC->executionTime_qmsoW); + } - return (I32) messageLen; + return (I32) messageLen; } /*! @@ -2699,625 +2701,625 @@ I32 ObjectControlBuildEXACMessage(EXACData * mqEXACData, EXACType * EXAC, U8 deb * \return Byte size of TRCM struct */ I32 ObjectControlBuildTRCMMessage(TRCMData * mqTRCMData, TRCMType * TRCM, U8 debug) { - // Header - TRCM->header.SyncWordU16 = SYNC_WORD; - TRCM->header.TransmitterIdU8 = 0; - TRCM->header.MessageCounterU8 = 0; - TRCM->header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; - TRCM->header.MessageIdU16 = COMMAND_TRCM_CODE; - - - // Data fields - TRCM->triggerID = mqTRCMData->triggerID; - TRCM->triggerType = mqTRCMData->triggerType; - TRCM->triggerTypeParameter1 = mqTRCMData->triggerTypeParameter1; - TRCM->triggerTypeParameter2 = mqTRCMData->triggerTypeParameter2; - TRCM->triggerTypeParameter3 = mqTRCMData->triggerTypeParameter3; - - // Value ID fields - TRCM->triggerIDValueID = VALUE_ID_TRIGGER_ID; - TRCM->triggerIDValueID = VALUE_ID_TRIGGER_TYPE; - TRCM->triggerTypeParameter1ValueID = VALUE_ID_TRIGGER_TYPE_PARAM1; - TRCM->triggerTypeParameter2ValueID = VALUE_ID_TRIGGER_TYPE_PARAM2; - TRCM->triggerTypeParameter3ValueID = VALUE_ID_TRIGGER_TYPE_PARAM3; - - // Content length fields - TRCM->triggerIDContentLength = sizeof (TRCM->triggerID); - TRCM->triggerTypeContentLength = sizeof (TRCM->triggerType); - TRCM->triggerTypeParameter1ContentLength = sizeof (TRCM->triggerTypeParameter1); - TRCM->triggerTypeParameter2ContentLength = sizeof (TRCM->triggerTypeParameter2); - TRCM->triggerTypeParameter3ContentLength = sizeof (TRCM->triggerTypeParameter3); - - - // Message length in header - TRCM->header.MessageLengthU32 = sizeof (TRCM->triggerID) + sizeof (TRCM->triggerType) - + sizeof (TRCM->triggerTypeParameter1) + sizeof (TRCM->triggerTypeParameter2) + - sizeof (TRCM->triggerTypeParameter3) - + sizeof (TRCM->triggerIDValueID) + sizeof (TRCM->triggerTypeValueID) - + sizeof (TRCM->triggerTypeParameter1ValueID) + sizeof (TRCM->triggerTypeParameter1ValueID) + - sizeof (TRCM->triggerTypeParameter3ValueID) - + sizeof (TRCM->triggerIDContentLength) + sizeof (TRCM->triggerTypeContentLength) - + sizeof (TRCM->triggerTypeParameter1ContentLength) + - sizeof (TRCM->triggerTypeParameter1ContentLength) + sizeof (TRCM->triggerTypeParameter3ContentLength); - - - // Footer (TODO) - TRCM->footer.Crc = 0; - - U32 messageLen = - TRCM->header.MessageLengthU32 + sizeof (TRCM->footer.Crc) + sizeof (TRCM->header.SyncWordU16) + - sizeof (TRCM->header.MessageIdU16) + sizeof (TRCM->header.AckReqProtVerU8) + - sizeof (TRCM->header.TransmitterIdU8) + sizeof (TRCM->header.MessageCounterU8) + - sizeof (TRCM->header.MessageLengthU32); - if (debug) { - LogPrint - ("TRCM (%u bytes):\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x", - messageLen, TRCM->triggerIDValueID, TRCM->triggerIDContentLength, TRCM->triggerID, - TRCM->triggerTypeValueID, TRCM->triggerTypeContentLength, TRCM->triggerType, - TRCM->triggerTypeParameter1ValueID, TRCM->triggerTypeParameter1ContentLength, - TRCM->triggerTypeParameter1, TRCM->triggerTypeParameter2ValueID, - TRCM->triggerTypeParameter2ContentLength, TRCM->triggerTypeParameter2, - TRCM->triggerTypeParameter3ValueID, TRCM->triggerTypeParameter3ContentLength, - TRCM->triggerTypeParameter3); - } - - return (I32) messageLen; + // Header + TRCM->header.SyncWordU16 = SYNC_WORD; + TRCM->header.TransmitterIdU8 = 0; + TRCM->header.MessageCounterU8 = 0; + TRCM->header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; + TRCM->header.MessageIdU16 = COMMAND_TRCM_CODE; + + + // Data fields + TRCM->triggerID = mqTRCMData->triggerID; + TRCM->triggerType = mqTRCMData->triggerType; + TRCM->triggerTypeParameter1 = mqTRCMData->triggerTypeParameter1; + TRCM->triggerTypeParameter2 = mqTRCMData->triggerTypeParameter2; + TRCM->triggerTypeParameter3 = mqTRCMData->triggerTypeParameter3; + + // Value ID fields + TRCM->triggerIDValueID = VALUE_ID_TRIGGER_ID; + TRCM->triggerIDValueID = VALUE_ID_TRIGGER_TYPE; + TRCM->triggerTypeParameter1ValueID = VALUE_ID_TRIGGER_TYPE_PARAM1; + TRCM->triggerTypeParameter2ValueID = VALUE_ID_TRIGGER_TYPE_PARAM2; + TRCM->triggerTypeParameter3ValueID = VALUE_ID_TRIGGER_TYPE_PARAM3; + + // Content length fields + TRCM->triggerIDContentLength = sizeof (TRCM->triggerID); + TRCM->triggerTypeContentLength = sizeof (TRCM->triggerType); + TRCM->triggerTypeParameter1ContentLength = sizeof (TRCM->triggerTypeParameter1); + TRCM->triggerTypeParameter2ContentLength = sizeof (TRCM->triggerTypeParameter2); + TRCM->triggerTypeParameter3ContentLength = sizeof (TRCM->triggerTypeParameter3); + + + // Message length in header + TRCM->header.MessageLengthU32 = sizeof (TRCM->triggerID) + sizeof (TRCM->triggerType) + + sizeof (TRCM->triggerTypeParameter1) + sizeof (TRCM->triggerTypeParameter2) + + sizeof (TRCM->triggerTypeParameter3) + + sizeof (TRCM->triggerIDValueID) + sizeof (TRCM->triggerTypeValueID) + + sizeof (TRCM->triggerTypeParameter1ValueID) + sizeof (TRCM->triggerTypeParameter1ValueID) + + sizeof (TRCM->triggerTypeParameter3ValueID) + + sizeof (TRCM->triggerIDContentLength) + sizeof (TRCM->triggerTypeContentLength) + + sizeof (TRCM->triggerTypeParameter1ContentLength) + + sizeof (TRCM->triggerTypeParameter1ContentLength) + sizeof (TRCM->triggerTypeParameter3ContentLength); + + + // Footer (TODO) + TRCM->footer.Crc = 0; + + U32 messageLen = + TRCM->header.MessageLengthU32 + sizeof (TRCM->footer.Crc) + sizeof (TRCM->header.SyncWordU16) + + sizeof (TRCM->header.MessageIdU16) + sizeof (TRCM->header.AckReqProtVerU8) + + sizeof (TRCM->header.TransmitterIdU8) + sizeof (TRCM->header.MessageCounterU8) + + sizeof (TRCM->header.MessageLengthU32); + if (debug) { + LogPrint + ("TRCM (%u bytes):\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x", + messageLen, TRCM->triggerIDValueID, TRCM->triggerIDContentLength, TRCM->triggerID, + TRCM->triggerTypeValueID, TRCM->triggerTypeContentLength, TRCM->triggerType, + TRCM->triggerTypeParameter1ValueID, TRCM->triggerTypeParameter1ContentLength, + TRCM->triggerTypeParameter1, TRCM->triggerTypeParameter2ValueID, + TRCM->triggerTypeParameter2ContentLength, TRCM->triggerTypeParameter2, + TRCM->triggerTypeParameter3ValueID, TRCM->triggerTypeParameter3ContentLength, + TRCM->triggerTypeParameter3); + } + + return (I32) messageLen; } I32 ObjectControlSendDTMMessage(C8 * DTMData, I32 * Socket, I32 RowCount, C8 * IP, U32 Port, - DOTMType * DOTMData, U8 debug) { + DOTMType * DOTMData, U8 debug) { - U32 Rest = 0, i = 0, MessageLength = 0, SumMessageLength = 0, Modulo = 0, Transmissions = 0; - U16 CrcU16 = 0; + U32 Rest = 0, i = 0, MessageLength = 0, SumMessageLength = 0, Modulo = 0, Transmissions = 0; + U16 CrcU16 = 0; - MessageLength = - ObjectControlBuildDTMMessage(TrajBuffer, DTMData, COMMAND_DOTM_ROWS_IN_TRANSMISSION, DOTMData, 0); + MessageLength = + ObjectControlBuildDTMMessage(TrajBuffer, DTMData, COMMAND_DOTM_ROWS_IN_TRANSMISSION, DOTMData, 0); - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "Transmission %d: %d bytes sent", i, MessageLength); + if (debug) + LogMessage(LOG_LEVEL_DEBUG, "Transmission %d: %d bytes sent", i, MessageLength); - LogMessage(LOG_LEVEL_INFO, "%d DTM bytes sent to %s:%d", SumMessageLength, IP, Port); + LogMessage(LOG_LEVEL_INFO, "%d DTM bytes sent to %s:%d", SumMessageLength, IP, Port); - return 0; + return 0; } I32 ObjectControlBuildDTMMessage(C8 * MessageBuffer, C8 * DTMData, I32 RowCount, DOTMType * DOTMData, - U8 debug) { - I32 MessageIndex = 0; - U32 Data; - C8 *src, *p; - U16 Crc = 0; - - bzero(MessageBuffer, COMMAND_DOTM_ROW_MESSAGE_LENGTH * RowCount); - - I32 i = 0, j = 0, n = 0; - - for (i = 0; i < RowCount; i++) { - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "DOTM row:"); - //Time - Data = 0; - Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 3); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 2); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 1); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 0); - DOTMData->RelativeTimeValueIdU16 = VALUE_ID_RELATIVE_TIME; - DOTMData->RelativeTimeContentLengthU16 = 4; - DOTMData->RelativeTimeU32 = SwapU32((U32) Data); - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "Time=%d", DOTMData->RelativeTimeU32); - - //x - Data = 0; - Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 7); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 6); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 5); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 4); - DOTMData->XPositionValueIdU16 = VALUE_ID_X_POSITION; - DOTMData->XPositionContentLengthU16 = 4; - DOTMData->XPositionI32 = SwapI32((I32) Data); - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "X=%d", DOTMData->XPositionI32); - - //y - Data = 0; - Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 11); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 10); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 9); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 8); - DOTMData->YPositionValueIdU16 = VALUE_ID_Y_POSITION; - DOTMData->YPositionContentLengthU16 = 4; - DOTMData->YPositionI32 = SwapI32((I32) Data); - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "Y=%d", DOTMData->YPositionI32); - - //z - Data = 0; - Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 15); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 14); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 13); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 12); - DOTMData->ZPositionValueIdU16 = VALUE_ID_Z_POSITION; - DOTMData->ZPositionContentLengthU16 = 4; - DOTMData->ZPositionI32 = SwapI32((I32) Data); - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "Z=%d", DOTMData->ZPositionI32); - - //Heading - Data = 0; - Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 17); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 16); - //Data = UtilRadToDeg(Data); - //Data = 4500 - Data; //Turn heading back pi/2 - //while(Data<0) Data+=360.0; - //while(Data>3600) Data-=360.0; - DOTMData->HeadingValueIdU16 = VALUE_ID_HEADING; - DOTMData->HeadingContentLengthU16 = 2; - DOTMData->HeadingU16 = SwapU16((U16) (Data)); - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "Heading=%d", DOTMData->HeadingU16); - - //Longitudinal speed - Data = 0; - Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 19); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 18); - DOTMData->LongitudinalSpeedValueIdU16 = VALUE_ID_LONGITUDINAL_SPEED; - DOTMData->LongitudinalSpeedContentLengthU16 = 2; - DOTMData->LongitudinalSpeedI16 = SwapI16((I16) Data); - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "LongitudinalSpeedI16=%d", DOTMData->LongitudinalSpeedI16); - - //Lateral speed - Data = 0; - Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 21); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 20); - DOTMData->LateralSpeedValueIdU16 = VALUE_ID_LATERAL_SPEED; - DOTMData->LateralSpeedContentLengthU16 = 2; - DOTMData->LateralSpeedI16 = SwapI16((I16) Data); - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "LateralSpeedI16=%d", DOTMData->LateralSpeedI16); - - //Longitudinal acceleration - Data = 0; - Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 23); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 22); - DOTMData->LongitudinalAccValueIdU16 = VALUE_ID_LONGITUDINAL_ACCELERATION; - DOTMData->LongitudinalAccContentLengthU16 = 2; - DOTMData->LongitudinalAccI16 = SwapI16((I16) Data); - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "LongitudinalAccI16=%d", DOTMData->LongitudinalAccI16); - - //Lateral acceleration - Data = 0; - Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 25); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 24); - DOTMData->LateralAccValueIdU16 = VALUE_ID_LATERAL_ACCELERATION; - DOTMData->LateralAccContentLengthU16 = 2; - DOTMData->LateralAccI16 = SwapI16((I16) Data); - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "LateralAccI16=%d", DOTMData->LateralAccI16); - - //Curvature - Data = 0; - Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 29); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 28); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 27); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 26); - DOTMData->CurvatureValueIdU16 = VALUE_ID_CURVATURE; - DOTMData->CurvatureContentLengthU16 = 4; - DOTMData->CurvatureI32 = SwapI32((I32) Data); - if (debug) - printf("CurvatureI32=%d \n", DOTMData->CurvatureI32); - - p = (C8 *) DOTMData; - for (j = 0; j < sizeof (DOTMType); j++, n++) - *(MessageBuffer + n) = *p++; - MessageIndex = n; - } - - - Crc = crc_16((const C8 *)MessageBuffer, sizeof (DOTMType)); - Crc = 0; - *(MessageBuffer + MessageIndex++) = (U8) (Crc); - *(MessageBuffer + MessageIndex++) = (U8) (Crc >> 8); - - - if (debug) { - int i = 0; - - for (i = 0; i < MessageIndex; i++) { - // TODO: Write to log when bytes thingy has been implemented - if ((unsigned char)MessageBuffer[i] >= 0 && (unsigned char)MessageBuffer[i] <= 15) - printf("0"); - printf("%x-", (unsigned char)MessageBuffer[i]); - } - printf("\n"); - } - - return MessageIndex; //Total number of bytes + U8 debug) { + I32 MessageIndex = 0; + U32 Data; + C8 *src, *p; + U16 Crc = 0; + + bzero(MessageBuffer, COMMAND_DOTM_ROW_MESSAGE_LENGTH * RowCount); + + I32 i = 0, j = 0, n = 0; + + for (i = 0; i < RowCount; i++) { + if (debug) + LogMessage(LOG_LEVEL_DEBUG, "DOTM row:"); + //Time + Data = 0; + Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 3); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 2); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 1); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 0); + DOTMData->RelativeTimeValueIdU16 = VALUE_ID_RELATIVE_TIME; + DOTMData->RelativeTimeContentLengthU16 = 4; + DOTMData->RelativeTimeU32 = SwapU32((U32) Data); + if (debug) + LogMessage(LOG_LEVEL_DEBUG, "Time=%d", DOTMData->RelativeTimeU32); + + //x + Data = 0; + Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 7); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 6); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 5); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 4); + DOTMData->XPositionValueIdU16 = VALUE_ID_X_POSITION; + DOTMData->XPositionContentLengthU16 = 4; + DOTMData->XPositionI32 = SwapI32((I32) Data); + if (debug) + LogMessage(LOG_LEVEL_DEBUG, "X=%d", DOTMData->XPositionI32); + + //y + Data = 0; + Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 11); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 10); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 9); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 8); + DOTMData->YPositionValueIdU16 = VALUE_ID_Y_POSITION; + DOTMData->YPositionContentLengthU16 = 4; + DOTMData->YPositionI32 = SwapI32((I32) Data); + if (debug) + LogMessage(LOG_LEVEL_DEBUG, "Y=%d", DOTMData->YPositionI32); + + //z + Data = 0; + Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 15); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 14); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 13); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 12); + DOTMData->ZPositionValueIdU16 = VALUE_ID_Z_POSITION; + DOTMData->ZPositionContentLengthU16 = 4; + DOTMData->ZPositionI32 = SwapI32((I32) Data); + if (debug) + LogMessage(LOG_LEVEL_DEBUG, "Z=%d", DOTMData->ZPositionI32); + + //Heading + Data = 0; + Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 17); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 16); + //Data = UtilRadToDeg(Data); + //Data = 4500 - Data; //Turn heading back pi/2 + //while(Data<0) Data+=360.0; + //while(Data>3600) Data-=360.0; + DOTMData->HeadingValueIdU16 = VALUE_ID_HEADING; + DOTMData->HeadingContentLengthU16 = 2; + DOTMData->HeadingU16 = SwapU16((U16) (Data)); + if (debug) + LogMessage(LOG_LEVEL_DEBUG, "Heading=%d", DOTMData->HeadingU16); + + //Longitudinal speed + Data = 0; + Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 19); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 18); + DOTMData->LongitudinalSpeedValueIdU16 = VALUE_ID_LONGITUDINAL_SPEED; + DOTMData->LongitudinalSpeedContentLengthU16 = 2; + DOTMData->LongitudinalSpeedI16 = SwapI16((I16) Data); + if (debug) + LogMessage(LOG_LEVEL_DEBUG, "LongitudinalSpeedI16=%d", DOTMData->LongitudinalSpeedI16); + + //Lateral speed + Data = 0; + Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 21); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 20); + DOTMData->LateralSpeedValueIdU16 = VALUE_ID_LATERAL_SPEED; + DOTMData->LateralSpeedContentLengthU16 = 2; + DOTMData->LateralSpeedI16 = SwapI16((I16) Data); + if (debug) + LogMessage(LOG_LEVEL_DEBUG, "LateralSpeedI16=%d", DOTMData->LateralSpeedI16); + + //Longitudinal acceleration + Data = 0; + Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 23); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 22); + DOTMData->LongitudinalAccValueIdU16 = VALUE_ID_LONGITUDINAL_ACCELERATION; + DOTMData->LongitudinalAccContentLengthU16 = 2; + DOTMData->LongitudinalAccI16 = SwapI16((I16) Data); + if (debug) + LogMessage(LOG_LEVEL_DEBUG, "LongitudinalAccI16=%d", DOTMData->LongitudinalAccI16); + + //Lateral acceleration + Data = 0; + Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 25); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 24); + DOTMData->LateralAccValueIdU16 = VALUE_ID_LATERAL_ACCELERATION; + DOTMData->LateralAccContentLengthU16 = 2; + DOTMData->LateralAccI16 = SwapI16((I16) Data); + if (debug) + LogMessage(LOG_LEVEL_DEBUG, "LateralAccI16=%d", DOTMData->LateralAccI16); + + //Curvature + Data = 0; + Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 29); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 28); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 27); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 26); + DOTMData->CurvatureValueIdU16 = VALUE_ID_CURVATURE; + DOTMData->CurvatureContentLengthU16 = 4; + DOTMData->CurvatureI32 = SwapI32((I32) Data); + if (debug) + printf("CurvatureI32=%d \n", DOTMData->CurvatureI32); + + p = (C8 *) DOTMData; + for (j = 0; j < sizeof (DOTMType); j++, n++) + *(MessageBuffer + n) = *p++; + MessageIndex = n; + } + + + Crc = crc_16((const C8 *)MessageBuffer, sizeof (DOTMType)); + Crc = 0; + *(MessageBuffer + MessageIndex++) = (U8) (Crc); + *(MessageBuffer + MessageIndex++) = (U8) (Crc >> 8); + + + if (debug) { + int i = 0; + + for (i = 0; i < MessageIndex; i++) { + // TODO: Write to log when bytes thingy has been implemented + if ((unsigned char)MessageBuffer[i] >= 0 && (unsigned char)MessageBuffer[i] <= 15) + printf("0"); + printf("%x-", (unsigned char)MessageBuffer[i]); + } + printf("\n"); + } + + return MessageIndex; //Total number of bytes } static int iGetObjectIndexFromObjectIP(in_addr_t ipAddr, in_addr_t objectIPs[], unsigned int numberOfObjects) { - for (unsigned int i = 0; i < numberOfObjects; ++i) { - if (objectIPs[i] == ipAddr) - return (int)i; - } - return -1; + for (unsigned int i = 0; i < numberOfObjects; ++i) { + if (objectIPs[i] == ipAddr) + return (int)i; + } + return -1; } static I32 vConnectObject(int *sockfd, const char *name, const uint32_t port, U8 * Disconnect) { - struct sockaddr_in serv_addr; - struct hostent *server; + struct sockaddr_in serv_addr; + struct hostent *server; - char buffer[256]; - int iResult; + char buffer[256]; + int iResult; - *sockfd = socket(AF_INET, SOCK_STREAM, 0); + *sockfd = socket(AF_INET, SOCK_STREAM, 0); - if (*sockfd < 0) { - util_error("[ObjectControl] ERR: Failed to open control socket"); - } + if (*sockfd < 0) { + util_error("[ObjectControl] ERR: Failed to open control socket"); + } - server = gethostbyname(name); - if (server == NULL) { - util_error("[ObjectControl] ERR: Unknown host "); - } + server = gethostbyname(name); + if (server == NULL) { + util_error("[ObjectControl] ERR: Unknown host "); + } - bzero((char *)&serv_addr, sizeof (serv_addr)); - serv_addr.sin_family = AF_INET; + bzero((char *)&serv_addr, sizeof (serv_addr)); + serv_addr.sin_family = AF_INET; - bcopy((char *)server->h_addr, (char *)&serv_addr.sin_addr.s_addr, server->h_length); - serv_addr.sin_port = htons(port); + bcopy((char *)server->h_addr, (char *)&serv_addr.sin_addr.s_addr, server->h_length); + serv_addr.sin_port = htons(port); - LogMessage(LOG_LEVEL_INFO, "Attempting to connect to socket: %s %i", name, port); + LogMessage(LOG_LEVEL_INFO, "Attempting to connect to socket: %s %i", name, port); - // do - { - iResult = connect(*sockfd, (struct sockaddr *)&serv_addr, sizeof (serv_addr)); + // do + { + iResult = connect(*sockfd, (struct sockaddr *)&serv_addr, sizeof (serv_addr)); - /*if ( iResult < 0) - { - if(errno == ECONNREFUSED) - { - printf("WARNiNG: Was not able to connect to object, [IP: %s] [PORT: %d], %d retry in 3 sec...\n",name,port, *Disconnect); - fflush(stdout); - (void)sleep(3); - } - else - { - util_error("ERR: Failed to connect to control socket"); - } */ - } - //} while(iResult < 0 && *Disconnect == 0); + /*if ( iResult < 0) + { + if(errno == ECONNREFUSED) + { + printf("WARNiNG: Was not able to connect to object, [IP: %s] [PORT: %d], %d retry in 3 sec...\n",name,port, *Disconnect); + fflush(stdout); + (void)sleep(3); + } + else + { + util_error("ERR: Failed to connect to control socket"); + } */ + } + //} while(iResult < 0 && *Disconnect == 0); - LogMessage(LOG_LEVEL_INFO, "Connected to command socket: %s %i", name, port); - // Enable polling of status to detect remote disconnect - fcntl(*sockfd, F_SETFL, O_NONBLOCK); + LogMessage(LOG_LEVEL_INFO, "Connected to command socket: %s %i", name, port); + // Enable polling of status to detect remote disconnect + fcntl(*sockfd, F_SETFL, O_NONBLOCK); - return iResult; + return iResult; } static void vDisconnectObject(int *sockfd) { - close(*sockfd); + close(*sockfd); } static void vCreateSafetyChannel(const char *name, const uint32_t port, int *sockfd, struct sockaddr_in *addr) { - int result; - struct hostent *object; + int result; + struct hostent *object; - /* Connect to object safety socket */ - LogMessage(LOG_LEVEL_DEBUG, "Creating safety socket"); + /* Connect to object safety socket */ + LogMessage(LOG_LEVEL_DEBUG, "Creating safety socket"); - *sockfd = socket(AF_INET, SOCK_DGRAM, 0); - if (*sockfd < 0) { - util_error("ERR: Failed to connect to monitor socket"); - } + *sockfd = socket(AF_INET, SOCK_DGRAM, 0); + if (*sockfd < 0) { + util_error("ERR: Failed to connect to monitor socket"); + } - /* Set address to object */ - object = gethostbyname(name); + /* Set address to object */ + object = gethostbyname(name); - if (object == 0) { - util_error("ERR: Unknown host"); - } + if (object == 0) { + util_error("ERR: Unknown host"); + } - bcopy((char *)object->h_addr, (char *)&addr->sin_addr.s_addr, object->h_length); - addr->sin_family = AF_INET; - addr->sin_port = htons(port); + bcopy((char *)object->h_addr, (char *)&addr->sin_addr.s_addr, object->h_length); + addr->sin_family = AF_INET; + addr->sin_port = htons(port); - /* set socket to non-blocking */ - result = fcntl(*sockfd, F_SETFL, fcntl(*sockfd, F_GETFL, 0) | O_NONBLOCK); - if (result < 0) { - util_error("ERR: calling fcntl"); - } + /* set socket to non-blocking */ + result = fcntl(*sockfd, F_SETFL, fcntl(*sockfd, F_GETFL, 0) | O_NONBLOCK); + if (result < 0) { + util_error("ERR: calling fcntl"); + } - LogMessage(LOG_LEVEL_INFO, "Created socket and safety address: %s:%d", name, port); + LogMessage(LOG_LEVEL_INFO, "Created socket and safety address: %s:%d", name, port); } static void vCloseSafetyChannel(int *sockfd) { - close(*sockfd); + close(*sockfd); } static I32 vCheckRemoteDisconnected(int *sockfd) { - char dummy; - ssize_t x = recv(*sockfd, &dummy, 1, MSG_PEEK); - - // Remote has disconnected: EOF => x=0 - if (x == 0) { - return 1; - } - - if (x == -1) { - // Everything is normal - no communication has been received - if (errno == EAGAIN || errno == EWOULDBLOCK) - return 0; - - // Other error occurred - LogMessage(LOG_LEVEL_WARNING, "Error when checking connection status"); - return 1; - } - - // Something has been received on socket - if (x > 0) { - LogMessage(LOG_LEVEL_INFO, "Received unexpected communication from object on command channel"); - return 0; - } - - return 1; + char dummy; + ssize_t x = recv(*sockfd, &dummy, 1, MSG_PEEK); + + // Remote has disconnected: EOF => x=0 + if (x == 0) { + return 1; + } + + if (x == -1) { + // Everything is normal - no communication has been received + if (errno == EAGAIN || errno == EWOULDBLOCK) + return 0; + + // Other error occurred + LogMessage(LOG_LEVEL_WARNING, "Error when checking connection status"); + return 1; + } + + // Something has been received on socket + if (x > 0) { + LogMessage(LOG_LEVEL_INFO, "Received unexpected communication from object on command channel"); + return 0; + } + + return 1; } int ObjectControlSendUDPData(int *sockfd, struct sockaddr_in *addr, char *SendData, int Length, char debug) { - ssize_t result; + ssize_t result; - // TODO: Change to log write when bytes thingy has been implemented - if (debug) { - printf("Bytes sent: "); - int i = 0; + // TODO: Change to log write when bytes thingy has been implemented + if (debug) { + printf("Bytes sent: "); + int i = 0; - for (i = 0; i < Length; i++) - printf("%x-", (unsigned char)*(SendData + i)); - printf("\n"); - } + for (i = 0; i < Length; i++) + printf("%x-", (unsigned char)*(SendData + i)); + printf("\n"); + } - result = sendto(*sockfd, SendData, Length, 0, (const struct sockaddr *)addr, sizeof (struct sockaddr_in)); + result = sendto(*sockfd, SendData, Length, 0, (const struct sockaddr *)addr, sizeof (struct sockaddr_in)); - if (result < 0) { - util_error("ERR: Failed to send on monitor socket"); - } + if (result < 0) { + util_error("ERR: Failed to send on monitor socket"); + } - return 0; + return 0; } static size_t uiRecvMonitor(int *sockfd, char *buffer, size_t length) { - ssize_t result = 0; - size_t recvDataSize = 0; - - // Read until receive buffer is empty, return last read message - do { - result = recv(*sockfd, buffer, length, 0); - - if (result < 0) { - if (errno != EAGAIN && errno != EWOULDBLOCK) { - util_error("Failed to receive from monitor socket"); - } - } - else { - recvDataSize = (size_t) (result); - LogMessage(LOG_LEVEL_DEBUG, "Received: <%s>", buffer); - } - } while (result > 0); - - return recvDataSize; + ssize_t result = 0; + size_t recvDataSize = 0; + + // Read until receive buffer is empty, return last read message + do { + result = recv(*sockfd, buffer, length, 0); + + if (result < 0) { + if (errno != EAGAIN && errno != EWOULDBLOCK) { + util_error("Failed to receive from monitor socket"); + } + } + else { + recvDataSize = (size_t) (result); + LogMessage(LOG_LEVEL_DEBUG, "Received: <%s>", buffer); + } + } while (result > 0); + + return recvDataSize; } int iFindObjectsInfo(C8 object_traj_file[MAX_OBJECTS][MAX_FILE_PATH], - C8 object_address_name[MAX_OBJECTS][MAX_FILE_PATH], in_addr_t objectIPs[MAX_OBJECTS], - I32 * nbr_objects) { - DIR *traj_directory; - struct dirent *directory_entry; - int iForceObjectToLocalhost; - struct sockaddr_in sockaddr; - int result; - char trajPathDir[MAX_FILE_PATH]; - int retval = 0; - - UtilGetTrajDirectoryPath(trajPathDir, sizeof (trajPathDir)); - - iForceObjectToLocalhost = 0; - - traj_directory = opendir(trajPathDir); - if (traj_directory == NULL) { - util_error("ERR: Failed to open trajectory directory"); - } - - (void)iUtilGetIntParaConfFile("ForceObjectToLocalhost", &iForceObjectToLocalhost); - - while ((directory_entry = readdir(traj_directory)) && ((*nbr_objects) < MAX_OBJECTS)) { - - /* Check so it's not . or .. */ - if (strncmp(directory_entry->d_name, ".", 1) && (strstr(directory_entry->d_name, "sync") == NULL)) { - bzero(object_address_name[(*nbr_objects)], MAX_FILE_PATH); - - bzero(object_traj_file[(*nbr_objects)], MAX_FILE_PATH); - (void)strcat(object_traj_file[(*nbr_objects)], trajPathDir); - (void)strcat(object_traj_file[(*nbr_objects)], directory_entry->d_name); - - if (UtilCheckTrajectoryFileFormat - (object_traj_file[*nbr_objects], sizeof (object_traj_file[*nbr_objects]))) { - LogMessage(LOG_LEVEL_ERROR, "Trajectory file <%s> is not valid", - object_traj_file[*nbr_objects]); - retval = -1; - } - - if (0 == iForceObjectToLocalhost) { - (void)strncat(object_address_name[(*nbr_objects)], directory_entry->d_name, - strlen(directory_entry->d_name)); - result = inet_pton(AF_INET, object_address_name[*nbr_objects], &sockaddr.sin_addr); - if (result == -1) { - LogMessage(LOG_LEVEL_ERROR, "Invalid address family"); - retval = -1; - continue; - } - else if (result == 0) { - LogMessage(LOG_LEVEL_WARNING, "Address <%s> is not a valid IPv4 address", - object_address_name[*nbr_objects]); - retval = -1; - continue; - } - else - objectIPs[*nbr_objects] = sockaddr.sin_addr.s_addr; - } - else { - if (USE_TEST_HOST == 0) - (void)strcat(object_address_name[(*nbr_objects)], LOCALHOST); - else if (USE_TEST_HOST == 1) - (void)strcat(object_address_name[(*nbr_objects)], TESTHOST_IP); - - } - - ++(*nbr_objects); - } - } - (void)closedir(traj_directory); - return retval; + C8 object_address_name[MAX_OBJECTS][MAX_FILE_PATH], in_addr_t objectIPs[MAX_OBJECTS], + I32 * nbr_objects) { + DIR *traj_directory; + struct dirent *directory_entry; + int iForceObjectToLocalhost; + struct sockaddr_in sockaddr; + int result; + char trajPathDir[MAX_FILE_PATH]; + int retval = 0; + + UtilGetTrajDirectoryPath(trajPathDir, sizeof (trajPathDir)); + + iForceObjectToLocalhost = 0; + + traj_directory = opendir(trajPathDir); + if (traj_directory == NULL) { + util_error("ERR: Failed to open trajectory directory"); + } + + (void)iUtilGetIntParaConfFile("ForceObjectToLocalhost", &iForceObjectToLocalhost); + + while ((directory_entry = readdir(traj_directory)) && ((*nbr_objects) < MAX_OBJECTS)) { + + /* Check so it's not . or .. */ + if (strncmp(directory_entry->d_name, ".", 1) && (strstr(directory_entry->d_name, "sync") == NULL)) { + bzero(object_address_name[(*nbr_objects)], MAX_FILE_PATH); + + bzero(object_traj_file[(*nbr_objects)], MAX_FILE_PATH); + (void)strcat(object_traj_file[(*nbr_objects)], trajPathDir); + (void)strcat(object_traj_file[(*nbr_objects)], directory_entry->d_name); + + if (UtilCheckTrajectoryFileFormat + (object_traj_file[*nbr_objects], sizeof (object_traj_file[*nbr_objects]))) { + LogMessage(LOG_LEVEL_ERROR, "Trajectory file <%s> is not valid", + object_traj_file[*nbr_objects]); + retval = -1; + } + + if (0 == iForceObjectToLocalhost) { + (void)strncat(object_address_name[(*nbr_objects)], directory_entry->d_name, + strlen(directory_entry->d_name)); + result = inet_pton(AF_INET, object_address_name[*nbr_objects], &sockaddr.sin_addr); + if (result == -1) { + LogMessage(LOG_LEVEL_ERROR, "Invalid address family"); + retval = -1; + continue; + } + else if (result == 0) { + LogMessage(LOG_LEVEL_WARNING, "Address <%s> is not a valid IPv4 address", + object_address_name[*nbr_objects]); + retval = -1; + continue; + } + else + objectIPs[*nbr_objects] = sockaddr.sin_addr.s_addr; + } + else { + if (USE_TEST_HOST == 0) + (void)strcat(object_address_name[(*nbr_objects)], LOCALHOST); + else if (USE_TEST_HOST == 1) + (void)strcat(object_address_name[(*nbr_objects)], TESTHOST_IP); + + } + + ++(*nbr_objects); + } + } + (void)closedir(traj_directory); + return retval; } OBCState_t vGetState(GSDType * GSD) { - return DataDictionaryGetOBCStateU8(GSD); + return DataDictionaryGetOBCStateU8(GSD); } StateTransitionResult vSetState(OBCState_t requestedState, GSDType * GSD) { - StateTransition transitionFunction; - StateTransitionResult retval = TRANSITION_RESULT_UNDEFINED; - OBCState_t currentState = DataDictionaryGetOBCStateU8(GSD); - - // Always allow transitions to these two states - if (requestedState == OBC_STATE_ERROR || requestedState == OBC_STATE_UNDEFINED) { - if (DataDictionarySetOBCStateU8(GSD, requestedState) == WRITE_OK) { - LogMessage(LOG_LEVEL_WARNING, "Transitioning to state %u", (unsigned char)requestedState); - retval = TRANSITION_OK; - } - else - retval = TRANSITION_MEMORY_ERROR; - } - else if (requestedState == currentState) { - retval = TRANSITION_OK; - } - else { - transitionFunction = tGetTransition(currentState); - retval = transitionFunction(¤tState, requestedState); - if (retval != TRANSITION_INVALID) { - if (DataDictionarySetOBCStateU8(GSD, currentState) == WRITE_OK) { - LogMessage(LOG_LEVEL_INFO, "Transitioning to state %u", (unsigned char)requestedState); - retval = TRANSITION_OK; - } - else - retval = TRANSITION_MEMORY_ERROR; - } - } - - if (retval == TRANSITION_INVALID) { - LogMessage(LOG_LEVEL_WARNING, "Invalid transition requested: from %d to %d", currentState, - requestedState); - } - else if (retval == TRANSITION_MEMORY_ERROR) { - LogMessage(LOG_LEVEL_ERROR, "Unable to set state to %u in shared memory!!", requestedState); - } - return retval; + StateTransition transitionFunction; + StateTransitionResult retval = TRANSITION_RESULT_UNDEFINED; + OBCState_t currentState = DataDictionaryGetOBCStateU8(GSD); + + // Always allow transitions to these two states + if (requestedState == OBC_STATE_ERROR || requestedState == OBC_STATE_UNDEFINED) { + if (DataDictionarySetOBCStateU8(GSD, requestedState) == WRITE_OK) { + LogMessage(LOG_LEVEL_WARNING, "Transitioning to state %u", (unsigned char)requestedState); + retval = TRANSITION_OK; + } + else + retval = TRANSITION_MEMORY_ERROR; + } + else if (requestedState == currentState) { + retval = TRANSITION_OK; + } + else { + transitionFunction = tGetTransition(currentState); + retval = transitionFunction(¤tState, requestedState); + if (retval != TRANSITION_INVALID) { + if (DataDictionarySetOBCStateU8(GSD, currentState) == WRITE_OK) { + LogMessage(LOG_LEVEL_INFO, "Transitioning to state %u", (unsigned char)requestedState); + retval = TRANSITION_OK; + } + else + retval = TRANSITION_MEMORY_ERROR; + } + } + + if (retval == TRANSITION_INVALID) { + LogMessage(LOG_LEVEL_WARNING, "Invalid transition requested: from %d to %d", currentState, + requestedState); + } + else if (retval == TRANSITION_MEMORY_ERROR) { + LogMessage(LOG_LEVEL_ERROR, "Unable to set state to %u in shared memory!!", requestedState); + } + return retval; } StateTransition tGetTransition(OBCState_t fromState) { - switch (fromState) { - case OBC_STATE_IDLE: - return &tFromIdle; - case OBC_STATE_INITIALIZED: - return &tFromInitialized; - case OBC_STATE_CONNECTED: - return &tFromConnected; - case OBC_STATE_ARMED: - return &tFromArmed; - case OBC_STATE_RUNNING: - return &tFromRunning; - case OBC_STATE_ERROR: - return &tFromError; - case OBC_STATE_UNDEFINED: - return &tFromUndefined; - } + switch (fromState) { + case OBC_STATE_IDLE: + return &tFromIdle; + case OBC_STATE_INITIALIZED: + return &tFromInitialized; + case OBC_STATE_CONNECTED: + return &tFromConnected; + case OBC_STATE_ARMED: + return &tFromArmed; + case OBC_STATE_RUNNING: + return &tFromRunning; + case OBC_STATE_ERROR: + return &tFromError; + case OBC_STATE_UNDEFINED: + return &tFromUndefined; + } } StateTransitionResult tFromIdle(OBCState_t * currentState, OBCState_t requestedState) { - if (requestedState == OBC_STATE_INITIALIZED) { - *currentState = requestedState; - return TRANSITION_OK; - } - return TRANSITION_INVALID; + if (requestedState == OBC_STATE_INITIALIZED) { + *currentState = requestedState; + return TRANSITION_OK; + } + return TRANSITION_INVALID; } StateTransitionResult tFromInitialized(OBCState_t * currentState, OBCState_t requestedState) { - if (requestedState == OBC_STATE_CONNECTED || requestedState == OBC_STATE_IDLE) { - *currentState = requestedState; - return TRANSITION_OK; - } - return TRANSITION_INVALID; + if (requestedState == OBC_STATE_CONNECTED || requestedState == OBC_STATE_IDLE) { + *currentState = requestedState; + return TRANSITION_OK; + } + return TRANSITION_INVALID; } StateTransitionResult tFromConnected(OBCState_t * currentState, OBCState_t requestedState) { - if (requestedState == OBC_STATE_ARMED || requestedState == OBC_STATE_IDLE) { - *currentState = requestedState; - return TRANSITION_OK; - } - return TRANSITION_INVALID; + if (requestedState == OBC_STATE_ARMED || requestedState == OBC_STATE_IDLE) { + *currentState = requestedState; + return TRANSITION_OK; + } + return TRANSITION_INVALID; } StateTransitionResult tFromArmed(OBCState_t * currentState, OBCState_t requestedState) { - if (requestedState == OBC_STATE_CONNECTED || requestedState == OBC_STATE_RUNNING - || requestedState == OBC_STATE_IDLE) { - *currentState = requestedState; - return TRANSITION_OK; - } - return TRANSITION_INVALID; + if (requestedState == OBC_STATE_CONNECTED || requestedState == OBC_STATE_RUNNING + || requestedState == OBC_STATE_IDLE) { + *currentState = requestedState; + return TRANSITION_OK; + } + return TRANSITION_INVALID; } StateTransitionResult tFromRunning(OBCState_t * currentState, OBCState_t requestedState) { - if (requestedState == OBC_STATE_CONNECTED || requestedState == OBC_STATE_IDLE) { - *currentState = requestedState; - return TRANSITION_OK; - } - return TRANSITION_INVALID; + if (requestedState == OBC_STATE_CONNECTED || requestedState == OBC_STATE_IDLE) { + *currentState = requestedState; + return TRANSITION_OK; + } + return TRANSITION_INVALID; } StateTransitionResult tFromError(OBCState_t * currentState, OBCState_t requestedState) { - if (requestedState == OBC_STATE_IDLE) { - *currentState = requestedState; - return TRANSITION_OK; - } - return TRANSITION_INVALID; + if (requestedState == OBC_STATE_IDLE) { + *currentState = requestedState; + return TRANSITION_OK; + } + return TRANSITION_INVALID; } StateTransitionResult tFromUndefined(OBCState_t * currentState, OBCState_t requestedState) { - return TRANSITION_INVALID; + return TRANSITION_INVALID; } OBCState_t vInitializeState(OBCState_t firstState, GSDType * GSD) { - static int8_t isInitialized = 0; - - if (!isInitialized) { - isInitialized = 1; - if (DataDictionarySetOBCStateU8(GSD, firstState) != WRITE_OK) - util_error("Unable to write object control state to shared memory"); - } - else { - LogMessage(LOG_LEVEL_WARNING, "Object control state already initialized"); - } - return DataDictionaryGetOBCStateU8(GSD); + static int8_t isInitialized = 0; + + if (!isInitialized) { + isInitialized = 1; + if (DataDictionarySetOBCStateU8(GSD, firstState) != WRITE_OK) + util_error("Unable to write object control state to shared memory"); + } + else { + LogMessage(LOG_LEVEL_WARNING, "Object control state already initialized"); + } + return DataDictionaryGetOBCStateU8(GSD); } diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index 4947adc5a..395172b18 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -41,13 +41,13 @@ -- Defines ------------------------------------------------------------*/ typedef enum { - SERVER_STATE_UNDEFINED, - SERVER_STATE_INITIALIZED, - SERVER_STATE_IDLE, - SERVER_STATE_READY, - SERVER_STATE_RUNNING, - SERVER_STATE_INWORK, - SERVER_STATE_ERROR, + SERVER_STATE_UNDEFINED, + SERVER_STATE_INITIALIZED, + SERVER_STATE_IDLE, + SERVER_STATE_READY, + SERVER_STATE_RUNNING, + SERVER_STATE_INWORK, + SERVER_STATE_ERROR, } ServerState_t; @@ -116,28 +116,28 @@ typedef enum { typedef enum { - Idle_0, GetServerStatus_0, ArmScenario_0, DisarmScenario_0, StartScenario_1, stop_0, AbortScenario_0, - InitializeScenario_0, - ConnectObject_0, DisconnectObject_0, GetServerParameterList_0, SetServerParameter_2, GetServerParameter_1, - DownloadFile_1, UploadFile_3, CheckFileDirectoryExist_1, GetDirectoryContent_1, - DeleteFileDirectory_1, CreateDirectory_1, GetTestOrigin_0, replay_1, control_0, Exit_0, start_ext_trigg_1, - nocommand + Idle_0, GetServerStatus_0, ArmScenario_0, DisarmScenario_0, StartScenario_1, stop_0, AbortScenario_0, + InitializeScenario_0, + ConnectObject_0, DisconnectObject_0, GetServerParameterList_0, SetServerParameter_2, GetServerParameter_1, + DownloadFile_1, UploadFile_3, CheckFileDirectoryExist_1, GetDirectoryContent_1, + DeleteFileDirectory_1, CreateDirectory_1, GetTestOrigin_0, replay_1, control_0, Exit_0, start_ext_trigg_1, + nocommand } SystemControlCommand_t; const char *SystemControlCommandsArr[] = { - "Idle_0", "GetServerStatus_0", "ArmScenario_0", "DisarmScenario_0", "StartScenario_1", "stop_0", - "AbortScenario_0", "InitializeScenario_0", - "ConnectObject_0", "DisconnectObject_0", "GetServerParameterList_0", "SetServerParameter_2", - "GetServerParameter_1", "DownloadFile_1", "UploadFile_3", "CheckFileDirectoryExist_1", - "GetDirectoryContent_1", - "DeleteFileDirectory_1", "CreateDirectory_1", "GetTestOrigin_0", "replay_1", "control_0", "Exit_0", - "start_ext_trigg_1" + "Idle_0", "GetServerStatus_0", "ArmScenario_0", "DisarmScenario_0", "StartScenario_1", "stop_0", + "AbortScenario_0", "InitializeScenario_0", + "ConnectObject_0", "DisconnectObject_0", "GetServerParameterList_0", "SetServerParameter_2", + "GetServerParameter_1", "DownloadFile_1", "UploadFile_3", "CheckFileDirectoryExist_1", + "GetDirectoryContent_1", + "DeleteFileDirectory_1", "CreateDirectory_1", "GetTestOrigin_0", "replay_1", "control_0", "Exit_0", + "start_ext_trigg_1" }; const char *SystemControlStatesArr[] = - { "UNDEFINED", "INITIALIZED", "IDLE", "READY", "RUNNING", "INWORK", "ERROR" }; + { "UNDEFINED", "INITIALIZED", "IDLE", "READY", "RUNNING", "INWORK", "ERROR" }; const char *SystemControlOBCStatesArr[] = - { "UNDEFINED", "IDLE", "INITIALIZED", "CONNECTED", "ARMED", "RUNNING", "ERROR" }; + { "UNDEFINED", "IDLE", "INITIALIZED", "CONNECTED", "ARMED", "RUNNING", "ERROR" }; const char *POSTRequestMandatoryContent[] = { "POST", "HTTP/1.1", "\r\n\r\n" }; @@ -152,41 +152,41 @@ C8 *STR_SYSTEM_CONTROL_TX_PACKET_SIZE = "1200"; ------------------------------------------------------------*/ //SystemControlCommand_t SystemControlFindCommandOld(const char* CommandBuffer, SystemControlCommand_t *CurrentCommand, int *ArgCount); SystemControlCommand_t SystemControlFindCommand(const char *CommandBuffer, - SystemControlCommand_t * CurrentCommand, int *ArgCount); + SystemControlCommand_t * CurrentCommand, int *ArgCount); static I32 SystemControlInitServer(int *ClientSocket, int *ServerHandle, struct in_addr *ip_addr); static I32 SystemControlConnectServer(int *sockfd, const char *name, const uint32_t port); static void SystemControlSendBytes(const char *data, int length, int *sockfd, int debug); void SystemControlSendControlResponse(U16 ResponseStatus, C8 * ResponseString, C8 * ResponseData, - I32 ResponseDataLength, I32 * Sockfd, U8 Debug); + I32 ResponseDataLength, I32 * Sockfd, U8 Debug); I32 SystemControlBuildControlResponse(U16 ResponseStatus, C8 * ResponseString, C8 * ResponseData, - I32 ResponseDataLength, U8 Debug); + I32 ResponseDataLength, U8 Debug); void SystemControlSendLog(C8 * LogString, I32 * Sockfd, U8 Debug); void SystemControlSendMONR(C8 * LogString, I32 * Sockfd, U8 Debug); static void SystemControlCreateProcessChannel(const C8 * name, const U32 port, I32 * sockfd, - struct sockaddr_in *addr); + struct sockaddr_in *addr); //I32 SystemControlSendUDPData(I32 *sockfd, struct sockaddr_in* addr, C8 *SendData, I32 Length, U8 debug); I32 SystemControlReadServerParameterList(C8 * ParameterList, U8 debug); I32 SystemControlGetServerParameter(GSDType * GSD, C8 * ParameterName, C8 * ReturnValue, U32 BufferLength, - U8 Debug); + U8 Debug); I32 SystemControlReadServerParameter(C8 * ParameterName, C8 * ReturnValue, U8 Debug); I32 SystemControlWriteServerParameter(C8 * ParameterName, C8 * NewValue, U8 Debug); I32 SystemControlSetServerParameter(GSDType * GSD, C8 * ParameterName, C8 * NewValue, U8 Debug); I32 SystemControlCheckFileDirectoryExist(C8 * ParameterName, C8 * ReturnValue, U8 Debug); I32 SystemControlUploadFile(C8 * Path, C8 * FileSize, C8 * PacketSize, C8 * ReturnValue, U8 Debug); I32 SystemControlReceiveRxData(I32 * sockfd, C8 * Path, C8 * FileSize, C8 * PacketSize, C8 * ReturnValue, - U8 Debug); + U8 Debug); I32 SystemControlDeleteFileDirectory(C8 * Path, C8 * ReturnValue, U8 Debug); I32 SystemControlBuildFileContentInfo(C8 * Path, C8 * ReturnValue, U8 Debug); I32 SystemControlSendFileContent(I32 * sockfd, C8 * Path, C8 * PacketSize, C8 * ReturnValue, U8 Remove, - U8 Debug); + U8 Debug); I32 SystemControlCreateDirectory(C8 * Path, C8 * ReturnValue, U8 Debug); I32 SystemControlBuildRVSSTimeChannelMessage(C8 * RVSSData, U32 * RVSSDataLengthU32, TimeType * GPSTime, - U8 Debug); + U8 Debug); I32 SystemControlBuildRVSSMaestroChannelMessage(C8 * RVSSData, U32 * RVSSDataLengthU32, GSDType * GSD, - U8 SysCtrlState, U8 Debug); + U8 SysCtrlState, U8 Debug); I32 SystemControlBuildRVSSAspChannelMessage(C8 * RVSSData, U32 * RVSSDataLengthU32, U8 Debug); I32 SystemControlBuildRVSSMONRChannelMessage(C8 * RVSSData, U32 * RVSSDataLengthU32, MonitorDataType MonrData, - U8 Debug); + U8 Debug); static ssize_t SystemControlReceiveUserControlData(I32 socket, C8 * dataBuffer, size_t dataBufferLength); static C8 SystemControlVerifyHostAddress(char *ip); static void signalHandler(int signo); @@ -204,974 +204,974 @@ static volatile int iExit = 0; void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { - I32 ServerHandle; - I32 ClientSocket = 0; - I32 ClientResult = 0; - struct sockaddr_in RVSSChannelAddr; - struct in_addr ip_addr; - I32 RVSSChannelSocket; - MonitorDataType monrData; - - ServerState_t server_state = SERVER_STATE_UNDEFINED; - OBCState_t objectControlState = OBC_STATE_UNDEFINED; - SystemControlCommand_t SystemControlCommand = Idle_0; - SystemControlCommand_t PreviousSystemControlCommand = Idle_0; - - int CommandArgCount = 0, /*CurrentCommandArgCounter=0, */ CurrentInputArgCount = 0; - C8 pcBuffer[IPC_BUFFER_SIZE]; - char inchr; - struct timeval tvTime; - - ObjectPosition OP; - int i, i1; - char *StartPtr, *StopPtr, *CmdPtr, *OpeningQuotationMarkPtr, *ClosingQuotationMarkPtr, *StringPos; - struct timespec tTime; - enum COMMAND iCommand; - ssize_t bytesReceived = 0; - char pcRecvBuffer[SC_RECV_MESSAGE_BUFFER]; - char ObjectIP[SMALL_BUFFER_SIZE_16]; - char ObjectPort[SMALL_BUFFER_SIZE_6]; - char TriggId[SMALL_BUFFER_SIZE_6]; - char TriggAction[SMALL_BUFFER_SIZE_6]; - char TriggDelay[SMALL_BUFFER_SIZE_20]; - U64 uiTime; - U32 DelayedStartU32; - U8 ModeU8 = 0; - C8 TextBufferC8[SMALL_BUFFER_SIZE_20]; - C8 ServerIPC8[SMALL_BUFFER_SIZE_20]; - C8 UsernameC8[SMALL_BUFFER_SIZE_20]; - C8 PasswordC8[SMALL_BUFFER_SIZE_20]; - U16 ServerPortU16; - I32 ServerSocketI32 = 0; - ServiceSessionType SessionData; - C8 RemoteServerRxData[1024]; - struct timespec sleep_time, ref_time; - const struct timespec mqEmptyPollPeriod = { SC_SLEEP_TIME_EMPTY_MQ_S, SC_SLEEP_TIME_EMPTY_MQ_NS }; - const struct timespec mqNonEmptyPollPeriod = - { SC_SLEEP_TIME_NONEMPTY_MQ_S, SC_SLEEP_TIME_NONEMPTY_MQ_NS }; - struct timeval CurrentTimeStruct; - U64 CurrentTimeU64 = 0; - U64 TimeDiffU64 = 0; - U64 OldTimeU64 = 0; - U64 PollRateU64 = 0; - C8 ControlResponseBuffer[SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE]; - C8 TextBuffer20[SMALL_BUFFER_SIZE_20]; - C8 UserControlIPC8[SMALL_BUFFER_SIZE_20]; - struct timeval now; - U16 MilliU16 = 0, NowU16 = 0; - U64 GPSmsU64 = 0; - C8 ParameterListC8[SYSTEM_CONTROL_SERVER_PARAMETER_LIST_SIZE]; - U32 LengthU32 = 0; - C8 BinBuffer[SMALL_BUFFER_SIZE_1024]; - - C8 TxBuffer[SYSTEM_CONTROL_TX_PACKET_SIZE]; - - HTTPHeaderContent HTTPHeader; - - //C8 SIDSData[128][10000][8]; - - C8 RVSSData[SYSTEM_CONTROL_RVSS_DATA_BUFFER]; - U16 RVSSSendCounterU16 = 0; - U32 RVSSConfigU32; - U32 RVSSMessageLengthU32; - U16 PCDMessageCodeU16; - - LogInit(MODULE_NAME, logLevel); - LogMessage(LOG_LEVEL_INFO, "System control task running with PID: %i", getpid()); - - // Set up signal handlers - if (signal(SIGINT, signalHandler) == SIG_ERR) - util_error("Unable to initialize signal handler"); - - if (iCommInit()) - util_error("Unable to connect to message bus"); - - DataDictionaryGetRVSSConfigU32(GSD, &RVSSConfigU32); - LogMessage(LOG_LEVEL_INFO, "RVSSConfigU32 = %d", RVSSConfigU32); - - U8 RVSSRateU8; - dbl RVSSRateDbl; - - DataDictionaryGetRVSSRateU8(GSD, &RVSSRateU8); - RVSSRateDbl = RVSSRateU8; - RVSSRateDbl = (1 / RVSSRateDbl) * 1000; - LogMessage(LOG_LEVEL_INFO, "RVSSRateU8 = %d", RVSSRateU8); - - if (ModeU8 == 0) { + I32 ServerHandle; + I32 ClientSocket = 0; + I32 ClientResult = 0; + struct sockaddr_in RVSSChannelAddr; + struct in_addr ip_addr; + I32 RVSSChannelSocket; + MonitorDataType monrData; + + ServerState_t server_state = SERVER_STATE_UNDEFINED; + OBCState_t objectControlState = OBC_STATE_UNDEFINED; + SystemControlCommand_t SystemControlCommand = Idle_0; + SystemControlCommand_t PreviousSystemControlCommand = Idle_0; + + int CommandArgCount = 0, /*CurrentCommandArgCounter=0, */ CurrentInputArgCount = 0; + C8 pcBuffer[IPC_BUFFER_SIZE]; + char inchr; + struct timeval tvTime; + + ObjectPosition OP; + int i, i1; + char *StartPtr, *StopPtr, *CmdPtr, *OpeningQuotationMarkPtr, *ClosingQuotationMarkPtr, *StringPos; + struct timespec tTime; + enum COMMAND iCommand; + ssize_t bytesReceived = 0; + char pcRecvBuffer[SC_RECV_MESSAGE_BUFFER]; + char ObjectIP[SMALL_BUFFER_SIZE_16]; + char ObjectPort[SMALL_BUFFER_SIZE_6]; + char TriggId[SMALL_BUFFER_SIZE_6]; + char TriggAction[SMALL_BUFFER_SIZE_6]; + char TriggDelay[SMALL_BUFFER_SIZE_20]; + U64 uiTime; + U32 DelayedStartU32; + U8 ModeU8 = 0; + C8 TextBufferC8[SMALL_BUFFER_SIZE_20]; + C8 ServerIPC8[SMALL_BUFFER_SIZE_20]; + C8 UsernameC8[SMALL_BUFFER_SIZE_20]; + C8 PasswordC8[SMALL_BUFFER_SIZE_20]; + U16 ServerPortU16; + I32 ServerSocketI32 = 0; + ServiceSessionType SessionData; + C8 RemoteServerRxData[1024]; + struct timespec sleep_time, ref_time; + const struct timespec mqEmptyPollPeriod = { SC_SLEEP_TIME_EMPTY_MQ_S, SC_SLEEP_TIME_EMPTY_MQ_NS }; + const struct timespec mqNonEmptyPollPeriod = + { SC_SLEEP_TIME_NONEMPTY_MQ_S, SC_SLEEP_TIME_NONEMPTY_MQ_NS }; + struct timeval CurrentTimeStruct; + U64 CurrentTimeU64 = 0; + U64 TimeDiffU64 = 0; + U64 OldTimeU64 = 0; + U64 PollRateU64 = 0; + C8 ControlResponseBuffer[SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE]; + C8 TextBuffer20[SMALL_BUFFER_SIZE_20]; + C8 UserControlIPC8[SMALL_BUFFER_SIZE_20]; + struct timeval now; + U16 MilliU16 = 0, NowU16 = 0; + U64 GPSmsU64 = 0; + C8 ParameterListC8[SYSTEM_CONTROL_SERVER_PARAMETER_LIST_SIZE]; + U32 LengthU32 = 0; + C8 BinBuffer[SMALL_BUFFER_SIZE_1024]; + + C8 TxBuffer[SYSTEM_CONTROL_TX_PACKET_SIZE]; + + HTTPHeaderContent HTTPHeader; + + //C8 SIDSData[128][10000][8]; + + C8 RVSSData[SYSTEM_CONTROL_RVSS_DATA_BUFFER]; + U16 RVSSSendCounterU16 = 0; + U32 RVSSConfigU32; + U32 RVSSMessageLengthU32; + U16 PCDMessageCodeU16; + + LogInit(MODULE_NAME, logLevel); + LogMessage(LOG_LEVEL_INFO, "System control task running with PID: %i", getpid()); + + // Set up signal handlers + if (signal(SIGINT, signalHandler) == SIG_ERR) + util_error("Unable to initialize signal handler"); + + if (iCommInit()) + util_error("Unable to connect to message bus"); + + DataDictionaryGetRVSSConfigU32(GSD, &RVSSConfigU32); + LogMessage(LOG_LEVEL_INFO, "RVSSConfigU32 = %d", RVSSConfigU32); + + U8 RVSSRateU8; + dbl RVSSRateDbl; + + DataDictionaryGetRVSSRateU8(GSD, &RVSSRateU8); + RVSSRateDbl = RVSSRateU8; + RVSSRateDbl = (1 / RVSSRateDbl) * 1000; + LogMessage(LOG_LEVEL_INFO, "RVSSRateU8 = %d", RVSSRateU8); + + if (ModeU8 == 0) { - } - else if (ModeU8 == 1) { - SessionData.SessionIdU32 = 0; - SessionData.UserIdU32 = 0; - SessionData.UserTypeU8 = 0; + } + else if (ModeU8 == 1) { + SessionData.SessionIdU32 = 0; + SessionData.UserIdU32 = 0; + SessionData.UserTypeU8 = 0; - /* */ - PollRateU64 = SYSTEM_CONTROL_SERVICE_POLL_TIME_MS; - CurrentTimeU64 = - (uint64_t) CurrentTimeStruct.tv_sec * 1000 + (uint64_t) CurrentTimeStruct.tv_usec / 1000; - OldTimeU64 = CurrentTimeU64; + /* */ + PollRateU64 = SYSTEM_CONTROL_SERVICE_POLL_TIME_MS; + CurrentTimeU64 = + (uint64_t) CurrentTimeStruct.tv_sec * 1000 + (uint64_t) CurrentTimeStruct.tv_usec / 1000; + OldTimeU64 = CurrentTimeU64; - } + } - while (!iExit) { - if (server_state == SERVER_STATE_ERROR) { - iCommSend(COMM_ABORT, NULL, 0); - continue; - } + while (!iExit) { + if (server_state == SERVER_STATE_ERROR) { + iCommSend(COMM_ABORT, NULL, 0); + continue; + } - if (ModeU8 == 0) { - if (ClientSocket <= 0) { - if (server_state == SERVER_STATE_UNDEFINED) { - //Do some initialization + if (ModeU8 == 0) { + if (ClientSocket <= 0) { + if (server_state == SERVER_STATE_UNDEFINED) { + //Do some initialization - //Send COMM_DATA_DICT to notify to update data from DataDictionary - iCommSend(COMM_DATA_DICT, ControlResponseBuffer, sizeof (ControlResponseBuffer)); + //Send COMM_DATA_DICT to notify to update data from DataDictionary + iCommSend(COMM_DATA_DICT, ControlResponseBuffer, sizeof (ControlResponseBuffer)); - server_state = SERVER_STATE_INITIALIZED; - } + server_state = SERVER_STATE_INITIALIZED; + } - if (USE_LOCAL_USER_CONTROL == 0) { + if (USE_LOCAL_USER_CONTROL == 0) { - ClientResult = SystemControlInitServer(&ClientSocket, &ServerHandle, &ip_addr); - bzero(UserControlIPC8, SMALL_BUFFER_SIZE_20); - sprintf(UserControlIPC8, "%s", inet_ntoa(ip_addr)); - LogMessage(LOG_LEVEL_INFO, "UserControl IP address is %s", inet_ntoa(ip_addr)); - SystemControlCreateProcessChannel(UserControlIPC8, SYSTEM_CONTROL_PROCESS_PORT, - &RVSSChannelSocket, &RVSSChannelAddr); + ClientResult = SystemControlInitServer(&ClientSocket, &ServerHandle, &ip_addr); + bzero(UserControlIPC8, SMALL_BUFFER_SIZE_20); + sprintf(UserControlIPC8, "%s", inet_ntoa(ip_addr)); + LogMessage(LOG_LEVEL_INFO, "UserControl IP address is %s", inet_ntoa(ip_addr)); + SystemControlCreateProcessChannel(UserControlIPC8, SYSTEM_CONTROL_PROCESS_PORT, + &RVSSChannelSocket, &RVSSChannelAddr); - } - if (USE_LOCAL_USER_CONTROL == 1) { - ClientResult = - SystemControlConnectServer(&ClientSocket, LOCAL_USER_CONTROL_IP, - LOCAL_USER_CONTROL_PORT); - SystemControlCreateProcessChannel(LOCAL_USER_CONTROL_IP, SYSTEM_CONTROL_PROCESS_PORT, - &RVSSChannelSocket, &RVSSChannelAddr); - } + } + if (USE_LOCAL_USER_CONTROL == 1) { + ClientResult = + SystemControlConnectServer(&ClientSocket, LOCAL_USER_CONTROL_IP, + LOCAL_USER_CONTROL_PORT); + SystemControlCreateProcessChannel(LOCAL_USER_CONTROL_IP, SYSTEM_CONTROL_PROCESS_PORT, + &RVSSChannelSocket, &RVSSChannelAddr); + } - server_state = SERVER_STATE_IDLE; - } + server_state = SERVER_STATE_IDLE; + } - PreviousSystemControlCommand = SystemControlCommand; - bzero(pcBuffer, IPC_BUFFER_SIZE); + PreviousSystemControlCommand = SystemControlCommand; + bzero(pcBuffer, IPC_BUFFER_SIZE); ClientResult = SystemControlReceiveUserControlData(ClientSocket, pcBuffer, sizeof (pcBuffer)); - if (ClientResult <= -1) { - if (errno != EAGAIN && errno != EWOULDBLOCK) { - LogMessage(LOG_LEVEL_ERROR, "Failed to receive from command socket"); - LogMessage(LOG_LEVEL_ERROR, "Waiting 5 seconds before exiting"); - usleep(5000000); //Wait 5 sec before sending exit, just so ObjectControl can send abort in HEAB before exit - if (iCommSend(COMM_EXIT, NULL, 0) < 0) - util_error("Fatal communication fault when sending EXIT command"); - LogMessage(LOG_LEVEL_ERROR, "System control exiting"); - exit(EXIT_FAILURE); - } - } - else if (ClientResult == 0) { - LogMessage(LOG_LEVEL_INFO, "Client closed connection"); - close(ClientSocket); - ClientSocket = -1; - if (USE_LOCAL_USER_CONTROL == 0) { - close(ServerHandle); - ServerHandle = -1; - } - - SystemControlCommand = AbortScenario_0; //Oops no client is connected, go to AbortScenario_0 - server_state == SERVER_STATE_UNDEFINED; // TODO: Should this be an assignment? - } - else if (ClientResult > 0 && ClientResult < TCP_RECV_BUFFER_SIZE) { - // TODO: Move this entire decoding process into a separate function - for (i = 0; i < SYSTEM_CONTROL_ARG_MAX_COUNT; i++) - bzero(SystemControlArgument[i], SYSTEM_CONTROL_ARGUMENT_MAX_LENGTH); - - CurrentInputArgCount = 0; - StartPtr = pcBuffer; - StopPtr = pcBuffer; - CmdPtr = NULL; - StringPos = pcBuffer; - - // Check so that all POST request mandatory content is contained in the message - for (i = 0; - i < sizeof (POSTRequestMandatoryContent) / sizeof (POSTRequestMandatoryContent[0]); - ++i) { - - StringPos = strstr(StringPos, POSTRequestMandatoryContent[i]); - if (StringPos == NULL) { - CmdPtr = NULL; - break; - } - else { - CmdPtr = StringPos + strlen(POSTRequestMandatoryContent[i]); - } - } - - if (CmdPtr != NULL) { - // It is now known that the request contains "POST" and "\r\n\r\n", so we can decode the header - UtilDecodeHTTPRequestHeader(pcBuffer, &HTTPHeader); - - if (HTTPHeader.Host[0] == '\0') { - LogMessage(LOG_LEVEL_INFO, "Unspecified host in request <%s>", pcBuffer); - } - else if (SystemControlVerifyHostAddress(HTTPHeader.Host)) { - // Find opening parenthesis - StartPtr = strchr(CmdPtr, '('); - // If there was no opening or closing parenthesis, the format is not correct - if (StartPtr == NULL || strchr(StartPtr, ')') == NULL) - LogMessage(LOG_LEVEL_WARNING, - "Received command not conforming to MSCP standards"); - else { - StartPtr++; - while (StopPtr != NULL) { - StopPtr = (char *)strchr(StartPtr, ','); - - // If there are no commas past this point, just copy the rest - if (StopPtr == NULL) { - strncpy(SystemControlArgument[CurrentInputArgCount], StartPtr, - (uint64_t) strchr(StartPtr, ')') - (uint64_t) StartPtr); - } - // Otherwise, check if the comma we found was inside quotation marks - else { - OpeningQuotationMarkPtr = (char *)strchr(StartPtr, '"'); - - if (OpeningQuotationMarkPtr == NULL) { - // It was not within quotation marks: copy until the next comma - strncpy(SystemControlArgument[CurrentInputArgCount], StartPtr, - (uint64_t) StopPtr - (uint64_t) StartPtr); - } - else if (OpeningQuotationMarkPtr != NULL - && OpeningQuotationMarkPtr < StopPtr) { - // A quotation mark was found and it was before the next comma: find the closing quotation mark - ClosingQuotationMarkPtr = - (char *)strchr(OpeningQuotationMarkPtr + 1, '"'); - - - if (ClosingQuotationMarkPtr == NULL) { - CmdPtr = NULL; - StopPtr = NULL; - LogMessage(LOG_LEVEL_WARNING, - "Received MSCP command with single quotation mark"); - break; - } - else { - // Copy all arguments within quotation marks including the quotation marks - strncpy(SystemControlArgument[CurrentInputArgCount], - OpeningQuotationMarkPtr + 1, - (uint64_t) (ClosingQuotationMarkPtr) - - (uint64_t) (OpeningQuotationMarkPtr + 1)); - // Find next comma after closing quotation mark - StopPtr = strchr(ClosingQuotationMarkPtr, ','); - } - } - } - StartPtr = StopPtr + 1; - CurrentInputArgCount++; - } - - if (CmdPtr != NULL) - SystemControlFindCommand(CmdPtr, &SystemControlCommand, &CommandArgCount); - else - LogMessage(LOG_LEVEL_WARNING, "Invalid MSCP command received"); - } - } - else { - LogMessage(LOG_LEVEL_INFO, - "Request specified host <%s> not among known local addresses", - HTTPHeader.Host); - } - } - else { - LogMessage(LOG_LEVEL_WARNING, - "Received badly formatted HTTP request: <%s>, must contain \"POST\" and \"\\r\\n\\r\\n\"", - pcBuffer); - } - } - else { - LogMessage(LOG_LEVEL_WARNING, "Ignored received TCP message which was too large to handle"); - } - - } - else if (ModeU8 == 1) { /* use util.c function to call time - gettimeofday(&CurrentTimeStruct, NULL); - CurrentTimeU64 = (uint64_t)CurrentTimeStruct.tv_sec*1000 + (uint64_t)CurrentTimeStruct.tv_usec/1000; - */ - CurrentTimeU64 = UtilgetCurrentUTCtimeMS(); - TimeDiffU64 = CurrentTimeU64 - OldTimeU64; - } - - - objectControlState = DataDictionaryGetOBCStateU8(GSD); - - if (server_state == SERVER_STATE_INWORK) { - if (SystemControlCommand == AbortScenario_0) { - SystemControlCommand = SystemControlCommand; - } - else if (SystemControlCommand == GetServerStatus_0) { - LogMessage(LOG_LEVEL_INFO, "State: %s, OBCState: %s, PreviousCommand: %s", - SystemControlStatesArr[server_state], - SystemControlOBCStatesArr[objectControlState], - SystemControlCommandsArr[PreviousSystemControlCommand]); - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - ControlResponseBuffer[0] = server_state; - ControlResponseBuffer[1] = DataDictionaryGetOBCStateU8(GSD); //OBCStateU8; - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "GetServerStatus:", - ControlResponseBuffer, 2, &ClientSocket, 0); - SystemControlCommand = PreviousSystemControlCommand; - } - else if (SystemControlCommand != PreviousSystemControlCommand) { - LogMessage(LOG_LEVEL_WARNING, - "Command not allowed, SystemControl is busy in state %s, PreviousCommand: %s", - SystemControlStatesArr[server_state], - SystemControlCommandsArr[PreviousSystemControlCommand]); - SystemControlSendLog - ("[SystemControl] Command not allowed, SystemControl is busy in state INWORK.\n", - &ClientSocket, 0); - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE, "", - ControlResponseBuffer, 0, &ClientSocket, 0); - SystemControlCommand = PreviousSystemControlCommand; - } - } - - bzero(pcRecvBuffer, SC_RECV_MESSAGE_BUFFER); - bytesReceived = iCommRecv(&iCommand, pcRecvBuffer, SC_RECV_MESSAGE_BUFFER, NULL); - - switch (iCommand) { - case COMM_FAILURE: - if (server_state == SERVER_STATE_INWORK) { - enum COMMAND failedCommand = (enum COMMAND)pcRecvBuffer[0]; - - if (failedCommand == COMM_INIT && PreviousSystemControlCommand == InitializeScenario_0) { - server_state = SERVER_STATE_IDLE; - SystemControlCommand = Idle_0; - LogMessage(LOG_LEVEL_INFO, "Initialization failed"); - // TODO: report to user? - } - else { - LogMessage(LOG_LEVEL_ERROR, "Unhandled FAILURE (command: %u) reply in state %s", - pcRecvBuffer[0], SystemControlStatesArr[server_state]); - } - } - else { - LogMessage(LOG_LEVEL_WARNING, "Received unexpected FAILURE (command: %u) reply in state %s", - pcRecvBuffer[0], SystemControlStatesArr[server_state]); - // TODO: React more? - } - break; - case COMM_OBC_STATE: - break; - case COMM_LOG: - // This creates a problem in GUC: disabled it for now - //SystemControlSendLog(pcRecvBuffer, &ClientSocket, 0); - break; - case COMM_MONR: - // TODO: Decode - if (RVSSChannelSocket != 0 && RVSSConfigU32 & RVSS_MONR_CHANNEL && bytesReceived >= 0) { - UtilPopulateMonitorDataStruct(pcRecvBuffer, (size_t) bytesReceived, &monrData, 0); - SystemControlBuildRVSSMONRChannelMessage(RVSSData, &RVSSMessageLengthU32, monrData, 0); - UtilSendUDPData("SystemControl", &RVSSChannelSocket, &RVSSChannelAddr, RVSSData, - RVSSMessageLengthU32, 0); - } - - SystemControlBuildRVSSMONRChannelMessage(RVSSData, &RVSSMessageLengthU32, monrData, 0); - UtilSendUDPData("SystemControl", &RVSSChannelSocket, &RVSSChannelAddr, RVSSData, - RVSSMessageLengthU32, 0); - break; - case COMM_INV: - break; - default: - LogMessage(LOG_LEVEL_WARNING, "Unhandled message bus command: %u", iCommand); - } - - switch (SystemControlCommand) { - // can you access GetServerParameterList_0, GetServerParameter_1, SetServerParameter_2 and DISarmScenario and Exit from the GUI - case Idle_0: - break; - case GetServerStatus_0: - if (SystemControlCommand != PreviousSystemControlCommand) { - LogMessage(LOG_LEVEL_INFO, "State: %s, OBCState: %s, %d", - SystemControlStatesArr[server_state], - SystemControlOBCStatesArr[objectControlState], DataDictionaryGetOBCStateU8(GSD)); - } - SystemControlCommand = Idle_0; - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - ControlResponseBuffer[0] = server_state; - ControlResponseBuffer[1] = DataDictionaryGetOBCStateU8(GSD); //OBCStateU8; - LogMessage(LOG_LEVEL_DEBUG, "GPSMillisecondsU64: %ld", GPSTime->GPSMillisecondsU64); // GPSTime just ticks from 0 up shouldent it be in the global GPStime? - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "GetServerStatus:", - ControlResponseBuffer, 2, &ClientSocket, 0); - break; - case GetServerParameterList_0: - SystemControlCommand = Idle_0; - bzero(ParameterListC8, SYSTEM_CONTROL_SERVER_PARAMETER_LIST_SIZE); - SystemControlReadServerParameterList(ParameterListC8, 0); - SystemControlSendControlResponse(strlen(ParameterListC8) > - 0 ? SYSTEM_CONTROL_RESPONSE_CODE_OK : - SYSTEM_CONTROL_RESPONSE_CODE_NO_DATA, "GetServerParameterList:", - ParameterListC8, strlen(ParameterListC8), &ClientSocket, 0); - break; - case GetTestOrigin_0: - SystemControlCommand = Idle_0; - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - DataDictionaryGetOriginLatitudeC8(GSD, TextBuffer20, SMALL_BUFFER_SIZE_20); - strcat(ControlResponseBuffer, TextBuffer20); - strcat(ControlResponseBuffer, ";"); - DataDictionaryGetOriginLongitudeC8(GSD, TextBuffer20, SMALL_BUFFER_SIZE_20); - strcat(ControlResponseBuffer, TextBuffer20); - strcat(ControlResponseBuffer, ";"); - DataDictionaryGetOriginAltitudeC8(GSD, TextBuffer20, SMALL_BUFFER_SIZE_20); - strcat(ControlResponseBuffer, TextBuffer20); - strcat(ControlResponseBuffer, ";"); - iCommSend(COMM_OSEM, ControlResponseBuffer, sizeof (ControlResponseBuffer)); - SystemControlSendControlResponse(strlen(ParameterListC8) > - 0 ? SYSTEM_CONTROL_RESPONSE_CODE_OK : - SYSTEM_CONTROL_RESPONSE_CODE_NO_DATA, "GetTestOrigin:", - ControlResponseBuffer, strlen(ControlResponseBuffer), - &ClientSocket, 0); - break; - case GetServerParameter_1: - if (CurrentInputArgCount == CommandArgCount) { - SystemControlCommand = Idle_0; - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlGetServerParameter(GSD, SystemControlArgument[0], ControlResponseBuffer, - SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE, 0); - SystemControlSendControlResponse(strlen(ControlResponseBuffer) > - 0 ? SYSTEM_CONTROL_RESPONSE_CODE_OK : - SYSTEM_CONTROL_RESPONSE_CODE_NO_DATA, "GetServerParameter:", - ControlResponseBuffer, strlen(ControlResponseBuffer), - &ClientSocket, 0); - } - else { - LogMessage(LOG_LEVEL_ERROR, "Wrong parameter count in GetServerParameter(Name)!"); - SystemControlCommand = Idle_0; - } - break; - case SetServerParameter_2: - if (CurrentInputArgCount == CommandArgCount) { - SystemControlCommand = Idle_0; - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlSetServerParameter(GSD, SystemControlArgument[0], SystemControlArgument[1], 1); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "SetServerParameter:", - ControlResponseBuffer, 0, &ClientSocket, 0); - //Send COMM_DATA_DICT to notify to update data from DataDictionary - iCommSend(COMM_DATA_DICT, ControlResponseBuffer, sizeof (ControlResponseBuffer)); - - } - else { - LogMessage(LOG_LEVEL_ERROR, "Wrong parameter count in SetServerParameter(Name, Value)!"); - SystemControlCommand = Idle_0; - } - break; - case CheckFileDirectoryExist_1: - if (CurrentInputArgCount == CommandArgCount) { - SystemControlCommand = Idle_0; - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlCheckFileDirectoryExist(SystemControlArgument[0], ControlResponseBuffer, 0); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "CheckFileDirectoryExist:", - ControlResponseBuffer, 1, &ClientSocket, 0); - - } - else { - LogMessage(LOG_LEVEL_ERROR, "Wrong parameter count in CheckFFExist(path)!"); - SystemControlCommand = Idle_0; - } - break; - case DeleteFileDirectory_1: - if (CurrentInputArgCount == CommandArgCount) { - SystemControlCommand = Idle_0; - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlDeleteFileDirectory(SystemControlArgument[0], ControlResponseBuffer, 0); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "DeleteFileDirectory:", - ControlResponseBuffer, 1, &ClientSocket, 0); - - } - else { - LogMessage(LOG_LEVEL_ERROR, "Wrong parameter count in DeleteFileDirectory(path)!"); - SystemControlCommand = Idle_0; - } - break; - case CreateDirectory_1: - if (CurrentInputArgCount == CommandArgCount) { - SystemControlCommand = Idle_0; - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlCreateDirectory(SystemControlArgument[0], ControlResponseBuffer, 0); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "CreateDirectory:", - ControlResponseBuffer, 1, &ClientSocket, 0); - - } - else { - LogMessage(LOG_LEVEL_ERROR, "Wrong parameter count in CreateDirectory(path)!"); - SystemControlCommand = Idle_0; - } - break; - case GetDirectoryContent_1: - if (CurrentInputArgCount == CommandArgCount) { - SystemControlCommand = Idle_0; - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlCheckFileDirectoryExist(SystemControlArgument[0], ControlResponseBuffer, 0); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "GetDirectoryContent:", - ControlResponseBuffer, 1, &ClientSocket, 0); - if (ControlResponseBuffer[0] == FOLDER_EXIST) { - UtilCreateDirContent(SystemControlArgument[0], "dir.info"); - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlBuildFileContentInfo("dir.info", ControlResponseBuffer, 0); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "GetDirectoryContent:", - ControlResponseBuffer, 4, &ClientSocket, 0); - SystemControlSendFileContent(&ClientSocket, "dir.info", STR_SYSTEM_CONTROL_TX_PACKET_SIZE, - ControlResponseBuffer, REMOVE_FILE, 0); - } - - } - else { - LogMessage(LOG_LEVEL_ERROR, "Wrong parameter count in GetDirectoryContent(path)!"); - SystemControlCommand = Idle_0; - } - break; - case DownloadFile_1: - if (CurrentInputArgCount == CommandArgCount) { - SystemControlCommand = Idle_0; - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlCheckFileDirectoryExist(SystemControlArgument[0], ControlResponseBuffer, 0); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "DownloadFile:", - ControlResponseBuffer, 1, &ClientSocket, 0); - if (ControlResponseBuffer[0] == FILE_EXIST) { - UtilCreateDirContent(SystemControlArgument[0], SystemControlArgument[0]); - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlBuildFileContentInfo(SystemControlArgument[0], ControlResponseBuffer, 0); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "SubDownloadFile:", - ControlResponseBuffer, 4, &ClientSocket, 0); - SystemControlSendFileContent(&ClientSocket, SystemControlArgument[0], - STR_SYSTEM_CONTROL_TX_PACKET_SIZE, ControlResponseBuffer, - KEEP_FILE, 0); - } - - } - else { - LogMessage(LOG_LEVEL_ERROR, "Wrong parameter count in GetDirectoryContent(path)!"); - SystemControlCommand = Idle_0; - } - break; - case UploadFile_3: - if (CurrentInputArgCount == CommandArgCount) { - SystemControlCommand = Idle_0; - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlUploadFile(SystemControlArgument[0], SystemControlArgument[1], - SystemControlArgument[2], ControlResponseBuffer, 0); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "UploadFile:", - ControlResponseBuffer, 1, &ClientSocket, 0); - LogMessage(LOG_LEVEL_DEBUG, "UploadFile filelength: %s", SystemControlArgument[1]); - if (ControlResponseBuffer[0] == SERVER_PREPARED_BIG_PACKET_SIZE) //Server is ready to receive data - { - LogMessage(LOG_LEVEL_INFO, "Receiving file: %s", SystemControlArgument[0]); - SystemControlReceiveRxData(&ClientSocket, SystemControlArgument[0], - SystemControlArgument[1], STR_SYSTEM_CONTROL_RX_PACKET_SIZE, - ControlResponseBuffer, 0); - } - else if (ControlResponseBuffer[0] == PATH_INVALID_MISSING) { - LogMessage(LOG_LEVEL_INFO, "Failed receiving file: %s", SystemControlArgument[0]); - SystemControlReceiveRxData(&ClientSocket, "file.tmp", SystemControlArgument[1], - STR_SYSTEM_CONTROL_RX_PACKET_SIZE, ControlResponseBuffer, 0); - SystemControlDeleteFileDirectory("file.tmp", ControlResponseBuffer, 0); - ControlResponseBuffer[0] = PATH_INVALID_MISSING; - } - else { - LogMessage(LOG_LEVEL_INFO, "Receiving file: %s", SystemControlArgument[0]); - SystemControlReceiveRxData(&ClientSocket, SystemControlArgument[0], - SystemControlArgument[1], SystemControlArgument[2], - ControlResponseBuffer, 0); - } - - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "UploadFile:", - ControlResponseBuffer, 1, &ClientSocket, 0); - - } - else { - LogMessage(LOG_LEVEL_ERROR, - "Wrong parameter count in PrepFileRx(path, filesize, packetsize)!"); - SystemControlCommand = Idle_0; - } - break; - case InitializeScenario_0: - if (server_state == SERVER_STATE_IDLE && objectControlState == OBC_STATE_IDLE) { - if (iCommSend(COMM_INIT, pcBuffer, strlen(pcBuffer) + 1) < 0) { - LogMessage(LOG_LEVEL_ERROR, "Fatal communication fault when sending INIT command"); - server_state = SERVER_STATE_ERROR; - } - server_state = SERVER_STATE_INWORK; - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "InitializeScenario:", - ControlResponseBuffer, 0, &ClientSocket, 0); - DataDictionaryInitMONR(GSD); - - SystemControlSendLog("[SystemControl] Sending INIT.\n", &ClientSocket, 0); - } - else if (server_state == SERVER_STATE_INWORK && objectControlState == OBC_STATE_INITIALIZED) { - SystemControlSendLog - ("[SystemControl] Simulate that all objects becomes successfully configured.\n", - &ClientSocket, 0); - SystemControlCommand = Idle_0; - server_state = SERVER_STATE_IDLE; - } - else if (server_state == SERVER_STATE_IDLE) { - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE, - "InitializeScenario:", ControlResponseBuffer, 0, - &ClientSocket, 0); - SystemControlSendLog("[SystemControl] INIT received, state errors!\n", &ClientSocket, 0); - SystemControlCommand = PreviousSystemControlCommand; - } - break; - case ConnectObject_0: - if (server_state == SERVER_STATE_IDLE && objectControlState == OBC_STATE_INITIALIZED) { - if (iCommSend(COMM_CONNECT, pcBuffer, strlen(pcBuffer) + 1) < 0) { - LogMessage(LOG_LEVEL_ERROR, "Fatal communication fault when sending CONNECT command"); - server_state = SERVER_STATE_ERROR; - } - SystemControlCommand = Idle_0; - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "ConnectObject:", - ControlResponseBuffer, 0, &ClientSocket, 0); - SystemControlSendLog("[SystemControl] Sending CONNECT.\n", &ClientSocket, 0); - } - else if (server_state == SERVER_STATE_INWORK && objectControlState == OBC_STATE_CONNECTED) { - SystemControlSendLog("[SystemControl] Simulate that all objects are connected.\n", - &ClientSocket, 0); - SystemControlCommand = Idle_0; - server_state = SERVER_STATE_IDLE; - } - else if (server_state == SERVER_STATE_IDLE) { - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE, - "ConnectObject:", ControlResponseBuffer, 0, &ClientSocket, - 0); - SystemControlSendLog("[SystemControl] CONNECT received, state errors!\n", &ClientSocket, 0); - SystemControlCommand = PreviousSystemControlCommand; - } - break; - case DisconnectObject_0: - if (server_state == SERVER_STATE_IDLE) { - if (iCommSend(COMM_DISCONNECT, pcBuffer, strlen(pcBuffer) + 1) < 0) { - LogMessage(LOG_LEVEL_ERROR, "Fatal communication fault when sending DISCONNECT command"); - server_state = SERVER_STATE_ERROR; - } - SystemControlCommand = Idle_0; - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "DisconnectObject:", - ControlResponseBuffer, 0, &ClientSocket, 0); - SystemControlSendLog("[SystemControl] Sending DISCONNECT.\n", &ClientSocket, 0); - } - else { - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE, - "ConnectObject:", ControlResponseBuffer, 0, &ClientSocket, - 0); - SystemControlSendLog("[SystemControl] DISCONNECT received, state errors!\n", &ClientSocket, - 0); - SystemControlCommand = PreviousSystemControlCommand; - } - break; - case ArmScenario_0: - if (server_state == SERVER_STATE_IDLE && objectControlState == OBC_STATE_CONNECTED) { - server_state = SERVER_STATE_INWORK; - if (iCommSend(COMM_ARM, NULL, 0) < 0) { - LogMessage(LOG_LEVEL_ERROR, "Fatal communication fault when sending ARM command"); - server_state = SERVER_STATE_ERROR; - } - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "ArmScenario:", - ControlResponseBuffer, 0, &ClientSocket, 0); - SystemControlSendLog("[SystemControl] Sending ARM.\n", &ClientSocket, 0); - } - else if (server_state == SERVER_STATE_INWORK && objectControlState == OBC_STATE_ARMED) { - SystemControlSendLog("[SystemControl] Simulate that all objects become armed.\n", - &ClientSocket, 0); - - SystemControlCommand = Idle_0; - server_state = SERVER_STATE_IDLE; - } - else if (server_state == SERVER_STATE_IDLE) { - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE, - "ArmScenario:", ControlResponseBuffer, 0, &ClientSocket, 0); - SystemControlSendLog("[SystemControl] ARM received, state errors!\n", &ClientSocket, 0); - SystemControlCommand = PreviousSystemControlCommand; - } - break; - case DisarmScenario_0: - if (server_state == SERVER_STATE_IDLE && objectControlState == OBC_STATE_ARMED) { - server_state = SERVER_STATE_IDLE; - if (iCommSend(COMM_DISARM, NULL, 0) < 0) { - LogMessage(LOG_LEVEL_ERROR, "Fatal communication fault when sending DISARM command"); - server_state = SERVER_STATE_ERROR; - } - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "DisarmScenario:", - ControlResponseBuffer, 0, &ClientSocket, 0); - SystemControlSendLog("[SystemControl] Sending DISARM.\n", &ClientSocket, 0); - } - else if (server_state == SERVER_STATE_INWORK && objectControlState == OBC_STATE_CONNECTED) { - SystemControlSendLog("[SystemControl] Simulate that all objects become disarmed.\n", - &ClientSocket, 0); - SystemControlCommand = Idle_0; - server_state = SERVER_STATE_IDLE; - } - else if (server_state == SERVER_STATE_IDLE) { - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE, - "DisarmScenario:", ControlResponseBuffer, 0, &ClientSocket, - 0); - SystemControlSendLog("[SystemControl] DISARM received, state errors!\n", &ClientSocket, 0); - SystemControlCommand = PreviousSystemControlCommand; - } - break; - case StartScenario_1: - if (CurrentInputArgCount == CommandArgCount) { - if (server_state == SERVER_STATE_IDLE && objectControlState == OBC_STATE_ARMED) //Temporary! - { - bzero(pcBuffer, IPC_BUFFER_SIZE); - /* Lest use UTC time everywhere instead of etsi and gps time - gettimeofday(&tvTime, NULL); - uiTime = (uint64_t)tvTime.tv_sec*1000 + (uint64_t)tvTime.tv_usec/1000 - MS_FROM_1970_TO_2004_NO_LEAP_SECS + DIFF_LEAP_SECONDS_UTC_ETSI*1000; - */ - uiTime = UtilgetCurrentUTCtimeMS(); - if (TIME_COMPENSATE_LAGING_VM) - uiTime = uiTime - TIME_COMPENSATE_LAGING_VM_VAL; - - LogMessage(LOG_LEVEL_INFO, "Current timestamp (gtd): %lu", uiTime); - - //clock_gettime(CLOCK_MONOTONIC_COARSE, &tTime); - //clock_gettime(CLOCK_REALTIME, &tTime); - //uiTime = (uint64_t)tTime.tv_sec*1000 + (uint64_t)tTime.tv_nsec/1000000 - MS_FROM_1970_TO_2004_NO_LEAP_SECS + DIFF_LEAP_SECONDS_UTC_ETSI*1000; - //printf("[SystemControl] Current timestamp (cgt): %lu\n",uiTime ); - //printf("[SystemControl] Current timestamp: %lu\n",uiTime ); - - //uiTime += atoi(SystemControlArgument[0]); - uiTime = atoi(SystemControlArgument[0]); - DelayedStartU32 = atoi(SystemControlArgument[1]); - sprintf(pcBuffer, "%" PRIu8 ";%" PRIu64 ";%" PRIu32 ";", 0, uiTime, DelayedStartU32); - LogMessage(LOG_LEVEL_INFO, "Sending START <%s> (delayed +%s ms)", pcBuffer, - SystemControlArgument[1]); - - if (iCommSend(COMM_STRT, pcBuffer, strlen(pcBuffer) + 1) < 0) { - LogMessage(LOG_LEVEL_ERROR, "Fatal communication fault when sending STRT command"); - server_state = SERVER_STATE_ERROR; - } - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "StartScenario:", - ControlResponseBuffer, 0, &ClientSocket, 0); - server_state = SERVER_STATE_INWORK; - //server_state = SERVER_STATE_IDLE; //Temporary! - //SystemControlCommand = Idle_0; //Temporary! - } - else if (server_state == SERVER_STATE_INWORK && objectControlState == OBC_STATE_RUNNING) { - - SystemControlCommand = Idle_0; - server_state = SERVER_STATE_IDLE; - } - else if (server_state == SERVER_STATE_IDLE) { - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE, - "StartScenario:", ControlResponseBuffer, 0, - &ClientSocket, 0); - SystemControlSendLog("[SystemControl] START received, state errors!\n", &ClientSocket, 0); - SystemControlCommand = PreviousSystemControlCommand; - } - - } - else - LogMessage(LOG_LEVEL_WARNING, "START command parameter count error"); - break; - case stop_0: - if (iCommSend(COMM_STOP, NULL, 0) < 0) { - LogMessage(LOG_LEVEL_ERROR, "Fatal communication fault when sending STOP command"); - server_state = SERVER_STATE_ERROR; - } - else { - server_state = SERVER_STATE_IDLE; - SystemControlCommand = Idle_0; - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "stop:", - ControlResponseBuffer, 0, &ClientSocket, 0); - } - break; - case AbortScenario_0: - if (objectControlState == OBC_STATE_RUNNING - /* || strstr(SystemControlOBCStatesArr[OBCStateU8], "CONNECTED") != NULL - * || strstr(SystemControlOBCStatesArr[OBCStateU8], "ARMED") != NULL*/ ) - // Abort should only be allowed in running state - { - if (iCommSend(COMM_ABORT, NULL, 0) < 0) { - LogMessage(LOG_LEVEL_ERROR, "Fatal communication fault when sending ABORT command"); - server_state = SERVER_STATE_ERROR; - } - else { - server_state = SERVER_STATE_IDLE; - SystemControlCommand = Idle_0; - if (ClientSocket >= 0) { - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "AbortScenario:", - ControlResponseBuffer, 0, &ClientSocket, 0); - } - } - } - else { - if (ClientSocket >= 0) { - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE, - "AbortScenario:", ControlResponseBuffer, 0, - &ClientSocket, 0); - SystemControlSendLog("[SystemControl] ABORT received, state errors!\n", &ClientSocket, 0); - } - server_state = SERVER_STATE_IDLE; - SystemControlCommand = Idle_0; - } - break; - /* - case replay_1: - if(CurrentCommandArgCounter == CommandArgCount) - { - if(!strcmp(SystemControlArgument[CurrentCommandArgCounter],"-help")) - { - printf("[SystemControl] -----REPLAY-----\n"); - printf("[SystemControl] Syntax: replay [arg]\n"); - printf("[SystemControl] Ex: replay log/33/event.log\n"); - fflush(stdout); - } - else - { - (void)iCommSend(COMM_REPLAY, SystemControlArgument[CurrentCommandArgCounter]); - printf("[SystemControl] System control sending REPLAY on IPC <%s>\n", SystemControlArgument[CurrentCommandArgCounter]); - fflush(stdout); - } - SystemControlCommand = idle_0; - CurrentCommandArgCounter = 0; - } else CurrentCommandArgCounter ++; - break; - case control_0: - (void)iCommSend(COMM_CONTROL, NULL); - //printf("INF: System control sending CONTROL on IPC <%s>\n", pcBuffer); - fflush(stdout); - SystemControlCommand = idle_0; - CurrentCommandArgCounter = 0; - break; */ - case Exit_0: - - if (iCommSend(COMM_EXIT, NULL, 0) < 0) { - LogMessage(LOG_LEVEL_ERROR, "Fatal communication fault when sending EXIT command"); - server_state = SERVER_STATE_ERROR; - } - else { - iExit = 1; - GSD->ExitU8 = 1; - usleep(1000000); - SystemControlCommand = Idle_0; - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "Exit:", - ControlResponseBuffer, 0, &ClientSocket, 0); - close(ClientSocket); - ClientSocket = -1; - if (USE_LOCAL_USER_CONTROL == 0) { - close(ServerHandle); - ServerHandle = -1; - } - LogMessage(LOG_LEVEL_INFO, "Server closing"); - } - break; - - default: - - break; - } - - - - sleep_time.tv_sec = 0; - sleep_time.tv_nsec = SYSTEM_CONTROL_TASK_PERIOD_MS * 1000000; - ++RVSSSendCounterU16; - if (RVSSSendCounterU16 >= ((U16) RVSSRateDbl)) { - RVSSSendCounterU16 = 0; - DataDictionaryGetRVSSRateU8(GSD, &RVSSRateU8); - RVSSRateDbl = RVSSRateU8; - RVSSRateDbl = (1 / RVSSRateDbl) * 100; //This is strange!! Should be 1000, but if it is the RVSSData is sent to slow by a factor of 10. - - if (RVSSChannelSocket != 0 && RVSSSendCounterU16 == 0 && RVSSConfigU32 > 0) { - bzero(RVSSData, SYSTEM_CONTROL_RVSS_DATA_BUFFER); - - if (RVSSConfigU32 & RVSS_TIME_CHANNEL) { - SystemControlBuildRVSSTimeChannelMessage(RVSSData, &RVSSMessageLengthU32, GPSTime, 0); - UtilSendUDPData("SystemControl", &RVSSChannelSocket, &RVSSChannelAddr, RVSSData, - RVSSMessageLengthU32, 0); - } - - if (RVSSConfigU32 & RVSS_MAESTRO_CHANNEL) { - SystemControlBuildRVSSMaestroChannelMessage(RVSSData, &RVSSMessageLengthU32, GSD, - server_state, 0); - UtilSendUDPData("SystemControl", &RVSSChannelSocket, &RVSSChannelAddr, RVSSData, - RVSSMessageLengthU32, 0); - } - - if (RVSSConfigU32 & RVSS_ASP_CHANNEL) { - SystemControlBuildRVSSAspChannelMessage(RVSSData, &RVSSMessageLengthU32, 0); - UtilSendUDPData("SystemControl", &RVSSChannelSocket, &RVSSChannelAddr, RVSSData, - RVSSMessageLengthU32, 0); - } - - } - - - } - - sleep_time = (iCommand == COMM_INV + if (ClientResult <= -1) { + if (errno != EAGAIN && errno != EWOULDBLOCK) { + LogMessage(LOG_LEVEL_ERROR, "Failed to receive from command socket"); + LogMessage(LOG_LEVEL_ERROR, "Waiting 5 seconds before exiting"); + usleep(5000000); //Wait 5 sec before sending exit, just so ObjectControl can send abort in HEAB before exit + if (iCommSend(COMM_EXIT, NULL, 0) < 0) + util_error("Fatal communication fault when sending EXIT command"); + LogMessage(LOG_LEVEL_ERROR, "System control exiting"); + exit(EXIT_FAILURE); + } + } + else if (ClientResult == 0) { + LogMessage(LOG_LEVEL_INFO, "Client closed connection"); + close(ClientSocket); + ClientSocket = -1; + if (USE_LOCAL_USER_CONTROL == 0) { + close(ServerHandle); + ServerHandle = -1; + } + + SystemControlCommand = AbortScenario_0; //Oops no client is connected, go to AbortScenario_0 + server_state == SERVER_STATE_UNDEFINED; // TODO: Should this be an assignment? + } + else if (ClientResult > 0 && ClientResult < TCP_RECV_BUFFER_SIZE) { + // TODO: Move this entire decoding process into a separate function + for (i = 0; i < SYSTEM_CONTROL_ARG_MAX_COUNT; i++) + bzero(SystemControlArgument[i], SYSTEM_CONTROL_ARGUMENT_MAX_LENGTH); + + CurrentInputArgCount = 0; + StartPtr = pcBuffer; + StopPtr = pcBuffer; + CmdPtr = NULL; + StringPos = pcBuffer; + + // Check so that all POST request mandatory content is contained in the message + for (i = 0; + i < sizeof (POSTRequestMandatoryContent) / sizeof (POSTRequestMandatoryContent[0]); + ++i) { + + StringPos = strstr(StringPos, POSTRequestMandatoryContent[i]); + if (StringPos == NULL) { + CmdPtr = NULL; + break; + } + else { + CmdPtr = StringPos + strlen(POSTRequestMandatoryContent[i]); + } + } + + if (CmdPtr != NULL) { + // It is now known that the request contains "POST" and "\r\n\r\n", so we can decode the header + UtilDecodeHTTPRequestHeader(pcBuffer, &HTTPHeader); + + if (HTTPHeader.Host[0] == '\0') { + LogMessage(LOG_LEVEL_INFO, "Unspecified host in request <%s>", pcBuffer); + } + else if (SystemControlVerifyHostAddress(HTTPHeader.Host)) { + // Find opening parenthesis + StartPtr = strchr(CmdPtr, '('); + // If there was no opening or closing parenthesis, the format is not correct + if (StartPtr == NULL || strchr(StartPtr, ')') == NULL) + LogMessage(LOG_LEVEL_WARNING, + "Received command not conforming to MSCP standards"); + else { + StartPtr++; + while (StopPtr != NULL) { + StopPtr = (char *)strchr(StartPtr, ','); + + // If there are no commas past this point, just copy the rest + if (StopPtr == NULL) { + strncpy(SystemControlArgument[CurrentInputArgCount], StartPtr, + (uint64_t) strchr(StartPtr, ')') - (uint64_t) StartPtr); + } + // Otherwise, check if the comma we found was inside quotation marks + else { + OpeningQuotationMarkPtr = (char *)strchr(StartPtr, '"'); + + if (OpeningQuotationMarkPtr == NULL) { + // It was not within quotation marks: copy until the next comma + strncpy(SystemControlArgument[CurrentInputArgCount], StartPtr, + (uint64_t) StopPtr - (uint64_t) StartPtr); + } + else if (OpeningQuotationMarkPtr != NULL + && OpeningQuotationMarkPtr < StopPtr) { + // A quotation mark was found and it was before the next comma: find the closing quotation mark + ClosingQuotationMarkPtr = + (char *)strchr(OpeningQuotationMarkPtr + 1, '"'); + + + if (ClosingQuotationMarkPtr == NULL) { + CmdPtr = NULL; + StopPtr = NULL; + LogMessage(LOG_LEVEL_WARNING, + "Received MSCP command with single quotation mark"); + break; + } + else { + // Copy all arguments within quotation marks including the quotation marks + strncpy(SystemControlArgument[CurrentInputArgCount], + OpeningQuotationMarkPtr + 1, + (uint64_t) (ClosingQuotationMarkPtr) - + (uint64_t) (OpeningQuotationMarkPtr + 1)); + // Find next comma after closing quotation mark + StopPtr = strchr(ClosingQuotationMarkPtr, ','); + } + } + } + StartPtr = StopPtr + 1; + CurrentInputArgCount++; + } + + if (CmdPtr != NULL) + SystemControlFindCommand(CmdPtr, &SystemControlCommand, &CommandArgCount); + else + LogMessage(LOG_LEVEL_WARNING, "Invalid MSCP command received"); + } + } + else { + LogMessage(LOG_LEVEL_INFO, + "Request specified host <%s> not among known local addresses", + HTTPHeader.Host); + } + } + else { + LogMessage(LOG_LEVEL_WARNING, + "Received badly formatted HTTP request: <%s>, must contain \"POST\" and \"\\r\\n\\r\\n\"", + pcBuffer); + } + } + else { + LogMessage(LOG_LEVEL_WARNING, "Ignored received TCP message which was too large to handle"); + } + + } + else if (ModeU8 == 1) { /* use util.c function to call time + gettimeofday(&CurrentTimeStruct, NULL); + CurrentTimeU64 = (uint64_t)CurrentTimeStruct.tv_sec*1000 + (uint64_t)CurrentTimeStruct.tv_usec/1000; + */ + CurrentTimeU64 = UtilgetCurrentUTCtimeMS(); + TimeDiffU64 = CurrentTimeU64 - OldTimeU64; + } + + + objectControlState = DataDictionaryGetOBCStateU8(GSD); + + if (server_state == SERVER_STATE_INWORK) { + if (SystemControlCommand == AbortScenario_0) { + SystemControlCommand = SystemControlCommand; + } + else if (SystemControlCommand == GetServerStatus_0) { + LogMessage(LOG_LEVEL_INFO, "State: %s, OBCState: %s, PreviousCommand: %s", + SystemControlStatesArr[server_state], + SystemControlOBCStatesArr[objectControlState], + SystemControlCommandsArr[PreviousSystemControlCommand]); + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + ControlResponseBuffer[0] = server_state; + ControlResponseBuffer[1] = DataDictionaryGetOBCStateU8(GSD); //OBCStateU8; + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "GetServerStatus:", + ControlResponseBuffer, 2, &ClientSocket, 0); + SystemControlCommand = PreviousSystemControlCommand; + } + else if (SystemControlCommand != PreviousSystemControlCommand) { + LogMessage(LOG_LEVEL_WARNING, + "Command not allowed, SystemControl is busy in state %s, PreviousCommand: %s", + SystemControlStatesArr[server_state], + SystemControlCommandsArr[PreviousSystemControlCommand]); + SystemControlSendLog + ("[SystemControl] Command not allowed, SystemControl is busy in state INWORK.\n", + &ClientSocket, 0); + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE, "", + ControlResponseBuffer, 0, &ClientSocket, 0); + SystemControlCommand = PreviousSystemControlCommand; + } + } + + bzero(pcRecvBuffer, SC_RECV_MESSAGE_BUFFER); + bytesReceived = iCommRecv(&iCommand, pcRecvBuffer, SC_RECV_MESSAGE_BUFFER, NULL); + + switch (iCommand) { + case COMM_FAILURE: + if (server_state == SERVER_STATE_INWORK) { + enum COMMAND failedCommand = (enum COMMAND)pcRecvBuffer[0]; + + if (failedCommand == COMM_INIT && PreviousSystemControlCommand == InitializeScenario_0) { + server_state = SERVER_STATE_IDLE; + SystemControlCommand = Idle_0; + LogMessage(LOG_LEVEL_INFO, "Initialization failed"); + // TODO: report to user? + } + else { + LogMessage(LOG_LEVEL_ERROR, "Unhandled FAILURE (command: %u) reply in state %s", + pcRecvBuffer[0], SystemControlStatesArr[server_state]); + } + } + else { + LogMessage(LOG_LEVEL_WARNING, "Received unexpected FAILURE (command: %u) reply in state %s", + pcRecvBuffer[0], SystemControlStatesArr[server_state]); + // TODO: React more? + } + break; + case COMM_OBC_STATE: + break; + case COMM_LOG: + // This creates a problem in GUC: disabled it for now + //SystemControlSendLog(pcRecvBuffer, &ClientSocket, 0); + break; + case COMM_MONR: + // TODO: Decode + if (RVSSChannelSocket != 0 && RVSSConfigU32 & RVSS_MONR_CHANNEL && bytesReceived >= 0) { + UtilPopulateMonitorDataStruct(pcRecvBuffer, (size_t) bytesReceived, &monrData, 0); + SystemControlBuildRVSSMONRChannelMessage(RVSSData, &RVSSMessageLengthU32, monrData, 0); + UtilSendUDPData("SystemControl", &RVSSChannelSocket, &RVSSChannelAddr, RVSSData, + RVSSMessageLengthU32, 0); + } + + SystemControlBuildRVSSMONRChannelMessage(RVSSData, &RVSSMessageLengthU32, monrData, 0); + UtilSendUDPData("SystemControl", &RVSSChannelSocket, &RVSSChannelAddr, RVSSData, + RVSSMessageLengthU32, 0); + break; + case COMM_INV: + break; + default: + LogMessage(LOG_LEVEL_WARNING, "Unhandled message bus command: %u", iCommand); + } + + switch (SystemControlCommand) { + // can you access GetServerParameterList_0, GetServerParameter_1, SetServerParameter_2 and DISarmScenario and Exit from the GUI + case Idle_0: + break; + case GetServerStatus_0: + if (SystemControlCommand != PreviousSystemControlCommand) { + LogMessage(LOG_LEVEL_INFO, "State: %s, OBCState: %s, %d", + SystemControlStatesArr[server_state], + SystemControlOBCStatesArr[objectControlState], DataDictionaryGetOBCStateU8(GSD)); + } + SystemControlCommand = Idle_0; + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + ControlResponseBuffer[0] = server_state; + ControlResponseBuffer[1] = DataDictionaryGetOBCStateU8(GSD); //OBCStateU8; + LogMessage(LOG_LEVEL_DEBUG, "GPSMillisecondsU64: %ld", GPSTime->GPSMillisecondsU64); // GPSTime just ticks from 0 up shouldent it be in the global GPStime? + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "GetServerStatus:", + ControlResponseBuffer, 2, &ClientSocket, 0); + break; + case GetServerParameterList_0: + SystemControlCommand = Idle_0; + bzero(ParameterListC8, SYSTEM_CONTROL_SERVER_PARAMETER_LIST_SIZE); + SystemControlReadServerParameterList(ParameterListC8, 0); + SystemControlSendControlResponse(strlen(ParameterListC8) > + 0 ? SYSTEM_CONTROL_RESPONSE_CODE_OK : + SYSTEM_CONTROL_RESPONSE_CODE_NO_DATA, "GetServerParameterList:", + ParameterListC8, strlen(ParameterListC8), &ClientSocket, 0); + break; + case GetTestOrigin_0: + SystemControlCommand = Idle_0; + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + DataDictionaryGetOriginLatitudeC8(GSD, TextBuffer20, SMALL_BUFFER_SIZE_20); + strcat(ControlResponseBuffer, TextBuffer20); + strcat(ControlResponseBuffer, ";"); + DataDictionaryGetOriginLongitudeC8(GSD, TextBuffer20, SMALL_BUFFER_SIZE_20); + strcat(ControlResponseBuffer, TextBuffer20); + strcat(ControlResponseBuffer, ";"); + DataDictionaryGetOriginAltitudeC8(GSD, TextBuffer20, SMALL_BUFFER_SIZE_20); + strcat(ControlResponseBuffer, TextBuffer20); + strcat(ControlResponseBuffer, ";"); + iCommSend(COMM_OSEM, ControlResponseBuffer, sizeof (ControlResponseBuffer)); + SystemControlSendControlResponse(strlen(ParameterListC8) > + 0 ? SYSTEM_CONTROL_RESPONSE_CODE_OK : + SYSTEM_CONTROL_RESPONSE_CODE_NO_DATA, "GetTestOrigin:", + ControlResponseBuffer, strlen(ControlResponseBuffer), + &ClientSocket, 0); + break; + case GetServerParameter_1: + if (CurrentInputArgCount == CommandArgCount) { + SystemControlCommand = Idle_0; + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlGetServerParameter(GSD, SystemControlArgument[0], ControlResponseBuffer, + SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE, 0); + SystemControlSendControlResponse(strlen(ControlResponseBuffer) > + 0 ? SYSTEM_CONTROL_RESPONSE_CODE_OK : + SYSTEM_CONTROL_RESPONSE_CODE_NO_DATA, "GetServerParameter:", + ControlResponseBuffer, strlen(ControlResponseBuffer), + &ClientSocket, 0); + } + else { + LogMessage(LOG_LEVEL_ERROR, "Wrong parameter count in GetServerParameter(Name)!"); + SystemControlCommand = Idle_0; + } + break; + case SetServerParameter_2: + if (CurrentInputArgCount == CommandArgCount) { + SystemControlCommand = Idle_0; + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlSetServerParameter(GSD, SystemControlArgument[0], SystemControlArgument[1], 1); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "SetServerParameter:", + ControlResponseBuffer, 0, &ClientSocket, 0); + //Send COMM_DATA_DICT to notify to update data from DataDictionary + iCommSend(COMM_DATA_DICT, ControlResponseBuffer, sizeof (ControlResponseBuffer)); + + } + else { + LogMessage(LOG_LEVEL_ERROR, "Wrong parameter count in SetServerParameter(Name, Value)!"); + SystemControlCommand = Idle_0; + } + break; + case CheckFileDirectoryExist_1: + if (CurrentInputArgCount == CommandArgCount) { + SystemControlCommand = Idle_0; + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlCheckFileDirectoryExist(SystemControlArgument[0], ControlResponseBuffer, 0); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "CheckFileDirectoryExist:", + ControlResponseBuffer, 1, &ClientSocket, 0); + + } + else { + LogMessage(LOG_LEVEL_ERROR, "Wrong parameter count in CheckFFExist(path)!"); + SystemControlCommand = Idle_0; + } + break; + case DeleteFileDirectory_1: + if (CurrentInputArgCount == CommandArgCount) { + SystemControlCommand = Idle_0; + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlDeleteFileDirectory(SystemControlArgument[0], ControlResponseBuffer, 0); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "DeleteFileDirectory:", + ControlResponseBuffer, 1, &ClientSocket, 0); + + } + else { + LogMessage(LOG_LEVEL_ERROR, "Wrong parameter count in DeleteFileDirectory(path)!"); + SystemControlCommand = Idle_0; + } + break; + case CreateDirectory_1: + if (CurrentInputArgCount == CommandArgCount) { + SystemControlCommand = Idle_0; + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlCreateDirectory(SystemControlArgument[0], ControlResponseBuffer, 0); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "CreateDirectory:", + ControlResponseBuffer, 1, &ClientSocket, 0); + + } + else { + LogMessage(LOG_LEVEL_ERROR, "Wrong parameter count in CreateDirectory(path)!"); + SystemControlCommand = Idle_0; + } + break; + case GetDirectoryContent_1: + if (CurrentInputArgCount == CommandArgCount) { + SystemControlCommand = Idle_0; + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlCheckFileDirectoryExist(SystemControlArgument[0], ControlResponseBuffer, 0); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "GetDirectoryContent:", + ControlResponseBuffer, 1, &ClientSocket, 0); + if (ControlResponseBuffer[0] == FOLDER_EXIST) { + UtilCreateDirContent(SystemControlArgument[0], "dir.info"); + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlBuildFileContentInfo("dir.info", ControlResponseBuffer, 0); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "GetDirectoryContent:", + ControlResponseBuffer, 4, &ClientSocket, 0); + SystemControlSendFileContent(&ClientSocket, "dir.info", STR_SYSTEM_CONTROL_TX_PACKET_SIZE, + ControlResponseBuffer, REMOVE_FILE, 0); + } + + } + else { + LogMessage(LOG_LEVEL_ERROR, "Wrong parameter count in GetDirectoryContent(path)!"); + SystemControlCommand = Idle_0; + } + break; + case DownloadFile_1: + if (CurrentInputArgCount == CommandArgCount) { + SystemControlCommand = Idle_0; + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlCheckFileDirectoryExist(SystemControlArgument[0], ControlResponseBuffer, 0); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "DownloadFile:", + ControlResponseBuffer, 1, &ClientSocket, 0); + if (ControlResponseBuffer[0] == FILE_EXIST) { + UtilCreateDirContent(SystemControlArgument[0], SystemControlArgument[0]); + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlBuildFileContentInfo(SystemControlArgument[0], ControlResponseBuffer, 0); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "SubDownloadFile:", + ControlResponseBuffer, 4, &ClientSocket, 0); + SystemControlSendFileContent(&ClientSocket, SystemControlArgument[0], + STR_SYSTEM_CONTROL_TX_PACKET_SIZE, ControlResponseBuffer, + KEEP_FILE, 0); + } + + } + else { + LogMessage(LOG_LEVEL_ERROR, "Wrong parameter count in GetDirectoryContent(path)!"); + SystemControlCommand = Idle_0; + } + break; + case UploadFile_3: + if (CurrentInputArgCount == CommandArgCount) { + SystemControlCommand = Idle_0; + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlUploadFile(SystemControlArgument[0], SystemControlArgument[1], + SystemControlArgument[2], ControlResponseBuffer, 0); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "UploadFile:", + ControlResponseBuffer, 1, &ClientSocket, 0); + LogMessage(LOG_LEVEL_DEBUG, "UploadFile filelength: %s", SystemControlArgument[1]); + if (ControlResponseBuffer[0] == SERVER_PREPARED_BIG_PACKET_SIZE) //Server is ready to receive data + { + LogMessage(LOG_LEVEL_INFO, "Receiving file: %s", SystemControlArgument[0]); + SystemControlReceiveRxData(&ClientSocket, SystemControlArgument[0], + SystemControlArgument[1], STR_SYSTEM_CONTROL_RX_PACKET_SIZE, + ControlResponseBuffer, 0); + } + else if (ControlResponseBuffer[0] == PATH_INVALID_MISSING) { + LogMessage(LOG_LEVEL_INFO, "Failed receiving file: %s", SystemControlArgument[0]); + SystemControlReceiveRxData(&ClientSocket, "file.tmp", SystemControlArgument[1], + STR_SYSTEM_CONTROL_RX_PACKET_SIZE, ControlResponseBuffer, 0); + SystemControlDeleteFileDirectory("file.tmp", ControlResponseBuffer, 0); + ControlResponseBuffer[0] = PATH_INVALID_MISSING; + } + else { + LogMessage(LOG_LEVEL_INFO, "Receiving file: %s", SystemControlArgument[0]); + SystemControlReceiveRxData(&ClientSocket, SystemControlArgument[0], + SystemControlArgument[1], SystemControlArgument[2], + ControlResponseBuffer, 0); + } + + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "UploadFile:", + ControlResponseBuffer, 1, &ClientSocket, 0); + + } + else { + LogMessage(LOG_LEVEL_ERROR, + "Wrong parameter count in PrepFileRx(path, filesize, packetsize)!"); + SystemControlCommand = Idle_0; + } + break; + case InitializeScenario_0: + if (server_state == SERVER_STATE_IDLE && objectControlState == OBC_STATE_IDLE) { + if (iCommSend(COMM_INIT, pcBuffer, strlen(pcBuffer) + 1) < 0) { + LogMessage(LOG_LEVEL_ERROR, "Fatal communication fault when sending INIT command"); + server_state = SERVER_STATE_ERROR; + } + server_state = SERVER_STATE_INWORK; + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "InitializeScenario:", + ControlResponseBuffer, 0, &ClientSocket, 0); + DataDictionaryInitMONR(GSD); + + SystemControlSendLog("[SystemControl] Sending INIT.\n", &ClientSocket, 0); + } + else if (server_state == SERVER_STATE_INWORK && objectControlState == OBC_STATE_INITIALIZED) { + SystemControlSendLog + ("[SystemControl] Simulate that all objects becomes successfully configured.\n", + &ClientSocket, 0); + SystemControlCommand = Idle_0; + server_state = SERVER_STATE_IDLE; + } + else if (server_state == SERVER_STATE_IDLE) { + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE, + "InitializeScenario:", ControlResponseBuffer, 0, + &ClientSocket, 0); + SystemControlSendLog("[SystemControl] INIT received, state errors!\n", &ClientSocket, 0); + SystemControlCommand = PreviousSystemControlCommand; + } + break; + case ConnectObject_0: + if (server_state == SERVER_STATE_IDLE && objectControlState == OBC_STATE_INITIALIZED) { + if (iCommSend(COMM_CONNECT, pcBuffer, strlen(pcBuffer) + 1) < 0) { + LogMessage(LOG_LEVEL_ERROR, "Fatal communication fault when sending CONNECT command"); + server_state = SERVER_STATE_ERROR; + } + SystemControlCommand = Idle_0; + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "ConnectObject:", + ControlResponseBuffer, 0, &ClientSocket, 0); + SystemControlSendLog("[SystemControl] Sending CONNECT.\n", &ClientSocket, 0); + } + else if (server_state == SERVER_STATE_INWORK && objectControlState == OBC_STATE_CONNECTED) { + SystemControlSendLog("[SystemControl] Simulate that all objects are connected.\n", + &ClientSocket, 0); + SystemControlCommand = Idle_0; + server_state = SERVER_STATE_IDLE; + } + else if (server_state == SERVER_STATE_IDLE) { + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE, + "ConnectObject:", ControlResponseBuffer, 0, &ClientSocket, + 0); + SystemControlSendLog("[SystemControl] CONNECT received, state errors!\n", &ClientSocket, 0); + SystemControlCommand = PreviousSystemControlCommand; + } + break; + case DisconnectObject_0: + if (server_state == SERVER_STATE_IDLE) { + if (iCommSend(COMM_DISCONNECT, pcBuffer, strlen(pcBuffer) + 1) < 0) { + LogMessage(LOG_LEVEL_ERROR, "Fatal communication fault when sending DISCONNECT command"); + server_state = SERVER_STATE_ERROR; + } + SystemControlCommand = Idle_0; + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "DisconnectObject:", + ControlResponseBuffer, 0, &ClientSocket, 0); + SystemControlSendLog("[SystemControl] Sending DISCONNECT.\n", &ClientSocket, 0); + } + else { + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE, + "ConnectObject:", ControlResponseBuffer, 0, &ClientSocket, + 0); + SystemControlSendLog("[SystemControl] DISCONNECT received, state errors!\n", &ClientSocket, + 0); + SystemControlCommand = PreviousSystemControlCommand; + } + break; + case ArmScenario_0: + if (server_state == SERVER_STATE_IDLE && objectControlState == OBC_STATE_CONNECTED) { + server_state = SERVER_STATE_INWORK; + if (iCommSend(COMM_ARM, NULL, 0) < 0) { + LogMessage(LOG_LEVEL_ERROR, "Fatal communication fault when sending ARM command"); + server_state = SERVER_STATE_ERROR; + } + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "ArmScenario:", + ControlResponseBuffer, 0, &ClientSocket, 0); + SystemControlSendLog("[SystemControl] Sending ARM.\n", &ClientSocket, 0); + } + else if (server_state == SERVER_STATE_INWORK && objectControlState == OBC_STATE_ARMED) { + SystemControlSendLog("[SystemControl] Simulate that all objects become armed.\n", + &ClientSocket, 0); + + SystemControlCommand = Idle_0; + server_state = SERVER_STATE_IDLE; + } + else if (server_state == SERVER_STATE_IDLE) { + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE, + "ArmScenario:", ControlResponseBuffer, 0, &ClientSocket, 0); + SystemControlSendLog("[SystemControl] ARM received, state errors!\n", &ClientSocket, 0); + SystemControlCommand = PreviousSystemControlCommand; + } + break; + case DisarmScenario_0: + if (server_state == SERVER_STATE_IDLE && objectControlState == OBC_STATE_ARMED) { + server_state = SERVER_STATE_IDLE; + if (iCommSend(COMM_DISARM, NULL, 0) < 0) { + LogMessage(LOG_LEVEL_ERROR, "Fatal communication fault when sending DISARM command"); + server_state = SERVER_STATE_ERROR; + } + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "DisarmScenario:", + ControlResponseBuffer, 0, &ClientSocket, 0); + SystemControlSendLog("[SystemControl] Sending DISARM.\n", &ClientSocket, 0); + } + else if (server_state == SERVER_STATE_INWORK && objectControlState == OBC_STATE_CONNECTED) { + SystemControlSendLog("[SystemControl] Simulate that all objects become disarmed.\n", + &ClientSocket, 0); + SystemControlCommand = Idle_0; + server_state = SERVER_STATE_IDLE; + } + else if (server_state == SERVER_STATE_IDLE) { + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE, + "DisarmScenario:", ControlResponseBuffer, 0, &ClientSocket, + 0); + SystemControlSendLog("[SystemControl] DISARM received, state errors!\n", &ClientSocket, 0); + SystemControlCommand = PreviousSystemControlCommand; + } + break; + case StartScenario_1: + if (CurrentInputArgCount == CommandArgCount) { + if (server_state == SERVER_STATE_IDLE && objectControlState == OBC_STATE_ARMED) //Temporary! + { + bzero(pcBuffer, IPC_BUFFER_SIZE); + /* Lest use UTC time everywhere instead of etsi and gps time + gettimeofday(&tvTime, NULL); + uiTime = (uint64_t)tvTime.tv_sec*1000 + (uint64_t)tvTime.tv_usec/1000 - MS_FROM_1970_TO_2004_NO_LEAP_SECS + DIFF_LEAP_SECONDS_UTC_ETSI*1000; + */ + uiTime = UtilgetCurrentUTCtimeMS(); + if (TIME_COMPENSATE_LAGING_VM) + uiTime = uiTime - TIME_COMPENSATE_LAGING_VM_VAL; + + LogMessage(LOG_LEVEL_INFO, "Current timestamp (gtd): %lu", uiTime); + + //clock_gettime(CLOCK_MONOTONIC_COARSE, &tTime); + //clock_gettime(CLOCK_REALTIME, &tTime); + //uiTime = (uint64_t)tTime.tv_sec*1000 + (uint64_t)tTime.tv_nsec/1000000 - MS_FROM_1970_TO_2004_NO_LEAP_SECS + DIFF_LEAP_SECONDS_UTC_ETSI*1000; + //printf("[SystemControl] Current timestamp (cgt): %lu\n",uiTime ); + //printf("[SystemControl] Current timestamp: %lu\n",uiTime ); + + //uiTime += atoi(SystemControlArgument[0]); + uiTime = atoi(SystemControlArgument[0]); + DelayedStartU32 = atoi(SystemControlArgument[1]); + sprintf(pcBuffer, "%" PRIu8 ";%" PRIu64 ";%" PRIu32 ";", 0, uiTime, DelayedStartU32); + LogMessage(LOG_LEVEL_INFO, "Sending START <%s> (delayed +%s ms)", pcBuffer, + SystemControlArgument[1]); + + if (iCommSend(COMM_STRT, pcBuffer, strlen(pcBuffer) + 1) < 0) { + LogMessage(LOG_LEVEL_ERROR, "Fatal communication fault when sending STRT command"); + server_state = SERVER_STATE_ERROR; + } + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "StartScenario:", + ControlResponseBuffer, 0, &ClientSocket, 0); + server_state = SERVER_STATE_INWORK; + //server_state = SERVER_STATE_IDLE; //Temporary! + //SystemControlCommand = Idle_0; //Temporary! + } + else if (server_state == SERVER_STATE_INWORK && objectControlState == OBC_STATE_RUNNING) { + + SystemControlCommand = Idle_0; + server_state = SERVER_STATE_IDLE; + } + else if (server_state == SERVER_STATE_IDLE) { + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE, + "StartScenario:", ControlResponseBuffer, 0, + &ClientSocket, 0); + SystemControlSendLog("[SystemControl] START received, state errors!\n", &ClientSocket, 0); + SystemControlCommand = PreviousSystemControlCommand; + } + + } + else + LogMessage(LOG_LEVEL_WARNING, "START command parameter count error"); + break; + case stop_0: + if (iCommSend(COMM_STOP, NULL, 0) < 0) { + LogMessage(LOG_LEVEL_ERROR, "Fatal communication fault when sending STOP command"); + server_state = SERVER_STATE_ERROR; + } + else { + server_state = SERVER_STATE_IDLE; + SystemControlCommand = Idle_0; + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "stop:", + ControlResponseBuffer, 0, &ClientSocket, 0); + } + break; + case AbortScenario_0: + if (objectControlState == OBC_STATE_RUNNING + /* || strstr(SystemControlOBCStatesArr[OBCStateU8], "CONNECTED") != NULL + * || strstr(SystemControlOBCStatesArr[OBCStateU8], "ARMED") != NULL*/ ) + // Abort should only be allowed in running state + { + if (iCommSend(COMM_ABORT, NULL, 0) < 0) { + LogMessage(LOG_LEVEL_ERROR, "Fatal communication fault when sending ABORT command"); + server_state = SERVER_STATE_ERROR; + } + else { + server_state = SERVER_STATE_IDLE; + SystemControlCommand = Idle_0; + if (ClientSocket >= 0) { + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "AbortScenario:", + ControlResponseBuffer, 0, &ClientSocket, 0); + } + } + } + else { + if (ClientSocket >= 0) { + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE, + "AbortScenario:", ControlResponseBuffer, 0, + &ClientSocket, 0); + SystemControlSendLog("[SystemControl] ABORT received, state errors!\n", &ClientSocket, 0); + } + server_state = SERVER_STATE_IDLE; + SystemControlCommand = Idle_0; + } + break; + /* + case replay_1: + if(CurrentCommandArgCounter == CommandArgCount) + { + if(!strcmp(SystemControlArgument[CurrentCommandArgCounter],"-help")) + { + printf("[SystemControl] -----REPLAY-----\n"); + printf("[SystemControl] Syntax: replay [arg]\n"); + printf("[SystemControl] Ex: replay log/33/event.log\n"); + fflush(stdout); + } + else + { + (void)iCommSend(COMM_REPLAY, SystemControlArgument[CurrentCommandArgCounter]); + printf("[SystemControl] System control sending REPLAY on IPC <%s>\n", SystemControlArgument[CurrentCommandArgCounter]); + fflush(stdout); + } + SystemControlCommand = idle_0; + CurrentCommandArgCounter = 0; + } else CurrentCommandArgCounter ++; + break; + case control_0: + (void)iCommSend(COMM_CONTROL, NULL); + //printf("INF: System control sending CONTROL on IPC <%s>\n", pcBuffer); + fflush(stdout); + SystemControlCommand = idle_0; + CurrentCommandArgCounter = 0; + break; */ + case Exit_0: + + if (iCommSend(COMM_EXIT, NULL, 0) < 0) { + LogMessage(LOG_LEVEL_ERROR, "Fatal communication fault when sending EXIT command"); + server_state = SERVER_STATE_ERROR; + } + else { + iExit = 1; + GSD->ExitU8 = 1; + usleep(1000000); + SystemControlCommand = Idle_0; + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "Exit:", + ControlResponseBuffer, 0, &ClientSocket, 0); + close(ClientSocket); + ClientSocket = -1; + if (USE_LOCAL_USER_CONTROL == 0) { + close(ServerHandle); + ServerHandle = -1; + } + LogMessage(LOG_LEVEL_INFO, "Server closing"); + } + break; + + default: + + break; + } + + + + sleep_time.tv_sec = 0; + sleep_time.tv_nsec = SYSTEM_CONTROL_TASK_PERIOD_MS * 1000000; + ++RVSSSendCounterU16; + if (RVSSSendCounterU16 >= ((U16) RVSSRateDbl)) { + RVSSSendCounterU16 = 0; + DataDictionaryGetRVSSRateU8(GSD, &RVSSRateU8); + RVSSRateDbl = RVSSRateU8; + RVSSRateDbl = (1 / RVSSRateDbl) * 100; //This is strange!! Should be 1000, but if it is the RVSSData is sent to slow by a factor of 10. + + if (RVSSChannelSocket != 0 && RVSSSendCounterU16 == 0 && RVSSConfigU32 > 0) { + bzero(RVSSData, SYSTEM_CONTROL_RVSS_DATA_BUFFER); + + if (RVSSConfigU32 & RVSS_TIME_CHANNEL) { + SystemControlBuildRVSSTimeChannelMessage(RVSSData, &RVSSMessageLengthU32, GPSTime, 0); + UtilSendUDPData("SystemControl", &RVSSChannelSocket, &RVSSChannelAddr, RVSSData, + RVSSMessageLengthU32, 0); + } + + if (RVSSConfigU32 & RVSS_MAESTRO_CHANNEL) { + SystemControlBuildRVSSMaestroChannelMessage(RVSSData, &RVSSMessageLengthU32, GSD, + server_state, 0); + UtilSendUDPData("SystemControl", &RVSSChannelSocket, &RVSSChannelAddr, RVSSData, + RVSSMessageLengthU32, 0); + } + + if (RVSSConfigU32 & RVSS_ASP_CHANNEL) { + SystemControlBuildRVSSAspChannelMessage(RVSSData, &RVSSMessageLengthU32, 0); + UtilSendUDPData("SystemControl", &RVSSChannelSocket, &RVSSChannelAddr, RVSSData, + RVSSMessageLengthU32, 0); + } + + } + + + } + + sleep_time = (iCommand == COMM_INV && server_state != SERVER_STATE_INWORK && ClientResult < 0) ? mqEmptyPollPeriod : mqNonEmptyPollPeriod; - nanosleep(&sleep_time, &ref_time); - } + nanosleep(&sleep_time, &ref_time); + } - (void)iCommClose(); + (void)iCommClose(); - LogMessage(LOG_LEVEL_INFO, "Exiting"); + LogMessage(LOG_LEVEL_INFO, "Exiting"); } /*------------------------------------------------------------ -- Private functions ------------------------------------------------------------*/ void signalHandler(int signo) { - if (signo == SIGINT) { - LogMessage(LOG_LEVEL_WARNING, "Caught keyboard interrupt"); - iExit = 1; - } - else { - LogMessage(LOG_LEVEL_ERROR, "Caught unhandled signal"); - } + if (signo == SIGINT) { + LogMessage(LOG_LEVEL_WARNING, "Caught keyboard interrupt"); + iExit = 1; + } + else { + LogMessage(LOG_LEVEL_ERROR, "Caught unhandled signal"); + } } SystemControlCommand_t SystemControlFindCommand(const char *CommandBuffer, - SystemControlCommand_t * CurrentCommand, - int *CommandArgCount) { - - SystemControlCommand_t command; - char StrippedCommandBuffer[SYSTEM_CONTROL_COMMAND_MAX_LENGTH]; - - bzero(StrippedCommandBuffer, SYSTEM_CONTROL_COMMAND_MAX_LENGTH); - //printf("CommandBuffer: %s\n", CommandBuffer); - strncpy(StrippedCommandBuffer, CommandBuffer, - (uint64_t) strchr(CommandBuffer, '(') - (uint64_t) CommandBuffer); - //printf("StrippedCommandBuffer: %s\n", StrippedCommandBuffer); - - for (command = Idle_0; command != nocommand; command++) { - bzero(SystemControlCommandArgCnt, SYSTEM_CONTROL_ARG_CHAR_COUNT); - bzero(SystemControlStrippedCommand, SYSTEM_CONTROL_COMMAND_MAX_LENGTH); - strncpy(SystemControlStrippedCommand, SystemControlCommandsArr[(int)command], - (uint64_t) strchr(SystemControlCommandsArr[(int)command], - '_') - (uint64_t) SystemControlCommandsArr[(int)command]); - strncpy(SystemControlCommandArgCnt, strchr(SystemControlCommandsArr[(int)command], '_') + 1, - strlen(SystemControlCommandsArr[(int)command]) - - ((uint64_t) strchr(SystemControlCommandsArr[(int)command], '_') - - (uint64_t) SystemControlCommandsArr[(int)command] + 1)); - - if (!strcmp(SystemControlStrippedCommand, StrippedCommandBuffer)) { - { - *CommandArgCount = atoi(SystemControlCommandArgCnt); - *CurrentCommand = command; - return command; - } - } - } - return nocommand; + SystemControlCommand_t * CurrentCommand, + int *CommandArgCount) { + + SystemControlCommand_t command; + char StrippedCommandBuffer[SYSTEM_CONTROL_COMMAND_MAX_LENGTH]; + + bzero(StrippedCommandBuffer, SYSTEM_CONTROL_COMMAND_MAX_LENGTH); + //printf("CommandBuffer: %s\n", CommandBuffer); + strncpy(StrippedCommandBuffer, CommandBuffer, + (uint64_t) strchr(CommandBuffer, '(') - (uint64_t) CommandBuffer); + //printf("StrippedCommandBuffer: %s\n", StrippedCommandBuffer); + + for (command = Idle_0; command != nocommand; command++) { + bzero(SystemControlCommandArgCnt, SYSTEM_CONTROL_ARG_CHAR_COUNT); + bzero(SystemControlStrippedCommand, SYSTEM_CONTROL_COMMAND_MAX_LENGTH); + strncpy(SystemControlStrippedCommand, SystemControlCommandsArr[(int)command], + (uint64_t) strchr(SystemControlCommandsArr[(int)command], + '_') - (uint64_t) SystemControlCommandsArr[(int)command]); + strncpy(SystemControlCommandArgCnt, strchr(SystemControlCommandsArr[(int)command], '_') + 1, + strlen(SystemControlCommandsArr[(int)command]) - + ((uint64_t) strchr(SystemControlCommandsArr[(int)command], '_') - + (uint64_t) SystemControlCommandsArr[(int)command] + 1)); + + if (!strcmp(SystemControlStrippedCommand, StrippedCommandBuffer)) { + { + *CommandArgCount = atoi(SystemControlCommandArgCnt); + *CurrentCommand = command; + return command; + } + } + } + return nocommand; } /*! @@ -1230,334 +1230,334 @@ ssize_t SystemControlReceiveUserControlData(I32 socket, C8 * dataBuffer, size_t void SystemControlSendMONR(C8 * MONRStr, I32 * Sockfd, U8 Debug) { - int i, n, j, t; - C8 Length[4]; - C8 Header[2] = { 0, 2 }; - C8 Data[SYSTEM_CONTROL_SEND_BUFFER_SIZE]; - - bzero(Data, SYSTEM_CONTROL_SEND_BUFFER_SIZE); - n = 2 + strlen(MONRStr); - Length[0] = (C8) (n >> 24); - Length[1] = (C8) (n >> 16); - Length[2] = (C8) (n >> 8); - Length[3] = (C8) n; - - - if (n + 4 < SYSTEM_CONTROL_SEND_BUFFER_SIZE) { - for (i = 0, j = 0; i < 4; i++, j++) - Data[j] = Length[i]; - for (i = 0; i < 2; i++, j++) - Data[j] = Header[i]; - t = strlen(MONRStr); - for (i = 0; i < t; i++, j++) - Data[j] = *(MONRStr + i); - //SystemControlSendBytes(Data, n + 4, Sockfd, 0); - UtilSendTCPData("System Control", Data, n + 4, Sockfd, 0); - } - else - LogMessage(LOG_LEVEL_ERROR, "MONR string longer than %d bytes!", SYSTEM_CONTROL_SEND_BUFFER_SIZE); + int i, n, j, t; + C8 Length[4]; + C8 Header[2] = { 0, 2 }; + C8 Data[SYSTEM_CONTROL_SEND_BUFFER_SIZE]; + + bzero(Data, SYSTEM_CONTROL_SEND_BUFFER_SIZE); + n = 2 + strlen(MONRStr); + Length[0] = (C8) (n >> 24); + Length[1] = (C8) (n >> 16); + Length[2] = (C8) (n >> 8); + Length[3] = (C8) n; + + + if (n + 4 < SYSTEM_CONTROL_SEND_BUFFER_SIZE) { + for (i = 0, j = 0; i < 4; i++, j++) + Data[j] = Length[i]; + for (i = 0; i < 2; i++, j++) + Data[j] = Header[i]; + t = strlen(MONRStr); + for (i = 0; i < t; i++, j++) + Data[j] = *(MONRStr + i); + //SystemControlSendBytes(Data, n + 4, Sockfd, 0); + UtilSendTCPData("System Control", Data, n + 4, Sockfd, 0); + } + else + LogMessage(LOG_LEVEL_ERROR, "MONR string longer than %d bytes!", SYSTEM_CONTROL_SEND_BUFFER_SIZE); } void SystemControlSendLog(C8 * LogString, I32 * Sockfd, U8 Debug) { - int i, n, j, t; - C8 Length[4]; - C8 Header[2] = { 0, 2 }; - C8 Data[SYSTEM_CONTROL_SEND_BUFFER_SIZE]; - - bzero(Data, SYSTEM_CONTROL_SEND_BUFFER_SIZE); - n = 2 + strlen(LogString); - Length[0] = (C8) (n >> 24); - Length[1] = (C8) (n >> 16); - Length[2] = (C8) (n >> 8); - Length[3] = (C8) n; - - //SystemControlSendBytes(Length, 4, Sockfd, 0); - //SystemControlSendBytes(Header, 5, Sockfd, 0); - //SystemControlSendBytes(LogString, strlen(LogString), Sockfd, 0); - - - if (n + 4 < SYSTEM_CONTROL_SEND_BUFFER_SIZE) { - for (i = 0, j = 0; i < 4; i++, j++) - Data[j] = Length[i]; - for (i = 0; i < 2; i++, j++) - Data[j] = Header[i]; - t = strlen(LogString); - for (i = 0; i < t; i++, j++) - Data[j] = *(LogString + i); - //SystemControlSendBytes(Data, n + 4, Sockfd, 0); - UtilSendTCPData("System Control", Data, n + 4, Sockfd, 0); - } - else - LogMessage(LOG_LEVEL_ERROR, "Log string longer than %d bytes!", SYSTEM_CONTROL_SEND_BUFFER_SIZE); + int i, n, j, t; + C8 Length[4]; + C8 Header[2] = { 0, 2 }; + C8 Data[SYSTEM_CONTROL_SEND_BUFFER_SIZE]; + + bzero(Data, SYSTEM_CONTROL_SEND_BUFFER_SIZE); + n = 2 + strlen(LogString); + Length[0] = (C8) (n >> 24); + Length[1] = (C8) (n >> 16); + Length[2] = (C8) (n >> 8); + Length[3] = (C8) n; + + //SystemControlSendBytes(Length, 4, Sockfd, 0); + //SystemControlSendBytes(Header, 5, Sockfd, 0); + //SystemControlSendBytes(LogString, strlen(LogString), Sockfd, 0); + + + if (n + 4 < SYSTEM_CONTROL_SEND_BUFFER_SIZE) { + for (i = 0, j = 0; i < 4; i++, j++) + Data[j] = Length[i]; + for (i = 0; i < 2; i++, j++) + Data[j] = Header[i]; + t = strlen(LogString); + for (i = 0; i < t; i++, j++) + Data[j] = *(LogString + i); + //SystemControlSendBytes(Data, n + 4, Sockfd, 0); + UtilSendTCPData("System Control", Data, n + 4, Sockfd, 0); + } + else + LogMessage(LOG_LEVEL_ERROR, "Log string longer than %d bytes!", SYSTEM_CONTROL_SEND_BUFFER_SIZE); } void SystemControlSendControlResponse(U16 ResponseStatus, C8 * ResponseString, C8 * ResponseData, - I32 ResponseDataLength, I32 * Sockfd, U8 Debug) { - int i, n, j, t; - C8 Length[4]; - C8 Status[2]; - C8 Data[SYSTEM_CONTROL_SEND_BUFFER_SIZE]; - - bzero(Data, SYSTEM_CONTROL_SEND_BUFFER_SIZE); - n = 2 + strlen(ResponseString) + ResponseDataLength; - Length[0] = (C8) (n >> 24); - Length[1] = (C8) (n >> 16); - Length[2] = (C8) (n >> 8); - Length[3] = (C8) n; - Status[0] = (C8) (ResponseStatus >> 8); - Status[1] = (C8) ResponseStatus; - - if (n + 4 < SYSTEM_CONTROL_SEND_BUFFER_SIZE) { - for (i = 0, j = 0; i < 4; i++, j++) - Data[j] = Length[i]; - for (i = 0; i < 2; i++, j++) - Data[j] = Status[i]; - t = strlen(ResponseString); - for (i = 0; i < t; i++, j++) - Data[j] = *(ResponseString + i); - for (i = 0; i < ResponseDataLength; i++, j++) - Data[j] = ResponseData[i]; - - if (Debug) { - for (i = 0; i < n + 4; i++) - printf("%x-", Data[i]); - printf("\n"); - } - - //SystemControlSendBytes(Data, n + 4, Sockfd, 0); - UtilSendTCPData("System Control", Data, n + 4, Sockfd, 0); - } - else - LogMessage(LOG_LEVEL_ERROR, "Response data more than %d bytes!", SYSTEM_CONTROL_SEND_BUFFER_SIZE); + I32 ResponseDataLength, I32 * Sockfd, U8 Debug) { + int i, n, j, t; + C8 Length[4]; + C8 Status[2]; + C8 Data[SYSTEM_CONTROL_SEND_BUFFER_SIZE]; + + bzero(Data, SYSTEM_CONTROL_SEND_BUFFER_SIZE); + n = 2 + strlen(ResponseString) + ResponseDataLength; + Length[0] = (C8) (n >> 24); + Length[1] = (C8) (n >> 16); + Length[2] = (C8) (n >> 8); + Length[3] = (C8) n; + Status[0] = (C8) (ResponseStatus >> 8); + Status[1] = (C8) ResponseStatus; + + if (n + 4 < SYSTEM_CONTROL_SEND_BUFFER_SIZE) { + for (i = 0, j = 0; i < 4; i++, j++) + Data[j] = Length[i]; + for (i = 0; i < 2; i++, j++) + Data[j] = Status[i]; + t = strlen(ResponseString); + for (i = 0; i < t; i++, j++) + Data[j] = *(ResponseString + i); + for (i = 0; i < ResponseDataLength; i++, j++) + Data[j] = ResponseData[i]; + + if (Debug) { + for (i = 0; i < n + 4; i++) + printf("%x-", Data[i]); + printf("\n"); + } + + //SystemControlSendBytes(Data, n + 4, Sockfd, 0); + UtilSendTCPData("System Control", Data, n + 4, Sockfd, 0); + } + else + LogMessage(LOG_LEVEL_ERROR, "Response data more than %d bytes!", SYSTEM_CONTROL_SEND_BUFFER_SIZE); } I32 SystemControlBuildControlResponse(U16 ResponseStatus, C8 * ResponseString, C8 * ResponseData, - I32 ResponseDataLength, U8 Debug) { - int i = 0, n = 0, j = 0, t = 0; - C8 Length[4]; - C8 Status[2]; - C8 Data[SYSTEM_CONTROL_SEND_BUFFER_SIZE]; - - bzero(Data, SYSTEM_CONTROL_SEND_BUFFER_SIZE); - n = 2 + strlen(ResponseString) + ResponseDataLength; - Length[0] = (C8) (n >> 24); - Length[1] = (C8) (n >> 16); - Length[2] = (C8) (n >> 8); - Length[3] = (C8) n; - Status[0] = (C8) (ResponseStatus >> 8); - Status[1] = (C8) ResponseStatus; - - if (n + 4 < SYSTEM_CONTROL_SEND_BUFFER_SIZE) { - for (i = 0, j = 0; i < 4; i++, j++) - Data[j] = Length[i]; - for (i = 0; i < 2; i++, j++) - Data[j] = Status[i]; - t = strlen(ResponseString); - for (i = 0; i < t; i++, j++) - Data[j] = *(ResponseString + i); - for (i = 0; i < ResponseDataLength; i++, j++) - Data[j] = ResponseData[i]; - - for (i = 0; i < n; i++) - *(ResponseData + i) = Data[i]; //Copy back - - if (Debug) { - for (i = 0; i < n + 4; i++) - printf("%x-", Data[i]); - printf("\n"); - } + I32 ResponseDataLength, U8 Debug) { + int i = 0, n = 0, j = 0, t = 0; + C8 Length[4]; + C8 Status[2]; + C8 Data[SYSTEM_CONTROL_SEND_BUFFER_SIZE]; + + bzero(Data, SYSTEM_CONTROL_SEND_BUFFER_SIZE); + n = 2 + strlen(ResponseString) + ResponseDataLength; + Length[0] = (C8) (n >> 24); + Length[1] = (C8) (n >> 16); + Length[2] = (C8) (n >> 8); + Length[3] = (C8) n; + Status[0] = (C8) (ResponseStatus >> 8); + Status[1] = (C8) ResponseStatus; + + if (n + 4 < SYSTEM_CONTROL_SEND_BUFFER_SIZE) { + for (i = 0, j = 0; i < 4; i++, j++) + Data[j] = Length[i]; + for (i = 0; i < 2; i++, j++) + Data[j] = Status[i]; + t = strlen(ResponseString); + for (i = 0; i < t; i++, j++) + Data[j] = *(ResponseString + i); + for (i = 0; i < ResponseDataLength; i++, j++) + Data[j] = ResponseData[i]; + + for (i = 0; i < n; i++) + *(ResponseData + i) = Data[i]; //Copy back + + if (Debug) { + for (i = 0; i < n + 4; i++) + printf("%x-", Data[i]); + printf("\n"); + } - } - else - LogMessage(LOG_LEVEL_ERROR, "Response data more than %d bytes!", SYSTEM_CONTROL_SEND_BUFFER_SIZE); + } + else + LogMessage(LOG_LEVEL_ERROR, "Response data more than %d bytes!", SYSTEM_CONTROL_SEND_BUFFER_SIZE); - return n; + return n; } static void SystemControlSendBytes(const char *data, int length, int *sockfd, int debug) { - int i, n; + int i, n; - if (debug == 1) { - printf("Bytes sent: "); - int i = 0; + if (debug == 1) { + printf("Bytes sent: "); + int i = 0; - for (i = 0; i < length; i++) - printf("%d ", (C8) * (data + i)); - printf("\n"); - } + for (i = 0; i < length; i++) + printf("%d ", (C8) * (data + i)); + printf("\n"); + } - n = write(*sockfd, data, length); - if (n < 0) { - util_error("[SystemControl] ERR: Failed to send on control socket"); - } + n = write(*sockfd, data, length); + if (n < 0) { + util_error("[SystemControl] ERR: Failed to send on control socket"); + } } static I32 SystemControlInitServer(int *ClientSocket, int *ServerHandle, struct in_addr *ip_addr) { - struct sockaddr_in command_server_addr; - struct sockaddr_in cli_addr; - socklen_t cli_length; - unsigned int control_port = SYSTEM_CONTROL_CONTROL_PORT; - int optval = 1; - int result = 0; - int sockFlags = 0; - - /* Init user control socket */ - LogMessage(LOG_LEVEL_INFO, "Init control socket"); - - *ServerHandle = socket(AF_INET, SOCK_STREAM, 0); - if (*ServerHandle < 0) { - perror("[SystemControl] ERR: Failed to create control socket"); - exit(1); - } + struct sockaddr_in command_server_addr; + struct sockaddr_in cli_addr; + socklen_t cli_length; + unsigned int control_port = SYSTEM_CONTROL_CONTROL_PORT; + int optval = 1; + int result = 0; + int sockFlags = 0; + + /* Init user control socket */ + LogMessage(LOG_LEVEL_INFO, "Init control socket"); + + *ServerHandle = socket(AF_INET, SOCK_STREAM, 0); + if (*ServerHandle < 0) { + perror("[SystemControl] ERR: Failed to create control socket"); + exit(1); + } - bzero((char *)&command_server_addr, sizeof (command_server_addr)); + bzero((char *)&command_server_addr, sizeof (command_server_addr)); - command_server_addr.sin_family = AF_INET; - command_server_addr.sin_addr.s_addr = INADDR_ANY; - command_server_addr.sin_port = htons(control_port); + command_server_addr.sin_family = AF_INET; + command_server_addr.sin_addr.s_addr = INADDR_ANY; + command_server_addr.sin_port = htons(control_port); - optval = 1; - result = setsockopt(*ServerHandle, SOL_SOCKET, SO_REUSEADDR, &optval, sizeof optval); + optval = 1; + result = setsockopt(*ServerHandle, SOL_SOCKET, SO_REUSEADDR, &optval, sizeof optval); - if (result < 0) { - perror("[SystemControl] ERR: Failed to call setsockopt"); - exit(1); - } + if (result < 0) { + perror("[SystemControl] ERR: Failed to call setsockopt"); + exit(1); + } - if (bind(*ServerHandle, (struct sockaddr *)&command_server_addr, sizeof (command_server_addr)) < 0) { - perror("[SystemControl] ERR: Failed to bind to control socket"); - exit(1); - } + if (bind(*ServerHandle, (struct sockaddr *)&command_server_addr, sizeof (command_server_addr)) < 0) { + perror("[SystemControl] ERR: Failed to bind to control socket"); + exit(1); + } - /* Monitor and control sockets up. Wait for central to connect to control socket to get server address */ - LogMessage(LOG_LEVEL_INFO, "Listening for connection from client..."); + /* Monitor and control sockets up. Wait for central to connect to control socket to get server address */ + LogMessage(LOG_LEVEL_INFO, "Listening for connection from client..."); - listen(*ServerHandle, 1); - cli_length = sizeof (cli_addr); + listen(*ServerHandle, 1); + cli_length = sizeof (cli_addr); - /* Set socket to nonblocking */ - sockFlags = fcntl(*ServerHandle, F_GETFL, 0); - if (sockFlags == -1) - util_error("Error calling fcntl"); + /* Set socket to nonblocking */ + sockFlags = fcntl(*ServerHandle, F_GETFL, 0); + if (sockFlags == -1) + util_error("Error calling fcntl"); - sockFlags = sockFlags | O_NONBLOCK; - if (fcntl(*ServerHandle, F_SETFL, sockFlags)) - util_error("Error calling fcntl"); + sockFlags = sockFlags | O_NONBLOCK; + if (fcntl(*ServerHandle, F_SETFL, sockFlags)) + util_error("Error calling fcntl"); - do { - *ClientSocket = accept(*ServerHandle, (struct sockaddr *)&cli_addr, &cli_length); - if ((*ClientSocket == -1 && errno != EAGAIN && errno != EWOULDBLOCK) || iExit) - util_error("Failed to establish connection"); - } while (*ClientSocket == -1); + do { + *ClientSocket = accept(*ServerHandle, (struct sockaddr *)&cli_addr, &cli_length); + if ((*ClientSocket == -1 && errno != EAGAIN && errno != EWOULDBLOCK) || iExit) + util_error("Failed to establish connection"); + } while (*ClientSocket == -1); - LogMessage(LOG_LEVEL_INFO, "Connection established: %s:%i", inet_ntoa(cli_addr.sin_addr), - htons(command_server_addr.sin_port)); + LogMessage(LOG_LEVEL_INFO, "Connection established: %s:%i", inet_ntoa(cli_addr.sin_addr), + htons(command_server_addr.sin_port)); - ip_addr->s_addr = cli_addr.sin_addr.s_addr; //Set IP-address of Usercontrol + ip_addr->s_addr = cli_addr.sin_addr.s_addr; //Set IP-address of Usercontrol - if (*ClientSocket < 0) { - perror("[SystemControl] ERR: Failed to accept from central"); - exit(1); - } + if (*ClientSocket < 0) { + perror("[SystemControl] ERR: Failed to accept from central"); + exit(1); + } - return result; + return result; } static I32 SystemControlConnectServer(int *sockfd, const char *name, const uint32_t port) { - struct sockaddr_in serv_addr; - struct hostent *server; + struct sockaddr_in serv_addr; + struct hostent *server; - char buffer[256]; - int iResult; + char buffer[256]; + int iResult; - *sockfd = socket(AF_INET, SOCK_STREAM, 0); + *sockfd = socket(AF_INET, SOCK_STREAM, 0); - if (*sockfd < 0) { - util_error("[SystemControl] ERR: Failed to open control socket"); - } + if (*sockfd < 0) { + util_error("[SystemControl] ERR: Failed to open control socket"); + } - server = gethostbyname(name); - if (server == NULL) { - util_error("[SystemControl] ERR: Unknown host "); - } + server = gethostbyname(name); + if (server == NULL) { + util_error("[SystemControl] ERR: Unknown host "); + } - bzero((char *)&serv_addr, sizeof (serv_addr)); - serv_addr.sin_family = AF_INET; + bzero((char *)&serv_addr, sizeof (serv_addr)); + serv_addr.sin_family = AF_INET; - bcopy((char *)server->h_addr, (char *)&serv_addr.sin_addr.s_addr, server->h_length); - serv_addr.sin_port = htons(port); + bcopy((char *)server->h_addr, (char *)&serv_addr.sin_addr.s_addr, server->h_length); + serv_addr.sin_port = htons(port); - LogMessage(LOG_LEVEL_INFO, "Attempting to connect to control socket: %s:%i", name, port); + LogMessage(LOG_LEVEL_INFO, "Attempting to connect to control socket: %s:%i", name, port); - do { - iResult = connect(*sockfd, (struct sockaddr *)&serv_addr, sizeof (serv_addr)); + do { + iResult = connect(*sockfd, (struct sockaddr *)&serv_addr, sizeof (serv_addr)); - if (iResult < 0) { - if (errno == ECONNREFUSED) { - LogMessage(LOG_LEVEL_WARNING, "Unable to connect to UserControl, retrying in 3 sec..."); - (void)sleep(3); - } - else { - util_error("[SystemControl] ERR: Failed to connect to control socket"); - } - } - } while (iResult < 0); + if (iResult < 0) { + if (errno == ECONNREFUSED) { + LogMessage(LOG_LEVEL_WARNING, "Unable to connect to UserControl, retrying in 3 sec..."); + (void)sleep(3); + } + else { + util_error("[SystemControl] ERR: Failed to connect to control socket"); + } + } + } while (iResult < 0); - iResult = fcntl(*sockfd, F_SETFL, fcntl(*sockfd, F_GETFL, 0) | O_NONBLOCK); + iResult = fcntl(*sockfd, F_SETFL, fcntl(*sockfd, F_GETFL, 0) | O_NONBLOCK); - LogMessage(LOG_LEVEL_DEBUG, "Maestro connected to UserControl: %s:%i", name, port); - return iResult; + LogMessage(LOG_LEVEL_DEBUG, "Maestro connected to UserControl: %s:%i", name, port); + return iResult; } static void SystemControlCreateProcessChannel(const C8 * name, const U32 port, I32 * sockfd, - struct sockaddr_in *addr) { - int result; - struct hostent *object; + struct sockaddr_in *addr) { + int result; + struct hostent *object; - /* Connect to object safety socket */ + /* Connect to object safety socket */ - LogMessage(LOG_LEVEL_DEBUG, "Creating process channel socket"); + LogMessage(LOG_LEVEL_DEBUG, "Creating process channel socket"); - *sockfd = socket(AF_INET, SOCK_DGRAM, 0); - if (*sockfd < 0) { - util_error("[SystemControl] ERR: Failed to connect to process channel socket"); - } + *sockfd = socket(AF_INET, SOCK_DGRAM, 0); + if (*sockfd < 0) { + util_error("[SystemControl] ERR: Failed to connect to process channel socket"); + } - /* Set address to object */ - object = gethostbyname(name); + /* Set address to object */ + object = gethostbyname(name); - if (object == 0) { - util_error("[SystemControl] ERR: Unknown host"); - } + if (object == 0) { + util_error("[SystemControl] ERR: Unknown host"); + } - bcopy((char *)object->h_addr, (char *)&addr->sin_addr.s_addr, object->h_length); - addr->sin_family = AF_INET; - addr->sin_port = htons(port); + bcopy((char *)object->h_addr, (char *)&addr->sin_addr.s_addr, object->h_length); + addr->sin_family = AF_INET; + addr->sin_port = htons(port); - /* set socket to non-blocking */ - result = fcntl(*sockfd, F_SETFL, fcntl(*sockfd, F_GETFL, 0) | O_NONBLOCK); - if (result < 0) { - util_error("[SystemControl] ERR: calling fcntl"); - } + /* set socket to non-blocking */ + result = fcntl(*sockfd, F_SETFL, fcntl(*sockfd, F_GETFL, 0) | O_NONBLOCK); + if (result < 0) { + util_error("[SystemControl] ERR: calling fcntl"); + } - LogMessage(LOG_LEVEL_INFO, "Created process channel socket and address: %s:%d", name, port); + LogMessage(LOG_LEVEL_INFO, "Created process channel socket and address: %s:%d", name, port); } /*! @@ -1566,45 +1566,45 @@ static void SystemControlCreateProcessChannel(const C8 * name, const U32 port, I * \return true if match, false if not */ C8 SystemControlVerifyHostAddress(char *addr) { - struct ifaddrs *ifaddr, *ifa; - int family, s, n; - char host[NI_MAXHOST]; - - if (getifaddrs(&ifaddr) == -1) { - LogMessage(LOG_LEVEL_ERROR, "Could not get interface data"); - freeifaddrs(ifaddr); - return 0; - } + struct ifaddrs *ifaddr, *ifa; + int family, s, n; + char host[NI_MAXHOST]; + + if (getifaddrs(&ifaddr) == -1) { + LogMessage(LOG_LEVEL_ERROR, "Could not get interface data"); + freeifaddrs(ifaddr); + return 0; + } - // Iterate over linked list using ifa - for (ifa = ifaddr, n = 0; ifa != NULL; ifa = ifa->ifa_next, n++) { - if (ifa->ifa_addr == NULL) { - // Interface had no addresses, skip to next - continue; - } - - family = ifa->ifa_addr->sa_family; - if (family == AF_INET || family == AF_INET6) { - s = getnameinfo(ifa->ifa_addr, - (family == AF_INET) ? sizeof (struct sockaddr_in) : - sizeof (struct sockaddr_in6), host, NI_MAXHOST, NULL, 0, NI_NUMERICHOST); - if (s != 0) { - LogMessage(LOG_LEVEL_ERROR, "getnameinfo() failed: %s", gai_strerror(s)); - continue; - } - - if (strcmp(host, addr) == 0) - return 1; - else - continue; - } - else { - continue; - } - } + // Iterate over linked list using ifa + for (ifa = ifaddr, n = 0; ifa != NULL; ifa = ifa->ifa_next, n++) { + if (ifa->ifa_addr == NULL) { + // Interface had no addresses, skip to next + continue; + } - freeifaddrs(ifaddr); - return 0; + family = ifa->ifa_addr->sa_family; + if (family == AF_INET || family == AF_INET6) { + s = getnameinfo(ifa->ifa_addr, + (family == AF_INET) ? sizeof (struct sockaddr_in) : + sizeof (struct sockaddr_in6), host, NI_MAXHOST, NULL, 0, NI_NUMERICHOST); + if (s != 0) { + LogMessage(LOG_LEVEL_ERROR, "getnameinfo() failed: %s", gai_strerror(s)); + continue; + } + + if (strcmp(host, addr) == 0) + return 1; + else + continue; + } + else { + continue; + } + } + + freeifaddrs(ifaddr); + return 0; } /* @@ -1624,445 +1624,445 @@ I32 SystemControlSendUDPData(I32 *sockfd, struct sockaddr_in* addr, C8 *SendData */ I32 SystemControlGetServerParameter(GSDType * GSD, C8 * ParameterName, C8 * ReturnValue, U32 BufferLength, - U8 Debug) { - bzero(ReturnValue, 20); - dbl ValueDbl = 0; - U32 ValueU32 = 0; - U16 ValueU16 = 0; - U8 ValueU8 = 0; - - if (strcmp("OrigoLatitude", ParameterName) == 0) { - DataDictionaryGetOriginLatitudeDbl(GSD, &ValueDbl); - sprintf(ReturnValue, "%3.12f", ValueDbl); - } - else if (strcmp("OrigoLongitude", ParameterName) == 0) { - DataDictionaryGetOriginLongitudeDbl(GSD, &ValueDbl); - sprintf(ReturnValue, "%3.12f", ValueDbl); - } - else if (strcmp("OrigoAltitude", ParameterName) == 0) { - DataDictionaryGetOriginAltitudeDbl(GSD, &ValueDbl); - sprintf(ReturnValue, "%3.12f", ValueDbl); - } - else if (strcmp("VisualizationServerName", ParameterName) == 0) { - DataDictionaryGetVisualizationServerC8(GSD, ReturnValue, BufferLength); - } - else if (strcmp("ForceObjectToLocalhost", ParameterName) == 0) { - DataDictionaryGetForceToLocalhostU8(GSD, &ValueU8); - sprintf(ReturnValue, "%" PRIu8, ValueU8); - } - else if (strcmp("ASPMaxTimeDiff", ParameterName) == 0) { - DataDictionaryGetASPMaxTimeDiffDbl(GSD, &ValueDbl); - sprintf(ReturnValue, "%3.3f", ValueDbl); - } - else if (strcmp("ASPMaxTrajDiff", ParameterName) == 0) { - DataDictionaryGetASPMaxTrajDiffDbl(GSD, &ValueDbl); - sprintf(ReturnValue, "%3.3f", ValueDbl); - } - else if (strcmp("ASPStepBackCount", ParameterName) == 0) { - DataDictionaryGetASPStepBackCountU32(GSD, &ValueU32); - sprintf(ReturnValue, "%" PRIu32, ValueU32); - } - else if (strcmp("ASPFilterLevel", ParameterName) == 0) { - DataDictionaryGetASPFilterLevelDbl(GSD, &ValueDbl); - sprintf(ReturnValue, "%3.3f", ValueDbl); - } - else if (strcmp("ASPMaxDeltaTime", ParameterName) == 0) { - DataDictionaryGetASPMaxDeltaTimeDbl(GSD, &ValueDbl); - sprintf(ReturnValue, "%3.3f", ValueDbl); - } - else if (strcmp("TimeServerIP", ParameterName) == 0) { - DataDictionaryGetTimeServerIPC8(GSD, ReturnValue, BufferLength); - } - else if (strcmp("TimeServerPort", ParameterName) == 0) { - DataDictionaryGetTimeServerPortU16(GSD, &ValueU16); - sprintf(ReturnValue, "%" PRIu16, ValueU16); - } - else if (strcmp("SimulatorIP", ParameterName) == 0) { - DataDictionaryGetSimulatorIPC8(GSD, ReturnValue, BufferLength); - } - else if (strcmp("SimulatorTCPPort", ParameterName) == 0) { - DataDictionaryGetSimulatorTCPPortU16(GSD, &ValueU16); - sprintf(ReturnValue, "%" PRIu16, ValueU16); - } - else if (strcmp("SimulatorUDPPort", ParameterName) == 0) { - DataDictionaryGetSimulatorUDPPortU16(GSD, &ValueU16); - sprintf(ReturnValue, "%" PRIu16, ValueU16); - } - else if (strcmp("SimulatorMode", ParameterName) == 0) { - DataDictionaryGetSimulatorModeU8(GSD, &ValueU8); - sprintf(ReturnValue, "%" PRIu8, ValueU8); - } - else if (strcmp("VOILReceivers", ParameterName) == 0) { - DataDictionaryGetVOILReceiversC8(GSD, ReturnValue, BufferLength); - } - else if (strcmp("DTMReceivers", ParameterName) == 0) { - DataDictionaryGetDTMReceiversC8(GSD, ReturnValue, BufferLength); - } - else if (strcmp("SupervisorIP", ParameterName) == 0) { - DataDictionaryGetExternalSupervisorIPC8(GSD, ReturnValue, BufferLength); - } - else if (strcmp("SupervisorTCPPort", ParameterName) == 0) { - DataDictionaryGetSupervisorTCPPortU16(GSD, &ValueU16); - sprintf(ReturnValue, "%" PRIu16, ValueU16); - } - else if (strcmp("MiscData", ParameterName) == 0) { - DataDictionaryGetMiscDataC8(GSD, ReturnValue, BufferLength); - } - else if (strcmp("RVSSConfig", ParameterName) == 0) { - DataDictionaryGetRVSSConfigU32(GSD, &ValueU32); - sprintf(ReturnValue, "%" PRIu32, ValueU32); - } - else if (strcmp("RVSSRate", ParameterName) == 0) { - DataDictionaryGetRVSSRateU8(GSD, &ValueU8); - sprintf(ReturnValue, "%" PRIu8, ValueU8); - } + U8 Debug) { + bzero(ReturnValue, 20); + dbl ValueDbl = 0; + U32 ValueU32 = 0; + U16 ValueU16 = 0; + U8 ValueU8 = 0; + + if (strcmp("OrigoLatitude", ParameterName) == 0) { + DataDictionaryGetOriginLatitudeDbl(GSD, &ValueDbl); + sprintf(ReturnValue, "%3.12f", ValueDbl); + } + else if (strcmp("OrigoLongitude", ParameterName) == 0) { + DataDictionaryGetOriginLongitudeDbl(GSD, &ValueDbl); + sprintf(ReturnValue, "%3.12f", ValueDbl); + } + else if (strcmp("OrigoAltitude", ParameterName) == 0) { + DataDictionaryGetOriginAltitudeDbl(GSD, &ValueDbl); + sprintf(ReturnValue, "%3.12f", ValueDbl); + } + else if (strcmp("VisualizationServerName", ParameterName) == 0) { + DataDictionaryGetVisualizationServerC8(GSD, ReturnValue, BufferLength); + } + else if (strcmp("ForceObjectToLocalhost", ParameterName) == 0) { + DataDictionaryGetForceToLocalhostU8(GSD, &ValueU8); + sprintf(ReturnValue, "%" PRIu8, ValueU8); + } + else if (strcmp("ASPMaxTimeDiff", ParameterName) == 0) { + DataDictionaryGetASPMaxTimeDiffDbl(GSD, &ValueDbl); + sprintf(ReturnValue, "%3.3f", ValueDbl); + } + else if (strcmp("ASPMaxTrajDiff", ParameterName) == 0) { + DataDictionaryGetASPMaxTrajDiffDbl(GSD, &ValueDbl); + sprintf(ReturnValue, "%3.3f", ValueDbl); + } + else if (strcmp("ASPStepBackCount", ParameterName) == 0) { + DataDictionaryGetASPStepBackCountU32(GSD, &ValueU32); + sprintf(ReturnValue, "%" PRIu32, ValueU32); + } + else if (strcmp("ASPFilterLevel", ParameterName) == 0) { + DataDictionaryGetASPFilterLevelDbl(GSD, &ValueDbl); + sprintf(ReturnValue, "%3.3f", ValueDbl); + } + else if (strcmp("ASPMaxDeltaTime", ParameterName) == 0) { + DataDictionaryGetASPMaxDeltaTimeDbl(GSD, &ValueDbl); + sprintf(ReturnValue, "%3.3f", ValueDbl); + } + else if (strcmp("TimeServerIP", ParameterName) == 0) { + DataDictionaryGetTimeServerIPC8(GSD, ReturnValue, BufferLength); + } + else if (strcmp("TimeServerPort", ParameterName) == 0) { + DataDictionaryGetTimeServerPortU16(GSD, &ValueU16); + sprintf(ReturnValue, "%" PRIu16, ValueU16); + } + else if (strcmp("SimulatorIP", ParameterName) == 0) { + DataDictionaryGetSimulatorIPC8(GSD, ReturnValue, BufferLength); + } + else if (strcmp("SimulatorTCPPort", ParameterName) == 0) { + DataDictionaryGetSimulatorTCPPortU16(GSD, &ValueU16); + sprintf(ReturnValue, "%" PRIu16, ValueU16); + } + else if (strcmp("SimulatorUDPPort", ParameterName) == 0) { + DataDictionaryGetSimulatorUDPPortU16(GSD, &ValueU16); + sprintf(ReturnValue, "%" PRIu16, ValueU16); + } + else if (strcmp("SimulatorMode", ParameterName) == 0) { + DataDictionaryGetSimulatorModeU8(GSD, &ValueU8); + sprintf(ReturnValue, "%" PRIu8, ValueU8); + } + else if (strcmp("VOILReceivers", ParameterName) == 0) { + DataDictionaryGetVOILReceiversC8(GSD, ReturnValue, BufferLength); + } + else if (strcmp("DTMReceivers", ParameterName) == 0) { + DataDictionaryGetDTMReceiversC8(GSD, ReturnValue, BufferLength); + } + else if (strcmp("SupervisorIP", ParameterName) == 0) { + DataDictionaryGetExternalSupervisorIPC8(GSD, ReturnValue, BufferLength); + } + else if (strcmp("SupervisorTCPPort", ParameterName) == 0) { + DataDictionaryGetSupervisorTCPPortU16(GSD, &ValueU16); + sprintf(ReturnValue, "%" PRIu16, ValueU16); + } + else if (strcmp("MiscData", ParameterName) == 0) { + DataDictionaryGetMiscDataC8(GSD, ReturnValue, BufferLength); + } + else if (strcmp("RVSSConfig", ParameterName) == 0) { + DataDictionaryGetRVSSConfigU32(GSD, &ValueU32); + sprintf(ReturnValue, "%" PRIu32, ValueU32); + } + else if (strcmp("RVSSRate", ParameterName) == 0) { + DataDictionaryGetRVSSRateU8(GSD, &ValueU8); + sprintf(ReturnValue, "%" PRIu8, ValueU8); + } } I32 SystemControlSetServerParameter(GSDType * GSD, C8 * ParameterName, C8 * NewValue, U8 Debug) { - if (Debug) - printf("[SystemControl] SetServerParameter: %s = %s\n", ParameterName, NewValue); - if (strcmp("OrigoLatitude", ParameterName) == 0) - DataDictionarySetOriginLatitudeDbl(GSD, NewValue); - else if (strcmp("OrigoLongitude", ParameterName) == 0) - DataDictionarySetOriginLongitudeDbl(GSD, NewValue); - else if (strcmp("OrigoAltitude", ParameterName) == 0) - DataDictionarySetOriginAltitudeDbl(GSD, NewValue); - else if (strcmp("VisualizationServerName", ParameterName) == 0) - DataDictionarySetVisualizationServerU32(GSD, NewValue); - else if (strcmp("ForceObjectToLocalhost", ParameterName) == 0) - DataDictionarySetForceToLocalhostU8(GSD, NewValue); - else if (strcmp("ASPMaxTimeDiff", ParameterName) == 0) - DataDictionarySetASPMaxTimeDiffDbl(GSD, NewValue); - else if (strcmp("ASPMaxTrajDiff", ParameterName) == 0) - DataDictionarySetASPMaxTrajDiffDbl(GSD, NewValue); - else if (strcmp("ASPStepBackCount", ParameterName) == 0) - DataDictionarySetASPStepBackCountU32(GSD, NewValue); - else if (strcmp("ASPFilterLevel", ParameterName) == 0) - DataDictionarySetASPFilterLevelDbl(GSD, NewValue); - else if (strcmp("ASPMaxDeltaTime", ParameterName) == 0) - DataDictionarySetASPMaxDeltaTimeDbl(GSD, NewValue); - else if (strcmp("TimeServerIP", ParameterName) == 0) - DataDictionarySetTimeServerIPU32(GSD, NewValue); - else if (strcmp("TimeServerPort", ParameterName) == 0) - DataDictionarySetTimeServerPortU16(GSD, NewValue); - else if (strcmp("SimulatorIP", ParameterName) == 0) - DataDictionarySetSimulatorIPU32(GSD, NewValue); - else if (strcmp("SimulatorTCPPort", ParameterName) == 0) - DataDictionarySetSimulatorTCPPortU16(GSD, NewValue); - else if (strcmp("SimulatorUDPPort", ParameterName) == 0) - DataDictionarySetSimulatorUDPPortU16(GSD, NewValue); - else if (strcmp("SimulatorMode", ParameterName) == 0) - DataDictionarySetSimulatorModeU8(GSD, NewValue); - else if (strcmp("VOILReceivers", ParameterName) == 0) - DataDictionarySetVOILReceiversC8(GSD, NewValue); - else if (strcmp("DTMReceivers", ParameterName) == 0) - DataDictionarySetDTMReceiversC8(GSD, NewValue); - else if (strcmp("SupervisorIP", ParameterName) == 0) - DataDictionarySetExternalSupervisorIPU32(GSD, NewValue); - else if (strcmp("SupervisorTCPPort", ParameterName) == 0) - DataDictionarySetSupervisorTCPPortU16(GSD, NewValue); - else if (strcmp("MiscData", ParameterName) == 0) - DataDictionarySetMiscDataC8(GSD, NewValue); - else if (strcmp("RVSSConfig", ParameterName) == 0) - DataDictionarySetRVSSConfigU32(GSD, (U32) atoi(NewValue)); - else if (strcmp("RVSSRate", ParameterName) == 0) - DataDictionarySetRVSSRateU8(GSD, (U32) atoi(NewValue)); + if (Debug) + printf("[SystemControl] SetServerParameter: %s = %s\n", ParameterName, NewValue); + if (strcmp("OrigoLatitude", ParameterName) == 0) + DataDictionarySetOriginLatitudeDbl(GSD, NewValue); + else if (strcmp("OrigoLongitude", ParameterName) == 0) + DataDictionarySetOriginLongitudeDbl(GSD, NewValue); + else if (strcmp("OrigoAltitude", ParameterName) == 0) + DataDictionarySetOriginAltitudeDbl(GSD, NewValue); + else if (strcmp("VisualizationServerName", ParameterName) == 0) + DataDictionarySetVisualizationServerU32(GSD, NewValue); + else if (strcmp("ForceObjectToLocalhost", ParameterName) == 0) + DataDictionarySetForceToLocalhostU8(GSD, NewValue); + else if (strcmp("ASPMaxTimeDiff", ParameterName) == 0) + DataDictionarySetASPMaxTimeDiffDbl(GSD, NewValue); + else if (strcmp("ASPMaxTrajDiff", ParameterName) == 0) + DataDictionarySetASPMaxTrajDiffDbl(GSD, NewValue); + else if (strcmp("ASPStepBackCount", ParameterName) == 0) + DataDictionarySetASPStepBackCountU32(GSD, NewValue); + else if (strcmp("ASPFilterLevel", ParameterName) == 0) + DataDictionarySetASPFilterLevelDbl(GSD, NewValue); + else if (strcmp("ASPMaxDeltaTime", ParameterName) == 0) + DataDictionarySetASPMaxDeltaTimeDbl(GSD, NewValue); + else if (strcmp("TimeServerIP", ParameterName) == 0) + DataDictionarySetTimeServerIPU32(GSD, NewValue); + else if (strcmp("TimeServerPort", ParameterName) == 0) + DataDictionarySetTimeServerPortU16(GSD, NewValue); + else if (strcmp("SimulatorIP", ParameterName) == 0) + DataDictionarySetSimulatorIPU32(GSD, NewValue); + else if (strcmp("SimulatorTCPPort", ParameterName) == 0) + DataDictionarySetSimulatorTCPPortU16(GSD, NewValue); + else if (strcmp("SimulatorUDPPort", ParameterName) == 0) + DataDictionarySetSimulatorUDPPortU16(GSD, NewValue); + else if (strcmp("SimulatorMode", ParameterName) == 0) + DataDictionarySetSimulatorModeU8(GSD, NewValue); + else if (strcmp("VOILReceivers", ParameterName) == 0) + DataDictionarySetVOILReceiversC8(GSD, NewValue); + else if (strcmp("DTMReceivers", ParameterName) == 0) + DataDictionarySetDTMReceiversC8(GSD, NewValue); + else if (strcmp("SupervisorIP", ParameterName) == 0) + DataDictionarySetExternalSupervisorIPU32(GSD, NewValue); + else if (strcmp("SupervisorTCPPort", ParameterName) == 0) + DataDictionarySetSupervisorTCPPortU16(GSD, NewValue); + else if (strcmp("MiscData", ParameterName) == 0) + DataDictionarySetMiscDataC8(GSD, NewValue); + else if (strcmp("RVSSConfig", ParameterName) == 0) + DataDictionarySetRVSSConfigU32(GSD, (U32) atoi(NewValue)); + else if (strcmp("RVSSRate", ParameterName) == 0) + DataDictionarySetRVSSRateU8(GSD, (U32) atoi(NewValue)); } I32 SystemControlWriteServerParameter(C8 * ParameterName, C8 * NewValue, U8 Debug) { - I32 RowCount, i; - C8 Parameter[SMALL_BUFFER_SIZE_64]; - C8 Row[SMALL_BUFFER_SIZE_128]; - C8 NewRow[SMALL_BUFFER_SIZE_128]; - FILE *fd, *TempFd; - C8 *ptr1, *ptr2; - U8 ParameterFound = 0; - char confPathFileDir[MAX_FILE_PATH]; - char tempConfPathFileDir[MAX_FILE_PATH]; - const char TEMP_FILE_NAME[] = "temp-" MODULE_NAME ".conf"; - - UtilGetConfDirectoryPath(confPathFileDir, sizeof (confPathFileDir)); - strcpy(tempConfPathFileDir, confPathFileDir); - strcat(confPathFileDir, CONF_FILE_NAME); - strcat(tempConfPathFileDir, TEMP_FILE_NAME); - - bzero(Parameter, SMALL_BUFFER_SIZE_64); - - strcat(Parameter, ParameterName); - strcat(Parameter, "="); - - //Remove temporary file - remove(tempConfPathFileDir); - - //Create temporary file - TempFd = fopen(tempConfPathFileDir, "w+"); - - //Open configuration file - fd = fopen(confPathFileDir, "r"); - - if (fd > 0) { - RowCount = UtilCountFileRows(fd); - fclose(fd); - fd = fopen(confPathFileDir, "r"); - - for (i = 0; i < RowCount; i++) { - bzero(Row, SMALL_BUFFER_SIZE_128); - UtilReadLine(fd, Row); - - ptr1 = strstr(Row, Parameter); - ptr2 = strstr(Row, "//"); - if (ptr2 == NULL) - ptr2 = ptr1; //No comment found - if (ptr1 != NULL && (U64) ptr2 >= (U64) ptr1 && ParameterFound == 0) { - ParameterFound = 1; - bzero(NewRow, SMALL_BUFFER_SIZE_128); - strncpy(NewRow, Row, (U64) ptr1 - (U64) Row + strlen(Parameter)); - strcat(NewRow, NewValue); - if ((U64) ptr2 > (U64) ptr1) { - strcat(NewRow, " "); // Add space - strcat(NewRow, ptr2); // Add the comment - } - - if (Debug) { - LogMessage(LOG_LEVEL_DEBUG, "Changed parameter: %s", NewRow); - } - - strcat(NewRow, "\n"); - (void)fwrite(NewRow, 1, strlen(NewRow), TempFd); - - } - else { - strcat(Row, "\n"); - (void)fwrite(Row, 1, strlen(Row), TempFd); - } - } - fclose(TempFd); - fclose(fd); - - //Remove test.conf - remove(confPathFileDir); - - //Rename temp.conf to test.conf - rename(tempConfPathFileDir, confPathFileDir); - - //Remove temporary file - remove(tempConfPathFileDir); - } + I32 RowCount, i; + C8 Parameter[SMALL_BUFFER_SIZE_64]; + C8 Row[SMALL_BUFFER_SIZE_128]; + C8 NewRow[SMALL_BUFFER_SIZE_128]; + FILE *fd, *TempFd; + C8 *ptr1, *ptr2; + U8 ParameterFound = 0; + char confPathFileDir[MAX_FILE_PATH]; + char tempConfPathFileDir[MAX_FILE_PATH]; + const char TEMP_FILE_NAME[] = "temp-" MODULE_NAME ".conf"; + + UtilGetConfDirectoryPath(confPathFileDir, sizeof (confPathFileDir)); + strcpy(tempConfPathFileDir, confPathFileDir); + strcat(confPathFileDir, CONF_FILE_NAME); + strcat(tempConfPathFileDir, TEMP_FILE_NAME); + + bzero(Parameter, SMALL_BUFFER_SIZE_64); + + strcat(Parameter, ParameterName); + strcat(Parameter, "="); + + //Remove temporary file + remove(tempConfPathFileDir); + + //Create temporary file + TempFd = fopen(tempConfPathFileDir, "w+"); + + //Open configuration file + fd = fopen(confPathFileDir, "r"); + + if (fd > 0) { + RowCount = UtilCountFileRows(fd); + fclose(fd); + fd = fopen(confPathFileDir, "r"); + + for (i = 0; i < RowCount; i++) { + bzero(Row, SMALL_BUFFER_SIZE_128); + UtilReadLine(fd, Row); + + ptr1 = strstr(Row, Parameter); + ptr2 = strstr(Row, "//"); + if (ptr2 == NULL) + ptr2 = ptr1; //No comment found + if (ptr1 != NULL && (U64) ptr2 >= (U64) ptr1 && ParameterFound == 0) { + ParameterFound = 1; + bzero(NewRow, SMALL_BUFFER_SIZE_128); + strncpy(NewRow, Row, (U64) ptr1 - (U64) Row + strlen(Parameter)); + strcat(NewRow, NewValue); + if ((U64) ptr2 > (U64) ptr1) { + strcat(NewRow, " "); // Add space + strcat(NewRow, ptr2); // Add the comment + } + + if (Debug) { + LogMessage(LOG_LEVEL_DEBUG, "Changed parameter: %s", NewRow); + } + + strcat(NewRow, "\n"); + (void)fwrite(NewRow, 1, strlen(NewRow), TempFd); - return 0; + } + else { + strcat(Row, "\n"); + (void)fwrite(Row, 1, strlen(Row), TempFd); + } + } + fclose(TempFd); + fclose(fd); + + //Remove test.conf + remove(confPathFileDir); + + //Rename temp.conf to test.conf + rename(tempConfPathFileDir, confPathFileDir); + + //Remove temporary file + remove(tempConfPathFileDir); + } + + return 0; } I32 SystemControlReadServerParameter(C8 * ParameterName, C8 * ReturnValue, U8 Debug) { - I32 RowCount, i; - C8 TextBuffer[SMALL_BUFFER_SIZE_128]; - char confPathDir[MAX_FILE_PATH]; + I32 RowCount, i; + C8 TextBuffer[SMALL_BUFFER_SIZE_128]; + char confPathDir[MAX_FILE_PATH]; - UtilGetConfDirectoryPath(confPathDir, sizeof (confPathDir)); - strcat(confPathDir, CONF_FILE_NAME); + UtilGetConfDirectoryPath(confPathDir, sizeof (confPathDir)); + strcat(confPathDir, CONF_FILE_NAME); - bzero(TextBuffer, SMALL_BUFFER_SIZE_128); + bzero(TextBuffer, SMALL_BUFFER_SIZE_128); - strcat(TextBuffer, ParameterName); - strcat(TextBuffer, "="); + strcat(TextBuffer, ParameterName); + strcat(TextBuffer, "="); - UtilSearchTextFile(confPathDir, TextBuffer, "", ReturnValue); + UtilSearchTextFile(confPathDir, TextBuffer, "", ReturnValue); - if (Debug) { - LogPrint("%s = %s\n", ParameterName, ReturnValue); - } + if (Debug) { + LogPrint("%s = %s\n", ParameterName, ReturnValue); + } - return strlen(ReturnValue); + return strlen(ReturnValue); } I32 SystemControlReadServerParameterList(C8 * ParameterList, U8 Debug) { - I32 RowCount, i; - C8 TextBuffer[SMALL_BUFFER_SIZE_128]; - FILE *fd; - char confPathDir[MAX_FILE_PATH]; - - UtilGetConfDirectoryPath(confPathDir, sizeof (confPathDir)); - strcat(confPathDir, CONF_FILE_NAME); - - fd = fopen(confPathDir, "r"); - if (fd > 0) { - RowCount = UtilCountFileRows(fd); - fclose(fd); - fd = fopen(confPathDir, "r"); - - for (i = 0; i < RowCount; i++) { - bzero(TextBuffer, SMALL_BUFFER_SIZE_128); - UtilReadLineCntSpecChars(fd, TextBuffer); - if (strlen(TextBuffer) > 0) { - strcat(ParameterList, TextBuffer); - strcat(ParameterList, ";"); - } - } - - fclose(fd); - } + I32 RowCount, i; + C8 TextBuffer[SMALL_BUFFER_SIZE_128]; + FILE *fd; + char confPathDir[MAX_FILE_PATH]; + + UtilGetConfDirectoryPath(confPathDir, sizeof (confPathDir)); + strcat(confPathDir, CONF_FILE_NAME); + + fd = fopen(confPathDir, "r"); + if (fd > 0) { + RowCount = UtilCountFileRows(fd); + fclose(fd); + fd = fopen(confPathDir, "r"); + + for (i = 0; i < RowCount; i++) { + bzero(TextBuffer, SMALL_BUFFER_SIZE_128); + UtilReadLineCntSpecChars(fd, TextBuffer); + if (strlen(TextBuffer) > 0) { + strcat(ParameterList, TextBuffer); + strcat(ParameterList, ";"); + } + } - if (Debug) { - LogMessage(LOG_LEVEL_INFO, "ParameterList = %s\n", ParameterList); - } + fclose(fd); + } - return strlen(ParameterList); + if (Debug) { + LogMessage(LOG_LEVEL_INFO, "ParameterList = %s\n", ParameterList); + } + + return strlen(ParameterList); } I32 SystemControlBuildFileContentInfo(C8 * Path, C8 * ReturnValue, U8 Debug) { - struct stat st; - C8 CompletePath[MAX_FILE_PATH]; + struct stat st; + C8 CompletePath[MAX_FILE_PATH]; - bzero(CompletePath, MAX_FILE_PATH); - UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); - strcat(CompletePath, Path); + bzero(CompletePath, MAX_FILE_PATH); + UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); + strcat(CompletePath, Path); - stat(CompletePath, &st); - *(ReturnValue + 0) = (U8) (st.st_size >> 24); - *(ReturnValue + 1) = (U8) (st.st_size >> 16); - *(ReturnValue + 2) = (U8) (st.st_size >> 8); - *(ReturnValue + 3) = (U8) st.st_size; + stat(CompletePath, &st); + *(ReturnValue + 0) = (U8) (st.st_size >> 24); + *(ReturnValue + 1) = (U8) (st.st_size >> 16); + *(ReturnValue + 2) = (U8) (st.st_size >> 8); + *(ReturnValue + 3) = (U8) st.st_size; - if (Debug) - LogMessage(LOG_LEVEL_DEBUG, "Filesize %d of %s", (I32) st.st_size, CompletePath); + if (Debug) + LogMessage(LOG_LEVEL_DEBUG, "Filesize %d of %s", (I32) st.st_size, CompletePath); - return 0; + return 0; } I32 SystemControlCheckFileDirectoryExist(C8 * ParameterName, C8 * ReturnValue, U8 Debug) { - DIR *pDir; - FILE *fd; - C8 CompletePath[MAX_FILE_PATH]; + DIR *pDir; + FILE *fd; + C8 CompletePath[MAX_FILE_PATH]; - bzero(CompletePath, MAX_FILE_PATH); - UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); - strcat(CompletePath, ParameterName); + bzero(CompletePath, MAX_FILE_PATH); + UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); + strcat(CompletePath, ParameterName); - *ReturnValue = PATH_INVALID_MISSING; + *ReturnValue = PATH_INVALID_MISSING; - pDir = opendir(CompletePath); - if (pDir == NULL) { - fd = fopen(CompletePath, "r"); - if (fd != NULL) { - *ReturnValue = FILE_EXIST; //File exist - fclose(fd); - } - } - else { - *ReturnValue = FOLDER_EXIST; //Directory exist - closedir(pDir); - } + pDir = opendir(CompletePath); + if (pDir == NULL) { + fd = fopen(CompletePath, "r"); + if (fd != NULL) { + *ReturnValue = FILE_EXIST; //File exist + fclose(fd); + } + } + else { + *ReturnValue = FOLDER_EXIST; //Directory exist + closedir(pDir); + } - if (Debug) - LogPrint("%d %s", *ReturnValue, CompletePath); + if (Debug) + LogPrint("%d %s", *ReturnValue, CompletePath); - return 0; + return 0; } I32 SystemControlDeleteFileDirectory(C8 * Path, C8 * ReturnValue, U8 Debug) { - DIR *pDir; - FILE *fd; - C8 CompletePath[MAX_FILE_PATH]; - - bzero(CompletePath, MAX_FILE_PATH); - UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); - strcat(CompletePath, Path); - - *ReturnValue = PATH_INVALID_MISSING; - - pDir = opendir(CompletePath); - if (pDir == NULL) { - fd = fopen(CompletePath, "r"); - if (fd == NULL) { - *ReturnValue = PATH_INVALID_MISSING; //Missing file - } - else { - if (0 == remove(CompletePath)) //Delete file - { - *ReturnValue = SUCCEDED_DELETE; - } - else { - *ReturnValue = FAILED_DELETE; - } - } - } - else { - if (0 == remove(CompletePath)) //Delete directory - { - *ReturnValue = SUCCEDED_DELETE; - } - else { - *ReturnValue = FAILED_DELETE; - } - } + DIR *pDir; + FILE *fd; + C8 CompletePath[MAX_FILE_PATH]; - if (*ReturnValue == SUCCEDED_DELETE) - LogMessage(LOG_LEVEL_INFO, "Deleted %s", CompletePath); - else if (*ReturnValue == FAILED_DELETE) - LogMessage(LOG_LEVEL_INFO, "Failed to delete %s", CompletePath); + bzero(CompletePath, MAX_FILE_PATH); + UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); + strcat(CompletePath, Path); - return 0; + *ReturnValue = PATH_INVALID_MISSING; + + pDir = opendir(CompletePath); + if (pDir == NULL) { + fd = fopen(CompletePath, "r"); + if (fd == NULL) { + *ReturnValue = PATH_INVALID_MISSING; //Missing file + } + else { + if (0 == remove(CompletePath)) //Delete file + { + *ReturnValue = SUCCEDED_DELETE; + } + else { + *ReturnValue = FAILED_DELETE; + } + } + } + else { + if (0 == remove(CompletePath)) //Delete directory + { + *ReturnValue = SUCCEDED_DELETE; + } + else { + *ReturnValue = FAILED_DELETE; + } + } + + if (*ReturnValue == SUCCEDED_DELETE) + LogMessage(LOG_LEVEL_INFO, "Deleted %s", CompletePath); + else if (*ReturnValue == FAILED_DELETE) + LogMessage(LOG_LEVEL_INFO, "Failed to delete %s", CompletePath); + + return 0; } I32 SystemControlCreateDirectory(C8 * Path, C8 * ReturnValue, U8 Debug) { - DIR *pDir; - FILE *fd; - C8 CompletePath[MAX_FILE_PATH]; - - bzero(CompletePath, MAX_FILE_PATH); - UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); - strcat(CompletePath, Path); - - *ReturnValue = PATH_INVALID_MISSING; - - pDir = opendir(CompletePath); - if (pDir == NULL) { - fd = fopen(CompletePath, "r"); - if (fd != NULL) { - *ReturnValue = FILE_EXIST; //This is a file! - fclose(fd); - } - else { - if (0 == mkdir(CompletePath, S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH)) //Make the new directory - { - *ReturnValue = SUCCEDED_CREATE_FOLDER; - } - else { - *ReturnValue = FAILED_CREATE_FOLDER; - } - } - } - else { - *ReturnValue = FOLDER_EXIST; //Directory exist - closedir(pDir); - } + DIR *pDir; + FILE *fd; + C8 CompletePath[MAX_FILE_PATH]; - if (Debug) - LogPrint("%d %s", *(ReturnValue), CompletePath); + bzero(CompletePath, MAX_FILE_PATH); + UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); + strcat(CompletePath, Path); - if (*ReturnValue == SUCCEDED_CREATE_FOLDER) - LogMessage(LOG_LEVEL_INFO, "Directory created: %s", CompletePath); + *ReturnValue = PATH_INVALID_MISSING; - return 0; + pDir = opendir(CompletePath); + if (pDir == NULL) { + fd = fopen(CompletePath, "r"); + if (fd != NULL) { + *ReturnValue = FILE_EXIST; //This is a file! + fclose(fd); + } + else { + if (0 == mkdir(CompletePath, S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH)) //Make the new directory + { + *ReturnValue = SUCCEDED_CREATE_FOLDER; + } + else { + *ReturnValue = FAILED_CREATE_FOLDER; + } + } + } + else { + *ReturnValue = FOLDER_EXIST; //Directory exist + closedir(pDir); + } + + if (Debug) + LogPrint("%d %s", *(ReturnValue), CompletePath); + + if (*ReturnValue == SUCCEDED_CREATE_FOLDER) + LogMessage(LOG_LEVEL_INFO, "Directory created: %s", CompletePath); + + return 0; } @@ -2070,205 +2070,205 @@ I32 SystemControlCreateDirectory(C8 * Path, C8 * ReturnValue, U8 Debug) { I32 SystemControlUploadFile(C8 * Path, C8 * FileSize, C8 * PacketSize, C8 * ReturnValue, U8 Debug) { - FILE *fd; - C8 CompletePath[MAX_FILE_PATH]; + FILE *fd; + C8 CompletePath[MAX_FILE_PATH]; - bzero(CompletePath, MAX_FILE_PATH); - UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); - strcat(CompletePath, Path); + bzero(CompletePath, MAX_FILE_PATH); + UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); + strcat(CompletePath, Path); - if (Debug) { - LogMessage(LOG_LEVEL_DEBUG, "Upload file:"); - LogMessage(LOG_LEVEL_DEBUG, "%s", Path); - LogMessage(LOG_LEVEL_DEBUG, "%s", FileSize); - LogMessage(LOG_LEVEL_DEBUG, "%s", PacketSize); - LogMessage(LOG_LEVEL_DEBUG, "%s", CompletePath); - } + if (Debug) { + LogMessage(LOG_LEVEL_DEBUG, "Upload file:"); + LogMessage(LOG_LEVEL_DEBUG, "%s", Path); + LogMessage(LOG_LEVEL_DEBUG, "%s", FileSize); + LogMessage(LOG_LEVEL_DEBUG, "%s", PacketSize); + LogMessage(LOG_LEVEL_DEBUG, "%s", CompletePath); + } - if (atoi(PacketSize) > SYSTEM_CONTROL_RX_PACKET_SIZE) //Check packet size - { - *ReturnValue = SERVER_PREPARED_BIG_PACKET_SIZE; - return 0; - } + if (atoi(PacketSize) > SYSTEM_CONTROL_RX_PACKET_SIZE) //Check packet size + { + *ReturnValue = SERVER_PREPARED_BIG_PACKET_SIZE; + return 0; + } - fd = fopen(CompletePath, "r"); - if (fd != NULL) { - fclose(fd); - remove(CompletePath); //Remove file if exist - } + fd = fopen(CompletePath, "r"); + if (fd != NULL) { + fclose(fd); + remove(CompletePath); //Remove file if exist + } - fd = fopen(CompletePath, "w+"); //Create the file - if (fd != NULL) { - *ReturnValue = SERVER_PREPARED; //Server prepared - fclose(fd); - return 0; - } - else { - //ok, path invalid create temporary file - bzero(CompletePath, MAX_FILE_PATH); - UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); - strcat(CompletePath, "file.tmp"); - fd = fopen(CompletePath, "r"); - if (fd != NULL) { - fclose(fd); - remove(CompletePath); //Remove file if exist - } - fd = fopen(CompletePath, "w+"); //Create the temporary file - - *ReturnValue = PATH_INVALID_MISSING; - - return 0; - } + fd = fopen(CompletePath, "w+"); //Create the file + if (fd != NULL) { + *ReturnValue = SERVER_PREPARED; //Server prepared + fclose(fd); + return 0; + } + else { + //ok, path invalid create temporary file + bzero(CompletePath, MAX_FILE_PATH); + UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); + strcat(CompletePath, "file.tmp"); + fd = fopen(CompletePath, "r"); + if (fd != NULL) { + fclose(fd); + remove(CompletePath); //Remove file if exist + } + fd = fopen(CompletePath, "w+"); //Create the temporary file - return 0; + *ReturnValue = PATH_INVALID_MISSING; + + return 0; + } + + return 0; } I32 SystemControlReceiveRxData(I32 * sockfd, C8 * Path, C8 * FileSize, C8 * PacketSize, C8 * ReturnValue, - U8 Debug) { - - FILE *fd; - C8 CompletePath[MAX_FILE_PATH]; - - bzero(CompletePath, MAX_FILE_PATH); - UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); - strcat(CompletePath, Path); - U32 FileSizeU32 = atoi(FileSize); - U16 PacketSizeU16 = atoi(PacketSize); - I32 ClientStatus = 0, Time1 = 0, Time2 = 0, TimeDiff = 0, i = 0, j = 0; - C8 RxBuffer[SYSTEM_CONTROL_RX_PACKET_SIZE]; - U32 TotalRxCount = 0, TransmissionCount = (U32) (FileSizeU32 / PacketSizeU16), RestCount = - FileSizeU32 % PacketSizeU16; - struct timeval CurTime; - - - if (Debug) { - LogMessage(LOG_LEVEL_DEBUG, "Receive Rx data:"); - LogMessage(LOG_LEVEL_DEBUG, "%s", Path); - LogMessage(LOG_LEVEL_DEBUG, "%s", FileSize); - LogMessage(LOG_LEVEL_DEBUG, "%s", PacketSize); - LogMessage(LOG_LEVEL_DEBUG, "%s", CompletePath); - } - + U8 Debug) { + + FILE *fd; + C8 CompletePath[MAX_FILE_PATH]; + + bzero(CompletePath, MAX_FILE_PATH); + UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); + strcat(CompletePath, Path); + U32 FileSizeU32 = atoi(FileSize); + U16 PacketSizeU16 = atoi(PacketSize); + I32 ClientStatus = 0, Time1 = 0, Time2 = 0, TimeDiff = 0, i = 0, j = 0; + C8 RxBuffer[SYSTEM_CONTROL_RX_PACKET_SIZE]; + U32 TotalRxCount = 0, TransmissionCount = (U32) (FileSizeU32 / PacketSizeU16), RestCount = + FileSizeU32 % PacketSizeU16; + struct timeval CurTime; + + + if (Debug) { + LogMessage(LOG_LEVEL_DEBUG, "Receive Rx data:"); + LogMessage(LOG_LEVEL_DEBUG, "%s", Path); + LogMessage(LOG_LEVEL_DEBUG, "%s", FileSize); + LogMessage(LOG_LEVEL_DEBUG, "%s", PacketSize); + LogMessage(LOG_LEVEL_DEBUG, "%s", CompletePath); + } - fd = fopen(CompletePath, "w+"); - if (fd != NULL) { - gettimeofday(&CurTime, NULL); - Time1 = CurTime.tv_sec * 1000 + CurTime.tv_usec / 1000; + fd = fopen(CompletePath, "w+"); + if (fd != NULL) { - while (TotalRxCount < FileSizeU32 && TimeDiff < 3000) { - gettimeofday(&CurTime, NULL); - Time2 = CurTime.tv_sec * 1000 + CurTime.tv_usec / 1000; + gettimeofday(&CurTime, NULL); + Time1 = CurTime.tv_sec * 1000 + CurTime.tv_usec / 1000; + while (TotalRxCount < FileSizeU32 && TimeDiff < 3000) { + gettimeofday(&CurTime, NULL); + Time2 = CurTime.tv_sec * 1000 + CurTime.tv_usec / 1000; - if (i == TransmissionCount) { - PacketSizeU16 = RestCount; - } - bzero(RxBuffer, PacketSizeU16); - ClientStatus = recv(*sockfd, RxBuffer, PacketSizeU16, MSG_WAITALL); + if (i == TransmissionCount) { + PacketSizeU16 = RestCount; + } - if (ClientStatus > 0) { - i++; - fwrite(RxBuffer, 1, ClientStatus, fd); - fflush(fd); - if (Debug) { - printf("%d, %d, %d, %d, %d, %d :", i, ClientStatus, TotalRxCount, TimeDiff, PacketSizeU16, - FileSizeU32); - for (j = 0; j < 10; j++) - printf("%x-", RxBuffer[j]); - printf("...\n"); - } - TotalRxCount = TotalRxCount + ClientStatus; - gettimeofday(&CurTime, NULL); - Time1 = CurTime.tv_sec * 1000 + CurTime.tv_usec / 1000; - } + bzero(RxBuffer, PacketSizeU16); + ClientStatus = recv(*sockfd, RxBuffer, PacketSizeU16, MSG_WAITALL); + + if (ClientStatus > 0) { + i++; + fwrite(RxBuffer, 1, ClientStatus, fd); + fflush(fd); + if (Debug) { + printf("%d, %d, %d, %d, %d, %d :", i, ClientStatus, TotalRxCount, TimeDiff, PacketSizeU16, + FileSizeU32); + for (j = 0; j < 10; j++) + printf("%x-", RxBuffer[j]); + printf("...\n"); + } + TotalRxCount = TotalRxCount + ClientStatus; + gettimeofday(&CurTime, NULL); + Time1 = CurTime.tv_sec * 1000 + CurTime.tv_usec / 1000; + } - TimeDiff = abs(Time1 - Time2); - } + TimeDiff = abs(Time1 - Time2); + } - fclose(fd); + fclose(fd); - if (TotalRxCount == FileSizeU32) { - *ReturnValue = FILE_UPLOADED; - } - else if (TotalRxCount > FileSizeU32) { - *ReturnValue = FILE_TO_MUCH_DATA; - remove(CompletePath); - LogMessage(LOG_LEVEL_INFO, "CORRUPT FILE, REMOVING..."); - } - else { - *ReturnValue = TIME_OUT; - remove(CompletePath); - LogMessage(LOG_LEVEL_INFO, "CORRUPT FILE, REMOVING..."); - } + if (TotalRxCount == FileSizeU32) { + *ReturnValue = FILE_UPLOADED; + } + else if (TotalRxCount > FileSizeU32) { + *ReturnValue = FILE_TO_MUCH_DATA; + remove(CompletePath); + LogMessage(LOG_LEVEL_INFO, "CORRUPT FILE, REMOVING..."); + } + else { + *ReturnValue = TIME_OUT; + remove(CompletePath); + LogMessage(LOG_LEVEL_INFO, "CORRUPT FILE, REMOVING..."); + } - LogMessage(LOG_LEVEL_DEBUG, "Rec count = %d, Req count = %d", TotalRxCount, FileSizeU32); + LogMessage(LOG_LEVEL_DEBUG, "Rec count = %d, Req count = %d", TotalRxCount, FileSizeU32); - } + } - return 0; + return 0; } I32 SystemControlSendFileContent(I32 * sockfd, C8 * Path, C8 * PacketSize, C8 * ReturnValue, U8 Remove, - U8 Debug) { - FILE *fd; - C8 CompletePath[MAX_FILE_PATH]; - - bzero(CompletePath, MAX_FILE_PATH); - UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); - strcat(CompletePath, Path); - U32 FileSizeU32 = 0; - U16 PacketSizeU16 = atoi(PacketSize); - I32 ClientStatus = 0, Time1 = 0, Time2 = 0, TimeDiff = 0, i = 0, j = 0; - C8 TxBuffer[SYSTEM_CONTROL_TX_PACKET_SIZE]; - U32 TotalRxCount = 0, TransmissionCount = 0, RestCount = 0; - struct timeval CurTime; - struct stat st; - - stat(CompletePath, &st); - TransmissionCount = (U32) (st.st_size) / PacketSizeU16; - RestCount = (U32) (st.st_size) % PacketSizeU16; - - if (Debug) { - LogPrint("Send file content:"); - LogPrint("%s", Path); - LogPrint("%s", PacketSize); - LogPrint("%s", CompletePath); - } + U8 Debug) { + FILE *fd; + C8 CompletePath[MAX_FILE_PATH]; + + bzero(CompletePath, MAX_FILE_PATH); + UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); + strcat(CompletePath, Path); + U32 FileSizeU32 = 0; + U16 PacketSizeU16 = atoi(PacketSize); + I32 ClientStatus = 0, Time1 = 0, Time2 = 0, TimeDiff = 0, i = 0, j = 0; + C8 TxBuffer[SYSTEM_CONTROL_TX_PACKET_SIZE]; + U32 TotalRxCount = 0, TransmissionCount = 0, RestCount = 0; + struct timeval CurTime; + struct stat st; + + stat(CompletePath, &st); + TransmissionCount = (U32) (st.st_size) / PacketSizeU16; + RestCount = (U32) (st.st_size) % PacketSizeU16; + + if (Debug) { + LogPrint("Send file content:"); + LogPrint("%s", Path); + LogPrint("%s", PacketSize); + LogPrint("%s", CompletePath); + } - fd = fopen(CompletePath, "r"); + fd = fopen(CompletePath, "r"); - if (fd != NULL) { + if (fd != NULL) { - for (i = 0; i < TransmissionCount; i++) { - bzero(TxBuffer, PacketSizeU16); - fread(TxBuffer, 1, PacketSizeU16, fd); - //SystemControlSendBytes(TxBuffer, PacketSizeU16, sockfd, 0); //Send a packet - UtilSendTCPData("System Control", TxBuffer, PacketSizeU16, sockfd, 0); - } + for (i = 0; i < TransmissionCount; i++) { + bzero(TxBuffer, PacketSizeU16); + fread(TxBuffer, 1, PacketSizeU16, fd); + //SystemControlSendBytes(TxBuffer, PacketSizeU16, sockfd, 0); //Send a packet + UtilSendTCPData("System Control", TxBuffer, PacketSizeU16, sockfd, 0); + } - if (RestCount > 0) { - bzero(TxBuffer, PacketSizeU16); - fread(TxBuffer, 1, RestCount, fd); - //SystemControlSendBytes(TxBuffer, RestCount, sockfd, 0); //Send the rest - UtilSendTCPData("System Control", TxBuffer, RestCount, sockfd, 0); - } + if (RestCount > 0) { + bzero(TxBuffer, PacketSizeU16); + fread(TxBuffer, 1, RestCount, fd); + //SystemControlSendBytes(TxBuffer, RestCount, sockfd, 0); //Send the rest + UtilSendTCPData("System Control", TxBuffer, RestCount, sockfd, 0); + } - fclose(fd); + fclose(fd); - if (Remove) - remove(CompletePath); + if (Remove) + remove(CompletePath); - LogMessage(LOG_LEVEL_INFO, "Sent file: %s, total size = %d, transmissions = %d", CompletePath, - (PacketSizeU16 * TransmissionCount + RestCount), i + 1); + LogMessage(LOG_LEVEL_INFO, "Sent file: %s, total size = %d, transmissions = %d", CompletePath, + (PacketSizeU16 * TransmissionCount + RestCount), i + 1); - } + } - return 0; + return 0; } @@ -2282,41 +2282,41 @@ See the architecture document for the protocol of RVSS. - Debug enable(1)/disable(0) debug printouts (Not used) */ I32 SystemControlBuildRVSSTimeChannelMessage(C8 * RVSSData, U32 * RVSSDataLengthU32, TimeType * GPSTime, - U8 Debug) { - I32 MessageIndex = 0, i; - C8 *p; - - RVSSTimeType RVSSTimeData; - - RVSSTimeData.MessageLengthU32 = SwapU32((U32) sizeof (RVSSTimeType) - 4); - RVSSTimeData.ChannelCodeU32 = SwapU32((U32) RVSS_TIME_CHANNEL); - RVSSTimeData.YearU16 = SwapU16(GPSTime->YearU16); - RVSSTimeData.MonthU8 = GPSTime->MonthU8; - RVSSTimeData.DayU8 = GPSTime->DayU8; - RVSSTimeData.HourU8 = GPSTime->HourU8; - RVSSTimeData.MinuteU8 = GPSTime->MinuteU8; - RVSSTimeData.SecondU8 = GPSTime->SecondU8; - RVSSTimeData.MillisecondU16 = SwapU16(TimeControlGetMillisecond(GPSTime)); - RVSSTimeData.SecondCounterU32 = SwapU32(GPSTime->SecondCounterU32); - RVSSTimeData.GPSMillisecondsU64 = - SwapU64(GPSTime->GPSMillisecondsU64 + (U64) TimeControlGetMillisecond(GPSTime)); - RVSSTimeData.GPSMinutesU32 = SwapU32(GPSTime->GPSMinutesU32); - RVSSTimeData.GPSWeekU16 = SwapU16(GPSTime->GPSWeekU16); - RVSSTimeData.GPSSecondsOfWeekU32 = SwapU32(GPSTime->GPSSecondsOfWeekU32); - RVSSTimeData.GPSSecondsOfDayU32 = SwapU32(GPSTime->GPSSecondsOfDayU32); - RVSSTimeData.FixQualityU8 = GPSTime->FixQualityU8; - RVSSTimeData.NSatellitesU8 = GPSTime->NSatellitesU8; - - p = (C8 *) & RVSSTimeData; - for (i = 0; i < sizeof (RVSSTimeType); i++) - *(RVSSData + i) = *p++; - *RVSSDataLengthU32 = i; - - if (Debug) { + U8 Debug) { + I32 MessageIndex = 0, i; + C8 *p; + + RVSSTimeType RVSSTimeData; + + RVSSTimeData.MessageLengthU32 = SwapU32((U32) sizeof (RVSSTimeType) - 4); + RVSSTimeData.ChannelCodeU32 = SwapU32((U32) RVSS_TIME_CHANNEL); + RVSSTimeData.YearU16 = SwapU16(GPSTime->YearU16); + RVSSTimeData.MonthU8 = GPSTime->MonthU8; + RVSSTimeData.DayU8 = GPSTime->DayU8; + RVSSTimeData.HourU8 = GPSTime->HourU8; + RVSSTimeData.MinuteU8 = GPSTime->MinuteU8; + RVSSTimeData.SecondU8 = GPSTime->SecondU8; + RVSSTimeData.MillisecondU16 = SwapU16(TimeControlGetMillisecond(GPSTime)); + RVSSTimeData.SecondCounterU32 = SwapU32(GPSTime->SecondCounterU32); + RVSSTimeData.GPSMillisecondsU64 = + SwapU64(GPSTime->GPSMillisecondsU64 + (U64) TimeControlGetMillisecond(GPSTime)); + RVSSTimeData.GPSMinutesU32 = SwapU32(GPSTime->GPSMinutesU32); + RVSSTimeData.GPSWeekU16 = SwapU16(GPSTime->GPSWeekU16); + RVSSTimeData.GPSSecondsOfWeekU32 = SwapU32(GPSTime->GPSSecondsOfWeekU32); + RVSSTimeData.GPSSecondsOfDayU32 = SwapU32(GPSTime->GPSSecondsOfDayU32); + RVSSTimeData.FixQualityU8 = GPSTime->FixQualityU8; + RVSSTimeData.NSatellitesU8 = GPSTime->NSatellitesU8; + + p = (C8 *) & RVSSTimeData; + for (i = 0; i < sizeof (RVSSTimeType); i++) + *(RVSSData + i) = *p++; + *RVSSDataLengthU32 = i; + + if (Debug) { - } + } - return 0; + return 0; } @@ -2331,28 +2331,28 @@ See the architecture document for the protocol of RVSS. - Debug enable(1)/disable(0) debug printouts (Not used) */ I32 SystemControlBuildRVSSMaestroChannelMessage(C8 * RVSSData, U32 * RVSSDataLengthU32, GSDType * GSD, - U8 SysCtrlState, U8 Debug) { - I32 MessageIndex = 0, i; - C8 *p; + U8 SysCtrlState, U8 Debug) { + I32 MessageIndex = 0, i; + C8 *p; - RVSSMaestroType RVSSMaestroData; + RVSSMaestroType RVSSMaestroData; - RVSSMaestroData.MessageLengthU32 = SwapU32((U32) sizeof (RVSSMaestroType) - 4); - RVSSMaestroData.ChannelCodeU32 = SwapU32((U32) RVSS_MAESTRO_CHANNEL); - RVSSMaestroData.OBCStateU8 = DataDictionaryGetOBCStateU8(GSD); - RVSSMaestroData.SysCtrlStateU8 = SysCtrlState; + RVSSMaestroData.MessageLengthU32 = SwapU32((U32) sizeof (RVSSMaestroType) - 4); + RVSSMaestroData.ChannelCodeU32 = SwapU32((U32) RVSS_MAESTRO_CHANNEL); + RVSSMaestroData.OBCStateU8 = DataDictionaryGetOBCStateU8(GSD); + RVSSMaestroData.SysCtrlStateU8 = SysCtrlState; - p = (C8 *) & RVSSMaestroData; - for (i = 0; i < sizeof (RVSSMaestroType); i++) - *(RVSSData + i) = *p++; - *RVSSDataLengthU32 = i; + p = (C8 *) & RVSSMaestroData; + for (i = 0; i < sizeof (RVSSMaestroType); i++) + *(RVSSData + i) = *p++; + *RVSSDataLengthU32 = i; - if (Debug) { + if (Debug) { - } + } - return 0; + return 0; } @@ -2369,29 +2369,29 @@ See the architecture document for the protocol of RVSS. */ I32 SystemControlBuildRVSSMONRChannelMessage(C8 * RVSSData, U32 * RVSSDataLengthU32, MonitorDataType MonrData, - U8 Debug) { - I32 MessageLength = 0; - char MonrDataString[MAX_MONR_STRING_LENGTH]; + U8 Debug) { + I32 MessageLength = 0; + char MonrDataString[MAX_MONR_STRING_LENGTH]; - // TODO: Convert MonrData to string - UtilMonitorDataToString(MonrData, MonrDataString, sizeof (MonrDataString)); + // TODO: Convert MonrData to string + UtilMonitorDataToString(MonrData, MonrDataString, sizeof (MonrDataString)); - MessageLength = strlen(MonrDataString) + 8; - bzero(RVSSData, MessageLength); - *RVSSDataLengthU32 = MessageLength; + MessageLength = strlen(MonrDataString) + 8; + bzero(RVSSData, MessageLength); + *RVSSDataLengthU32 = MessageLength; - *(RVSSData + 0) = (C8) (MessageLength >> 24); - *(RVSSData + 1) = (C8) (MessageLength >> 16); - *(RVSSData + 2) = (C8) (MessageLength >> 8); - *(RVSSData + 3) = (C8) (MessageLength); - *(RVSSData + 7) = (C8) (RVSS_MONR_CHANNEL); - strcat(RVSSData + 8, MonrDataString); + *(RVSSData + 0) = (C8) (MessageLength >> 24); + *(RVSSData + 1) = (C8) (MessageLength >> 16); + *(RVSSData + 2) = (C8) (MessageLength >> 8); + *(RVSSData + 3) = (C8) (MessageLength); + *(RVSSData + 7) = (C8) (RVSS_MONR_CHANNEL); + strcat(RVSSData + 8, MonrDataString); - if (Debug) { + if (Debug) { - } + } - return 0; + return 0; } @@ -2406,10 +2406,10 @@ See the architecture document for the protocol of RVSS. */ I32 SystemControlBuildRVSSAspChannelMessage(C8 * RVSSData, U32 * RVSSDataLengthU32, U8 Debug) { - RVSSTimeType RVSSTimeData; + RVSSTimeType RVSSTimeData; - return 0; + return 0; } From 797ba023658c4ae888dc06dbfd9d69c9deda2cef Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 24 Jan 2020 16:34:47 +0100 Subject: [PATCH 152/523] Ran code formatter --- server/src/iso22133.c | 8 +++++--- server/src/objectcontrol.c | 14 +++++++++----- 2 files changed, 14 insertions(+), 8 deletions(-) diff --git a/server/src/iso22133.c b/server/src/iso22133.c index fa2e0bc9e..1baa46de9 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -535,10 +535,11 @@ ssize_t encodeSTRTMessage(const struct timeval *timeOfStart, char *strtDataBuffe * \return Number of bytes written or -1 in case of an error */ ssize_t encodeHEABMessage(const ControlCenterStatusType status, char *heabDataBuffer, - const size_t bufferLength, const char debug) { + const size_t bufferLength, const char debug) { HEABType HEABData; struct timeval currentTime; + TimeSetToCurrentSystemTime(¤tTime); memset(heabDataBuffer, 0, bufferLength); @@ -554,12 +555,13 @@ ssize_t encodeHEABMessage(const ControlCenterStatusType status, char *heabDataBu // Fill contents HEABData.HEABStructValueID = VALUE_ID_HEAB_STRUCT; HEABData.HEABStructContentLength = sizeof (HEABType) - sizeof (HeaderType) - sizeof (FooterType) - - sizeof (HEABData.HEABStructValueID) - sizeof (HEABData.HEABStructContentLength); + - sizeof (HEABData.HEABStructValueID) - sizeof (HEABData.HEABStructContentLength); HEABData.GPSQmsOfWeek = TimeGetAsGPSqmsOfWeek(¤tTime); if (!(status == CONTROL_CENTER_STATUS_INIT || status == CONTROL_CENTER_STATUS_READY || status == CONTROL_CENTER_STATUS_ABORT || status == CONTROL_CENTER_STATUS_RUNNING || status == CONTROL_CENTER_STATUS_TEST_DONE || status == CONTROL_CENTER_STATUS_NORMAL_STOP)) { - LogMessage(LOG_LEVEL_ERROR, "HEAB does not support status ID %u - defaulting to ABORT", (uint8_t) status); + LogMessage(LOG_LEVEL_ERROR, "HEAB does not support status ID %u - defaulting to ABORT", + (uint8_t) status); HEABData.controlCenterStatus = (uint8_t) CONTROL_CENTER_STATUS_ABORT; } else { diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index fe99ea102..4029be876 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -210,8 +210,10 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { { OC_SLEEP_TIME_NONEMPTY_MQ_S, OC_SLEEP_TIME_NONEMPTY_MQ_NS }; const struct timeval stateReportPeriod = { OC_STATE_REPORT_PERIOD_S, OC_STATE_REPORT_PERIOD_US }; struct timeval currentTime, nextStateReportTime, nextHeartbeatTime; + const struct timeval heartbeatPeriod = { 1 / HEAB_FREQUENCY_HZ, - (1000000 / HEAB_FREQUENCY_HZ) % 1000000 }; + (1000000 / HEAB_FREQUENCY_HZ) % 1000000 + }; U8 iForceObjectToLocalhostU8 = 0; FILE *fd; @@ -314,22 +316,24 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { if (vGetState(GSD) == OBC_STATE_ERROR) { objectControlServerStatus = CONTROL_CENTER_STATUS_ABORT; - MessageLength = encodeHEABMessage(objectControlServerStatus, MessageBuffer, sizeof (MessageBuffer), 0); + MessageLength = + encodeHEABMessage(objectControlServerStatus, MessageBuffer, sizeof (MessageBuffer), 0); UtilSendUDPData("Object Control", &safety_socket_fd[iIndex], &safety_object_addr[iIndex], MessageBuffer, MessageLength, 0); } // Heartbeat if ((vGetState(GSD) == OBC_STATE_RUNNING || vGetState(GSD) == OBC_STATE_ARMED - || vGetState(GSD) == OBC_STATE_CONNECTED) && timercmp(¤tTime, &nextHeartbeatTime, >)) { + || vGetState(GSD) == OBC_STATE_CONNECTED) && timercmp(¤tTime, &nextHeartbeatTime, >)) { timeradd(&nextHeartbeatTime, &heartbeatPeriod, &nextHeartbeatTime); - MessageLength = encodeHEABMessage(objectControlServerStatus, MessageBuffer, sizeof (MessageBuffer), 0); + MessageLength = + encodeHEABMessage(objectControlServerStatus, MessageBuffer, sizeof (MessageBuffer), 0); // Transmit heartbeat to all objects for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { UtilSendUDPData("Object Control", &safety_socket_fd[iIndex], &safety_object_addr[iIndex], - MessageBuffer, MessageLength, 0); + MessageBuffer, MessageLength, 0); } // Check if any object has disconnected - if so, disconnect all objects and return to idle From 841270b06818a724b3cb77794ac49cf7da6de3e2 Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Mon, 27 Jan 2020 08:52:49 +0100 Subject: [PATCH 153/523] Fixed size in init --- server/src/datadictionary.c | 2 +- server/src/objectcontrol.c | 22 ++++++++++------------ 2 files changed, 11 insertions(+), 13 deletions(-) diff --git a/server/src/datadictionary.c b/server/src/datadictionary.c index ea2acf303..58be390c6 100644 --- a/server/src/datadictionary.c +++ b/server/src/datadictionary.c @@ -1699,7 +1699,7 @@ ReadWriteAccess_t DataDictionaryInitMONR(GSDType * GSD) { // Map memory to created file GSD->MonrMessages = - (MONRType *) mmap(NULL, (sizeof (MONRType)), PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0); + (MONRType *) mmap(NULL, (sizeof (MONRType) * GSD->numberOfObjects), PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0); LogPrint("Initing"); diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index a32cb058b..b38575147 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -489,18 +489,16 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { //Store MONR in GSD DataDictionarySetMONR(GSD, &MONRData, iIndex); + //UtilSendUDPData("ObjectControl", &ObjectControlUDPSocketfdI32, &simulator_addr, &MONRData, sizeof(MONRData), 0) - //UtilSendUDPData("ObjectControl", &ObjectControlUDPSocketfdI32, &simulator_addr, &MONRData, sizeof(MONRData), 0); - - /* - for (i = 0; - i < - (MONRData.Header.MessageLengthU32 + COMMAND_MESSAGE_HEADER_LENGTH + - COMMAND_MESSAGE_FOOTER_LENGTH); i++) - GSD->MONRData[i] = buffer[i]; - GSD->MONRSizeU8 = - MONRData.Header.MessageLengthU32 + COMMAND_MESSAGE_HEADER_LENGTH + - COMMAND_MESSAGE_FOOTER_LENGTH; */ + for (i = 0; + i < + (MONRData.Header.MessageLengthU32 + COMMAND_MESSAGE_HEADER_LENGTH + + COMMAND_MESSAGE_FOOTER_LENGTH); i++) + GSD->MONRData[i] = buffer[i]; + GSD->MONRSizeU8 = + MONRData.Header.MessageLengthU32 + COMMAND_MESSAGE_HEADER_LENGTH + + COMMAND_MESSAGE_FOOTER_LENGTH; ObjectControlMONRToASCII(&MONRData, &OriginPosition, iIndex, Id, Timestamp, XPosition, YPosition, ZPosition, LongitudinalSpeed, LateralSpeed, @@ -785,7 +783,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { //OSEMSentU8 = 0; STRTSentU8 = 0; - + DataDictionarySetNumberOfObjectsU8(GSD, nbr_objects); DataDictionaryInitMONR(GSD); } From edde561d18db31287043c76be2f0e62226a32c0e Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Mon, 27 Jan 2020 09:57:26 +0100 Subject: [PATCH 154/523] removed uses --- server/src/datadictionary.c | 10 +++++----- server/src/objectcontrol.c | 2 -- server/src/systemcontrol.c | 1 - 3 files changed, 5 insertions(+), 8 deletions(-) diff --git a/server/src/datadictionary.c b/server/src/datadictionary.c index 58be390c6..0dbf7f98f 100644 --- a/server/src/datadictionary.c +++ b/server/src/datadictionary.c @@ -1698,13 +1698,14 @@ ReadWriteAccess_t DataDictionaryInitMONR(GSDType * GSD) { //printf("File size after writing: %ld\n", st.st_size); // Map memory to created file - GSD->MonrMessages = - (MONRType *) mmap(NULL, (sizeof (MONRType) * GSD->numberOfObjects), PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0); + // int objectCount = GSD->numberOfObjects; + + + GSD->MonrMessages = (MONRType *) mmap(NULL, (sizeof (MONRType)), PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0); LogPrint("Initing"); - //munmap(memory, 6); pthread_mutex_unlock(&MONRMutex); return Res; @@ -1723,7 +1724,6 @@ ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, MONRType * MONR, U8 id) { Res = WRITE_OK; pthread_mutex_lock(&MONRMutex); GSD->MonrMessages[id] = *MONR; - LogPrint("Setting %d", GSD->MonrMessages[id].HeadingU16); pthread_mutex_unlock(&MONRMutex); return Res; } @@ -1774,7 +1774,7 @@ ReadWriteAccess_t DataDictionarySetNumberOfObjectsU8(GSDType * GSD, U8 * numberO Res = WRITE_OK; pthread_mutex_lock(&numberOfObjectsMutex); - GSD->numberOfObjects = *numberOfObjects; + GSD->numberOfObjects = *numberOfObjects; pthread_mutex_unlock(&numberOfObjectsMutex); return Res; } diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index b38575147..cfaaccc61 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -783,8 +783,6 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { //OSEMSentU8 = 0; STRTSentU8 = 0; - DataDictionarySetNumberOfObjectsU8(GSD, nbr_objects); - DataDictionaryInitMONR(GSD); } else { diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index 395172b18..78d5ef2aa 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -801,7 +801,6 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "InitializeScenario:", ControlResponseBuffer, 0, &ClientSocket, 0); - DataDictionaryInitMONR(GSD); SystemControlSendLog("[SystemControl] Sending INIT.\n", &ClientSocket, 0); } From 0c3eac2665a6668087de1696495a8ea0fb77c657 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 27 Jan 2020 09:57:30 +0100 Subject: [PATCH 155/523] Moved document-wide definitions to .c file --- server/inc/iso22133.h | 20 +------------------- server/src/iso22133.c | 28 +++++++++++++++++++++++++--- 2 files changed, 26 insertions(+), 22 deletions(-) diff --git a/server/inc/iso22133.h b/server/inc/iso22133.h index c57d21418..0936e555d 100644 --- a/server/inc/iso22133.h +++ b/server/inc/iso22133.h @@ -15,6 +15,7 @@ extern "C" { #include #include +// TODO: move to .c-file #define ISO_PROTOCOL_VERSION 2 #define ACK_REQ 0 @@ -38,25 +39,6 @@ typedef struct -//! Predefined integer values with special meaning -#define LATITUDE_UNAVAILABLE_VALUE 900000000001 -#define LATITUDE_ONE_DEGREE_VALUE 10000000000 -#define LONGITUDE_UNAVAILABLE_VALUE 1800000000001 -#define LONGITUDE_ONE_DEGREE_VALUE 10000000000 -#define ALTITUDE_UNAVAILABLE_VALUE 800001 -#define ALTITUDE_ONE_METER_VALUE 100 -#define DATE_UNAVAILABLE_VALUE 0 -#define GPS_WEEK_UNAVAILABLE_VALUE 10001 -#define GPS_SECOND_OF_WEEK_UNAVAILABLE_VALUE 2419200000 -#define MAX_WAY_DEVIATION_UNAVAILABLE_VALUE 65535 -#define MAX_WAY_DEVIATION_ONE_METER_VALUE 1000 -#define MAX_LATERAL_DEVIATION_UNAVAILABLE_VALUE 65535 -#define MAX_LATERAL_DEVIATION_ONE_METER_VALUE 1000 -#define MIN_POSITIONING_ACCURACY_NOT_REQUIRED_VALUE 0 -#define MIN_POSITIONING_ACCURACY_ONE_METER_VALUE 1000 // ISO specification unclear on this value - - - //! *************************** OSEM typedef struct { diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 1baa46de9..0a601ed6a 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -6,9 +6,31 @@ #include #include +// ************************* ISO protocol versions supported by functions in this file *************************** static const uint8_t SupportedProtocolVersions[] = { 2 }; -// ************************* Byte swapper definitions for 6 byte values + +// ************************* Type definitions according ISO protocol specification ******************************* +//! Predefined integer values with special meaning +#define LATITUDE_UNAVAILABLE_VALUE 900000000001 +#define LATITUDE_ONE_DEGREE_VALUE 10000000000 +#define LONGITUDE_UNAVAILABLE_VALUE 1800000000001 +#define LONGITUDE_ONE_DEGREE_VALUE 10000000000 +#define ALTITUDE_UNAVAILABLE_VALUE 800001 +#define ALTITUDE_ONE_METER_VALUE 100 +#define DATE_UNAVAILABLE_VALUE 0 +#define GPS_WEEK_UNAVAILABLE_VALUE 10001 +#define GPS_SECOND_OF_WEEK_UNAVAILABLE_VALUE 2419200000 +#define MAX_WAY_DEVIATION_UNAVAILABLE_VALUE 65535 +#define MAX_WAY_DEVIATION_ONE_METER_VALUE 1000 +#define MAX_LATERAL_DEVIATION_UNAVAILABLE_VALUE 65535 +#define MAX_LATERAL_DEVIATION_ONE_METER_VALUE 1000 +#define MIN_POSITIONING_ACCURACY_NOT_REQUIRED_VALUE 0 +#define MIN_POSITIONING_ACCURACY_ONE_METER_VALUE 1000 // ISO specification unclear on this value + + +// ************************* Non-ISO type definitions and defines ************************************************ +// Byte swapper definitions for 6 byte values #if __BYTE_ORDER == __LITTLE_ENDIAN #define le48toh(x) __uint64_identity (x) #define htole48(x) __uint64_identity (x) @@ -17,7 +39,7 @@ static const uint8_t SupportedProtocolVersions[] = { 2 }; #define htole48(x) (__bswap_64(x) >> 16) #endif -// ************************** static function declarations +// ************************** static function declarations ******************************************************** static ISOMessageReturnValue decodeISOHeader(const char *MessageBuffer, const size_t length, HeaderType * HeaderData, const char debug); static ISOMessageReturnValue decodeISOFooter(const char *MessageBuffer, const size_t length, @@ -26,7 +48,7 @@ static HeaderType buildISOHeader(ISOMessageID id, uint32_t messageLength, const static FooterType buildISOFooter(const void *message, const size_t sizeExclFooter, const char debug); static char isValidMessageID(const uint16_t id); -// ************************** function definitions +// ************************** function definitions **************************************************************** /*! * \brief decodeISOHeader Convert data in a buffer to an ISO header From 3a5fe67ade731b5c3ed1ed8a43baace0497f8e2b Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 27 Jan 2020 09:59:30 +0100 Subject: [PATCH 156/523] Fixed accidental pointer-to-pointer cast to pointer --- server/src/iso22133.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/server/src/iso22133.c b/server/src/iso22133.c index d66ec0ae5..48e5e403e 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -431,10 +431,10 @@ ssize_t encodeSTRTMessage(const struct timeval *timeOfStart, char *strtDataBuffe STRTData.StartTimeValueIdU16 = VALUE_ID_STRT_GPS_QMS_OF_WEEK; STRTData.StartTimeContentLengthU16 = sizeof (STRTData.StartTimeU32); STRTData.StartTimeU32 = - timeOfStart == NULL ? GPS_SECOND_OF_WEEK_UNAVAILABLE_VALUE : TimeGetAsGPSqmsOfWeek(&timeOfStart); + timeOfStart == NULL ? GPS_SECOND_OF_WEEK_UNAVAILABLE_VALUE : TimeGetAsGPSqmsOfWeek(timeOfStart); STRTData.GPSWeekValueID = VALUE_ID_STRT_GPS_WEEK; STRTData.GPSWeekContentLength = sizeof (STRTData.GPSWeek); - STRTData.GPSWeek = timeOfStart == NULL ? GPS_WEEK_UNAVAILABLE_VALUE : TimeGetAsGPSweek(&timeOfStart); + STRTData.GPSWeek = timeOfStart == NULL ? GPS_WEEK_UNAVAILABLE_VALUE : TimeGetAsGPSweek(timeOfStart); if (debug) { LogPrint("STRT message:\n\tGPS second of week value ID: 0x%x\n\t" From 9a7612cbfe133389cd2520ec29a1896d931f6443 Mon Sep 17 00:00:00 2001 From: Sebastian Loh Lindholm Date: Mon, 27 Jan 2020 10:37:57 +0100 Subject: [PATCH 157/523] When in OBC_STATE_ERROR send HEAB to ALL objects. --- server/src/objectcontrol.c | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index adcca89e9..a8226c019 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -380,8 +380,11 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { MessageLength = ObjectControlBuildHEABMessage(MessageBuffer, &HEABData, GPSTime, ObjectControlServerStatus, 0); - UtilSendUDPData("Object Control", &safety_socket_fd[iIndex], &safety_object_addr[iIndex], - MessageBuffer, MessageLength, 0); + for(iIndex = 0; iIndex < nbr_objects; ++iIndex) + { + UtilSendUDPData("Object Control", &safety_socket_fd[iIndex], &safety_object_addr[iIndex], + MessageBuffer, MessageLength, 0); + } } if (vGetState(GSD) == OBC_STATE_RUNNING || vGetState(GSD) == OBC_STATE_ARMED From 90e4fb0e3cb7e3d097909dc18bf7f8090764935f Mon Sep 17 00:00:00 2001 From: alfaro01 Date: Mon, 27 Jan 2020 10:58:59 +0100 Subject: [PATCH 158/523] save --- runServer.sh | 2 +- server/src/logger.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/runServer.sh b/runServer.sh index 8a047d57d..fd9f36d3e 100755 --- a/runServer.sh +++ b/runServer.sh @@ -3,7 +3,7 @@ #### User settings # Modify this array by adding more modules to include them in the execution -MODULES=(ScenarioControl, Visualization) +MODULES=(ScenarioControl) #### # Save top directory diff --git a/server/src/logger.c b/server/src/logger.c index 01c6f4612..f3f7f7f45 100644 --- a/server/src/logger.c +++ b/server/src/logger.c @@ -289,7 +289,7 @@ void logger_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { } } - (void)iCommClose(); + (void)iCommClose(); LogMessage(LOG_LEVEL_INFO, "Logger exiting"); } From f00658995f4760cd2d498c4cc7470ba8145ec720 Mon Sep 17 00:00:00 2001 From: alfaro01 Date: Mon, 27 Jan 2020 11:16:37 +0100 Subject: [PATCH 159/523] new format update with indent 2.2.11 --- modules/Visualization/src/main.c | 114 ++++++++++++++++--------------- server/src/logger.c | 2 +- 2 files changed, 59 insertions(+), 57 deletions(-) diff --git a/modules/Visualization/src/main.c b/modules/Visualization/src/main.c index 1395dc3a0..d852b29e1 100644 --- a/modules/Visualization/src/main.c +++ b/modules/Visualization/src/main.c @@ -42,34 +42,37 @@ void vCreateVisualizationMessage(MonitorDataType * _monitorData, char *_visualiz int _sizeOfVisualizationMessage, int _debug) { char ipStringBuffer[INET_ADDRSTRLEN]; - sprintf(ipStringBuffer, "%s", - inet_ntop(AF_INET, &_monitorData->ClientIP, ipStringBuffer, sizeof (ipStringBuffer))); - char GPSMsOfWeekString[ENOUGH_BUFFER_SIZE]; - sprintf(GPSMsOfWeekString, "%u", _monitorData->MONR.GPSQmsOfWeekU32); - char xPosString[ENOUGH_BUFFER_SIZE]; - sprintf(xPosString, "%d", _monitorData->MONR.XPositionI32); - char yPosString[ENOUGH_BUFFER_SIZE]; - sprintf(yPosString, "%d", _monitorData->MONR.YPositionI32); - char zPosString[ENOUGH_BUFFER_SIZE]; - sprintf(zPosString, "%d", _monitorData->MONR.ZPositionI32); - char headingString[ENOUGH_BUFFER_SIZE]; - sprintf(headingString, "%u", _monitorData->MONR.HeadingU16); - char longSpeedString[ENOUGH_BUFFER_SIZE]; - sprintf(longSpeedString, "%d", _monitorData->MONR.LongitudinalSpeedI16); - char stateString[ENOUGH_BUFFER_SIZE]; - sprintf(stateString, "%u", _monitorData->MONR.StateU8); + + sprintf(ipStringBuffer, "%s", + inet_ntop(AF_INET, &_monitorData->ClientIP, ipStringBuffer, sizeof (ipStringBuffer))); + char GPSMsOfWeekString[ENOUGH_BUFFER_SIZE]; + + sprintf(GPSMsOfWeekString, "%u", _monitorData->MONR.GPSQmsOfWeekU32); + char xPosString[ENOUGH_BUFFER_SIZE]; + + sprintf(xPosString, "%d", _monitorData->MONR.XPositionI32); + char yPosString[ENOUGH_BUFFER_SIZE]; + + sprintf(yPosString, "%d", _monitorData->MONR.YPositionI32); + char zPosString[ENOUGH_BUFFER_SIZE]; + + sprintf(zPosString, "%d", _monitorData->MONR.ZPositionI32); + char headingString[ENOUGH_BUFFER_SIZE]; + + sprintf(headingString, "%u", _monitorData->MONR.HeadingU16); + char longSpeedString[ENOUGH_BUFFER_SIZE]; + + sprintf(longSpeedString, "%d", _monitorData->MONR.LongitudinalSpeedI16); + char stateString[ENOUGH_BUFFER_SIZE]; + + sprintf(stateString, "%u", _monitorData->MONR.StateU8); //Build message from MonitorStruct - snprintf(_visualizationMessage, _sizeOfVisualizationMessage, "%s;%s;%s;%s;%s;%s;%s;%s;", + snprintf(_visualizationMessage, _sizeOfVisualizationMessage, "%s;%s;%s;%s;%s;%s;%s;%s;", ipStringBuffer, - GPSMsOfWeekString, - xPosString, - yPosString, - zPosString, - headingString, - longSpeedString, - stateString); + GPSMsOfWeekString, + xPosString, yPosString, zPosString, headingString, longSpeedString, stateString); if (_debug) { @@ -82,7 +85,7 @@ void vCreateVisualizationMessage(MonitorDataType * _monitorData, char *_visualiz LogMessage(LOG_LEVEL_INFO, "Heading: %u", _monitorData->MONR.HeadingU16); LogMessage(LOG_LEVEL_INFO, "LongSpeed: %d", _monitorData->MONR.LongitudinalSpeedI16); LogMessage(LOG_LEVEL_INFO, "State: %u", _monitorData->MONR.StateU8); - LogMessage(LOG_LEVEL_INFO, "MESSAGE-SIZE = %d",_sizeOfVisualizationMessage); + LogMessage(LOG_LEVEL_INFO, "MESSAGE-SIZE = %d", _sizeOfVisualizationMessage); } } @@ -104,9 +107,9 @@ int main() { vConnectVisualizationChannel(&visual_server, &visual_server_addr); - //Setup signal handlers - if (signal(SIGINT, signalHandler) == SIG_ERR) - util_error("Unable to initialize signal handler"); + //Setup signal handlers + if (signal(SIGINT, signalHandler) == SIG_ERR) + util_error("Unable to initialize signal handler"); @@ -116,7 +119,7 @@ int main() { nanosleep(&sleepTimePeriod, &remTime); } - while (!iExit) { + while (!iExit) { if (iCommRecv(&command, mqRecvData, MQ_MSG_SIZE, NULL) < 0) { @@ -145,31 +148,30 @@ int main() { //Populate the monitorType UtilPopulateMonitorDataStruct(mqRecvData, (size_t) (sizeof (mqRecvData)), &monitorData, 0); - char dummy[1]; - int sizeOfVisualizationMessage; - - //Calculate size of incoming buffer - sizeOfVisualizationMessage = snprintf(dummy, sizeof(dummy),"%u;%d;%d;%d;%u;%d;%u;", - monitorData.MONR.GPSQmsOfWeekU32, - monitorData.MONR.XPositionI32, - monitorData.MONR.YPositionI32, - monitorData.MONR.ZPositionI32, - monitorData.MONR.HeadingU16, - monitorData.MONR.LongitudinalSpeedI16, - monitorData.MONR.StateU8 - ); - sizeOfVisualizationMessage += INET_ADDRSTRLEN; - sizeOfVisualizationMessage += 8; //(;) + char dummy[1]; + int sizeOfVisualizationMessage; + + //Calculate size of incoming buffer + sizeOfVisualizationMessage = snprintf(dummy, sizeof (dummy), "%u;%d;%d;%d;%u;%d;%u;", + monitorData.MONR.GPSQmsOfWeekU32, + monitorData.MONR.XPositionI32, + monitorData.MONR.YPositionI32, + monitorData.MONR.ZPositionI32, + monitorData.MONR.HeadingU16, + monitorData.MONR.LongitudinalSpeedI16, + monitorData.MONR.StateU8); + sizeOfVisualizationMessage += INET_ADDRSTRLEN; + sizeOfVisualizationMessage += 8; //(;) //Allocate memory char *visualizationMessage = malloc(sizeOfVisualizationMessage * sizeof (char)); //Create visualization message and insert values from the monitor datastruct above - vCreateVisualizationMessage(&monitorData, visualizationMessage, sizeOfVisualizationMessage, 0); + vCreateVisualizationMessage(&monitorData, visualizationMessage, sizeOfVisualizationMessage, 0); //Send visualization message on the UDP socket UtilSendUDPData((const uint8_t *)"Visualization", &visual_server, &visual_server_addr, - visualizationMessage, strlen(visualizationMessage), 0); + visualizationMessage, strlen(visualizationMessage), 0); //Free memory used by malloc free(visualizationMessage); @@ -189,10 +191,10 @@ int main() { } } - //Return MQBus to "stack" - (void)iCommClose(); + //Return MQBus to "stack" + (void)iCommClose(); - LogMessage(LOG_LEVEL_INFO, "Visualization exiting..."); + LogMessage(LOG_LEVEL_INFO, "Visualization exiting..."); return 0; } @@ -202,13 +204,13 @@ int main() { -- Private functions ------------------------------------------------------------*/ void signalHandler(int signo) { - if (signo == SIGINT) { - LogMessage(LOG_LEVEL_WARNING, "Caught keyboard interrupt"); - iExit = 1; - } - else { - LogMessage(LOG_LEVEL_ERROR, "Caught unhandled signal"); - } + if (signo == SIGINT) { + LogMessage(LOG_LEVEL_WARNING, "Caught keyboard interrupt"); + iExit = 1; + } + else { + LogMessage(LOG_LEVEL_ERROR, "Caught unhandled signal"); + } } diff --git a/server/src/logger.c b/server/src/logger.c index f3f7f7f45..01c6f4612 100644 --- a/server/src/logger.c +++ b/server/src/logger.c @@ -289,7 +289,7 @@ void logger_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { } } - (void)iCommClose(); + (void)iCommClose(); LogMessage(LOG_LEVEL_INFO, "Logger exiting"); } From 7bd7541a71d599ac7ef31f0c2fba906c25a6d524 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 27 Jan 2020 12:48:32 +0100 Subject: [PATCH 160/523] Moved typedefs not externally referenced to .c file --- server/inc/iso22133.h | 103 ++++-------------------------------------- server/src/iso22133.c | 93 ++++++++++++++++++++++++++++++++++++++ 2 files changed, 101 insertions(+), 95 deletions(-) diff --git a/server/inc/iso22133.h b/server/inc/iso22133.h index 0936e555d..900a1d55d 100644 --- a/server/inc/iso22133.h +++ b/server/inc/iso22133.h @@ -39,64 +39,7 @@ typedef struct -//! *************************** OSEM -typedef struct -{ - HeaderType header; - uint16_t latitudeValueID; - uint16_t latitudeContentLength; - int64_t latitude; - uint16_t longitudeValueID; - uint16_t longitudeContentLength; - int64_t longitude; - uint16_t altitudeValueID; - uint16_t altitudeContentLength; - int32_t altitude; - uint16_t dateValueID; - uint16_t dateContentLength; - uint32_t date; - uint16_t GPSWeekValueID; - uint16_t GPSWeekContentLength; - uint16_t GPSWeek; - uint16_t GPSQmsOfWeekValueID; - uint16_t GPSQmsOfWeekContentLength; - uint32_t GPSQmsOfWeek; - uint16_t maxWayDeviationValueID; - uint16_t maxWayDeviationContentLength; - uint16_t maxWayDeviation; - uint16_t maxLateralDeviationValueID; - uint16_t maxLateralDeviationContentLength; - uint16_t maxLateralDeviation; - uint16_t minPosAccuracyValueID; - uint16_t minPosAccuracyContentLength; - uint16_t minPosAccuracy; - FooterType footer; -} OSEMType; //85 bytes - -//! OSEM value IDs -#define VALUE_ID_OSEM_LATITUDE 0x0020 -#define VALUE_ID_OSEM_LONGITUDE 0x0021 -#define VALUE_ID_OSEM_ALTITUDE 0x0022 -#define VALUE_ID_OSEM_DATE 0x0004 -#define VALUE_ID_OSEM_GPS_WEEK 0x0003 -#define VALUE_ID_OSEM_GPS_QUARTER_MILLISECOND_OF_WEEK 0x0002 -#define VALUE_ID_OSEM_MAX_WAY_DEVIATION 0x0070 -#define VALUE_ID_OSEM_MAX_LATERAL_DEVIATION 0x0072 -#define VALUE_ID_OSEM_MIN_POSITIONING_ACCURACY 0x0074 - - -//! *************************** OSTM -typedef struct -{ - HeaderType header; - uint16_t stateValueID; - uint16_t stateContentLength; - uint8_t state; - FooterType footer; -} OSTMType; //16 bytes - -//! OSTM value IDs -#define VALUE_ID_OSTM_STATE_CHANGE_REQUEST 0x0064 +/*! OSTM commands */ typedef enum { OBJECT_COMMAND_ARM = 0x02, //!< Request to arm the target object OBJECT_COMMAND_DISARM = 0x03, //!< Request to disarm the target object @@ -104,45 +47,15 @@ typedef enum { } ObjectCommandType; -//! *************************** STRT -typedef struct -{ - HeaderType header; - uint16_t StartTimeValueIdU16; - uint16_t StartTimeContentLengthU16; - uint32_t StartTimeU32; - uint16_t GPSWeekValueID; - uint16_t GPSWeekContentLength; - uint16_t GPSWeek; - FooterType footer; -} STRTType; //27 bytes - -//! STRT value IDs -#define VALUE_ID_STRT_GPS_QMS_OF_WEEK 0x0002 -#define VALUE_ID_STRT_GPS_WEEK 0x0003 - - -//! *************************** HEAB #define HEAB_FREQUENCY_HZ 100 -typedef struct -{ - HeaderType header; - uint16_t HEABStructValueID; - uint16_t HEABStructContentLength; - uint32_t GPSQmsOfWeek; - uint8_t controlCenterStatus; - FooterType footer; -} HEABType; //16 bytes - -//! HEAB value IDs -#define VALUE_ID_HEAB_STRUCT 0x0090 +/*! HEAB control center statuses */ typedef enum { - CONTROL_CENTER_STATUS_INIT = 0x00, - CONTROL_CENTER_STATUS_READY = 0x01, - CONTROL_CENTER_STATUS_ABORT = 0x02, - CONTROL_CENTER_STATUS_RUNNING = 0x03, - CONTROL_CENTER_STATUS_TEST_DONE = 0x04, - CONTROL_CENTER_STATUS_NORMAL_STOP = 0x05 + CONTROL_CENTER_STATUS_INIT = 0x00, //!< + CONTROL_CENTER_STATUS_READY = 0x01, //!< + CONTROL_CENTER_STATUS_ABORT = 0x02, //!< + CONTROL_CENTER_STATUS_RUNNING = 0x03, //!< + CONTROL_CENTER_STATUS_TEST_DONE = 0x04, //!< + CONTROL_CENTER_STATUS_NORMAL_STOP = 0x05 //!< } ControlCenterStatusType; //! *************************** MONR diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 9292b33b8..232e985b0 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -10,6 +10,7 @@ static const uint8_t SupportedProtocolVersions[] = { 2 }; + // ************************* Type definitions according ISO protocol specification ******************************* //! Predefined integer values with special meaning #define LATITUDE_UNAVAILABLE_VALUE 900000000001 @@ -29,6 +30,98 @@ static const uint8_t SupportedProtocolVersions[] = { 2 }; #define MIN_POSITIONING_ACCURACY_ONE_METER_VALUE 1000 // ISO specification unclear on this value +//! *************************** OSEM +typedef struct +{ + HeaderType header; + uint16_t latitudeValueID; + uint16_t latitudeContentLength; + int64_t latitude; + uint16_t longitudeValueID; + uint16_t longitudeContentLength; + int64_t longitude; + uint16_t altitudeValueID; + uint16_t altitudeContentLength; + int32_t altitude; + uint16_t dateValueID; + uint16_t dateContentLength; + uint32_t date; + uint16_t GPSWeekValueID; + uint16_t GPSWeekContentLength; + uint16_t GPSWeek; + uint16_t GPSQmsOfWeekValueID; + uint16_t GPSQmsOfWeekContentLength; + uint32_t GPSQmsOfWeek; + uint16_t maxWayDeviationValueID; + uint16_t maxWayDeviationContentLength; + uint16_t maxWayDeviation; + uint16_t maxLateralDeviationValueID; + uint16_t maxLateralDeviationContentLength; + uint16_t maxLateralDeviation; + uint16_t minPosAccuracyValueID; + uint16_t minPosAccuracyContentLength; + uint16_t minPosAccuracy; + FooterType footer; +} OSEMType; //85 bytes + +//! OSEM value IDs +#define VALUE_ID_OSEM_LATITUDE 0x0020 +#define VALUE_ID_OSEM_LONGITUDE 0x0021 +#define VALUE_ID_OSEM_ALTITUDE 0x0022 +#define VALUE_ID_OSEM_DATE 0x0004 +#define VALUE_ID_OSEM_GPS_WEEK 0x0003 +#define VALUE_ID_OSEM_GPS_QUARTER_MILLISECOND_OF_WEEK 0x0002 +#define VALUE_ID_OSEM_MAX_WAY_DEVIATION 0x0070 +#define VALUE_ID_OSEM_MAX_LATERAL_DEVIATION 0x0072 +#define VALUE_ID_OSEM_MIN_POSITIONING_ACCURACY 0x0074 + + +/*! OSTM message */ +typedef struct +{ + HeaderType header; + uint16_t stateValueID; + uint16_t stateContentLength; + uint8_t state; + FooterType footer; +} OSTMType; //16 bytes + +//! OSTM value IDs +#define VALUE_ID_OSTM_STATE_CHANGE_REQUEST 0x0064 + + +/*! STRT message */ +typedef struct +{ + HeaderType header; + uint16_t StartTimeValueIdU16; + uint16_t StartTimeContentLengthU16; + uint32_t StartTimeU32; + uint16_t GPSWeekValueID; + uint16_t GPSWeekContentLength; + uint16_t GPSWeek; + FooterType footer; +} STRTType; //27 bytes + +//! STRT value IDs +#define VALUE_ID_STRT_GPS_QMS_OF_WEEK 0x0002 +#define VALUE_ID_STRT_GPS_WEEK 0x0003 + + +/*! HEAB message */ +typedef struct +{ + HeaderType header; + uint16_t HEABStructValueID; + uint16_t HEABStructContentLength; + uint32_t GPSQmsOfWeek; + uint8_t controlCenterStatus; + FooterType footer; +} HEABType; //16 bytes + +//! HEAB value IDs +#define VALUE_ID_HEAB_STRUCT 0x0090 + // ************************* Non-ISO type definitions and defines ************************************************ // Byte swapper definitions for 6 byte values #if __BYTE_ORDER == __LITTLE_ENDIAN From a2e27690f0cd375302bd67624c5d38ce5f938a06 Mon Sep 17 00:00:00 2001 From: alfaro01 Date: Mon, 27 Jan 2020 13:05:49 +0100 Subject: [PATCH 161/523] Fixed build issue --- modules/ScenarioControl/CMakeLists.txt | 4 ++-- modules/Visualization/CMakeLists.txt | 2 ++ 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/modules/ScenarioControl/CMakeLists.txt b/modules/ScenarioControl/CMakeLists.txt index a0fa949a2..11ab4eb91 100644 --- a/modules/ScenarioControl/CMakeLists.txt +++ b/modules/ScenarioControl/CMakeLists.txt @@ -37,8 +37,8 @@ add_library(MQBus add_library(util ../../server/src/util.c ../../server/inc/util.h - ../../server/src/iso22133.c - ../../server/inc/iso22133.h + ../../server/src/iso22133.c + ../../server/inc/iso22133.h ) add_library(Trigger diff --git a/modules/Visualization/CMakeLists.txt b/modules/Visualization/CMakeLists.txt index 38d96ae0e..79b2b1b0c 100644 --- a/modules/Visualization/CMakeLists.txt +++ b/modules/Visualization/CMakeLists.txt @@ -37,6 +37,8 @@ add_library(MQBus add_library(util ../../server/src/util.c ../../server/inc/util.h + ../../server/inc/iso22133.h + ../../server/src/iso22133.c ) add_executable(Visualization From 440914f3358dc9f9b846d00f03e53eb7e3ed07ec Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 27 Jan 2020 13:56:22 +0100 Subject: [PATCH 162/523] Moved TRCM and ACCM to .c file --- server/inc/iso22133.h | 65 +--------- server/src/iso22133.c | 276 +++++++++++++++++++++++++++++++++++++++++- 2 files changed, 276 insertions(+), 65 deletions(-) diff --git a/server/inc/iso22133.h b/server/inc/iso22133.h index 900a1d55d..0485d10f0 100644 --- a/server/inc/iso22133.h +++ b/server/inc/iso22133.h @@ -85,29 +85,6 @@ typedef struct #define VALUE_ID_MONR_STRUCT 0x80 -//! *************************** TRCM -#define COMMAND_TRCM_CODE 0x0011 -typedef struct -{ - HeaderType header; - uint16_t triggerIDValueID; - uint16_t triggerIDContentLength; - uint16_t triggerID; - uint16_t triggerTypeValueID; - uint16_t triggerTypeContentLength; - uint16_t triggerType; - uint16_t triggerTypeParameter1ValueID; - uint16_t triggerTypeParameter1ContentLength; - uint32_t triggerTypeParameter1; - uint16_t triggerTypeParameter2ValueID; - uint16_t triggerTypeParameter2ContentLength; - uint32_t triggerTypeParameter2; - uint16_t triggerTypeParameter3ValueID; - uint16_t triggerTypeParameter3ContentLength; - uint32_t triggerTypeParameter3; - FooterType footer; -} TRCMType; - typedef enum { TRIGGER_UNDEFINED = 0x0000, TRIGGER_TYPE_1 = 0x0001, @@ -165,29 +142,6 @@ typedef enum { } TriggerTypeParameter_t; -//! *************************** ACCM -#define COMMAND_ACCM_CODE 0x0012 -typedef struct -{ - HeaderType header; - uint16_t actionIDValueID; - uint16_t actionIDContentLength; - uint16_t actionID; - uint16_t actionTypeValueID; - uint16_t actionTypeContentLength; - uint16_t actionType; - uint16_t actionTypeParameter1ValueID; - uint16_t actionTypeParameter1ContentLength; - uint32_t actionTypeParameter1; - uint16_t actionTypeParameter2ValueID; - uint16_t actionTypeParameter2ContentLength; - uint32_t actionTypeParameter2; - uint16_t actionTypeParameter3ValueID; - uint16_t actionTypeParameter3ContentLength; - uint32_t actionTypeParameter3; - FooterType footer; -} ACCMType; - typedef enum { ACTION_NONE = 0x0000, ACTION_TYPE_1 = 0x0001, @@ -257,23 +211,6 @@ typedef struct FooterType footer; } EXACType; - -//! ACCM / EXAC / CATA value IDs -#define VALUE_ID_ACTION_ID 0x0002 -#define VALUE_ID_ACTION_TYPE 0x0003 -#define VALUE_ID_ACTION_TYPE_PARAM1 0x00A1 -#define VALUE_ID_ACTION_TYPE_PARAM2 0x00A2 -#define VALUE_ID_ACTION_TYPE_PARAM3 0x00A3 -#define VALUE_ID_ACTION_EXECUTE_TIME 0x0003 - -//! TRCM / TREO / CATA value IDs -#define VALUE_ID_TRIGGER_ID 0x0001 -#define VALUE_ID_TRIGGER_TYPE 0x0002 -#define VALUE_ID_TRIGGER_TYPE_PARAM1 0x0011 -#define VALUE_ID_TRIGGER_TYPE_PARAM2 0x0012 -#define VALUE_ID_TRIGGER_TYPE_PARAM3 0x0013 -#define VALUE_ID_TRIGGER_TIMESTAMP 0x0002 - #pragma pack(pop) typedef enum { @@ -323,6 +260,8 @@ ssize_t encodeSTRTMessage(const struct timeval* timeOfStart, char * strtDataBuff ssize_t encodeOSEMMessage(const double * latitude_deg, const double * longitude_deg, const float * altitude_m, const float * maxPositionDeviation_m, const float * maxLateralDeviation_m, const float * minimumPositioningAccuracy_m, char * osemDataBuffer, const size_t bufferLength, const char debug); ssize_t encodeOSTMMessage(const ObjectCommandType command, char * ostmDataBuffer, const size_t bufferLength, const char debug); ssize_t encodeHEABMessage(const ControlCenterStatusType status, char * heabDataBuffer, const size_t bufferLength, const char debug); +ssize_t encodeTRCMMessage(const uint16_t triggerID, const TriggerType_t triggerType, const TriggerTypeParameter_t param1, const TriggerTypeParameter_t param2, const TriggerTypeParameter_t param3, char * trcmDataBuffer, const size_t bufferLength, const char debug); +ssize_t encodeACCMMessage(const uint16_t actionID, const ActionType_t actionType, const ActionTypeParameter_t param1, const ActionTypeParameter_t param2, const ActionTypeParameter_t param3, char * accmDataBuffer, const size_t bufferLength, const char debug); ISOMessageReturnValue MONRToASCII(const MONRType * MONRData, char * asciiBuffer, const size_t bufferLength, const char debug); ISOMessageReturnValue ASCIIToMONR(const char * asciiBuffer, MONRType * MONRData, const char debug); ISOMessageID getISOMessageType(const char * messageData, const size_t length, const char debug); diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 232e985b0..59145f92a 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -29,8 +29,8 @@ static const uint8_t SupportedProtocolVersions[] = { 2 }; #define MIN_POSITIONING_ACCURACY_NOT_REQUIRED_VALUE 0 #define MIN_POSITIONING_ACCURACY_ONE_METER_VALUE 1000 // ISO specification unclear on this value - -//! *************************** OSEM +#pragma pack(push,1) +/*! OSEM message */ typedef struct { HeaderType header; @@ -122,6 +122,88 @@ typedef struct //! HEAB value IDs #define VALUE_ID_HEAB_STRUCT 0x0090 + +/*! TRCM message */ +typedef struct +{ + HeaderType header; + uint16_t triggerIDValueID; + uint16_t triggerIDContentLength; + uint16_t triggerID; + uint16_t triggerTypeValueID; + uint16_t triggerTypeContentLength; + uint16_t triggerType; + uint16_t triggerTypeParameter1ValueID; + uint16_t triggerTypeParameter1ContentLength; + uint32_t triggerTypeParameter1; + uint16_t triggerTypeParameter2ValueID; + uint16_t triggerTypeParameter2ContentLength; + uint32_t triggerTypeParameter2; + uint16_t triggerTypeParameter3ValueID; + uint16_t triggerTypeParameter3ContentLength; + uint32_t triggerTypeParameter3; + FooterType footer; +} TRCMType; + +//! TRCM value IDs +#define VALUE_ID_TRCM_TRIGGER_ID 0x0001 +#define VALUE_ID_TRCM_TRIGGER_TYPE 0x0002 +#define VALUE_ID_TRCM_TRIGGER_TYPE_PARAM1 0x0011 +#define VALUE_ID_TRCM_TRIGGER_TYPE_PARAM2 0x0012 +#define VALUE_ID_TRCM_TRIGGER_TYPE_PARAM3 0x0013 + + +/*! TREO message */ + +//! TREO value IDs +#define VALUE_ID_TREO_TRIGGER_ID 0x0001 +#define VALUE_ID_TREO_TRIGGER_TIMESTAMP 0x0002 + + +/*! ACCM message */ +typedef struct +{ + HeaderType header; + uint16_t actionIDValueID; + uint16_t actionIDContentLength; + uint16_t actionID; + uint16_t actionTypeValueID; + uint16_t actionTypeContentLength; + uint16_t actionType; + uint16_t actionTypeParameter1ValueID; + uint16_t actionTypeParameter1ContentLength; + uint32_t actionTypeParameter1; + uint16_t actionTypeParameter2ValueID; + uint16_t actionTypeParameter2ContentLength; + uint32_t actionTypeParameter2; + uint16_t actionTypeParameter3ValueID; + uint16_t actionTypeParameter3ContentLength; + uint32_t actionTypeParameter3; + FooterType footer; +} ACCMType; + +//! ACCM value IDs +#define VALUE_ID_ACCM_ACTION_ID 0x0002 +#define VALUE_ID_ACCM_ACTION_TYPE 0x0003 +#define VALUE_ID_ACCM_ACTION_TYPE_PARAM1 0x00A1 +#define VALUE_ID_ACCM_ACTION_TYPE_PARAM2 0x00A2 +#define VALUE_ID_ACCM_ACTION_TYPE_PARAM3 0x00A3 + + +/*! EXAC message */ + +//! EXAC value IDs +#define VALUE_ID_EXAC_ACTION_ID 0x0002 +#define VALUE_ID_EXAC_ACTION_EXECUTE_TIME 0x0003 + +/*! CATA message */ +// TODO +//! CATA value IDs +// TODO + +#pragma pack(pop) + + // ************************* Non-ISO type definitions and defines ************************************************ // Byte swapper definitions for 6 byte values #if __BYTE_ORDER == __LITTLE_ENDIAN @@ -976,3 +1058,193 @@ ISOMessageReturnValue ASCIIToMONR(const char *asciiBuffer, MONRType * MONRData, return MESSAGE_OK; } + + + +/*! + * \brief encodeTRCMMessage Fills an ISO TRCM struct with relevant data fields, and corresponding value IDs and content lengths + * \param triggerID ID of the trigger to be configured + * \param triggerType Type of the trigger to be configured according to ::TriggerType_t + * \param param1 First parameter of the trigger to be configured according to ::TriggerTypeParameter_t + * \param param2 Second parameter of the trigger to be configured ::TriggerTypeParameter_t + * \param param3 Third parameter of the trigger to be configured ::TriggerTypeParameter_t + * \param trcmDataBuffer Buffer to which TRCM message is to be written + * \param bufferLength Size of buffer to which TRCM message is to be written + * \param debug Flag for enabling debugging + * \return Number of bytes written or -1 in case of an error + */ +ssize_t encodeTRCMMessage(const uint16_t triggerID, const TriggerType_t triggerType, const TriggerTypeParameter_t param1, + const TriggerTypeParameter_t param2, const TriggerTypeParameter_t param3, char * trcmDataBuffer, + const size_t bufferLength, const char debug) { + + TRCMType TRCMData; + + memset(trcmDataBuffer, 0, bufferLength); + + // If buffer too small to hold TRCM data, generate an error + if (bufferLength < sizeof (TRCMType)) { + LogMessage(LOG_LEVEL_ERROR, "Buffer too small to hold necessary TRCM data"); + return -1; + } + + // Construct header + TRCMData.header = buildISOHeader(MESSAGE_ID_TRCM, sizeof (TRCMData), debug); + + // Fill contents + TRCMData.triggerIDValueID = VALUE_ID_TRCM_TRIGGER_ID; + TRCMData.triggerIDContentLength = sizeof (TRCMData.triggerID); + TRCMData.triggerID = triggerID; + + TRCMData.triggerTypeValueID = VALUE_ID_TRCM_TRIGGER_TYPE; + TRCMData.triggerTypeContentLength = sizeof (TRCMData.triggerType); + TRCMData.triggerType = (uint16_t) triggerType; + + TRCMData.triggerTypeParameter1ValueID = VALUE_ID_TRCM_TRIGGER_TYPE_PARAM1; + TRCMData.triggerTypeParameter2ValueID = VALUE_ID_TRCM_TRIGGER_TYPE_PARAM2; + TRCMData.triggerTypeParameter3ValueID = VALUE_ID_TRCM_TRIGGER_TYPE_PARAM3; + + TRCMData.triggerTypeParameter1ContentLength = sizeof (TRCMData.triggerTypeParameter1); + TRCMData.triggerTypeParameter2ContentLength = sizeof (TRCMData.triggerTypeParameter2); + TRCMData.triggerTypeParameter3ContentLength = sizeof (TRCMData.triggerTypeParameter3); + + TRCMData.triggerTypeParameter1 = (uint32_t) param1; + TRCMData.triggerTypeParameter2 = (uint32_t) param2; + TRCMData.triggerTypeParameter3 = (uint32_t) param3; + + if (debug) { + LogPrint("TRCM message:\n\tTrigger ID value ID: 0x%x\n\tTrigger ID content length: %u\n\t" + "Trigger ID: %u\n\tTrigger type value ID: 0x%x\n\tTrigger type content length: %u\n\t" + "Trigger type: %u\n\tTrigger type parameter 1 value ID: 0x%x\n\tTrigger type parameter 1 content length: %u\n\t" + "Trigger type parameter 1: %u\n\tTrigger type parameter 2 value ID: 0x%x\n\tTrigger type parameter 2 content length: %u\n\t" + "Trigger type parameter 2: %u\n\tTrigger type parameter 3 value ID: 0x%x\n\tTrigger type parameter 3 content length: %u" + "Trigger type parameter 3: %u\n\t", TRCMData.triggerIDValueID, TRCMData.triggerIDContentLength, TRCMData.triggerID, + TRCMData.triggerTypeValueID, TRCMData.triggerTypeContentLength, TRCMData.triggerType, TRCMData.triggerTypeParameter1ValueID, + TRCMData.triggerTypeParameter1ContentLength, TRCMData.triggerTypeParameter1, TRCMData.triggerTypeParameter2ValueID, + TRCMData.triggerTypeParameter2ContentLength, TRCMData.triggerTypeParameter2, TRCMData.triggerTypeParameter3ValueID, + TRCMData.triggerTypeParameter3ContentLength, TRCMData.triggerTypeParameter3); + } + + // Switch from host endianness to little endian + TRCMData.triggerIDValueID = htole16(TRCMData.triggerIDValueID); + TRCMData.triggerIDContentLength = htole16(TRCMData.triggerIDContentLength); + TRCMData.triggerID = htole16(TRCMData.triggerID); + + TRCMData.triggerTypeValueID = htole16(TRCMData.triggerTypeValueID); + TRCMData.triggerTypeContentLength = htole16(TRCMData.triggerTypeContentLength); + TRCMData.triggerType = htole16(TRCMData.triggerType); + + TRCMData.triggerTypeParameter1ValueID = htole16(TRCMData.triggerTypeParameter1ValueID); + TRCMData.triggerTypeParameter2ValueID = htole16(TRCMData.triggerTypeParameter2ValueID); + TRCMData.triggerTypeParameter3ValueID = htole16(TRCMData.triggerTypeParameter3ValueID); + + TRCMData.triggerTypeParameter1ContentLength = htole16(TRCMData.triggerTypeParameter1ContentLength); + TRCMData.triggerTypeParameter2ContentLength = htole16(TRCMData.triggerTypeParameter2ContentLength); + TRCMData.triggerTypeParameter3ContentLength = htole16(TRCMData.triggerTypeParameter3ContentLength); + + TRCMData.triggerTypeParameter1 = htole32(TRCMData.triggerTypeParameter1); + TRCMData.triggerTypeParameter2 = htole32(TRCMData.triggerTypeParameter2); + TRCMData.triggerTypeParameter3 = htole32(TRCMData.triggerTypeParameter3); + + // Construct footer + TRCMData.footer = buildISOFooter(&TRCMData, sizeof (TRCMData), debug); + + memcpy(trcmDataBuffer, &TRCMData, sizeof (TRCMData)); + + return sizeof (TRCMData); +} + + + + + +/*! + * \brief encodeACCMMessage Fills an ISO ACCM struct with relevant data fields, and corresponding value IDs and content lengths + * \param actionID ID of the action to be configured + * \param actionType Type of the action to be configured according to ::ActionType_t + * \param param1 First parameter of the action to be configured according to ::ActionTypeParameter_t + * \param param2 Second parameter of the action to be configured ::ActionTypeParameter_t + * \param param3 Third parameter of the action to be configured ::ActionTypeParameter_t + * \param trcmDataBuffer Buffer to which ACCM message is to be written + * \param bufferLength Size of buffer to which ACCM message is to be written + * \param debug Flag for enabling debugging + * \return Number of bytes written or -1 in case of an error + */ +ssize_t encodeACCMMessage(const uint16_t actionID, const ActionType_t actionType, const ActionTypeParameter_t param1, + const ActionTypeParameter_t param2, const ActionTypeParameter_t param3, char * accmDataBuffer, + const size_t bufferLength, const char debug) { + + ACCMType ACCMData; + + memset(accmDataBuffer, 0, bufferLength); + + // If buffer too small to hold ACCM data, generate an error + if (bufferLength < sizeof (ACCMType)) { + LogMessage(LOG_LEVEL_ERROR, "Buffer too small to hold necessary ACCM data"); + return -1; + } + + // Construct header + ACCMData.header = buildISOHeader(MESSAGE_ID_ACCM, sizeof (ACCMData), debug); + + // Fill contents + ACCMData.actionIDValueID = VALUE_ID_ACCM_ACTION_ID; + ACCMData.actionIDContentLength = sizeof (ACCMData.actionID); + ACCMData.actionID = actionID; + + ACCMData.actionTypeValueID = VALUE_ID_ACCM_ACTION_TYPE; + ACCMData.actionTypeContentLength = sizeof (ACCMData.actionType); + ACCMData.actionType = (uint16_t) actionType; + + ACCMData.actionTypeParameter1ValueID = VALUE_ID_ACCM_ACTION_TYPE_PARAM1; + ACCMData.actionTypeParameter2ValueID = VALUE_ID_ACCM_ACTION_TYPE_PARAM2; + ACCMData.actionTypeParameter3ValueID = VALUE_ID_ACCM_ACTION_TYPE_PARAM3; + + ACCMData.actionTypeParameter1ContentLength = sizeof (ACCMData.actionTypeParameter1); + ACCMData.actionTypeParameter2ContentLength = sizeof (ACCMData.actionTypeParameter2); + ACCMData.actionTypeParameter3ContentLength = sizeof (ACCMData.actionTypeParameter3); + + ACCMData.actionTypeParameter1 = (uint32_t) param1; + ACCMData.actionTypeParameter2 = (uint32_t) param2; + ACCMData.actionTypeParameter3 = (uint32_t) param3; + + if (debug) { + LogPrint("ACCM message:\n\tAction ID value ID: 0x%x\n\tAction ID content length: %u\n\t" + "Action ID: %u\n\tAction type value ID: 0x%x\n\tAction type content length: %u\n\t" + "Action type: %u\n\tAction type parameter 1 value ID: 0x%x\n\tAction type parameter 1 content length: %u\n\t" + "Action type parameter 1: %u\n\tAction type parameter 2 value ID: 0x%x\n\tAction type parameter 2 content length: %u\n\t" + "Action type parameter 2: %u\n\tAction type parameter 3 value ID: 0x%x\n\tAction type parameter 3 content length: %u" + "Action type parameter 3: %u\n\t", ACCMData.actionIDValueID, ACCMData.actionIDContentLength, ACCMData.actionID, + ACCMData.actionTypeValueID, ACCMData.actionTypeContentLength, ACCMData.actionType, ACCMData.actionTypeParameter1ValueID, + ACCMData.actionTypeParameter1ContentLength, ACCMData.actionTypeParameter1, ACCMData.actionTypeParameter2ValueID, + ACCMData.actionTypeParameter2ContentLength, ACCMData.actionTypeParameter2, ACCMData.actionTypeParameter3ValueID, + ACCMData.actionTypeParameter3ContentLength, ACCMData.actionTypeParameter3); + } + + // Switch from host endianness to little endian + ACCMData.actionIDValueID = htole16(ACCMData.actionIDValueID); + ACCMData.actionIDContentLength = htole16(ACCMData.actionIDContentLength); + ACCMData.actionID = htole16(ACCMData.actionID); + + ACCMData.actionTypeValueID = htole16(ACCMData.actionTypeValueID); + ACCMData.actionTypeContentLength = htole16(ACCMData.actionTypeContentLength); + ACCMData.actionType = htole16(ACCMData.actionType); + + ACCMData.actionTypeParameter1ValueID = htole16(ACCMData.actionTypeParameter1ValueID); + ACCMData.actionTypeParameter2ValueID = htole16(ACCMData.actionTypeParameter2ValueID); + ACCMData.actionTypeParameter3ValueID = htole16(ACCMData.actionTypeParameter3ValueID); + + ACCMData.actionTypeParameter1ContentLength = htole16(ACCMData.actionTypeParameter1ContentLength); + ACCMData.actionTypeParameter2ContentLength = htole16(ACCMData.actionTypeParameter2ContentLength); + ACCMData.actionTypeParameter3ContentLength = htole16(ACCMData.actionTypeParameter3ContentLength); + + ACCMData.actionTypeParameter1 = htole32(ACCMData.actionTypeParameter1); + ACCMData.actionTypeParameter2 = htole32(ACCMData.actionTypeParameter2); + ACCMData.actionTypeParameter3 = htole32(ACCMData.actionTypeParameter3); + + // Construct footer + ACCMData.footer = buildISOFooter(&ACCMData, sizeof (ACCMData), debug); + + memcpy(accmDataBuffer, &ACCMData, sizeof (ACCMData)); + + return sizeof (ACCMData); +} From 22f4324d25c01c6ef3fe7ae3cc1d35fb61abfbb9 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 27 Jan 2020 14:47:25 +0100 Subject: [PATCH 163/523] All trigger/action ISO messages moved to iso22133 --- server/inc/iso22133.h | 34 +-- server/src/iso22133.c | 79 ++++++ server/src/objectcontrol.c | 510 ++----------------------------------- 3 files changed, 100 insertions(+), 523 deletions(-) diff --git a/server/inc/iso22133.h b/server/inc/iso22133.h index 0485d10f0..93fba7fad 100644 --- a/server/inc/iso22133.h +++ b/server/inc/iso22133.h @@ -84,6 +84,7 @@ typedef struct //! MONR value IDs #define VALUE_ID_MONR_STRUCT 0x80 +#pragma pack(pop) typedef enum { TRIGGER_UNDEFINED = 0x0000, @@ -182,37 +183,6 @@ typedef enum { } ActionTypeParameter_t; -//! *************************** TREO -#define COMMAND_TREO_CODE 0x0013 -typedef struct -{ - HeaderType header; - uint16_t triggerIDValueID; - uint16_t triggerIDContentLength; - uint16_t triggerID; - uint16_t timestamp_qmsowValueID; - uint16_t timestamp_qmsowContentLength; - uint32_t timestamp_qmsow; - FooterType footer; -} TREOType; - - -//! *************************** EXAC -#define COMMAND_EXAC_CODE 0x0014 -typedef struct -{ - HeaderType header; - uint16_t actionIDValueID; - uint16_t actionIDContentLength; - uint16_t actionID; - uint16_t executionTime_qmsoWValueID; - uint16_t executionTime_qmsoWContentLength; - uint32_t executionTime_qmsoW; - FooterType footer; -} EXACType; - -#pragma pack(pop) - typedef enum { MESSAGE_OK, MESSAGE_LENGTH_ERROR, @@ -261,7 +231,9 @@ ssize_t encodeOSEMMessage(const double * latitude_deg, const double * longitude_ ssize_t encodeOSTMMessage(const ObjectCommandType command, char * ostmDataBuffer, const size_t bufferLength, const char debug); ssize_t encodeHEABMessage(const ControlCenterStatusType status, char * heabDataBuffer, const size_t bufferLength, const char debug); ssize_t encodeTRCMMessage(const uint16_t triggerID, const TriggerType_t triggerType, const TriggerTypeParameter_t param1, const TriggerTypeParameter_t param2, const TriggerTypeParameter_t param3, char * trcmDataBuffer, const size_t bufferLength, const char debug); +ssize_t decodeTREOMessage(); ssize_t encodeACCMMessage(const uint16_t actionID, const ActionType_t actionType, const ActionTypeParameter_t param1, const ActionTypeParameter_t param2, const 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); ISOMessageReturnValue MONRToASCII(const MONRType * MONRData, char * asciiBuffer, const size_t bufferLength, const char debug); ISOMessageReturnValue ASCIIToMONR(const char * asciiBuffer, MONRType * MONRData, const char debug); ISOMessageID getISOMessageType(const char * messageData, const size_t length, const char debug); diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 59145f92a..4e7cb95f7 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -154,6 +154,17 @@ typedef struct /*! TREO message */ +typedef struct +{ + HeaderType header; + uint16_t triggerIDValueID; + uint16_t triggerIDContentLength; + uint16_t triggerID; + uint16_t timestamp_qmsowValueID; + uint16_t timestamp_qmsowContentLength; + uint32_t timestamp_qmsow; + FooterType footer; +} TREOType; //! TREO value IDs #define VALUE_ID_TREO_TRIGGER_ID 0x0001 @@ -191,6 +202,17 @@ typedef struct /*! EXAC message */ +typedef struct +{ + HeaderType header; + uint16_t actionIDValueID; + uint16_t actionIDContentLength; + uint16_t actionID; + uint16_t executionTime_qmsoWValueID; + uint16_t executionTime_qmsoWContentLength; + uint32_t executionTime_qmsoW; + FooterType footer; +} EXACType; //! EXAC value IDs #define VALUE_ID_EXAC_ACTION_ID 0x0002 @@ -1248,3 +1270,60 @@ ssize_t encodeACCMMessage(const uint16_t actionID, const ActionType_t actionType return sizeof (ACCMData); } + + + +/*! + * \brief encodeEXACMessage Fills an ISO EXAC struct with relevant data fields, and corresponding value IDs and content lengths + * \param actionID ID of the action to be executed + * \param executionTime Time when the action is to be executed + * \param exacDataBuffer Buffer to which EXAC message is to be written + * \param bufferLength Size of buffer to which EXAC message is to be written + * \param debug Flag for enabling debugging + * \return Number of bytes written or -1 in case of an error + */ +ssize_t encodeEXACMessage(const uint16_t actionID, const struct timeval * executionTime, char * exacDataBuffer, const size_t bufferLength, const char debug) { + + EXACType EXACData; + + memset(exacDataBuffer, 0, bufferLength); + + // If buffer too small to hold EXAC data, generate an error + if (bufferLength < sizeof (EXACType)) { + LogMessage(LOG_LEVEL_ERROR, "Buffer too small to hold necessary EXAC data"); + return -1; + } + + // Construct header + EXACData.header = buildISOHeader(MESSAGE_ID_EXAC, sizeof (EXACData), debug); + + EXACData.actionIDValueID = VALUE_ID_EXAC_ACTION_ID; + EXACData.actionIDContentLength = sizeof (EXACData.actionID); + EXACData.actionID = actionID; + + EXACData.executionTime_qmsoWValueID = VALUE_ID_EXAC_ACTION_EXECUTE_TIME; + EXACData.executionTime_qmsoWContentLength = sizeof (EXACData.executionTime_qmsoW); + EXACData.executionTime_qmsoW = TimeGetAsGPSqmsOfWeek(executionTime); + + if (debug) { + LogPrint("EXAC message:\n\tAction ID value ID: 0x%x\n\tAction ID content length: %u\n\tAction ID: %u\n\t" + "Action execute time value ID: 0x%x\n\tAction execute time content length: %u\n\tAction execute time: %u [¼ ms]", + EXACData.actionIDValueID, EXACData.actionIDContentLength, EXACData.actionID, EXACData.executionTime_qmsoWValueID, + EXACData.executionTime_qmsoWContentLength, EXACData.executionTime_qmsoW); + } + + // Switch from host endianness to little endian + EXACData.actionIDValueID = htole16(EXACData.actionIDValueID); + EXACData.actionIDContentLength = htole16(EXACData.actionIDContentLength); + EXACData.actionID = htole16(EXACData.actionID); + EXACData.executionTime_qmsoWValueID = htole16(EXACData.executionTime_qmsoWValueID); + EXACData.executionTime_qmsoWContentLength = htole16(EXACData.executionTime_qmsoWContentLength); + EXACData.executionTime_qmsoW = htole32(EXACData.executionTime_qmsoW); + + // Construct footer + EXACData.footer = buildISOFooter(&EXACData, sizeof (EXACData), debug); + + memcpy(exacDataBuffer, &EXACData, sizeof (EXACData)); + + return sizeof (EXACType); +} diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index ee5e1f9b0..d494e9b8d 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -152,11 +152,6 @@ I32 ObjectControlSendDTMMessage(C8 * DTMData, I32 * Socket, I32 RowCount, C8 * I I32 ObjectControlBuildDTMMessage(C8 * MessageBuffer, C8 * DTMData, I32 RowCount, DOTMType * DOTMData, U8 debug); I32 ObjectControlBuildASPMessage(C8 * MessageBuffer, ASPType * ASPData, U8 debug); -I32 ObjectControlBuildACCMMessage(ACCMData * mqACCMData, ACCMType * ACCM, U8 debug); -I32 ObjectControlBuildEXACMessage(EXACData * mqEXACData, EXACType * EXAC, U8 debug); -I32 ObjectControlBuildTRCMMessage(TRCMData * mqTRCMData, TRCMType * TRCM, U8 debug); -I32 ObjectControlSendACCMMessage(ACCMData * ACCM, I32 * socket, U8 debug); -I32 ObjectControlSendTRCMMessage(TRCMData * TRCM, I32 * socket, U8 debug); I32 ObjectControlSendEXACMessage(EXACData * EXAC, I32 * socket, U8 debug); static int iFindObjectsInfo(C8 object_traj_file[MAX_OBJECTS][MAX_FILE_PATH], @@ -251,6 +246,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { MTSPType MTSPData; ACCMData mqACCMData; EXACData mqEXACData; + TRCMData mqTRCMData; GeoPosition OriginPosition; ASPType ASPData; @@ -593,7 +589,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { TimeSetToUTCms(&startTime, (int64_t) strtoul(MiscPtr, &MiscPtr, 10)); TimeSetToUTCms(&startDelay, (int64_t) strtoul(MiscPtr + 1, NULL, 10)); timeradd(&startTime, &startDelay, &startTime); - MessageLength = (int)encodeSTRTMessage(&startTime, MessageBuffer, sizeof (MessageBuffer), 0); + MessageLength = encodeSTRTMessage(&startTime, MessageBuffer, sizeof (MessageBuffer), 0); ASPData.MTSPU32 = 0; ASPData.TimeToSyncPointDbl = 0; @@ -699,18 +695,31 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { iGetObjectIndexFromObjectIP(mqACCMData.ip, objectIPs, sizeof (objectIPs) / sizeof (objectIPs[0])); if (iIndex != -1) { - ObjectControlSendACCMMessage(&mqACCMData, &(socket_fds[iIndex]), 0); + MessageLength = encodeACCMMessage(mqACCMData.actionID, mqACCMData.actionType, mqACCMData.actionTypeParameter1, + mqACCMData.actionTypeParameter2, mqACCMData.actionTypeParameter3, + MessageBuffer, sizeof (MessageBuffer), 0); + UtilSendTCPData(MODULE_NAME, MessageBuffer, MessageLength, &(socket_fds[iIndex]), 0); } else LogMessage(LOG_LEVEL_WARNING, "Unable to send ACCM: no valid socket found"); } + else if (iCommand == COMM_TRCM && vGetState(GSD) == OBC_STATE_CONNECTED) { + UtilPopulateTRCMDataStructFromMQ(pcRecvBuffer, sizeof (pcRecvBuffer), &mqTRCMData); + iIndex = iGetObjectIndexFromObjectIP(mqTRCMData.ip, objectIPs, + sizeof (objectIPs) / sizeof (objectIPs[0])); + } else if (iCommand == COMM_EXAC && vGetState(GSD) == OBC_STATE_RUNNING) { UtilPopulateEXACDataStructFromMQ(pcRecvBuffer, sizeof (pcRecvBuffer), &mqEXACData); iIndex = iGetObjectIndexFromObjectIP(mqEXACData.ip, objectIPs, sizeof (objectIPs) / sizeof (objectIPs[0])); - if (iIndex != -1) - ObjectControlSendEXACMessage(&mqEXACData, &(socket_fds[iIndex]), 0); + if (iIndex != -1) { + struct timeval executionTime; + TimeSetToGPStime(&executionTime, TimeGetAsGPSweek(¤tTime), mqEXACData.executionTime_qmsoW); + MessageLength = encodeEXACMessage(mqEXACData.actionID, &executionTime, + MessageBuffer, sizeof (MessageBuffer), 0); + UtilSendTCPData(MODULE_NAME, MessageBuffer, MessageLength, &(socket_fds[iIndex]), 0); + } else LogMessage(LOG_LEVEL_WARNING, "Unable to send EXAC: no valid socket found"); } @@ -1633,489 +1642,6 @@ I32 ObjectControlBuildASPMessage(C8 * MessageBuffer, ASPType * ASPData, U8 debug } -/*! - * \brief ObjectControlSendACCMMessage Sends ACCM data, reformatted to an ISO compliant message, to specified TCP socket - * \param ACCM ACCM data from message bus - * \param socket Socket where to send ACCM - * \param debug Debug flag - * \return Length of sent message - */ -I32 ObjectControlSendACCMMessage(ACCMData * ACCM, I32 * socket, U8 debug) { - ACCMType isoACCM; - C8 messageBuffer[sizeof (isoACCM)]; - C8 *ptr = messageBuffer; - U32 messageSize = 0; - - ObjectControlBuildACCMMessage(ACCM, &isoACCM, debug); - - // Copy ACCM header to send buffer - memcpy(ptr, &isoACCM.header.SyncWordU16, sizeof (isoACCM.header.SyncWordU16)); - ptr += sizeof (isoACCM.header.SyncWordU16); - - memcpy(ptr, &isoACCM.header.TransmitterIdU8, sizeof (isoACCM.header.TransmitterIdU8)); - ptr += sizeof (isoACCM.header.TransmitterIdU8); - - memcpy(ptr, &isoACCM.header.MessageCounterU8, sizeof (isoACCM.header.MessageCounterU8)); - ptr += sizeof (isoACCM.header.MessageCounterU8); - - memcpy(ptr, &isoACCM.header.AckReqProtVerU8, sizeof (isoACCM.header.AckReqProtVerU8)); - ptr += sizeof (isoACCM.header.AckReqProtVerU8); - - memcpy(ptr, &isoACCM.header.MessageIdU16, sizeof (isoACCM.header.MessageIdU16)); - ptr += sizeof (isoACCM.header.MessageIdU16); - - memcpy(ptr, &isoACCM.header.MessageLengthU32, sizeof (isoACCM.header.MessageLengthU32)); - ptr += sizeof (isoACCM.header.MessageLengthU32); - - - // Copy ACCM action ID to send buffer - memcpy(ptr, &isoACCM.actionIDValueID, sizeof (isoACCM.actionIDValueID)); - ptr += sizeof (isoACCM.actionIDValueID); - - memcpy(ptr, &isoACCM.actionIDContentLength, sizeof (isoACCM.actionIDContentLength)); - ptr += sizeof (isoACCM.actionIDContentLength); - - memcpy(ptr, &isoACCM.actionID, sizeof (isoACCM.actionID)); - ptr += sizeof (isoACCM.actionID); - - // Copy ACCM action type to send buffer - memcpy(ptr, &isoACCM.actionTypeValueID, sizeof (isoACCM.actionTypeValueID)); - ptr += sizeof (isoACCM.actionTypeValueID); - - memcpy(ptr, &isoACCM.actionTypeContentLength, sizeof (isoACCM.actionTypeContentLength)); - ptr += sizeof (isoACCM.actionTypeContentLength); - - memcpy(ptr, &isoACCM.actionType, sizeof (isoACCM.actionType)); - ptr += sizeof (isoACCM.actionType); - - // Copy ACCM action parameter 1 to send buffer - memcpy(ptr, &isoACCM.actionTypeParameter1ValueID, sizeof (isoACCM.actionTypeParameter1ValueID)); - ptr += sizeof (isoACCM.actionTypeParameter1ValueID); - - memcpy(ptr, &isoACCM.actionTypeParameter1ContentLength, - sizeof (isoACCM.actionTypeParameter1ContentLength)); - ptr += sizeof (isoACCM.actionTypeParameter1ContentLength); - - memcpy(ptr, &isoACCM.actionTypeParameter1, sizeof (isoACCM.actionTypeParameter1)); - ptr += sizeof (isoACCM.actionTypeParameter1); - - // Copy ACCM action parameter 2 to send buffer - memcpy(ptr, &isoACCM.actionTypeParameter2ValueID, sizeof (isoACCM.actionTypeParameter2ValueID)); - ptr += sizeof (isoACCM.actionTypeParameter2ValueID); - - memcpy(ptr, &isoACCM.actionTypeParameter2ContentLength, - sizeof (isoACCM.actionTypeParameter2ContentLength)); - ptr += sizeof (isoACCM.actionTypeParameter2ContentLength); - - memcpy(ptr, &isoACCM.actionTypeParameter2, sizeof (isoACCM.actionTypeParameter2)); - ptr += sizeof (isoACCM.actionTypeParameter2); - - // Copy ACCM action parameter 3 to send buffer - memcpy(ptr, &isoACCM.actionTypeParameter3ValueID, sizeof (isoACCM.actionTypeParameter3ValueID)); - ptr += sizeof (isoACCM.actionTypeParameter3ValueID); - - memcpy(ptr, &isoACCM.actionTypeParameter3ContentLength, - sizeof (isoACCM.actionTypeParameter3ContentLength)); - ptr += sizeof (isoACCM.actionTypeParameter3ContentLength); - - memcpy(ptr, &isoACCM.actionTypeParameter3, sizeof (isoACCM.actionTypeParameter3)); - ptr += sizeof (isoACCM.actionTypeParameter3); - - - // Copy ACCM footer to send buffer - memcpy(ptr, &isoACCM.footer.Crc, sizeof (isoACCM.footer.Crc)); - ptr += sizeof (isoACCM.footer.Crc); - - if (ptr > messageBuffer) - messageSize = (U32) (ptr - messageBuffer); - - if (messageSize - sizeof (isoACCM.header) - sizeof (isoACCM.footer) != isoACCM.header.MessageLengthU32) - LogMessage(LOG_LEVEL_WARNING, "ACCM message sent with invalid message length"); - - UtilSendTCPData(MODULE_NAME, messageBuffer, (I32) messageSize, socket, debug); - - return (I32) messageSize; -} - -/*! - * \brief ObjectControlSendTRCMMessage Sends TRCM data, reformatted to an ISO compliant message, to specified TCP socket - * \param ACCM TRCM data from message bus - * \param socket Socket where to send TRCM - * \param debug Debug flag - * \return Length of sent message - */ -I32 ObjectControlSendTRCMMessage(TRCMData * TRCM, I32 * socket, U8 debug) { - TRCMType isoTRCM; - C8 messageBuffer[sizeof (isoTRCM)]; - C8 *ptr = messageBuffer; - U32 messageSize = 0; - - ObjectControlBuildTRCMMessage(TRCM, &isoTRCM, debug); - - // Copy TRCM header to send buffer - memcpy(ptr, &isoTRCM.header.SyncWordU16, sizeof (isoTRCM.header.SyncWordU16)); - ptr += sizeof (isoTRCM.header.SyncWordU16); - - memcpy(ptr, &isoTRCM.header.TransmitterIdU8, sizeof (isoTRCM.header.TransmitterIdU8)); - ptr += sizeof (isoTRCM.header.TransmitterIdU8); - - memcpy(ptr, &isoTRCM.header.MessageCounterU8, sizeof (isoTRCM.header.MessageCounterU8)); - ptr += sizeof (isoTRCM.header.MessageCounterU8); - - memcpy(ptr, &isoTRCM.header.AckReqProtVerU8, sizeof (isoTRCM.header.AckReqProtVerU8)); - ptr += sizeof (isoTRCM.header.AckReqProtVerU8); - - memcpy(ptr, &isoTRCM.header.MessageIdU16, sizeof (isoTRCM.header.MessageIdU16)); - ptr += sizeof (isoTRCM.header.MessageIdU16); - - memcpy(ptr, &isoTRCM.header.MessageLengthU32, sizeof (isoTRCM.header.MessageLengthU32)); - ptr += sizeof (isoTRCM.header.MessageLengthU32); - - - // Copy TRCM trigger ID to send buffer - memcpy(ptr, &isoTRCM.triggerIDValueID, sizeof (isoTRCM.triggerIDValueID)); - ptr += sizeof (isoTRCM.triggerIDValueID); - - memcpy(ptr, &isoTRCM.triggerIDContentLength, sizeof (isoTRCM.triggerIDContentLength)); - ptr += sizeof (isoTRCM.triggerIDContentLength); - - memcpy(ptr, &isoTRCM.triggerID, sizeof (isoTRCM.triggerID)); - ptr += sizeof (isoTRCM.triggerID); - - // Copy TRCM trigger type to send buffer - memcpy(ptr, &isoTRCM.triggerTypeValueID, sizeof (isoTRCM.triggerTypeValueID)); - ptr += sizeof (isoTRCM.triggerTypeValueID); - - memcpy(ptr, &isoTRCM.triggerTypeContentLength, sizeof (isoTRCM.triggerTypeContentLength)); - ptr += sizeof (isoTRCM.triggerTypeContentLength); - - memcpy(ptr, &isoTRCM.triggerType, sizeof (isoTRCM.triggerType)); - ptr += sizeof (isoTRCM.triggerType); - - // Copy TRCM trigger parameter 1 to send buffer - memcpy(ptr, &isoTRCM.triggerTypeParameter1ValueID, sizeof (isoTRCM.triggerTypeParameter1ValueID)); - ptr += sizeof (isoTRCM.triggerTypeParameter1ValueID); - - memcpy(ptr, &isoTRCM.triggerTypeParameter1ContentLength, - sizeof (isoTRCM.triggerTypeParameter1ContentLength)); - ptr += sizeof (isoTRCM.triggerTypeParameter1ContentLength); - - memcpy(ptr, &isoTRCM.triggerTypeParameter1, sizeof (isoTRCM.triggerTypeParameter1)); - ptr += sizeof (isoTRCM.triggerTypeParameter1); - - // Copy TRCM trigger parameter 2 to send buffer - memcpy(ptr, &isoTRCM.triggerTypeParameter2ValueID, sizeof (isoTRCM.triggerTypeParameter2ValueID)); - ptr += sizeof (isoTRCM.triggerTypeParameter2ValueID); - - memcpy(ptr, &isoTRCM.triggerTypeParameter2ContentLength, - sizeof (isoTRCM.triggerTypeParameter2ContentLength)); - ptr += sizeof (isoTRCM.triggerTypeParameter2ContentLength); - - memcpy(ptr, &isoTRCM.triggerTypeParameter2, sizeof (isoTRCM.triggerTypeParameter2)); - ptr += sizeof (isoTRCM.triggerTypeParameter2); - - // Copy TRCM trigger parameter 3 to send buffer - memcpy(ptr, &isoTRCM.triggerTypeParameter3ValueID, sizeof (isoTRCM.triggerTypeParameter3ValueID)); - ptr += sizeof (isoTRCM.triggerTypeParameter3ValueID); - - memcpy(ptr, &isoTRCM.triggerTypeParameter3ContentLength, - sizeof (isoTRCM.triggerTypeParameter3ContentLength)); - ptr += sizeof (isoTRCM.triggerTypeParameter3ContentLength); - - memcpy(ptr, &isoTRCM.triggerTypeParameter3, sizeof (isoTRCM.triggerTypeParameter3)); - ptr += sizeof (isoTRCM.triggerTypeParameter3); - - - // Copy TRCM footer to send buffer - memcpy(ptr, &isoTRCM.footer.Crc, sizeof (isoTRCM.footer.Crc)); - ptr += sizeof (isoTRCM.footer.Crc); - - if (ptr > messageBuffer) - messageSize = (U32) (ptr - messageBuffer); - - if (messageSize - sizeof (isoTRCM.header) - sizeof (isoTRCM.footer) != isoTRCM.header.MessageLengthU32) - LogMessage(LOG_LEVEL_WARNING, "TRCM message sent with invalid message length"); - - UtilSendTCPData(MODULE_NAME, messageBuffer, (I32) messageSize, socket, 0); - - return (I32) messageSize; -} - -/*! - * \brief ObjectControlSendEXACMessage Sends EXAC data, reformatted to an ISO compliant message, to specified TCP socket - * \param ACCM EXAC data from message bus - * \param socket Socket where to send EXAC - * \param debug Debug flag - * \return Length of sent message - */ -I32 ObjectControlSendEXACMessage(EXACData * EXAC, I32 * socket, U8 debug) { - EXACType isoEXAC; - C8 messageBuffer[sizeof (isoEXAC)]; - C8 *ptr = messageBuffer; - U32 messageSize = 0; - - ObjectControlBuildEXACMessage(EXAC, &isoEXAC, debug); - - // Copy EXAC header to send buffer - memcpy(ptr, &isoEXAC.header.SyncWordU16, sizeof (isoEXAC.header.SyncWordU16)); - ptr += sizeof (isoEXAC.header.SyncWordU16); - - memcpy(ptr, &isoEXAC.header.TransmitterIdU8, sizeof (isoEXAC.header.TransmitterIdU8)); - ptr += sizeof (isoEXAC.header.TransmitterIdU8); - - memcpy(ptr, &isoEXAC.header.MessageCounterU8, sizeof (isoEXAC.header.MessageCounterU8)); - ptr += sizeof (isoEXAC.header.MessageCounterU8); - - memcpy(ptr, &isoEXAC.header.AckReqProtVerU8, sizeof (isoEXAC.header.AckReqProtVerU8)); - ptr += sizeof (isoEXAC.header.AckReqProtVerU8); - - memcpy(ptr, &isoEXAC.header.MessageIdU16, sizeof (isoEXAC.header.MessageIdU16)); - ptr += sizeof (isoEXAC.header.MessageIdU16); - - memcpy(ptr, &isoEXAC.header.MessageLengthU32, sizeof (isoEXAC.header.MessageLengthU32)); - ptr += sizeof (isoEXAC.header.MessageLengthU32); - - - // Copy EXAC action ID to send buffer - memcpy(ptr, &isoEXAC.actionIDValueID, sizeof (isoEXAC.actionIDValueID)); - ptr += sizeof (isoEXAC.actionIDValueID); - - memcpy(ptr, &isoEXAC.actionIDContentLength, sizeof (isoEXAC.actionIDContentLength)); - ptr += sizeof (isoEXAC.actionIDContentLength); - - memcpy(ptr, &isoEXAC.actionID, sizeof (isoEXAC.actionID)); - ptr += sizeof (isoEXAC.actionID); - - // Copy EXAC action execution time to send buffer - memcpy(ptr, &isoEXAC.executionTime_qmsoWValueID, sizeof (isoEXAC.executionTime_qmsoWValueID)); - ptr += sizeof (isoEXAC.executionTime_qmsoWValueID); - - memcpy(ptr, &isoEXAC.executionTime_qmsoWContentLength, sizeof (isoEXAC.executionTime_qmsoWContentLength)); - ptr += sizeof (isoEXAC.executionTime_qmsoWContentLength); - - memcpy(ptr, &isoEXAC.executionTime_qmsoW, sizeof (isoEXAC.executionTime_qmsoW)); - ptr += sizeof (isoEXAC.executionTime_qmsoW); - - - // Copy EXAC footer to send buffer - memcpy(ptr, &isoEXAC.footer.Crc, sizeof (isoEXAC.footer.Crc)); - ptr += sizeof (isoEXAC.footer.Crc); - - if (ptr > messageBuffer) - messageSize = (U32) (ptr - messageBuffer); - - if (messageSize - sizeof (isoEXAC.header) - sizeof (isoEXAC.footer) != isoEXAC.header.MessageLengthU32) - LogMessage(LOG_LEVEL_WARNING, "EXAC message sent with invalid message length"); - - UtilSendTCPData(MODULE_NAME, messageBuffer, (I32) messageSize, socket, 0); - - return (I32) messageSize; -} - - -/*! - * \brief ObjectControlBuildACCMMessage Fills an ISO ACCM struct with relevant data fields, and corresponding value IDs and content lengths - * \param mqACCMData Data which is to fill ACCM struct - * \param ACCM Output ACCM struct - * \param debug Debug flag - * \return Byte size of ACCM struct - */ -I32 ObjectControlBuildACCMMessage(ACCMData * mqACCMData, ACCMType * ACCM, U8 debug) { - // Header - ACCM->header.SyncWordU16 = ISO_SYNC_WORD; - ACCM->header.TransmitterIdU8 = 0; - ACCM->header.MessageCounterU8 = 0; - ACCM->header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; - ACCM->header.MessageIdU16 = COMMAND_ACCM_CODE; - ACCM->header.MessageLengthU32 = sizeof (ACCMType) - sizeof (HeaderType) - sizeof (FooterType); - - // Data fields - ACCM->actionID = mqACCMData->actionID; - ACCM->actionType = mqACCMData->actionType; - ACCM->actionTypeParameter1 = mqACCMData->actionTypeParameter1; - ACCM->actionTypeParameter2 = mqACCMData->actionTypeParameter2; - ACCM->actionTypeParameter3 = mqACCMData->actionTypeParameter3; - - // Value ID fields - ACCM->actionIDValueID = VALUE_ID_ACTION_ID; - ACCM->actionTypeValueID = VALUE_ID_ACTION_TYPE; - ACCM->actionTypeParameter1ValueID = VALUE_ID_ACTION_TYPE_PARAM1; - ACCM->actionTypeParameter2ValueID = VALUE_ID_ACTION_TYPE_PARAM2; - ACCM->actionTypeParameter3ValueID = VALUE_ID_ACTION_TYPE_PARAM3; - - // Content length fields - ACCM->actionIDContentLength = sizeof (ACCM->actionID); - ACCM->actionTypeContentLength = sizeof (ACCM->actionType); - ACCM->actionTypeParameter1ContentLength = sizeof (ACCM->actionTypeParameter1); - ACCM->actionTypeParameter2ContentLength = sizeof (ACCM->actionTypeParameter2); - ACCM->actionTypeParameter3ContentLength = sizeof (ACCM->actionTypeParameter3); - - // Header content length - ACCM->header.MessageLengthU32 = sizeof (ACCM->actionID) + sizeof (ACCM->actionType) - + sizeof (ACCM->actionTypeParameter1) + sizeof (ACCM->actionTypeParameter2) + - sizeof (ACCM->actionTypeParameter3) - + sizeof (ACCM->actionIDValueID) + sizeof (ACCM->actionTypeValueID) - + sizeof (ACCM->actionTypeParameter1ValueID) + sizeof (ACCM->actionTypeParameter1ValueID) + - sizeof (ACCM->actionTypeParameter3ValueID) - + sizeof (ACCM->actionIDContentLength) + sizeof (ACCM->actionTypeContentLength) - + sizeof (ACCM->actionTypeParameter1ContentLength) + - sizeof (ACCM->actionTypeParameter1ContentLength) + sizeof (ACCM->actionTypeParameter3ContentLength); - - - // Footer (TODO) - ACCM->footer.Crc = 0; - - U32 messageLen = - ACCM->header.MessageLengthU32 + sizeof (ACCM->footer.Crc) + sizeof (ACCM->header.SyncWordU16) + - sizeof (ACCM->header.MessageIdU16) + sizeof (ACCM->header.AckReqProtVerU8) + - sizeof (ACCM->header.TransmitterIdU8) + sizeof (ACCM->header.MessageCounterU8) + - sizeof (ACCM->header.MessageLengthU32); - - if (debug) { - LogPrint - ("ACCM (%u bytes):\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x", - messageLen, ACCM->actionIDValueID, ACCM->actionIDContentLength, ACCM->actionID, - ACCM->actionTypeValueID, ACCM->actionTypeContentLength, ACCM->actionType, - ACCM->actionTypeParameter1ValueID, ACCM->actionTypeParameter1ContentLength, - ACCM->actionTypeParameter1, ACCM->actionTypeParameter2ValueID, - ACCM->actionTypeParameter2ContentLength, ACCM->actionTypeParameter2, - ACCM->actionTypeParameter3ValueID, ACCM->actionTypeParameter3ContentLength, - ACCM->actionTypeParameter3); - } - - return (I32) messageLen; -} - -/*! - * \brief ObjectControlBuildEXACMessage Fills an ISO EXAC struct with relevant data fields, and corresponding value IDs and content lengths - * \param mqEXACData Data which is to fill EXAC struct - * \param EXAC Output EXAC struct - * \param debug Debug flag - * \return Byte size of EXAC struct - */ -I32 ObjectControlBuildEXACMessage(EXACData * mqEXACData, EXACType * EXAC, U8 debug) { - // TODO: Make system time follow GPS time (better) or pass the GSD pointer into here somehow - struct timeval systemTime; - - TimeSetToCurrentSystemTime(&systemTime); - - // Header - EXAC->header.SyncWordU16 = ISO_SYNC_WORD; - EXAC->header.TransmitterIdU8 = 0; - EXAC->header.MessageCounterU8 = 0; - EXAC->header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; - EXAC->header.MessageIdU16 = COMMAND_EXAC_CODE; - - // Data fields - EXAC->actionID = mqEXACData->actionID; - EXAC->executionTime_qmsoW = mqEXACData->executionTime_qmsoW; - - // Value ID fields - EXAC->actionIDValueID = VALUE_ID_ACTION_ID; - EXAC->executionTime_qmsoWValueID = VALUE_ID_ACTION_EXECUTE_TIME; - - // Content length fields - EXAC->actionIDContentLength = sizeof (EXAC->actionID); - EXAC->executionTime_qmsoWContentLength = sizeof (EXAC->executionTime_qmsoW); - - - // Header message length - EXAC->header.MessageLengthU32 = sizeof (EXAC->actionID) + sizeof (EXAC->executionTime_qmsoW) - + sizeof (EXAC->actionIDValueID) + sizeof (EXAC->executionTime_qmsoWValueID) - + sizeof (EXAC->actionIDContentLength) + sizeof (EXAC->executionTime_qmsoWContentLength); - - // Footer (TODO) - EXAC->footer.Crc = 0; - - U32 messageLen = - EXAC->header.MessageLengthU32 + sizeof (EXAC->footer.Crc) + sizeof (EXAC->header.SyncWordU16) + - sizeof (EXAC->header.MessageIdU16) + sizeof (EXAC->header.AckReqProtVerU8) + - sizeof (EXAC->header.TransmitterIdU8) + sizeof (EXAC->header.MessageCounterU8) + - sizeof (EXAC->header.MessageLengthU32); - - if (debug) { - LogPrint("EXAC (%u bytes):\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x", messageLen, - EXAC->actionIDValueID, EXAC->actionIDContentLength, EXAC->actionID, - EXAC->executionTime_qmsoWValueID, EXAC->executionTime_qmsoWContentLength, - EXAC->executionTime_qmsoW); - } - - return (I32) messageLen; -} - -/*! - * \brief ObjectControlBuildTRCMMessage Fills an ISO TRCM struct with relevant data fields, and corresponding value IDs and content lengths - * \param mqTRCMData Data which is to fill TRCM struct - * \param TRCM Output TRCM struct - * \param debug Debug flag - * \return Byte size of TRCM struct - */ -I32 ObjectControlBuildTRCMMessage(TRCMData * mqTRCMData, TRCMType * TRCM, U8 debug) { - // Header - TRCM->header.SyncWordU16 = ISO_SYNC_WORD; - TRCM->header.TransmitterIdU8 = 0; - TRCM->header.MessageCounterU8 = 0; - TRCM->header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; - TRCM->header.MessageIdU16 = COMMAND_TRCM_CODE; - - - // Data fields - TRCM->triggerID = mqTRCMData->triggerID; - TRCM->triggerType = mqTRCMData->triggerType; - TRCM->triggerTypeParameter1 = mqTRCMData->triggerTypeParameter1; - TRCM->triggerTypeParameter2 = mqTRCMData->triggerTypeParameter2; - TRCM->triggerTypeParameter3 = mqTRCMData->triggerTypeParameter3; - - // Value ID fields - TRCM->triggerIDValueID = VALUE_ID_TRIGGER_ID; - TRCM->triggerIDValueID = VALUE_ID_TRIGGER_TYPE; - TRCM->triggerTypeParameter1ValueID = VALUE_ID_TRIGGER_TYPE_PARAM1; - TRCM->triggerTypeParameter2ValueID = VALUE_ID_TRIGGER_TYPE_PARAM2; - TRCM->triggerTypeParameter3ValueID = VALUE_ID_TRIGGER_TYPE_PARAM3; - - // Content length fields - TRCM->triggerIDContentLength = sizeof (TRCM->triggerID); - TRCM->triggerTypeContentLength = sizeof (TRCM->triggerType); - TRCM->triggerTypeParameter1ContentLength = sizeof (TRCM->triggerTypeParameter1); - TRCM->triggerTypeParameter2ContentLength = sizeof (TRCM->triggerTypeParameter2); - TRCM->triggerTypeParameter3ContentLength = sizeof (TRCM->triggerTypeParameter3); - - - // Message length in header - TRCM->header.MessageLengthU32 = sizeof (TRCM->triggerID) + sizeof (TRCM->triggerType) - + sizeof (TRCM->triggerTypeParameter1) + sizeof (TRCM->triggerTypeParameter2) + - sizeof (TRCM->triggerTypeParameter3) - + sizeof (TRCM->triggerIDValueID) + sizeof (TRCM->triggerTypeValueID) - + sizeof (TRCM->triggerTypeParameter1ValueID) + sizeof (TRCM->triggerTypeParameter1ValueID) + - sizeof (TRCM->triggerTypeParameter3ValueID) - + sizeof (TRCM->triggerIDContentLength) + sizeof (TRCM->triggerTypeContentLength) - + sizeof (TRCM->triggerTypeParameter1ContentLength) + - sizeof (TRCM->triggerTypeParameter1ContentLength) + sizeof (TRCM->triggerTypeParameter3ContentLength); - - - // Footer (TODO) - TRCM->footer.Crc = 0; - - U32 messageLen = - TRCM->header.MessageLengthU32 + sizeof (TRCM->footer.Crc) + sizeof (TRCM->header.SyncWordU16) + - sizeof (TRCM->header.MessageIdU16) + sizeof (TRCM->header.AckReqProtVerU8) + - sizeof (TRCM->header.TransmitterIdU8) + sizeof (TRCM->header.MessageCounterU8) + - sizeof (TRCM->header.MessageLengthU32); - if (debug) { - LogPrint - ("TRCM (%u bytes):\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x", - messageLen, TRCM->triggerIDValueID, TRCM->triggerIDContentLength, TRCM->triggerID, - TRCM->triggerTypeValueID, TRCM->triggerTypeContentLength, TRCM->triggerType, - TRCM->triggerTypeParameter1ValueID, TRCM->triggerTypeParameter1ContentLength, - TRCM->triggerTypeParameter1, TRCM->triggerTypeParameter2ValueID, - TRCM->triggerTypeParameter2ContentLength, TRCM->triggerTypeParameter2, - TRCM->triggerTypeParameter3ValueID, TRCM->triggerTypeParameter3ContentLength, - TRCM->triggerTypeParameter3); - } - - return (I32) messageLen; -} - I32 ObjectControlSendDTMMessage(C8 * DTMData, I32 * Socket, I32 RowCount, C8 * IP, U32 Port, DOTMType * DOTMData, U8 debug) { From 1d452b4e576e0d1dd73bcd803cfe5c44fffdec14 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 27 Jan 2020 15:59:27 +0100 Subject: [PATCH 164/523] Moved INSUP message to iso22133 --- server/inc/iso22133.h | 11 +++++- server/inc/util.h | 16 --------- server/src/iso22133.c | 61 ++++++++++++++++++++++++++++++++++ server/src/supervisorcontrol.c | 7 ++-- server/src/util.c | 47 -------------------------- 5 files changed, 73 insertions(+), 69 deletions(-) diff --git a/server/inc/iso22133.h b/server/inc/iso22133.h index 93fba7fad..091b339cb 100644 --- a/server/inc/iso22133.h +++ b/server/inc/iso22133.h @@ -193,6 +193,7 @@ typedef enum { MESSAGE_SYNC_WORD_ERROR } ISOMessageReturnValue; +/*! Valid ISO message identifiers */ typedef enum { MESSAGE_ID_INVALID = 0x0000, MESSAGE_ID_TRAJ = 0x0001, @@ -222,9 +223,16 @@ typedef enum { MESSAGE_ID_RESERVE_RANGE_2_LOWER_LIMIT = 0xF000, MESSAGE_ID_RESERVE_RANGE_2_UPPER_LIMIT = 0xFFFF, MESSAGE_ID_VENDOR_SPECIFIC_LOWER_LIMIT = 0xA100, - MESSAGE_ID_VENDOR_SPECIFIC_UPPER_LIMIT = 0xBFFF + MESSAGE_ID_VENDOR_SPECIFIC_UPPER_LIMIT = 0xBFFF, + MESSAGE_ID_VENDOR_SPECIFIC_RISE_INSUP = 0xA102 } ISOMessageID; +/*! Supervisor command */ +typedef enum { + SUPERVISOR_COMMAND_NORMAL = 1, //!< Place supervisor in normal mode + SUPERVISOR_COMMAND_DEBUG = 2 //!< Place supervisor in debug mode +} SupervisorCommandType; + ISOMessageReturnValue decodeMONRMessage(const char * MonrData, const size_t length, MONRType * MONRData, const char debug); ssize_t encodeSTRTMessage(const struct timeval* timeOfStart, char * strtDataBuffer, const size_t bufferLength, const char debug); ssize_t encodeOSEMMessage(const double * latitude_deg, const double * longitude_deg, const float * altitude_m, const float * maxPositionDeviation_m, const float * maxLateralDeviation_m, const float * minimumPositioningAccuracy_m, char * osemDataBuffer, const size_t bufferLength, const char debug); @@ -234,6 +242,7 @@ ssize_t encodeTRCMMessage(const uint16_t triggerID, const TriggerType_t triggerT ssize_t decodeTREOMessage(); ssize_t encodeACCMMessage(const uint16_t actionID, const ActionType_t actionType, const ActionTypeParameter_t param1, const ActionTypeParameter_t param2, const 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 encodeINSUPMessage(const SupervisorCommandType, char * insupDataBuffer, const size_t bufferLength, const char debug); ISOMessageReturnValue MONRToASCII(const MONRType * MONRData, char * asciiBuffer, const size_t bufferLength, const char debug); ISOMessageReturnValue ASCIIToMONR(const char * asciiBuffer, MONRType * MONRData, const char debug); ISOMessageID getISOMessageType(const char * messageData, const size_t length, const char debug); diff --git a/server/inc/util.h b/server/inc/util.h index a507f92d1..dd2d78e1b 100644 --- a/server/inc/util.h +++ b/server/inc/util.h @@ -197,12 +197,6 @@ extern "C"{ #define ISO_MESSAGE_HEADER_LENGTH sizeof(HeaderType) -#define ISO_INSUP_CODE 0xA102 -#define ISO_INSUP_NOFV 1 -#define ISO_INSUP_MESSAGE_LENGTH sizeof(OSTMType) -#define ISO_INSUP_OPT_SET_ARMED_STATE 2 -#define ISO_INSUP_OPT_SET_DISARMED_STATE 3 - #define ISO_TRAJ_CODE 1 #define ISO_DTM_ROWS_IN_TRANSMISSION 40 #define ISO_DTM_ROW_MESSAGE_LENGTH sizeof(DOTMType) @@ -270,15 +264,6 @@ typedef struct } CartesianPosition; -typedef struct -{ - HeaderType Header; - U16 ModeValueIdU16; - U16 ModeContentLengthU16; - U8 ModeU8; -} INSUPType; //16 bytes - - typedef struct { HeaderType Header; @@ -798,7 +783,6 @@ U32 UtilHexTextToBinary(U32 DataLength, C8 *Text, C8 *Binary, U8 Debug); U32 UtilCreateDirContent(C8* DirPath, C8* TempPath); U16 UtilGetMillisecond(TimeType *GPSTime); -I32 UtilISOBuildINSUPMessage(C8* MessageBuffer, INSUPType *INSUPData, C8 CommandOption, U8 Debug); I32 UtilISOBuildTRAJMessageHeader(C8* MessageBuffer, I32 RowCount, HeaderType *HeaderData, TRAJInfoType *TRAJInfoData, U8 Debug); I32 UtilISOBuildTRAJMessage(C8 *MessageBuffer, C8 *DTMData, I32 RowCount, DOTMType *DOTMData, U8 debug); I32 UtilISOBuildTRAJInfo(C8* MessageBuffer, TRAJInfoType *TRAJInfoData, U8 debug); diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 4e7cb95f7..32491615b 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -223,6 +223,20 @@ typedef struct //! CATA value IDs // TODO +/*! INSUP message */ +typedef struct +{ + HeaderType header; + uint16_t modeValueID; + uint16_t modeContentLength; + uint8_t mode; + FooterType footer; +} INSUPType; + +//! INSUP value IDs +#define VALUE_ID_INSUP_MODE 0x0200 + + #pragma pack(pop) @@ -1297,6 +1311,7 @@ ssize_t encodeEXACMessage(const uint16_t actionID, const struct timeval * execut // Construct header EXACData.header = buildISOHeader(MESSAGE_ID_EXAC, sizeof (EXACData), debug); + // Fill contents EXACData.actionIDValueID = VALUE_ID_EXAC_ACTION_ID; EXACData.actionIDContentLength = sizeof (EXACData.actionID); EXACData.actionID = actionID; @@ -1327,3 +1342,49 @@ ssize_t encodeEXACMessage(const uint16_t actionID, const struct timeval * execut return sizeof (EXACType); } + + +ssize_t encodeINSUPMessage(const SupervisorCommandType command, char * insupDataBuffer, const size_t bufferLength, const char debug) { + INSUPType INSUPData; + + memset(insupDataBuffer, 0, bufferLength); + + // If buffer too small to hold EXAC data, generate an error + if (bufferLength < sizeof (INSUPType)) { + LogMessage(LOG_LEVEL_ERROR, "Buffer too small to hold necessary INSUP data"); + return -1; + } + + // Construct header + INSUPData.header = buildISOHeader(MESSAGE_ID_VENDOR_SPECIFIC_RISE_INSUP, sizeof (INSUPData), debug); + + // Fill contents + INSUPData.modeValueID = VALUE_ID_INSUP_MODE; + INSUPData.modeContentLength = sizeof (INSUPData.mode); + INSUPData.mode = (uint8_t) command; + + if (debug) { + + } + + // Switch from host endianness to little endian + INSUPData.modeValueID = htole16(INSUPData.modeValueID); + INSUPData.modeContentLength = htole16(INSUPData.modeContentLength); + + // Construct footer + INSUPData.footer = buildISOFooter(&INSUPData, sizeof (INSUPData), debug); + + memcpy(insupDataBuffer, &INSUPData, sizeof (INSUPData)); + + return sizeof (INSUPData); +} + + + + + + + + + + diff --git a/server/src/supervisorcontrol.c b/server/src/supervisorcontrol.c index 69240ee6c..5c2d2db84 100644 --- a/server/src/supervisorcontrol.c +++ b/server/src/supervisorcontrol.c @@ -86,8 +86,6 @@ void supervisorcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLeve I32 i, j; HeaderType HeaderData; - INSUPType INSUPData; - HEABType HEABData; DOTMType DOTMData; C8 MessageBuffer[SUP_MESSAGE_BUFFER]; @@ -166,10 +164,9 @@ void supervisorcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLeve //Initiate the simulator if not initialized and a there is a valid TCP connection if (SupervisorInitiatedU8 == 0 && SupervisorTCPSocketfdI32 > 0) { - UtilISOBuildINSUPMessage(TxBuffer, &INSUPData, SUP_MODE_NORMAL, 0); + MessageLength = encodeINSUPMessage(SUPERVISOR_COMMAND_NORMAL, TxBuffer, sizeof (TxBuffer), 0); UtilSendTCPData("SupervisorControl", TxBuffer, - INSUPData.Header.MessageLengthU32 + ISO_MESSAGE_HEADER_LENGTH + - ISO_MESSAGE_FOOTER_LENGTH, &SupervisorTCPSocketfdI32, 0); + MessageLength, &SupervisorTCPSocketfdI32, 0); SupervisorInitiatedU8 = 1; } diff --git a/server/src/util.c b/server/src/util.c index 66e00b489..39d1deebb 100644 --- a/server/src/util.c +++ b/server/src/util.c @@ -3211,53 +3211,6 @@ U32 UtilCreateDirContent(C8 * DirPath, C8 * TempPath) { } - -I32 UtilISOBuildINSUPMessage(C8 * MessageBuffer, INSUPType * INSUPData, C8 CommandOption, U8 Debug) { - I32 MessageIndex = 0, i; - U16 Crc = 0; - C8 *p; - - bzero(MessageBuffer, ISO_INSUP_MESSAGE_LENGTH + ISO_MESSAGE_FOOTER_LENGTH); - - INSUPData->Header.SyncWordU16 = ISO_SYNC_WORD; - INSUPData->Header.TransmitterIdU8 = 0; - INSUPData->Header.MessageCounterU8 = 0; - INSUPData->Header.AckReqProtVerU8 = 0; - INSUPData->Header.MessageIdU16 = ISO_INSUP_CODE; - INSUPData->Header.MessageLengthU32 = sizeof (INSUPType) - sizeof (HeaderType); - INSUPData->ModeValueIdU16 = VALUE_ID_INSUP_MODE; - INSUPData->ModeContentLengthU16 = 1; - INSUPData->ModeU8 = (U8) CommandOption; - - p = (C8 *) INSUPData; - for (i = 0; i < sizeof (INSUPType); i++) - *(MessageBuffer + i) = *p++; - Crc = crc_16((const C8 *)MessageBuffer, sizeof (OSTMType)); - Crc = 0; - *(MessageBuffer + i++) = (U8) (Crc >> 8); - *(MessageBuffer + i++) = (U8) (Crc); - MessageIndex = i; - - if (Debug) { - // TODO: Change this when bytes thingy has been implemented in logging - printf("INSUP total length = %d bytes (header+message+footer)\n", - (int)(ISO_INSUP_MESSAGE_LENGTH + ISO_MESSAGE_FOOTER_LENGTH)); - printf("----HEADER----\n"); - for (i = 0; i < sizeof (HeaderType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----MESSAGE----\n"); - for (; i < sizeof (INSUPType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----FOOTER----\n"); - for (; i < MessageIndex; i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n"); - } - - return MessageIndex; //Total number of bytes -} - - U16 UtilGetMillisecond(TimeType * GPSTime) { struct timeval now; U16 MilliU16 = 0, NowU16 = 0; From 0b40c69f4f5da8c79221b71a50e2b3646bbe10ad Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 27 Jan 2020 16:04:14 +0100 Subject: [PATCH 165/523] Transferred refactoring to visualization --- modules/Visualization/src/main.c | 42 ++++++++++++++++---------------- 1 file changed, 21 insertions(+), 21 deletions(-) diff --git a/modules/Visualization/src/main.c b/modules/Visualization/src/main.c index d852b29e1..57565c7c5 100644 --- a/modules/Visualization/src/main.c +++ b/modules/Visualization/src/main.c @@ -47,25 +47,25 @@ void vCreateVisualizationMessage(MonitorDataType * _monitorData, char *_visualiz inet_ntop(AF_INET, &_monitorData->ClientIP, ipStringBuffer, sizeof (ipStringBuffer))); char GPSMsOfWeekString[ENOUGH_BUFFER_SIZE]; - sprintf(GPSMsOfWeekString, "%u", _monitorData->MONR.GPSQmsOfWeekU32); + sprintf(GPSMsOfWeekString, "%u", _monitorData->MONR.gpsQmsOfWeek); char xPosString[ENOUGH_BUFFER_SIZE]; - sprintf(xPosString, "%d", _monitorData->MONR.XPositionI32); + sprintf(xPosString, "%d", _monitorData->MONR.xPosition); char yPosString[ENOUGH_BUFFER_SIZE]; - sprintf(yPosString, "%d", _monitorData->MONR.YPositionI32); + sprintf(yPosString, "%d", _monitorData->MONR.yPosition); char zPosString[ENOUGH_BUFFER_SIZE]; - sprintf(zPosString, "%d", _monitorData->MONR.ZPositionI32); + sprintf(zPosString, "%d", _monitorData->MONR.zPosition); char headingString[ENOUGH_BUFFER_SIZE]; - sprintf(headingString, "%u", _monitorData->MONR.HeadingU16); + sprintf(headingString, "%u", _monitorData->MONR.heading); char longSpeedString[ENOUGH_BUFFER_SIZE]; - sprintf(longSpeedString, "%d", _monitorData->MONR.LongitudinalSpeedI16); + sprintf(longSpeedString, "%d", _monitorData->MONR.longitudinalSpeed); char stateString[ENOUGH_BUFFER_SIZE]; - sprintf(stateString, "%u", _monitorData->MONR.StateU8); + sprintf(stateString, "%u", _monitorData->MONR.state); //Build message from MonitorStruct @@ -78,13 +78,13 @@ void vCreateVisualizationMessage(MonitorDataType * _monitorData, char *_visualiz if (_debug) { //LogMessage(LOG_LEVEL_INFO, "%s", _visualizationMessage); LogMessage(LOG_LEVEL_INFO, "IP: %s", ipStringBuffer); - LogMessage(LOG_LEVEL_INFO, "GPSQmsOfWeek: %u", _monitorData->MONR.GPSQmsOfWeekU32); - LogMessage(LOG_LEVEL_INFO, "X: %d", _monitorData->MONR.XPositionI32); - LogMessage(LOG_LEVEL_INFO, "Y: %d", _monitorData->MONR.YPositionI32); - LogMessage(LOG_LEVEL_INFO, "Z: %d", _monitorData->MONR.ZPositionI32); - LogMessage(LOG_LEVEL_INFO, "Heading: %u", _monitorData->MONR.HeadingU16); - LogMessage(LOG_LEVEL_INFO, "LongSpeed: %d", _monitorData->MONR.LongitudinalSpeedI16); - LogMessage(LOG_LEVEL_INFO, "State: %u", _monitorData->MONR.StateU8); + LogMessage(LOG_LEVEL_INFO, "GPSQmsOfWeek: %u", _monitorData->MONR.gpsQmsOfWeek); + LogMessage(LOG_LEVEL_INFO, "X: %d", _monitorData->MONR.xPosition); + LogMessage(LOG_LEVEL_INFO, "Y: %d", _monitorData->MONR.yPosition); + LogMessage(LOG_LEVEL_INFO, "Z: %d", _monitorData->MONR.zPosition); + LogMessage(LOG_LEVEL_INFO, "Heading: %u", _monitorData->MONR.heading); + LogMessage(LOG_LEVEL_INFO, "LongSpeed: %d", _monitorData->MONR.longitudinalSpeed); + LogMessage(LOG_LEVEL_INFO, "State: %u", _monitorData->MONR.state); LogMessage(LOG_LEVEL_INFO, "MESSAGE-SIZE = %d", _sizeOfVisualizationMessage); } } @@ -153,13 +153,13 @@ int main() { //Calculate size of incoming buffer sizeOfVisualizationMessage = snprintf(dummy, sizeof (dummy), "%u;%d;%d;%d;%u;%d;%u;", - monitorData.MONR.GPSQmsOfWeekU32, - monitorData.MONR.XPositionI32, - monitorData.MONR.YPositionI32, - monitorData.MONR.ZPositionI32, - monitorData.MONR.HeadingU16, - monitorData.MONR.LongitudinalSpeedI16, - monitorData.MONR.StateU8); + monitorData.MONR.gpsQmsOfWeek, + monitorData.MONR.xPosition, + monitorData.MONR.yPosition, + monitorData.MONR.zPosition, + monitorData.MONR.heading, + monitorData.MONR.longitudinalSpeed, + monitorData.MONR.state); sizeOfVisualizationMessage += INET_ADDRSTRLEN; sizeOfVisualizationMessage += 8; //(;) From 083b651bd0032fda40f7f055e1ebe27d5d15309e Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 27 Jan 2020 16:08:53 +0100 Subject: [PATCH 166/523] Ran code formatter --- modules/Visualization/src/main.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/modules/Visualization/src/main.c b/modules/Visualization/src/main.c index 57565c7c5..fb3b87868 100644 --- a/modules/Visualization/src/main.c +++ b/modules/Visualization/src/main.c @@ -158,8 +158,7 @@ int main() { monitorData.MONR.yPosition, monitorData.MONR.zPosition, monitorData.MONR.heading, - monitorData.MONR.longitudinalSpeed, - monitorData.MONR.state); + monitorData.MONR.longitudinalSpeed, monitorData.MONR.state); sizeOfVisualizationMessage += INET_ADDRSTRLEN; sizeOfVisualizationMessage += 8; //(;) From 55a0959a5b5796a3598e5ac5e61cf753726a19a5 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 27 Jan 2020 16:13:54 +0100 Subject: [PATCH 167/523] Ran code formatter --- server/src/iso22133.c | 178 ++++++++++++++++----------------- server/src/objectcontrol.c | 17 ++-- server/src/supervisorcontrol.c | 7 +- 3 files changed, 100 insertions(+), 102 deletions(-) diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 32491615b..46e45d27a 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -27,42 +27,41 @@ static const uint8_t SupportedProtocolVersions[] = { 2 }; #define MAX_LATERAL_DEVIATION_UNAVAILABLE_VALUE 65535 #define MAX_LATERAL_DEVIATION_ONE_METER_VALUE 1000 #define MIN_POSITIONING_ACCURACY_NOT_REQUIRED_VALUE 0 -#define MIN_POSITIONING_ACCURACY_ONE_METER_VALUE 1000 // ISO specification unclear on this value +#define MIN_POSITIONING_ACCURACY_ONE_METER_VALUE 1000 // ISO specification unclear on this value #pragma pack(push,1) /*! OSEM message */ -typedef struct -{ - HeaderType header; - uint16_t latitudeValueID; - uint16_t latitudeContentLength; - int64_t latitude; - uint16_t longitudeValueID; - uint16_t longitudeContentLength; - int64_t longitude; - uint16_t altitudeValueID; - uint16_t altitudeContentLength; - int32_t altitude; - uint16_t dateValueID; - uint16_t dateContentLength; - uint32_t date; - uint16_t GPSWeekValueID; - uint16_t GPSWeekContentLength; - uint16_t GPSWeek; - uint16_t GPSQmsOfWeekValueID; - uint16_t GPSQmsOfWeekContentLength; - uint32_t GPSQmsOfWeek; - uint16_t maxWayDeviationValueID; - uint16_t maxWayDeviationContentLength; - uint16_t maxWayDeviation; - uint16_t maxLateralDeviationValueID; - uint16_t maxLateralDeviationContentLength; - uint16_t maxLateralDeviation; - uint16_t minPosAccuracyValueID; - uint16_t minPosAccuracyContentLength; - uint16_t minPosAccuracy; - FooterType footer; -} OSEMType; //85 bytes +typedef struct { + HeaderType header; + uint16_t latitudeValueID; + uint16_t latitudeContentLength; + int64_t latitude; + uint16_t longitudeValueID; + uint16_t longitudeContentLength; + int64_t longitude; + uint16_t altitudeValueID; + uint16_t altitudeContentLength; + int32_t altitude; + uint16_t dateValueID; + uint16_t dateContentLength; + uint32_t date; + uint16_t GPSWeekValueID; + uint16_t GPSWeekContentLength; + uint16_t GPSWeek; + uint16_t GPSQmsOfWeekValueID; + uint16_t GPSQmsOfWeekContentLength; + uint32_t GPSQmsOfWeek; + uint16_t maxWayDeviationValueID; + uint16_t maxWayDeviationContentLength; + uint16_t maxWayDeviation; + uint16_t maxLateralDeviationValueID; + uint16_t maxLateralDeviationContentLength; + uint16_t maxLateralDeviation; + uint16_t minPosAccuracyValueID; + uint16_t minPosAccuracyContentLength; + uint16_t minPosAccuracy; + FooterType footer; +} OSEMType; //85 bytes //! OSEM value IDs #define VALUE_ID_OSEM_LATITUDE 0x0020 @@ -77,22 +76,20 @@ typedef struct /*! OSTM message */ -typedef struct -{ - HeaderType header; - uint16_t stateValueID; - uint16_t stateContentLength; - uint8_t state; - FooterType footer; -} OSTMType; //16 bytes +typedef struct { + HeaderType header; + uint16_t stateValueID; + uint16_t stateContentLength; + uint8_t state; + FooterType footer; +} OSTMType; //16 bytes //! OSTM value IDs #define VALUE_ID_OSTM_STATE_CHANGE_REQUEST 0x0064 /*! STRT message */ -typedef struct -{ +typedef struct { HeaderType header; uint16_t StartTimeValueIdU16; uint16_t StartTimeContentLengthU16; @@ -101,7 +98,7 @@ typedef struct uint16_t GPSWeekContentLength; uint16_t GPSWeek; FooterType footer; -} STRTType; //27 bytes +} STRTType; //27 bytes //! STRT value IDs #define VALUE_ID_STRT_GPS_QMS_OF_WEEK 0x0002 @@ -109,23 +106,21 @@ typedef struct /*! HEAB message */ -typedef struct -{ - HeaderType header; - uint16_t HEABStructValueID; - uint16_t HEABStructContentLength; - uint32_t GPSQmsOfWeek; - uint8_t controlCenterStatus; - FooterType footer; -} HEABType; //16 bytes +typedef struct { + HeaderType header; + uint16_t HEABStructValueID; + uint16_t HEABStructContentLength; + uint32_t GPSQmsOfWeek; + uint8_t controlCenterStatus; + FooterType footer; +} HEABType; //16 bytes //! HEAB value IDs #define VALUE_ID_HEAB_STRUCT 0x0090 /*! TRCM message */ -typedef struct -{ +typedef struct { HeaderType header; uint16_t triggerIDValueID; uint16_t triggerIDContentLength; @@ -154,8 +149,7 @@ typedef struct /*! TREO message */ -typedef struct -{ +typedef struct { HeaderType header; uint16_t triggerIDValueID; uint16_t triggerIDContentLength; @@ -172,8 +166,7 @@ typedef struct /*! ACCM message */ -typedef struct -{ +typedef struct { HeaderType header; uint16_t actionIDValueID; uint16_t actionIDContentLength; @@ -202,8 +195,7 @@ typedef struct /*! EXAC message */ -typedef struct -{ +typedef struct { HeaderType header; uint16_t actionIDValueID; uint16_t actionIDContentLength; @@ -224,8 +216,7 @@ typedef struct // TODO /*! INSUP message */ -typedef struct -{ +typedef struct { HeaderType header; uint16_t modeValueID; uint16_t modeContentLength; @@ -1109,8 +1100,9 @@ ISOMessageReturnValue ASCIIToMONR(const char *asciiBuffer, MONRType * MONRData, * \param debug Flag for enabling debugging * \return Number of bytes written or -1 in case of an error */ -ssize_t encodeTRCMMessage(const uint16_t triggerID, const TriggerType_t triggerType, const TriggerTypeParameter_t param1, - const TriggerTypeParameter_t param2, const TriggerTypeParameter_t param3, char * trcmDataBuffer, +ssize_t encodeTRCMMessage(const uint16_t triggerID, const TriggerType_t triggerType, + const TriggerTypeParameter_t param1, const TriggerTypeParameter_t param2, + const TriggerTypeParameter_t param3, char *trcmDataBuffer, const size_t bufferLength, const char debug) { TRCMType TRCMData; @@ -1153,11 +1145,14 @@ ssize_t encodeTRCMMessage(const uint16_t triggerID, const TriggerType_t triggerT "Trigger type: %u\n\tTrigger type parameter 1 value ID: 0x%x\n\tTrigger type parameter 1 content length: %u\n\t" "Trigger type parameter 1: %u\n\tTrigger type parameter 2 value ID: 0x%x\n\tTrigger type parameter 2 content length: %u\n\t" "Trigger type parameter 2: %u\n\tTrigger type parameter 3 value ID: 0x%x\n\tTrigger type parameter 3 content length: %u" - "Trigger type parameter 3: %u\n\t", TRCMData.triggerIDValueID, TRCMData.triggerIDContentLength, TRCMData.triggerID, - TRCMData.triggerTypeValueID, TRCMData.triggerTypeContentLength, TRCMData.triggerType, TRCMData.triggerTypeParameter1ValueID, - TRCMData.triggerTypeParameter1ContentLength, TRCMData.triggerTypeParameter1, TRCMData.triggerTypeParameter2ValueID, - TRCMData.triggerTypeParameter2ContentLength, TRCMData.triggerTypeParameter2, TRCMData.triggerTypeParameter3ValueID, - TRCMData.triggerTypeParameter3ContentLength, TRCMData.triggerTypeParameter3); + "Trigger type parameter 3: %u\n\t", TRCMData.triggerIDValueID, + TRCMData.triggerIDContentLength, TRCMData.triggerID, TRCMData.triggerTypeValueID, + TRCMData.triggerTypeContentLength, TRCMData.triggerType, + TRCMData.triggerTypeParameter1ValueID, TRCMData.triggerTypeParameter1ContentLength, + TRCMData.triggerTypeParameter1, TRCMData.triggerTypeParameter2ValueID, + TRCMData.triggerTypeParameter2ContentLength, TRCMData.triggerTypeParameter2, + TRCMData.triggerTypeParameter3ValueID, TRCMData.triggerTypeParameter3ContentLength, + TRCMData.triggerTypeParameter3); } // Switch from host endianness to little endian @@ -1205,9 +1200,10 @@ ssize_t encodeTRCMMessage(const uint16_t triggerID, const TriggerType_t triggerT * \param debug Flag for enabling debugging * \return Number of bytes written or -1 in case of an error */ -ssize_t encodeACCMMessage(const uint16_t actionID, const ActionType_t actionType, const ActionTypeParameter_t param1, - const ActionTypeParameter_t param2, const ActionTypeParameter_t param3, char * accmDataBuffer, - const size_t bufferLength, const char debug) { +ssize_t encodeACCMMessage(const uint16_t actionID, const ActionType_t actionType, + const ActionTypeParameter_t param1, const ActionTypeParameter_t param2, + const ActionTypeParameter_t param3, char *accmDataBuffer, const size_t bufferLength, + const char debug) { ACCMType ACCMData; @@ -1249,10 +1245,12 @@ ssize_t encodeACCMMessage(const uint16_t actionID, const ActionType_t actionType "Action type: %u\n\tAction type parameter 1 value ID: 0x%x\n\tAction type parameter 1 content length: %u\n\t" "Action type parameter 1: %u\n\tAction type parameter 2 value ID: 0x%x\n\tAction type parameter 2 content length: %u\n\t" "Action type parameter 2: %u\n\tAction type parameter 3 value ID: 0x%x\n\tAction type parameter 3 content length: %u" - "Action type parameter 3: %u\n\t", ACCMData.actionIDValueID, ACCMData.actionIDContentLength, ACCMData.actionID, - ACCMData.actionTypeValueID, ACCMData.actionTypeContentLength, ACCMData.actionType, ACCMData.actionTypeParameter1ValueID, - ACCMData.actionTypeParameter1ContentLength, ACCMData.actionTypeParameter1, ACCMData.actionTypeParameter2ValueID, - ACCMData.actionTypeParameter2ContentLength, ACCMData.actionTypeParameter2, ACCMData.actionTypeParameter3ValueID, + "Action type parameter 3: %u\n\t", ACCMData.actionIDValueID, ACCMData.actionIDContentLength, + ACCMData.actionID, ACCMData.actionTypeValueID, ACCMData.actionTypeContentLength, + ACCMData.actionType, ACCMData.actionTypeParameter1ValueID, + ACCMData.actionTypeParameter1ContentLength, ACCMData.actionTypeParameter1, + ACCMData.actionTypeParameter2ValueID, ACCMData.actionTypeParameter2ContentLength, + ACCMData.actionTypeParameter2, ACCMData.actionTypeParameter3ValueID, ACCMData.actionTypeParameter3ContentLength, ACCMData.actionTypeParameter3); } @@ -1296,7 +1294,8 @@ ssize_t encodeACCMMessage(const uint16_t actionID, const ActionType_t actionType * \param debug Flag for enabling debugging * \return Number of bytes written or -1 in case of an error */ -ssize_t encodeEXACMessage(const uint16_t actionID, const struct timeval * executionTime, char * exacDataBuffer, 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) { EXACType EXACData; @@ -1321,10 +1320,12 @@ ssize_t encodeEXACMessage(const uint16_t actionID, const struct timeval * execut EXACData.executionTime_qmsoW = TimeGetAsGPSqmsOfWeek(executionTime); if (debug) { - LogPrint("EXAC message:\n\tAction ID value ID: 0x%x\n\tAction ID content length: %u\n\tAction ID: %u\n\t" - "Action execute time value ID: 0x%x\n\tAction execute time content length: %u\n\tAction execute time: %u [¼ ms]", - EXACData.actionIDValueID, EXACData.actionIDContentLength, EXACData.actionID, EXACData.executionTime_qmsoWValueID, - EXACData.executionTime_qmsoWContentLength, EXACData.executionTime_qmsoW); + LogPrint + ("EXAC message:\n\tAction ID value ID: 0x%x\n\tAction ID content length: %u\n\tAction ID: %u\n\t" + "Action execute time value ID: 0x%x\n\tAction execute time content length: %u\n\tAction execute time: %u [¼ ms]", + EXACData.actionIDValueID, EXACData.actionIDContentLength, EXACData.actionID, + EXACData.executionTime_qmsoWValueID, EXACData.executionTime_qmsoWContentLength, + EXACData.executionTime_qmsoW); } // Switch from host endianness to little endian @@ -1344,7 +1345,8 @@ ssize_t encodeEXACMessage(const uint16_t actionID, const struct timeval * execut } -ssize_t encodeINSUPMessage(const SupervisorCommandType command, char * insupDataBuffer, const size_t bufferLength, const char debug) { +ssize_t encodeINSUPMessage(const SupervisorCommandType command, char *insupDataBuffer, + const size_t bufferLength, const char debug) { INSUPType INSUPData; memset(insupDataBuffer, 0, bufferLength); @@ -1378,13 +1380,3 @@ ssize_t encodeINSUPMessage(const SupervisorCommandType command, char * insupData return sizeof (INSUPData); } - - - - - - - - - - diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index d494e9b8d..6f97abb95 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -695,9 +695,11 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { iGetObjectIndexFromObjectIP(mqACCMData.ip, objectIPs, sizeof (objectIPs) / sizeof (objectIPs[0])); if (iIndex != -1) { - MessageLength = encodeACCMMessage(mqACCMData.actionID, mqACCMData.actionType, mqACCMData.actionTypeParameter1, - mqACCMData.actionTypeParameter2, mqACCMData.actionTypeParameter3, - MessageBuffer, sizeof (MessageBuffer), 0); + MessageLength = + encodeACCMMessage(mqACCMData.actionID, mqACCMData.actionType, + mqACCMData.actionTypeParameter1, mqACCMData.actionTypeParameter2, + mqACCMData.actionTypeParameter3, MessageBuffer, + sizeof (MessageBuffer), 0); UtilSendTCPData(MODULE_NAME, MessageBuffer, MessageLength, &(socket_fds[iIndex]), 0); } else @@ -715,9 +717,12 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { sizeof (objectIPs) / sizeof (objectIPs[0])); if (iIndex != -1) { struct timeval executionTime; - TimeSetToGPStime(&executionTime, TimeGetAsGPSweek(¤tTime), mqEXACData.executionTime_qmsoW); - MessageLength = encodeEXACMessage(mqEXACData.actionID, &executionTime, - MessageBuffer, sizeof (MessageBuffer), 0); + + TimeSetToGPStime(&executionTime, TimeGetAsGPSweek(¤tTime), + mqEXACData.executionTime_qmsoW); + MessageLength = + encodeEXACMessage(mqEXACData.actionID, &executionTime, MessageBuffer, + sizeof (MessageBuffer), 0); UtilSendTCPData(MODULE_NAME, MessageBuffer, MessageLength, &(socket_fds[iIndex]), 0); } else diff --git a/server/src/supervisorcontrol.c b/server/src/supervisorcontrol.c index 5c2d2db84..f061d63c2 100644 --- a/server/src/supervisorcontrol.c +++ b/server/src/supervisorcontrol.c @@ -164,9 +164,10 @@ void supervisorcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLeve //Initiate the simulator if not initialized and a there is a valid TCP connection if (SupervisorInitiatedU8 == 0 && SupervisorTCPSocketfdI32 > 0) { - MessageLength = encodeINSUPMessage(SUPERVISOR_COMMAND_NORMAL, TxBuffer, sizeof (TxBuffer), 0); - UtilSendTCPData("SupervisorControl", TxBuffer, - MessageLength, &SupervisorTCPSocketfdI32, 0); + MessageLength = + encodeINSUPMessage(SUPERVISOR_COMMAND_NORMAL, TxBuffer, sizeof (TxBuffer), 0); + UtilSendTCPData("SupervisorControl", TxBuffer, MessageLength, &SupervisorTCPSocketfdI32, + 0); SupervisorInitiatedU8 = 1; } From b1a9aeb15462906fcc7a77b6fc245468b827aaf9 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 28 Jan 2020 09:59:37 +0100 Subject: [PATCH 168/523] WIP, implementation of MTSP encoder --- server/inc/util.h | 20 ----------------- server/src/iso22133.c | 52 +++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 52 insertions(+), 20 deletions(-) diff --git a/server/inc/util.h b/server/inc/util.h index dd2d78e1b..dce866e33 100644 --- a/server/inc/util.h +++ b/server/inc/util.h @@ -264,26 +264,6 @@ typedef struct } CartesianPosition; -typedef struct -{ - HeaderType Header; - U16 SyncPointTimeValueIdU16; - U16 SyncPointTimeContentLengthU16; - U32 SyncPointTimeU32; - U16 FreezeTimeValueIdU16; - U16 FreezeTimeContentLengthU16; - U32 FreezeTimeU32; -} SYPMType; // - - -typedef struct -{ - HeaderType Header; - U16 EstSyncPointTimeValueIdU16; - U16 EstSyncPointTimeContentLengthU16; - U32 EstSyncPointTimeU32; -} MTSPType; // - typedef struct { diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 46e45d27a..7f432179a 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -119,6 +119,35 @@ typedef struct { #define VALUE_ID_HEAB_STRUCT 0x0090 +/*! SYPM message */ +typedef struct { + HeaderType header; + uint16_t syncPointTimeValueID; + uint16_t syncPointTimeContentLength; + uint32_t syncPointTime; + uint16_t freezeTimeValueID; + uint16_t freezeTimeContentLength; + uint32_t freezeTime; + FooterType footer; +} SYPMType; + +//! SYPM value IDs +#define VALUE_ID_SYPM_SYNC_POINT_TIME 0x0001 +#define VALUE_ID_SYPM_FREEZE_TIME 0x0002 + + +/*! MTSP message */ +typedef struct { + HeaderType header; + uint16_t estSyncPointTimeValueID; + uint16_t estSyncPointTimeContentLength; + uint32_t estSyncPointTime; +} MTSPType; + +//! MTSP value IDs +#define VALUE_ID_MTSP_EST_SYNC_POINT_TIME 0x0001 + + /*! TRCM message */ typedef struct { HeaderType header; @@ -1087,6 +1116,29 @@ ISOMessageReturnValue ASCIIToMONR(const char *asciiBuffer, MONRType * MONRData, } +ssize_t encodeMTSPMessage(const struct timeval * estSyncPointTime, char * mtspDataBuffer, const size_t bufferLength, const char debug) { + + MTSPType MTSPData; + + memset(mtspDataBuffer, 0, bufferLength); + + // If buffer too small to hold TRCM data, generate an error + if (bufferLength < sizeof (MTSPType)) { + LogMessage(LOG_LEVEL_ERROR, "Buffer too small to hold necessary MTSP data"); + return -1; + } + + // Construct header + MTSPData.header = buildISOHeader(MESSAGE_ID_MTSP, sizeof (MTSPData), debug); + + // Fill contents + MTSPData.estSyncPointTimeValueID = VALUE_ID_MTSP_EST_SYNC_POINT_TIME; + MTSPData.estSyncPointTimeContentLength = sizeof (MTSPData.estSyncPointTime); + MTSPData.estSyncPointTime = estSyncPointTime == NULL ? GPS_SECOND_OF_WEEK_UNAVAILABLE_VALUE : TimeGetAsGPSqmsOfWeek(estSyncPointTime); + + // TODO +} + /*! * \brief encodeTRCMMessage Fills an ISO TRCM struct with relevant data fields, and corresponding value IDs and content lengths From 7a43c2382eef45d99399d1812feeeceb41f50a66 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 28 Jan 2020 10:47:37 +0100 Subject: [PATCH 169/523] Made parameters which have an 'unavailable' value option in their ISO fields optional as function call parameters --- server/inc/iso22133.h | 6 ++--- server/src/iso22133.c | 45 ++++++++++++++++++++++---------------- server/src/objectcontrol.c | 18 +++++++++++---- 3 files changed, 43 insertions(+), 26 deletions(-) diff --git a/server/inc/iso22133.h b/server/inc/iso22133.h index 091b339cb..b7c903eeb 100644 --- a/server/inc/iso22133.h +++ b/server/inc/iso22133.h @@ -238,10 +238,10 @@ ssize_t encodeSTRTMessage(const struct timeval* timeOfStart, char * strtDataBuff ssize_t encodeOSEMMessage(const double * latitude_deg, const double * longitude_deg, const float * altitude_m, const float * maxPositionDeviation_m, const float * maxLateralDeviation_m, const float * minimumPositioningAccuracy_m, char * osemDataBuffer, const size_t bufferLength, const char debug); ssize_t encodeOSTMMessage(const ObjectCommandType command, char * ostmDataBuffer, const size_t bufferLength, const char debug); ssize_t encodeHEABMessage(const ControlCenterStatusType status, char * heabDataBuffer, const size_t bufferLength, const char debug); -ssize_t encodeTRCMMessage(const uint16_t triggerID, const TriggerType_t triggerType, const TriggerTypeParameter_t param1, const TriggerTypeParameter_t param2, const TriggerTypeParameter_t param3, char * trcmDataBuffer, const size_t bufferLength, const char debug); +ssize_t encodeTRCMMessage(const uint16_t* triggerID, const TriggerType_t* triggerType, const TriggerTypeParameter_t* param1, const TriggerTypeParameter_t* param2, const TriggerTypeParameter_t* param3, char * trcmDataBuffer, const size_t bufferLength, const char debug); ssize_t decodeTREOMessage(); -ssize_t encodeACCMMessage(const uint16_t actionID, const ActionType_t actionType, const ActionTypeParameter_t param1, const ActionTypeParameter_t param2, const 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 encodeACCMMessage(const uint16_t* actionID, const ActionType_t* actionType, const ActionTypeParameter_t* param1, const ActionTypeParameter_t* param2, const 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 encodeINSUPMessage(const SupervisorCommandType, char * insupDataBuffer, const size_t bufferLength, const char debug); ISOMessageReturnValue MONRToASCII(const MONRType * MONRData, char * asciiBuffer, const size_t bufferLength, const char debug); ISOMessageReturnValue ASCIIToMONR(const char * asciiBuffer, MONRType * MONRData, const char debug); diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 46e45d27a..5ca42bd9b 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -28,6 +28,13 @@ static const uint8_t SupportedProtocolVersions[] = { 2 }; #define MAX_LATERAL_DEVIATION_ONE_METER_VALUE 1000 #define MIN_POSITIONING_ACCURACY_NOT_REQUIRED_VALUE 0 #define MIN_POSITIONING_ACCURACY_ONE_METER_VALUE 1000 // ISO specification unclear on this value +#define TRIGGER_ID_UNAVAILABLE 65535 +#define TRIGGER_TYPE_UNAVAILABLE 65535 +#define TRIGGER_TYPE_PARAMETER_UNAVAILABLE 4294967295 +#define ACTION_ID_UNAVAILABLE 65535 +#define ACTION_TYPE_UNAVAILABLE 65535 +#define ACTION_TYPE_PARAMETER_UNAVAILABLE 4294967295 + #pragma pack(push,1) /*! OSEM message */ @@ -1100,9 +1107,9 @@ ISOMessageReturnValue ASCIIToMONR(const char *asciiBuffer, MONRType * MONRData, * \param debug Flag for enabling debugging * \return Number of bytes written or -1 in case of an error */ -ssize_t encodeTRCMMessage(const uint16_t triggerID, const TriggerType_t triggerType, - const TriggerTypeParameter_t param1, const TriggerTypeParameter_t param2, - const TriggerTypeParameter_t param3, char *trcmDataBuffer, +ssize_t encodeTRCMMessage(const uint16_t * triggerID, const TriggerType_t * triggerType, + const TriggerTypeParameter_t * param1, const TriggerTypeParameter_t * param2, + const TriggerTypeParameter_t * param3, char *trcmDataBuffer, const size_t bufferLength, const char debug) { TRCMType TRCMData; @@ -1121,11 +1128,11 @@ ssize_t encodeTRCMMessage(const uint16_t triggerID, const TriggerType_t triggerT // Fill contents TRCMData.triggerIDValueID = VALUE_ID_TRCM_TRIGGER_ID; TRCMData.triggerIDContentLength = sizeof (TRCMData.triggerID); - TRCMData.triggerID = triggerID; + TRCMData.triggerID = triggerID == NULL ? TRIGGER_ID_UNAVAILABLE : *triggerID; TRCMData.triggerTypeValueID = VALUE_ID_TRCM_TRIGGER_TYPE; TRCMData.triggerTypeContentLength = sizeof (TRCMData.triggerType); - TRCMData.triggerType = (uint16_t) triggerType; + TRCMData.triggerType = triggerType == NULL ? TRIGGER_TYPE_UNAVAILABLE : (uint16_t) (*triggerType); TRCMData.triggerTypeParameter1ValueID = VALUE_ID_TRCM_TRIGGER_TYPE_PARAM1; TRCMData.triggerTypeParameter2ValueID = VALUE_ID_TRCM_TRIGGER_TYPE_PARAM2; @@ -1135,9 +1142,9 @@ ssize_t encodeTRCMMessage(const uint16_t triggerID, const TriggerType_t triggerT TRCMData.triggerTypeParameter2ContentLength = sizeof (TRCMData.triggerTypeParameter2); TRCMData.triggerTypeParameter3ContentLength = sizeof (TRCMData.triggerTypeParameter3); - TRCMData.triggerTypeParameter1 = (uint32_t) param1; - TRCMData.triggerTypeParameter2 = (uint32_t) param2; - TRCMData.triggerTypeParameter3 = (uint32_t) param3; + TRCMData.triggerTypeParameter1 = param1 == NULL ? TRIGGER_TYPE_PARAMETER_UNAVAILABLE : (uint32_t) (*param1); + TRCMData.triggerTypeParameter2 = param2 == NULL ? TRIGGER_TYPE_PARAMETER_UNAVAILABLE : (uint32_t) (*param2); + TRCMData.triggerTypeParameter3 = param3 == NULL ? TRIGGER_TYPE_PARAMETER_UNAVAILABLE : (uint32_t) (*param3); if (debug) { LogPrint("TRCM message:\n\tTrigger ID value ID: 0x%x\n\tTrigger ID content length: %u\n\t" @@ -1200,9 +1207,9 @@ ssize_t encodeTRCMMessage(const uint16_t triggerID, const TriggerType_t triggerT * \param debug Flag for enabling debugging * \return Number of bytes written or -1 in case of an error */ -ssize_t encodeACCMMessage(const uint16_t actionID, const ActionType_t actionType, - const ActionTypeParameter_t param1, const ActionTypeParameter_t param2, - const ActionTypeParameter_t param3, char *accmDataBuffer, const size_t bufferLength, +ssize_t encodeACCMMessage(const uint16_t *actionID, const ActionType_t *actionType, + const ActionTypeParameter_t *param1, const ActionTypeParameter_t *param2, + const ActionTypeParameter_t *param3, char *accmDataBuffer, const size_t bufferLength, const char debug) { ACCMType ACCMData; @@ -1221,11 +1228,11 @@ ssize_t encodeACCMMessage(const uint16_t actionID, const ActionType_t actionType // Fill contents ACCMData.actionIDValueID = VALUE_ID_ACCM_ACTION_ID; ACCMData.actionIDContentLength = sizeof (ACCMData.actionID); - ACCMData.actionID = actionID; + ACCMData.actionID = actionID == NULL ? ACTION_ID_UNAVAILABLE : *actionID; ACCMData.actionTypeValueID = VALUE_ID_ACCM_ACTION_TYPE; ACCMData.actionTypeContentLength = sizeof (ACCMData.actionType); - ACCMData.actionType = (uint16_t) actionType; + ACCMData.actionType = actionType == NULL ? ACTION_TYPE_UNAVAILABLE : (uint16_t) (*actionType); ACCMData.actionTypeParameter1ValueID = VALUE_ID_ACCM_ACTION_TYPE_PARAM1; ACCMData.actionTypeParameter2ValueID = VALUE_ID_ACCM_ACTION_TYPE_PARAM2; @@ -1235,9 +1242,9 @@ ssize_t encodeACCMMessage(const uint16_t actionID, const ActionType_t actionType ACCMData.actionTypeParameter2ContentLength = sizeof (ACCMData.actionTypeParameter2); ACCMData.actionTypeParameter3ContentLength = sizeof (ACCMData.actionTypeParameter3); - ACCMData.actionTypeParameter1 = (uint32_t) param1; - ACCMData.actionTypeParameter2 = (uint32_t) param2; - ACCMData.actionTypeParameter3 = (uint32_t) param3; + ACCMData.actionTypeParameter1 = param1 == NULL ? ACTION_TYPE_PARAMETER_UNAVAILABLE : (uint32_t) (*param1); + ACCMData.actionTypeParameter2 = param2 == NULL ? ACTION_TYPE_PARAMETER_UNAVAILABLE : (uint32_t) (*param2); + ACCMData.actionTypeParameter3 = param3 == NULL ? ACTION_TYPE_PARAMETER_UNAVAILABLE : (uint32_t) (*param3); if (debug) { LogPrint("ACCM message:\n\tAction ID value ID: 0x%x\n\tAction ID content length: %u\n\t" @@ -1294,7 +1301,7 @@ ssize_t encodeACCMMessage(const uint16_t actionID, const ActionType_t actionType * \param debug Flag for enabling debugging * \return Number of bytes written or -1 in case of an error */ -ssize_t encodeEXACMessage(const uint16_t actionID, const struct timeval *executionTime, char *exacDataBuffer, +ssize_t encodeEXACMessage(const uint16_t * actionID, const struct timeval *executionTime, char *exacDataBuffer, const size_t bufferLength, const char debug) { EXACType EXACData; @@ -1313,11 +1320,11 @@ ssize_t encodeEXACMessage(const uint16_t actionID, const struct timeval *executi // Fill contents EXACData.actionIDValueID = VALUE_ID_EXAC_ACTION_ID; EXACData.actionIDContentLength = sizeof (EXACData.actionID); - EXACData.actionID = actionID; + EXACData.actionID = actionID == NULL ? ACTION_ID_UNAVAILABLE : *actionID; EXACData.executionTime_qmsoWValueID = VALUE_ID_EXAC_ACTION_EXECUTE_TIME; EXACData.executionTime_qmsoWContentLength = sizeof (EXACData.executionTime_qmsoW); - EXACData.executionTime_qmsoW = TimeGetAsGPSqmsOfWeek(executionTime); + EXACData.executionTime_qmsoW = executionTime == NULL ? GPS_SECOND_OF_WEEK_UNAVAILABLE_VALUE : TimeGetAsGPSqmsOfWeek(executionTime); if (debug) { LogPrint diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 6f97abb95..1c25d5027 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -696,9 +696,9 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { sizeof (objectIPs) / sizeof (objectIPs[0])); if (iIndex != -1) { MessageLength = - encodeACCMMessage(mqACCMData.actionID, mqACCMData.actionType, - mqACCMData.actionTypeParameter1, mqACCMData.actionTypeParameter2, - mqACCMData.actionTypeParameter3, MessageBuffer, + encodeACCMMessage(&mqACCMData.actionID, &mqACCMData.actionType, + &mqACCMData.actionTypeParameter1, &mqACCMData.actionTypeParameter2, + &mqACCMData.actionTypeParameter3, MessageBuffer, sizeof (MessageBuffer), 0); UtilSendTCPData(MODULE_NAME, MessageBuffer, MessageLength, &(socket_fds[iIndex]), 0); } @@ -709,6 +709,16 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { UtilPopulateTRCMDataStructFromMQ(pcRecvBuffer, sizeof (pcRecvBuffer), &mqTRCMData); iIndex = iGetObjectIndexFromObjectIP(mqTRCMData.ip, objectIPs, sizeof (objectIPs) / sizeof (objectIPs[0])); + if (iIndex != -1) { + MessageLength = + encodeTRCMMessage(&mqTRCMData.triggerID, &mqTRCMData.triggerType, + &mqTRCMData.triggerTypeParameter1, &mqTRCMData.triggerTypeParameter2, + &mqTRCMData.triggerTypeParameter3, MessageBuffer, + sizeof (MessageBuffer), 0); + UtilSendTCPData(MODULE_NAME, MessageBuffer, MessageLength, &(socket_fds[iIndex]), 0); + } + else + LogMessage(LOG_LEVEL_WARNING, "Unable to send TRCM: no valid socket found"); } else if (iCommand == COMM_EXAC && vGetState(GSD) == OBC_STATE_RUNNING) { UtilPopulateEXACDataStructFromMQ(pcRecvBuffer, sizeof (pcRecvBuffer), &mqEXACData); @@ -721,7 +731,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { TimeSetToGPStime(&executionTime, TimeGetAsGPSweek(¤tTime), mqEXACData.executionTime_qmsoW); MessageLength = - encodeEXACMessage(mqEXACData.actionID, &executionTime, MessageBuffer, + encodeEXACMessage(&mqEXACData.actionID, &executionTime, MessageBuffer, sizeof (MessageBuffer), 0); UtilSendTCPData(MODULE_NAME, MessageBuffer, MessageLength, &(socket_fds[iIndex]), 0); } From 6a6128d7370169f62d4935d56b5e595eeb472e14 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 28 Jan 2020 11:07:27 +0100 Subject: [PATCH 170/523] Ran code formatter --- server/src/iso22133.c | 24 ++++++++++++++---------- server/src/objectcontrol.c | 9 +++++---- 2 files changed, 19 insertions(+), 14 deletions(-) diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 5ca42bd9b..fef7d9464 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -1142,9 +1142,12 @@ ssize_t encodeTRCMMessage(const uint16_t * triggerID, const TriggerType_t * trig TRCMData.triggerTypeParameter2ContentLength = sizeof (TRCMData.triggerTypeParameter2); TRCMData.triggerTypeParameter3ContentLength = sizeof (TRCMData.triggerTypeParameter3); - TRCMData.triggerTypeParameter1 = param1 == NULL ? TRIGGER_TYPE_PARAMETER_UNAVAILABLE : (uint32_t) (*param1); - TRCMData.triggerTypeParameter2 = param2 == NULL ? TRIGGER_TYPE_PARAMETER_UNAVAILABLE : (uint32_t) (*param2); - TRCMData.triggerTypeParameter3 = param3 == NULL ? TRIGGER_TYPE_PARAMETER_UNAVAILABLE : (uint32_t) (*param3); + TRCMData.triggerTypeParameter1 = + param1 == NULL ? TRIGGER_TYPE_PARAMETER_UNAVAILABLE : (uint32_t) (*param1); + TRCMData.triggerTypeParameter2 = + param2 == NULL ? TRIGGER_TYPE_PARAMETER_UNAVAILABLE : (uint32_t) (*param2); + TRCMData.triggerTypeParameter3 = + param3 == NULL ? TRIGGER_TYPE_PARAMETER_UNAVAILABLE : (uint32_t) (*param3); if (debug) { LogPrint("TRCM message:\n\tTrigger ID value ID: 0x%x\n\tTrigger ID content length: %u\n\t" @@ -1207,10 +1210,10 @@ ssize_t encodeTRCMMessage(const uint16_t * triggerID, const TriggerType_t * trig * \param debug Flag for enabling debugging * \return Number of bytes written or -1 in case of an error */ -ssize_t encodeACCMMessage(const uint16_t *actionID, const ActionType_t *actionType, - const ActionTypeParameter_t *param1, const ActionTypeParameter_t *param2, - const ActionTypeParameter_t *param3, char *accmDataBuffer, const size_t bufferLength, - const char debug) { +ssize_t encodeACCMMessage(const uint16_t * actionID, const ActionType_t * actionType, + const ActionTypeParameter_t * param1, const ActionTypeParameter_t * param2, + const ActionTypeParameter_t * param3, char *accmDataBuffer, + const size_t bufferLength, const char debug) { ACCMType ACCMData; @@ -1301,8 +1304,8 @@ ssize_t encodeACCMMessage(const uint16_t *actionID, const ActionType_t *actionTy * \param debug Flag for enabling debugging * \return Number of bytes written or -1 in case of an error */ -ssize_t encodeEXACMessage(const uint16_t * actionID, const struct timeval *executionTime, char *exacDataBuffer, - 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) { EXACType EXACData; @@ -1324,7 +1327,8 @@ ssize_t encodeEXACMessage(const uint16_t * actionID, const struct timeval *execu EXACData.executionTime_qmsoWValueID = VALUE_ID_EXAC_ACTION_EXECUTE_TIME; EXACData.executionTime_qmsoWContentLength = sizeof (EXACData.executionTime_qmsoW); - EXACData.executionTime_qmsoW = executionTime == NULL ? GPS_SECOND_OF_WEEK_UNAVAILABLE_VALUE : TimeGetAsGPSqmsOfWeek(executionTime); + EXACData.executionTime_qmsoW = + executionTime == NULL ? GPS_SECOND_OF_WEEK_UNAVAILABLE_VALUE : TimeGetAsGPSqmsOfWeek(executionTime); if (debug) { LogPrint diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 1c25d5027..9b64e6d1a 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -711,10 +711,11 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { sizeof (objectIPs) / sizeof (objectIPs[0])); if (iIndex != -1) { MessageLength = - encodeTRCMMessage(&mqTRCMData.triggerID, &mqTRCMData.triggerType, - &mqTRCMData.triggerTypeParameter1, &mqTRCMData.triggerTypeParameter2, - &mqTRCMData.triggerTypeParameter3, MessageBuffer, - sizeof (MessageBuffer), 0); + encodeTRCMMessage(&mqTRCMData.triggerID, &mqTRCMData.triggerType, + &mqTRCMData.triggerTypeParameter1, + &mqTRCMData.triggerTypeParameter2, + &mqTRCMData.triggerTypeParameter3, MessageBuffer, + sizeof (MessageBuffer), 0); UtilSendTCPData(MODULE_NAME, MessageBuffer, MessageLength, &(socket_fds[iIndex]), 0); } else From cd733ec477ff64c5b4cf1286186df68c3b644957 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 28 Jan 2020 11:27:59 +0100 Subject: [PATCH 171/523] Redefined byte swapper functions --- server/src/iso22133.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 5bf2b7b7e..4ccb86e9f 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -10,11 +10,11 @@ static const uint8_t SupportedProtocolVersions[] = { 2 }; // ************************* Byte swapper definitions for 6 byte values #if __BYTE_ORDER == __LITTLE_ENDIAN -#define le48toh(x) __uint64_identity (x) -#define htole48(x) __uint64_identity (x) +#define le48toh(x) (x) +#define htole48(x) (x) #else -#define le48toh(x) (__bswap_64(x) >> 16) -#define htole48(x) (__bswap_64(x) >> 16) +#define le48toh(x) (le64toh(x) >> 16) +#define htole48(x) (htole64(x) >> 16) #endif // ************************** static function declarations From 2f273d56895899fb38b22b7a414a8e964b8f1fb2 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 28 Jan 2020 11:38:15 +0100 Subject: [PATCH 172/523] Added function description --- server/src/iso22133.c | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/server/src/iso22133.c b/server/src/iso22133.c index d387b8762..0c0601050 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -1357,7 +1357,15 @@ ssize_t encodeEXACMessage(const uint16_t * actionID, const struct timeval *execu return sizeof (EXACType); } - +/*! + * \brief encodeINSUPMessage Fills an ISO vendor specific (RISE) INSUP struct with relevant data fields, + * and corresponding value IDs and content lengths + * \param command Command to send to supervisor + * \param insupDataBuffer Data buffer to which INSUP is to be written + * \param bufferLength Length of data buffer to which INSUP is to be written + * \param debug Flag for enabling debugging + * \return Number of bytes written to buffer, or -1 in case of error + */ ssize_t encodeINSUPMessage(const SupervisorCommandType command, char *insupDataBuffer, const size_t bufferLength, const char debug) { INSUPType INSUPData; From b0e89914f612e03cadbbde6407d0897e586354d6 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 28 Jan 2020 11:45:07 +0100 Subject: [PATCH 173/523] Added debug printout for INSUP --- server/src/iso22133.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 0c0601050..a7f79ef53 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -1387,7 +1387,9 @@ ssize_t encodeINSUPMessage(const SupervisorCommandType command, char *insupDataB INSUPData.mode = (uint8_t) command; if (debug) { - + LogPrint("INSUP message:\n\tMode value ID: 0x%x\n\t" + "Mode content length: %u\n\tMode: %u", INSUPData.modeValueID, + INSUPData.modeContentLength, INSUPData.mode); } // Switch from host endianness to little endian From aacf95e05c38ed325f4de6cbf80b8e111cdfcea6 Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Tue, 28 Jan 2020 12:52:23 +0100 Subject: [PATCH 174/523] Changed id to transmitterID and numbrofobjects from u8 to u32 --- server/inc/datadictionary.h | 8 ++++---- server/src/datadictionary.c | 12 ++++++------ 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/server/inc/datadictionary.h b/server/inc/datadictionary.h index f25cc3cc4..914cd3c37 100644 --- a/server/inc/datadictionary.h +++ b/server/inc/datadictionary.h @@ -123,11 +123,11 @@ OBCState_t DataDictionaryGetOBCStateU8(GSDType *GSD); ReadWriteAccess_t DataDictionaryFreeMONR(GSDType *GSD); ReadWriteAccess_t DataDictionaryInitMONR(GSDType *GSD); -ReadWriteAccess_t DataDictionarySetMONR(GSDType *GSD, MONRType * MONR, U8 index); -ReadWriteAccess_t DataDictionaryGetMONR(GSDType *GSD, MONRType * MONR, U8 index); +ReadWriteAccess_t DataDictionarySetMONR(GSDType *GSD, MONRType * MONR, U32 TransmitterId); +ReadWriteAccess_t DataDictionaryGetMONR(GSDType *GSD, MONRType * MONR, U32 TransmitterId); -ReadWriteAccess_t DataDictionarySetNumberOfObjectsU8(GSDType * GSD, U8 *NumberOfObjects); -ReadWriteAccess_t DataDictionaryGetNumberOfObjectsU8(GSDType * GSD, U8 *NumberOfObjects); +ReadWriteAccess_t DataDictionarySetNumberOfObjectsU8(GSDType * GSD, U32 *numberOfObjects); +ReadWriteAccess_t DataDictionaryGetNumberOfObjectsU8(GSDType * GSD, U32 *numberOfObjects); ReadWriteAccess_t DataDictionaryConstructor(GSDType *GSD); diff --git a/server/src/datadictionary.c b/server/src/datadictionary.c index 0dbf7f98f..f3b54accc 100644 --- a/server/src/datadictionary.c +++ b/server/src/datadictionary.c @@ -1718,12 +1718,12 @@ ReadWriteAccess_t DataDictionaryInitMONR(GSDType * GSD) { * \param id object id * \return Result according to ::ReadWriteAccess_t */ -ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, MONRType * MONR, U8 id) { +ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, MONRType * MONR, U32 TransmitterId) { ReadWriteAccess_t Res; Res = WRITE_OK; pthread_mutex_lock(&MONRMutex); - GSD->MonrMessages[id] = *MONR; + GSD->MonrMessages[TransmitterId] = *MONR; pthread_mutex_unlock(&MONRMutex); return Res; } @@ -1735,9 +1735,9 @@ ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, MONRType * MONR, U8 id) { * \param id requesed object id * \return Result according to ::ReadWriteAccess_t */ -ReadWriteAccess_t DataDictionaryGetMONR(GSDType * GSD, MONRType * MONR, U8 id) { +ReadWriteAccess_t DataDictionaryGetMONR(GSDType * GSD, MONRType * MONR, U32 TransmitterId) { pthread_mutex_lock(&MONRMutex); - *MONR = GSD->MonrMessages[id]; + *MONR = GSD->MonrMessages[TransmitterId]; pthread_mutex_unlock(&MONRMutex); return READ_OK; } @@ -1769,7 +1769,7 @@ ReadWriteAccess_t DataDictionaryFreeMONR(GSDType * GSD) { * \param OBCState * \return Result according to ::ReadWriteAccess_t */ -ReadWriteAccess_t DataDictionarySetNumberOfObjectsU8(GSDType * GSD, U8 * numberOfObjects) { +ReadWriteAccess_t DataDictionarySetNumberOfObjectsU8(GSDType * GSD, U32 *numberOfObjects) { ReadWriteAccess_t Res; Res = WRITE_OK; @@ -1784,7 +1784,7 @@ ReadWriteAccess_t DataDictionarySetNumberOfObjectsU8(GSDType * GSD, U8 * numberO * \param GSD Pointer to shared allocated memory * \return Current object control state according to ::OBCState_t */ -ReadWriteAccess_t DataDictionaryGetNumberOfObjectsU8(GSDType * GSD, U8 * numberOfObjects) { +ReadWriteAccess_t DataDictionaryGetNumberOfObjectsU8(GSDType * GSD, U32 *numberOfObjects) { pthread_mutex_lock(&numberOfObjectsMutex); *numberOfObjects = GSD->numberOfObjects; pthread_mutex_unlock(&numberOfObjectsMutex); From de21452c315a1bc7b6670c4d0c6efc9150164e7f Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Tue, 28 Jan 2020 12:59:32 +0100 Subject: [PATCH 175/523] Added setting number of objects to 0 on free --- server/src/datadictionary.c | 1 + 1 file changed, 1 insertion(+) diff --git a/server/src/datadictionary.c b/server/src/datadictionary.c index f3b54accc..cc789478c 100644 --- a/server/src/datadictionary.c +++ b/server/src/datadictionary.c @@ -1755,6 +1755,7 @@ ReadWriteAccess_t DataDictionaryFreeMONR(GSDType * GSD) { pthread_mutex_lock(&MONRMutex); munmap(GSD->MonrMessages, sizeof (MONRType)); free(GSD->MonrMessages); + DataDictionarySetNumberOfObjectsU8(GSD, 0); pthread_mutex_unlock(&MONRMutex); return Res; } From b2c5d0b67d0e3fe8f7139c13be57e353beb65a9f Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Tue, 28 Jan 2020 13:07:20 +0100 Subject: [PATCH 176/523] Removed unused leftovers --- server/src/systemcontrol.c | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index 78d5ef2aa..07d3e8103 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -571,11 +571,7 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { SystemControlBuildRVSSMONRChannelMessage(RVSSData, &RVSSMessageLengthU32, monrData, 0); UtilSendUDPData("SystemControl", &RVSSChannelSocket, &RVSSChannelAddr, RVSSData, RVSSMessageLengthU32, 0); - } - - SystemControlBuildRVSSMONRChannelMessage(RVSSData, &RVSSMessageLengthU32, monrData, 0); - UtilSendUDPData("SystemControl", &RVSSChannelSocket, &RVSSChannelAddr, RVSSData, - RVSSMessageLengthU32, 0); + } break; case COMM_INV: break; From adca2acb49113b0a762aed97ec05c860e38f62ab Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Tue, 28 Jan 2020 13:18:02 +0100 Subject: [PATCH 177/523] Added null checks and Id checks --- server/src/datadictionary.c | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/server/src/datadictionary.c b/server/src/datadictionary.c index cc789478c..68c7d02ce 100644 --- a/server/src/datadictionary.c +++ b/server/src/datadictionary.c @@ -1723,8 +1723,10 @@ ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, MONRType * MONR, U32 Tran Res = WRITE_OK; pthread_mutex_lock(&MONRMutex); - GSD->MonrMessages[TransmitterId] = *MONR; - pthread_mutex_unlock(&MONRMutex); + if(GSD->MonrMessages != NULL && TransmitterId < GSD->numberOfObjects){ + GSD->MonrMessages[TransmitterId] = *MONR; + } + pthread_mutex_unlock(&MONRMutex); return Res; } @@ -1737,7 +1739,9 @@ ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, MONRType * MONR, U32 Tran */ ReadWriteAccess_t DataDictionaryGetMONR(GSDType * GSD, MONRType * MONR, U32 TransmitterId) { pthread_mutex_lock(&MONRMutex); - *MONR = GSD->MonrMessages[TransmitterId]; + if(GSD->MonrMessages != NULL && TransmitterId < GSD->numberOfObjects){ + *MONR = GSD->MonrMessages[TransmitterId]; + } pthread_mutex_unlock(&MONRMutex); return READ_OK; } From fedf4525631460e8dd78d024211dceccc5128ec4 Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Tue, 28 Jan 2020 13:26:56 +0100 Subject: [PATCH 178/523] Added const for some parameters --- server/inc/datadictionary.h | 4 ++-- server/src/datadictionary.c | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/server/inc/datadictionary.h b/server/inc/datadictionary.h index 914cd3c37..3321b640a 100644 --- a/server/inc/datadictionary.h +++ b/server/inc/datadictionary.h @@ -123,8 +123,8 @@ OBCState_t DataDictionaryGetOBCStateU8(GSDType *GSD); ReadWriteAccess_t DataDictionaryFreeMONR(GSDType *GSD); ReadWriteAccess_t DataDictionaryInitMONR(GSDType *GSD); -ReadWriteAccess_t DataDictionarySetMONR(GSDType *GSD, MONRType * MONR, U32 TransmitterId); -ReadWriteAccess_t DataDictionaryGetMONR(GSDType *GSD, MONRType * MONR, U32 TransmitterId); +ReadWriteAccess_t DataDictionarySetMONR(GSDType *GSD, const MONRType * MONR, const U32 TransmitterId); +ReadWriteAccess_t DataDictionaryGetMONR(GSDType *GSD, MONRType * MONR, const U32 TransmitterId); ReadWriteAccess_t DataDictionarySetNumberOfObjectsU8(GSDType * GSD, U32 *numberOfObjects); ReadWriteAccess_t DataDictionaryGetNumberOfObjectsU8(GSDType * GSD, U32 *numberOfObjects); diff --git a/server/src/datadictionary.c b/server/src/datadictionary.c index 68c7d02ce..2f1863514 100644 --- a/server/src/datadictionary.c +++ b/server/src/datadictionary.c @@ -1718,7 +1718,7 @@ ReadWriteAccess_t DataDictionaryInitMONR(GSDType * GSD) { * \param id object id * \return Result according to ::ReadWriteAccess_t */ -ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, MONRType * MONR, U32 TransmitterId) { +ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, const MONRType * MONR, const U32 TransmitterId) { ReadWriteAccess_t Res; Res = WRITE_OK; @@ -1737,7 +1737,7 @@ ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, MONRType * MONR, U32 Tran * \param id requesed object id * \return Result according to ::ReadWriteAccess_t */ -ReadWriteAccess_t DataDictionaryGetMONR(GSDType * GSD, MONRType * MONR, U32 TransmitterId) { +ReadWriteAccess_t DataDictionaryGetMONR(GSDType * GSD, MONRType * MONR, const U32 TransmitterId) { pthread_mutex_lock(&MONRMutex); if(GSD->MonrMessages != NULL && TransmitterId < GSD->numberOfObjects){ *MONR = GSD->MonrMessages[TransmitterId]; From ef567c75184abe417bf77a3aa027d0748ad303b0 Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Tue, 28 Jan 2020 13:33:04 +0100 Subject: [PATCH 179/523] Added check for munmap --- server/src/datadictionary.c | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/server/src/datadictionary.c b/server/src/datadictionary.c index 2f1863514..120de33e1 100644 --- a/server/src/datadictionary.c +++ b/server/src/datadictionary.c @@ -1757,8 +1757,11 @@ ReadWriteAccess_t DataDictionaryFreeMONR(GSDType * GSD) { Res = WRITE_OK; pthread_mutex_lock(&MONRMutex); - munmap(GSD->MonrMessages, sizeof (MONRType)); - free(GSD->MonrMessages); + int res= munmap(GSD->MonrMessages, sizeof (MONRType)); + if(res < 0){ + util_error("Unable to unmap monrMessages file!"); + } + free(GSD->MonrMessages); DataDictionarySetNumberOfObjectsU8(GSD, 0); pthread_mutex_unlock(&MONRMutex); return Res; From 11330052215a5a8874135996130520f89b0b421c Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Tue, 28 Jan 2020 13:34:15 +0100 Subject: [PATCH 180/523] Changed name of memory file --- server/src/datadictionary.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/server/src/datadictionary.c b/server/src/datadictionary.c index 120de33e1..ad02c2f33 100644 --- a/server/src/datadictionary.c +++ b/server/src/datadictionary.c @@ -1685,7 +1685,7 @@ ReadWriteAccess_t DataDictionaryInitMONR(GSDType * GSD) { struct stat st; pthread_mutex_lock(&MONRMutex); - sprintf(filePath, "%stest%d.mem", SHARED_MEMORY_PATH, 5); + sprintf(filePath, "%smonrMessageMemory%d.mem", SHARED_MEMORY_PATH, 5); fd = open(filePath, O_RDWR | O_CREAT, S_IRUSR | S_IWUSR); stat(filePath, &st); From 8a45bee100e974092bd944636916b2466f13be15 Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Tue, 28 Jan 2020 13:38:54 +0100 Subject: [PATCH 181/523] added close --- server/src/datadictionary.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/server/src/datadictionary.c b/server/src/datadictionary.c index ad02c2f33..c4dbf33ca 100644 --- a/server/src/datadictionary.c +++ b/server/src/datadictionary.c @@ -1705,7 +1705,7 @@ ReadWriteAccess_t DataDictionaryInitMONR(GSDType * GSD) { LogPrint("Initing"); - + close(fd); pthread_mutex_unlock(&MONRMutex); return Res; From 2e4bbe821fd4a47dbb8f8d80608fafa5333f16d3 Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Tue, 28 Jan 2020 14:26:27 +0100 Subject: [PATCH 182/523] Added close and logPrint after init --- server/src/datadictionary.c | 12 +- server/src/objectcontrol.c | 5520 ++++++++++++++++++----------------- 2 files changed, 2765 insertions(+), 2767 deletions(-) diff --git a/server/src/datadictionary.c b/server/src/datadictionary.c index c4dbf33ca..7efec5a09 100644 --- a/server/src/datadictionary.c +++ b/server/src/datadictionary.c @@ -1698,15 +1698,11 @@ ReadWriteAccess_t DataDictionaryInitMONR(GSDType * GSD) { //printf("File size after writing: %ld\n", st.st_size); // Map memory to created file - // int objectCount = GSD->numberOfObjects; - - GSD->MonrMessages = (MONRType *) mmap(NULL, (sizeof (MONRType)), PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0); - - - LogPrint("Initing"); close(fd); + LogMessage(LOG_LEVEL_INFO, "Init MonrMessage memory"); + pthread_mutex_unlock(&MONRMutex); return Res; } @@ -1720,13 +1716,13 @@ ReadWriteAccess_t DataDictionaryInitMONR(GSDType * GSD) { */ ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, const MONRType * MONR, const U32 TransmitterId) { ReadWriteAccess_t Res; - Res = WRITE_OK; pthread_mutex_lock(&MONRMutex); if(GSD->MonrMessages != NULL && TransmitterId < GSD->numberOfObjects){ - GSD->MonrMessages[TransmitterId] = *MONR; + GSD->MonrMessages[0] = *MONR; } pthread_mutex_unlock(&MONRMutex); + return Res; } diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 9b8ae9d6a..b017ce4ba 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -134,16 +134,16 @@ typedef enum { - COMMAND_HEARTBEAT_GO, - COMMAND_HEARTBEAT_ABORT + COMMAND_HEARTBEAT_GO, + COMMAND_HEARTBEAT_ABORT } hearbeatCommand_t; typedef enum { - TRANSITION_RESULT_UNDEFINED, - TRANSITION_OK, - TRANSITION_INVALID, - TRANSITION_MEMORY_ERROR + TRANSITION_RESULT_UNDEFINED, + TRANSITION_OK, + TRANSITION_INVALID, + TRANSITION_MEMORY_ERROR } StateTransitionResult; /* Small note: syntax for declaring a function pointer is (example for a function taking an int and a float, @@ -156,8 +156,9 @@ typedef enum { */ typedef StateTransitionResult(*StateTransition) (OBCState_t * currentState, OBCState_t requestedState); + C8 TrajBuffer[COMMAND_DOTM_ROWS_IN_TRANSMISSION * COMMAND_DOTM_ROW_MESSAGE_LENGTH + - COMMAND_MESSAGE_HEADER_LENGTH + COMMAND_TRAJ_INFO_ROW_MESSAGE_LENGTH]; + COMMAND_MESSAGE_HEADER_LENGTH + COMMAND_TRAJ_INFO_ROW_MESSAGE_LENGTH]; /*------------------------------------------------------------ @@ -168,40 +169,40 @@ static void vDisconnectObject(int *sockfd); static I32 vCheckRemoteDisconnected(int *sockfd); static void vCreateSafetyChannel(const char *name, const uint32_t port, int *sockfd, - struct sockaddr_in *addr); + struct sockaddr_in *addr); static void vCloseSafetyChannel(int *sockfd); I32 ObjectControlBuildOSEMMessage(C8 * MessageBuffer, OSEMType * OSEMData, TimeType * GPSTime, C8 * Latitude, - C8 * Longitude, C8 * Altitude, U8 debug); + C8 * Longitude, C8 * Altitude, U8 debug); static size_t uiRecvMonitor(int *sockfd, char *buffer, size_t length); static int iGetObjectIndexFromObjectIP(in_addr_t ipAddr, in_addr_t objectIPs[], unsigned int numberOfObjects); static void signalHandler(int signo); I32 ObjectControlBuildSTRTMessage(C8 * MessageBuffer, STRTType * STRTData, TimeType * GPSTime, - U32 ScenarioStartTime, U32 DelayStart, U32 * OutgoingStartTime, U8 debug); + U32 ScenarioStartTime, U32 DelayStart, U32 * OutgoingStartTime, U8 debug); I32 ObjectControlBuildOSTMMessage(C8 * MessageBuffer, OSTMType * OSTMData, C8 CommandOption, U8 debug); I32 ObjectControlBuildHEABMessage(C8 * MessageBuffer, HEABType * HEABData, TimeType * GPSTime, U8 CCStatus, - U8 debug); + U8 debug); int ObjectControlBuildLLCMMessage(char *MessageBuffer, unsigned short Speed, unsigned short Curvature, - unsigned char Mode, char debug); + unsigned char Mode, char debug); I32 ObjectControlBuildSYPMMessage(C8 * MessageBuffer, SYPMType * SYPMData, U32 SyncPoint, U32 StopTime, - U8 debug); + U8 debug); I32 ObjectControlBuildMTSPMessage(C8 * MessageBuffer, MTSPType * MTSPData, U32 SyncTimestamp, U8 debug); I32 ObjectControlBuildTRAJMessageHeader(C8 * MessageBuffer, I32 * RowCount, HeaderType * HeaderData, - TRAJInfoType * TRAJInfoData, C8 * TrajFileHeader, U8 debug); + TRAJInfoType * TRAJInfoData, C8 * TrajFileHeader, U8 debug); I32 ObjectControlBuildTRAJMessage(C8 * MessageBuffer, FILE * fd, I32 RowCount, DOTMType * DOTMData, U8 debug); I32 ObjectControlSendTRAJMessage(C8 * Filename, I32 * Socket, I32 RowCount, C8 * IP, U32 Port, - DOTMType * DOTMData, U8 debug); + DOTMType * DOTMData, U8 debug); int ObjectControlSendUDPData(int *sockfd, struct sockaddr_in *addr, char *SendData, int Length, char debug); I32 ObjectControlMONRToASCII(MONRType * MONRData, GeoPosition * OriginPosition, I32 Idn, C8 * Id, - C8 * Timestamp, C8 * XPosition, C8 * YPosition, C8 * ZPosition, - C8 * LongitudinalSpeed, C8 * LateralSpeed, C8 * LongitudinalAcc, C8 * LateralAcc, - C8 * Heading, C8 * DriveDirection, C8 * ObjectState, C8 * ReadyToArm, - C8 * ErrorStatus, C8 debug); -I32 ObjectControlBuildMONRMessage(C8 * MonrData, const size_t length, MONRType * MONRData, U8 debug); + C8 * Timestamp, C8 * XPosition, C8 * YPosition, C8 * ZPosition, + C8 * LongitudinalSpeed, C8 * LateralSpeed, C8 * LongitudinalAcc, C8 * LateralAcc, + C8 * Heading, C8 * DriveDirection, C8 * ObjectState, C8 * ReadyToArm, + C8 * ErrorStatus, C8 debug); +I32 ObjectControlBuildMONRMessage(C8 * MonrData, MONRType * MONRData, U8 debug); I32 ObjectControlBuildVOILMessage(C8 * MessageBuffer, VOILType * VOILData, C8 * SimData, U8 debug); I32 ObjectControlSendDTMMessage(C8 * DTMData, I32 * Socket, I32 RowCount, C8 * IP, U32 Port, - DOTMType * DOTMData, U8 debug); + DOTMType * DOTMData, U8 debug); I32 ObjectControlBuildDTMMessage(C8 * MessageBuffer, C8 * DTMData, I32 RowCount, DOTMType * DOTMData, - U8 debug); + U8 debug); I32 ObjectControlBuildASPMessage(C8 * MessageBuffer, ASPType * ASPData, U8 debug); I32 ObjectControlBuildACCMMessage(ACCMData * mqACCMData, ACCMType * ACCM, U8 debug); I32 ObjectControlBuildEXACMessage(EXACData * mqEXACData, EXACType * EXAC, U8 debug); @@ -211,8 +212,8 @@ I32 ObjectControlSendTRCMMessage(TRCMData * TRCM, I32 * socket, U8 debug); I32 ObjectControlSendEXACMessage(EXACData * EXAC, I32 * socket, U8 debug); static int iFindObjectsInfo(C8 object_traj_file[MAX_OBJECTS][MAX_FILE_PATH], - C8 object_address_name[MAX_OBJECTS][MAX_FILE_PATH], - in_addr_t objectIPs[MAX_OBJECTS], I32 * nbr_objects); + C8 object_address_name[MAX_OBJECTS][MAX_FILE_PATH], + in_addr_t objectIPs[MAX_OBJECTS], I32 * nbr_objects); OBCState_t vInitializeState(OBCState_t firstState, GSDType * GSD); inline OBCState_t vGetState(GSDType * GSD); @@ -238,262 +239,255 @@ static volatile int iExit = 0; ------------------------------------------------------------*/ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { - I32 safety_socket_fd[MAX_OBJECTS]; - struct sockaddr_in safety_object_addr[MAX_OBJECTS]; - I32 socket_fds[MAX_OBJECTS]; - I32 socket_fd = 0; - C8 object_traj_file[MAX_OBJECTS][MAX_FILE_PATH]; - C8 object_address_name[MAX_OBJECTS][MAX_FILE_PATH]; - in_addr_t objectIPs[MAX_OBJECTS]; - U32 object_udp_port[MAX_OBJECTS]; - U32 object_tcp_port[MAX_OBJECTS]; - I32 nbr_objects = 0; - enum COMMAND iCommand; - U8 pcRecvBuffer[RECV_MESSAGE_BUFFER]; - C8 pcTempBuffer[512]; - C8 MessageBuffer[BUFFER_SIZE_3100]; - I32 iIndex = 0, i = 0; - struct timespec sleep_time, ref_time; - - /*! Timers for reporting state over message bus */ - const struct timespec mqEmptyPollPeriod = { OC_SLEEP_TIME_EMPTY_MQ_S, OC_SLEEP_TIME_EMPTY_MQ_NS }; - const struct timespec mqNonEmptyPollPeriod = - { OC_SLEEP_TIME_NONEMPTY_MQ_S, OC_SLEEP_TIME_NONEMPTY_MQ_NS }; - const struct timeval stateReportPeriod = { OC_STATE_REPORT_PERIOD_S, OC_STATE_REPORT_PERIOD_US }; - struct timeval currentTime, nextStateReportTime; - U8 iForceObjectToLocalhostU8 = 0; - - FILE *fd; - C8 Id[SMALL_BUFFER_SIZE_0]; - C8 GPSWeek[SMALL_BUFFER_SIZE_0], Timestamp[SMALL_BUFFER_SIZE_0], XPosition[SMALL_BUFFER_SIZE_0], - YPosition[SMALL_BUFFER_SIZE_0], ZPosition[SMALL_BUFFER_SIZE_0]; - C8 Speed[SMALL_BUFFER_SIZE_0], LongitudinalSpeed[SMALL_BUFFER_SIZE_0], LateralSpeed[SMALL_BUFFER_SIZE_0], - LongitudinalAcc[SMALL_BUFFER_SIZE_0], LateralAcc[SMALL_BUFFER_SIZE_0]; - C8 Heading[SMALL_BUFFER_SIZE_0], DriveDirection[SMALL_BUFFER_SIZE_1], ObjectState[SMALL_BUFFER_SIZE_1], - ReadyToArm[SMALL_BUFFER_SIZE_1], MTSP[SMALL_BUFFER_SIZE_0], ErrorStatus[SMALL_BUFFER_SIZE_0]; - I32 MessageLength; - C8 *MiscPtr; - C8 MiscText[SMALL_BUFFER_SIZE_0]; - U32 StartTimeU32 = 0; - U32 OutgoingStartTimeU32 = 0; - U32 DelayedStartU32 = 0; - U32 CurrentTimeU32 = 0; - U32 OldTimeU32 = 0; - U64 TimeCap1, TimeCap2; - struct timeval CurrentTimeStruct; - I32 HeartbeatMessageCounter = 0; - - ObjectPosition OP[MAX_OBJECTS]; - flt SpaceArr[MAX_OBJECTS][TRAJECTORY_FILE_MAX_ROWS]; - flt TimeArr[MAX_OBJECTS][TRAJECTORY_FILE_MAX_ROWS]; - SpaceTime SpaceTimeArr[MAX_OBJECTS][TRAJECTORY_FILE_MAX_ROWS]; - C8 OriginLatitude[SMALL_BUFFER_SIZE_0], OriginLongitude[SMALL_BUFFER_SIZE_0], - OriginAltitude[SMALL_BUFFER_SIZE_0], OriginHeading[SMALL_BUFFER_SIZE_0]; - C8 TextBuffer[SMALL_BUFFER_SIZE_0]; - dbl OriginLatitudeDbl; - dbl OriginLongitudeDbl; - dbl OriginAltitudeDbl; - dbl OriginHeadingDbl; - C8 pcSendBuffer[MBUS_MAX_DATALEN]; - C8 ObjectPort[SMALL_BUFFER_SIZE_0]; - HeaderType HeaderData; - OSEMType OSEMData; - STRTType STRTData; - OSTMType OSTMData; - HEABType HEABData; - MONRType MONRData; - DOTMType DOTMData; - TRAJInfoType TRAJInfoData; - VOILType VOILData; - SYPMType SYPMData; - MTSPType MTSPData; - ACCMData mqACCMData; - EXACData mqEXACData; - GeoPosition OriginPosition; - ASPType ASPData; - - ASPData.MTSPU32 = 0; - ASPData.TimeToSyncPointDbl = 0; - ASPData.PrevTimeToSyncPointDbl = 0; - ASPData.CurrentTimeDbl = 0; - AdaptiveSyncPoint ASP[MAX_ADAPTIVE_SYNC_POINTS]; - I32 SyncPointCount = 0; - I32 SearchStartIndex = 0; - dbl ASPMaxTimeDiffDbl = 0; - dbl ASPMaxTrajDiffDbl = 0; - dbl ASPFilterLevelDbl = 0; - dbl ASPMaxDeltaTimeDbl = 0; - I32 ASPDebugRate = 1; - I32 ASPStepBackCount = 0; - char confDirectoryPath[MAX_FILE_PATH]; - - U8 ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_BOOTING; - - vInitializeState(OBC_STATE_IDLE, GSD); - U8 uiTimeCycle = 0; - I32 ObjectcontrolExecutionMode = OBJECT_CONTROL_CONTROL_MODE; - - C8 Buffer2[SMALL_BUFFER_SIZE_1]; - C8 LogBuffer[LOG_BUFFER_LENGTH]; - C8 VOILReceivers[SMALL_BUFFER_SIZE_254]; - C8 DTMReceivers[SMALL_BUFFER_SIZE_254]; - U32 RowCount; - U32 DTMIpU32; - U32 DTMLengthU32; - - U8 DisconnectU8 = 0; - I32 iResult; - - FILE *TempFd; - U16 MiscU16; - I32 j = 0; - - U8 STRTSentU8 = 0; - C8 FileHeaderBufferC8[TRAJ_FILE_HEADER_ROW]; - - - // Create log - LogInit(MODULE_NAME, logLevel); - LogMessage(LOG_LEVEL_INFO, "Object control task running with PID: %i", getpid()); - - - // Set up signal handlers - if (signal(SIGINT, signalHandler) == SIG_ERR) - util_error("Unable to initialize signal handler"); - - // Set up message bus connection - if (iCommInit()) - util_error("Unable to connect to message queue bus"); - - // Initialize timer for sending state - TimeSetToCurrentSystemTime(¤tTime); - nextStateReportTime = currentTime; - - // Initialize timer for sending state - TimeSetToCurrentSystemTime(¤tTime); - nextStateReportTime = currentTime; - - while (!iExit) { - - if (vGetState(GSD) == OBC_STATE_ERROR) { - ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_ABORT; - MessageLength = - ObjectControlBuildHEABMessage(MessageBuffer, &HEABData, GPSTime, ObjectControlServerStatus, - 0); - UtilSendUDPData("Object Control", &safety_socket_fd[iIndex], &safety_object_addr[iIndex], - MessageBuffer, MessageLength, 0); - } - - if (vGetState(GSD) == OBC_STATE_RUNNING || vGetState(GSD) == OBC_STATE_ARMED - || vGetState(GSD) == OBC_STATE_CONNECTED) { - /*HEAB*/ for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - if (uiTimeCycle == 0) { - //HeartbeatMessageCounter ++; - MessageLength = - ObjectControlBuildHEABMessage(MessageBuffer, &HEABData, GPSTime, - ObjectControlServerStatus, 0); - //ObjectControlSendUDPData(&safety_socket_fd[iIndex], &safety_object_addr[iIndex], MessageBuffer, MessageLength, 1); - UtilSendUDPData("Object Control", &safety_socket_fd[iIndex], &safety_object_addr[iIndex], - MessageBuffer, MessageLength, 0); - } - } - - - // Check if any object has disconnected - if so, disconnect all objects and return to idle - DisconnectU8 = 0; - for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - DisconnectU8 |= vCheckRemoteDisconnected(&socket_fds[iIndex]); - if (DisconnectU8) { - LogMessage(LOG_LEVEL_WARNING, "Lost connection to IP %s - returning to IDLE", - object_address_name[iIndex]); - - for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - vDisconnectObject(&socket_fds[iIndex]); - } - - /* Close safety socket */ - for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - vCloseSafetyChannel(&safety_socket_fd[iIndex]); - } - vSetState(OBC_STATE_IDLE, GSD); - break; - } - } - } - - if (vGetState(GSD) == OBC_STATE_RUNNING || vGetState(GSD) == OBC_STATE_CONNECTED - || vGetState(GSD) == OBC_STATE_ARMED) { - char buffer[RECV_MESSAGE_BUFFER]; - size_t receivedMONRData = 0; - - // this is etsi time lets remov it ans use utc instead - //gettimeofday(&CurrentTimeStruct, NULL); - - CurrentTimeU32 = - ((GPSTime->GPSSecondsOfWeekU32 * 1000 + (U32) TimeControlGetMillisecond(GPSTime)) << 2) + - GPSTime->MicroSecondU16; - - - /*MTSP*/ if (HeartbeatMessageCounter == 0) { - HeartbeatMessageCounter = 0; - for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - for (i = 0; i < SyncPointCount; i++) { - if (TEST_SYNC_POINTS == 0 - && strstr(object_address_name[iIndex], ASP[i].SlaveIP) != NULL - && ASPData.MTSPU32 > 0 && ASPData.TimeToSyncPointDbl > -1) { - /*Send Master time to adaptive sync point */ - MessageLength = - ObjectControlBuildMTSPMessage(MessageBuffer, &MTSPData, ASPData.MTSPU32, 0); - //ObjectControlSendUDPData(&safety_socket_fd[iIndex], &safety_object_addr[iIndex], MessageBuffer, MessageLength, 0); - UtilSendUDPData("Object Control", &safety_socket_fd[iIndex], - &safety_object_addr[iIndex], MessageBuffer, MessageLength, 0); - } - /*else if(TEST_SYNC_POINTS == 1 && iIndex == 1 && ASPData.MTSPU32 > 0 && ASPData.TimeToSyncPointDbl > -1 ) - { - Send Master time to adaptive sync point - MessageLength =ObjectControlBuildMTSPMessage(MessageBuffer, &MTSPData, (U32)ASPData.MTSPU32, 0); - ObjectControlSendUDPData(&safety_socket_fd[iIndex], &safety_object_addr[iIndex], MessageBuffer, MessageLength, 0); - } */ - } - } - } - - - for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - memset(buffer, 0, sizeof (buffer)); - receivedMONRData = uiRecvMonitor(&safety_socket_fd[iIndex], buffer, sizeof (buffer)); - - if (receivedMONRData > 0) { - LogMessage(LOG_LEVEL_DEBUG, "Recieved new data from %s %d %d: %s", - object_address_name[iIndex], object_udp_port[iIndex], receivedMONRData, - buffer); - - if (ObjectControlBuildMONRMessage(buffer, receivedMONRData, &MONRData, 0) == -1) { - LogMessage(LOG_LEVEL_INFO, "Error decoding MONR from %s: disconnecting object", - object_address_name[iIndex]); - vDisconnectObject(&safety_socket_fd[iIndex]); - // TODO smarter way of handling? - continue; - } - - if (ObjectcontrolExecutionMode == OBJECT_CONTROL_CONTROL_MODE) { - // Append IP to buffer - memcpy(&buffer[receivedMONRData], &safety_object_addr[iIndex].sin_addr.s_addr, - sizeof (in_addr_t)); - // Send MONR message as bytes - - if (iCommSend(COMM_MONR, buffer, (size_t) (receivedMONRData) + sizeof (in_addr_t)) < - 0) { - LogMessage(LOG_LEVEL_ERROR, - "Fatal communication fault when sending MONR command - entering error state"); - vSetState(OBC_STATE_ERROR, GSD); - ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_ABORT; - } - } - - - //Store MONR in GSD - DataDictionarySetMONR(GSD, &MONRData, iIndex); + I32 safety_socket_fd[MAX_OBJECTS]; + struct sockaddr_in safety_object_addr[MAX_OBJECTS]; + I32 socket_fds[MAX_OBJECTS]; + I32 socket_fd = 0; + C8 object_traj_file[MAX_OBJECTS][MAX_FILE_PATH]; + C8 object_address_name[MAX_OBJECTS][MAX_FILE_PATH]; + in_addr_t objectIPs[MAX_OBJECTS]; + U32 object_udp_port[MAX_OBJECTS]; + U32 object_tcp_port[MAX_OBJECTS]; + I32 nbr_objects = 0; + enum COMMAND iCommand; + U8 pcRecvBuffer[RECV_MESSAGE_BUFFER]; + C8 pcTempBuffer[512]; + C8 MessageBuffer[BUFFER_SIZE_3100]; + I32 iIndex = 0, i = 0; + struct timespec sleep_time, ref_time; + + /*! Timers for reporting state over message bus */ + const struct timespec mqEmptyPollPeriod = { OC_SLEEP_TIME_EMPTY_MQ_S, OC_SLEEP_TIME_EMPTY_MQ_NS }; + const struct timespec mqNonEmptyPollPeriod = + { OC_SLEEP_TIME_NONEMPTY_MQ_S, OC_SLEEP_TIME_NONEMPTY_MQ_NS }; + const struct timeval stateReportPeriod = { OC_STATE_REPORT_PERIOD_S, OC_STATE_REPORT_PERIOD_US }; + struct timeval currentTime, nextStateReportTime; + U8 iForceObjectToLocalhostU8 = 0; + + FILE *fd; + C8 Id[SMALL_BUFFER_SIZE_0]; + C8 GPSWeek[SMALL_BUFFER_SIZE_0], Timestamp[SMALL_BUFFER_SIZE_0], XPosition[SMALL_BUFFER_SIZE_0], + YPosition[SMALL_BUFFER_SIZE_0], ZPosition[SMALL_BUFFER_SIZE_0]; + C8 Speed[SMALL_BUFFER_SIZE_0], LongitudinalSpeed[SMALL_BUFFER_SIZE_0], LateralSpeed[SMALL_BUFFER_SIZE_0], + LongitudinalAcc[SMALL_BUFFER_SIZE_0], LateralAcc[SMALL_BUFFER_SIZE_0]; + C8 Heading[SMALL_BUFFER_SIZE_0], DriveDirection[SMALL_BUFFER_SIZE_1], ObjectState[SMALL_BUFFER_SIZE_1], + ReadyToArm[SMALL_BUFFER_SIZE_1], MTSP[SMALL_BUFFER_SIZE_0], ErrorStatus[SMALL_BUFFER_SIZE_0]; + I32 MessageLength; + C8 *MiscPtr; + C8 MiscText[SMALL_BUFFER_SIZE_0]; + U32 StartTimeU32 = 0; + U32 OutgoingStartTimeU32 = 0; + U32 DelayedStartU32 = 0; + U32 CurrentTimeU32 = 0; + U32 OldTimeU32 = 0; + U64 TimeCap1, TimeCap2; + struct timeval CurrentTimeStruct; + I32 HeartbeatMessageCounter = 0; + + ObjectPosition OP[MAX_OBJECTS]; + flt SpaceArr[MAX_OBJECTS][TRAJECTORY_FILE_MAX_ROWS]; + flt TimeArr[MAX_OBJECTS][TRAJECTORY_FILE_MAX_ROWS]; + SpaceTime SpaceTimeArr[MAX_OBJECTS][TRAJECTORY_FILE_MAX_ROWS]; + C8 OriginLatitude[SMALL_BUFFER_SIZE_0], OriginLongitude[SMALL_BUFFER_SIZE_0], + OriginAltitude[SMALL_BUFFER_SIZE_0], OriginHeading[SMALL_BUFFER_SIZE_0]; + C8 TextBuffer[SMALL_BUFFER_SIZE_0]; + dbl OriginLatitudeDbl; + dbl OriginLongitudeDbl; + dbl OriginAltitudeDbl; + dbl OriginHeadingDbl; + C8 pcSendBuffer[MBUS_MAX_DATALEN]; + C8 ObjectPort[SMALL_BUFFER_SIZE_0]; + HeaderType HeaderData; + OSEMType OSEMData; + STRTType STRTData; + OSTMType OSTMData; + HEABType HEABData; + MONRType MONRData; + DOTMType DOTMData; + TRAJInfoType TRAJInfoData; + VOILType VOILData; + SYPMType SYPMData; + MTSPType MTSPData; + ACCMData mqACCMData; + EXACData mqEXACData; + GeoPosition OriginPosition; + ASPType ASPData; + + ASPData.MTSPU32 = 0; + ASPData.TimeToSyncPointDbl = 0; + ASPData.PrevTimeToSyncPointDbl = 0; + ASPData.CurrentTimeDbl = 0; + AdaptiveSyncPoint ASP[MAX_ADAPTIVE_SYNC_POINTS]; + I32 SyncPointCount = 0; + I32 SearchStartIndex = 0; + dbl ASPMaxTimeDiffDbl = 0; + dbl ASPMaxTrajDiffDbl = 0; + dbl ASPFilterLevelDbl = 0; + dbl ASPMaxDeltaTimeDbl = 0; + I32 ASPDebugRate = 1; + I32 ASPStepBackCount = 0; + char confDirectoryPath[MAX_FILE_PATH]; + + U8 ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_BOOTING; + + vInitializeState(OBC_STATE_IDLE, GSD); + U8 uiTimeCycle = 0; + I32 ObjectcontrolExecutionMode = OBJECT_CONTROL_CONTROL_MODE; + + C8 Buffer2[SMALL_BUFFER_SIZE_1]; + C8 LogBuffer[LOG_BUFFER_LENGTH]; + C8 VOILReceivers[SMALL_BUFFER_SIZE_254]; + C8 DTMReceivers[SMALL_BUFFER_SIZE_254]; + U32 RowCount; + U32 DTMIpU32; + U32 DTMLengthU32; + + U8 DisconnectU8 = 0; + I32 iResult; + + FILE *TempFd; + U16 MiscU16; + I32 j = 0; + + U8 STRTSentU8 = 0; + C8 FileHeaderBufferC8[TRAJ_FILE_HEADER_ROW]; + + + // Create log + LogInit(MODULE_NAME, logLevel); + LogMessage(LOG_LEVEL_INFO, "Object control task running with PID: %i", getpid()); + + + // Set up signal handlers + if (signal(SIGINT, signalHandler) == SIG_ERR) + util_error("Unable to initialize signal handler"); + + // Set up message bus connection + if (iCommInit()) + util_error("Unable to connect to message queue bus"); + + // Initialize timer for sending state + TimeSetToCurrentSystemTime(¤tTime); + nextStateReportTime = currentTime; + + // Initialize timer for sending state + TimeSetToCurrentSystemTime(¤tTime); + nextStateReportTime = currentTime; + + while (!iExit) { + + if (vGetState(GSD) == OBC_STATE_ERROR) { + ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_ABORT; + MessageLength = + ObjectControlBuildHEABMessage(MessageBuffer, &HEABData, GPSTime, ObjectControlServerStatus, + 0); + UtilSendUDPData("Object Control", &safety_socket_fd[iIndex], &safety_object_addr[iIndex], + MessageBuffer, MessageLength, 0); + } + + if (vGetState(GSD) == OBC_STATE_RUNNING || vGetState(GSD) == OBC_STATE_ARMED + || vGetState(GSD) == OBC_STATE_CONNECTED) { + /*HEAB*/ for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { + if (uiTimeCycle == 0) { + //HeartbeatMessageCounter ++; + MessageLength = + ObjectControlBuildHEABMessage(MessageBuffer, &HEABData, GPSTime, + ObjectControlServerStatus, 0); + //ObjectControlSendUDPData(&safety_socket_fd[iIndex], &safety_object_addr[iIndex], MessageBuffer, MessageLength, 1); + UtilSendUDPData("Object Control", &safety_socket_fd[iIndex], &safety_object_addr[iIndex], + MessageBuffer, MessageLength, 0); + } + } + + + // Check if any object has disconnected - if so, disconnect all objects and return to idle + DisconnectU8 = 0; + for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { + DisconnectU8 |= vCheckRemoteDisconnected(&socket_fds[iIndex]); + if (DisconnectU8) { + LogMessage(LOG_LEVEL_WARNING, "Lost connection to IP %s - returning to IDLE", + object_address_name[iIndex]); + + for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { + vDisconnectObject(&socket_fds[iIndex]); + } + + /* Close safety socket */ + for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { + vCloseSafetyChannel(&safety_socket_fd[iIndex]); + } + vSetState(OBC_STATE_IDLE, GSD); + break; + } + } + } + + if (vGetState(GSD) == OBC_STATE_RUNNING || vGetState(GSD) == OBC_STATE_CONNECTED + || vGetState(GSD) == OBC_STATE_ARMED) { + char buffer[RECV_MESSAGE_BUFFER]; + size_t receivedMONRData = 0; + + // this is etsi time lets remov it ans use utc instead + //gettimeofday(&CurrentTimeStruct, NULL); + + CurrentTimeU32 = + ((GPSTime->GPSSecondsOfWeekU32 * 1000 + (U32) TimeControlGetMillisecond(GPSTime)) << 2) + + GPSTime->MicroSecondU16; + + + /*MTSP*/ if (HeartbeatMessageCounter == 0) { + HeartbeatMessageCounter = 0; + for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { + for (i = 0; i < SyncPointCount; i++) { + if (TEST_SYNC_POINTS == 0 + && strstr(object_address_name[iIndex], ASP[i].SlaveIP) != NULL + && ASPData.MTSPU32 > 0 && ASPData.TimeToSyncPointDbl > -1) { + /*Send Master time to adaptive sync point */ + MessageLength = + ObjectControlBuildMTSPMessage(MessageBuffer, &MTSPData, ASPData.MTSPU32, 0); + //ObjectControlSendUDPData(&safety_socket_fd[iIndex], &safety_object_addr[iIndex], MessageBuffer, MessageLength, 0); + UtilSendUDPData("Object Control", &safety_socket_fd[iIndex], + &safety_object_addr[iIndex], MessageBuffer, MessageLength, 0); + } + /*else if(TEST_SYNC_POINTS == 1 && iIndex == 1 && ASPData.MTSPU32 > 0 && ASPData.TimeToSyncPointDbl > -1 ) + { + Send Master time to adaptive sync point + MessageLength =ObjectControlBuildMTSPMessage(MessageBuffer, &MTSPData, (U32)ASPData.MTSPU32, 0); + ObjectControlSendUDPData(&safety_socket_fd[iIndex], &safety_object_addr[iIndex], MessageBuffer, MessageLength, 0); + } */ + } + } + } + + + for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { + bzero(buffer, RECV_MESSAGE_BUFFER); + receivedMONRData = uiRecvMonitor(&safety_socket_fd[iIndex], buffer, RECV_MESSAGE_BUFFER); + + if (receivedMONRData == sizeof (MONRType)) { + LogMessage(LOG_LEVEL_DEBUG, "Recieved new data from %s %d %d: %s", + object_address_name[iIndex], object_udp_port[iIndex], receivedMONRData, + buffer); + + if (ObjectcontrolExecutionMode == OBJECT_CONTROL_CONTROL_MODE) { + // Append IP to buffer + memcpy(&buffer[receivedMONRData], &safety_object_addr[iIndex].sin_addr.s_addr, + sizeof (in_addr_t)); + // Send MONR message as bytes + + if (iCommSend(COMM_MONR, buffer, (size_t) (receivedMONRData) + sizeof (in_addr_t)) < + 0) { + LogMessage(LOG_LEVEL_ERROR, + "Fatal communication fault when sending MONR command - entering error state"); + vSetState(OBC_STATE_ERROR, GSD); + ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_ABORT; + } + } + + ObjectControlBuildMONRMessage(buffer, &MONRData, 0); + + //Store MONR in GSD + DataDictionarySetMONR(GSD, &MONRData, iIndex); //UtilSendUDPData("ObjectControl", &ObjectControlUDPSocketfdI32, &simulator_addr, &MONRData, sizeof(MONRData), 0) @@ -506,617 +500,620 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { MONRData.Header.MessageLengthU32 + COMMAND_MESSAGE_HEADER_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH; - ObjectControlMONRToASCII(&MONRData, &OriginPosition, iIndex, Id, Timestamp, XPosition, - YPosition, ZPosition, LongitudinalSpeed, LateralSpeed, - LongitudinalAcc, LateralAcc, Heading, DriveDirection, - ObjectState, ReadyToArm, ErrorStatus, 1); - bzero(buffer, OBJECT_MESS_BUFFER_SIZE); - strcat(buffer, object_address_name[iIndex]); - strcat(buffer, ";"); - strcat(buffer, "0"); - strcat(buffer, ";"); - strcat(buffer, Timestamp); - strcat(buffer, ";"); - strcat(buffer, XPosition); - strcat(buffer, ";"); - strcat(buffer, YPosition); - strcat(buffer, ";"); - strcat(buffer, ZPosition); - strcat(buffer, ";"); - strcat(buffer, Heading); - strcat(buffer, ";"); - strcat(buffer, LongitudinalSpeed); - strcat(buffer, ";"); - strcat(buffer, LateralSpeed); - strcat(buffer, ";"); - strcat(buffer, LongitudinalAcc); - strcat(buffer, ";"); - strcat(buffer, LateralAcc); - strcat(buffer, ";"); - strcat(buffer, DriveDirection); - strcat(buffer, ";"); - strcat(buffer, ObjectState); - strcat(buffer, ";"); - strcat(buffer, ReadyToArm); - strcat(buffer, ";"); - strcat(buffer, ErrorStatus); - strcat(buffer, ";"); - - - if (ASPData.MTSPU32 != 0) { - //Add MTSP to MONR if not 0 - bzero(MTSP, SMALL_BUFFER_SIZE_0); - sprintf(MTSP, "%" PRIu32, ASPData.MTSPU32); - strcat(buffer, MTSP); - strcat(buffer, ";"); - } - - //Ok, let's do the ASP - for (i = 0; i < SyncPointCount; i++) { - if (TEST_SYNC_POINTS == 0 - && strstr(object_address_name[iIndex], ASP[i].MasterIP) != NULL - && CurrentTimeU32 > StartTimeU32 && StartTimeU32 > 0 - && ASPData.TimeToSyncPointDbl > -1 - /*|| TEST_SYNC_POINTS == 1 && ASP[0].TestPort == object_udp_port[iIndex] && StartTimeU32 > 0 && iIndex == 0 && TimeToSyncPoint > -1 */ - ) { - // Use the util.c function for time here but it soent mather - gettimeofday(&CurrentTimeStruct, NULL); //Capture time - - TimeCap1 = (uint64_t) CurrentTimeStruct.tv_sec * 1000 + (uint64_t) CurrentTimeStruct.tv_usec / 1000; //Calculate initial timestamp - - OP[iIndex].x = ((dbl) atoi(XPosition)) / 1000; //Set x and y on OP (ObjectPosition) - OP[iIndex].y = ((dbl) atoi(YPosition)) / 1000; - - //OP[iIndex].OrigoDistance = sqrt(pow(OP[iIndex].x,2) + pow(OP[iIndex].y,2)); //Calculate hypotenuse - - UtilCalcPositionDelta(OriginLatitudeDbl, OriginLongitudeDbl, - atof(XPosition) / 1e7, atof(YPosition) / 1e7, &OP[iIndex]); - - if (OP[iIndex].BestFoundTrajectoryIndex <= OP[iIndex].SyncIndex) { - ASPData.CurrentTimeU32 = CurrentTimeU32; - ASPData.CurrentTimeDbl = - (((double)CurrentTimeU32 - (double)StartTimeU32) / 1000); - SearchStartIndex = OP[iIndex].BestFoundTrajectoryIndex - ASPStepBackCount; - UtilFindCurrentTrajectoryPosition(&OP[iIndex], SearchStartIndex, - ASPData.CurrentTimeDbl, ASPMaxTrajDiffDbl, - ASPMaxTimeDiffDbl, 0); - ASPData.BestFoundIndexI32 = OP[iIndex].BestFoundTrajectoryIndex; - - if (OP[iIndex].BestFoundTrajectoryIndex != TRAJ_POSITION_NOT_FOUND) { - ASPData.TimeToSyncPointDbl = UtilCalculateTimeToSync(&OP[iIndex]); - if (ASPData.TimeToSyncPointDbl > 0) { - if (ASPData.PrevTimeToSyncPointDbl != 0 && ASPFilterLevelDbl > 0) { - if (ASPData.TimeToSyncPointDbl / ASPData.PrevTimeToSyncPointDbl > - (1 + ASPFilterLevelDbl / 100)) - ASPData.TimeToSyncPointDbl = ASPData.PrevTimeToSyncPointDbl + ASPMaxDeltaTimeDbl; //TimeToSyncPoint*ASPFilterLevelDbl/500; - else if (ASPData.TimeToSyncPointDbl / - ASPData.PrevTimeToSyncPointDbl < - (1 - ASPFilterLevelDbl / 100)) - ASPData.TimeToSyncPointDbl = ASPData.PrevTimeToSyncPointDbl - ASPMaxDeltaTimeDbl; //TimeToSyncPoint*ASPFilterLevelDbl/500; - } - ASPData.MTSPU32 = - CurrentTimeU32 + (U32) (ASPData.TimeToSyncPointDbl * 4000); - - ASPData.PrevTimeToSyncPointDbl = ASPData.TimeToSyncPointDbl; - OldTimeU32 = CurrentTimeU32; - } - else { - CurrentTimeU32 = 0; - ASPData.TimeToSyncPointDbl = -1; - } - - } - - gettimeofday(&CurrentTimeStruct, NULL); - TimeCap2 = - (uint64_t) CurrentTimeStruct.tv_sec * 1000 + - (uint64_t) CurrentTimeStruct.tv_usec / 1000; - - ASPData.SyncPointIndexI32 = OP[iIndex].SyncIndex; - ASPData.IterationTimeU16 = (U16) (TimeCap2 - TimeCap1); - //Build ASP debug data and set to GSD - //bzero(buffer,OBJECT_MESS_BUFFER_SIZE); - //ObjectControlBuildASPMessage(buffer, &ASPData, 0); - DataDictionarySetRVSSAsp(GSD, &ASPData); - - if (atoi(Timestamp) % ASPDebugRate == 0) { - printf("%d, %d, %3.3f, %s, %s\n", CurrentTimeU32, StartTimeU32, - ASPData.TimeToSyncPointDbl, object_address_name[iIndex], - ASP[i].MasterIP); - printf - ("TtS=%3.3f, BestIndex=%d, MTSP=%d, iIndex=%d, IterationTime=%3.0f ms\n", - ASPData.TimeToSyncPointDbl, OP[iIndex].BestFoundTrajectoryIndex, - ASPData.MTSPU32, iIndex, ((double)(TimeCap2) - (double)TimeCap1)); - printf("CurrentTime=%3.3f, x=%3.3f mm, y=%3.3f\n\n", - ASPData.CurrentTimeDbl, OP[iIndex].x, OP[iIndex].y); - - //Build and send ASP on message queue - //(void)iCommSend(COMM_ASP,buffer); - } - } - } - } - OP[iIndex].Speed = atof(Speed); - } - else if (receivedMONRData > 0) - LogMessage(LOG_LEVEL_INFO, "MONR length error (should be %d but is %ld) from %s.", - sizeof (MONRType), object_address_name[iIndex]); - } - } - - - bzero(pcRecvBuffer, RECV_MESSAGE_BUFFER); - //Have we recieved a command? - if (iCommRecv(&iCommand, pcRecvBuffer, RECV_MESSAGE_BUFFER, NULL)) { - LogMessage(LOG_LEVEL_INFO, "Received command %d", iCommand); - - - if (iCommand == COMM_ARM && vGetState(GSD) == OBC_STATE_CONNECTED) { - - LogMessage(LOG_LEVEL_INFO, "Sending ARM"); - LOG_SEND(LogBuffer, "[ObjectControl] Sending ARM"); - vSetState(OBC_STATE_ARMED, GSD); - MessageLength = - ObjectControlBuildOSTMMessage(MessageBuffer, &OSTMData, COMMAND_OSTM_OPT_SET_ARMED_STATE, - 0); - for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - /*Send OSTM message */ - UtilSendTCPData("[Object Control]", MessageBuffer, MessageLength, &socket_fds[iIndex], 0); - } - - ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_OK; //Set server to READY - } - else if (iCommand == COMM_DISARM && vGetState(GSD) == OBC_STATE_ARMED) { - - LogMessage(LOG_LEVEL_INFO, "Sending DISARM"); - LOG_SEND(LogBuffer, "[ObjectControl] Sending DISARM"); - vSetState(OBC_STATE_CONNECTED, GSD); - - MessageLength = - ObjectControlBuildOSTMMessage(MessageBuffer, &OSTMData, - COMMAND_OSTM_OPT_SET_DISARMED_STATE, 0); - for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - /*Send OSTM message */ - UtilSendTCPData("[" MODULE_NAME "]", MessageBuffer, MessageLength, &socket_fds[iIndex], - 0); - } - - ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_OK; //Set server to READY - } - else if (iCommand == COMM_STRT && (vGetState(GSD) == OBC_STATE_ARMED) /*|| OBC_STATE_INITIALIZED) */ ) //OBC_STATE_INITIALIZED is temporary! - { - bzero(Timestamp, SMALL_BUFFER_SIZE_0); - MiscPtr = strchr(pcRecvBuffer, ';'); - strncpy(Timestamp, MiscPtr + 1, (uint64_t) strchr(MiscPtr + 1, ';') - (uint64_t) MiscPtr - 1); - StartTimeU32 = atol(Timestamp); - bzero(Timestamp, SMALL_BUFFER_SIZE_0); - MiscPtr += 1; - MiscPtr = strchr(pcRecvBuffer, ';'); - strncpy(Timestamp, MiscPtr + 1, (uint64_t) strchr(MiscPtr + 1, ';') - (uint64_t) MiscPtr - 1); - DelayedStartU32 = atoi(Timestamp); - ASPData.MTSPU32 = 0; - ASPData.TimeToSyncPointDbl = 0; - SearchStartIndex = -1; - ASPData.PrevTimeToSyncPointDbl = 0; - OldTimeU32 = CurrentTimeU32; - ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_OK; //Set server to READY - MessageLength = - ObjectControlBuildSTRTMessage(MessageBuffer, &STRTData, GPSTime, (U32) StartTimeU32, - DelayedStartU32, &OutgoingStartTimeU32, 0); - for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - UtilSendTCPData("Object Control", MessageBuffer, MessageLength, &socket_fds[iIndex], 0); - } - vSetState(OBC_STATE_RUNNING, GSD); - - //Store STRT in GSD - if (STRTSentU8 == 0) { - //for(i = 0; i < MessageLength; i++) GSD->STRTData[i] = MessageBuffer[i]; - //GSD->STRTSizeU8 = (U8)MessageLength; - STRTSentU8 = 1; - } - //OBCState = OBC_STATE_INITIALIZED; //This is temporary - //printf("OutgoingStartTimeU32 = %d\n", OutgoingStartTimeU32); - - - GSD->ScenarioStartTimeU32 = OutgoingStartTimeU32; - bzero(MiscText, SMALL_BUFFER_SIZE_0); - sprintf(MiscText, "%" PRIu32, GSD->ScenarioStartTimeU32 << 2); - LOG_SEND(LogBuffer, "[ObjectControl] START received <%s>, GPS time <%s>", pcRecvBuffer, - MiscText); - } - else if (iCommand == COMM_REPLAY) { - ObjectcontrolExecutionMode = OBJECT_CONTROL_REPLAY_MODE; - LogMessage(LOG_LEVEL_INFO, "Entering REPLAY mode <%s>", pcRecvBuffer); - } - else if (iCommand == COMM_ABORT && vGetState(GSD) == OBC_STATE_RUNNING) { - vSetState(OBC_STATE_CONNECTED, GSD); - ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_ABORT; //Set server to ABORT - LogMessage(LOG_LEVEL_WARNING, "ABORT received"); - LOG_SEND(LogBuffer, "[ObjectControl] ABORT received."); - } - else if (iCommand == COMM_CONTROL) { - ObjectcontrolExecutionMode = OBJECT_CONTROL_CONTROL_MODE; - printf("[ObjectControl] Object control in CONTROL mode\n"); - } - else if (iCommand == COMM_INIT) { - LogMessage(LOG_LEVEL_INFO, "INIT received"); - - LOG_SEND(LogBuffer, "[ObjectControl] INIT received."); - nbr_objects = 0; - if (iFindObjectsInfo(object_traj_file, object_address_name, objectIPs, &nbr_objects) == 0) { - // Get objects; name and drive file - DataDictionaryGetForceToLocalhostU8(GSD, &iForceObjectToLocalhostU8); - - for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - if (0 == iForceObjectToLocalhostU8) { - object_udp_port[iIndex] = SAFETY_CHANNEL_PORT; - object_tcp_port[iIndex] = CONTROL_CHANNEL_PORT; - } - else { - object_udp_port[iIndex] = SAFETY_CHANNEL_PORT + iIndex * 2; - object_tcp_port[iIndex] = CONTROL_CHANNEL_PORT + iIndex * 2; - } - } - - /*Setup Adaptive Sync Points (ASP) */ - UtilGetConfDirectoryPath(confDirectoryPath, sizeof (confDirectoryPath)); - strcat(confDirectoryPath, ADAPTIVE_SYNC_FILE_NAME); - fd = fopen(confDirectoryPath, "r"); - if (fd) { - SyncPointCount = UtilCountFileRows(fd) - 1; - fclose(fd); - fd = fopen(confDirectoryPath, "r"); - UtilReadLineCntSpecChars(fd, pcTempBuffer); //Read header - - for (i = 0; i < SyncPointCount; i++) { - UtilSetAdaptiveSyncPoint(&ASP[i], fd, 0); - if (TEST_SYNC_POINTS == 1) - ASP[i].TestPort = SAFETY_CHANNEL_PORT; - } - fclose(fd); - } - - vSetState(OBC_STATE_INITIALIZED, GSD); - LogMessage(LOG_LEVEL_INFO, "ObjectControl is initialized"); - LOG_SEND(LogBuffer, "[ObjectControl] ObjectControl is initialized."); - - //Remove temporary file - remove(TEMP_LOG_FILE); - if (USE_TEMP_LOGFILE) { - //Create temporary file - TempFd = fopen(TEMP_LOG_FILE, "w+"); - } - - //OSEMSentU8 = 0; - STRTSentU8 = 0; - - } - else { - LogMessage(LOG_LEVEL_INFO, - "Could not initialize: object info was not processed successfully"); - pcSendBuffer[0] = (uint8_t) iCommand; - iCommSend(COMM_FAILURE, pcSendBuffer, sizeof (iCommand)); - } - - - } - else if (iCommand == COMM_ACCM && vGetState(GSD) == OBC_STATE_CONNECTED) { - UtilPopulateACCMDataStructFromMQ(pcRecvBuffer, sizeof (pcRecvBuffer), &mqACCMData); - iIndex = - iGetObjectIndexFromObjectIP(mqACCMData.ip, objectIPs, - sizeof (objectIPs) / sizeof (objectIPs[0])); - if (iIndex != -1) { - ObjectControlSendACCMMessage(&mqACCMData, &(socket_fds[iIndex]), 0); - } - else - LogMessage(LOG_LEVEL_WARNING, "Unable to send ACCM: no valid socket found"); - } - else if (iCommand == COMM_EXAC && vGetState(GSD) == OBC_STATE_RUNNING) { - UtilPopulateEXACDataStructFromMQ(pcRecvBuffer, sizeof (pcRecvBuffer), &mqEXACData); - iIndex = - iGetObjectIndexFromObjectIP(mqEXACData.ip, objectIPs, - sizeof (objectIPs) / sizeof (objectIPs[0])); - if (iIndex != -1) - ObjectControlSendEXACMessage(&mqEXACData, &(socket_fds[iIndex]), 0); - else - LogMessage(LOG_LEVEL_WARNING, "Unable to send EXAC: no valid socket found"); - } - else if (iCommand == COMM_CONNECT && vGetState(GSD) == OBC_STATE_INITIALIZED) { - LogMessage(LOG_LEVEL_INFO, "CONNECT received"); - LOG_SEND(LogBuffer, "[ObjectControl] CONNECT received."); - - /* Connect and send drive files */ - for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - - UtilSetObjectPositionIP(&OP[iIndex], object_address_name[iIndex]); - - MessageLength = ObjectControlBuildOSEMMessage(MessageBuffer, &OSEMData, GPSTime, - OriginLatitude, OriginLongitude, - OriginAltitude, 0); - - DisconnectU8 = 0; - - do { - - iResult = - vConnectObject(&socket_fds[iIndex], object_address_name[iIndex], - object_tcp_port[iIndex], &DisconnectU8); - - if (iResult < 0) { - switch (errno) { - case ECONNREFUSED: - LogMessage(LOG_LEVEL_INFO, - "Unable to connect to object %s:%d, retry in %d sec...", - object_address_name[iIndex], object_tcp_port[iIndex], - (!(1 & DisconnectU8)) * 3); - LOG_SEND(LogBuffer, - "[ObjectControl] Was not able to connect to object, [IP: %s] [PORT: %d], retry in %d sec...", - object_address_name[iIndex], object_tcp_port[iIndex], - (!(1 & DisconnectU8)) * 3); - break; - case EADDRINUSE: - LogMessage(LOG_LEVEL_ERROR, - "[ObjectControl] Local address/port already in use"); - break; - case EALREADY: - LogMessage(LOG_LEVEL_ERROR, - "[ObjectControl] Previous connection attempt still in progress"); - break; - case EISCONN: - LogMessage(LOG_LEVEL_ERROR, "[ObjectControl] Socket is already connected"); - break; - case ENETUNREACH: - LogMessage(LOG_LEVEL_ERROR, "[ObjectControl] Network unreachable"); - break; - case ETIMEDOUT: - LogMessage(LOG_LEVEL_ERROR, "[ObjectControl] Connection timed out"); - break; - default: - LogMessage(LOG_LEVEL_ERROR, "ERR: Failed to connect to control socket"); - break; - } - - } - - bzero(pcRecvBuffer, RECV_MESSAGE_BUFFER); - //Have we received a command? - if (iCommRecv(&iCommand, pcRecvBuffer, RECV_MESSAGE_BUFFER, NULL)) { - if (iCommand == COMM_DISCONNECT) { - DisconnectU8 = 1; - LOG_SEND(LogBuffer, "[ObjectControl] DISCONNECT received."); - } - - } - - } while (iExit == 0 && iResult < 0 && DisconnectU8 == 0); - - if (iResult >= 0) { - /* Send OSEM command in mq so that we get some information like GPSweek, origin (latitude,logitude,altitude in gps coordinates) */ - LogMessage(LOG_LEVEL_INFO, "Sending OSEM"); - LOG_SEND(LogBuffer, "[ObjectControl] Sending OSEM."); - - ObjectControlOSEMtoASCII(&OSEMData, GPSWeek, OriginLatitude, OriginLongitude, - OriginAltitude); - bzero(pcSendBuffer, sizeof (pcSendBuffer)); - strcat(pcSendBuffer, GPSWeek); - strcat(pcSendBuffer, ";"); - strcat(pcSendBuffer, OriginLatitude); - strcat(pcSendBuffer, ";"); - strcat(pcSendBuffer, OriginLongitude); - strcat(pcSendBuffer, ";"); - strcat(pcSendBuffer, OriginAltitude); - - //Restore the buffers - DataDictionaryGetOriginLatitudeC8(GSD, OriginLatitude, SMALL_BUFFER_SIZE_0); - DataDictionaryGetOriginLongitudeC8(GSD, OriginLongitude, SMALL_BUFFER_SIZE_0); - DataDictionaryGetOriginAltitudeC8(GSD, OriginAltitude, SMALL_BUFFER_SIZE_0); - - if (iCommSend(COMM_OSEM, pcSendBuffer, strlen(pcSendBuffer) + 1) < 0) { - LogMessage(LOG_LEVEL_ERROR, - "Fatal communication fault when sending OSEM command - entering error state"); - vSetState(OBC_STATE_ERROR, GSD); - ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_ABORT; - } - UtilSendTCPData("Object Control", MessageBuffer, MessageLength, &socket_fds[iIndex], - 0); - - /*Here we send TRAJ, if the IP-address not is found */ - if (strstr(DTMReceivers, object_address_name[iIndex]) == NULL) { - - fd = fopen(object_traj_file[iIndex], "r"); - - if (fd != NULL) { - //RowCount = UtilCountFileRows(fd); - //printf("RowCount: %d\n", RowCount); - //fclose(fd); - - //fd = fopen(object_traj_file[iIndex], "r"); - //printf("Open file: %s\n", object_traj_file[iIndex]); - UtilReadLineCntSpecChars(fd, FileHeaderBufferC8); - fclose(fd); - - /*TRAJ*/ - MessageLength = ObjectControlBuildTRAJMessageHeader(TrajBuffer, - &RowCount, - &HeaderData, - &TRAJInfoData, - FileHeaderBufferC8, - 0); - - //printf("RowCount: %d\n", RowCount); - - /*Send TRAJ header */ - UtilSendTCPData("Object Control", TrajBuffer, MessageLength, - &socket_fds[iIndex], 0); - - /*Send TRAJ data */ - ObjectControlSendTRAJMessage(object_traj_file[iIndex], &socket_fds[iIndex], - RowCount, - (char *)&object_address_name[iIndex], - object_tcp_port[iIndex], &DOTMData, 0); - - } - else - LogMessage(LOG_LEVEL_WARNING, "Could not open file <%s>", - object_traj_file[iIndex]); - } - - - /* Adaptive Sync Points object configuration start... */ - if (TEST_SYNC_POINTS == 1) - printf("Trajfile: %s\n", object_traj_file[iIndex]); - OP[iIndex].TrajectoryPositionCount = RowCount; - OP[iIndex].SpaceArr = SpaceArr[iIndex]; - OP[iIndex].TimeArr = TimeArr[iIndex]; - OP[iIndex].SpaceTimeArr = SpaceTimeArr[iIndex]; - UtilPopulateSpaceTimeArr(&OP[iIndex], object_traj_file[iIndex]); - - LogMessage(LOG_LEVEL_INFO, "Sync point counts: %d", SyncPointCount); - for (i = 0; i < SyncPointCount; i++) { - if (TEST_SYNC_POINTS == 1 && iIndex == 1) { - /*Send SYPM to slave */ - MessageLength = - ObjectControlBuildSYPMMessage(MessageBuffer, &SYPMData, - ASP[i].SlaveTrajSyncTime * 1000, - ASP[i].SlaveSyncStopTime * 1000, 1); - UtilSendTCPData("Object Control", MessageBuffer, MessageLength, - &socket_fds[iIndex], 0); - } - else if (TEST_SYNC_POINTS == 0 - && strstr(object_address_name[iIndex], ASP[i].SlaveIP) != NULL) { - /*Send SYPM to slave */ - MessageLength = - ObjectControlBuildSYPMMessage(MessageBuffer, &SYPMData, - ASP[i].SlaveTrajSyncTime * 1000, - ASP[i].SlaveSyncStopTime * 1000, 1); - UtilSendTCPData("Object Control", MessageBuffer, MessageLength, - &socket_fds[iIndex], 0); - } - } - - /*Set Sync point in OP */ - for (i = 0; i < SyncPointCount; i++) { - if (TEST_SYNC_POINTS == 1 && iIndex == 0) - UtilSetSyncPoint(&OP[iIndex], 0, 0, 0, ASP[i].MasterTrajSyncTime); - else if (TEST_SYNC_POINTS == 0 - && strstr(object_address_name[iIndex], ASP[i].MasterIP) != NULL) - UtilSetSyncPoint(&OP[iIndex], 0, 0, 0, ASP[i].MasterTrajSyncTime); - } - /* ...end */ - } - - } - for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - if (USE_TEST_HOST == 0) - vCreateSafetyChannel(object_address_name[iIndex], object_udp_port[iIndex], - &safety_socket_fd[iIndex], &safety_object_addr[iIndex]); - else if (USE_TEST_HOST == 1) - vCreateSafetyChannel(TESTSERVER_IP, object_udp_port[iIndex], - &safety_socket_fd[iIndex], &safety_object_addr[iIndex]); - } - - uiTimeCycle = 0; - - /* Execution mode */ - ObjectcontrolExecutionMode = OBJECT_CONTROL_CONTROL_MODE; - - /*Set server status */ - ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_OK; //Set server to READY - - if (DisconnectU8 == 0) { - vSetState(OBC_STATE_CONNECTED, GSD); - iCommSend(COMM_OBJECTS_CONNECTED, NULL, 0); - } - else if (DisconnectU8 == 1) - vSetState(OBC_STATE_IDLE, GSD); - } - else if (iCommand == COMM_DATA_DICT) { - - LogMessage(LOG_LEVEL_INFO, "Updating variables from DataDictionary."); - DataDictionaryGetOriginLatitudeC8(GSD, OriginLatitude, SMALL_BUFFER_SIZE_0); - DataDictionaryGetOriginLongitudeC8(GSD, OriginLongitude, SMALL_BUFFER_SIZE_0); - DataDictionaryGetOriginAltitudeC8(GSD, OriginAltitude, SMALL_BUFFER_SIZE_0); - - DataDictionaryGetOriginLatitudeDbl(GSD, &OriginLatitudeDbl); - DataDictionaryGetOriginLongitudeDbl(GSD, &OriginLongitudeDbl); - DataDictionaryGetOriginAltitudeDbl(GSD, &OriginAltitudeDbl); - - OriginLatitudeDbl = atof(OriginLatitude); - OriginLongitudeDbl = atof(OriginLongitude); - OriginAltitudeDbl = atof(OriginAltitude); - OriginHeadingDbl = atof(OriginHeading); - OriginPosition.Latitude = OriginLatitudeDbl; - OriginPosition.Longitude = OriginLongitudeDbl; - OriginPosition.Altitude = OriginAltitudeDbl; - OriginPosition.Heading = OriginHeadingDbl; - - DataDictionaryGetForceToLocalhostU8(GSD, &iForceObjectToLocalhostU8); - LogMessage(LOG_LEVEL_INFO, "ForceObjectToLocalhost = %d", iForceObjectToLocalhostU8); - LOG_SEND(LogBuffer, "[ObjectControl] ForceObjectToLocalhost = %d", iForceObjectToLocalhostU8); - - DataDictionaryGetASPMaxTimeDiffDbl(GSD, &ASPMaxTimeDiffDbl); - DataDictionaryGetASPMaxTrajDiffDbl(GSD, &ASPMaxTrajDiffDbl); - DataDictionaryGetASPStepBackCountU32(GSD, &ASPStepBackCount); - DataDictionaryGetASPFilterLevelDbl(GSD, &ASPFilterLevelDbl); - DataDictionaryGetASPMaxDeltaTimeDbl(GSD, &ASPMaxDeltaTimeDbl); - ASPDebugRate = 1; - DataDictionaryGetVOILReceiversC8(GSD, VOILReceivers, SMALL_BUFFER_SIZE_254); - DataDictionaryGetDTMReceiversC8(GSD, DTMReceivers, SMALL_BUFFER_SIZE_254); - } - else if (iCommand == COMM_DISCONNECT) { - //#ifndef NOTCP - for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - vDisconnectObject(&socket_fds[iIndex]); - } - DataDictionaryFreeMONR(GSD); - //#endif //NOTCP - - LogMessage(LOG_LEVEL_INFO, "DISCONNECT received"); - LOG_SEND(LogBuffer, "[ObjectControl] DISCONNECT received."); - /* Close safety socket */ - for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - vCloseSafetyChannel(&safety_socket_fd[iIndex]); - } - vSetState(OBC_STATE_IDLE, GSD); - } - else if (iCommand == COMM_EXIT) { - iExit = 1; - iCommClose(); - } - else { - LogMessage(LOG_LEVEL_WARNING, "Unhandled command in object control: %d", iCommand); - } - } - - if (!iExit) { - /* Make call periodic */ - sleep_time = iCommand == COMM_INV ? mqEmptyPollPeriod : mqNonEmptyPollPeriod; - - ++uiTimeCycle; - if (uiTimeCycle >= HEARTBEAT_TIME_MS / TASK_PERIOD_MS) { - uiTimeCycle = 0; - } - - // Periodically send state to signal aliveness - TimeSetToCurrentSystemTime(¤tTime); - if (timercmp(¤tTime, &nextStateReportTime, >)) { - timeradd(&nextStateReportTime, &stateReportPeriod, &nextStateReportTime); - - bzero(Buffer2, sizeof (Buffer2)); - Buffer2[0] = (uint8_t) (DataDictionaryGetOBCStateU8(GSD)); - if (iCommSend(COMM_OBC_STATE, Buffer2, sizeof (Buffer2)) < 0) { - LogMessage(LOG_LEVEL_ERROR, - "Fatal communication fault when sending OBC_STATE command - entering error state"); - vSetState(OBC_STATE_ERROR, GSD); - ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_ABORT; - } - } - - (void)nanosleep(&sleep_time, &ref_time); - } - } - - LogMessage(LOG_LEVEL_INFO, "Object control exiting"); + ObjectControlMONRToASCII(&MONRData, &OriginPosition, iIndex, Id, Timestamp, XPosition, + YPosition, ZPosition, LongitudinalSpeed, LateralSpeed, + LongitudinalAcc, LateralAcc, Heading, DriveDirection, + ObjectState, ReadyToArm, ErrorStatus, 1); + bzero(buffer, OBJECT_MESS_BUFFER_SIZE); + strcat(buffer, object_address_name[iIndex]); + strcat(buffer, ";"); + strcat(buffer, "0"); + strcat(buffer, ";"); + strcat(buffer, Timestamp); + strcat(buffer, ";"); + strcat(buffer, XPosition); + strcat(buffer, ";"); + strcat(buffer, YPosition); + strcat(buffer, ";"); + strcat(buffer, ZPosition); + strcat(buffer, ";"); + strcat(buffer, Heading); + strcat(buffer, ";"); + strcat(buffer, LongitudinalSpeed); + strcat(buffer, ";"); + strcat(buffer, LateralSpeed); + strcat(buffer, ";"); + strcat(buffer, LongitudinalAcc); + strcat(buffer, ";"); + strcat(buffer, LateralAcc); + strcat(buffer, ";"); + strcat(buffer, DriveDirection); + strcat(buffer, ";"); + strcat(buffer, ObjectState); + strcat(buffer, ";"); + strcat(buffer, ReadyToArm); + strcat(buffer, ";"); + strcat(buffer, ErrorStatus); + strcat(buffer, ";"); + + + if (ASPData.MTSPU32 != 0) { + //Add MTSP to MONR if not 0 + bzero(MTSP, SMALL_BUFFER_SIZE_0); + sprintf(MTSP, "%" PRIu32, ASPData.MTSPU32); + strcat(buffer, MTSP); + strcat(buffer, ";"); + } + + //Ok, let's do the ASP + for (i = 0; i < SyncPointCount; i++) { + if (TEST_SYNC_POINTS == 0 + && strstr(object_address_name[iIndex], ASP[i].MasterIP) != NULL + && CurrentTimeU32 > StartTimeU32 && StartTimeU32 > 0 + && ASPData.TimeToSyncPointDbl > -1 + /*|| TEST_SYNC_POINTS == 1 && ASP[0].TestPort == object_udp_port[iIndex] && StartTimeU32 > 0 && iIndex == 0 && TimeToSyncPoint > -1 */ + ) { + // Use the util.c function for time here but it soent mather + gettimeofday(&CurrentTimeStruct, NULL); //Capture time + + TimeCap1 = (uint64_t) CurrentTimeStruct.tv_sec * 1000 + (uint64_t) CurrentTimeStruct.tv_usec / 1000; //Calculate initial timestamp + + OP[iIndex].x = ((dbl) atoi(XPosition)) / 1000; //Set x and y on OP (ObjectPosition) + OP[iIndex].y = ((dbl) atoi(YPosition)) / 1000; + + //OP[iIndex].OrigoDistance = sqrt(pow(OP[iIndex].x,2) + pow(OP[iIndex].y,2)); //Calculate hypotenuse + + UtilCalcPositionDelta(OriginLatitudeDbl, OriginLongitudeDbl, + atof(XPosition) / 1e7, atof(YPosition) / 1e7, &OP[iIndex]); + + if (OP[iIndex].BestFoundTrajectoryIndex <= OP[iIndex].SyncIndex) { + ASPData.CurrentTimeU32 = CurrentTimeU32; + ASPData.CurrentTimeDbl = + (((double)CurrentTimeU32 - (double)StartTimeU32) / 1000); + SearchStartIndex = OP[iIndex].BestFoundTrajectoryIndex - ASPStepBackCount; + UtilFindCurrentTrajectoryPosition(&OP[iIndex], SearchStartIndex, + ASPData.CurrentTimeDbl, ASPMaxTrajDiffDbl, + ASPMaxTimeDiffDbl, 0); + ASPData.BestFoundIndexI32 = OP[iIndex].BestFoundTrajectoryIndex; + + if (OP[iIndex].BestFoundTrajectoryIndex != TRAJ_POSITION_NOT_FOUND) { + ASPData.TimeToSyncPointDbl = UtilCalculateTimeToSync(&OP[iIndex]); + if (ASPData.TimeToSyncPointDbl > 0) { + if (ASPData.PrevTimeToSyncPointDbl != 0 && ASPFilterLevelDbl > 0) { + if (ASPData.TimeToSyncPointDbl / ASPData.PrevTimeToSyncPointDbl > + (1 + ASPFilterLevelDbl / 100)) + ASPData.TimeToSyncPointDbl = ASPData.PrevTimeToSyncPointDbl + ASPMaxDeltaTimeDbl; //TimeToSyncPoint*ASPFilterLevelDbl/500; + else if (ASPData.TimeToSyncPointDbl / + ASPData.PrevTimeToSyncPointDbl < + (1 - ASPFilterLevelDbl / 100)) + ASPData.TimeToSyncPointDbl = ASPData.PrevTimeToSyncPointDbl - ASPMaxDeltaTimeDbl; //TimeToSyncPoint*ASPFilterLevelDbl/500; + } + ASPData.MTSPU32 = + CurrentTimeU32 + (U32) (ASPData.TimeToSyncPointDbl * 4000); + + ASPData.PrevTimeToSyncPointDbl = ASPData.TimeToSyncPointDbl; + OldTimeU32 = CurrentTimeU32; + } + else { + CurrentTimeU32 = 0; + ASPData.TimeToSyncPointDbl = -1; + } + + } + + gettimeofday(&CurrentTimeStruct, NULL); + TimeCap2 = + (uint64_t) CurrentTimeStruct.tv_sec * 1000 + + (uint64_t) CurrentTimeStruct.tv_usec / 1000; + + ASPData.SyncPointIndexI32 = OP[iIndex].SyncIndex; + ASPData.IterationTimeU16 = (U16) (TimeCap2 - TimeCap1); + //Build ASP debug data and set to GSD + //bzero(buffer,OBJECT_MESS_BUFFER_SIZE); + //ObjectControlBuildASPMessage(buffer, &ASPData, 0); + DataDictionarySetRVSSAsp(GSD, &ASPData); + + if (atoi(Timestamp) % ASPDebugRate == 0) { + printf("%d, %d, %3.3f, %s, %s\n", CurrentTimeU32, StartTimeU32, + ASPData.TimeToSyncPointDbl, object_address_name[iIndex], + ASP[i].MasterIP); + printf + ("TtS=%3.3f, BestIndex=%d, MTSP=%d, iIndex=%d, IterationTime=%3.0f ms\n", + ASPData.TimeToSyncPointDbl, OP[iIndex].BestFoundTrajectoryIndex, + ASPData.MTSPU32, iIndex, ((double)(TimeCap2) - (double)TimeCap1)); + printf("CurrentTime=%3.3f, x=%3.3f mm, y=%3.3f\n\n", + ASPData.CurrentTimeDbl, OP[iIndex].x, OP[iIndex].y); + + //Build and send ASP on message queue + //(void)iCommSend(COMM_ASP,buffer); + } + } + } + } + OP[iIndex].Speed = atof(Speed); + } + else if (receivedMONRData > 0) + LogMessage(LOG_LEVEL_INFO, "MONR length error (should be %d but is %ld) from %s.", + sizeof (MONRType), object_address_name[iIndex]); + } + } + + + bzero(pcRecvBuffer, RECV_MESSAGE_BUFFER); + //Have we recieved a command? + if (iCommRecv(&iCommand, pcRecvBuffer, RECV_MESSAGE_BUFFER, NULL)) { + LogMessage(LOG_LEVEL_INFO, "Received command %d", iCommand); + + + if (iCommand == COMM_ARM && vGetState(GSD) == OBC_STATE_CONNECTED) { + + LogMessage(LOG_LEVEL_INFO, "Sending ARM"); + LOG_SEND(LogBuffer, "[ObjectControl] Sending ARM"); + vSetState(OBC_STATE_ARMED, GSD); + MessageLength = + ObjectControlBuildOSTMMessage(MessageBuffer, &OSTMData, COMMAND_OSTM_OPT_SET_ARMED_STATE, + 0); + for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { + /*Send OSTM message */ + UtilSendTCPData("[Object Control]", MessageBuffer, MessageLength, &socket_fds[iIndex], 0); + } + + ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_OK; //Set server to READY + } + else if (iCommand == COMM_DISARM && vGetState(GSD) == OBC_STATE_ARMED) { + + LogMessage(LOG_LEVEL_INFO, "Sending DISARM"); + LOG_SEND(LogBuffer, "[ObjectControl] Sending DISARM"); + vSetState(OBC_STATE_CONNECTED, GSD); + + MessageLength = + ObjectControlBuildOSTMMessage(MessageBuffer, &OSTMData, + COMMAND_OSTM_OPT_SET_DISARMED_STATE, 0); + for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { + /*Send OSTM message */ + UtilSendTCPData("[" MODULE_NAME "]", MessageBuffer, MessageLength, &socket_fds[iIndex], + 0); + } + + ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_OK; //Set server to READY + } + else if (iCommand == COMM_STRT && (vGetState(GSD) == OBC_STATE_ARMED) /*|| OBC_STATE_INITIALIZED) */ ) //OBC_STATE_INITIALIZED is temporary! + { + bzero(Timestamp, SMALL_BUFFER_SIZE_0); + MiscPtr = strchr(pcRecvBuffer, ';'); + strncpy(Timestamp, MiscPtr + 1, (uint64_t) strchr(MiscPtr + 1, ';') - (uint64_t) MiscPtr - 1); + StartTimeU32 = atol(Timestamp); + bzero(Timestamp, SMALL_BUFFER_SIZE_0); + MiscPtr += 1; + MiscPtr = strchr(pcRecvBuffer, ';'); + strncpy(Timestamp, MiscPtr + 1, (uint64_t) strchr(MiscPtr + 1, ';') - (uint64_t) MiscPtr - 1); + DelayedStartU32 = atoi(Timestamp); + ASPData.MTSPU32 = 0; + ASPData.TimeToSyncPointDbl = 0; + SearchStartIndex = -1; + ASPData.PrevTimeToSyncPointDbl = 0; + OldTimeU32 = CurrentTimeU32; + ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_OK; //Set server to READY + MessageLength = + ObjectControlBuildSTRTMessage(MessageBuffer, &STRTData, GPSTime, (U32) StartTimeU32, + DelayedStartU32, &OutgoingStartTimeU32, 0); + for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { + UtilSendTCPData("Object Control", MessageBuffer, MessageLength, &socket_fds[iIndex], 0); + } + vSetState(OBC_STATE_RUNNING, GSD); + + //Store STRT in GSD + if (STRTSentU8 == 0) { + //for(i = 0; i < MessageLength; i++) GSD->STRTData[i] = MessageBuffer[i]; + //GSD->STRTSizeU8 = (U8)MessageLength; + STRTSentU8 = 1; + } + //OBCState = OBC_STATE_INITIALIZED; //This is temporary + //printf("OutgoingStartTimeU32 = %d\n", OutgoingStartTimeU32); + + + GSD->ScenarioStartTimeU32 = OutgoingStartTimeU32; + bzero(MiscText, SMALL_BUFFER_SIZE_0); + sprintf(MiscText, "%" PRIu32, GSD->ScenarioStartTimeU32 << 2); + LOG_SEND(LogBuffer, "[ObjectControl] START received <%s>, GPS time <%s>", pcRecvBuffer, + MiscText); + } + else if (iCommand == COMM_REPLAY) { + ObjectcontrolExecutionMode = OBJECT_CONTROL_REPLAY_MODE; + LogMessage(LOG_LEVEL_INFO, "Entering REPLAY mode <%s>", pcRecvBuffer); + } + else if (iCommand == COMM_ABORT && vGetState(GSD) == OBC_STATE_RUNNING) { + vSetState(OBC_STATE_CONNECTED, GSD); + ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_ABORT; //Set server to ABORT + LogMessage(LOG_LEVEL_WARNING, "ABORT received"); + LOG_SEND(LogBuffer, "[ObjectControl] ABORT received."); + } + else if (iCommand == COMM_CONTROL) { + ObjectcontrolExecutionMode = OBJECT_CONTROL_CONTROL_MODE; + printf("[ObjectControl] Object control in CONTROL mode\n"); + } + else if (iCommand == COMM_INIT) { + LogMessage(LOG_LEVEL_INFO, "INIT received"); + + LOG_SEND(LogBuffer, "[ObjectControl] INIT received."); + nbr_objects = 0; + if (iFindObjectsInfo(object_traj_file, object_address_name, objectIPs, &nbr_objects) == 0) { + // Get objects; name and drive file + DataDictionaryGetForceToLocalhostU8(GSD, &iForceObjectToLocalhostU8); + + for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { + if (0 == iForceObjectToLocalhostU8) { + object_udp_port[iIndex] = SAFETY_CHANNEL_PORT; + object_tcp_port[iIndex] = CONTROL_CHANNEL_PORT; + } + else { + object_udp_port[iIndex] = SAFETY_CHANNEL_PORT + iIndex * 2; + object_tcp_port[iIndex] = CONTROL_CHANNEL_PORT + iIndex * 2; + } + } + + /*Setup Adaptive Sync Points (ASP) */ + UtilGetConfDirectoryPath(confDirectoryPath, sizeof (confDirectoryPath)); + strcat(confDirectoryPath, ADAPTIVE_SYNC_FILE_NAME); + fd = fopen(confDirectoryPath, "r"); + if (fd) { + SyncPointCount = UtilCountFileRows(fd) - 1; + fclose(fd); + fd = fopen(confDirectoryPath, "r"); + UtilReadLineCntSpecChars(fd, pcTempBuffer); //Read header + + for (i = 0; i < SyncPointCount; i++) { + UtilSetAdaptiveSyncPoint(&ASP[i], fd, 0); + if (TEST_SYNC_POINTS == 1) + ASP[i].TestPort = SAFETY_CHANNEL_PORT; + } + fclose(fd); + } + + vSetState(OBC_STATE_INITIALIZED, GSD); + LogMessage(LOG_LEVEL_INFO, "ObjectControl is initialized"); + LOG_SEND(LogBuffer, "[ObjectControl] ObjectControl is initialized."); + + //Remove temporary file + remove(TEMP_LOG_FILE); + if (USE_TEMP_LOGFILE) { + //Create temporary file + TempFd = fopen(TEMP_LOG_FILE, "w+"); + } + + //OSEMSentU8 = 0; + STRTSentU8 = 0; + DataDictionarySetNumberOfObjectsU8(GSD, nbr_objects); + DataDictionaryInitMONR(GSD); + + } + else { + LogMessage(LOG_LEVEL_INFO, + "Could not initialize: object info was not processed successfully"); + pcSendBuffer[0] = (uint8_t) iCommand; + iCommSend(COMM_FAILURE, pcSendBuffer, sizeof (iCommand)); + } + + + } + else if (iCommand == COMM_ACCM && vGetState(GSD) == OBC_STATE_CONNECTED) { + UtilPopulateACCMDataStructFromMQ(pcRecvBuffer, sizeof (pcRecvBuffer), &mqACCMData); + iIndex = + iGetObjectIndexFromObjectIP(mqACCMData.ip, objectIPs, + sizeof (objectIPs) / sizeof (objectIPs[0])); + if (iIndex != -1) { + ObjectControlSendACCMMessage(&mqACCMData, &(socket_fds[iIndex]), 0); + } + else + LogMessage(LOG_LEVEL_WARNING, "Unable to send ACCM: no valid socket found"); + } + else if (iCommand == COMM_EXAC && vGetState(GSD) == OBC_STATE_RUNNING) { + UtilPopulateEXACDataStructFromMQ(pcRecvBuffer, sizeof (pcRecvBuffer), &mqEXACData); + iIndex = + iGetObjectIndexFromObjectIP(mqEXACData.ip, objectIPs, + sizeof (objectIPs) / sizeof (objectIPs[0])); + if (iIndex != -1) + ObjectControlSendEXACMessage(&mqEXACData, &(socket_fds[iIndex]), 0); + else + LogMessage(LOG_LEVEL_WARNING, "Unable to send EXAC: no valid socket found"); + } + else if (iCommand == COMM_CONNECT && vGetState(GSD) == OBC_STATE_INITIALIZED) { + LogMessage(LOG_LEVEL_INFO, "CONNECT received"); + LOG_SEND(LogBuffer, "[ObjectControl] CONNECT received."); + + /* Connect and send drive files */ + for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { + + UtilSetObjectPositionIP(&OP[iIndex], object_address_name[iIndex]); + + MessageLength = ObjectControlBuildOSEMMessage(MessageBuffer, &OSEMData, GPSTime, + OriginLatitude, OriginLongitude, + OriginAltitude, 0); + + DisconnectU8 = 0; + + do { + + iResult = + vConnectObject(&socket_fds[iIndex], object_address_name[iIndex], + object_tcp_port[iIndex], &DisconnectU8); + + if (iResult < 0) { + switch (errno) { + case ECONNREFUSED: + LogMessage(LOG_LEVEL_INFO, + "Unable to connect to object %s:%d, retry in %d sec...", + object_address_name[iIndex], object_tcp_port[iIndex], + (!(1 & DisconnectU8)) * 3); + LOG_SEND(LogBuffer, + "[ObjectControl] Was not able to connect to object, [IP: %s] [PORT: %d], retry in %d sec...", + object_address_name[iIndex], object_tcp_port[iIndex], + (!(1 & DisconnectU8)) * 3); + (void)sleep(3); // TODO: Move this to the rest of the sleep operations? Also, remove the hardcoded 3 + break; + case EADDRINUSE: + LogMessage(LOG_LEVEL_ERROR, + "[ObjectControl] Local address/port already in use"); + break; + case EALREADY: + LogMessage(LOG_LEVEL_ERROR, + "[ObjectControl] Previous connection attempt still in progress"); + break; + case EISCONN: + LogMessage(LOG_LEVEL_ERROR, "[ObjectControl] Socket is already connected"); + break; + case ENETUNREACH: + LogMessage(LOG_LEVEL_ERROR, "[ObjectControl] Network unreachable"); + break; + case ETIMEDOUT: + LogMessage(LOG_LEVEL_ERROR, "[ObjectControl] Connection timed out"); + break; + default: + LogMessage(LOG_LEVEL_ERROR, "ERR: Failed to connect to control socket"); + break; + } + + } + + bzero(pcRecvBuffer, RECV_MESSAGE_BUFFER); + //Have we received a command? + if (iCommRecv(&iCommand, pcRecvBuffer, RECV_MESSAGE_BUFFER, NULL)) { + if (iCommand == COMM_DISCONNECT) { + DisconnectU8 = 1; + LOG_SEND(LogBuffer, "[ObjectControl] DISCONNECT received."); + } + + } + + } while (iExit == 0 && iResult < 0 && DisconnectU8 == 0); + + if (iResult >= 0) { + /* Send OSEM command in mq so that we get some information like GPSweek, origin (latitude,logitude,altitude in gps coordinates) */ + LogMessage(LOG_LEVEL_INFO, "Sending OSEM"); + LOG_SEND(LogBuffer, "[ObjectControl] Sending OSEM."); + + ObjectControlOSEMtoASCII(&OSEMData, GPSWeek, OriginLatitude, OriginLongitude, + OriginAltitude); + bzero(pcSendBuffer, sizeof (pcSendBuffer)); + strcat(pcSendBuffer, GPSWeek); + strcat(pcSendBuffer, ";"); + strcat(pcSendBuffer, OriginLatitude); + strcat(pcSendBuffer, ";"); + strcat(pcSendBuffer, OriginLongitude); + strcat(pcSendBuffer, ";"); + strcat(pcSendBuffer, OriginAltitude); + + //Restore the buffers + DataDictionaryGetOriginLatitudeC8(GSD, OriginLatitude, SMALL_BUFFER_SIZE_0); + DataDictionaryGetOriginLongitudeC8(GSD, OriginLongitude, SMALL_BUFFER_SIZE_0); + DataDictionaryGetOriginAltitudeC8(GSD, OriginAltitude, SMALL_BUFFER_SIZE_0); + + if (iCommSend(COMM_OSEM, pcSendBuffer, strlen(pcSendBuffer) + 1) < 0) { + LogMessage(LOG_LEVEL_ERROR, + "Fatal communication fault when sending OSEM command - entering error state"); + vSetState(OBC_STATE_ERROR, GSD); + ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_ABORT; + } + UtilSendTCPData("Object Control", MessageBuffer, MessageLength, &socket_fds[iIndex], + 0); + + /*Here we send TRAJ, if the IP-address not is found */ + if (strstr(DTMReceivers, object_address_name[iIndex]) == NULL) { + + fd = fopen(object_traj_file[iIndex], "r"); + + if (fd != NULL) { + //RowCount = UtilCountFileRows(fd); + //printf("RowCount: %d\n", RowCount); + //fclose(fd); + + //fd = fopen(object_traj_file[iIndex], "r"); + //printf("Open file: %s\n", object_traj_file[iIndex]); + UtilReadLineCntSpecChars(fd, FileHeaderBufferC8); + fclose(fd); + + /*TRAJ*/ + MessageLength = ObjectControlBuildTRAJMessageHeader(TrajBuffer, + &RowCount, + &HeaderData, + &TRAJInfoData, + FileHeaderBufferC8, + 0); + + //printf("RowCount: %d\n", RowCount); + + /*Send TRAJ header */ + UtilSendTCPData("Object Control", TrajBuffer, MessageLength, + &socket_fds[iIndex], 0); + + /*Send TRAJ data */ + ObjectControlSendTRAJMessage(object_traj_file[iIndex], &socket_fds[iIndex], + RowCount, + (char *)&object_address_name[iIndex], + object_tcp_port[iIndex], &DOTMData, 0); + + } + else + LogMessage(LOG_LEVEL_WARNING, "Could not open file <%s>", + object_traj_file[iIndex]); + } + + + /* Adaptive Sync Points object configuration start... */ + if (TEST_SYNC_POINTS == 1) + printf("Trajfile: %s\n", object_traj_file[iIndex]); + OP[iIndex].TrajectoryPositionCount = RowCount; + OP[iIndex].SpaceArr = SpaceArr[iIndex]; + OP[iIndex].TimeArr = TimeArr[iIndex]; + OP[iIndex].SpaceTimeArr = SpaceTimeArr[iIndex]; + UtilPopulateSpaceTimeArr(&OP[iIndex], object_traj_file[iIndex]); + + LogMessage(LOG_LEVEL_INFO, "Sync point counts: %d", SyncPointCount); + for (i = 0; i < SyncPointCount; i++) { + if (TEST_SYNC_POINTS == 1 && iIndex == 1) { + /*Send SYPM to slave */ + MessageLength = + ObjectControlBuildSYPMMessage(MessageBuffer, &SYPMData, + ASP[i].SlaveTrajSyncTime * 1000, + ASP[i].SlaveSyncStopTime * 1000, 1); + UtilSendTCPData("Object Control", MessageBuffer, MessageLength, + &socket_fds[iIndex], 0); + } + else if (TEST_SYNC_POINTS == 0 + && strstr(object_address_name[iIndex], ASP[i].SlaveIP) != NULL) { + /*Send SYPM to slave */ + MessageLength = + ObjectControlBuildSYPMMessage(MessageBuffer, &SYPMData, + ASP[i].SlaveTrajSyncTime * 1000, + ASP[i].SlaveSyncStopTime * 1000, 1); + UtilSendTCPData("Object Control", MessageBuffer, MessageLength, + &socket_fds[iIndex], 0); + } + } + + /*Set Sync point in OP */ + for (i = 0; i < SyncPointCount; i++) { + if (TEST_SYNC_POINTS == 1 && iIndex == 0) + UtilSetSyncPoint(&OP[iIndex], 0, 0, 0, ASP[i].MasterTrajSyncTime); + else if (TEST_SYNC_POINTS == 0 + && strstr(object_address_name[iIndex], ASP[i].MasterIP) != NULL) + UtilSetSyncPoint(&OP[iIndex], 0, 0, 0, ASP[i].MasterTrajSyncTime); + } + /* ...end */ + } + + } + for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { + if (USE_TEST_HOST == 0) + vCreateSafetyChannel(object_address_name[iIndex], object_udp_port[iIndex], + &safety_socket_fd[iIndex], &safety_object_addr[iIndex]); + else if (USE_TEST_HOST == 1) + vCreateSafetyChannel(TESTSERVER_IP, object_udp_port[iIndex], + &safety_socket_fd[iIndex], &safety_object_addr[iIndex]); + } + + uiTimeCycle = 0; + + /* Execution mode */ + ObjectcontrolExecutionMode = OBJECT_CONTROL_CONTROL_MODE; + + /*Set server status */ + ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_OK; //Set server to READY + + if (DisconnectU8 == 0) { + vSetState(OBC_STATE_CONNECTED, GSD); + iCommSend(COMM_OBJECTS_CONNECTED, NULL, 0); + } + else if (DisconnectU8 == 1) + vSetState(OBC_STATE_IDLE, GSD); + } + else if (iCommand == COMM_DATA_DICT) { + + LogMessage(LOG_LEVEL_INFO, "Updating variables from DataDictionary."); + DataDictionaryGetOriginLatitudeC8(GSD, OriginLatitude, SMALL_BUFFER_SIZE_0); + DataDictionaryGetOriginLongitudeC8(GSD, OriginLongitude, SMALL_BUFFER_SIZE_0); + DataDictionaryGetOriginAltitudeC8(GSD, OriginAltitude, SMALL_BUFFER_SIZE_0); + + DataDictionaryGetOriginLatitudeDbl(GSD, &OriginLatitudeDbl); + DataDictionaryGetOriginLongitudeDbl(GSD, &OriginLongitudeDbl); + DataDictionaryGetOriginAltitudeDbl(GSD, &OriginAltitudeDbl); + + OriginLatitudeDbl = atof(OriginLatitude); + OriginLongitudeDbl = atof(OriginLongitude); + OriginAltitudeDbl = atof(OriginAltitude); + OriginHeadingDbl = atof(OriginHeading); + OriginPosition.Latitude = OriginLatitudeDbl; + OriginPosition.Longitude = OriginLongitudeDbl; + OriginPosition.Altitude = OriginAltitudeDbl; + OriginPosition.Heading = OriginHeadingDbl; + + DataDictionaryGetForceToLocalhostU8(GSD, &iForceObjectToLocalhostU8); + LogMessage(LOG_LEVEL_INFO, "ForceObjectToLocalhost = %d", iForceObjectToLocalhostU8); + LOG_SEND(LogBuffer, "[ObjectControl] ForceObjectToLocalhost = %d", iForceObjectToLocalhostU8); + + DataDictionaryGetASPMaxTimeDiffDbl(GSD, &ASPMaxTimeDiffDbl); + DataDictionaryGetASPMaxTrajDiffDbl(GSD, &ASPMaxTrajDiffDbl); + DataDictionaryGetASPStepBackCountU32(GSD, &ASPStepBackCount); + DataDictionaryGetASPFilterLevelDbl(GSD, &ASPFilterLevelDbl); + DataDictionaryGetASPMaxDeltaTimeDbl(GSD, &ASPMaxDeltaTimeDbl); + ASPDebugRate = 1; + DataDictionaryGetVOILReceiversC8(GSD, VOILReceivers, SMALL_BUFFER_SIZE_254); + DataDictionaryGetDTMReceiversC8(GSD, DTMReceivers, SMALL_BUFFER_SIZE_254); + } + else if (iCommand == COMM_DISCONNECT) { + //#ifndef NOTCP + for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { + vDisconnectObject(&socket_fds[iIndex]); + } + DataDictionaryFreeMONR(GSD); + //#endif //NOTCP + + LogMessage(LOG_LEVEL_INFO, "DISCONNECT received"); + LOG_SEND(LogBuffer, "[ObjectControl] DISCONNECT received."); + /* Close safety socket */ + for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { + vCloseSafetyChannel(&safety_socket_fd[iIndex]); + } + vSetState(OBC_STATE_IDLE, GSD); + } + else if (iCommand == COMM_EXIT) { + iExit = 1; + iCommClose(); + } + else { + LogMessage(LOG_LEVEL_WARNING, "Unhandled command in object control: %d", iCommand); + } + } + + if (!iExit) { + /* Make call periodic */ + sleep_time = iCommand == COMM_INV ? mqEmptyPollPeriod : mqNonEmptyPollPeriod; + + ++uiTimeCycle; + if (uiTimeCycle >= HEARTBEAT_TIME_MS / TASK_PERIOD_MS) { + uiTimeCycle = 0; + } + + // Periodically send state to signal aliveness + TimeSetToCurrentSystemTime(¤tTime); + if (timercmp(¤tTime, &nextStateReportTime, >)) { + timeradd(&nextStateReportTime, &stateReportPeriod, &nextStateReportTime); + + bzero(Buffer2, sizeof (Buffer2)); + Buffer2[0] = (uint8_t) (DataDictionaryGetOBCStateU8(GSD)); + if (iCommSend(COMM_OBC_STATE, Buffer2, sizeof (Buffer2)) < 0) { + LogMessage(LOG_LEVEL_ERROR, + "Fatal communication fault when sending OBC_STATE command - entering error state"); + vSetState(OBC_STATE_ERROR, GSD); + ObjectControlServerStatus = COMMAND_HEAB_OPT_SERVER_STATUS_ABORT; + } + } + + (void)nanosleep(&sleep_time, &ref_time); + } + } + + LogMessage(LOG_LEVEL_INFO, "Object control exiting"); } @@ -1125,1157 +1122,1162 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { ------------------------------------------------------------*/ void signalHandler(int signo) { - if (signo == SIGINT) { - LogMessage(LOG_LEVEL_WARNING, "Caught keyboard interrupt"); - iExit = 1; - } - else { - LogMessage(LOG_LEVEL_ERROR, "Caught unhandled signal"); - } + if (signo == SIGINT) { + LogMessage(LOG_LEVEL_WARNING, "Caught keyboard interrupt"); + iExit = 1; + } + else { + LogMessage(LOG_LEVEL_ERROR, "Caught unhandled signal"); + } } I32 ObjectControlBuildVOILMessage(C8 * MessageBuffer, VOILType * VOILData, C8 * SimData, U8 debug) { - I32 MessageIndex = 0, i; - U16 Crc = 0; - C8 *p; - U16 U16Data = 0; - I16 I16Data = 0; - U32 U32Data = 0; - I32 I32Data = 0; - - if (debug) { - printf("Length: %d\n", *(SimData + 3)); - for (i = 0; i < *(SimData + 3) + 4; i++) - printf("%x-", *(SimData + i)); - printf("\n"); - } - - U16Data = (U16Data | *(SimData + 5)) << 8; - U16Data = (U16Data | *(SimData + 4)); - U16 MessageId = U16Data; - - //printf("MessageId = %x\n", MessageId); - - U32Data = (U32Data | *(SimData + 6)) << 8; - U32Data = (U32Data | *(SimData + 7)) << 8; - U32Data = (U32Data | *(SimData + 8)) << 8; - U32Data = (U32Data | *(SimData + 9)); - U32 GPSSOW = U32Data; - - //printf("GPSSOW = %x\n", GPSSOW); - U8 DynamicWorldState = *(SimData + 10); - U8 ObjectCount = *(SimData + 11); - - //printf("ObjectCount = %d\n", ObjectCount); - - U8 ObjectId = *(SimData + 12); - U8 ObjectState = *(SimData + 13); - - I32Data = (I32Data | *(SimData + 14)) << 8; - I32Data = (I32Data | *(SimData + 15)) << 8; - I32Data = (I32Data | *(SimData + 16)) << 8; - I32Data = (I32Data | *(SimData + 17)); - I32 XPosition = I32Data; - - I32Data = 0; - I32Data = (I32Data | *(SimData + 18)) << 8; - I32Data = (I32Data | *(SimData + 19)) << 8; - I32Data = (I32Data | *(SimData + 20)) << 8; - I32Data = (I32Data | *(SimData + 21)); - I32 YPosition = I32Data; - - I32Data = 0; - I32Data = (I32Data | *(SimData + 22)) << 8; - I32Data = (I32Data | *(SimData + 23)) << 8; - I32Data = (I32Data | *(SimData + 24)) << 8; - I32Data = (I32Data | *(SimData + 25)); - I32 ZPosition = I32Data; - - U16Data = 0; - U16Data = (U16Data | *(SimData + 26)) << 8; - U16Data = (U16Data | *(SimData + 27)); - U16 Heading = U16Data; - - U16Data = 0; - U16Data = (U16Data | *(SimData + 28)) << 8; - U16Data = (U16Data | *(SimData + 29)); - U16 Pitch = U16Data; - - U16Data = 0; - U16Data = (U16Data | *(SimData + 30)) << 8; - U16Data = (U16Data | *(SimData + 31)); - U16 Roll = U16Data; - - //printf("Roll = %d\n", Roll); - I16Data = 0; - I16Data = (I16Data | *(SimData + 32)) << 8; - I16Data = (I16Data | *(SimData + 33)); - I16 Speed = I16Data; - - //printf("Speed = %d\n", Speed); - - - bzero(MessageBuffer, - ObjectCount * sizeof (Sim1Type) + 6 + COMMAND_MESSAGE_FOOTER_LENGTH + - COMMAND_MESSAGE_HEADER_LENGTH); - - - VOILData->Header.SyncWordU16 = ISO_SYNC_WORD; - VOILData->Header.TransmitterIdU8 = 0; - VOILData->Header.MessageCounterU8 = 0; - VOILData->Header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; - VOILData->Header.MessageIdU16 = COMMAND_VOIL_CODE; - VOILData->Header.MessageLengthU32 = ObjectCount * sizeof (Sim1Type) + 6 - COMMAND_MESSAGE_HEADER_LENGTH; - VOILData->GPSQmsOfWeekU32 = GPSSOW; - VOILData->WorldStateU8 = DynamicWorldState; - VOILData->ObjectCountU8 = ObjectCount; - VOILData->SimObjects[0].ObjectIdU8 = ObjectId; - VOILData->SimObjects[0].ObjectStateU8 = ObjectState; - VOILData->SimObjects[0].XPositionI32 = XPosition; - VOILData->SimObjects[0].YPositionI32 = YPosition; - VOILData->SimObjects[0].ZPositionI32 = ZPosition; - VOILData->SimObjects[0].HeadingU16 = Heading; - VOILData->SimObjects[0].PitchU16 = Pitch; - VOILData->SimObjects[0].RollU16 = Roll; - VOILData->SimObjects[0].SpeedI16 = Speed; - - - p = (C8 *) VOILData; - for (i = 0; i < ObjectCount * sizeof (Sim1Type) + 6 + COMMAND_MESSAGE_HEADER_LENGTH; i++) - *(MessageBuffer + i) = *p++; - //Crc = crc_16((const C8*)MessageBuffer, sizeof(VOILData)); - Crc = 0; - *(MessageBuffer + i++) = (U8) (Crc); - *(MessageBuffer + i++) = (U8) (Crc >> 8); - MessageIndex = i; - - if (debug) { - // TODO: use byte printout from logging when it has been implemented - printf("VOILData total length = %d bytes (header+message+footer)\n", - (int)(ObjectCount * sizeof (Sim1Type) + 6 + COMMAND_MESSAGE_FOOTER_LENGTH + - COMMAND_MESSAGE_HEADER_LENGTH)); - printf("----HEADER----\n"); - for (i = 0; i < sizeof (HeaderType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----MESSAGE----\n"); - for (; i < sizeof (Sim1Type) * ObjectCount + 6 + COMMAND_MESSAGE_HEADER_LENGTH; i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----FOOTER----\n"); - for (; i < MessageIndex; i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n"); - } - - - return ObjectCount * sizeof (Sim1Type) + 6 + COMMAND_MESSAGE_HEADER_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH; //Total number of bytes + I32 MessageIndex = 0, i; + U16 Crc = 0; + C8 *p; + U16 U16Data = 0; + I16 I16Data = 0; + U32 U32Data = 0; + I32 I32Data = 0; + + if (debug) { + printf("Length: %d\n", *(SimData + 3)); + for (i = 0; i < *(SimData + 3) + 4; i++) + printf("%x-", *(SimData + i)); + printf("\n"); + } + + U16Data = (U16Data | *(SimData + 5)) << 8; + U16Data = (U16Data | *(SimData + 4)); + U16 MessageId = U16Data; + + //printf("MessageId = %x\n", MessageId); + + U32Data = (U32Data | *(SimData + 6)) << 8; + U32Data = (U32Data | *(SimData + 7)) << 8; + U32Data = (U32Data | *(SimData + 8)) << 8; + U32Data = (U32Data | *(SimData + 9)); + U32 GPSSOW = U32Data; + + //printf("GPSSOW = %x\n", GPSSOW); + U8 DynamicWorldState = *(SimData + 10); + U8 ObjectCount = *(SimData + 11); + + //printf("ObjectCount = %d\n", ObjectCount); + + U8 ObjectId = *(SimData + 12); + U8 ObjectState = *(SimData + 13); + + I32Data = (I32Data | *(SimData + 14)) << 8; + I32Data = (I32Data | *(SimData + 15)) << 8; + I32Data = (I32Data | *(SimData + 16)) << 8; + I32Data = (I32Data | *(SimData + 17)); + I32 XPosition = I32Data; + + I32Data = 0; + I32Data = (I32Data | *(SimData + 18)) << 8; + I32Data = (I32Data | *(SimData + 19)) << 8; + I32Data = (I32Data | *(SimData + 20)) << 8; + I32Data = (I32Data | *(SimData + 21)); + I32 YPosition = I32Data; + + I32Data = 0; + I32Data = (I32Data | *(SimData + 22)) << 8; + I32Data = (I32Data | *(SimData + 23)) << 8; + I32Data = (I32Data | *(SimData + 24)) << 8; + I32Data = (I32Data | *(SimData + 25)); + I32 ZPosition = I32Data; + + U16Data = 0; + U16Data = (U16Data | *(SimData + 26)) << 8; + U16Data = (U16Data | *(SimData + 27)); + U16 Heading = U16Data; + + U16Data = 0; + U16Data = (U16Data | *(SimData + 28)) << 8; + U16Data = (U16Data | *(SimData + 29)); + U16 Pitch = U16Data; + + U16Data = 0; + U16Data = (U16Data | *(SimData + 30)) << 8; + U16Data = (U16Data | *(SimData + 31)); + U16 Roll = U16Data; + + //printf("Roll = %d\n", Roll); + I16Data = 0; + I16Data = (I16Data | *(SimData + 32)) << 8; + I16Data = (I16Data | *(SimData + 33)); + I16 Speed = I16Data; + + //printf("Speed = %d\n", Speed); + + + bzero(MessageBuffer, + ObjectCount * sizeof (Sim1Type) + 6 + COMMAND_MESSAGE_FOOTER_LENGTH + + COMMAND_MESSAGE_HEADER_LENGTH); + + + VOILData->Header.SyncWordU16 = SYNC_WORD; + VOILData->Header.TransmitterIdU8 = 0; + VOILData->Header.MessageCounterU8 = 0; + VOILData->Header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; + VOILData->Header.MessageIdU16 = COMMAND_VOIL_CODE; + VOILData->Header.MessageLengthU32 = ObjectCount * sizeof (Sim1Type) + 6 - COMMAND_MESSAGE_HEADER_LENGTH; + VOILData->GPSQmsOfWeekU32 = GPSSOW; + VOILData->WorldStateU8 = DynamicWorldState; + VOILData->ObjectCountU8 = ObjectCount; + VOILData->SimObjects[0].ObjectIdU8 = ObjectId; + VOILData->SimObjects[0].ObjectStateU8 = ObjectState; + VOILData->SimObjects[0].XPositionI32 = XPosition; + VOILData->SimObjects[0].YPositionI32 = YPosition; + VOILData->SimObjects[0].ZPositionI32 = ZPosition; + VOILData->SimObjects[0].HeadingU16 = Heading; + VOILData->SimObjects[0].PitchU16 = Pitch; + VOILData->SimObjects[0].RollU16 = Roll; + VOILData->SimObjects[0].SpeedI16 = Speed; + + + p = (C8 *) VOILData; + for (i = 0; i < ObjectCount * sizeof (Sim1Type) + 6 + COMMAND_MESSAGE_HEADER_LENGTH; i++) + *(MessageBuffer + i) = *p++; + //Crc = crc_16((const C8*)MessageBuffer, sizeof(VOILData)); + Crc = 0; + *(MessageBuffer + i++) = (U8) (Crc); + *(MessageBuffer + i++) = (U8) (Crc >> 8); + MessageIndex = i; + + if (debug) { + // TODO: use byte printout from logging when it has been implemented + printf("VOILData total length = %d bytes (header+message+footer)\n", + (int)(ObjectCount * sizeof (Sim1Type) + 6 + COMMAND_MESSAGE_FOOTER_LENGTH + + COMMAND_MESSAGE_HEADER_LENGTH)); + printf("----HEADER----\n"); + for (i = 0; i < sizeof (HeaderType); i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n----MESSAGE----\n"); + for (; i < sizeof (Sim1Type) * ObjectCount + 6 + COMMAND_MESSAGE_HEADER_LENGTH; i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n----FOOTER----\n"); + for (; i < MessageIndex; i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n"); + } + + + return ObjectCount * sizeof (Sim1Type) + 6 + COMMAND_MESSAGE_HEADER_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH; //Total number of bytes } -/*! - * \brief ObjectControlBuildMONRMessage Fills a MONRType struct from a buffer of raw data - * \param MonrData Raw data to be decoded - * \param length Length of the raw data buffer - * \param MONRData Struct to be filled - * \param debug Flag for enabling of debugging - * \return 0 on success, -1 otherwise - */ -I32 ObjectControlBuildMONRMessage(C8 * MonrData, const size_t length, MONRType * MONRData, U8 debug) { - C8 *p = MonrData; - const U16 ExpectedMONRStructSize = (U16) (sizeof (*MONRData) - sizeof (MONRData->Header) - - sizeof (MONRData->CRC) - - sizeof (MONRData->MonrStructValueIdU16) - - sizeof (MONRData->MonrStructContentLengthU16)); - - if (UtilISOBuildHeader(MonrData, length, &MONRData->Header, 0) == -1) { - memset(MONRData, 0, sizeof (*MONRData)); - return -1; - } - p += sizeof (MONRData->Header); - - // Decode content header - memcpy(&MONRData->MonrStructValueIdU16, p, sizeof (MONRData->MonrStructValueIdU16)); - p += sizeof (MONRData->MonrStructValueIdU16); - - if (MONRData->MonrStructValueIdU16 != VALUE_ID_MONR_STRUCT) { - errno = EINVAL; - LogMessage(LOG_LEVEL_ERROR, "Attempted to pass non-MONR struct into MONR parsing function"); - memset(MONRData, 0, sizeof (*MONRData)); - return -1; - } - - memcpy(&MONRData->MonrStructContentLengthU16, p, sizeof (MONRData->MonrStructContentLengthU16)); - p += sizeof (MONRData->MonrStructContentLengthU16); - - if (MONRData->MonrStructContentLengthU16 != ExpectedMONRStructSize) { - errno = EINVAL; - LogMessage(LOG_LEVEL_ERROR, "MONR content length %u differs from the expected length %u", - MONRData->MonrStructContentLengthU16, ExpectedMONRStructSize); - memset(MONRData, 0, sizeof (*MONRData)); - return -1; - } - - // Decode content - memcpy(&MONRData->GPSQmsOfWeekU32, p, sizeof (MONRData->GPSQmsOfWeekU32)); - p += sizeof (MONRData->GPSQmsOfWeekU32); - - memcpy(&MONRData->XPositionI32, p, sizeof (MONRData->XPositionI32)); - p += sizeof (MONRData->XPositionI32); - - memcpy(&MONRData->YPositionI32, p, sizeof (MONRData->YPositionI32)); - p += sizeof (MONRData->YPositionI32); - - memcpy(&MONRData->ZPositionI32, p, sizeof (MONRData->ZPositionI32)); - p += sizeof (MONRData->ZPositionI32); - - memcpy(&MONRData->HeadingU16, p, sizeof (MONRData->HeadingU16)); - p += sizeof (MONRData->HeadingU16); - - memcpy(&MONRData->LongitudinalSpeedI16, p, sizeof (MONRData->LongitudinalSpeedI16)); - p += sizeof (MONRData->LongitudinalSpeedI16); - - memcpy(&MONRData->LateralSpeedI16, p, sizeof (MONRData->LateralSpeedI16)); - p += sizeof (MONRData->LateralSpeedI16); - - memcpy(&MONRData->LongitudinalAccI16, p, sizeof (MONRData->LongitudinalAccI16)); - p += sizeof (MONRData->LongitudinalAccI16); - - memcpy(&MONRData->LateralAccI16, p, sizeof (MONRData->LateralAccI16)); - p += sizeof (MONRData->LateralAccI16); - - memcpy(&MONRData->DriveDirectionU8, p, sizeof (MONRData->DriveDirectionU8)); - p += sizeof (MONRData->DriveDirectionU8); - - memcpy(&MONRData->StateU8, p, sizeof (MONRData->StateU8)); - p += sizeof (MONRData->StateU8); - - memcpy(&MONRData->ReadyToArmU8, p, sizeof (MONRData->ReadyToArmU8)); - p += sizeof (MONRData->ReadyToArmU8); - - memcpy(&MONRData->ErrorStatusU8, p, sizeof (MONRData->ErrorStatusU8)); - p += sizeof (MONRData->ErrorStatusU8); - - // Footer - memcpy(&MONRData->CRC, p, sizeof (MONRData->CRC)); - p += sizeof (MONRData->CRC); - - // TODO: check on CRC - - if (debug == 1) { - LogPrint("MONR:"); - LogPrint("SyncWord = %x", MONRData->Header.SyncWordU16); - LogPrint("TransmitterId = %d", MONRData->Header.TransmitterIdU8); - LogPrint("PackageCounter = %d", MONRData->Header.MessageCounterU8); - LogPrint("AckReq = %d", MONRData->Header.AckReqProtVerU8); - LogPrint("MessageId = %d", MONRData->Header.MessageIdU16); - LogPrint("MessageLength = %d", MONRData->Header.MessageLengthU32); - LogPrint("ValueId = %d", MONRData->MonrStructValueIdU16); - LogPrint("ContentLength = %d", MONRData->MonrStructContentLengthU16); - LogPrint("GPSSOW = %d", MONRData->GPSQmsOfWeekU32); - LogPrint("XPosition = %d", MONRData->XPositionI32); - LogPrint("YPosition = %d", MONRData->YPositionI32); - LogPrint("ZPosition = %d", MONRData->ZPositionI32); - LogPrint("Heading = %d", MONRData->HeadingU16); - LogPrint("LongitudinalSpeed = %d", MONRData->LongitudinalSpeedI16); - LogPrint("LateralSpeed = %d", MONRData->LateralSpeedI16); - LogPrint("LongitudinalAcc = %d", MONRData->LongitudinalAccI16); - LogPrint("LateralAcc = %d", MONRData->LateralAccI16); - LogPrint("DriveDirection = %d", MONRData->DriveDirectionU8); - LogPrint("State = %d", MONRData->StateU8); - LogPrint("ReadyToArm = %d", MONRData->ReadyToArmU8); - LogPrint("ErrorStatus = %d", MONRData->ErrorStatusU8); - } - - return 0; + +I32 ObjectControlBuildMONRMessage(C8 * MonrData, MONRType * MONRData, U8 debug) { + I32 MessageIndex = 0, i = 0; + dbl Data; + U16 Crc = 0, U16Data = 0; + I16 I16Data = 0; + U32 U32Data = 0; + I32 I32Data = 0; + U64 U64Data = 0; + U16 contentLength = 0; + U16 valueID = 0; + C8 *p = MonrData; + + // Decode ISO header + memcpy(&MONRData->Header.SyncWordU16, p, sizeof (MONRData->Header.SyncWordU16)); + p += sizeof (MONRData->Header.SyncWordU16); + + memcpy(&MONRData->Header.TransmitterIdU8, p, sizeof (MONRData->Header.TransmitterIdU8)); + p += sizeof (MONRData->Header.TransmitterIdU8); + + memcpy(&MONRData->Header.MessageCounterU8, p, sizeof (MONRData->Header.MessageCounterU8)); + p += sizeof (MONRData->Header.MessageCounterU8); + + memcpy(&MONRData->Header.AckReqProtVerU8, p, sizeof (MONRData->Header.AckReqProtVerU8)); + p += sizeof (MONRData->Header.AckReqProtVerU8); + + memcpy(&MONRData->Header.MessageIdU16, p, sizeof (MONRData->Header.MessageIdU16)); + p += sizeof (MONRData->Header.MessageIdU16); + + memcpy(&MONRData->Header.MessageLengthU32, p, sizeof (MONRData->Header.MessageLengthU32)); + p += sizeof (MONRData->Header.MessageLengthU32); + + // Decode content header + memcpy(&valueID, p, sizeof (valueID)); + if (valueID == VALUE_ID_MONR_STRUCT) { + memcpy(&MONRData->MonrStructValueIdU16, p, sizeof (MONRData->MonrStructValueIdU16)); + p += sizeof (MONRData->MonrStructValueIdU16); + + //memcpy(&contentLength, p, sizeof (contentLength)); + memcpy(&MONRData->MonrStructContentLengthU16, p, sizeof (MONRData->MonrStructContentLengthU16)); + p += sizeof (MONRData->MonrStructContentLengthU16); + + // TODO: check on content length + } + else if (debug) { + LogPrint("MONR message did not contain a content header"); + } + + // Decode content + memcpy(&MONRData->GPSQmsOfWeekU32, p, sizeof (MONRData->GPSQmsOfWeekU32)); + p += sizeof (MONRData->GPSQmsOfWeekU32); + + memcpy(&MONRData->XPositionI32, p, sizeof (MONRData->XPositionI32)); + p += sizeof (MONRData->XPositionI32); + + memcpy(&MONRData->YPositionI32, p, sizeof (MONRData->YPositionI32)); + p += sizeof (MONRData->YPositionI32); + + memcpy(&MONRData->ZPositionI32, p, sizeof (MONRData->ZPositionI32)); + p += sizeof (MONRData->ZPositionI32); + + memcpy(&MONRData->HeadingU16, p, sizeof (MONRData->HeadingU16)); + p += sizeof (MONRData->HeadingU16); + + memcpy(&MONRData->LongitudinalSpeedI16, p, sizeof (MONRData->LongitudinalSpeedI16)); + p += sizeof (MONRData->LongitudinalSpeedI16); + + memcpy(&MONRData->LateralSpeedI16, p, sizeof (MONRData->LateralSpeedI16)); + p += sizeof (MONRData->LateralSpeedI16); + + memcpy(&MONRData->LongitudinalAccI16, p, sizeof (MONRData->LongitudinalAccI16)); + p += sizeof (MONRData->LongitudinalAccI16); + + memcpy(&MONRData->LateralAccI16, p, sizeof (MONRData->LateralAccI16)); + p += sizeof (MONRData->LateralAccI16); + + memcpy(&MONRData->DriveDirectionU8, p, sizeof (MONRData->DriveDirectionU8)); + p += sizeof (MONRData->DriveDirectionU8); + + memcpy(&MONRData->StateU8, p, sizeof (MONRData->StateU8)); + p += sizeof (MONRData->StateU8); + + memcpy(&MONRData->ReadyToArmU8, p, sizeof (MONRData->ReadyToArmU8)); + p += sizeof (MONRData->ReadyToArmU8); + + memcpy(&MONRData->ErrorStatusU8, p, sizeof (MONRData->ErrorStatusU8)); + p += sizeof (MONRData->ErrorStatusU8); + + // Footer + memcpy(&MONRData->CRC, p, sizeof (MONRData->CRC)); + p += sizeof (MONRData->CRC); + + // TODO: check on CRC + + if (debug == 1) { + LogPrint("MONR:"); + LogPrint("SyncWord = %x", MONRData->Header.SyncWordU16); + LogPrint("TransmitterId = %d", MONRData->Header.TransmitterIdU8); + LogPrint("PackageCounter = %d", MONRData->Header.MessageCounterU8); + LogPrint("AckReq = %d", MONRData->Header.AckReqProtVerU8); + LogPrint("MessageId = %d", MONRData->Header.MessageIdU16); + LogPrint("MessageLength = %d", MONRData->Header.MessageLengthU32); + LogPrint("ValueId = %d", MONRData->MonrStructValueIdU16); + LogPrint("ContentLength = %d", MONRData->MonrStructContentLengthU16); + LogPrint("GPSSOW = %d", MONRData->GPSQmsOfWeekU32); + LogPrint("XPosition = %d", MONRData->XPositionI32); + LogPrint("YPosition = %d", MONRData->YPositionI32); + LogPrint("ZPosition = %d", MONRData->ZPositionI32); + LogPrint("Heading = %d", MONRData->HeadingU16); + LogPrint("LongitudinalSpeed = %d", MONRData->LongitudinalSpeedI16); + LogPrint("LateralSpeed = %d", MONRData->LateralSpeedI16); + LogPrint("LongitudinalAcc = %d", MONRData->LongitudinalAccI16); + LogPrint("LateralAcc = %d", MONRData->LateralAccI16); + LogPrint("DriveDirection = %d", MONRData->DriveDirectionU8); + LogPrint("State = %d", MONRData->StateU8); + LogPrint("ReadyToArm = %d", MONRData->ReadyToArmU8); + LogPrint("ErrorStatus = %d", MONRData->ErrorStatusU8); + } + + return 0; } I32 ObjectControlMONRToASCII(MONRType * MONRData, GeoPosition * OriginPosition, I32 Idn, C8 * Id, - C8 * Timestamp, C8 * XPosition, C8 * YPosition, C8 * ZPosition, - C8 * LongitudinalSpeed, C8 * LateralSpeed, C8 * LongitudinalAcc, C8 * LateralAcc, - C8 * Heading, C8 * DriveDirection, C8 * ObjectState, C8 * ReadyToArm, - C8 * ErrorStatus, C8 debug) { - char Buffer[6]; - long unsigned int MonrValueU64; - unsigned int MonrValueU32; - unsigned short MonrValueU16; - unsigned char MonrValueU8; - double iLlh[3] = { 0, 0, 0 }; - double xyz[3] = { 0, 0, 0 }; - double Llh[3] = { 0, 0, 0 }; - uint64_t ConvertGPStoUTC; - - bzero(Id, SMALL_BUFFER_SIZE_1); - bzero(Timestamp, SMALL_BUFFER_SIZE_0); - bzero(XPosition, SMALL_BUFFER_SIZE_0); - bzero(YPosition, SMALL_BUFFER_SIZE_0); - bzero(ZPosition, SMALL_BUFFER_SIZE_0); - bzero(LongitudinalSpeed, SMALL_BUFFER_SIZE_0); - bzero(LateralSpeed, SMALL_BUFFER_SIZE_0); - bzero(LongitudinalAcc, SMALL_BUFFER_SIZE_0); - bzero(LateralAcc, SMALL_BUFFER_SIZE_0); - bzero(Heading, SMALL_BUFFER_SIZE_0); - bzero(DriveDirection, SMALL_BUFFER_SIZE_1); - bzero(ObjectState, SMALL_BUFFER_SIZE_1); - bzero(ReadyToArm, SMALL_BUFFER_SIZE_1); - - - if (MONRData->Header.MessageIdU16 == COMMAND_MONR_CODE) { - //Index - sprintf(Id, "%" PRIu8, (C8) Idn); - - //Timestamp - MonrValueU64 = 0; - //for(i = 0; i <= 5; i++, j++) MonrValueU64 = *(MonrData+j) | (MonrValueU64 << 8); - ConvertGPStoUTC = sprintf(Timestamp, "%" PRIu32, MONRData->GPSQmsOfWeekU32); - - if (debug && MONRData->GPSQmsOfWeekU32 % 400 == 0) { - LogMessage(LOG_LEVEL_DEBUG, "MONR = %x-%x-%x-%x-%x-%x-%d-%d-%d-%d-%d-%d-%d-%d-%d-%d-%d-%d", - MONRData->Header.MessageIdU16, - MONRData->Header.SyncWordU16, - MONRData->Header.TransmitterIdU8, - MONRData->Header.MessageCounterU8, - MONRData->Header.AckReqProtVerU8, - MONRData->Header.MessageLengthU32, - MONRData->MonrStructValueIdU16, - MONRData->MonrStructContentLengthU16, - MONRData->GPSQmsOfWeekU32, - MONRData->XPositionI32, - MONRData->YPositionI32, - MONRData->ZPositionI32, - MONRData->LongitudinalSpeedI16, - MONRData->HeadingU16, - MONRData->DriveDirectionU8, - MONRData->StateU8, MONRData->ReadyToArmU8, MONRData->ErrorStatusU8); - } - - iLlh[0] = OriginPosition->Latitude; - iLlh[1] = OriginPosition->Longitude; - iLlh[2] = OriginPosition->Altitude; - - xyz[0] = ((dbl) MONRData->XPositionI32) / 1000; - xyz[1] = ((dbl) MONRData->YPositionI32) / 1000; - xyz[2] = ((dbl) MONRData->ZPositionI32) / 1000; - - enuToLlh(iLlh, xyz, Llh); - - //XPosition - //MonrValueU32 = 0; - //for(i = 0; i <= 3; i++, j++) MonrValueU32 = *(MonrData+j) | (MonrValueU32 << 8); - //sprintf(Latitude, "%" PRIi32, (I32)(Llh[0]*1e7)); - sprintf(XPosition, "%" PRIi32, MONRData->XPositionI32); - - //YPosition - //MonrValueU32 = 0; - //for(i = 0; i <= 3; i++, j++) MonrValueU32 = *(MonrData+j) | (MonrValueU32 << 8); - //sprintf(Longitude, "%" PRIi32, (I32)(Llh[1]*1e7)); - sprintf(YPosition, "%" PRIi32, MONRData->YPositionI32); - - //ZPosition - //MonrValueU32 = 0; - //for(i = 0; i <= 3; i++, j++) MonrValueU32 = *(MonrData+j) | (MonrValueU32 << 8); - //sprintf(Altitude, "%" PRIi32, (I32)(Llh[2])); - sprintf(ZPosition, "%" PRIi32, MONRData->ZPositionI32); - - //Speed - //MonrValueU16 = 0; - //for(i = 0; i <= 1; i++, j++) MonrValueU16 = *(MonrData+j) | (MonrValueU16 << 8); - sprintf(LongitudinalSpeed, "%" PRIi16, MONRData->LongitudinalSpeedI16); - - //LatSpeed - //MonrValueU16 = 0; - //for(i = 0; i <= 1; i++, j++) MonrValueU16 = *(MonrData+j) | (MonrValueU16 << 8); - sprintf(LateralSpeed, "%" PRIi16, MONRData->LateralSpeedI16); - - //LongAcc - //MonrValueU16 = 0; - //for(i = 0; i <= 1; i++, j++) MonrValueU16 = *(MonrData+j) | (MonrValueU16 << 8); - sprintf(LongitudinalAcc, "%" PRIi16, MONRData->LongitudinalAccI16); - - //LatAcc - //MonrValueU16 = 0; - //for(i = 0; i <= 1; i++, j++) MonrValueU16 = *(MonrData+j) | (MonrValueU16 << 8); - sprintf(LateralAcc, "%" PRIi16, MONRData->LateralAccI16); - - //Heading - //MonrValueU16 = 0; - //for(i = 0; i <= 1; i++, j++) MonrValueU16 = *(MonrData+j) | (MonrValueU16 << 8); - sprintf(Heading, "%" PRIu16, MONRData->HeadingU16); - - //Driving direction - //MonrValueU8 = (unsigned char)*(MonrData+j); - //printf("D: %d\n", MonrValueU8 ); - - sprintf(DriveDirection, "%" PRIu8, MONRData->DriveDirectionU8); - - //State - //MonrValueU8 = (unsigned char)*(MonrData+j); - sprintf(ObjectState, "%" PRIu8, MONRData->StateU8); - - //ReadyToArmU8 - //MonrValueU8 = (unsigned char)*(MonrData+j); - sprintf(ReadyToArm, "%" PRIu8, MONRData->ReadyToArmU8); - - //ErrorStatusU8 - //MonrValueU8 = (unsigned char)*(MonrData+j); - sprintf(ErrorStatus, "%" PRIu8, MONRData->ErrorStatusU8); - - } - - return 0; + C8 * Timestamp, C8 * XPosition, C8 * YPosition, C8 * ZPosition, + C8 * LongitudinalSpeed, C8 * LateralSpeed, C8 * LongitudinalAcc, C8 * LateralAcc, + C8 * Heading, C8 * DriveDirection, C8 * ObjectState, C8 * ReadyToArm, + C8 * ErrorStatus, C8 debug) { + char Buffer[6]; + long unsigned int MonrValueU64; + unsigned int MonrValueU32; + unsigned short MonrValueU16; + unsigned char MonrValueU8; + double iLlh[3] = { 0, 0, 0 }; + double xyz[3] = { 0, 0, 0 }; + double Llh[3] = { 0, 0, 0 }; + uint64_t ConvertGPStoUTC; + + bzero(Id, SMALL_BUFFER_SIZE_1); + bzero(Timestamp, SMALL_BUFFER_SIZE_0); + bzero(XPosition, SMALL_BUFFER_SIZE_0); + bzero(YPosition, SMALL_BUFFER_SIZE_0); + bzero(ZPosition, SMALL_BUFFER_SIZE_0); + bzero(LongitudinalSpeed, SMALL_BUFFER_SIZE_0); + bzero(LateralSpeed, SMALL_BUFFER_SIZE_0); + bzero(LongitudinalAcc, SMALL_BUFFER_SIZE_0); + bzero(LateralAcc, SMALL_BUFFER_SIZE_0); + bzero(Heading, SMALL_BUFFER_SIZE_0); + bzero(DriveDirection, SMALL_BUFFER_SIZE_1); + bzero(ObjectState, SMALL_BUFFER_SIZE_1); + bzero(ReadyToArm, SMALL_BUFFER_SIZE_1); + + + if (MONRData->Header.MessageIdU16 == COMMAND_MONR_CODE) { + //Index + sprintf(Id, "%" PRIu8, (C8) Idn); + + //Timestamp + MonrValueU64 = 0; + //for(i = 0; i <= 5; i++, j++) MonrValueU64 = *(MonrData+j) | (MonrValueU64 << 8); + ConvertGPStoUTC = sprintf(Timestamp, "%" PRIu32, MONRData->GPSQmsOfWeekU32); + + if (debug && MONRData->GPSQmsOfWeekU32 % 400 == 0) { + LogMessage(LOG_LEVEL_DEBUG, "MONR = %x-%x-%x-%x-%x-%x-%d-%d-%d-%d-%d-%d-%d-%d-%d-%d-%d-%d", + MONRData->Header.MessageIdU16, + MONRData->Header.SyncWordU16, + MONRData->Header.TransmitterIdU8, + MONRData->Header.MessageCounterU8, + MONRData->Header.AckReqProtVerU8, + MONRData->Header.MessageLengthU32, + MONRData->MonrStructValueIdU16, + MONRData->MonrStructContentLengthU16, + MONRData->GPSQmsOfWeekU32, + MONRData->XPositionI32, + MONRData->YPositionI32, + MONRData->ZPositionI32, + MONRData->LongitudinalSpeedI16, + MONRData->HeadingU16, + MONRData->DriveDirectionU8, + MONRData->StateU8, MONRData->ReadyToArmU8, MONRData->ErrorStatusU8); + } + + iLlh[0] = OriginPosition->Latitude; + iLlh[1] = OriginPosition->Longitude; + iLlh[2] = OriginPosition->Altitude; + + xyz[0] = ((dbl) MONRData->XPositionI32) / 1000; + xyz[1] = ((dbl) MONRData->YPositionI32) / 1000; + xyz[2] = ((dbl) MONRData->ZPositionI32) / 1000; + + enuToLlh(iLlh, xyz, Llh); + + //XPosition + //MonrValueU32 = 0; + //for(i = 0; i <= 3; i++, j++) MonrValueU32 = *(MonrData+j) | (MonrValueU32 << 8); + //sprintf(Latitude, "%" PRIi32, (I32)(Llh[0]*1e7)); + sprintf(XPosition, "%" PRIi32, MONRData->XPositionI32); + + //YPosition + //MonrValueU32 = 0; + //for(i = 0; i <= 3; i++, j++) MonrValueU32 = *(MonrData+j) | (MonrValueU32 << 8); + //sprintf(Longitude, "%" PRIi32, (I32)(Llh[1]*1e7)); + sprintf(YPosition, "%" PRIi32, MONRData->YPositionI32); + + //ZPosition + //MonrValueU32 = 0; + //for(i = 0; i <= 3; i++, j++) MonrValueU32 = *(MonrData+j) | (MonrValueU32 << 8); + //sprintf(Altitude, "%" PRIi32, (I32)(Llh[2])); + sprintf(ZPosition, "%" PRIi32, MONRData->ZPositionI32); + + //Speed + //MonrValueU16 = 0; + //for(i = 0; i <= 1; i++, j++) MonrValueU16 = *(MonrData+j) | (MonrValueU16 << 8); + sprintf(LongitudinalSpeed, "%" PRIi16, MONRData->LongitudinalSpeedI16); + + //LatSpeed + //MonrValueU16 = 0; + //for(i = 0; i <= 1; i++, j++) MonrValueU16 = *(MonrData+j) | (MonrValueU16 << 8); + sprintf(LateralSpeed, "%" PRIi16, MONRData->LateralSpeedI16); + + //LongAcc + //MonrValueU16 = 0; + //for(i = 0; i <= 1; i++, j++) MonrValueU16 = *(MonrData+j) | (MonrValueU16 << 8); + sprintf(LongitudinalAcc, "%" PRIi16, MONRData->LongitudinalAccI16); + + //LatAcc + //MonrValueU16 = 0; + //for(i = 0; i <= 1; i++, j++) MonrValueU16 = *(MonrData+j) | (MonrValueU16 << 8); + sprintf(LateralAcc, "%" PRIi16, MONRData->LateralAccI16); + + //Heading + //MonrValueU16 = 0; + //for(i = 0; i <= 1; i++, j++) MonrValueU16 = *(MonrData+j) | (MonrValueU16 << 8); + sprintf(Heading, "%" PRIu16, MONRData->HeadingU16); + + //Driving direction + //MonrValueU8 = (unsigned char)*(MonrData+j); + //printf("D: %d\n", MonrValueU8 ); + + sprintf(DriveDirection, "%" PRIu8, MONRData->DriveDirectionU8); + + //State + //MonrValueU8 = (unsigned char)*(MonrData+j); + sprintf(ObjectState, "%" PRIu8, MONRData->StateU8); + + //ReadyToArmU8 + //MonrValueU8 = (unsigned char)*(MonrData+j); + sprintf(ReadyToArm, "%" PRIu8, MONRData->ReadyToArmU8); + + //ErrorStatusU8 + //MonrValueU8 = (unsigned char)*(MonrData+j); + sprintf(ErrorStatus, "%" PRIu8, MONRData->ErrorStatusU8); + + } + + return 0; } I32 ObjectControlBuildOSEMMessage(C8 * MessageBuffer, OSEMType * OSEMData, TimeType * GPSTime, C8 * Latitude, - C8 * Longitude, C8 * Altitude, U8 debug) { - I32 MessageIndex = 0, i = 0; - dbl Data; - U16 Crc = 0; - C8 *p; - U32 ISODate = 0; - - bzero(MessageBuffer, COMMAND_OSEM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); - - OSEMData->Header.SyncWordU16 = ISO_SYNC_WORD; - OSEMData->Header.TransmitterIdU8 = 0; - OSEMData->Header.MessageCounterU8 = 0; - OSEMData->Header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; - OSEMData->Header.MessageIdU16 = COMMAND_OSEM_CODE; - OSEMData->Header.MessageLengthU32 = sizeof (OSEMType) - sizeof (HeaderType) - 4; - OSEMData->LatitudeValueIdU16 = VALUE_ID_LATITUDE; - OSEMData->LatitudeContentLengthU16 = 6; - OSEMData->LatitudeI64 = (I64) ((atof((const char *)Latitude) * 1e10)); - OSEMData->LongitudeValueIdU16 = VALUE_ID_LONGITUDE; - OSEMData->LongitudeContentLengthU16 = 6; - OSEMData->LongitudeI64 = (I64) ((atof((const char *)Longitude) * 1e10)); - OSEMData->AltitudeValueIdU16 = VALUE_ID_ALTITUDE; - OSEMData->AltitudeContentLengthU16 = 4; - OSEMData->AltitudeI32 = (I32) (atof((char *)Altitude) * 1e2); - OSEMData->DateValueIdU16 = VALUE_ID_DATE_ISO8601; - OSEMData->DateContentLengthU16 = 4; - OSEMData->DateU32 = - ((U32) GPSTime->YearU16 * 10000) + ((U32) GPSTime->MonthU8 * 100) + ((U32) GPSTime->DayU8); - OSEMData->GPSWeekValueIdU16 = VALUE_ID_GPS_WEEK; - OSEMData->GPSWeekContentLengthU16 = 2; - OSEMData->GPSWeekU16 = GPSTime->GPSWeekU16; - OSEMData->GPSSOWValueIdU16 = VALUE_ID_GPS_SECOND_OF_WEEK; - OSEMData->GPSSOWContentLengthU16 = 4; - OSEMData->GPSQmsOfWeekU32 = - ((GPSTime->GPSSecondsOfWeekU32 * 1000 + GPSTime->MillisecondU16) << 2) + GPSTime->MicroSecondU16; - OSEMData->MaxWayDeviationValueIdU16 = VALUE_ID_MAX_WAY_DEVIATION; - OSEMData->MaxWayDeviationContentLengthU16 = 2; - OSEMData->MaxWayDeviationU16 = 65535; - OSEMData->MaxLateralDeviationValueIdU16 = VALUE_ID_MAX_LATERAL_DEVIATION; - OSEMData->MaxLateralDeviationContentLengthU16 = 2; - OSEMData->MaxLateralDeviationU16 = 65535; - OSEMData->MinPosAccuracyContentLengthU16 = 2; - OSEMData->MinPosAccuracyValueIdU16 = VALUE_ID_MIN_POS_ACCURACY; - OSEMData->MinPosAccuracyU16 = 65535; - - if (!GPSTime->isGPSenabled) { - OSEMData->DateU32 = UtilgetIntDateFromMS(UtilgetCurrentUTCtimeMS()); - UtilgetCurrentGPStime(&OSEMData->GPSWeekU16, &OSEMData->GPSQmsOfWeekU32); - } - - p = (C8 *) OSEMData; - for (i = 0; i < 21; i++) - *(MessageBuffer + i) = *p++; - *p++; - *p++; - for (; i < 31; i++) - *(MessageBuffer + i) = *p++; - *p++; - *p++; - for (; i < sizeof (OSEMType) - 4; i++) - *(MessageBuffer + i) = *p++; - - Crc = crc_16((const C8 *)MessageBuffer, sizeof (OSEMType) - 4); - Crc = 0; - *(MessageBuffer + i++) = (U8) (Crc); - *(MessageBuffer + i++) = (U8) (Crc >> 8); - - MessageIndex = i; - - if (debug) { - // TODO: Change to log printout when byte thingy has been implemented - printf("OSEM total length = %d bytes (header+message+footer)\n", - (int)(COMMAND_OSEM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH)); - printf("----HEADER----\n"); - for (i = 0; i < sizeof (HeaderType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----MESSAGE----\n"); - for (; i < sizeof (OSEMType) - 4; i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----FOOTER----\n"); - for (; i < MessageIndex; i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n"); - printf("Latitude = %ld\n", OSEMData->LatitudeI64); - printf("Longitude = %ld\n", OSEMData->LongitudeI64); - printf("ISODate = %d\n", OSEMData->DateU32); - } - return MessageIndex; //Total number of bytes + C8 * Longitude, C8 * Altitude, U8 debug) { + I32 MessageIndex = 0, i = 0; + dbl Data; + U16 Crc = 0; + C8 *p; + U32 ISODate = 0; + + bzero(MessageBuffer, COMMAND_OSEM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); + + OSEMData->Header.SyncWordU16 = SYNC_WORD; + OSEMData->Header.TransmitterIdU8 = 0; + OSEMData->Header.MessageCounterU8 = 0; + OSEMData->Header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; + OSEMData->Header.MessageIdU16 = COMMAND_OSEM_CODE; + OSEMData->Header.MessageLengthU32 = sizeof (OSEMType) - sizeof (HeaderType) - 4; + OSEMData->LatitudeValueIdU16 = VALUE_ID_LATITUDE; + OSEMData->LatitudeContentLengthU16 = 6; + OSEMData->LatitudeI64 = (I64) ((atof((const char *)Latitude) * 1e10)); + OSEMData->LongitudeValueIdU16 = VALUE_ID_LONGITUDE; + OSEMData->LongitudeContentLengthU16 = 6; + OSEMData->LongitudeI64 = (I64) ((atof((const char *)Longitude) * 1e10)); + OSEMData->AltitudeValueIdU16 = VALUE_ID_ALTITUDE; + OSEMData->AltitudeContentLengthU16 = 4; + OSEMData->AltitudeI32 = (I32) (atof((char *)Altitude) * 1e2); + OSEMData->DateValueIdU16 = VALUE_ID_DATE_ISO8601; + OSEMData->DateContentLengthU16 = 4; + OSEMData->DateU32 = + ((U32) GPSTime->YearU16 * 10000) + ((U32) GPSTime->MonthU8 * 100) + ((U32) GPSTime->DayU8); + OSEMData->GPSWeekValueIdU16 = VALUE_ID_GPS_WEEK; + OSEMData->GPSWeekContentLengthU16 = 2; + OSEMData->GPSWeekU16 = GPSTime->GPSWeekU16; + OSEMData->GPSSOWValueIdU16 = VALUE_ID_GPS_SECOND_OF_WEEK; + OSEMData->GPSSOWContentLengthU16 = 4; + OSEMData->GPSQmsOfWeekU32 = + ((GPSTime->GPSSecondsOfWeekU32 * 1000 + GPSTime->MillisecondU16) << 2) + GPSTime->MicroSecondU16; + OSEMData->MaxWayDeviationValueIdU16 = VALUE_ID_MAX_WAY_DEVIATION; + OSEMData->MaxWayDeviationContentLengthU16 = 2; + OSEMData->MaxWayDeviationU16 = 65535; + OSEMData->MaxLateralDeviationValueIdU16 = VALUE_ID_MAX_LATERAL_DEVIATION; + OSEMData->MaxLateralDeviationContentLengthU16 = 2; + OSEMData->MaxLateralDeviationU16 = 65535; + OSEMData->MinPosAccuracyContentLengthU16 = 2; + OSEMData->MinPosAccuracyValueIdU16 = VALUE_ID_MIN_POS_ACCURACY; + OSEMData->MinPosAccuracyU16 = 65535; + + if (!GPSTime->isGPSenabled) { + OSEMData->DateU32 = UtilgetIntDateFromMS(UtilgetCurrentUTCtimeMS()); + UtilgetCurrentGPStime(&OSEMData->GPSWeekU16, &OSEMData->GPSQmsOfWeekU32); + } + + p = (C8 *) OSEMData; + for (i = 0; i < 21; i++) + *(MessageBuffer + i) = *p++; + *p++; + *p++; + for (; i < 31; i++) + *(MessageBuffer + i) = *p++; + *p++; + *p++; + for (; i < sizeof (OSEMType) - 4; i++) + *(MessageBuffer + i) = *p++; + + Crc = crc_16((const C8 *)MessageBuffer, sizeof (OSEMType) - 4); + Crc = 0; + *(MessageBuffer + i++) = (U8) (Crc); + *(MessageBuffer + i++) = (U8) (Crc >> 8); + + MessageIndex = i; + + if (debug) { + // TODO: Change to log printout when byte thingy has been implemented + printf("OSEM total length = %d bytes (header+message+footer)\n", + (int)(COMMAND_OSEM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH)); + printf("----HEADER----\n"); + for (i = 0; i < sizeof (HeaderType); i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n----MESSAGE----\n"); + for (; i < sizeof (OSEMType) - 4; i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n----FOOTER----\n"); + for (; i < MessageIndex; i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n"); + printf("Latitude = %ld\n", OSEMData->LatitudeI64); + printf("Longitude = %ld\n", OSEMData->LongitudeI64); + printf("ISODate = %d\n", OSEMData->DateU32); + } + return MessageIndex; //Total number of bytes } int ObjectControlOSEMtoASCII(OSEMType * OSEMData, char *GPSWeek, char *GPSLatitude, char *GPSLongitude, - char *GPSAltitude) { - // what do i want? in my mq? gps week, origin in lat and long coordinates - bzero(GPSWeek, SMALL_BUFFER_SIZE_0); - bzero(GPSLatitude, SMALL_BUFFER_SIZE_0); - bzero(GPSLongitude, SMALL_BUFFER_SIZE_0); - bzero(GPSAltitude, SMALL_BUFFER_SIZE_0); + char *GPSAltitude) { + // what do i want? in my mq? gps week, origin in lat and long coordinates + bzero(GPSWeek, SMALL_BUFFER_SIZE_0); + bzero(GPSLatitude, SMALL_BUFFER_SIZE_0); + bzero(GPSLongitude, SMALL_BUFFER_SIZE_0); + bzero(GPSAltitude, SMALL_BUFFER_SIZE_0); - if (OSEMData->Header.MessageIdU16 == COMMAND_OSEM_CODE) { - sprintf(GPSWeek, "%" PRIu16, OSEMData->GPSWeekU16); + if (OSEMData->Header.MessageIdU16 == COMMAND_OSEM_CODE) { + sprintf(GPSWeek, "%" PRIu16, OSEMData->GPSWeekU16); - sprintf(GPSLatitude, "%" PRIi64, OSEMData->LatitudeI64); + sprintf(GPSLatitude, "%" PRIi64, OSEMData->LatitudeI64); - sprintf(GPSLongitude, "%" PRIi64, OSEMData->LongitudeI64); + sprintf(GPSLongitude, "%" PRIi64, OSEMData->LongitudeI64); - sprintf(GPSAltitude, "%" PRIi32, OSEMData->AltitudeI32); - } - return 0; + sprintf(GPSAltitude, "%" PRIi32, OSEMData->AltitudeI32); + } + return 0; } int ObjectControlBuildSTRTMessage(C8 * MessageBuffer, STRTType * STRTData, TimeType * GPSTime, - U32 ScenarioStartTime, U32 DelayStart, U32 * OutgoingStartTime, U8 debug) { - I32 MessageIndex = 0, i = 0; - U16 Crc = 0; - C8 *p; - - bzero(MessageBuffer, COMMAND_STRT_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); - - STRTData->Header.SyncWordU16 = ISO_SYNC_WORD; - STRTData->Header.TransmitterIdU8 = 0; - STRTData->Header.MessageCounterU8 = 0; - STRTData->Header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; - STRTData->Header.MessageIdU16 = COMMAND_STRT_CODE; - STRTData->Header.MessageLengthU32 = sizeof (STRTType) - sizeof (HeaderType); - STRTData->StartTimeValueIdU16 = VALUE_ID_GPS_SECOND_OF_WEEK; - STRTData->StartTimeContentLengthU16 = sizeof (STRTData->StartTimeU32); - STRTData->StartTimeU32 = - ((GPSTime->GPSSecondsOfWeekU32 * 1000 + (U32) TimeControlGetMillisecond(GPSTime) + - ScenarioStartTime) << 2) + GPSTime->MicroSecondU16; - STRTData->GPSWeekValueIdU16 = VALUE_ID_GPS_WEEK; - STRTData->GPSWeekContentLengthU16 = sizeof (STRTData->GPSWeekU16); - STRTData->GPSWeekU16 = GPSTime->GPSWeekU16; - // STRTData->DelayStartValueIdU16 = VALUE_ID_RELATIVE_TIME; - // STRTData->DelayStartContentLengthU16 = 4; - // STRTData->DelayStartU32 = DelayStart; - - *OutgoingStartTime = (STRTData->StartTimeU32) >> 2; - - if (!GPSTime->isGPSenabled) { - UtilgetCurrentGPStime(NULL, &STRTData->StartTimeU32); - } - - p = (char *)STRTData; - for (i = 0; i < sizeof (STRTType); i++) - *(MessageBuffer + i) = *p++; - Crc = crc_16((const unsigned char *)MessageBuffer, sizeof (STRTType)); - Crc = 0; - *(MessageBuffer + i++) = (U8) (Crc); - *(MessageBuffer + i++) = (U8) (Crc >> 8); - MessageIndex = i; - - if (debug) { - // TODO: Change to log printout when byte thingy has been implemented - printf("STRT total length = %d bytes (header+message+footer)\n", - (int)(COMMAND_STRT_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH)); - printf("----HEADER----\n"); - for (i = 0; i < sizeof (HeaderType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----MESSAGE----\n"); - for (; i < sizeof (STRTType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----FOOTER----\n"); - for (; i < MessageIndex; i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n"); - } - - return MessageIndex; //Total number of bytes + U32 ScenarioStartTime, U32 DelayStart, U32 * OutgoingStartTime, U8 debug) { + I32 MessageIndex = 0, i = 0; + U16 Crc = 0; + C8 *p; + + bzero(MessageBuffer, COMMAND_STRT_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); + + STRTData->Header.SyncWordU16 = SYNC_WORD; + STRTData->Header.TransmitterIdU8 = 0; + STRTData->Header.MessageCounterU8 = 0; + STRTData->Header.AckReqProtVerU8 = 0; + STRTData->Header.MessageIdU16 = COMMAND_STRT_CODE; + STRTData->Header.MessageLengthU32 = sizeof (STRTType) - sizeof (HeaderType); + STRTData->StartTimeValueIdU16 = VALUE_ID_GPS_SECOND_OF_WEEK; + STRTData->StartTimeContentLengthU16 = sizeof (STRTData->StartTimeU32); + STRTData->StartTimeU32 = + ((GPSTime->GPSSecondsOfWeekU32 * 1000 + (U32) TimeControlGetMillisecond(GPSTime) + + ScenarioStartTime) << 2) + GPSTime->MicroSecondU16; + STRTData->GPSWeekValueIdU16 = VALUE_ID_GPS_WEEK; + STRTData->GPSWeekContentLengthU16 = sizeof (STRTData->GPSWeekU16); + STRTData->GPSWeekU16 = GPSTime->GPSWeekU16; + // STRTData->DelayStartValueIdU16 = VALUE_ID_RELATIVE_TIME; + // STRTData->DelayStartContentLengthU16 = 4; + // STRTData->DelayStartU32 = DelayStart; + + *OutgoingStartTime = (STRTData->StartTimeU32) >> 2; + + if (!GPSTime->isGPSenabled) { + UtilgetCurrentGPStime(NULL, &STRTData->StartTimeU32); + } + + p = (char *)STRTData; + for (i = 0; i < sizeof (STRTType); i++) + *(MessageBuffer + i) = *p++; + Crc = crc_16((const unsigned char *)MessageBuffer, sizeof (STRTType)); + Crc = 0; + *(MessageBuffer + i++) = (U8) (Crc); + *(MessageBuffer + i++) = (U8) (Crc >> 8); + MessageIndex = i; + + if (debug) { + // TODO: Change to log printout when byte thingy has been implemented + printf("STRT total length = %d bytes (header+message+footer)\n", + (int)(COMMAND_STRT_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH)); + printf("----HEADER----\n"); + for (i = 0; i < sizeof (HeaderType); i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n----MESSAGE----\n"); + for (; i < sizeof (STRTType); i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n----FOOTER----\n"); + for (; i < MessageIndex; i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n"); + } + + return MessageIndex; //Total number of bytes } I32 ObjectControlBuildOSTMMessage(C8 * MessageBuffer, OSTMType * OSTMData, C8 CommandOption, U8 debug) { - I32 MessageIndex = 0, i; - U16 Crc = 0; - C8 *p; - - bzero(MessageBuffer, COMMAND_OSTM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); - - OSTMData->Header.SyncWordU16 = ISO_SYNC_WORD; - OSTMData->Header.TransmitterIdU8 = 0; - OSTMData->Header.MessageCounterU8 = 0; - OSTMData->Header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; - OSTMData->Header.MessageIdU16 = COMMAND_OSTM_CODE; - OSTMData->Header.MessageLengthU32 = sizeof (OSTMType) - sizeof (HeaderType); - OSTMData->StateValueIdU16 = VALUE_ID_STATE_CHANGE_REQUEST; - OSTMData->StateContentLengthU16 = sizeof (OSTMData->StateU8); - OSTMData->StateU8 = (U8) CommandOption; - - p = (C8 *) OSTMData; - for (i = 0; i < sizeof (OSTMType); i++) - *(MessageBuffer + i) = *p++; - Crc = crc_16((const C8 *)MessageBuffer, sizeof (OSTMType)); - Crc = 0; - *(MessageBuffer + i++) = (U8) (Crc >> 8); - *(MessageBuffer + i++) = (U8) (Crc); - MessageIndex = i; - - if (debug) { - // TODO: Change to log printout when byte thingy has been implemented - printf("OSTM total length = %d bytes (header+message+footer)\n", - (int)(COMMAND_OSTM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH)); - printf("----HEADER----\n"); - for (i = 0; i < sizeof (HeaderType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----MESSAGE----\n"); - for (; i < sizeof (OSTMType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----FOOTER----\n"); - for (; i < MessageIndex; i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n"); - } - - return MessageIndex; //Total number of bytes + I32 MessageIndex = 0, i; + U16 Crc = 0; + C8 *p; + + bzero(MessageBuffer, COMMAND_OSTM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); + + OSTMData->Header.SyncWordU16 = SYNC_WORD; + OSTMData->Header.TransmitterIdU8 = 0; + OSTMData->Header.MessageCounterU8 = 0; + OSTMData->Header.AckReqProtVerU8 = 0; + OSTMData->Header.MessageIdU16 = COMMAND_OSTM_CODE; + OSTMData->Header.MessageLengthU32 = sizeof (OSTMType) - sizeof (HeaderType); + OSTMData->StateValueIdU16 = VALUE_ID_STATE_CHANGE_REQUEST; + OSTMData->StateContentLengthU16 = sizeof (OSTMData->StateU8); + OSTMData->StateU8 = (U8) CommandOption; + + p = (C8 *) OSTMData; + for (i = 0; i < sizeof (OSTMType); i++) + *(MessageBuffer + i) = *p++; + Crc = crc_16((const C8 *)MessageBuffer, sizeof (OSTMType)); + Crc = 0; + *(MessageBuffer + i++) = (U8) (Crc >> 8); + *(MessageBuffer + i++) = (U8) (Crc); + MessageIndex = i; + + if (debug) { + // TODO: Change to log printout when byte thingy has been implemented + printf("OSTM total length = %d bytes (header+message+footer)\n", + (int)(COMMAND_OSTM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH)); + printf("----HEADER----\n"); + for (i = 0; i < sizeof (HeaderType); i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n----MESSAGE----\n"); + for (; i < sizeof (OSTMType); i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n----FOOTER----\n"); + for (; i < MessageIndex; i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n"); + } + + return MessageIndex; //Total number of bytes } I32 ObjectControlBuildHEABMessage(C8 * MessageBuffer, HEABType * HEABData, TimeType * GPSTime, U8 CCStatus, - U8 debug) { - I32 MessageIndex = 0, i; - U16 Crc = 0; - C8 *p; - - bzero(MessageBuffer, COMMAND_HEAB_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); - - HEABData->Header.SyncWordU16 = ISO_SYNC_WORD; - HEABData->Header.TransmitterIdU8 = 0; - HEABData->Header.MessageCounterU8 = 0; - HEABData->Header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; - HEABData->Header.MessageIdU16 = COMMAND_HEAB_CODE; - HEABData->Header.MessageLengthU32 = sizeof (HEABType) - sizeof (HeaderType); - HEABData->HeabStructValueIdU16 = VALUE_ID_HEAB_STRUCT; - HEABData->HeabStructContentLengthU16 = sizeof (HEABType) - sizeof (HeaderType) - - sizeof (HEABData->HeabStructValueIdU16) - sizeof (HEABData->HeabStructContentLengthU16); - HEABData->GPSQmsOfWeekU32 = - ((GPSTime->GPSSecondsOfWeekU32 * 1000 + (U32) TimeControlGetMillisecond(GPSTime)) << 2) + - GPSTime->MicroSecondU16; - HEABData->CCStatusU8 = CCStatus; - - if (!GPSTime->isGPSenabled) { - UtilgetCurrentGPStime(NULL, &HEABData->GPSQmsOfWeekU32); - } - - p = (C8 *) HEABData; - for (i = 0; i < sizeof (HEABType); i++) - *(MessageBuffer + i) = *p++; - Crc = crc_16((const C8 *)MessageBuffer, sizeof (HEABType)); - Crc = 0; - *(MessageBuffer + i++) = (U8) (Crc); - *(MessageBuffer + i++) = (U8) (Crc >> 8); - MessageIndex = i; - - if (debug) { - // TODO: Change to log printout when byte thingy has been implemented - printf("HEAB total length = %d bytes (header+message+footer)\n", - (int)(COMMAND_HEAB_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH)); - printf("----HEADER----\n"); - for (i = 0; i < sizeof (HeaderType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----MESSAGE----\n"); - for (; i < sizeof (HEABType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----FOOTER----\n"); - for (; i < MessageIndex; i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n"); - } - - return MessageIndex; //Total number of bytes + U8 debug) { + I32 MessageIndex = 0, i; + U16 Crc = 0; + C8 *p; + + bzero(MessageBuffer, COMMAND_HEAB_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); + + HEABData->Header.SyncWordU16 = SYNC_WORD; + HEABData->Header.TransmitterIdU8 = 0; + HEABData->Header.MessageCounterU8 = 0; + HEABData->Header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; + HEABData->Header.MessageIdU16 = COMMAND_HEAB_CODE; + HEABData->Header.MessageLengthU32 = sizeof (HEABType) - sizeof (HeaderType); + HEABData->HeabStructValueIdU16 = VALUE_ID_HEAB_STRUCT; + HEABData->HeabStructContentLengthU16 = sizeof (HEABType) - sizeof (HeaderType) + - sizeof (HEABData->HeabStructValueIdU16) - sizeof (HEABData->HeabStructContentLengthU16); + HEABData->GPSQmsOfWeekU32 = + ((GPSTime->GPSSecondsOfWeekU32 * 1000 + (U32) TimeControlGetMillisecond(GPSTime)) << 2) + + GPSTime->MicroSecondU16; + HEABData->CCStatusU8 = CCStatus; + + if (!GPSTime->isGPSenabled) { + UtilgetCurrentGPStime(NULL, &HEABData->GPSQmsOfWeekU32); + } + + p = (C8 *) HEABData; + for (i = 0; i < sizeof (HEABType); i++) + *(MessageBuffer + i) = *p++; + Crc = crc_16((const C8 *)MessageBuffer, sizeof (HEABType)); + Crc = 0; + *(MessageBuffer + i++) = (U8) (Crc); + *(MessageBuffer + i++) = (U8) (Crc >> 8); + MessageIndex = i; + + if (debug) { + // TODO: Change to log printout when byte thingy has been implemented + printf("HEAB total length = %d bytes (header+message+footer)\n", + (int)(COMMAND_HEAB_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH)); + printf("----HEADER----\n"); + for (i = 0; i < sizeof (HeaderType); i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n----MESSAGE----\n"); + for (; i < sizeof (HEABType); i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n----FOOTER----\n"); + for (; i < MessageIndex; i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n"); + } + + return MessageIndex; //Total number of bytes } int ObjectControlBuildLLCMMessage(char *MessageBuffer, unsigned short Speed, unsigned short Curvature, - unsigned char Mode, char debug) { - int MessageIndex = 0; + unsigned char Mode, char debug) { + int MessageIndex = 0; - bzero(MessageBuffer, COMMAND_LLCM_MESSAGE_LENGTH + COMMAND_MESSAGE_HEADER_LENGTH); + bzero(MessageBuffer, COMMAND_LLCM_MESSAGE_LENGTH + COMMAND_MESSAGE_HEADER_LENGTH); - UtilAddOneByteMessageData(MessageBuffer, COMMAND_CODE_INDEX, COMMAND_LLCM_CODE); + UtilAddOneByteMessageData(MessageBuffer, COMMAND_CODE_INDEX, COMMAND_LLCM_CODE); - MessageIndex = - UtilAddTwoBytesMessageData(MessageBuffer, MessageIndex + COMMAND_MESSAGE_HEADER_LENGTH, Speed); + MessageIndex = + UtilAddTwoBytesMessageData(MessageBuffer, MessageIndex + COMMAND_MESSAGE_HEADER_LENGTH, Speed); - MessageIndex = UtilAddTwoBytesMessageData(MessageBuffer, MessageIndex, Curvature); + MessageIndex = UtilAddTwoBytesMessageData(MessageBuffer, MessageIndex, Curvature); - MessageIndex = UtilAddOneByteMessageData(MessageBuffer, MessageIndex, Mode); + MessageIndex = UtilAddOneByteMessageData(MessageBuffer, MessageIndex, Mode); - UtilAddFourBytesMessageData(MessageBuffer, COMMAND_MESSAGE_LENGTH_INDEX, - (unsigned int)MessageIndex - COMMAND_MESSAGE_HEADER_LENGTH); + UtilAddFourBytesMessageData(MessageBuffer, COMMAND_MESSAGE_LENGTH_INDEX, + (unsigned int)MessageIndex - COMMAND_MESSAGE_HEADER_LENGTH); - if (debug) { - int i = 0; + if (debug) { + int i = 0; - LogMessage(LOG_LEVEL_DEBUG, "LLCM:"); - for (i = 0; i < MessageIndex; i++) - LogMessage(LOG_LEVEL_DEBUG, "[%d]= %x", i, (unsigned char)MessageBuffer[i]); - } + LogMessage(LOG_LEVEL_DEBUG, "LLCM:"); + for (i = 0; i < MessageIndex; i++) + LogMessage(LOG_LEVEL_DEBUG, "[%d]= %x", i, (unsigned char)MessageBuffer[i]); + } - return MessageIndex; //Total number of bytes = COMMAND_MESSAGE_HEADER_LENGTH + message data count + return MessageIndex; //Total number of bytes = COMMAND_MESSAGE_HEADER_LENGTH + message data count } I32 ObjectControlBuildSYPMMessage(C8 * MessageBuffer, SYPMType * SYPMData, U32 SyncPoint, U32 StopTime, - U8 debug) { - - I32 MessageIndex = 0, i; - U16 Crc = 0; - C8 *p; - - bzero(MessageBuffer, COMMAND_SYPM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); - - SYPMData->Header.SyncWordU16 = ISO_SYNC_WORD; - SYPMData->Header.TransmitterIdU8 = 0; - SYPMData->Header.MessageCounterU8 = 0; - SYPMData->Header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; - SYPMData->Header.MessageIdU16 = COMMAND_SYPM_CODE; - SYPMData->Header.MessageLengthU32 = sizeof (SYPMType) - sizeof (HeaderType); - SYPMData->SyncPointTimeValueIdU16 = 1; - SYPMData->SyncPointTimeContentLengthU16 = 4; - SYPMData->SyncPointTimeU32 = SyncPoint; - SYPMData->FreezeTimeValueIdU16 = 2; - SYPMData->FreezeTimeContentLengthU16 = 4; - SYPMData->FreezeTimeU32 = StopTime; - - - p = (C8 *) SYPMData; - for (i = 0; i < sizeof (SYPMType); i++) - *(MessageBuffer + i) = *p++; - Crc = crc_16((const C8 *)MessageBuffer, sizeof (SYPMType)); - Crc = 0; - *(MessageBuffer + i++) = (U8) (Crc >> 8); - *(MessageBuffer + i++) = (U8) (Crc); - MessageIndex = i; - - if (debug) { - // TODO: Change to log printout when byte thingy has been implemented - printf("SYPM total length = %d bytes (header+message+footer)\n", - (int)(COMMAND_SYPM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH)); - printf("----HEADER----\n"); - for (i = 0; i < sizeof (HeaderType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----MESSAGE----\n"); - for (; i < sizeof (SYPMType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----FOOTER----\n"); - for (; i < MessageIndex; i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n"); - } - - return MessageIndex; //Total number of bytes + U8 debug) { + + I32 MessageIndex = 0, i; + U16 Crc = 0; + C8 *p; + + bzero(MessageBuffer, COMMAND_SYPM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); + + SYPMData->Header.SyncWordU16 = SYNC_WORD; + SYPMData->Header.TransmitterIdU8 = 0; + SYPMData->Header.MessageCounterU8 = 0; + SYPMData->Header.AckReqProtVerU8 = 0; + SYPMData->Header.MessageIdU16 = COMMAND_SYPM_CODE; + SYPMData->Header.MessageLengthU32 = sizeof (SYPMType) - sizeof (HeaderType); + SYPMData->SyncPointTimeValueIdU16 = 1; + SYPMData->SyncPointTimeContentLengthU16 = 4; + SYPMData->SyncPointTimeU32 = SyncPoint; + SYPMData->FreezeTimeValueIdU16 = 2; + SYPMData->FreezeTimeContentLengthU16 = 4; + SYPMData->FreezeTimeU32 = StopTime; + + + p = (C8 *) SYPMData; + for (i = 0; i < sizeof (SYPMType); i++) + *(MessageBuffer + i) = *p++; + Crc = crc_16((const C8 *)MessageBuffer, sizeof (SYPMType)); + Crc = 0; + *(MessageBuffer + i++) = (U8) (Crc >> 8); + *(MessageBuffer + i++) = (U8) (Crc); + MessageIndex = i; + + if (debug) { + // TODO: Change to log printout when byte thingy has been implemented + printf("SYPM total length = %d bytes (header+message+footer)\n", + (int)(COMMAND_SYPM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH)); + printf("----HEADER----\n"); + for (i = 0; i < sizeof (HeaderType); i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n----MESSAGE----\n"); + for (; i < sizeof (SYPMType); i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n----FOOTER----\n"); + for (; i < MessageIndex; i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n"); + } + + return MessageIndex; //Total number of bytes } I32 ObjectControlBuildMTSPMessage(C8 * MessageBuffer, MTSPType * MTSPData, U32 SyncTimestamp, U8 debug) { - I32 MessageIndex = 0, i; - U16 Crc = 0; - C8 *p; - - bzero(MessageBuffer, COMMAND_MTSP_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); - - MTSPData->Header.SyncWordU16 = ISO_SYNC_WORD; - MTSPData->Header.TransmitterIdU8 = 0; - MTSPData->Header.MessageCounterU8 = 0; - MTSPData->Header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; - MTSPData->Header.MessageIdU16 = COMMAND_MTSP_CODE; - MTSPData->Header.MessageLengthU32 = sizeof (MTSPType) - sizeof (HeaderType); - MTSPData->EstSyncPointTimeValueIdU16 = 1; - MTSPData->EstSyncPointTimeContentLengthU16 = 4; - MTSPData->EstSyncPointTimeU32 = SyncTimestamp; - - - p = (C8 *) MTSPData; - for (i = 0; i < sizeof (MTSPType); i++) - *(MessageBuffer + i) = *p++; - Crc = crc_16((const C8 *)MessageBuffer, sizeof (MTSPType)); - Crc = 0; - *(MessageBuffer + i++) = (U8) (Crc >> 8); - *(MessageBuffer + i++) = (U8) (Crc); - MessageIndex = i; - - if (debug) { - // TODO: Change to log printout when byte thingy has been implemented - printf("MTSPData total length = %d bytes (header+message+footer)\n", - (int)(COMMAND_MTSP_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH)); - printf("----HEADER----\n"); - for (i = 0; i < sizeof (HeaderType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----MESSAGE----\n"); - for (; i < sizeof (MTSPType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----FOOTER----\n"); - for (; i < MessageIndex; i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n"); - } - - return MessageIndex; //Total number of bytes + I32 MessageIndex = 0, i; + U16 Crc = 0; + C8 *p; + + bzero(MessageBuffer, COMMAND_MTSP_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); + + MTSPData->Header.SyncWordU16 = SYNC_WORD; + MTSPData->Header.TransmitterIdU8 = 0; + MTSPData->Header.MessageCounterU8 = 0; + MTSPData->Header.AckReqProtVerU8 = 0; + MTSPData->Header.MessageIdU16 = COMMAND_MTSP_CODE; + MTSPData->Header.MessageLengthU32 = sizeof (MTSPType) - sizeof (HeaderType); + MTSPData->EstSyncPointTimeValueIdU16 = 1; + MTSPData->EstSyncPointTimeContentLengthU16 = 4; + MTSPData->EstSyncPointTimeU32 = SyncTimestamp; + + + p = (C8 *) MTSPData; + for (i = 0; i < sizeof (MTSPType); i++) + *(MessageBuffer + i) = *p++; + Crc = crc_16((const C8 *)MessageBuffer, sizeof (MTSPType)); + Crc = 0; + *(MessageBuffer + i++) = (U8) (Crc >> 8); + *(MessageBuffer + i++) = (U8) (Crc); + MessageIndex = i; + + if (debug) { + // TODO: Change to log printout when byte thingy has been implemented + printf("MTSPData total length = %d bytes (header+message+footer)\n", + (int)(COMMAND_MTSP_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH)); + printf("----HEADER----\n"); + for (i = 0; i < sizeof (HeaderType); i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n----MESSAGE----\n"); + for (; i < sizeof (MTSPType); i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n----FOOTER----\n"); + for (; i < MessageIndex; i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n"); + } + + return MessageIndex; //Total number of bytes } I32 ObjectControlBuildTRAJMessageHeader(C8 * MessageBuffer, I32 * RowCount, HeaderType * HeaderData, - TRAJInfoType * TRAJInfoData, C8 * TrajFileHeader, U8 debug) { - I32 MessageIndex = 0, i, j; - U16 Crc = 0; - C8 *p; - C8 *token; - - - if (strlen(TrajFileHeader) >= 1) { - j = 0; - token = strtok(TrajFileHeader, ";"); - while (token != NULL) { - if (j == 1) { - TRAJInfoData->TrajectoryIDValueIdU16 = VALUE_ID_TRAJECTORY_ID; - TRAJInfoData->TrajectoryIDContentLengthU16 = 2; - TRAJInfoData->TrajectoryIDU16 = atoi(token); - } - else if (j == 2) { - TRAJInfoData->TrajectoryNameValueIdU16 = VALUE_ID_TRAJECTORY_NAME; - TRAJInfoData->TrajectoryNameContentLengthU16 = 64; - bzero(TRAJInfoData->TrajectoryNameC8, 64); - strncpy(TRAJInfoData->TrajectoryNameC8, token, strlen(token)); - } - else if (j == 3) { - TRAJInfoData->TrajectoryVersionValueIdU16 = VALUE_ID_TRAJECTORY_VERSION; - TRAJInfoData->TrajectoryVersionContentLengthU16 = 2; - TRAJInfoData->TrajectoryVersionU16 = atoi(token); - } - else if (j == 4) { - *RowCount = atoi(token); - } - - j++; - token = strtok(NULL, ";"); - } - } - - TRAJInfoData->IpAddressValueIdU16 = 0xA000; - TRAJInfoData->IpAddressContentLengthU16 = 4; - TRAJInfoData->IpAddressU32 = 0; - - bzero(MessageBuffer, COMMAND_MESSAGE_HEADER_LENGTH + COMMAND_TRAJ_INFO_ROW_MESSAGE_LENGTH); - - HeaderData->SyncWordU16 = ISO_SYNC_WORD; - HeaderData->TransmitterIdU8 = 0; - HeaderData->MessageCounterU8 = 0; - HeaderData->AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; - HeaderData->MessageIdU16 = COMMAND_DOTM_CODE; - HeaderData->MessageLengthU32 = - *RowCount * COMMAND_DOTM_ROW_MESSAGE_LENGTH + COMMAND_TRAJ_INFO_ROW_MESSAGE_LENGTH; - - p = (C8 *) HeaderData; - for (i = 0; i < COMMAND_MESSAGE_HEADER_LENGTH; i++) - *(MessageBuffer + i) = *p++; - - p = (C8 *) TRAJInfoData; - for (; i < COMMAND_MESSAGE_HEADER_LENGTH + COMMAND_TRAJ_INFO_ROW_MESSAGE_LENGTH; i++) - *(MessageBuffer + i) = *p++; - - MessageIndex = i; - - - if (debug) { - // TODO: Change to log printout when byte thingy has been implemented - printf("Header + TRAJInfo total length = %d bytes\n", - (int)(COMMAND_MESSAGE_HEADER_LENGTH + COMMAND_TRAJ_INFO_ROW_MESSAGE_LENGTH)); - printf("----HEADER + TRAJInfo----\n"); - for (i = 0; i < sizeof (HeaderType) + sizeof (TRAJInfoType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n"); - printf("DOTM message total length = %d bytes.\n", (int)HeaderData->MessageLengthU32); - printf("Traj file header = %s\n", TrajFileHeader); - printf("TrajectoryID = %d\n", TRAJInfoData->TrajectoryIDU16); - printf("TrajectoryName = %s\n", TRAJInfoData->TrajectoryNameC8); - printf("TrajectoryVersion = %d\n", TRAJInfoData->TrajectoryVersionU16); - printf("RowCount = %d\n", *RowCount); - printf("IpAddress = %d\n", TRAJInfoData->IpAddressU32); - - printf("\n----MESSAGE----\n"); - } - - return MessageIndex; //Total number of bytes = COMMAND_MESSAGE_HEADER_LENGTH + TRAJInfoType * TRAJInfoData, C8 * TrajFileHeader, U8 debug) { + I32 MessageIndex = 0, i, j; + U16 Crc = 0; + C8 *p; + C8 *token; + + + if (strlen(TrajFileHeader) >= 1) { + j = 0; + token = strtok(TrajFileHeader, ";"); + while (token != NULL) { + if (j == 1) { + TRAJInfoData->TrajectoryIDValueIdU16 = VALUE_ID_TRAJECTORY_ID; + TRAJInfoData->TrajectoryIDContentLengthU16 = 2; + TRAJInfoData->TrajectoryIDU16 = atoi(token); + } + else if (j == 2) { + TRAJInfoData->TrajectoryNameValueIdU16 = VALUE_ID_TRAJECTORY_NAME; + TRAJInfoData->TrajectoryNameContentLengthU16 = 64; + bzero(TRAJInfoData->TrajectoryNameC8, 64); + strncpy(TRAJInfoData->TrajectoryNameC8, token, strlen(token)); + } + else if (j == 3) { + TRAJInfoData->TrajectoryVersionValueIdU16 = VALUE_ID_TRAJECTORY_VERSION; + TRAJInfoData->TrajectoryVersionContentLengthU16 = 2; + TRAJInfoData->TrajectoryVersionU16 = atoi(token); + } + else if (j == 4) { + *RowCount = atoi(token); + } + + j++; + token = strtok(NULL, ";"); + } + } + + TRAJInfoData->IpAddressValueIdU16 = 0xA000; + TRAJInfoData->IpAddressContentLengthU16 = 4; + TRAJInfoData->IpAddressU32 = 0; + + bzero(MessageBuffer, COMMAND_MESSAGE_HEADER_LENGTH + COMMAND_TRAJ_INFO_ROW_MESSAGE_LENGTH); + + HeaderData->SyncWordU16 = SYNC_WORD; + HeaderData->TransmitterIdU8 = 0; + HeaderData->MessageCounterU8 = 0; + HeaderData->AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; + HeaderData->MessageIdU16 = COMMAND_DOTM_CODE; + HeaderData->MessageLengthU32 = + *RowCount * COMMAND_DOTM_ROW_MESSAGE_LENGTH + COMMAND_TRAJ_INFO_ROW_MESSAGE_LENGTH; + + p = (C8 *) HeaderData; + for (i = 0; i < COMMAND_MESSAGE_HEADER_LENGTH; i++) + *(MessageBuffer + i) = *p++; + + p = (C8 *) TRAJInfoData; + for (; i < COMMAND_MESSAGE_HEADER_LENGTH + COMMAND_TRAJ_INFO_ROW_MESSAGE_LENGTH; i++) + *(MessageBuffer + i) = *p++; + + MessageIndex = i; + + + if (debug) { + // TODO: Change to log printout when byte thingy has been implemented + printf("Header + TRAJInfo total length = %d bytes\n", + (int)(COMMAND_MESSAGE_HEADER_LENGTH + COMMAND_TRAJ_INFO_ROW_MESSAGE_LENGTH)); + printf("----HEADER + TRAJInfo----\n"); + for (i = 0; i < sizeof (HeaderType) + sizeof (TRAJInfoType); i++) + printf("%x ", (unsigned char)MessageBuffer[i]); + printf("\n"); + printf("DOTM message total length = %d bytes.\n", (int)HeaderData->MessageLengthU32); + printf("Traj file header = %s\n", TrajFileHeader); + printf("TrajectoryID = %d\n", TRAJInfoData->TrajectoryIDU16); + printf("TrajectoryName = %s\n", TRAJInfoData->TrajectoryNameC8); + printf("TrajectoryVersion = %d\n", TRAJInfoData->TrajectoryVersionU16); + printf("RowCount = %d\n", *RowCount); + printf("IpAddress = %d\n", TRAJInfoData->IpAddressU32); + + printf("\n----MESSAGE----\n"); + } + + return MessageIndex; //Total number of bytes = COMMAND_MESSAGE_HEADER_LENGTH } I32 ObjectControlSendTRAJMessage(C8 * Filename, I32 * Socket, I32 RowCount, C8 * IP, U32 Port, - DOTMType * DOTMData, U8 debug) { - FILE *fd; - - // Save socket settings and set it to blocking - int retval = fcntl(*Socket, F_GETFL); - - if (retval < 0) { - LogMessage(LOG_LEVEL_ERROR, "Error getting socket options with fcntl"); - return -1; - } - int socketOptions = retval; - - retval = fcntl(*Socket, F_SETFL, socketOptions & ~O_NONBLOCK); - if (retval < 0) { - LogMessage(LOG_LEVEL_ERROR, "Error setting socket options with fcntl"); - return -1; - } - - - fd = fopen(Filename, "r"); - if (fd == NULL) { - LogMessage(LOG_LEVEL_ERROR, "Unable to open file <%s>", Filename); - return -1; - } - - UtilReadLineCntSpecChars(fd, TrajBuffer); //Read first line - int Rest = 0, i = 0, MessageLength = 0, SumMessageLength = 0, Modulo = 0, Transmissions = 0; - - Transmissions = RowCount / COMMAND_DOTM_ROWS_IN_TRANSMISSION; - Rest = RowCount % COMMAND_DOTM_ROWS_IN_TRANSMISSION; - U16 CrcU16 = 0; - - - for (i = 0; i < Transmissions; i++) { - MessageLength = - ObjectControlBuildTRAJMessage(TrajBuffer, fd, COMMAND_DOTM_ROWS_IN_TRANSMISSION, DOTMData, debug); - - if (i == Transmissions && Rest == 0) { - TrajBuffer[MessageLength] = (U8) (CrcU16); - TrajBuffer[MessageLength + 1] = (U8) (CrcU16 >> 8); - MessageLength = MessageLength + 2; - UtilSendTCPData("Object Control", TrajBuffer, MessageLength, Socket, 0); - SumMessageLength = SumMessageLength + MessageLength; - } - else { - UtilSendTCPData("Object Control", TrajBuffer, MessageLength, Socket, 0); - SumMessageLength = SumMessageLength + MessageLength; - } - - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "Transmission %d: %d bytes sent", i, MessageLength); - } - - if (Rest > 0) { - MessageLength = ObjectControlBuildTRAJMessage(TrajBuffer, fd, Rest, DOTMData, debug); - TrajBuffer[MessageLength] = (U8) (CrcU16); - TrajBuffer[MessageLength + 1] = (U8) (CrcU16 >> 8); - MessageLength = MessageLength + 2; - UtilSendTCPData("Object Control", TrajBuffer, MessageLength, Socket, 0); - SumMessageLength = SumMessageLength + MessageLength; - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "Transmission %d: %d bytes sent.\n", i, MessageLength); - } - - LogMessage(LOG_LEVEL_INFO, "%d DOTM bytes sent to %s:%d", SumMessageLength, IP, Port); - fclose(fd); - - // Reset socket settings - retval = fcntl(*Socket, F_SETFL, socketOptions); - if (retval < 0) { - LogMessage(LOG_LEVEL_ERROR, "Error setting socket options with fcntl"); - return -1; - } - - - return 0; + DOTMType * DOTMData, U8 debug) { + FILE *fd; + + // Save socket settings and set it to blocking + int retval = fcntl(*Socket, F_GETFL); + + if (retval < 0) { + LogMessage(LOG_LEVEL_ERROR, "Error getting socket options with fcntl"); + return -1; + } + int socketOptions = retval; + + retval = fcntl(*Socket, F_SETFL, socketOptions & ~O_NONBLOCK); + if (retval < 0) { + LogMessage(LOG_LEVEL_ERROR, "Error setting socket options with fcntl"); + return -1; + } + + + fd = fopen(Filename, "r"); + if (fd == NULL) { + LogMessage(LOG_LEVEL_ERROR, "Unable to open file <%s>", Filename); + return -1; + } + + UtilReadLineCntSpecChars(fd, TrajBuffer); //Read first line + int Rest = 0, i = 0, MessageLength = 0, SumMessageLength = 0, Modulo = 0, Transmissions = 0; + + Transmissions = RowCount / COMMAND_DOTM_ROWS_IN_TRANSMISSION; + Rest = RowCount % COMMAND_DOTM_ROWS_IN_TRANSMISSION; + U16 CrcU16 = 0; + + + for (i = 0; i < Transmissions; i++) { + MessageLength = + ObjectControlBuildTRAJMessage(TrajBuffer, fd, COMMAND_DOTM_ROWS_IN_TRANSMISSION, DOTMData, debug); + + if (i == Transmissions && Rest == 0) { + TrajBuffer[MessageLength] = (U8) (CrcU16); + TrajBuffer[MessageLength + 1] = (U8) (CrcU16 >> 8); + MessageLength = MessageLength + 2; + UtilSendTCPData("Object Control", TrajBuffer, MessageLength, Socket, 0); + SumMessageLength = SumMessageLength + MessageLength; + } + else { + UtilSendTCPData("Object Control", TrajBuffer, MessageLength, Socket, 0); + SumMessageLength = SumMessageLength + MessageLength; + } + + if (debug) + LogMessage(LOG_LEVEL_DEBUG, "Transmission %d: %d bytes sent", i, MessageLength); + } + + if (Rest > 0) { + MessageLength = ObjectControlBuildTRAJMessage(TrajBuffer, fd, Rest, DOTMData, debug); + TrajBuffer[MessageLength] = (U8) (CrcU16); + TrajBuffer[MessageLength + 1] = (U8) (CrcU16 >> 8); + MessageLength = MessageLength + 2; + UtilSendTCPData("Object Control", TrajBuffer, MessageLength, Socket, 0); + SumMessageLength = SumMessageLength + MessageLength; + if (debug) + LogMessage(LOG_LEVEL_DEBUG, "Transmission %d: %d bytes sent.\n", i, MessageLength); + } + + LogMessage(LOG_LEVEL_INFO, "%d DOTM bytes sent to %s:%d", SumMessageLength, IP, Port); + fclose(fd); + + // Reset socket settings + retval = fcntl(*Socket, F_SETFL, socketOptions); + if (retval < 0) { + LogMessage(LOG_LEVEL_ERROR, "Error setting socket options with fcntl"); + return -1; + } + + + return 0; } I32 ObjectControlBuildTRAJMessage(C8 * MessageBuffer, FILE * fd, I32 RowCount, DOTMType * DOTMData, U8 debug) { - I32 MessageIndex = 0; - C8 RowBuffer[100]; - C8 DataBuffer[20]; - dbl Data; - C8 *src, *p; - U16 Crc = 0; - flt curv = 0; - C8 *pc; - - bzero(MessageBuffer, COMMAND_DOTM_ROW_MESSAGE_LENGTH * RowCount); - - I32 i = 0, j = 0, n = 0; - - for (i = 0; i < RowCount; i++) { - bzero(RowBuffer, 100); - UtilReadLineCntSpecChars(fd, RowBuffer); - - //Read to ';' in row = LINE;0.00;21.239000;39.045000;0.000000;-1.240001;0.029103;0.004005;0.000000;3;ENDLINE; - //Time - src = strchr(RowBuffer, ';'); - bzero(DataBuffer, 20); - strncpy(DataBuffer, src + 1, (U64) strchr(src + 1, ';') - (U64) src - 1); - Data = atof(DataBuffer) * 1e3; - DOTMData->RelativeTimeValueIdU16 = VALUE_ID_RELATIVE_TIME; - DOTMData->RelativeTimeContentLengthU16 = 4; - DOTMData->RelativeTimeU32 = (U32) Data; - if (debug) - printf("Time DataBuffer=%s float=%3.6f\n", DataBuffer, Data); - - //x - src = strchr(src + 1, ';'); - bzero(DataBuffer, 20); - strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); - Data = atof(DataBuffer) * 1e3; - DOTMData->XPositionValueIdU16 = VALUE_ID_X_POSITION; - DOTMData->XPositionContentLengthU16 = 4; - DOTMData->XPositionI32 = (I32) Data; - if (debug) - printf("X DataBuffer=%s float=%3.6f\n", DataBuffer, Data); - - //y - src = strchr(src + 1, ';'); - bzero(DataBuffer, 20); - strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); - Data = atof(DataBuffer) * 1e3; - DOTMData->YPositionValueIdU16 = VALUE_ID_Y_POSITION; - DOTMData->YPositionContentLengthU16 = 4; - DOTMData->YPositionI32 = (I32) Data; - if (debug) - printf("Y DataBuffer=%s float=%3.6f\n", DataBuffer, Data); - - //z - src = strchr(src + 1, ';'); - bzero(DataBuffer, 20); - strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); - Data = atof(DataBuffer) * 1e3; - DOTMData->ZPositionValueIdU16 = VALUE_ID_Z_POSITION; - DOTMData->ZPositionContentLengthU16 = 4; - DOTMData->ZPositionI32 = (I32) Data; - if (debug) - printf("Z DataBuffer=%s float=%3.6f\n", DataBuffer, Data); - - //Heading - src = strchr(src + 1, ';'); - bzero(DataBuffer, 20); - strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); - Data = UtilRadToDeg(atof(DataBuffer)); - Data = 450 - Data; //Turn heading back pi/2 - while (Data < 0) - Data += 360.0; - while (Data > 360) - Data -= 360.0; - DOTMData->HeadingValueIdU16 = VALUE_ID_HEADING; - DOTMData->HeadingContentLengthU16 = 2; - DOTMData->HeadingU16 = (U16) (Data * 1e2); - if (debug) - printf("Heading DataBuffer=%s float=%3.6f\n", DataBuffer, Data); - - //Longitudinal speed - src = strchr(src + 1, ';'); - bzero(DataBuffer, 20); - strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); - Data = atof(DataBuffer) * 1e2; - DOTMData->LongitudinalSpeedValueIdU16 = VALUE_ID_LONGITUDINAL_SPEED; - DOTMData->LongitudinalSpeedContentLengthU16 = 2; - DOTMData->LongitudinalSpeedI16 = (I16) Data; - if (debug) - printf("Long speed DataBuffer=%s float=%3.6f\n", DataBuffer, Data); - - //Lateral speed - src = strchr(src + 1, ';'); - bzero(DataBuffer, 20); - strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); - Data = atof(DataBuffer) * 1e2; - DOTMData->LateralSpeedValueIdU16 = VALUE_ID_LATERAL_SPEED; - DOTMData->LateralSpeedContentLengthU16 = 2; - DOTMData->LateralSpeedI16 = (I16) Data; - if (debug) - printf("Lat speed DataBuffer=%s float=%3.6f\n", DataBuffer, Data); - - //Longitudinal acceleration - src = strchr(src + 1, ';'); - bzero(DataBuffer, 20); - strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); - Data = atof(DataBuffer) * 1e3; - DOTMData->LongitudinalAccValueIdU16 = VALUE_ID_LONGITUDINAL_ACCELERATION; - DOTMData->LongitudinalAccContentLengthU16 = 2; - DOTMData->LongitudinalAccI16 = (I16) Data; - if (debug) - printf("Long acc DataBuffer=%s float=%3.6f\n", DataBuffer, Data); - - //Lateral acceleration - src = strchr(src + 1, ';'); - bzero(DataBuffer, 20); - strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); - Data = atof(DataBuffer) * 1e3; - DOTMData->LateralAccValueIdU16 = VALUE_ID_LATERAL_ACCELERATION; - DOTMData->LateralAccContentLengthU16 = 2; - DOTMData->LateralAccI16 = (I16) Data; - if (debug) - printf("Lat accDataBuffer=%s float=%3.6f\n", DataBuffer, Data); - - //Curvature - src = strchr(src + 1, ';'); - bzero(DataBuffer, 20); - strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); - //Data = atof(DataBuffer) * 3e4; - curv = atof(DataBuffer); - pc = (C8 *) & curv; - DOTMData->CurvatureValueIdU16 = VALUE_ID_CURVATURE; - DOTMData->CurvatureContentLengthU16 = 4; - //DOTMData->CurvatureI32 = (I32) Data; - DOTMData->CurvatureI32 = pc[0]; - DOTMData->CurvatureI32 = DOTMData->CurvatureI32 | ((I32) pc[1]) << 8; - DOTMData->CurvatureI32 = DOTMData->CurvatureI32 | ((I32) pc[2]) << 16; - DOTMData->CurvatureI32 = DOTMData->CurvatureI32 | ((I32) pc[3]) << 24; - - if (debug) - printf("Curv DataBuffer=%s float=%3.6f\n", DataBuffer, Data); - - p = (C8 *) DOTMData; - for (j = 0; j < sizeof (DOTMType); j++, n++) - *(MessageBuffer + n) = *p++; - MessageIndex = n; - } - - - if (debug) { - int i = 0; - - for (i = 0; i < MessageIndex; i++) { - // TODO: Write to log when bytes thingy has been implemented - if ((unsigned char)MessageBuffer[i] >= 0 && (unsigned char)MessageBuffer[i] <= 15) - printf("0"); - printf("%x-", (unsigned char)MessageBuffer[i]); - } - printf("\n"); - } - - return MessageIndex; //Total number of bytes + I32 MessageIndex = 0; + C8 RowBuffer[100]; + C8 DataBuffer[20]; + dbl Data; + C8 *src, *p; + U16 Crc = 0; + flt curv = 0; + C8 *pc; + + bzero(MessageBuffer, COMMAND_DOTM_ROW_MESSAGE_LENGTH * RowCount); + + I32 i = 0, j = 0, n = 0; + + for (i = 0; i < RowCount; i++) { + bzero(RowBuffer, 100); + UtilReadLineCntSpecChars(fd, RowBuffer); + + //Read to ';' in row = LINE;0.00;21.239000;39.045000;0.000000;-1.240001;0.029103;0.004005;0.000000;3;ENDLINE; + //Time + src = strchr(RowBuffer, ';'); + bzero(DataBuffer, 20); + strncpy(DataBuffer, src + 1, (U64) strchr(src + 1, ';') - (U64) src - 1); + Data = atof(DataBuffer) * 1e3; + DOTMData->RelativeTimeValueIdU16 = VALUE_ID_RELATIVE_TIME; + DOTMData->RelativeTimeContentLengthU16 = 4; + DOTMData->RelativeTimeU32 = (U32) Data; + if (debug) + printf("Time DataBuffer=%s float=%3.6f\n", DataBuffer, Data); + + //x + src = strchr(src + 1, ';'); + bzero(DataBuffer, 20); + strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); + Data = atof(DataBuffer) * 1e3; + DOTMData->XPositionValueIdU16 = VALUE_ID_X_POSITION; + DOTMData->XPositionContentLengthU16 = 4; + DOTMData->XPositionI32 = (I32) Data; + if (debug) + printf("X DataBuffer=%s float=%3.6f\n", DataBuffer, Data); + + //y + src = strchr(src + 1, ';'); + bzero(DataBuffer, 20); + strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); + Data = atof(DataBuffer) * 1e3; + DOTMData->YPositionValueIdU16 = VALUE_ID_Y_POSITION; + DOTMData->YPositionContentLengthU16 = 4; + DOTMData->YPositionI32 = (I32) Data; + if (debug) + printf("Y DataBuffer=%s float=%3.6f\n", DataBuffer, Data); + + //z + src = strchr(src + 1, ';'); + bzero(DataBuffer, 20); + strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); + Data = atof(DataBuffer) * 1e3; + DOTMData->ZPositionValueIdU16 = VALUE_ID_Z_POSITION; + DOTMData->ZPositionContentLengthU16 = 4; + DOTMData->ZPositionI32 = (I32) Data; + if (debug) + printf("Z DataBuffer=%s float=%3.6f\n", DataBuffer, Data); + + //Heading + src = strchr(src + 1, ';'); + bzero(DataBuffer, 20); + strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); + Data = UtilRadToDeg(atof(DataBuffer)); + Data = 450 - Data; //Turn heading back pi/2 + while (Data < 0) + Data += 360.0; + while (Data > 360) + Data -= 360.0; + DOTMData->HeadingValueIdU16 = VALUE_ID_HEADING; + DOTMData->HeadingContentLengthU16 = 2; + DOTMData->HeadingU16 = (U16) (Data * 1e2); + if (debug) + printf("Heading DataBuffer=%s float=%3.6f\n", DataBuffer, Data); + + //Longitudinal speed + src = strchr(src + 1, ';'); + bzero(DataBuffer, 20); + strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); + Data = atof(DataBuffer) * 1e2; + DOTMData->LongitudinalSpeedValueIdU16 = VALUE_ID_LONGITUDINAL_SPEED; + DOTMData->LongitudinalSpeedContentLengthU16 = 2; + DOTMData->LongitudinalSpeedI16 = (I16) Data; + if (debug) + printf("Long speed DataBuffer=%s float=%3.6f\n", DataBuffer, Data); + + //Lateral speed + src = strchr(src + 1, ';'); + bzero(DataBuffer, 20); + strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); + Data = atof(DataBuffer) * 1e2; + DOTMData->LateralSpeedValueIdU16 = VALUE_ID_LATERAL_SPEED; + DOTMData->LateralSpeedContentLengthU16 = 2; + DOTMData->LateralSpeedI16 = (I16) Data; + if (debug) + printf("Lat speed DataBuffer=%s float=%3.6f\n", DataBuffer, Data); + + //Longitudinal acceleration + src = strchr(src + 1, ';'); + bzero(DataBuffer, 20); + strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); + Data = atof(DataBuffer) * 1e3; + DOTMData->LongitudinalAccValueIdU16 = VALUE_ID_LONGITUDINAL_ACCELERATION; + DOTMData->LongitudinalAccContentLengthU16 = 2; + DOTMData->LongitudinalAccI16 = (I16) Data; + if (debug) + printf("Long acc DataBuffer=%s float=%3.6f\n", DataBuffer, Data); + + //Lateral acceleration + src = strchr(src + 1, ';'); + bzero(DataBuffer, 20); + strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); + Data = atof(DataBuffer) * 1e3; + DOTMData->LateralAccValueIdU16 = VALUE_ID_LATERAL_ACCELERATION; + DOTMData->LateralAccContentLengthU16 = 2; + DOTMData->LateralAccI16 = (I16) Data; + if (debug) + printf("Lat accDataBuffer=%s float=%3.6f\n", DataBuffer, Data); + + //Curvature + src = strchr(src + 1, ';'); + bzero(DataBuffer, 20); + strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); + //Data = atof(DataBuffer) * 3e4; + curv = atof(DataBuffer); + pc = (C8 *) & curv; + DOTMData->CurvatureValueIdU16 = VALUE_ID_CURVATURE; + DOTMData->CurvatureContentLengthU16 = 4; + //DOTMData->CurvatureI32 = (I32) Data; + DOTMData->CurvatureI32 = pc[0]; + DOTMData->CurvatureI32 = DOTMData->CurvatureI32 | ((I32) pc[1]) << 8; + DOTMData->CurvatureI32 = DOTMData->CurvatureI32 | ((I32) pc[2]) << 16; + DOTMData->CurvatureI32 = DOTMData->CurvatureI32 | ((I32) pc[3]) << 24; + + if (debug) + printf("Curv DataBuffer=%s float=%3.6f\n", DataBuffer, Data); + + p = (C8 *) DOTMData; + for (j = 0; j < sizeof (DOTMType); j++, n++) + *(MessageBuffer + n) = *p++; + MessageIndex = n; + } + + + if (debug) { + int i = 0; + + for (i = 0; i < MessageIndex; i++) { + // TODO: Write to log when bytes thingy has been implemented + if ((unsigned char)MessageBuffer[i] >= 0 && (unsigned char)MessageBuffer[i] <= 15) + printf("0"); + printf("%x-", (unsigned char)MessageBuffer[i]); + } + printf("\n"); + } + + return MessageIndex; //Total number of bytes } I32 ObjectControlBuildASPMessage(C8 * MessageBuffer, ASPType * ASPData, U8 debug) { - I32 MessageIndex = 0, i; - C8 *p; - - bzero(MessageBuffer, ASP_MESSAGE_LENGTH); - p = (C8 *) ASPData; - for (i = 0; i < sizeof (ASPType); i++) - *(MessageBuffer + i) = *p++; - MessageIndex = i; - - if (debug) { - // TODO: Write to log when bytes thingy has been implemented - printf("ASP total length = %d bytes \n", (int)(ASP_MESSAGE_LENGTH)); - printf("\n----MESSAGE----\n"); - for (i = 0; i < sizeof (ASPType); i++) - printf("%x ", (C8) MessageBuffer[i]); - printf("\n"); - } - - return MessageIndex; //Total number of bytes + I32 MessageIndex = 0, i; + C8 *p; + + bzero(MessageBuffer, ASP_MESSAGE_LENGTH); + p = (C8 *) ASPData; + for (i = 0; i < sizeof (ASPType); i++) + *(MessageBuffer + i) = *p++; + MessageIndex = i; + + if (debug) { + // TODO: Write to log when bytes thingy has been implemented + printf("ASP total length = %d bytes \n", (int)(ASP_MESSAGE_LENGTH)); + printf("\n----MESSAGE----\n"); + for (i = 0; i < sizeof (ASPType); i++) + printf("%x ", (C8) MessageBuffer[i]); + printf("\n"); + } + + return MessageIndex; //Total number of bytes } @@ -2287,100 +2289,100 @@ I32 ObjectControlBuildASPMessage(C8 * MessageBuffer, ASPType * ASPData, U8 debug * \return Length of sent message */ I32 ObjectControlSendACCMMessage(ACCMData * ACCM, I32 * socket, U8 debug) { - ACCMType isoACCM; - C8 messageBuffer[sizeof (isoACCM)]; - C8 *ptr = messageBuffer; - U32 messageSize = 0; + ACCMType isoACCM; + C8 messageBuffer[sizeof (isoACCM)]; + C8 *ptr = messageBuffer; + U32 messageSize = 0; - ObjectControlBuildACCMMessage(ACCM, &isoACCM, debug); + ObjectControlBuildACCMMessage(ACCM, &isoACCM, debug); - // Copy ACCM header to send buffer - memcpy(ptr, &isoACCM.header.SyncWordU16, sizeof (isoACCM.header.SyncWordU16)); - ptr += sizeof (isoACCM.header.SyncWordU16); + // Copy ACCM header to send buffer + memcpy(ptr, &isoACCM.header.SyncWordU16, sizeof (isoACCM.header.SyncWordU16)); + ptr += sizeof (isoACCM.header.SyncWordU16); - memcpy(ptr, &isoACCM.header.TransmitterIdU8, sizeof (isoACCM.header.TransmitterIdU8)); - ptr += sizeof (isoACCM.header.TransmitterIdU8); + memcpy(ptr, &isoACCM.header.TransmitterIdU8, sizeof (isoACCM.header.TransmitterIdU8)); + ptr += sizeof (isoACCM.header.TransmitterIdU8); - memcpy(ptr, &isoACCM.header.MessageCounterU8, sizeof (isoACCM.header.MessageCounterU8)); - ptr += sizeof (isoACCM.header.MessageCounterU8); + memcpy(ptr, &isoACCM.header.MessageCounterU8, sizeof (isoACCM.header.MessageCounterU8)); + ptr += sizeof (isoACCM.header.MessageCounterU8); - memcpy(ptr, &isoACCM.header.AckReqProtVerU8, sizeof (isoACCM.header.AckReqProtVerU8)); - ptr += sizeof (isoACCM.header.AckReqProtVerU8); + memcpy(ptr, &isoACCM.header.AckReqProtVerU8, sizeof (isoACCM.header.AckReqProtVerU8)); + ptr += sizeof (isoACCM.header.AckReqProtVerU8); - memcpy(ptr, &isoACCM.header.MessageIdU16, sizeof (isoACCM.header.MessageIdU16)); - ptr += sizeof (isoACCM.header.MessageIdU16); + memcpy(ptr, &isoACCM.header.MessageIdU16, sizeof (isoACCM.header.MessageIdU16)); + ptr += sizeof (isoACCM.header.MessageIdU16); - memcpy(ptr, &isoACCM.header.MessageLengthU32, sizeof (isoACCM.header.MessageLengthU32)); - ptr += sizeof (isoACCM.header.MessageLengthU32); + memcpy(ptr, &isoACCM.header.MessageLengthU32, sizeof (isoACCM.header.MessageLengthU32)); + ptr += sizeof (isoACCM.header.MessageLengthU32); - // Copy ACCM action ID to send buffer - memcpy(ptr, &isoACCM.actionIDValueID, sizeof (isoACCM.actionIDValueID)); - ptr += sizeof (isoACCM.actionIDValueID); + // Copy ACCM action ID to send buffer + memcpy(ptr, &isoACCM.actionIDValueID, sizeof (isoACCM.actionIDValueID)); + ptr += sizeof (isoACCM.actionIDValueID); - memcpy(ptr, &isoACCM.actionIDContentLength, sizeof (isoACCM.actionIDContentLength)); - ptr += sizeof (isoACCM.actionIDContentLength); + memcpy(ptr, &isoACCM.actionIDContentLength, sizeof (isoACCM.actionIDContentLength)); + ptr += sizeof (isoACCM.actionIDContentLength); - memcpy(ptr, &isoACCM.actionID, sizeof (isoACCM.actionID)); - ptr += sizeof (isoACCM.actionID); + memcpy(ptr, &isoACCM.actionID, sizeof (isoACCM.actionID)); + ptr += sizeof (isoACCM.actionID); - // Copy ACCM action type to send buffer - memcpy(ptr, &isoACCM.actionTypeValueID, sizeof (isoACCM.actionTypeValueID)); - ptr += sizeof (isoACCM.actionTypeValueID); + // Copy ACCM action type to send buffer + memcpy(ptr, &isoACCM.actionTypeValueID, sizeof (isoACCM.actionTypeValueID)); + ptr += sizeof (isoACCM.actionTypeValueID); - memcpy(ptr, &isoACCM.actionTypeContentLength, sizeof (isoACCM.actionTypeContentLength)); - ptr += sizeof (isoACCM.actionTypeContentLength); + memcpy(ptr, &isoACCM.actionTypeContentLength, sizeof (isoACCM.actionTypeContentLength)); + ptr += sizeof (isoACCM.actionTypeContentLength); - memcpy(ptr, &isoACCM.actionType, sizeof (isoACCM.actionType)); - ptr += sizeof (isoACCM.actionType); + memcpy(ptr, &isoACCM.actionType, sizeof (isoACCM.actionType)); + ptr += sizeof (isoACCM.actionType); - // Copy ACCM action parameter 1 to send buffer - memcpy(ptr, &isoACCM.actionTypeParameter1ValueID, sizeof (isoACCM.actionTypeParameter1ValueID)); - ptr += sizeof (isoACCM.actionTypeParameter1ValueID); + // Copy ACCM action parameter 1 to send buffer + memcpy(ptr, &isoACCM.actionTypeParameter1ValueID, sizeof (isoACCM.actionTypeParameter1ValueID)); + ptr += sizeof (isoACCM.actionTypeParameter1ValueID); - memcpy(ptr, &isoACCM.actionTypeParameter1ContentLength, - sizeof (isoACCM.actionTypeParameter1ContentLength)); - ptr += sizeof (isoACCM.actionTypeParameter1ContentLength); + memcpy(ptr, &isoACCM.actionTypeParameter1ContentLength, + sizeof (isoACCM.actionTypeParameter1ContentLength)); + ptr += sizeof (isoACCM.actionTypeParameter1ContentLength); - memcpy(ptr, &isoACCM.actionTypeParameter1, sizeof (isoACCM.actionTypeParameter1)); - ptr += sizeof (isoACCM.actionTypeParameter1); + memcpy(ptr, &isoACCM.actionTypeParameter1, sizeof (isoACCM.actionTypeParameter1)); + ptr += sizeof (isoACCM.actionTypeParameter1); - // Copy ACCM action parameter 2 to send buffer - memcpy(ptr, &isoACCM.actionTypeParameter2ValueID, sizeof (isoACCM.actionTypeParameter2ValueID)); - ptr += sizeof (isoACCM.actionTypeParameter2ValueID); + // Copy ACCM action parameter 2 to send buffer + memcpy(ptr, &isoACCM.actionTypeParameter2ValueID, sizeof (isoACCM.actionTypeParameter2ValueID)); + ptr += sizeof (isoACCM.actionTypeParameter2ValueID); - memcpy(ptr, &isoACCM.actionTypeParameter2ContentLength, - sizeof (isoACCM.actionTypeParameter2ContentLength)); - ptr += sizeof (isoACCM.actionTypeParameter2ContentLength); + memcpy(ptr, &isoACCM.actionTypeParameter2ContentLength, + sizeof (isoACCM.actionTypeParameter2ContentLength)); + ptr += sizeof (isoACCM.actionTypeParameter2ContentLength); - memcpy(ptr, &isoACCM.actionTypeParameter2, sizeof (isoACCM.actionTypeParameter2)); - ptr += sizeof (isoACCM.actionTypeParameter2); + memcpy(ptr, &isoACCM.actionTypeParameter2, sizeof (isoACCM.actionTypeParameter2)); + ptr += sizeof (isoACCM.actionTypeParameter2); - // Copy ACCM action parameter 3 to send buffer - memcpy(ptr, &isoACCM.actionTypeParameter3ValueID, sizeof (isoACCM.actionTypeParameter3ValueID)); - ptr += sizeof (isoACCM.actionTypeParameter3ValueID); + // Copy ACCM action parameter 3 to send buffer + memcpy(ptr, &isoACCM.actionTypeParameter3ValueID, sizeof (isoACCM.actionTypeParameter3ValueID)); + ptr += sizeof (isoACCM.actionTypeParameter3ValueID); - memcpy(ptr, &isoACCM.actionTypeParameter3ContentLength, - sizeof (isoACCM.actionTypeParameter3ContentLength)); - ptr += sizeof (isoACCM.actionTypeParameter3ContentLength); + memcpy(ptr, &isoACCM.actionTypeParameter3ContentLength, + sizeof (isoACCM.actionTypeParameter3ContentLength)); + ptr += sizeof (isoACCM.actionTypeParameter3ContentLength); - memcpy(ptr, &isoACCM.actionTypeParameter3, sizeof (isoACCM.actionTypeParameter3)); - ptr += sizeof (isoACCM.actionTypeParameter3); + memcpy(ptr, &isoACCM.actionTypeParameter3, sizeof (isoACCM.actionTypeParameter3)); + ptr += sizeof (isoACCM.actionTypeParameter3); - // Copy ACCM footer to send buffer - memcpy(ptr, &isoACCM.footer.Crc, sizeof (isoACCM.footer.Crc)); - ptr += sizeof (isoACCM.footer.Crc); + // Copy ACCM footer to send buffer + memcpy(ptr, &isoACCM.footer.Crc, sizeof (isoACCM.footer.Crc)); + ptr += sizeof (isoACCM.footer.Crc); - if (ptr > messageBuffer) - messageSize = (U32) (ptr - messageBuffer); + if (ptr > messageBuffer) + messageSize = (U32) (ptr - messageBuffer); - if (messageSize - sizeof (isoACCM.header) - sizeof (isoACCM.footer) != isoACCM.header.MessageLengthU32) - LogMessage(LOG_LEVEL_WARNING, "ACCM message sent with invalid message length"); + if (messageSize - sizeof (isoACCM.header) - sizeof (isoACCM.footer) != isoACCM.header.MessageLengthU32) + LogMessage(LOG_LEVEL_WARNING, "ACCM message sent with invalid message length"); - UtilSendTCPData(MODULE_NAME, messageBuffer, (I32) messageSize, socket, debug); + UtilSendTCPData(MODULE_NAME, messageBuffer, (I32) messageSize, socket, debug); - return (I32) messageSize; + return (I32) messageSize; } /*! @@ -2391,100 +2393,100 @@ I32 ObjectControlSendACCMMessage(ACCMData * ACCM, I32 * socket, U8 debug) { * \return Length of sent message */ I32 ObjectControlSendTRCMMessage(TRCMData * TRCM, I32 * socket, U8 debug) { - TRCMType isoTRCM; - C8 messageBuffer[sizeof (isoTRCM)]; - C8 *ptr = messageBuffer; - U32 messageSize = 0; + TRCMType isoTRCM; + C8 messageBuffer[sizeof (isoTRCM)]; + C8 *ptr = messageBuffer; + U32 messageSize = 0; - ObjectControlBuildTRCMMessage(TRCM, &isoTRCM, debug); + ObjectControlBuildTRCMMessage(TRCM, &isoTRCM, debug); - // Copy TRCM header to send buffer - memcpy(ptr, &isoTRCM.header.SyncWordU16, sizeof (isoTRCM.header.SyncWordU16)); - ptr += sizeof (isoTRCM.header.SyncWordU16); + // Copy TRCM header to send buffer + memcpy(ptr, &isoTRCM.header.SyncWordU16, sizeof (isoTRCM.header.SyncWordU16)); + ptr += sizeof (isoTRCM.header.SyncWordU16); - memcpy(ptr, &isoTRCM.header.TransmitterIdU8, sizeof (isoTRCM.header.TransmitterIdU8)); - ptr += sizeof (isoTRCM.header.TransmitterIdU8); + memcpy(ptr, &isoTRCM.header.TransmitterIdU8, sizeof (isoTRCM.header.TransmitterIdU8)); + ptr += sizeof (isoTRCM.header.TransmitterIdU8); - memcpy(ptr, &isoTRCM.header.MessageCounterU8, sizeof (isoTRCM.header.MessageCounterU8)); - ptr += sizeof (isoTRCM.header.MessageCounterU8); + memcpy(ptr, &isoTRCM.header.MessageCounterU8, sizeof (isoTRCM.header.MessageCounterU8)); + ptr += sizeof (isoTRCM.header.MessageCounterU8); - memcpy(ptr, &isoTRCM.header.AckReqProtVerU8, sizeof (isoTRCM.header.AckReqProtVerU8)); - ptr += sizeof (isoTRCM.header.AckReqProtVerU8); + memcpy(ptr, &isoTRCM.header.AckReqProtVerU8, sizeof (isoTRCM.header.AckReqProtVerU8)); + ptr += sizeof (isoTRCM.header.AckReqProtVerU8); - memcpy(ptr, &isoTRCM.header.MessageIdU16, sizeof (isoTRCM.header.MessageIdU16)); - ptr += sizeof (isoTRCM.header.MessageIdU16); + memcpy(ptr, &isoTRCM.header.MessageIdU16, sizeof (isoTRCM.header.MessageIdU16)); + ptr += sizeof (isoTRCM.header.MessageIdU16); - memcpy(ptr, &isoTRCM.header.MessageLengthU32, sizeof (isoTRCM.header.MessageLengthU32)); - ptr += sizeof (isoTRCM.header.MessageLengthU32); + memcpy(ptr, &isoTRCM.header.MessageLengthU32, sizeof (isoTRCM.header.MessageLengthU32)); + ptr += sizeof (isoTRCM.header.MessageLengthU32); - // Copy TRCM trigger ID to send buffer - memcpy(ptr, &isoTRCM.triggerIDValueID, sizeof (isoTRCM.triggerIDValueID)); - ptr += sizeof (isoTRCM.triggerIDValueID); + // Copy TRCM trigger ID to send buffer + memcpy(ptr, &isoTRCM.triggerIDValueID, sizeof (isoTRCM.triggerIDValueID)); + ptr += sizeof (isoTRCM.triggerIDValueID); - memcpy(ptr, &isoTRCM.triggerIDContentLength, sizeof (isoTRCM.triggerIDContentLength)); - ptr += sizeof (isoTRCM.triggerIDContentLength); + memcpy(ptr, &isoTRCM.triggerIDContentLength, sizeof (isoTRCM.triggerIDContentLength)); + ptr += sizeof (isoTRCM.triggerIDContentLength); - memcpy(ptr, &isoTRCM.triggerID, sizeof (isoTRCM.triggerID)); - ptr += sizeof (isoTRCM.triggerID); + memcpy(ptr, &isoTRCM.triggerID, sizeof (isoTRCM.triggerID)); + ptr += sizeof (isoTRCM.triggerID); - // Copy TRCM trigger type to send buffer - memcpy(ptr, &isoTRCM.triggerTypeValueID, sizeof (isoTRCM.triggerTypeValueID)); - ptr += sizeof (isoTRCM.triggerTypeValueID); + // Copy TRCM trigger type to send buffer + memcpy(ptr, &isoTRCM.triggerTypeValueID, sizeof (isoTRCM.triggerTypeValueID)); + ptr += sizeof (isoTRCM.triggerTypeValueID); - memcpy(ptr, &isoTRCM.triggerTypeContentLength, sizeof (isoTRCM.triggerTypeContentLength)); - ptr += sizeof (isoTRCM.triggerTypeContentLength); + memcpy(ptr, &isoTRCM.triggerTypeContentLength, sizeof (isoTRCM.triggerTypeContentLength)); + ptr += sizeof (isoTRCM.triggerTypeContentLength); - memcpy(ptr, &isoTRCM.triggerType, sizeof (isoTRCM.triggerType)); - ptr += sizeof (isoTRCM.triggerType); + memcpy(ptr, &isoTRCM.triggerType, sizeof (isoTRCM.triggerType)); + ptr += sizeof (isoTRCM.triggerType); - // Copy TRCM trigger parameter 1 to send buffer - memcpy(ptr, &isoTRCM.triggerTypeParameter1ValueID, sizeof (isoTRCM.triggerTypeParameter1ValueID)); - ptr += sizeof (isoTRCM.triggerTypeParameter1ValueID); + // Copy TRCM trigger parameter 1 to send buffer + memcpy(ptr, &isoTRCM.triggerTypeParameter1ValueID, sizeof (isoTRCM.triggerTypeParameter1ValueID)); + ptr += sizeof (isoTRCM.triggerTypeParameter1ValueID); - memcpy(ptr, &isoTRCM.triggerTypeParameter1ContentLength, - sizeof (isoTRCM.triggerTypeParameter1ContentLength)); - ptr += sizeof (isoTRCM.triggerTypeParameter1ContentLength); + memcpy(ptr, &isoTRCM.triggerTypeParameter1ContentLength, + sizeof (isoTRCM.triggerTypeParameter1ContentLength)); + ptr += sizeof (isoTRCM.triggerTypeParameter1ContentLength); - memcpy(ptr, &isoTRCM.triggerTypeParameter1, sizeof (isoTRCM.triggerTypeParameter1)); - ptr += sizeof (isoTRCM.triggerTypeParameter1); + memcpy(ptr, &isoTRCM.triggerTypeParameter1, sizeof (isoTRCM.triggerTypeParameter1)); + ptr += sizeof (isoTRCM.triggerTypeParameter1); - // Copy TRCM trigger parameter 2 to send buffer - memcpy(ptr, &isoTRCM.triggerTypeParameter2ValueID, sizeof (isoTRCM.triggerTypeParameter2ValueID)); - ptr += sizeof (isoTRCM.triggerTypeParameter2ValueID); + // Copy TRCM trigger parameter 2 to send buffer + memcpy(ptr, &isoTRCM.triggerTypeParameter2ValueID, sizeof (isoTRCM.triggerTypeParameter2ValueID)); + ptr += sizeof (isoTRCM.triggerTypeParameter2ValueID); - memcpy(ptr, &isoTRCM.triggerTypeParameter2ContentLength, - sizeof (isoTRCM.triggerTypeParameter2ContentLength)); - ptr += sizeof (isoTRCM.triggerTypeParameter2ContentLength); + memcpy(ptr, &isoTRCM.triggerTypeParameter2ContentLength, + sizeof (isoTRCM.triggerTypeParameter2ContentLength)); + ptr += sizeof (isoTRCM.triggerTypeParameter2ContentLength); - memcpy(ptr, &isoTRCM.triggerTypeParameter2, sizeof (isoTRCM.triggerTypeParameter2)); - ptr += sizeof (isoTRCM.triggerTypeParameter2); + memcpy(ptr, &isoTRCM.triggerTypeParameter2, sizeof (isoTRCM.triggerTypeParameter2)); + ptr += sizeof (isoTRCM.triggerTypeParameter2); - // Copy TRCM trigger parameter 3 to send buffer - memcpy(ptr, &isoTRCM.triggerTypeParameter3ValueID, sizeof (isoTRCM.triggerTypeParameter3ValueID)); - ptr += sizeof (isoTRCM.triggerTypeParameter3ValueID); + // Copy TRCM trigger parameter 3 to send buffer + memcpy(ptr, &isoTRCM.triggerTypeParameter3ValueID, sizeof (isoTRCM.triggerTypeParameter3ValueID)); + ptr += sizeof (isoTRCM.triggerTypeParameter3ValueID); - memcpy(ptr, &isoTRCM.triggerTypeParameter3ContentLength, - sizeof (isoTRCM.triggerTypeParameter3ContentLength)); - ptr += sizeof (isoTRCM.triggerTypeParameter3ContentLength); + memcpy(ptr, &isoTRCM.triggerTypeParameter3ContentLength, + sizeof (isoTRCM.triggerTypeParameter3ContentLength)); + ptr += sizeof (isoTRCM.triggerTypeParameter3ContentLength); - memcpy(ptr, &isoTRCM.triggerTypeParameter3, sizeof (isoTRCM.triggerTypeParameter3)); - ptr += sizeof (isoTRCM.triggerTypeParameter3); + memcpy(ptr, &isoTRCM.triggerTypeParameter3, sizeof (isoTRCM.triggerTypeParameter3)); + ptr += sizeof (isoTRCM.triggerTypeParameter3); - // Copy TRCM footer to send buffer - memcpy(ptr, &isoTRCM.footer.Crc, sizeof (isoTRCM.footer.Crc)); - ptr += sizeof (isoTRCM.footer.Crc); + // Copy TRCM footer to send buffer + memcpy(ptr, &isoTRCM.footer.Crc, sizeof (isoTRCM.footer.Crc)); + ptr += sizeof (isoTRCM.footer.Crc); - if (ptr > messageBuffer) - messageSize = (U32) (ptr - messageBuffer); + if (ptr > messageBuffer) + messageSize = (U32) (ptr - messageBuffer); - if (messageSize - sizeof (isoTRCM.header) - sizeof (isoTRCM.footer) != isoTRCM.header.MessageLengthU32) - LogMessage(LOG_LEVEL_WARNING, "TRCM message sent with invalid message length"); + if (messageSize - sizeof (isoTRCM.header) - sizeof (isoTRCM.footer) != isoTRCM.header.MessageLengthU32) + LogMessage(LOG_LEVEL_WARNING, "TRCM message sent with invalid message length"); - UtilSendTCPData(MODULE_NAME, messageBuffer, (I32) messageSize, socket, 0); + UtilSendTCPData(MODULE_NAME, messageBuffer, (I32) messageSize, socket, 0); - return (I32) messageSize; + return (I32) messageSize; } /*! @@ -2495,67 +2497,67 @@ I32 ObjectControlSendTRCMMessage(TRCMData * TRCM, I32 * socket, U8 debug) { * \return Length of sent message */ I32 ObjectControlSendEXACMessage(EXACData * EXAC, I32 * socket, U8 debug) { - EXACType isoEXAC; - C8 messageBuffer[sizeof (isoEXAC)]; - C8 *ptr = messageBuffer; - U32 messageSize = 0; + EXACType isoEXAC; + C8 messageBuffer[sizeof (isoEXAC)]; + C8 *ptr = messageBuffer; + U32 messageSize = 0; - ObjectControlBuildEXACMessage(EXAC, &isoEXAC, debug); + ObjectControlBuildEXACMessage(EXAC, &isoEXAC, debug); - // Copy EXAC header to send buffer - memcpy(ptr, &isoEXAC.header.SyncWordU16, sizeof (isoEXAC.header.SyncWordU16)); - ptr += sizeof (isoEXAC.header.SyncWordU16); + // Copy EXAC header to send buffer + memcpy(ptr, &isoEXAC.header.SyncWordU16, sizeof (isoEXAC.header.SyncWordU16)); + ptr += sizeof (isoEXAC.header.SyncWordU16); - memcpy(ptr, &isoEXAC.header.TransmitterIdU8, sizeof (isoEXAC.header.TransmitterIdU8)); - ptr += sizeof (isoEXAC.header.TransmitterIdU8); + memcpy(ptr, &isoEXAC.header.TransmitterIdU8, sizeof (isoEXAC.header.TransmitterIdU8)); + ptr += sizeof (isoEXAC.header.TransmitterIdU8); - memcpy(ptr, &isoEXAC.header.MessageCounterU8, sizeof (isoEXAC.header.MessageCounterU8)); - ptr += sizeof (isoEXAC.header.MessageCounterU8); + memcpy(ptr, &isoEXAC.header.MessageCounterU8, sizeof (isoEXAC.header.MessageCounterU8)); + ptr += sizeof (isoEXAC.header.MessageCounterU8); - memcpy(ptr, &isoEXAC.header.AckReqProtVerU8, sizeof (isoEXAC.header.AckReqProtVerU8)); - ptr += sizeof (isoEXAC.header.AckReqProtVerU8); + memcpy(ptr, &isoEXAC.header.AckReqProtVerU8, sizeof (isoEXAC.header.AckReqProtVerU8)); + ptr += sizeof (isoEXAC.header.AckReqProtVerU8); - memcpy(ptr, &isoEXAC.header.MessageIdU16, sizeof (isoEXAC.header.MessageIdU16)); - ptr += sizeof (isoEXAC.header.MessageIdU16); + memcpy(ptr, &isoEXAC.header.MessageIdU16, sizeof (isoEXAC.header.MessageIdU16)); + ptr += sizeof (isoEXAC.header.MessageIdU16); - memcpy(ptr, &isoEXAC.header.MessageLengthU32, sizeof (isoEXAC.header.MessageLengthU32)); - ptr += sizeof (isoEXAC.header.MessageLengthU32); + memcpy(ptr, &isoEXAC.header.MessageLengthU32, sizeof (isoEXAC.header.MessageLengthU32)); + ptr += sizeof (isoEXAC.header.MessageLengthU32); - // Copy EXAC action ID to send buffer - memcpy(ptr, &isoEXAC.actionIDValueID, sizeof (isoEXAC.actionIDValueID)); - ptr += sizeof (isoEXAC.actionIDValueID); + // Copy EXAC action ID to send buffer + memcpy(ptr, &isoEXAC.actionIDValueID, sizeof (isoEXAC.actionIDValueID)); + ptr += sizeof (isoEXAC.actionIDValueID); - memcpy(ptr, &isoEXAC.actionIDContentLength, sizeof (isoEXAC.actionIDContentLength)); - ptr += sizeof (isoEXAC.actionIDContentLength); + memcpy(ptr, &isoEXAC.actionIDContentLength, sizeof (isoEXAC.actionIDContentLength)); + ptr += sizeof (isoEXAC.actionIDContentLength); - memcpy(ptr, &isoEXAC.actionID, sizeof (isoEXAC.actionID)); - ptr += sizeof (isoEXAC.actionID); + memcpy(ptr, &isoEXAC.actionID, sizeof (isoEXAC.actionID)); + ptr += sizeof (isoEXAC.actionID); - // Copy EXAC action execution time to send buffer - memcpy(ptr, &isoEXAC.executionTime_qmsoWValueID, sizeof (isoEXAC.executionTime_qmsoWValueID)); - ptr += sizeof (isoEXAC.executionTime_qmsoWValueID); + // Copy EXAC action execution time to send buffer + memcpy(ptr, &isoEXAC.executionTime_qmsoWValueID, sizeof (isoEXAC.executionTime_qmsoWValueID)); + ptr += sizeof (isoEXAC.executionTime_qmsoWValueID); - memcpy(ptr, &isoEXAC.executionTime_qmsoWContentLength, sizeof (isoEXAC.executionTime_qmsoWContentLength)); - ptr += sizeof (isoEXAC.executionTime_qmsoWContentLength); + memcpy(ptr, &isoEXAC.executionTime_qmsoWContentLength, sizeof (isoEXAC.executionTime_qmsoWContentLength)); + ptr += sizeof (isoEXAC.executionTime_qmsoWContentLength); - memcpy(ptr, &isoEXAC.executionTime_qmsoW, sizeof (isoEXAC.executionTime_qmsoW)); - ptr += sizeof (isoEXAC.executionTime_qmsoW); + memcpy(ptr, &isoEXAC.executionTime_qmsoW, sizeof (isoEXAC.executionTime_qmsoW)); + ptr += sizeof (isoEXAC.executionTime_qmsoW); - // Copy EXAC footer to send buffer - memcpy(ptr, &isoEXAC.footer.Crc, sizeof (isoEXAC.footer.Crc)); - ptr += sizeof (isoEXAC.footer.Crc); + // Copy EXAC footer to send buffer + memcpy(ptr, &isoEXAC.footer.Crc, sizeof (isoEXAC.footer.Crc)); + ptr += sizeof (isoEXAC.footer.Crc); - if (ptr > messageBuffer) - messageSize = (U32) (ptr - messageBuffer); + if (ptr > messageBuffer) + messageSize = (U32) (ptr - messageBuffer); - if (messageSize - sizeof (isoEXAC.header) - sizeof (isoEXAC.footer) != isoEXAC.header.MessageLengthU32) - LogMessage(LOG_LEVEL_WARNING, "EXAC message sent with invalid message length"); + if (messageSize - sizeof (isoEXAC.header) - sizeof (isoEXAC.footer) != isoEXAC.header.MessageLengthU32) + LogMessage(LOG_LEVEL_WARNING, "EXAC message sent with invalid message length"); - UtilSendTCPData(MODULE_NAME, messageBuffer, (I32) messageSize, socket, 0); + UtilSendTCPData(MODULE_NAME, messageBuffer, (I32) messageSize, socket, 0); - return (I32) messageSize; + return (I32) messageSize; } @@ -2567,69 +2569,69 @@ I32 ObjectControlSendEXACMessage(EXACData * EXAC, I32 * socket, U8 debug) { * \return Byte size of ACCM struct */ I32 ObjectControlBuildACCMMessage(ACCMData * mqACCMData, ACCMType * ACCM, U8 debug) { - // Header - ACCM->header.SyncWordU16 = ISO_SYNC_WORD; - ACCM->header.TransmitterIdU8 = 0; - ACCM->header.MessageCounterU8 = 0; - ACCM->header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; - ACCM->header.MessageIdU16 = COMMAND_ACCM_CODE; - ACCM->header.MessageLengthU32 = sizeof (ACCMType) - sizeof (HeaderType) - sizeof (FooterType); - - // Data fields - ACCM->actionID = mqACCMData->actionID; - ACCM->actionType = mqACCMData->actionType; - ACCM->actionTypeParameter1 = mqACCMData->actionTypeParameter1; - ACCM->actionTypeParameter2 = mqACCMData->actionTypeParameter2; - ACCM->actionTypeParameter3 = mqACCMData->actionTypeParameter3; - - // Value ID fields - ACCM->actionIDValueID = VALUE_ID_ACTION_ID; - ACCM->actionTypeValueID = VALUE_ID_ACTION_TYPE; - ACCM->actionTypeParameter1ValueID = VALUE_ID_ACTION_TYPE_PARAM1; - ACCM->actionTypeParameter2ValueID = VALUE_ID_ACTION_TYPE_PARAM2; - ACCM->actionTypeParameter3ValueID = VALUE_ID_ACTION_TYPE_PARAM3; - - // Content length fields - ACCM->actionIDContentLength = sizeof (ACCM->actionID); - ACCM->actionTypeContentLength = sizeof (ACCM->actionType); - ACCM->actionTypeParameter1ContentLength = sizeof (ACCM->actionTypeParameter1); - ACCM->actionTypeParameter2ContentLength = sizeof (ACCM->actionTypeParameter2); - ACCM->actionTypeParameter3ContentLength = sizeof (ACCM->actionTypeParameter3); - - // Header content length - ACCM->header.MessageLengthU32 = sizeof (ACCM->actionID) + sizeof (ACCM->actionType) - + sizeof (ACCM->actionTypeParameter1) + sizeof (ACCM->actionTypeParameter2) + - sizeof (ACCM->actionTypeParameter3) - + sizeof (ACCM->actionIDValueID) + sizeof (ACCM->actionTypeValueID) - + sizeof (ACCM->actionTypeParameter1ValueID) + sizeof (ACCM->actionTypeParameter1ValueID) + - sizeof (ACCM->actionTypeParameter3ValueID) - + sizeof (ACCM->actionIDContentLength) + sizeof (ACCM->actionTypeContentLength) - + sizeof (ACCM->actionTypeParameter1ContentLength) + - sizeof (ACCM->actionTypeParameter1ContentLength) + sizeof (ACCM->actionTypeParameter3ContentLength); - - - // Footer (TODO) - ACCM->footer.Crc = 0; - - U32 messageLen = - ACCM->header.MessageLengthU32 + sizeof (ACCM->footer.Crc) + sizeof (ACCM->header.SyncWordU16) + - sizeof (ACCM->header.MessageIdU16) + sizeof (ACCM->header.AckReqProtVerU8) + - sizeof (ACCM->header.TransmitterIdU8) + sizeof (ACCM->header.MessageCounterU8) + - sizeof (ACCM->header.MessageLengthU32); - - if (debug) { - LogPrint - ("ACCM (%u bytes):\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x", - messageLen, ACCM->actionIDValueID, ACCM->actionIDContentLength, ACCM->actionID, - ACCM->actionTypeValueID, ACCM->actionTypeContentLength, ACCM->actionType, - ACCM->actionTypeParameter1ValueID, ACCM->actionTypeParameter1ContentLength, - ACCM->actionTypeParameter1, ACCM->actionTypeParameter2ValueID, - ACCM->actionTypeParameter2ContentLength, ACCM->actionTypeParameter2, - ACCM->actionTypeParameter3ValueID, ACCM->actionTypeParameter3ContentLength, - ACCM->actionTypeParameter3); - } - - return (I32) messageLen; + // Header + ACCM->header.SyncWordU16 = SYNC_WORD; + ACCM->header.TransmitterIdU8 = 0; + ACCM->header.MessageCounterU8 = 0; + ACCM->header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; + ACCM->header.MessageIdU16 = COMMAND_ACCM_CODE; + ACCM->header.MessageLengthU32 = sizeof (ACCMType) - sizeof (HeaderType) - sizeof (FooterType); + + // Data fields + ACCM->actionID = mqACCMData->actionID; + ACCM->actionType = mqACCMData->actionType; + ACCM->actionTypeParameter1 = mqACCMData->actionTypeParameter1; + ACCM->actionTypeParameter2 = mqACCMData->actionTypeParameter2; + ACCM->actionTypeParameter3 = mqACCMData->actionTypeParameter3; + + // Value ID fields + ACCM->actionIDValueID = VALUE_ID_ACTION_ID; + ACCM->actionTypeValueID = VALUE_ID_ACTION_TYPE; + ACCM->actionTypeParameter1ValueID = VALUE_ID_ACTION_TYPE_PARAM1; + ACCM->actionTypeParameter2ValueID = VALUE_ID_ACTION_TYPE_PARAM2; + ACCM->actionTypeParameter3ValueID = VALUE_ID_ACTION_TYPE_PARAM3; + + // Content length fields + ACCM->actionIDContentLength = sizeof (ACCM->actionID); + ACCM->actionTypeContentLength = sizeof (ACCM->actionType); + ACCM->actionTypeParameter1ContentLength = sizeof (ACCM->actionTypeParameter1); + ACCM->actionTypeParameter2ContentLength = sizeof (ACCM->actionTypeParameter2); + ACCM->actionTypeParameter3ContentLength = sizeof (ACCM->actionTypeParameter3); + + // Header content length + ACCM->header.MessageLengthU32 = sizeof (ACCM->actionID) + sizeof (ACCM->actionType) + + sizeof (ACCM->actionTypeParameter1) + sizeof (ACCM->actionTypeParameter2) + + sizeof (ACCM->actionTypeParameter3) + + sizeof (ACCM->actionIDValueID) + sizeof (ACCM->actionTypeValueID) + + sizeof (ACCM->actionTypeParameter1ValueID) + sizeof (ACCM->actionTypeParameter1ValueID) + + sizeof (ACCM->actionTypeParameter3ValueID) + + sizeof (ACCM->actionIDContentLength) + sizeof (ACCM->actionTypeContentLength) + + sizeof (ACCM->actionTypeParameter1ContentLength) + + sizeof (ACCM->actionTypeParameter1ContentLength) + sizeof (ACCM->actionTypeParameter3ContentLength); + + + // Footer (TODO) + ACCM->footer.Crc = 0; + + U32 messageLen = + ACCM->header.MessageLengthU32 + sizeof (ACCM->footer.Crc) + sizeof (ACCM->header.SyncWordU16) + + sizeof (ACCM->header.MessageIdU16) + sizeof (ACCM->header.AckReqProtVerU8) + + sizeof (ACCM->header.TransmitterIdU8) + sizeof (ACCM->header.MessageCounterU8) + + sizeof (ACCM->header.MessageLengthU32); + + if (debug) { + LogPrint + ("ACCM (%u bytes):\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x", + messageLen, ACCM->actionIDValueID, ACCM->actionIDContentLength, ACCM->actionID, + ACCM->actionTypeValueID, ACCM->actionTypeContentLength, ACCM->actionType, + ACCM->actionTypeParameter1ValueID, ACCM->actionTypeParameter1ContentLength, + ACCM->actionTypeParameter1, ACCM->actionTypeParameter2ValueID, + ACCM->actionTypeParameter2ContentLength, ACCM->actionTypeParameter2, + ACCM->actionTypeParameter3ValueID, ACCM->actionTypeParameter3ContentLength, + ACCM->actionTypeParameter3); + } + + return (I32) messageLen; } /*! @@ -2640,53 +2642,53 @@ I32 ObjectControlBuildACCMMessage(ACCMData * mqACCMData, ACCMType * ACCM, U8 deb * \return Byte size of EXAC struct */ I32 ObjectControlBuildEXACMessage(EXACData * mqEXACData, EXACType * EXAC, U8 debug) { - // TODO: Make system time follow GPS time (better) or pass the GSD pointer into here somehow - struct timeval systemTime; + // TODO: Make system time follow GPS time (better) or pass the GSD pointer into here somehow + struct timeval systemTime; - TimeSetToCurrentSystemTime(&systemTime); + TimeSetToCurrentSystemTime(&systemTime); - // Header - EXAC->header.SyncWordU16 = ISO_SYNC_WORD; - EXAC->header.TransmitterIdU8 = 0; - EXAC->header.MessageCounterU8 = 0; - EXAC->header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; - EXAC->header.MessageIdU16 = COMMAND_EXAC_CODE; + // Header + EXAC->header.SyncWordU16 = SYNC_WORD; + EXAC->header.TransmitterIdU8 = 0; + EXAC->header.MessageCounterU8 = 0; + EXAC->header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; + EXAC->header.MessageIdU16 = COMMAND_EXAC_CODE; - // Data fields - EXAC->actionID = mqEXACData->actionID; - EXAC->executionTime_qmsoW = mqEXACData->executionTime_qmsoW; + // Data fields + EXAC->actionID = mqEXACData->actionID; + EXAC->executionTime_qmsoW = mqEXACData->executionTime_qmsoW; - // Value ID fields - EXAC->actionIDValueID = VALUE_ID_ACTION_ID; - EXAC->executionTime_qmsoWValueID = VALUE_ID_ACTION_EXECUTE_TIME; + // Value ID fields + EXAC->actionIDValueID = VALUE_ID_ACTION_ID; + EXAC->executionTime_qmsoWValueID = VALUE_ID_ACTION_EXECUTE_TIME; - // Content length fields - EXAC->actionIDContentLength = sizeof (EXAC->actionID); - EXAC->executionTime_qmsoWContentLength = sizeof (EXAC->executionTime_qmsoW); + // Content length fields + EXAC->actionIDContentLength = sizeof (EXAC->actionID); + EXAC->executionTime_qmsoWContentLength = sizeof (EXAC->executionTime_qmsoW); - // Header message length - EXAC->header.MessageLengthU32 = sizeof (EXAC->actionID) + sizeof (EXAC->executionTime_qmsoW) - + sizeof (EXAC->actionIDValueID) + sizeof (EXAC->executionTime_qmsoWValueID) - + sizeof (EXAC->actionIDContentLength) + sizeof (EXAC->executionTime_qmsoWContentLength); + // Header message length + EXAC->header.MessageLengthU32 = sizeof (EXAC->actionID) + sizeof (EXAC->executionTime_qmsoW) + + sizeof (EXAC->actionIDValueID) + sizeof (EXAC->executionTime_qmsoWValueID) + + sizeof (EXAC->actionIDContentLength) + sizeof (EXAC->executionTime_qmsoWContentLength); - // Footer (TODO) - EXAC->footer.Crc = 0; + // Footer (TODO) + EXAC->footer.Crc = 0; - U32 messageLen = - EXAC->header.MessageLengthU32 + sizeof (EXAC->footer.Crc) + sizeof (EXAC->header.SyncWordU16) + - sizeof (EXAC->header.MessageIdU16) + sizeof (EXAC->header.AckReqProtVerU8) + - sizeof (EXAC->header.TransmitterIdU8) + sizeof (EXAC->header.MessageCounterU8) + - sizeof (EXAC->header.MessageLengthU32); + U32 messageLen = + EXAC->header.MessageLengthU32 + sizeof (EXAC->footer.Crc) + sizeof (EXAC->header.SyncWordU16) + + sizeof (EXAC->header.MessageIdU16) + sizeof (EXAC->header.AckReqProtVerU8) + + sizeof (EXAC->header.TransmitterIdU8) + sizeof (EXAC->header.MessageCounterU8) + + sizeof (EXAC->header.MessageLengthU32); - if (debug) { - LogPrint("EXAC (%u bytes):\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x", messageLen, - EXAC->actionIDValueID, EXAC->actionIDContentLength, EXAC->actionID, - EXAC->executionTime_qmsoWValueID, EXAC->executionTime_qmsoWContentLength, - EXAC->executionTime_qmsoW); - } + if (debug) { + LogPrint("EXAC (%u bytes):\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x", messageLen, + EXAC->actionIDValueID, EXAC->actionIDContentLength, EXAC->actionID, + EXAC->executionTime_qmsoWValueID, EXAC->executionTime_qmsoWContentLength, + EXAC->executionTime_qmsoW); + } - return (I32) messageLen; + return (I32) messageLen; } /*! @@ -2697,625 +2699,625 @@ I32 ObjectControlBuildEXACMessage(EXACData * mqEXACData, EXACType * EXAC, U8 deb * \return Byte size of TRCM struct */ I32 ObjectControlBuildTRCMMessage(TRCMData * mqTRCMData, TRCMType * TRCM, U8 debug) { - // Header - TRCM->header.SyncWordU16 = ISO_SYNC_WORD; - TRCM->header.TransmitterIdU8 = 0; - TRCM->header.MessageCounterU8 = 0; - TRCM->header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; - TRCM->header.MessageIdU16 = COMMAND_TRCM_CODE; - - - // Data fields - TRCM->triggerID = mqTRCMData->triggerID; - TRCM->triggerType = mqTRCMData->triggerType; - TRCM->triggerTypeParameter1 = mqTRCMData->triggerTypeParameter1; - TRCM->triggerTypeParameter2 = mqTRCMData->triggerTypeParameter2; - TRCM->triggerTypeParameter3 = mqTRCMData->triggerTypeParameter3; - - // Value ID fields - TRCM->triggerIDValueID = VALUE_ID_TRIGGER_ID; - TRCM->triggerIDValueID = VALUE_ID_TRIGGER_TYPE; - TRCM->triggerTypeParameter1ValueID = VALUE_ID_TRIGGER_TYPE_PARAM1; - TRCM->triggerTypeParameter2ValueID = VALUE_ID_TRIGGER_TYPE_PARAM2; - TRCM->triggerTypeParameter3ValueID = VALUE_ID_TRIGGER_TYPE_PARAM3; - - // Content length fields - TRCM->triggerIDContentLength = sizeof (TRCM->triggerID); - TRCM->triggerTypeContentLength = sizeof (TRCM->triggerType); - TRCM->triggerTypeParameter1ContentLength = sizeof (TRCM->triggerTypeParameter1); - TRCM->triggerTypeParameter2ContentLength = sizeof (TRCM->triggerTypeParameter2); - TRCM->triggerTypeParameter3ContentLength = sizeof (TRCM->triggerTypeParameter3); - - - // Message length in header - TRCM->header.MessageLengthU32 = sizeof (TRCM->triggerID) + sizeof (TRCM->triggerType) - + sizeof (TRCM->triggerTypeParameter1) + sizeof (TRCM->triggerTypeParameter2) + - sizeof (TRCM->triggerTypeParameter3) - + sizeof (TRCM->triggerIDValueID) + sizeof (TRCM->triggerTypeValueID) - + sizeof (TRCM->triggerTypeParameter1ValueID) + sizeof (TRCM->triggerTypeParameter1ValueID) + - sizeof (TRCM->triggerTypeParameter3ValueID) - + sizeof (TRCM->triggerIDContentLength) + sizeof (TRCM->triggerTypeContentLength) - + sizeof (TRCM->triggerTypeParameter1ContentLength) + - sizeof (TRCM->triggerTypeParameter1ContentLength) + sizeof (TRCM->triggerTypeParameter3ContentLength); - - - // Footer (TODO) - TRCM->footer.Crc = 0; - - U32 messageLen = - TRCM->header.MessageLengthU32 + sizeof (TRCM->footer.Crc) + sizeof (TRCM->header.SyncWordU16) + - sizeof (TRCM->header.MessageIdU16) + sizeof (TRCM->header.AckReqProtVerU8) + - sizeof (TRCM->header.TransmitterIdU8) + sizeof (TRCM->header.MessageCounterU8) + - sizeof (TRCM->header.MessageLengthU32); - if (debug) { - LogPrint - ("TRCM (%u bytes):\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x", - messageLen, TRCM->triggerIDValueID, TRCM->triggerIDContentLength, TRCM->triggerID, - TRCM->triggerTypeValueID, TRCM->triggerTypeContentLength, TRCM->triggerType, - TRCM->triggerTypeParameter1ValueID, TRCM->triggerTypeParameter1ContentLength, - TRCM->triggerTypeParameter1, TRCM->triggerTypeParameter2ValueID, - TRCM->triggerTypeParameter2ContentLength, TRCM->triggerTypeParameter2, - TRCM->triggerTypeParameter3ValueID, TRCM->triggerTypeParameter3ContentLength, - TRCM->triggerTypeParameter3); - } - - return (I32) messageLen; + // Header + TRCM->header.SyncWordU16 = SYNC_WORD; + TRCM->header.TransmitterIdU8 = 0; + TRCM->header.MessageCounterU8 = 0; + TRCM->header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; + TRCM->header.MessageIdU16 = COMMAND_TRCM_CODE; + + + // Data fields + TRCM->triggerID = mqTRCMData->triggerID; + TRCM->triggerType = mqTRCMData->triggerType; + TRCM->triggerTypeParameter1 = mqTRCMData->triggerTypeParameter1; + TRCM->triggerTypeParameter2 = mqTRCMData->triggerTypeParameter2; + TRCM->triggerTypeParameter3 = mqTRCMData->triggerTypeParameter3; + + // Value ID fields + TRCM->triggerIDValueID = VALUE_ID_TRIGGER_ID; + TRCM->triggerIDValueID = VALUE_ID_TRIGGER_TYPE; + TRCM->triggerTypeParameter1ValueID = VALUE_ID_TRIGGER_TYPE_PARAM1; + TRCM->triggerTypeParameter2ValueID = VALUE_ID_TRIGGER_TYPE_PARAM2; + TRCM->triggerTypeParameter3ValueID = VALUE_ID_TRIGGER_TYPE_PARAM3; + + // Content length fields + TRCM->triggerIDContentLength = sizeof (TRCM->triggerID); + TRCM->triggerTypeContentLength = sizeof (TRCM->triggerType); + TRCM->triggerTypeParameter1ContentLength = sizeof (TRCM->triggerTypeParameter1); + TRCM->triggerTypeParameter2ContentLength = sizeof (TRCM->triggerTypeParameter2); + TRCM->triggerTypeParameter3ContentLength = sizeof (TRCM->triggerTypeParameter3); + + + // Message length in header + TRCM->header.MessageLengthU32 = sizeof (TRCM->triggerID) + sizeof (TRCM->triggerType) + + sizeof (TRCM->triggerTypeParameter1) + sizeof (TRCM->triggerTypeParameter2) + + sizeof (TRCM->triggerTypeParameter3) + + sizeof (TRCM->triggerIDValueID) + sizeof (TRCM->triggerTypeValueID) + + sizeof (TRCM->triggerTypeParameter1ValueID) + sizeof (TRCM->triggerTypeParameter1ValueID) + + sizeof (TRCM->triggerTypeParameter3ValueID) + + sizeof (TRCM->triggerIDContentLength) + sizeof (TRCM->triggerTypeContentLength) + + sizeof (TRCM->triggerTypeParameter1ContentLength) + + sizeof (TRCM->triggerTypeParameter1ContentLength) + sizeof (TRCM->triggerTypeParameter3ContentLength); + + + // Footer (TODO) + TRCM->footer.Crc = 0; + + U32 messageLen = + TRCM->header.MessageLengthU32 + sizeof (TRCM->footer.Crc) + sizeof (TRCM->header.SyncWordU16) + + sizeof (TRCM->header.MessageIdU16) + sizeof (TRCM->header.AckReqProtVerU8) + + sizeof (TRCM->header.TransmitterIdU8) + sizeof (TRCM->header.MessageCounterU8) + + sizeof (TRCM->header.MessageLengthU32); + if (debug) { + LogPrint + ("TRCM (%u bytes):\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x\n\t%#x-%#x-%#x", + messageLen, TRCM->triggerIDValueID, TRCM->triggerIDContentLength, TRCM->triggerID, + TRCM->triggerTypeValueID, TRCM->triggerTypeContentLength, TRCM->triggerType, + TRCM->triggerTypeParameter1ValueID, TRCM->triggerTypeParameter1ContentLength, + TRCM->triggerTypeParameter1, TRCM->triggerTypeParameter2ValueID, + TRCM->triggerTypeParameter2ContentLength, TRCM->triggerTypeParameter2, + TRCM->triggerTypeParameter3ValueID, TRCM->triggerTypeParameter3ContentLength, + TRCM->triggerTypeParameter3); + } + + return (I32) messageLen; } I32 ObjectControlSendDTMMessage(C8 * DTMData, I32 * Socket, I32 RowCount, C8 * IP, U32 Port, - DOTMType * DOTMData, U8 debug) { + DOTMType * DOTMData, U8 debug) { - U32 Rest = 0, i = 0, MessageLength = 0, SumMessageLength = 0, Modulo = 0, Transmissions = 0; - U16 CrcU16 = 0; + U32 Rest = 0, i = 0, MessageLength = 0, SumMessageLength = 0, Modulo = 0, Transmissions = 0; + U16 CrcU16 = 0; - MessageLength = - ObjectControlBuildDTMMessage(TrajBuffer, DTMData, COMMAND_DOTM_ROWS_IN_TRANSMISSION, DOTMData, 0); + MessageLength = + ObjectControlBuildDTMMessage(TrajBuffer, DTMData, COMMAND_DOTM_ROWS_IN_TRANSMISSION, DOTMData, 0); - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "Transmission %d: %d bytes sent", i, MessageLength); + if (debug) + LogMessage(LOG_LEVEL_DEBUG, "Transmission %d: %d bytes sent", i, MessageLength); - LogMessage(LOG_LEVEL_INFO, "%d DTM bytes sent to %s:%d", SumMessageLength, IP, Port); + LogMessage(LOG_LEVEL_INFO, "%d DTM bytes sent to %s:%d", SumMessageLength, IP, Port); - return 0; + return 0; } I32 ObjectControlBuildDTMMessage(C8 * MessageBuffer, C8 * DTMData, I32 RowCount, DOTMType * DOTMData, - U8 debug) { - I32 MessageIndex = 0; - U32 Data; - C8 *src, *p; - U16 Crc = 0; - - bzero(MessageBuffer, COMMAND_DOTM_ROW_MESSAGE_LENGTH * RowCount); - - I32 i = 0, j = 0, n = 0; - - for (i = 0; i < RowCount; i++) { - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "DOTM row:"); - //Time - Data = 0; - Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 3); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 2); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 1); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 0); - DOTMData->RelativeTimeValueIdU16 = VALUE_ID_RELATIVE_TIME; - DOTMData->RelativeTimeContentLengthU16 = 4; - DOTMData->RelativeTimeU32 = SwapU32((U32) Data); - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "Time=%d", DOTMData->RelativeTimeU32); - - //x - Data = 0; - Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 7); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 6); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 5); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 4); - DOTMData->XPositionValueIdU16 = VALUE_ID_X_POSITION; - DOTMData->XPositionContentLengthU16 = 4; - DOTMData->XPositionI32 = SwapI32((I32) Data); - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "X=%d", DOTMData->XPositionI32); - - //y - Data = 0; - Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 11); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 10); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 9); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 8); - DOTMData->YPositionValueIdU16 = VALUE_ID_Y_POSITION; - DOTMData->YPositionContentLengthU16 = 4; - DOTMData->YPositionI32 = SwapI32((I32) Data); - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "Y=%d", DOTMData->YPositionI32); - - //z - Data = 0; - Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 15); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 14); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 13); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 12); - DOTMData->ZPositionValueIdU16 = VALUE_ID_Z_POSITION; - DOTMData->ZPositionContentLengthU16 = 4; - DOTMData->ZPositionI32 = SwapI32((I32) Data); - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "Z=%d", DOTMData->ZPositionI32); - - //Heading - Data = 0; - Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 17); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 16); - //Data = UtilRadToDeg(Data); - //Data = 4500 - Data; //Turn heading back pi/2 - //while(Data<0) Data+=360.0; - //while(Data>3600) Data-=360.0; - DOTMData->HeadingValueIdU16 = VALUE_ID_HEADING; - DOTMData->HeadingContentLengthU16 = 2; - DOTMData->HeadingU16 = SwapU16((U16) (Data)); - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "Heading=%d", DOTMData->HeadingU16); - - //Longitudinal speed - Data = 0; - Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 19); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 18); - DOTMData->LongitudinalSpeedValueIdU16 = VALUE_ID_LONGITUDINAL_SPEED; - DOTMData->LongitudinalSpeedContentLengthU16 = 2; - DOTMData->LongitudinalSpeedI16 = SwapI16((I16) Data); - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "LongitudinalSpeedI16=%d", DOTMData->LongitudinalSpeedI16); - - //Lateral speed - Data = 0; - Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 21); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 20); - DOTMData->LateralSpeedValueIdU16 = VALUE_ID_LATERAL_SPEED; - DOTMData->LateralSpeedContentLengthU16 = 2; - DOTMData->LateralSpeedI16 = SwapI16((I16) Data); - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "LateralSpeedI16=%d", DOTMData->LateralSpeedI16); - - //Longitudinal acceleration - Data = 0; - Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 23); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 22); - DOTMData->LongitudinalAccValueIdU16 = VALUE_ID_LONGITUDINAL_ACCELERATION; - DOTMData->LongitudinalAccContentLengthU16 = 2; - DOTMData->LongitudinalAccI16 = SwapI16((I16) Data); - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "LongitudinalAccI16=%d", DOTMData->LongitudinalAccI16); - - //Lateral acceleration - Data = 0; - Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 25); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 24); - DOTMData->LateralAccValueIdU16 = VALUE_ID_LATERAL_ACCELERATION; - DOTMData->LateralAccContentLengthU16 = 2; - DOTMData->LateralAccI16 = SwapI16((I16) Data); - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "LateralAccI16=%d", DOTMData->LateralAccI16); - - //Curvature - Data = 0; - Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 29); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 28); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 27); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 26); - DOTMData->CurvatureValueIdU16 = VALUE_ID_CURVATURE; - DOTMData->CurvatureContentLengthU16 = 4; - DOTMData->CurvatureI32 = SwapI32((I32) Data); - if (debug) - printf("CurvatureI32=%d \n", DOTMData->CurvatureI32); - - p = (C8 *) DOTMData; - for (j = 0; j < sizeof (DOTMType); j++, n++) - *(MessageBuffer + n) = *p++; - MessageIndex = n; - } - - - Crc = crc_16((const C8 *)MessageBuffer, sizeof (DOTMType)); - Crc = 0; - *(MessageBuffer + MessageIndex++) = (U8) (Crc); - *(MessageBuffer + MessageIndex++) = (U8) (Crc >> 8); - - - if (debug) { - int i = 0; - - for (i = 0; i < MessageIndex; i++) { - // TODO: Write to log when bytes thingy has been implemented - if ((unsigned char)MessageBuffer[i] >= 0 && (unsigned char)MessageBuffer[i] <= 15) - printf("0"); - printf("%x-", (unsigned char)MessageBuffer[i]); - } - printf("\n"); - } - - return MessageIndex; //Total number of bytes + U8 debug) { + I32 MessageIndex = 0; + U32 Data; + C8 *src, *p; + U16 Crc = 0; + + bzero(MessageBuffer, COMMAND_DOTM_ROW_MESSAGE_LENGTH * RowCount); + + I32 i = 0, j = 0, n = 0; + + for (i = 0; i < RowCount; i++) { + if (debug) + LogMessage(LOG_LEVEL_DEBUG, "DOTM row:"); + //Time + Data = 0; + Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 3); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 2); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 1); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 0); + DOTMData->RelativeTimeValueIdU16 = VALUE_ID_RELATIVE_TIME; + DOTMData->RelativeTimeContentLengthU16 = 4; + DOTMData->RelativeTimeU32 = SwapU32((U32) Data); + if (debug) + LogMessage(LOG_LEVEL_DEBUG, "Time=%d", DOTMData->RelativeTimeU32); + + //x + Data = 0; + Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 7); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 6); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 5); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 4); + DOTMData->XPositionValueIdU16 = VALUE_ID_X_POSITION; + DOTMData->XPositionContentLengthU16 = 4; + DOTMData->XPositionI32 = SwapI32((I32) Data); + if (debug) + LogMessage(LOG_LEVEL_DEBUG, "X=%d", DOTMData->XPositionI32); + + //y + Data = 0; + Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 11); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 10); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 9); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 8); + DOTMData->YPositionValueIdU16 = VALUE_ID_Y_POSITION; + DOTMData->YPositionContentLengthU16 = 4; + DOTMData->YPositionI32 = SwapI32((I32) Data); + if (debug) + LogMessage(LOG_LEVEL_DEBUG, "Y=%d", DOTMData->YPositionI32); + + //z + Data = 0; + Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 15); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 14); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 13); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 12); + DOTMData->ZPositionValueIdU16 = VALUE_ID_Z_POSITION; + DOTMData->ZPositionContentLengthU16 = 4; + DOTMData->ZPositionI32 = SwapI32((I32) Data); + if (debug) + LogMessage(LOG_LEVEL_DEBUG, "Z=%d", DOTMData->ZPositionI32); + + //Heading + Data = 0; + Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 17); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 16); + //Data = UtilRadToDeg(Data); + //Data = 4500 - Data; //Turn heading back pi/2 + //while(Data<0) Data+=360.0; + //while(Data>3600) Data-=360.0; + DOTMData->HeadingValueIdU16 = VALUE_ID_HEADING; + DOTMData->HeadingContentLengthU16 = 2; + DOTMData->HeadingU16 = SwapU16((U16) (Data)); + if (debug) + LogMessage(LOG_LEVEL_DEBUG, "Heading=%d", DOTMData->HeadingU16); + + //Longitudinal speed + Data = 0; + Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 19); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 18); + DOTMData->LongitudinalSpeedValueIdU16 = VALUE_ID_LONGITUDINAL_SPEED; + DOTMData->LongitudinalSpeedContentLengthU16 = 2; + DOTMData->LongitudinalSpeedI16 = SwapI16((I16) Data); + if (debug) + LogMessage(LOG_LEVEL_DEBUG, "LongitudinalSpeedI16=%d", DOTMData->LongitudinalSpeedI16); + + //Lateral speed + Data = 0; + Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 21); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 20); + DOTMData->LateralSpeedValueIdU16 = VALUE_ID_LATERAL_SPEED; + DOTMData->LateralSpeedContentLengthU16 = 2; + DOTMData->LateralSpeedI16 = SwapI16((I16) Data); + if (debug) + LogMessage(LOG_LEVEL_DEBUG, "LateralSpeedI16=%d", DOTMData->LateralSpeedI16); + + //Longitudinal acceleration + Data = 0; + Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 23); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 22); + DOTMData->LongitudinalAccValueIdU16 = VALUE_ID_LONGITUDINAL_ACCELERATION; + DOTMData->LongitudinalAccContentLengthU16 = 2; + DOTMData->LongitudinalAccI16 = SwapI16((I16) Data); + if (debug) + LogMessage(LOG_LEVEL_DEBUG, "LongitudinalAccI16=%d", DOTMData->LongitudinalAccI16); + + //Lateral acceleration + Data = 0; + Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 25); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 24); + DOTMData->LateralAccValueIdU16 = VALUE_ID_LATERAL_ACCELERATION; + DOTMData->LateralAccContentLengthU16 = 2; + DOTMData->LateralAccI16 = SwapI16((I16) Data); + if (debug) + LogMessage(LOG_LEVEL_DEBUG, "LateralAccI16=%d", DOTMData->LateralAccI16); + + //Curvature + Data = 0; + Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 29); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 28); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 27); + Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 26); + DOTMData->CurvatureValueIdU16 = VALUE_ID_CURVATURE; + DOTMData->CurvatureContentLengthU16 = 4; + DOTMData->CurvatureI32 = SwapI32((I32) Data); + if (debug) + printf("CurvatureI32=%d \n", DOTMData->CurvatureI32); + + p = (C8 *) DOTMData; + for (j = 0; j < sizeof (DOTMType); j++, n++) + *(MessageBuffer + n) = *p++; + MessageIndex = n; + } + + + Crc = crc_16((const C8 *)MessageBuffer, sizeof (DOTMType)); + Crc = 0; + *(MessageBuffer + MessageIndex++) = (U8) (Crc); + *(MessageBuffer + MessageIndex++) = (U8) (Crc >> 8); + + + if (debug) { + int i = 0; + + for (i = 0; i < MessageIndex; i++) { + // TODO: Write to log when bytes thingy has been implemented + if ((unsigned char)MessageBuffer[i] >= 0 && (unsigned char)MessageBuffer[i] <= 15) + printf("0"); + printf("%x-", (unsigned char)MessageBuffer[i]); + } + printf("\n"); + } + + return MessageIndex; //Total number of bytes } static int iGetObjectIndexFromObjectIP(in_addr_t ipAddr, in_addr_t objectIPs[], unsigned int numberOfObjects) { - for (unsigned int i = 0; i < numberOfObjects; ++i) { - if (objectIPs[i] == ipAddr) - return (int)i; - } - return -1; + for (unsigned int i = 0; i < numberOfObjects; ++i) { + if (objectIPs[i] == ipAddr) + return (int)i; + } + return -1; } static I32 vConnectObject(int *sockfd, const char *name, const uint32_t port, U8 * Disconnect) { - struct sockaddr_in serv_addr; - struct hostent *server; + struct sockaddr_in serv_addr; + struct hostent *server; - char buffer[256]; - int iResult; + char buffer[256]; + int iResult; - *sockfd = socket(AF_INET, SOCK_STREAM, 0); + *sockfd = socket(AF_INET, SOCK_STREAM, 0); - if (*sockfd < 0) { - util_error("[ObjectControl] ERR: Failed to open control socket"); - } + if (*sockfd < 0) { + util_error("[ObjectControl] ERR: Failed to open control socket"); + } - server = gethostbyname(name); - if (server == NULL) { - util_error("[ObjectControl] ERR: Unknown host "); - } + server = gethostbyname(name); + if (server == NULL) { + util_error("[ObjectControl] ERR: Unknown host "); + } - bzero((char *)&serv_addr, sizeof (serv_addr)); - serv_addr.sin_family = AF_INET; + bzero((char *)&serv_addr, sizeof (serv_addr)); + serv_addr.sin_family = AF_INET; - bcopy((char *)server->h_addr, (char *)&serv_addr.sin_addr.s_addr, server->h_length); - serv_addr.sin_port = htons(port); + bcopy((char *)server->h_addr, (char *)&serv_addr.sin_addr.s_addr, server->h_length); + serv_addr.sin_port = htons(port); - LogMessage(LOG_LEVEL_INFO, "Attempting to connect to socket: %s %i", name, port); + LogMessage(LOG_LEVEL_INFO, "Attempting to connect to socket: %s %i", name, port); - // do - { - iResult = connect(*sockfd, (struct sockaddr *)&serv_addr, sizeof (serv_addr)); + // do + { + iResult = connect(*sockfd, (struct sockaddr *)&serv_addr, sizeof (serv_addr)); - /*if ( iResult < 0) - { - if(errno == ECONNREFUSED) - { - printf("WARNiNG: Was not able to connect to object, [IP: %s] [PORT: %d], %d retry in 3 sec...\n",name,port, *Disconnect); - fflush(stdout); - (void)sleep(3); - } - else - { - util_error("ERR: Failed to connect to control socket"); - } */ - } - //} while(iResult < 0 && *Disconnect == 0); + /*if ( iResult < 0) + { + if(errno == ECONNREFUSED) + { + printf("WARNiNG: Was not able to connect to object, [IP: %s] [PORT: %d], %d retry in 3 sec...\n",name,port, *Disconnect); + fflush(stdout); + (void)sleep(3); + } + else + { + util_error("ERR: Failed to connect to control socket"); + } */ + } + //} while(iResult < 0 && *Disconnect == 0); - LogMessage(LOG_LEVEL_INFO, "Connected to command socket: %s %i", name, port); - // Enable polling of status to detect remote disconnect - fcntl(*sockfd, F_SETFL, O_NONBLOCK); + LogMessage(LOG_LEVEL_INFO, "Connected to command socket: %s %i", name, port); + // Enable polling of status to detect remote disconnect + fcntl(*sockfd, F_SETFL, O_NONBLOCK); - return iResult; + return iResult; } static void vDisconnectObject(int *sockfd) { - close(*sockfd); + close(*sockfd); } static void vCreateSafetyChannel(const char *name, const uint32_t port, int *sockfd, struct sockaddr_in *addr) { - int result; - struct hostent *object; + int result; + struct hostent *object; - /* Connect to object safety socket */ - LogMessage(LOG_LEVEL_DEBUG, "Creating safety socket"); + /* Connect to object safety socket */ + LogMessage(LOG_LEVEL_DEBUG, "Creating safety socket"); - *sockfd = socket(AF_INET, SOCK_DGRAM, 0); - if (*sockfd < 0) { - util_error("ERR: Failed to connect to monitor socket"); - } + *sockfd = socket(AF_INET, SOCK_DGRAM, 0); + if (*sockfd < 0) { + util_error("ERR: Failed to connect to monitor socket"); + } - /* Set address to object */ - object = gethostbyname(name); + /* Set address to object */ + object = gethostbyname(name); - if (object == 0) { - util_error("ERR: Unknown host"); - } + if (object == 0) { + util_error("ERR: Unknown host"); + } - bcopy((char *)object->h_addr, (char *)&addr->sin_addr.s_addr, object->h_length); - addr->sin_family = AF_INET; - addr->sin_port = htons(port); + bcopy((char *)object->h_addr, (char *)&addr->sin_addr.s_addr, object->h_length); + addr->sin_family = AF_INET; + addr->sin_port = htons(port); - /* set socket to non-blocking */ - result = fcntl(*sockfd, F_SETFL, fcntl(*sockfd, F_GETFL, 0) | O_NONBLOCK); - if (result < 0) { - util_error("ERR: calling fcntl"); - } + /* set socket to non-blocking */ + result = fcntl(*sockfd, F_SETFL, fcntl(*sockfd, F_GETFL, 0) | O_NONBLOCK); + if (result < 0) { + util_error("ERR: calling fcntl"); + } - LogMessage(LOG_LEVEL_INFO, "Created socket and safety address: %s:%d", name, port); + LogMessage(LOG_LEVEL_INFO, "Created socket and safety address: %s:%d", name, port); } static void vCloseSafetyChannel(int *sockfd) { - close(*sockfd); + close(*sockfd); } static I32 vCheckRemoteDisconnected(int *sockfd) { - char dummy; - ssize_t x = recv(*sockfd, &dummy, 1, MSG_PEEK); - - // Remote has disconnected: EOF => x=0 - if (x == 0) { - return 1; - } - - if (x == -1) { - // Everything is normal - no communication has been received - if (errno == EAGAIN || errno == EWOULDBLOCK) - return 0; - - // Other error occurred - LogMessage(LOG_LEVEL_WARNING, "Error when checking connection status"); - return 1; - } - - // Something has been received on socket - if (x > 0) { - LogMessage(LOG_LEVEL_INFO, "Received unexpected communication from object on command channel"); - return 0; - } - - return 1; + char dummy; + ssize_t x = recv(*sockfd, &dummy, 1, MSG_PEEK); + + // Remote has disconnected: EOF => x=0 + if (x == 0) { + return 1; + } + + if (x == -1) { + // Everything is normal - no communication has been received + if (errno == EAGAIN || errno == EWOULDBLOCK) + return 0; + + // Other error occurred + LogMessage(LOG_LEVEL_WARNING, "Error when checking connection status"); + return 1; + } + + // Something has been received on socket + if (x > 0) { + LogMessage(LOG_LEVEL_INFO, "Received unexpected communication from object on command channel"); + return 0; + } + + return 1; } int ObjectControlSendUDPData(int *sockfd, struct sockaddr_in *addr, char *SendData, int Length, char debug) { - ssize_t result; + ssize_t result; - // TODO: Change to log write when bytes thingy has been implemented - if (debug) { - printf("Bytes sent: "); - int i = 0; + // TODO: Change to log write when bytes thingy has been implemented + if (debug) { + printf("Bytes sent: "); + int i = 0; - for (i = 0; i < Length; i++) - printf("%x-", (unsigned char)*(SendData + i)); - printf("\n"); - } + for (i = 0; i < Length; i++) + printf("%x-", (unsigned char)*(SendData + i)); + printf("\n"); + } - result = sendto(*sockfd, SendData, Length, 0, (const struct sockaddr *)addr, sizeof (struct sockaddr_in)); + result = sendto(*sockfd, SendData, Length, 0, (const struct sockaddr *)addr, sizeof (struct sockaddr_in)); - if (result < 0) { - util_error("ERR: Failed to send on monitor socket"); - } + if (result < 0) { + util_error("ERR: Failed to send on monitor socket"); + } - return 0; + return 0; } static size_t uiRecvMonitor(int *sockfd, char *buffer, size_t length) { - ssize_t result = 0; - size_t recvDataSize = 0; - - // Read until receive buffer is empty, return last read message - do { - result = recv(*sockfd, buffer, length, 0); - - if (result < 0) { - if (errno != EAGAIN && errno != EWOULDBLOCK) { - util_error("Failed to receive from monitor socket"); - } - } - else { - recvDataSize = (size_t) (result); - LogMessage(LOG_LEVEL_DEBUG, "Received: <%s>", buffer); - } - } while (result > 0); - - return recvDataSize; + ssize_t result = 0; + size_t recvDataSize = 0; + + // Read until receive buffer is empty, return last read message + do { + result = recv(*sockfd, buffer, length, 0); + + if (result < 0) { + if (errno != EAGAIN && errno != EWOULDBLOCK) { + util_error("Failed to receive from monitor socket"); + } + } + else { + recvDataSize = (size_t) (result); + LogMessage(LOG_LEVEL_DEBUG, "Received: <%s>", buffer); + } + } while (result > 0); + + return recvDataSize; } int iFindObjectsInfo(C8 object_traj_file[MAX_OBJECTS][MAX_FILE_PATH], - C8 object_address_name[MAX_OBJECTS][MAX_FILE_PATH], in_addr_t objectIPs[MAX_OBJECTS], - I32 * nbr_objects) { - DIR *traj_directory; - struct dirent *directory_entry; - int iForceObjectToLocalhost; - struct sockaddr_in sockaddr; - int result; - char trajPathDir[MAX_FILE_PATH]; - int retval = 0; - - UtilGetTrajDirectoryPath(trajPathDir, sizeof (trajPathDir)); - - iForceObjectToLocalhost = 0; - - traj_directory = opendir(trajPathDir); - if (traj_directory == NULL) { - util_error("ERR: Failed to open trajectory directory"); - } - - (void)iUtilGetIntParaConfFile("ForceObjectToLocalhost", &iForceObjectToLocalhost); - - while ((directory_entry = readdir(traj_directory)) && ((*nbr_objects) < MAX_OBJECTS)) { - - /* Check so it's not . or .. */ - if (strncmp(directory_entry->d_name, ".", 1) && (strstr(directory_entry->d_name, "sync") == NULL)) { - bzero(object_address_name[(*nbr_objects)], MAX_FILE_PATH); - - bzero(object_traj_file[(*nbr_objects)], MAX_FILE_PATH); - (void)strcat(object_traj_file[(*nbr_objects)], trajPathDir); - (void)strcat(object_traj_file[(*nbr_objects)], directory_entry->d_name); - - if (UtilCheckTrajectoryFileFormat - (object_traj_file[*nbr_objects], sizeof (object_traj_file[*nbr_objects]))) { - LogMessage(LOG_LEVEL_ERROR, "Trajectory file <%s> is not valid", - object_traj_file[*nbr_objects]); - retval = -1; - } - - if (0 == iForceObjectToLocalhost) { - (void)strncat(object_address_name[(*nbr_objects)], directory_entry->d_name, - strlen(directory_entry->d_name)); - result = inet_pton(AF_INET, object_address_name[*nbr_objects], &sockaddr.sin_addr); - if (result == -1) { - LogMessage(LOG_LEVEL_ERROR, "Invalid address family"); - retval = -1; - continue; - } - else if (result == 0) { - LogMessage(LOG_LEVEL_WARNING, "Address <%s> is not a valid IPv4 address", - object_address_name[*nbr_objects]); - retval = -1; - continue; - } - else - objectIPs[*nbr_objects] = sockaddr.sin_addr.s_addr; - } - else { - if (USE_TEST_HOST == 0) - (void)strcat(object_address_name[(*nbr_objects)], LOCALHOST); - else if (USE_TEST_HOST == 1) - (void)strcat(object_address_name[(*nbr_objects)], TESTHOST_IP); - - } - - ++(*nbr_objects); - } - } - (void)closedir(traj_directory); - return retval; + C8 object_address_name[MAX_OBJECTS][MAX_FILE_PATH], in_addr_t objectIPs[MAX_OBJECTS], + I32 * nbr_objects) { + DIR *traj_directory; + struct dirent *directory_entry; + int iForceObjectToLocalhost; + struct sockaddr_in sockaddr; + int result; + char trajPathDir[MAX_FILE_PATH]; + int retval = 0; + + UtilGetTrajDirectoryPath(trajPathDir, sizeof (trajPathDir)); + + iForceObjectToLocalhost = 0; + + traj_directory = opendir(trajPathDir); + if (traj_directory == NULL) { + util_error("ERR: Failed to open trajectory directory"); + } + + (void)iUtilGetIntParaConfFile("ForceObjectToLocalhost", &iForceObjectToLocalhost); + + while ((directory_entry = readdir(traj_directory)) && ((*nbr_objects) < MAX_OBJECTS)) { + + /* Check so it's not . or .. */ + if (strncmp(directory_entry->d_name, ".", 1) && (strstr(directory_entry->d_name, "sync") == NULL)) { + bzero(object_address_name[(*nbr_objects)], MAX_FILE_PATH); + + bzero(object_traj_file[(*nbr_objects)], MAX_FILE_PATH); + (void)strcat(object_traj_file[(*nbr_objects)], trajPathDir); + (void)strcat(object_traj_file[(*nbr_objects)], directory_entry->d_name); + + if (UtilCheckTrajectoryFileFormat + (object_traj_file[*nbr_objects], sizeof (object_traj_file[*nbr_objects]))) { + LogMessage(LOG_LEVEL_ERROR, "Trajectory file <%s> is not valid", + object_traj_file[*nbr_objects]); + retval = -1; + } + + if (0 == iForceObjectToLocalhost) { + (void)strncat(object_address_name[(*nbr_objects)], directory_entry->d_name, + strlen(directory_entry->d_name)); + result = inet_pton(AF_INET, object_address_name[*nbr_objects], &sockaddr.sin_addr); + if (result == -1) { + LogMessage(LOG_LEVEL_ERROR, "Invalid address family"); + retval = -1; + continue; + } + else if (result == 0) { + LogMessage(LOG_LEVEL_WARNING, "Address <%s> is not a valid IPv4 address", + object_address_name[*nbr_objects]); + retval = -1; + continue; + } + else + objectIPs[*nbr_objects] = sockaddr.sin_addr.s_addr; + } + else { + if (USE_TEST_HOST == 0) + (void)strcat(object_address_name[(*nbr_objects)], LOCALHOST); + else if (USE_TEST_HOST == 1) + (void)strcat(object_address_name[(*nbr_objects)], TESTHOST_IP); + + } + + ++(*nbr_objects); + } + } + (void)closedir(traj_directory); + return retval; } OBCState_t vGetState(GSDType * GSD) { - return DataDictionaryGetOBCStateU8(GSD); + return DataDictionaryGetOBCStateU8(GSD); } StateTransitionResult vSetState(OBCState_t requestedState, GSDType * GSD) { - StateTransition transitionFunction; - StateTransitionResult retval = TRANSITION_RESULT_UNDEFINED; - OBCState_t currentState = DataDictionaryGetOBCStateU8(GSD); - - // Always allow transitions to these two states - if (requestedState == OBC_STATE_ERROR || requestedState == OBC_STATE_UNDEFINED) { - if (DataDictionarySetOBCStateU8(GSD, requestedState) == WRITE_OK) { - LogMessage(LOG_LEVEL_WARNING, "Transitioning to state %u", (unsigned char)requestedState); - retval = TRANSITION_OK; - } - else - retval = TRANSITION_MEMORY_ERROR; - } - else if (requestedState == currentState) { - retval = TRANSITION_OK; - } - else { - transitionFunction = tGetTransition(currentState); - retval = transitionFunction(¤tState, requestedState); - if (retval != TRANSITION_INVALID) { - if (DataDictionarySetOBCStateU8(GSD, currentState) == WRITE_OK) { - LogMessage(LOG_LEVEL_INFO, "Transitioning to state %u", (unsigned char)requestedState); - retval = TRANSITION_OK; - } - else - retval = TRANSITION_MEMORY_ERROR; - } - } - - if (retval == TRANSITION_INVALID) { - LogMessage(LOG_LEVEL_WARNING, "Invalid transition requested: from %d to %d", currentState, - requestedState); - } - else if (retval == TRANSITION_MEMORY_ERROR) { - LogMessage(LOG_LEVEL_ERROR, "Unable to set state to %u in shared memory!!", requestedState); - } - return retval; + StateTransition transitionFunction; + StateTransitionResult retval = TRANSITION_RESULT_UNDEFINED; + OBCState_t currentState = DataDictionaryGetOBCStateU8(GSD); + + // Always allow transitions to these two states + if (requestedState == OBC_STATE_ERROR || requestedState == OBC_STATE_UNDEFINED) { + if (DataDictionarySetOBCStateU8(GSD, requestedState) == WRITE_OK) { + LogMessage(LOG_LEVEL_WARNING, "Transitioning to state %u", (unsigned char)requestedState); + retval = TRANSITION_OK; + } + else + retval = TRANSITION_MEMORY_ERROR; + } + else if (requestedState == currentState) { + retval = TRANSITION_OK; + } + else { + transitionFunction = tGetTransition(currentState); + retval = transitionFunction(¤tState, requestedState); + if (retval != TRANSITION_INVALID) { + if (DataDictionarySetOBCStateU8(GSD, currentState) == WRITE_OK) { + LogMessage(LOG_LEVEL_INFO, "Transitioning to state %u", (unsigned char)requestedState); + retval = TRANSITION_OK; + } + else + retval = TRANSITION_MEMORY_ERROR; + } + } + + if (retval == TRANSITION_INVALID) { + LogMessage(LOG_LEVEL_WARNING, "Invalid transition requested: from %d to %d", currentState, + requestedState); + } + else if (retval == TRANSITION_MEMORY_ERROR) { + LogMessage(LOG_LEVEL_ERROR, "Unable to set state to %u in shared memory!!", requestedState); + } + return retval; } StateTransition tGetTransition(OBCState_t fromState) { - switch (fromState) { - case OBC_STATE_IDLE: - return &tFromIdle; - case OBC_STATE_INITIALIZED: - return &tFromInitialized; - case OBC_STATE_CONNECTED: - return &tFromConnected; - case OBC_STATE_ARMED: - return &tFromArmed; - case OBC_STATE_RUNNING: - return &tFromRunning; - case OBC_STATE_ERROR: - return &tFromError; - case OBC_STATE_UNDEFINED: - return &tFromUndefined; - } + switch (fromState) { + case OBC_STATE_IDLE: + return &tFromIdle; + case OBC_STATE_INITIALIZED: + return &tFromInitialized; + case OBC_STATE_CONNECTED: + return &tFromConnected; + case OBC_STATE_ARMED: + return &tFromArmed; + case OBC_STATE_RUNNING: + return &tFromRunning; + case OBC_STATE_ERROR: + return &tFromError; + case OBC_STATE_UNDEFINED: + return &tFromUndefined; + } } StateTransitionResult tFromIdle(OBCState_t * currentState, OBCState_t requestedState) { - if (requestedState == OBC_STATE_INITIALIZED) { - *currentState = requestedState; - return TRANSITION_OK; - } - return TRANSITION_INVALID; + if (requestedState == OBC_STATE_INITIALIZED) { + *currentState = requestedState; + return TRANSITION_OK; + } + return TRANSITION_INVALID; } StateTransitionResult tFromInitialized(OBCState_t * currentState, OBCState_t requestedState) { - if (requestedState == OBC_STATE_CONNECTED || requestedState == OBC_STATE_IDLE) { - *currentState = requestedState; - return TRANSITION_OK; - } - return TRANSITION_INVALID; + if (requestedState == OBC_STATE_CONNECTED || requestedState == OBC_STATE_IDLE) { + *currentState = requestedState; + return TRANSITION_OK; + } + return TRANSITION_INVALID; } StateTransitionResult tFromConnected(OBCState_t * currentState, OBCState_t requestedState) { - if (requestedState == OBC_STATE_ARMED || requestedState == OBC_STATE_IDLE) { - *currentState = requestedState; - return TRANSITION_OK; - } - return TRANSITION_INVALID; + if (requestedState == OBC_STATE_ARMED || requestedState == OBC_STATE_IDLE) { + *currentState = requestedState; + return TRANSITION_OK; + } + return TRANSITION_INVALID; } StateTransitionResult tFromArmed(OBCState_t * currentState, OBCState_t requestedState) { - if (requestedState == OBC_STATE_CONNECTED || requestedState == OBC_STATE_RUNNING - || requestedState == OBC_STATE_IDLE) { - *currentState = requestedState; - return TRANSITION_OK; - } - return TRANSITION_INVALID; + if (requestedState == OBC_STATE_CONNECTED || requestedState == OBC_STATE_RUNNING + || requestedState == OBC_STATE_IDLE) { + *currentState = requestedState; + return TRANSITION_OK; + } + return TRANSITION_INVALID; } StateTransitionResult tFromRunning(OBCState_t * currentState, OBCState_t requestedState) { - if (requestedState == OBC_STATE_CONNECTED || requestedState == OBC_STATE_IDLE) { - *currentState = requestedState; - return TRANSITION_OK; - } - return TRANSITION_INVALID; + if (requestedState == OBC_STATE_CONNECTED || requestedState == OBC_STATE_IDLE) { + *currentState = requestedState; + return TRANSITION_OK; + } + return TRANSITION_INVALID; } StateTransitionResult tFromError(OBCState_t * currentState, OBCState_t requestedState) { - if (requestedState == OBC_STATE_IDLE) { - *currentState = requestedState; - return TRANSITION_OK; - } - return TRANSITION_INVALID; + if (requestedState == OBC_STATE_IDLE) { + *currentState = requestedState; + return TRANSITION_OK; + } + return TRANSITION_INVALID; } StateTransitionResult tFromUndefined(OBCState_t * currentState, OBCState_t requestedState) { - return TRANSITION_INVALID; + return TRANSITION_INVALID; } OBCState_t vInitializeState(OBCState_t firstState, GSDType * GSD) { - static int8_t isInitialized = 0; - - if (!isInitialized) { - isInitialized = 1; - if (DataDictionarySetOBCStateU8(GSD, firstState) != WRITE_OK) - util_error("Unable to write object control state to shared memory"); - } - else { - LogMessage(LOG_LEVEL_WARNING, "Object control state already initialized"); - } - return DataDictionaryGetOBCStateU8(GSD); + static int8_t isInitialized = 0; + + if (!isInitialized) { + isInitialized = 1; + if (DataDictionarySetOBCStateU8(GSD, firstState) != WRITE_OK) + util_error("Unable to write object control state to shared memory"); + } + else { + LogMessage(LOG_LEVEL_WARNING, "Object control state already initialized"); + } + return DataDictionaryGetOBCStateU8(GSD); } From 01eb8566766214f1191398de9fc43656f73a18f8 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 28 Jan 2020 14:37:07 +0100 Subject: [PATCH 183/523] Fixed memory copying error --- server/src/iso22133.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 4ccb86e9f..51e59609a 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -286,12 +286,12 @@ ssize_t encodeOSEMMessage(const double *latitude_deg, const double *longitude_de ALTITUDE_UNAVAILABLE_VALUE : (int32_t) (*altitude_m * ALTITUDE_ONE_METER_VALUE); OSEMData.dateValueID = VALUE_ID_OSEM_DATE; - OSEMData.dateContentLength = sizeof (OSEMData.dateContentLength); + OSEMData.dateContentLength = sizeof (OSEMData.date); OSEMData.date = (uint32_t) ((printableTime->tm_year + 1900) * 10000 + (printableTime->tm_mon + 1) * 100 + (printableTime->tm_mday)); OSEMData.GPSWeekValueID = VALUE_ID_OSEM_GPS_WEEK; - OSEMData.GPSWeekContentLength = sizeof (OSEMData.GPSWeekContentLength); + OSEMData.GPSWeekContentLength = sizeof (OSEMData.GPSWeek); OSEMData.GPSWeek = TimeGetAsGPSweek(¤tTime); OSEMData.GPSQmsOfWeekValueID = VALUE_ID_OSEM_GPS_QUARTER_MILLISECOND_OF_WEEK; @@ -320,7 +320,7 @@ ssize_t encodeOSEMMessage(const double *latitude_deg, const double *longitude_de LogPrint ("OSEM message:\n\tLatitude value ID: 0x%x\n\tLatitude content length: %u\n\tLatitude: %ld [100 nanodegrees]\n\t" "Longitude value ID: 0x%x\n\tLongitude content length: %u\n\tLongitude: %ld [100 nanodegrees]\n\t" - "Altitude value ID: 0x%x\n\tAltitude content length: %u\n\tAltitude: %d [cm]" + "Altitude value ID: 0x%x\n\tAltitude content length: %u\n\tAltitude: %d [cm]\n\t" "Date value ID: 0x%x\n\tDate content length: %u\n\tDate: %u\n\t" "GPS week value ID: 0x%x\n\tGPS week content length: %u\n\tGPS week: %u\n\t" "GPS second of week value ID: 0x%x\n\tGPS second of week content length: %u\n\tGPS second of week: %u [¼ ms]\n\t" @@ -389,7 +389,7 @@ ssize_t encodeOSEMMessage(const double *latitude_deg, const double *longitude_de p += sizeof (OSEMData.longitude) - SizeDifference64bitTo48bit; // Copy rest of struct (excluding footer) directly into buffer since no more byte anomalies remain - memcpy(p, &OSEMData.altitude, sizeof (OSEMData) - sizeof (OSEMData.footer) + memcpy(p, &OSEMData.altitudeValueID, sizeof (OSEMData) - sizeof (OSEMData.footer) - (size_t) (p - osemDataBuffer + 2 * SizeDifference64bitTo48bit)); p += sizeof (OSEMData) - sizeof (OSEMData.footer) - (size_t) (p - osemDataBuffer + 2 * SizeDifference64bitTo48bit); From c856285f981385af13c70283b3d7a9b9ee4b2e19 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 28 Jan 2020 14:37:39 +0100 Subject: [PATCH 184/523] Fixed double pointer to float pointer typecast --- server/src/objectcontrol.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 7748675c5..9d9a22053 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -751,10 +751,10 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { UtilSetObjectPositionIP(&OP[iIndex], object_address_name[iIndex]); - + float altitude = (float) OriginPosition.Altitude; MessageLength = encodeOSEMMessage(&OriginPosition.Latitude, &OriginPosition.Longitude, - &OriginPosition.Altitude, NULL, NULL, NULL, MessageBuffer, + &altitude, NULL, NULL, NULL, MessageBuffer, sizeof (MessageBuffer), 0); if (MessageLength < 0) { util_error("OSEM encoding error"); From 5cf84b73e75602a648052e0640667f3058ea10f3 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 28 Jan 2020 14:37:55 +0100 Subject: [PATCH 185/523] Ran code formatter --- server/src/objectcontrol.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 9d9a22053..225382c22 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -751,7 +751,8 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { UtilSetObjectPositionIP(&OP[iIndex], object_address_name[iIndex]); - float altitude = (float) OriginPosition.Altitude; + float altitude = (float)OriginPosition.Altitude; + MessageLength = encodeOSEMMessage(&OriginPosition.Latitude, &OriginPosition.Longitude, &altitude, NULL, NULL, NULL, MessageBuffer, From 0b054f759a65d8db6ece0ae274f771702acbaf8b Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Tue, 28 Jan 2020 14:53:23 +0100 Subject: [PATCH 186/523] Reverted OC to Dev OC --- server/src/objectcontrol.c | 154 +++++++++++++++---------------------- 1 file changed, 64 insertions(+), 90 deletions(-) diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index f3e15a8a2..36e349db5 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -156,7 +156,6 @@ typedef enum { */ typedef StateTransitionResult(*StateTransition) (OBCState_t * currentState, OBCState_t requestedState); - C8 TrajBuffer[COMMAND_DOTM_ROWS_IN_TRANSMISSION * COMMAND_DOTM_ROW_MESSAGE_LENGTH + COMMAND_MESSAGE_HEADER_LENGTH + COMMAND_TRAJ_INFO_ROW_MESSAGE_LENGTH]; @@ -257,8 +256,8 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { U8 iForceObjectToLocalhostU8 = 0; FILE *fd; - C8 Timestamp[SMALL_BUFFER_SIZE_0]; - C8 GPSWeek[SMALL_BUFFER_SIZE_0]; + C8 Timestamp[SMALL_BUFFER_SIZE_0]; + C8 GPSWeek[SMALL_BUFFER_SIZE_0]; I32 MessageLength; C8 *MiscPtr; C8 MiscText[SMALL_BUFFER_SIZE_0]; @@ -445,15 +444,22 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - bzero(buffer, RECV_MESSAGE_BUFFER); - receivedMONRData = uiRecvMonitor(&safety_socket_fd[iIndex], buffer, RECV_MESSAGE_BUFFER); + memset(buffer, 0, sizeof (buffer)); + receivedMONRData = uiRecvMonitor(&safety_socket_fd[iIndex], buffer, sizeof (buffer)); - if (receivedMONRData == sizeof (MONRType)) { + if (receivedMONRData > 0) { LogMessage(LOG_LEVEL_DEBUG, "Recieved new data from %s %d %d: %s", object_address_name[iIndex], object_udp_port[iIndex], receivedMONRData, buffer); - if (decodeMONRMessage(buffer, receivedMONRData, &MONRData, 0) != MESSAGE_OK) { + if (decodeMONRMessage(buffer, receivedMONRData, &MONRData, 0) != MESSAGE_OK) { + LogMessage(LOG_LEVEL_INFO, "Error decoding MONR from %s: disconnecting object", + object_address_name[iIndex]); + vDisconnectObject(&safety_socket_fd[iIndex]); + // TODO smarter way of handling? + continue; + } + if (ObjectcontrolExecutionMode == OBJECT_CONTROL_CONTROL_MODE) { // Append IP to buffer memcpy(&buffer[receivedMONRData], &safety_object_addr[iIndex].sin_addr.s_addr, @@ -469,30 +475,26 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { } } - ObjectControlBuildMONRMessage(buffer, &MONRData, 0); //Store MONR in GSD - DataDictionarySetMONR(GSD, &MONRData, iIndex); - - //UtilSendUDPData("ObjectControl", &ObjectControlUDPSocketfdI32, &simulator_addr, &MONRData, sizeof(MONRData), 0) - - for (i = 0; - i < - (MONRData.header.MessageLengthU32 + sizeof (MONRData.header) + - sizeof (MONRData.footer)); i++) - GSD->MONRData[i] = buffer[i]; - GSD->MONRSizeU8 = - MONRData.header.MessageLengthU32 + sizeof (MONRData.header) + - sizeof (MONRData.footer); - memset(buffer, 0, sizeof (buffer)); - memcpy(buffer, object_address_name[iIndex], strlen(object_address_name[iIndex])); - strcat(buffer, ";0;"); - MONRToASCII(&MONRData, buffer + strlen(buffer), sizeof (buffer) - strlen(buffer), 0); + //UtilSendUDPData("ObjectControl", &ObjectControlUDPSocketfdI32, &simulator_addr, &MONRData, sizeof(MONRData), 0); + for (i = 0; + i < + (MONRData.header.MessageLengthU32 + sizeof (MONRData.header) + + sizeof (MONRData.footer)); i++) + GSD->MONRData[i] = buffer[i]; + GSD->MONRSizeU8 = + MONRData.header.MessageLengthU32 + sizeof (MONRData.header) + + sizeof (MONRData.footer); + memset(buffer, 0, sizeof (buffer)); + memcpy(buffer, object_address_name[iIndex], strlen(object_address_name[iIndex])); + strcat(buffer, ";0;"); + MONRToASCII(&MONRData, buffer + strlen(buffer), sizeof (buffer) - strlen(buffer), 0); if (ASPData.MTSPU32 != 0) { //Add MTSP to MONR if not 0 - sprintf(buffer + strlen(buffer), "%" PRIu32 ";", ASPData.MTSPU32); + sprintf(buffer + strlen(buffer), "%" PRIu32 ";", ASPData.MTSPU32); } //Ok, let's do the ASP @@ -508,15 +510,15 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { TimeCap1 = (uint64_t) CurrentTimeStruct.tv_sec * 1000 + (uint64_t) CurrentTimeStruct.tv_usec / 1000; //Calculate initial timestamp - OP[iIndex].x = ((double)MONRData.xPosition) / 1000; //Set x and y on OP (ObjectPosition) - OP[iIndex].y = ((double)MONRData.yPosition) / 1000; + OP[iIndex].x = ((double)MONRData.xPosition) / 1000; //Set x and y on OP (ObjectPosition) + OP[iIndex].y = ((double)MONRData.yPosition) / 1000; //OP[iIndex].OrigoDistance = sqrt(pow(OP[iIndex].x,2) + pow(OP[iIndex].y,2)); //Calculate hypotenuse - // TODO: check use of this function since it should take two lat/long points but is here used with x/y + // TODO: check use of this function since it should take two lat/long points but is here used with x/y UtilCalcPositionDelta(OriginLatitudeDbl, OriginLongitudeDbl, - MONRData.xPosition / 1e7, MONRData.yPosition / 1e7, - &OP[iIndex]); + MONRData.xPosition / 1e7, MONRData.yPosition / 1e7, + &OP[iIndex]); if (OP[iIndex].BestFoundTrajectoryIndex <= OP[iIndex].SyncIndex) { ASPData.CurrentTimeU32 = CurrentTimeU32; @@ -565,7 +567,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { //ObjectControlBuildASPMessage(buffer, &ASPData, 0); DataDictionarySetRVSSAsp(GSD, &ASPData); - if (MONRData.gpsQmsOfWeek % ASPDebugRate == 0) { + if (MONRData.gpsQmsOfWeek % ASPDebugRate == 0) { printf("%d, %d, %3.3f, %s, %s\n", CurrentTimeU32, StartTimeU32, ASPData.TimeToSyncPointDbl, object_address_name[iIndex], ASP[i].MasterIP); @@ -582,8 +584,8 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { } } } - OP[iIndex].Speed = - sqrt(pow(MONRData.lateralSpeed, 2) + pow(MONRData.longitudinalSpeed, 2)); + OP[iIndex].Speed = + sqrt(pow(MONRData.lateralSpeed, 2) + pow(MONRData.longitudinalSpeed, 2)); } else if (receivedMONRData > 0) LogMessage(LOG_LEVEL_INFO, "MONR length error (should be %d but is %ld) from %s.", @@ -632,15 +634,15 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { } else if (iCommand == COMM_STRT && (vGetState(GSD) == OBC_STATE_ARMED) /*|| OBC_STATE_INITIALIZED) */ ) //OBC_STATE_INITIALIZED is temporary! { - struct timeval startTime, startDelay; + struct timeval startTime, startDelay; - MiscPtr = pcRecvBuffer; - TimeSetToUTCms(&startTime, (int64_t) strtoul(MiscPtr, &MiscPtr, 10)); - TimeSetToUTCms(&startDelay, (int64_t) strtoul(MiscPtr + 1, NULL, 10)); - timeradd(&startTime, &startDelay, &startTime); - MessageLength = - (int)encodeSTRTMessage(TimeGetAsGPSqmsOfWeek(&startTime), TimeGetAsGPSweek(&startTime), - MessageBuffer, sizeof (MessageBuffer), 0); + MiscPtr = pcRecvBuffer; + TimeSetToUTCms(&startTime, (int64_t) strtoul(MiscPtr, &MiscPtr, 10)); + TimeSetToUTCms(&startDelay, (int64_t) strtoul(MiscPtr + 1, NULL, 10)); + timeradd(&startTime, &startDelay, &startTime); + MessageLength = + (int)encodeSTRTMessage(TimeGetAsGPSqmsOfWeek(&startTime), TimeGetAsGPSweek(&startTime), + MessageBuffer, sizeof (MessageBuffer), 0); ASPData.MTSPU32 = 0; ASPData.TimeToSyncPointDbl = 0; @@ -660,10 +662,9 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { //GSD->STRTSizeU8 = (U8)MessageLength; STRTSentU8 = 1; } - //OBCState = OBC_STATE_INITIALIZED; //This is temporary + //OBCState = OBC_STATE_INITIALIZED; //This is temporary! //printf("OutgoingStartTimeU32 = %d\n", OutgoingStartTimeU32); - GSD->ScenarioStartTimeU32 = TimeGetAsGPSqmsOfWeek(&startTime) >> 2; - + GSD->ScenarioStartTimeU32 = TimeGetAsGPSqmsOfWeek(&startTime) >> 2; bzero(MiscText, SMALL_BUFFER_SIZE_0); sprintf(MiscText, "%" PRIu32, GSD->ScenarioStartTimeU32 << 2); LOG_SEND(LogBuffer, "[ObjectControl] START received <%s>, GPS time <%s>", pcRecvBuffer, @@ -685,7 +686,6 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { } else if (iCommand == COMM_INIT) { LogMessage(LOG_LEVEL_INFO, "INIT received"); - LOG_SEND(LogBuffer, "[ObjectControl] INIT received."); nbr_objects = 0; if (iFindObjectsInfo(object_traj_file, object_address_name, objectIPs, &nbr_objects) == 0) { @@ -734,9 +734,6 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { //OSEMSentU8 = 0; STRTSentU8 = 0; - DataDictionarySetNumberOfObjectsU8(GSD, nbr_objects); - DataDictionaryInitMONR(GSD); - } else { LogMessage(LOG_LEVEL_INFO, @@ -744,8 +741,6 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { pcSendBuffer[0] = (uint8_t) iCommand; iCommSend(COMM_FAILURE, pcSendBuffer, sizeof (iCommand)); } - - } else if (iCommand == COMM_ACCM && vGetState(GSD) == OBC_STATE_CONNECTED) { UtilPopulateACCMDataStructFromMQ(pcRecvBuffer, sizeof (pcRecvBuffer), &mqACCMData); @@ -800,27 +795,24 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { "[ObjectControl] Was not able to connect to object, [IP: %s] [PORT: %d], retry in %d sec...", object_address_name[iIndex], object_tcp_port[iIndex], (!(1 & DisconnectU8)) * 3); - (void)sleep(3); // TODO: Move this to the rest of the sleep operations? Also, remove the hardcoded 3 break; case EADDRINUSE: - LogMessage(LOG_LEVEL_ERROR, - "[ObjectControl] Local address/port already in use"); + util_error("[ObjectControl] Local address/port already in use"); break; case EALREADY: - LogMessage(LOG_LEVEL_ERROR, - "[ObjectControl] Previous connection attempt still in progress"); + util_error("[ObjectControl] Previous connection attempt still in progress"); break; case EISCONN: - LogMessage(LOG_LEVEL_ERROR, "[ObjectControl] Socket is already connected"); + util_error("[ObjectControl] Socket is already connected"); break; case ENETUNREACH: - LogMessage(LOG_LEVEL_ERROR, "[ObjectControl] Network unreachable"); + util_error("[ObjectControl] Network unreachable"); break; case ETIMEDOUT: - LogMessage(LOG_LEVEL_ERROR, "[ObjectControl] Connection timed out"); + util_error("[ObjectControl] Connection timed out"); break; default: - LogMessage(LOG_LEVEL_ERROR, "ERR: Failed to connect to control socket"); + util_error("ERR: Failed to connect to control socket"); break; } @@ -1016,7 +1008,6 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { vDisconnectObject(&socket_fds[iIndex]); } - DataDictionaryFreeMONR(GSD); //#endif //NOTCP LogMessage(LOG_LEVEL_INFO, "DISCONNECT received"); @@ -1168,7 +1159,7 @@ I32 ObjectControlBuildVOILMessage(C8 * MessageBuffer, VOILType * VOILData, C8 * COMMAND_MESSAGE_HEADER_LENGTH); - VOILData->Header.SyncWordU16 = SYNC_WORD; + VOILData->Header.SyncWordU16 = ISO_SYNC_WORD; VOILData->Header.TransmitterIdU8 = 0; VOILData->Header.MessageCounterU8 = 0; VOILData->Header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; @@ -1219,23 +1210,6 @@ I32 ObjectControlBuildVOILMessage(C8 * MessageBuffer, VOILType * VOILData, C8 * } - U16 contentLength = 0; - U16 valueID = 0; - p += sizeof (MONRData->Header.TransmitterIdU8); - - memcpy(&MONRData->Header.MessageCounterU8, p, sizeof (MONRData->Header.MessageCounterU8)); - p += sizeof (MONRData->Header.MessageCounterU8); - - memcpy(&MONRData->Header.AckReqProtVerU8, p, sizeof (MONRData->Header.AckReqProtVerU8)); - p += sizeof (MONRData->Header.AckReqProtVerU8); - - memcpy(&MONRData->Header.MessageIdU16, p, sizeof (MONRData->Header.MessageIdU16)); - p += sizeof (MONRData->Header.MessageIdU16); - - memcpy(&MONRData->Header.MessageLengthU32, p, sizeof (MONRData->Header.MessageLengthU32)); - p += sizeof (MONRData->Header.MessageLengthU32); - memcpy(&valueID, p, sizeof (valueID)); - if (valueID == VALUE_ID_MONR_STRUCT) { I32 ObjectControlBuildOSEMMessage(C8 * MessageBuffer, OSEMType * OSEMData, TimeType * GPSTime, C8 * Latitude, C8 * Longitude, C8 * Altitude, U8 debug) { @@ -1247,7 +1221,7 @@ I32 ObjectControlBuildOSEMMessage(C8 * MessageBuffer, OSEMType * OSEMData, TimeT bzero(MessageBuffer, COMMAND_OSEM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); - OSEMData->Header.SyncWordU16 = SYNC_WORD; + OSEMData->Header.SyncWordU16 = ISO_SYNC_WORD; OSEMData->Header.TransmitterIdU8 = 0; OSEMData->Header.MessageCounterU8 = 0; OSEMData->Header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; @@ -1355,10 +1329,10 @@ I32 ObjectControlBuildOSTMMessage(C8 * MessageBuffer, OSTMType * OSTMData, C8 Co bzero(MessageBuffer, COMMAND_OSTM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); - OSTMData->Header.SyncWordU16 = SYNC_WORD; + OSTMData->Header.SyncWordU16 = ISO_SYNC_WORD; OSTMData->Header.TransmitterIdU8 = 0; OSTMData->Header.MessageCounterU8 = 0; - OSTMData->Header.AckReqProtVerU8 = 0; + OSTMData->Header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; OSTMData->Header.MessageIdU16 = COMMAND_OSTM_CODE; OSTMData->Header.MessageLengthU32 = sizeof (OSTMType) - sizeof (HeaderType); OSTMData->StateValueIdU16 = VALUE_ID_STATE_CHANGE_REQUEST; @@ -1402,7 +1376,7 @@ I32 ObjectControlBuildHEABMessage(C8 * MessageBuffer, HEABType * HEABData, TimeT bzero(MessageBuffer, COMMAND_HEAB_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); - HEABData->Header.SyncWordU16 = SYNC_WORD; + HEABData->Header.SyncWordU16 = ISO_SYNC_WORD; HEABData->Header.TransmitterIdU8 = 0; HEABData->Header.MessageCounterU8 = 0; HEABData->Header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; @@ -1488,10 +1462,10 @@ I32 ObjectControlBuildSYPMMessage(C8 * MessageBuffer, SYPMType * SYPMData, U32 S bzero(MessageBuffer, COMMAND_SYPM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); - SYPMData->Header.SyncWordU16 = SYNC_WORD; + SYPMData->Header.SyncWordU16 = ISO_SYNC_WORD; SYPMData->Header.TransmitterIdU8 = 0; SYPMData->Header.MessageCounterU8 = 0; - SYPMData->Header.AckReqProtVerU8 = 0; + SYPMData->Header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; SYPMData->Header.MessageIdU16 = COMMAND_SYPM_CODE; SYPMData->Header.MessageLengthU32 = sizeof (SYPMType) - sizeof (HeaderType); SYPMData->SyncPointTimeValueIdU16 = 1; @@ -1538,10 +1512,10 @@ I32 ObjectControlBuildMTSPMessage(C8 * MessageBuffer, MTSPType * MTSPData, U32 S bzero(MessageBuffer, COMMAND_MTSP_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); - MTSPData->Header.SyncWordU16 = SYNC_WORD; + MTSPData->Header.SyncWordU16 = ISO_SYNC_WORD; MTSPData->Header.TransmitterIdU8 = 0; MTSPData->Header.MessageCounterU8 = 0; - MTSPData->Header.AckReqProtVerU8 = 0; + MTSPData->Header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; MTSPData->Header.MessageIdU16 = COMMAND_MTSP_CODE; MTSPData->Header.MessageLengthU32 = sizeof (MTSPType) - sizeof (HeaderType); MTSPData->EstSyncPointTimeValueIdU16 = 1; @@ -1621,7 +1595,7 @@ I32 ObjectControlBuildTRAJMessageHeader(C8 * MessageBuffer, I32 * RowCount, Head bzero(MessageBuffer, COMMAND_MESSAGE_HEADER_LENGTH + COMMAND_TRAJ_INFO_ROW_MESSAGE_LENGTH); - HeaderData->SyncWordU16 = SYNC_WORD; + HeaderData->SyncWordU16 = ISO_SYNC_WORD; HeaderData->TransmitterIdU8 = 0; HeaderData->MessageCounterU8 = 0; HeaderData->AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; @@ -2220,7 +2194,7 @@ I32 ObjectControlSendEXACMessage(EXACData * EXAC, I32 * socket, U8 debug) { */ I32 ObjectControlBuildACCMMessage(ACCMData * mqACCMData, ACCMType * ACCM, U8 debug) { // Header - ACCM->header.SyncWordU16 = SYNC_WORD; + ACCM->header.SyncWordU16 = ISO_SYNC_WORD; ACCM->header.TransmitterIdU8 = 0; ACCM->header.MessageCounterU8 = 0; ACCM->header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; @@ -2298,7 +2272,7 @@ I32 ObjectControlBuildEXACMessage(EXACData * mqEXACData, EXACType * EXAC, U8 deb TimeSetToCurrentSystemTime(&systemTime); // Header - EXAC->header.SyncWordU16 = SYNC_WORD; + EXAC->header.SyncWordU16 = ISO_SYNC_WORD; EXAC->header.TransmitterIdU8 = 0; EXAC->header.MessageCounterU8 = 0; EXAC->header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; @@ -2350,7 +2324,7 @@ I32 ObjectControlBuildEXACMessage(EXACData * mqEXACData, EXACType * EXAC, U8 deb */ I32 ObjectControlBuildTRCMMessage(TRCMData * mqTRCMData, TRCMType * TRCM, U8 debug) { // Header - TRCM->header.SyncWordU16 = SYNC_WORD; + TRCM->header.SyncWordU16 = ISO_SYNC_WORD; TRCM->header.TransmitterIdU8 = 0; TRCM->header.MessageCounterU8 = 0; TRCM->header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; From 2625d3aa5a7c6b2bbf7a1e7813e413d10017c55d Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 28 Jan 2020 16:06:19 +0100 Subject: [PATCH 187/523] Implemented encoder functions for MTSP and SYPM --- server/inc/iso22133.h | 2 ++ server/src/iso22133.c | 83 +++++++++++++++++++++++++++++++++++++++++-- 2 files changed, 83 insertions(+), 2 deletions(-) diff --git a/server/inc/iso22133.h b/server/inc/iso22133.h index 52d1c0b71..733b78c77 100644 --- a/server/inc/iso22133.h +++ b/server/inc/iso22133.h @@ -238,6 +238,8 @@ ssize_t encodeSTRTMessage(const struct timeval* timeOfStart, char * strtDataBuff ssize_t encodeOSEMMessage(const double * latitude_deg, const double * longitude_deg, const float * altitude_m, const float * maxPositionDeviation_m, const float * maxLateralDeviation_m, const float * minimumPositioningAccuracy_m, char * osemDataBuffer, const size_t bufferLength, const char debug); ssize_t encodeOSTMMessage(const ObjectCommandType command, char * ostmDataBuffer, const size_t bufferLength, const char debug); ssize_t encodeHEABMessage(const ControlCenterStatusType status, char * heabDataBuffer, const size_t bufferLength, const char debug); +ssize_t encodeSYPMMessage(const uint32_t synchronizationTime, const uint32_t freezeTime, char * sypmDataBuffer, const size_t bufferLength, const char debug); +ssize_t encodeMTSPMessage(const struct timeval * estSyncPointTime, char * mtspDataBuffer, const size_t bufferLength, const char debug); ssize_t encodeTRCMMessage(const uint16_t* triggerID, const TriggerType_t* triggerType, const TriggerTypeParameter_t* param1, const TriggerTypeParameter_t* param2, const TriggerTypeParameter_t* param3, char * trcmDataBuffer, const size_t bufferLength, const char debug); ssize_t decodeTREOMessage(); ssize_t encodeACCMMessage(const uint16_t* actionID, const ActionType_t* actionType, const ActionTypeParameter_t* param1, const ActionTypeParameter_t* param2, const ActionTypeParameter_t* param3, char * accmDataBuffer, const size_t bufferLength, const char debug); diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 3cd1eed91..7b3add232 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -149,6 +149,7 @@ typedef struct { uint16_t estSyncPointTimeValueID; uint16_t estSyncPointTimeContentLength; uint32_t estSyncPointTime; + FooterType footer; } MTSPType; //! MTSP value IDs @@ -1124,14 +1125,73 @@ ISOMessageReturnValue ASCIIToMONR(const char *asciiBuffer, MONRType * MONRData, return MESSAGE_OK; } +/*! + * \brief encodeSYPMMessage Fills an ISO SYPM struct with relevant data fields, and corresponding value IDs and content lengths + * \param synchronizationTime Time along trajectory at which objects are to be synchronized + * \param freezeTime Time along trajectory after which no further adaptation to the master is allowed + * \param mtspDataBuffer Buffer to which SYPM message is to be written + * \param bufferLength Size of buffer to which SYPM message is to be written + * \param debug Flag for enabling debugging + * \return Number of bytes written to buffer, or -1 in case of an error + */ +ssize_t encodeSYPMMessage(const uint32_t synchronizationTime, const uint32_t freezeTime, char * sypmDataBuffer, + const size_t bufferLength, const char debug) { + + SYPMType SYPMData; + // If buffer too small to hold SYPM data, generate an error + if (bufferLength < sizeof (SYPMType)) { + LogMessage(LOG_LEVEL_ERROR, "Buffer too small to hold necessary SYPM data"); + return -1; + } + + // Construct header + SYPMData.header = buildISOHeader(MESSAGE_ID_SYPM, sizeof (SYPMData), debug); + + // Fill contents + SYPMData.syncPointTimeValueID = VALUE_ID_SYPM_SYNC_POINT_TIME; + SYPMData.syncPointTimeContentLength = sizeof (SYPMData.syncPointTime); + SYPMData.syncPointTime = synchronizationTime; + + SYPMData.freezeTimeValueID = VALUE_ID_SYPM_FREEZE_TIME; + SYPMData.freezeTimeContentLength = sizeof (SYPMData.freezeTime); + SYPMData.freezeTime = freezeTime; + + if (debug) { + + } + + // Switch from host endianness to little endian + SYPMData.syncPointTimeValueID = htole16(SYPMData.syncPointTimeValueID); + SYPMData.syncPointTimeContentLength = htole16(SYPMData.syncPointTimeContentLength); + SYPMData.syncPointTime = htole16(SYPMData.syncPointTime); + SYPMData.freezeTimeValueID = htole16(SYPMData.freezeTimeValueID); + SYPMData.freezeTimeContentLength = htole16(SYPMData.freezeTimeContentLength); + SYPMData.freezeTime = htobe16(SYPMData.freezeTime); + + // Construct footer + SYPMData.footer = buildISOFooter(&SYPMData, sizeof (SYPMData), debug); + + memcpy(sypmDataBuffer, &SYPMData, sizeof (SYPMData)); + + return sizeof (SYPMType); +} + +/*! + * \brief encodeMTSPMessage Fills an ISO MTSP struct with relevant data fields, and corresponding value IDs and content lengths + * \param estSyncPointTime Estimated time when the master object will reach the synchronization point + * \param mtspDataBuffer Buffer to which MTSP message is to be written + * \param bufferLength Size of buffer to which MTSP message is to be written + * \param debug Flag for enabling debugging + * \return Number of bytes written to buffer, or -1 in case of an error + */ ssize_t encodeMTSPMessage(const struct timeval * estSyncPointTime, char * mtspDataBuffer, const size_t bufferLength, const char debug) { MTSPType MTSPData; memset(mtspDataBuffer, 0, bufferLength); - // If buffer too small to hold TRCM data, generate an error + // If buffer too small to hold MTSP data, generate an error if (bufferLength < sizeof (MTSPType)) { LogMessage(LOG_LEVEL_ERROR, "Buffer too small to hold necessary MTSP data"); return -1; @@ -1145,7 +1205,26 @@ ssize_t encodeMTSPMessage(const struct timeval * estSyncPointTime, char * mtspDa MTSPData.estSyncPointTimeContentLength = sizeof (MTSPData.estSyncPointTime); MTSPData.estSyncPointTime = estSyncPointTime == NULL ? GPS_SECOND_OF_WEEK_UNAVAILABLE_VALUE : TimeGetAsGPSqmsOfWeek(estSyncPointTime); - // TODO + if (debug) { + LogPrint("MTSP message:\n\t" + "Estimated sync point time value ID: 0x%x\n\t" + "Estimated sync point time content length: %u\n\t" + "Estimated sync point time: %u [¼ ms]", + MTSPData.estSyncPointTimeValueID, MTSPData.estSyncPointTimeContentLength, + MTSPData.estSyncPointTime); + } + + // Switch from host endianness to little endian + MTSPData.estSyncPointTimeValueID = htole16(MTSPData.estSyncPointTimeValueID); + MTSPData.estSyncPointTimeContentLength = htole16(MTSPData.estSyncPointTimeContentLength); + MTSPData.estSyncPointTime = htole32(MTSPData.estSyncPointTime); + + // Construct footer + MTSPData.footer = buildISOFooter(&MTSPData, sizeof (MTSPData), debug); + + memcpy(mtspDataBuffer, &MTSPData, sizeof (MTSPData)); + + return sizeof (MTSPType); } From a01989a5e8717dd2194ae2bd930431bad648bc70 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 28 Jan 2020 16:15:37 +0100 Subject: [PATCH 188/523] Removed old ASP functions from object control --- server/inc/objectcontrol.h | 2 - server/src/objectcontrol.c | 102 ------------------------------------- 2 files changed, 104 deletions(-) diff --git a/server/inc/objectcontrol.h b/server/inc/objectcontrol.h index 639c7ffb0..51ab97491 100644 --- a/server/inc/objectcontrol.h +++ b/server/inc/objectcontrol.h @@ -22,8 +22,6 @@ void objectcontrol_task(TimeType *GPSTime, GSDType *GSD, LOG_LEVEL logLevel); int ObjectControlBuildLLCMMessage(char* MessageBuffer, unsigned short Speed, unsigned short Curvature, unsigned char Mode, char debug); -I32 ObjectControlBuildSYPMMessage(C8* MessageBuffer, SYPMType *SYPMData, U32 SyncPoint, U32 StopTime, U8 debug); -I32 ObjectControlBuildMTSPMessage(C8* MessageBuffer, MTSPType *MTSPData, U32 SyncTimestamp, U8 debug); I32 ObjectControlBuildTRAJMessageHeader(C8* MessageBuffer, I32 *RowCount, HeaderType *HeaderData, TRAJInfoType *TRAJInfoData, C8 *TrajFileHeader, U8 debug); I32 ObjectControlBuildTRAJMessage(C8* MessageBuffer, FILE *fd, I32 RowCount, DOTMType *DOTMType, U8 debug); I32 ObjectControlBuildVOILMessage(C8* MessageBuffer, VOILType *VOILData, C8* SimData, U8 debug); diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index e59b8a2b9..dd483ed2e 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -137,9 +137,6 @@ static void signalHandler(int signo); int ObjectControlBuildLLCMMessage(char *MessageBuffer, unsigned short Speed, unsigned short Curvature, unsigned char Mode, char debug); -I32 ObjectControlBuildSYPMMessage(C8 * MessageBuffer, SYPMType * SYPMData, U32 SyncPoint, U32 StopTime, - U8 debug); -I32 ObjectControlBuildMTSPMessage(C8 * MessageBuffer, MTSPType * MTSPData, U32 SyncTimestamp, U8 debug); I32 ObjectControlBuildTRAJMessageHeader(C8 * MessageBuffer, I32 * RowCount, HeaderType * HeaderData, TRAJInfoType * TRAJInfoData, C8 * TrajFileHeader, U8 debug); I32 ObjectControlBuildTRAJMessage(C8 * MessageBuffer, FILE * fd, I32 RowCount, DOTMType * DOTMData, U8 debug); @@ -242,8 +239,6 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { DOTMType DOTMData; TRAJInfoType TRAJInfoData; VOILType VOILData; - SYPMType SYPMData; - MTSPType MTSPData; ACCMData mqACCMData; EXACData mqEXACData; TRCMData mqTRCMData; @@ -1205,103 +1200,6 @@ int ObjectControlBuildLLCMMessage(char *MessageBuffer, unsigned short Speed, uns return MessageIndex; //Total number of bytes = COMMAND_MESSAGE_HEADER_LENGTH + message data count } -I32 ObjectControlBuildSYPMMessage(C8 * MessageBuffer, SYPMType * SYPMData, U32 SyncPoint, U32 StopTime, - U8 debug) { - - I32 MessageIndex = 0, i; - U16 Crc = 0; - C8 *p; - - bzero(MessageBuffer, COMMAND_SYPM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); - - SYPMData->Header.SyncWordU16 = ISO_SYNC_WORD; - SYPMData->Header.TransmitterIdU8 = 0; - SYPMData->Header.MessageCounterU8 = 0; - SYPMData->Header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; - SYPMData->Header.MessageIdU16 = COMMAND_SYPM_CODE; - SYPMData->Header.MessageLengthU32 = sizeof (SYPMType) - sizeof (HeaderType); - SYPMData->SyncPointTimeValueIdU16 = 1; - SYPMData->SyncPointTimeContentLengthU16 = 4; - SYPMData->SyncPointTimeU32 = SyncPoint; - SYPMData->FreezeTimeValueIdU16 = 2; - SYPMData->FreezeTimeContentLengthU16 = 4; - SYPMData->FreezeTimeU32 = StopTime; - - - p = (C8 *) SYPMData; - for (i = 0; i < sizeof (SYPMType); i++) - *(MessageBuffer + i) = *p++; - Crc = crc_16((const C8 *)MessageBuffer, sizeof (SYPMType)); - Crc = 0; - *(MessageBuffer + i++) = (U8) (Crc >> 8); - *(MessageBuffer + i++) = (U8) (Crc); - MessageIndex = i; - - if (debug) { - // TODO: Change to log printout when byte thingy has been implemented - printf("SYPM total length = %d bytes (header+message+footer)\n", - (int)(COMMAND_SYPM_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH)); - printf("----HEADER----\n"); - for (i = 0; i < sizeof (HeaderType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----MESSAGE----\n"); - for (; i < sizeof (SYPMType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----FOOTER----\n"); - for (; i < MessageIndex; i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n"); - } - - return MessageIndex; //Total number of bytes -} - -I32 ObjectControlBuildMTSPMessage(C8 * MessageBuffer, MTSPType * MTSPData, U32 SyncTimestamp, U8 debug) { - - I32 MessageIndex = 0, i; - U16 Crc = 0; - C8 *p; - - bzero(MessageBuffer, COMMAND_MTSP_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH); - - MTSPData->Header.SyncWordU16 = ISO_SYNC_WORD; - MTSPData->Header.TransmitterIdU8 = 0; - MTSPData->Header.MessageCounterU8 = 0; - MTSPData->Header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; - MTSPData->Header.MessageIdU16 = COMMAND_MTSP_CODE; - MTSPData->Header.MessageLengthU32 = sizeof (MTSPType) - sizeof (HeaderType); - MTSPData->EstSyncPointTimeValueIdU16 = 1; - MTSPData->EstSyncPointTimeContentLengthU16 = 4; - MTSPData->EstSyncPointTimeU32 = SyncTimestamp; - - - p = (C8 *) MTSPData; - for (i = 0; i < sizeof (MTSPType); i++) - *(MessageBuffer + i) = *p++; - Crc = crc_16((const C8 *)MessageBuffer, sizeof (MTSPType)); - Crc = 0; - *(MessageBuffer + i++) = (U8) (Crc >> 8); - *(MessageBuffer + i++) = (U8) (Crc); - MessageIndex = i; - - if (debug) { - // TODO: Change to log printout when byte thingy has been implemented - printf("MTSPData total length = %d bytes (header+message+footer)\n", - (int)(COMMAND_MTSP_MESSAGE_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH)); - printf("----HEADER----\n"); - for (i = 0; i < sizeof (HeaderType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----MESSAGE----\n"); - for (; i < sizeof (MTSPType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----FOOTER----\n"); - for (; i < MessageIndex; i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n"); - } - - return MessageIndex; //Total number of bytes -} I32 ObjectControlBuildTRAJMessageHeader(C8 * MessageBuffer, I32 * RowCount, HeaderType * HeaderData, From 48100bb4911f43c5dec5bababaeaa12f25c0d54e Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 29 Jan 2020 11:07:18 +0100 Subject: [PATCH 189/523] Made MTSP and SYPM encoder functions take timevals as time representation --- server/inc/iso22133.h | 2 +- server/src/iso22133.c | 16 ++++++++++++---- 2 files changed, 13 insertions(+), 5 deletions(-) diff --git a/server/inc/iso22133.h b/server/inc/iso22133.h index 733b78c77..1214c3028 100644 --- a/server/inc/iso22133.h +++ b/server/inc/iso22133.h @@ -238,7 +238,7 @@ ssize_t encodeSTRTMessage(const struct timeval* timeOfStart, char * strtDataBuff ssize_t encodeOSEMMessage(const double * latitude_deg, const double * longitude_deg, const float * altitude_m, const float * maxPositionDeviation_m, const float * maxLateralDeviation_m, const float * minimumPositioningAccuracy_m, char * osemDataBuffer, const size_t bufferLength, const char debug); ssize_t encodeOSTMMessage(const ObjectCommandType command, char * ostmDataBuffer, const size_t bufferLength, const char debug); ssize_t encodeHEABMessage(const ControlCenterStatusType status, char * heabDataBuffer, const size_t bufferLength, const char debug); -ssize_t encodeSYPMMessage(const uint32_t synchronizationTime, const uint32_t freezeTime, char * sypmDataBuffer, const size_t bufferLength, const char debug); +ssize_t encodeSYPMMessage(const timeval synchronizationTime, const timeval freezeTime, char * sypmDataBuffer, const size_t bufferLength, const char debug); ssize_t encodeMTSPMessage(const struct timeval * estSyncPointTime, char * mtspDataBuffer, const size_t bufferLength, const char debug); ssize_t encodeTRCMMessage(const uint16_t* triggerID, const TriggerType_t* triggerType, const TriggerTypeParameter_t* param1, const TriggerTypeParameter_t* param2, const TriggerTypeParameter_t* param3, char * trcmDataBuffer, const size_t bufferLength, const char debug); ssize_t decodeTREOMessage(); diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 7b3add232..902b29e42 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -1134,7 +1134,7 @@ ISOMessageReturnValue ASCIIToMONR(const char *asciiBuffer, MONRType * MONRData, * \param debug Flag for enabling debugging * \return Number of bytes written to buffer, or -1 in case of an error */ -ssize_t encodeSYPMMessage(const uint32_t synchronizationTime, const uint32_t freezeTime, char * sypmDataBuffer, +ssize_t encodeSYPMMessage(const struct timeval synchronizationTime, const struct timeval freezeTime, char * sypmDataBuffer, const size_t bufferLength, const char debug) { SYPMType SYPMData; @@ -1151,14 +1151,22 @@ ssize_t encodeSYPMMessage(const uint32_t synchronizationTime, const uint32_t fre // Fill contents SYPMData.syncPointTimeValueID = VALUE_ID_SYPM_SYNC_POINT_TIME; SYPMData.syncPointTimeContentLength = sizeof (SYPMData.syncPointTime); - SYPMData.syncPointTime = synchronizationTime; + SYPMData.syncPointTime = TimeGetAsUTCms(&synchronizationTime); SYPMData.freezeTimeValueID = VALUE_ID_SYPM_FREEZE_TIME; SYPMData.freezeTimeContentLength = sizeof (SYPMData.freezeTime); - SYPMData.freezeTime = freezeTime; + SYPMData.freezeTime = TimeGetAsUTCms(&freezeTime); if (debug) { - + LogPrint("SYPM message:\n\tSynchronization point time value ID: 0x%x\n\t" + "Synchronization point time content length: %u\n\t" + "Synchronization point time: %u [ms]\n\t" + "Freeze time value ID: 0x%x\n\t" + "Freeze time content length: %u\n\t" + "Freeze time: %u [ms]", SYPMData.syncPointTimeValueID, + SYPMData.syncPointTimeContentLength, SYPMData.syncPointTime, + SYPMData.freezeTimeValueID, SYPMData.freezeTimeContentLength, + SYPMData.freezeTime); } // Switch from host endianness to little endian From 5cb08a206a15d08a0f33c11428a640cfa4594609 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 29 Jan 2020 11:52:02 +0100 Subject: [PATCH 190/523] Made object control use ISO encoder functions for MTSP and SYPM --- server/src/objectcontrol.c | 51 +++++++++++++++++--------------------- 1 file changed, 23 insertions(+), 28 deletions(-) diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index dd483ed2e..94d82c594 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -79,14 +79,6 @@ #define COMMAND_LLCM_CODE 8 #define COMMAND_LLCM_MESSAGE_LENGTH 5 -#define COMMAND_SYPM_CODE 0xA103 -#define COMMAND_SYPM_MESSAGE_LENGTH sizeof(SYPMType) - -#define COMMAND_MTSP_CODE 0xA104 -#define COMMAND_MTSP_MESSAGE_LENGTH sizeof(MTSPType) - -#define ASP_MESSAGE_LENGTH sizeof(ASPType) - #define SMALL_BUFFER_SIZE_0 20 #define SMALL_BUFFER_SIZE_1 2 #define SMALL_BUFFER_SIZE_2 1 @@ -201,11 +193,13 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { const struct timespec mqNonEmptyPollPeriod = { OC_SLEEP_TIME_NONEMPTY_MQ_S, OC_SLEEP_TIME_NONEMPTY_MQ_NS }; const struct timeval stateReportPeriod = { OC_STATE_REPORT_PERIOD_S, OC_STATE_REPORT_PERIOD_US }; - struct timeval currentTime, nextStateReportTime, nextHeartbeatTime; + struct timeval currentTime, nextStateReportTime, nextHeartbeatTime, nextAdaptiveSyncMessageTime; const struct timeval heartbeatPeriod = { 1 / HEAB_FREQUENCY_HZ, (1000000 / HEAB_FREQUENCY_HZ) % 1000000 }; + const struct timeval adaptiveSyncMessagePeriod = heartbeatPeriod; + U8 iForceObjectToLocalhostU8 = 0; FILE *fd; @@ -302,6 +296,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { TimeSetToCurrentSystemTime(¤tTime); nextStateReportTime = currentTime; nextHeartbeatTime = currentTime; + nextAdaptiveSyncMessageTime = currentTime; while (!iExit) { @@ -354,26 +349,27 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { char buffer[RECV_MESSAGE_BUFFER]; size_t receivedMONRData = 0; - // this is etsi time lets remov it ans use utc instead - //gettimeofday(&CurrentTimeStruct, NULL); - CurrentTimeU32 = ((GPSTime->GPSSecondsOfWeekU32 * 1000 + (U32) TimeControlGetMillisecond(GPSTime)) << 2) + GPSTime->MicroSecondU16; + /*MTSP*/ + if (timercmp(¤tTime, &nextAdaptiveSyncMessageTime, >)) { + + timeradd(&nextAdaptiveSyncMessageTime, &adaptiveSyncMessagePeriod, &nextAdaptiveSyncMessageTime); + + struct timeval estSyncPointTime; + TimeSetToGPStime(&estSyncPointTime, TimeGetAsGPSweek(¤tTime), ASPData.MTSPU32); - /*MTSP*/ if (HeartbeatMessageCounter == 0) { - HeartbeatMessageCounter = 0; for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { for (i = 0; i < SyncPointCount; i++) { if (TEST_SYNC_POINTS == 0 && strstr(object_address_name[iIndex], ASP[i].SlaveIP) != NULL && ASPData.MTSPU32 > 0 && ASPData.TimeToSyncPointDbl > -1) { + /*Send Master time to adaptive sync point */ - MessageLength = - ObjectControlBuildMTSPMessage(MessageBuffer, &MTSPData, ASPData.MTSPU32, 0); - //ObjectControlSendUDPData(&safety_socket_fd[iIndex], &safety_object_addr[iIndex], MessageBuffer, MessageLength, 0); - UtilSendUDPData("Object Control", &safety_socket_fd[iIndex], + MessageLength = encodeMTSPMessage(&estSyncPointTime, MessageBuffer, sizeof (MessageBuffer), 0); + UtilSendUDPData(MODULE_NAME, &safety_socket_fd[iIndex], &safety_object_addr[iIndex], MessageBuffer, MessageLength, 0); } /*else if(TEST_SYNC_POINTS == 1 && iIndex == 1 && ASPData.MTSPU32 > 0 && ASPData.TimeToSyncPointDbl > -1 ) @@ -878,23 +874,20 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { LogMessage(LOG_LEVEL_INFO, "Sync point counts: %d", SyncPointCount); for (i = 0; i < SyncPointCount; i++) { + struct timeval syncPointTime, syncStopTime; + TimeSetToUTCms(&syncPointTime, (int64_t) (ASP[i].SlaveTrajSyncTime * 1000.0f)); + TimeSetToUTCms(&syncStopTime, (int64_t) (ASP[i].SlaveSyncStopTime * 1000.0f)); if (TEST_SYNC_POINTS == 1 && iIndex == 1) { /*Send SYPM to slave */ - MessageLength = - ObjectControlBuildSYPMMessage(MessageBuffer, &SYPMData, - ASP[i].SlaveTrajSyncTime * 1000, - ASP[i].SlaveSyncStopTime * 1000, 1); - UtilSendTCPData("Object Control", MessageBuffer, MessageLength, + MessageLength = encodeSYPMMessage(syncPointTime, syncStopTime, MessageBuffer, sizeof (MessageBuffer), 0); + UtilSendTCPData(MODULE_NAME, MessageBuffer, MessageLength, &socket_fds[iIndex], 0); } else if (TEST_SYNC_POINTS == 0 && strstr(object_address_name[iIndex], ASP[i].SlaveIP) != NULL) { /*Send SYPM to slave */ - MessageLength = - ObjectControlBuildSYPMMessage(MessageBuffer, &SYPMData, - ASP[i].SlaveTrajSyncTime * 1000, - ASP[i].SlaveSyncStopTime * 1000, 1); - UtilSendTCPData("Object Control", MessageBuffer, MessageLength, + MessageLength = encodeSYPMMessage(syncPointTime, syncStopTime, MessageBuffer, sizeof (MessageBuffer), 0); + UtilSendTCPData(MODULE_NAME, MessageBuffer, MessageLength, &socket_fds[iIndex], 0); } } @@ -929,6 +922,8 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { objectControlServerStatus = CONTROL_CENTER_STATUS_READY; if (DisconnectU8 == 0) { + nextHeartbeatTime = currentTime; + nextAdaptiveSyncMessageTime = currentTime; vSetState(OBC_STATE_CONNECTED, GSD); iCommSend(COMM_OBJECTS_CONNECTED, NULL, 0); } From 17e5a98241bc5f7b3ab5e315241baf00994c47b4 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 29 Jan 2020 11:58:13 +0100 Subject: [PATCH 191/523] Fixed header function declaration --- server/inc/iso22133.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/server/inc/iso22133.h b/server/inc/iso22133.h index 1214c3028..0d2d2674b 100644 --- a/server/inc/iso22133.h +++ b/server/inc/iso22133.h @@ -238,7 +238,7 @@ ssize_t encodeSTRTMessage(const struct timeval* timeOfStart, char * strtDataBuff ssize_t encodeOSEMMessage(const double * latitude_deg, const double * longitude_deg, const float * altitude_m, const float * maxPositionDeviation_m, const float * maxLateralDeviation_m, const float * minimumPositioningAccuracy_m, char * osemDataBuffer, const size_t bufferLength, const char debug); ssize_t encodeOSTMMessage(const ObjectCommandType command, char * ostmDataBuffer, const size_t bufferLength, const char debug); ssize_t encodeHEABMessage(const ControlCenterStatusType status, char * heabDataBuffer, const size_t bufferLength, const char debug); -ssize_t encodeSYPMMessage(const timeval synchronizationTime, const timeval freezeTime, char * sypmDataBuffer, const size_t bufferLength, const char debug); +ssize_t encodeSYPMMessage(const struct timeval synchronizationTime, const struct timeval freezeTime, char * sypmDataBuffer, const size_t bufferLength, const char debug); ssize_t encodeMTSPMessage(const struct timeval * estSyncPointTime, char * mtspDataBuffer, const size_t bufferLength, const char debug); ssize_t encodeTRCMMessage(const uint16_t* triggerID, const TriggerType_t* triggerType, const TriggerTypeParameter_t* param1, const TriggerTypeParameter_t* param2, const TriggerTypeParameter_t* param3, char * trcmDataBuffer, const size_t bufferLength, const char debug); ssize_t decodeTREOMessage(); From 46f262a3c04bc606339cc78535f865e021860686 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 29 Jan 2020 11:58:50 +0100 Subject: [PATCH 192/523] Fixed missing define --- server/src/objectcontrol.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 94d82c594..2b6f9c19f 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -1531,7 +1531,7 @@ I32 ObjectControlBuildASPMessage(C8 * MessageBuffer, ASPType * ASPData, U8 debug I32 MessageIndex = 0, i; C8 *p; - bzero(MessageBuffer, ASP_MESSAGE_LENGTH); + memset(MessageBuffer, 0, sizeof (ASPType)); p = (C8 *) ASPData; for (i = 0; i < sizeof (ASPType); i++) *(MessageBuffer + i) = *p++; @@ -1539,7 +1539,7 @@ I32 ObjectControlBuildASPMessage(C8 * MessageBuffer, ASPType * ASPData, U8 debug if (debug) { // TODO: Write to log when bytes thingy has been implemented - printf("ASP total length = %d bytes \n", (int)(ASP_MESSAGE_LENGTH)); + printf("ASP total length = %d bytes \n", (int)(sizeof (ASPType))); printf("\n----MESSAGE----\n"); for (i = 0; i < sizeof (ASPType); i++) printf("%x ", (C8) MessageBuffer[i]); From 3b2d40bdda436d30cda3db00845ffee340803e93 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 29 Jan 2020 15:18:31 +0100 Subject: [PATCH 193/523] Fixed a bug where argument count was increased even when an empty argument was received --- server/src/systemcontrol.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index cf8f816a7..dbaf626a6 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -470,7 +470,9 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { } } StartPtr = StopPtr + 1; - CurrentInputArgCount++; + if (SystemControlArgument[CurrentInputArgCount][0] != '\0') { + CurrentInputArgCount++; // In case the argument was empty, don't add it to the argument count + } } if (CmdPtr != NULL) From 0f47a5a71b19ef0ece0b100b66febe9cc4242ef6 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 29 Jan 2020 15:47:49 +0100 Subject: [PATCH 194/523] Added missing response code in AbortScenario response --- server/src/systemcontrol.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index dbaf626a6..688012794 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -993,8 +993,9 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { SystemControlCommand = Idle_0; if (ClientSocket >= 0) { bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + ControlResponseBuffer[0] = 1; SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "AbortScenario:", - ControlResponseBuffer, 0, &ClientSocket, 0); + ControlResponseBuffer, 1, &ClientSocket, 0); } } } @@ -1002,7 +1003,7 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { if (ClientSocket >= 0) { bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE, - "AbortScenario:", ControlResponseBuffer, 0, + "AbortScenario:", ControlResponseBuffer, 1, &ClientSocket, 0); SystemControlSendLog("[SystemControl] ABORT received, state errors!\n", &ClientSocket, 0); } From 3bf07121ddd5fd988658d5a676fff29adbe0a58a Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 29 Jan 2020 16:12:59 +0100 Subject: [PATCH 195/523] Added debug printout for when poll fails --- server/integration-tests/100-SingleTypicalTest.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/server/integration-tests/100-SingleTypicalTest.py b/server/integration-tests/100-SingleTypicalTest.py index 59a22da13..c07b06082 100644 --- a/server/integration-tests/100-SingleTypicalTest.py +++ b/server/integration-tests/100-SingleTypicalTest.py @@ -14,8 +14,10 @@ def checkProgramStatus(failurePrintout): if server != None: - if server.poll(): + deadPIDs = server.poll() + if deadPIDs: print(failurePrintout) + print("Dead PIDs: " + str(deadPIDs)) if userControl != None: userControl.shutdown() server.stop() From 8e54aa6925d70a836efeaf786a47c2449547758a Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 29 Jan 2020 16:33:09 +0100 Subject: [PATCH 196/523] Moved a function call in logger to be after LogInit so error printouts can happen --- server/src/logger.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/server/src/logger.c b/server/src/logger.c index 4ebaf2702..ac2e1b9d6 100644 --- a/server/src/logger.c +++ b/server/src/logger.c @@ -87,7 +87,6 @@ void logger_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { char subStrings[MBUS_MAX_DATALEN]; char journalPathDir[MAX_FILE_PATH]; - UtilGetJournalDirectoryPath(journalPathDir, sizeof (journalPathDir)); struct timeval recvTime; // Listen for commands @@ -116,6 +115,7 @@ void logger_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { util_error("Unable to initialize connection to message bus"); // Create log directory if it does not exist + UtilGetJournalDirectoryPath(journalPathDir, sizeof (journalPathDir)); vCreateLogFolder(journalPathDir); // our time From 0f89e8b29369f86f5a503146cd5967a37c4e092f Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Thu, 30 Jan 2020 10:23:19 +0100 Subject: [PATCH 197/523] Updated documentation --- server/src/datadictionary.c | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/server/src/datadictionary.c b/server/src/datadictionary.c index 7efec5a09..967970a83 100644 --- a/server/src/datadictionary.c +++ b/server/src/datadictionary.c @@ -1673,7 +1673,6 @@ OBCState_t DataDictionaryGetOBCStateU8(GSDType * GSD) { /*! * \brief DataDictionaryInitMONR inits a data structure for saving object monr * \param GSD Pointer to shared allocated memory - * \param objects number of objects that will transmitt monr * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitMONR(GSDType * GSD) { @@ -1711,14 +1710,14 @@ ReadWriteAccess_t DataDictionaryInitMONR(GSDType * GSD) { * \brief DataDictionarySetMONR Parses input variable and sets variable to corresponding value * \param GSD Pointer to shared allocated memory * \param MONRdata Monitor data - * \param id object id + * \param transmitterId requested object transmitterId * \return Result according to ::ReadWriteAccess_t */ -ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, const MONRType * MONR, const U32 TransmitterId) { +ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, const MONRType * MONR, const U32 transmitterId) { ReadWriteAccess_t Res; Res = WRITE_OK; pthread_mutex_lock(&MONRMutex); - if(GSD->MonrMessages != NULL && TransmitterId < GSD->numberOfObjects){ + if(GSD->MonrMessages != NULL && transmitterId < GSD->numberOfObjects){ GSD->MonrMessages[0] = *MONR; } pthread_mutex_unlock(&MONRMutex); @@ -1730,13 +1729,13 @@ ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, const MONRType * MONR, co * \brief DataDictionaryGetMONR Reads variable from shared memory * \param GSD Pointer to shared allocated memory * \param MONRdata Return variable pointer - * \param id requesed object id + * \param transmitterId requested object transmitterId * \return Result according to ::ReadWriteAccess_t */ -ReadWriteAccess_t DataDictionaryGetMONR(GSDType * GSD, MONRType * MONR, const U32 TransmitterId) { +ReadWriteAccess_t DataDictionaryGetMONR(GSDType * GSD, MONRType * MONR, const U32 transmitterId) { pthread_mutex_lock(&MONRMutex); if(GSD->MonrMessages != NULL && TransmitterId < GSD->numberOfObjects){ - *MONR = GSD->MonrMessages[TransmitterId]; + *MONR = GSD->MonrMessages[transmitterId]; } pthread_mutex_unlock(&MONRMutex); return READ_OK; @@ -1745,7 +1744,6 @@ ReadWriteAccess_t DataDictionaryGetMONR(GSDType * GSD, MONRType * MONR, const U3 /*! * \brief DataDictionaryInitMONR inits a data structure for saving object monr * \param GSD Pointer to shared allocated memory - * \param objects number of objects that will transmitt monr * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryFreeMONR(GSDType * GSD) { @@ -1770,7 +1768,7 @@ ReadWriteAccess_t DataDictionaryFreeMONR(GSDType * GSD) { /*! * \brief DataDictionarySetOBCStateU8 Parses input variable and sets variable to corresponding value * \param GSD Pointer to shared allocated memory - * \param OBCState + * \param numberOfobjects number of objects in a test * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetNumberOfObjectsU8(GSDType * GSD, U32 *numberOfObjects) { @@ -1786,6 +1784,7 @@ ReadWriteAccess_t DataDictionarySetNumberOfObjectsU8(GSDType * GSD, U32 *numberO /*! * \brief DataDictionaryGetOBCStateU8 Reads variable from shared memory * \param GSD Pointer to shared allocated memory + * \param numberOfobjects number of objects in a test * \return Current object control state according to ::OBCState_t */ ReadWriteAccess_t DataDictionaryGetNumberOfObjectsU8(GSDType * GSD, U32 *numberOfObjects) { From c90fe2c2ff1657a5700cb7db2e3776a63cf14ccd Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Thu, 30 Jan 2020 10:27:37 +0100 Subject: [PATCH 198/523] Ran formatting script --- server/src/datadictionary.c | 43 ++++++++++++++++++++----------------- 1 file changed, 23 insertions(+), 20 deletions(-) diff --git a/server/src/datadictionary.c b/server/src/datadictionary.c index 967970a83..8899545e9 100644 --- a/server/src/datadictionary.c +++ b/server/src/datadictionary.c @@ -1684,7 +1684,7 @@ ReadWriteAccess_t DataDictionaryInitMONR(GSDType * GSD) { struct stat st; pthread_mutex_lock(&MONRMutex); - sprintf(filePath, "%smonrMessageMemory%d.mem", SHARED_MEMORY_PATH, 5); + sprintf(filePath, "%smonrMessageMemory%d.mem", SHARED_MEMORY_PATH, 5); fd = open(filePath, O_RDWR | O_CREAT, S_IRUSR | S_IWUSR); stat(filePath, &st); @@ -1697,10 +1697,11 @@ ReadWriteAccess_t DataDictionaryInitMONR(GSDType * GSD) { //printf("File size after writing: %ld\n", st.st_size); // Map memory to created file - GSD->MonrMessages = (MONRType *) mmap(NULL, (sizeof (MONRType)), PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0); - close(fd); + GSD->MonrMessages = + (MONRType *) mmap(NULL, (sizeof (MONRType)), PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0); + close(fd); - LogMessage(LOG_LEVEL_INFO, "Init MonrMessage memory"); + LogMessage(LOG_LEVEL_INFO, "Init MonrMessage memory"); pthread_mutex_unlock(&MONRMutex); return Res; @@ -1715,12 +1716,13 @@ ReadWriteAccess_t DataDictionaryInitMONR(GSDType * GSD) { */ ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, const MONRType * MONR, const U32 transmitterId) { ReadWriteAccess_t Res; + Res = WRITE_OK; pthread_mutex_lock(&MONRMutex); - if(GSD->MonrMessages != NULL && transmitterId < GSD->numberOfObjects){ - GSD->MonrMessages[0] = *MONR; - } - pthread_mutex_unlock(&MONRMutex); + if (GSD->MonrMessages != NULL && transmitterId < GSD->numberOfObjects) { + GSD->MonrMessages[0] = *MONR; + } + pthread_mutex_unlock(&MONRMutex); return Res; } @@ -1734,9 +1736,9 @@ ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, const MONRType * MONR, co */ ReadWriteAccess_t DataDictionaryGetMONR(GSDType * GSD, MONRType * MONR, const U32 transmitterId) { pthread_mutex_lock(&MONRMutex); - if(GSD->MonrMessages != NULL && TransmitterId < GSD->numberOfObjects){ - *MONR = GSD->MonrMessages[transmitterId]; - } + if (GSD->MonrMessages != NULL && TransmitterId < GSD->numberOfObjects) { + *MONR = GSD->MonrMessages[transmitterId]; + } pthread_mutex_unlock(&MONRMutex); return READ_OK; } @@ -1751,12 +1753,13 @@ ReadWriteAccess_t DataDictionaryFreeMONR(GSDType * GSD) { Res = WRITE_OK; pthread_mutex_lock(&MONRMutex); - int res= munmap(GSD->MonrMessages, sizeof (MONRType)); - if(res < 0){ - util_error("Unable to unmap monrMessages file!"); - } - free(GSD->MonrMessages); - DataDictionarySetNumberOfObjectsU8(GSD, 0); + int res = munmap(GSD->MonrMessages, sizeof (MONRType)); + + if (res < 0) { + util_error("Unable to unmap monrMessages file!"); + } + free(GSD->MonrMessages); + DataDictionarySetNumberOfObjectsU8(GSD, 0); pthread_mutex_unlock(&MONRMutex); return Res; } @@ -1771,12 +1774,12 @@ ReadWriteAccess_t DataDictionaryFreeMONR(GSDType * GSD) { * \param numberOfobjects number of objects in a test * \return Result according to ::ReadWriteAccess_t */ -ReadWriteAccess_t DataDictionarySetNumberOfObjectsU8(GSDType * GSD, U32 *numberOfObjects) { +ReadWriteAccess_t DataDictionarySetNumberOfObjectsU8(GSDType * GSD, U32 * numberOfObjects) { ReadWriteAccess_t Res; Res = WRITE_OK; pthread_mutex_lock(&numberOfObjectsMutex); - GSD->numberOfObjects = *numberOfObjects; + GSD->numberOfObjects = *numberOfObjects; pthread_mutex_unlock(&numberOfObjectsMutex); return Res; } @@ -1787,7 +1790,7 @@ ReadWriteAccess_t DataDictionarySetNumberOfObjectsU8(GSDType * GSD, U32 *numberO * \param numberOfobjects number of objects in a test * \return Current object control state according to ::OBCState_t */ -ReadWriteAccess_t DataDictionaryGetNumberOfObjectsU8(GSDType * GSD, U32 *numberOfObjects) { +ReadWriteAccess_t DataDictionaryGetNumberOfObjectsU8(GSDType * GSD, U32 * numberOfObjects) { pthread_mutex_lock(&numberOfObjectsMutex); *numberOfObjects = GSD->numberOfObjects; pthread_mutex_unlock(&numberOfObjectsMutex); From f54d29cf3aefd928e984a8ced34eacc4c13cda47 Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Thu, 30 Jan 2020 10:30:28 +0100 Subject: [PATCH 199/523] changed T to t --- server/src/datadictionary.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/server/src/datadictionary.c b/server/src/datadictionary.c index 8899545e9..7e133fd77 100644 --- a/server/src/datadictionary.c +++ b/server/src/datadictionary.c @@ -1736,7 +1736,7 @@ ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, const MONRType * MONR, co */ ReadWriteAccess_t DataDictionaryGetMONR(GSDType * GSD, MONRType * MONR, const U32 transmitterId) { pthread_mutex_lock(&MONRMutex); - if (GSD->MonrMessages != NULL && TransmitterId < GSD->numberOfObjects) { + if (GSD->MonrMessages != NULL && transmitterId < GSD->numberOfObjects) { *MONR = GSD->MonrMessages[transmitterId]; } pthread_mutex_unlock(&MONRMutex); From 97c962893d7326a011d69c0eaf027201e243cc28 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 31 Jan 2020 13:34:31 +0100 Subject: [PATCH 200/523] Updated util ref --- util | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/util b/util index 13d1481a9..5171c2cb4 160000 --- a/util +++ b/util @@ -1 +1 @@ -Subproject commit 13d1481a9be4ba2e851717c650f7ff884467ce1f +Subproject commit 5171c2cb43b958fb4eb5b3df18a92b0e2e9e518f From 2142dd66162438a1f701b37bae18bd42c9bbb4e1 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 31 Jan 2020 13:36:34 +0100 Subject: [PATCH 201/523] Added type cast --- server/src/iso22133.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 902b29e42..06018274c 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -1151,11 +1151,11 @@ ssize_t encodeSYPMMessage(const struct timeval synchronizationTime, const struct // Fill contents SYPMData.syncPointTimeValueID = VALUE_ID_SYPM_SYNC_POINT_TIME; SYPMData.syncPointTimeContentLength = sizeof (SYPMData.syncPointTime); - SYPMData.syncPointTime = TimeGetAsUTCms(&synchronizationTime); + SYPMData.syncPointTime = (uint32_t) TimeGetAsUTCms(&synchronizationTime); SYPMData.freezeTimeValueID = VALUE_ID_SYPM_FREEZE_TIME; SYPMData.freezeTimeContentLength = sizeof (SYPMData.freezeTime); - SYPMData.freezeTime = TimeGetAsUTCms(&freezeTime); + SYPMData.freezeTime = (uint32_t) TimeGetAsUTCms(&freezeTime); if (debug) { LogPrint("SYPM message:\n\tSynchronization point time value ID: 0x%x\n\t" From 3e619a87d0919fd9ba240e63b59672cd0812fb53 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 4 Feb 2020 09:47:32 +0100 Subject: [PATCH 202/523] Implemented a function converting ISO heading to Maestro heading --- server/inc/iso22133.h | 2 +- server/src/iso22133.c | 31 +++++++++++++++++++++++++++++++ 2 files changed, 32 insertions(+), 1 deletion(-) diff --git a/server/inc/iso22133.h b/server/inc/iso22133.h index 9c7d3b3cd..87fbe3571 100644 --- a/server/inc/iso22133.h +++ b/server/inc/iso22133.h @@ -54,7 +54,7 @@ typedef struct #define MAX_LATERAL_DEVIATION_ONE_METER_VALUE 1000 #define MIN_POSITIONING_ACCURACY_NOT_REQUIRED_VALUE 0 #define MIN_POSITIONING_ACCURACY_ONE_METER_VALUE 1000 // ISO specification unclear on this value - +#define HEADING_UNAVAILABLE_VALUE 36001 //! *************************** OSEM diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 11812a57d..595eac513 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -5,6 +5,7 @@ #include #include #include +#include static const uint8_t SupportedProtocolVersions[] = { 2 }; @@ -25,6 +26,7 @@ static ISOMessageReturnValue decodeISOFooter(const char *MessageBuffer, const si static HeaderType buildISOHeader(ISOMessageID id, uint32_t messageLength, const char debug); static FooterType buildISOFooter(const void *message, const size_t sizeExclFooter, const char debug); static char isValidMessageID(const uint16_t id); +static double mapISOHeadingToHostHeading(const double isoHeading_rad); // ************************** function definitions @@ -724,6 +726,10 @@ ISOMessageReturnValue decodeMONRMessage(const char *MonrData, const size_t lengt LogPrint("ErrorStatus = %d", MONRData->errorStatus); } + + MONRData->heading = MONRData->heading == HEADING_UNAVAILABLE_VALUE ? + HEADING_UNAVAILABLE_VALUE : (uint16_t)(mapISOHeadingToHostHeading(MONRData->heading / 100.0) * 100.0); + return retval; } @@ -863,3 +869,28 @@ ISOMessageReturnValue ASCIIToMONR(const char *asciiBuffer, MONRType * MONRData, return MESSAGE_OK; } + + +/*! + * \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 + * \return Heading, in radians, measured from the test x axis + */ +double mapISOHeadingToHostHeading(const double isoHeading_rad) { + // TODO: Reevaluate this when ISO specification is updated with new heading and rotated coordinate system + + double retval = isoHeading_rad; + // Host heading is CCW while ISO is CW + retval = -retval; + // Host heading is measured from the x axis while ISO is measured from the y axis + retval = retval + M_PI / 2.0; + // Ensure angle lies between 0 and 2pi + while (retval < 0.0) { + retval += 2.0 * M_PI; + } + while (retval >= 2.0 * M_PI) { + retval -= 2.0 * M_PI; + } + return retval; +} + From 2a6f3f7424befe058ebda25df51f86f3566488be Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 4 Feb 2020 09:57:20 +0100 Subject: [PATCH 203/523] Moved MONR typedef from .h to .c --- server/inc/iso22133.h | 24 ------------------------ server/src/iso22133.c | 27 ++++++++++++++++++++++++++- 2 files changed, 26 insertions(+), 25 deletions(-) diff --git a/server/inc/iso22133.h b/server/inc/iso22133.h index 0d2d2674b..7d1f1be73 100644 --- a/server/inc/iso22133.h +++ b/server/inc/iso22133.h @@ -58,31 +58,7 @@ typedef enum { CONTROL_CENTER_STATUS_NORMAL_STOP = 0x05 //!< } ControlCenterStatusType; -//! *************************** MONR #define MONR_EXPECTED_FREQUENCY_HZ 100 -typedef struct -{ - HeaderType header; - uint16_t monrStructValueID; - uint16_t monrStructContentLength; - uint32_t gpsQmsOfWeek; - int32_t xPosition; - int32_t yPosition; - int32_t zPosition; - uint16_t heading; - int16_t longitudinalSpeed; - int16_t lateralSpeed; - int16_t longitudinalAcc; - int16_t lateralAcc; - uint8_t driveDirection; - uint8_t state; - uint8_t readyToArm; - uint8_t errorStatus; - FooterType footer; -} MONRType; - -//! MONR value IDs -#define VALUE_ID_MONR_STRUCT 0x80 #pragma pack(pop) diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 06018274c..21183594b 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -36,7 +36,7 @@ static const uint8_t SupportedProtocolVersions[] = { 2 }; #define ACTION_TYPE_PARAMETER_UNAVAILABLE 4294967295 -#pragma pack(push,1) +#pragma pack(push,1) // Ensure sizeof() is useable for (most) network byte lengths /*! OSEM message */ typedef struct { HeaderType header; @@ -112,6 +112,31 @@ typedef struct { #define VALUE_ID_STRT_GPS_WEEK 0x0003 +//! MONR message */ +typedef struct { + HeaderType header; + uint16_t monrStructValueID; + uint16_t monrStructContentLength; + uint32_t gpsQmsOfWeek; + int32_t xPosition; + int32_t yPosition; + int32_t zPosition; + uint16_t heading; + int16_t longitudinalSpeed; + int16_t lateralSpeed; + int16_t longitudinalAcc; + int16_t lateralAcc; + uint8_t driveDirection; + uint8_t state; + uint8_t readyToArm; + uint8_t errorStatus; + FooterType footer; +} MONRType; + +//! MONR value IDs +#define VALUE_ID_MONR_STRUCT 0x80 + + /*! HEAB message */ typedef struct { HeaderType header; From 467aa65e4d348f95c09dbaaf9b31e7f627009867 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 4 Feb 2020 13:05:23 +0100 Subject: [PATCH 204/523] Added a new file containing internal monitoring data representation --- server/CMakeLists.txt | 3 +- server/inc/positioning.h | 79 ++++++++++++++++++++++++++++++++++++++++ 2 files changed, 81 insertions(+), 1 deletion(-) create mode 100644 server/inc/positioning.h diff --git a/server/CMakeLists.txt b/server/CMakeLists.txt index 6eb0224aa..881398d95 100644 --- a/server/CMakeLists.txt +++ b/server/CMakeLists.txt @@ -79,10 +79,11 @@ add_library(MQBus # Create library add_library(util - src/util.c inc/util.h + src/util.c inc/iso22133.h src/iso22133.c + inc/positioning.h ) # Create library diff --git a/server/inc/positioning.h b/server/inc/positioning.h new file mode 100644 index 000000000..0903e0291 --- /dev/null +++ b/server/inc/positioning.h @@ -0,0 +1,79 @@ +#ifndef POSITIONING_H +#define POSITIONING_H +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include + +typedef struct { + double xCoord_m; + double yCoord_m; + double zCoord_m; + double heading_deg; + bool isPositionValid; + bool isHeadingValid; +} CartesianPosition; // TODO: rename + +typedef struct { + double longitudinal_m_s; + double lateral_m_s; + bool isValid; +} SpeedType; + +typedef struct { + double longitudinal_m_s2; + double lateral_m_s2; + bool isValid; +} AccelerationType; + +typedef enum { + OBJECT_DRIVE_DIRECTION_FORWARD, + OBJECT_DRIVE_DIRECTION_BACKWARD, + OBJECT_DRIVE_DIRECTION_UNAVAILABLE +} DriveDirectionType; + +typedef enum { + OBJECT_STATE_UNKNOWN, + OBJECT_STATE_DISARMED, + OBJECT_STATE_ARMED, + OBJECT_STATE_ACTIVE, + OBJECT_STATE_POSTRUN, + OBJECT_STATE_ABORTING, + OBJECT_STATE_REMOTE_CONTROL +} ObjectStateType; + +typedef enum { + OBJECT_NOT_READY_TO_ARM, + OBJECT_READY_TO_ARM, + OBJECT_READY_TO_ARM_UNAVAILABLE +} ObjectArmReadinessType; + +typedef struct { + bool abortRequest; + bool outsideGeofence; + bool badPositioningAccuracy; + bool engineFault; + bool batteryFault; + bool syncPointEnded; + bool unknownError; +} ObjectErrorType; + +typedef struct { + bool isTimestampValid; + struct timeval timestamp; + CartesianPosition position; + SpeedType speed; + AccelerationType acceleration; + DriveDirectionType drivingDirection; + ObjectStateType state; + ObjectArmReadinessType armReadiness; + ObjectErrorType error; +} ObjectMonitorType; + +#ifdef __cplusplus +} +#endif +#endif From 700ad3a4914518f0de5a5011c856f7942f6c416a Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 4 Feb 2020 13:06:05 +0100 Subject: [PATCH 205/523] Made MONR decoder function use the internal monitoring data representation --- server/inc/iso22133.h | 20 ++- server/src/iso22133.c | 322 +++++++++++++++++++++++++++++++----------- 2 files changed, 255 insertions(+), 87 deletions(-) diff --git a/server/inc/iso22133.h b/server/inc/iso22133.h index 7d1f1be73..d1f357831 100644 --- a/server/inc/iso22133.h +++ b/server/inc/iso22133.h @@ -3,11 +3,17 @@ #ifdef __cplusplus extern "C" { #endif -/*! This file contains all definitions pertaining to the ISO standard 22133 - * - * - * - * +/*! ------------------------------------------------------------------------------ + * -- Copyright : (C) AstaZero AB + * ------------------------------------------------------------------------------ + * -- File : iso22133.h + * -- Author : Lukas Wikander + * -- Description : This file specifies an interface for converting from network + * messages in the ISO 22133 format to native data types. + * -- Purpose : Reduce the amount of boilerplate needed to read an ISO 22133 + * message. + * -- Reference : ISO/TC 22/SC 33/WG 16 - ISO/WD 22133-1 + * ------------------------------------------------------------------------------ */ #include @@ -15,6 +21,8 @@ extern "C" { #include #include +#include "positioning.h" + // TODO: move to .c-file #define ISO_PROTOCOL_VERSION 2 #define ACK_REQ 0 @@ -209,7 +217,7 @@ typedef enum { SUPERVISOR_COMMAND_DEBUG = 2 //!< Place supervisor in debug mode } SupervisorCommandType; -ISOMessageReturnValue decodeMONRMessage(const char * MonrData, const size_t length, MONRType * MONRData, const char debug); +ISOMessageReturnValue decodeMONRMessage(const char * monrDataBuffer, const size_t bufferLength, ObjectMonitorType * MonitorData, const char debug); ssize_t encodeSTRTMessage(const struct timeval* timeOfStart, char * strtDataBuffer, const size_t bufferLength, const char debug); ssize_t encodeOSEMMessage(const double * latitude_deg, const double * longitude_deg, const float * altitude_m, const float * maxPositionDeviation_m, const float * maxLateralDeviation_m, const float * minimumPositioningAccuracy_m, char * osemDataBuffer, const size_t bufferLength, const char debug); ssize_t encodeOSTMMessage(const ObjectCommandType command, char * ostmDataBuffer, const size_t bufferLength, const char debug); diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 21183594b..682459886 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -34,6 +34,41 @@ static const uint8_t SupportedProtocolVersions[] = { 2 }; #define ACTION_ID_UNAVAILABLE 65535 #define ACTION_TYPE_UNAVAILABLE 65535 #define ACTION_TYPE_PARAMETER_UNAVAILABLE 4294967295 +#define POSITION_ONE_METER_VALUE 1000 +#define HEADING_UNAVAILABLE_VALUE 36001 +#define SPEED_UNAVAILABLE_VALUE (-32768) +#define SPEED_ONE_METER_PER_SECOND_VALUE 100 +#define ACCELERATION_UNAVAILABLE_VALUE 32001 +#define ACCELERATION_ONE_METER_PER_SECOND_SQUARED_VALUE 1000 +typedef enum { + ISO_DRIVE_DIRECTION_FORWARD = 0, + ISO_DRIVE_DIRECTION_BACKWARD = 1, + ISO_DRIVE_DIRECTION_UNAVAILABLE = 2 +} DriveDirectionValues; +typedef enum { + ISO_OBJECT_STATE_OFF = 0, + ISO_OBJECT_STATE_INIT = 1, + ISO_OBJECT_STATE_ARMED = 2, + ISO_OBJECT_STATE_DISARMED = 3, + ISO_OBJECT_STATE_RUNNING = 4, + ISO_OBJECT_STATE_POSTRUN = 5, + ISO_OBJECT_STATE_REMOTE_CONTROLLED = 6, + ISO_OBJECT_STATE_ABORTING = 7 +} ObjectStateValues; +typedef enum { + ISO_NOT_READY_TO_ARM = 0, + ISO_READY_TO_ARM = 1, + ISO_READY_TO_ARM_UNAVAILABLE = 2 +} ArmReadinessValues; +#define BITMASK_ERROR_ABORT_REQUEST 0x80 +#define BITMASK_ERROR_OUTSIDE_GEOFENCE 0x40 +#define BITMASK_ERROR_BAD_POSITIONING_ACCURACY 0x20 +#define BITMASK_ERROR_ENGINE_FAULT 0x10 +#define BITMASK_ERROR_BATTERY_FAULT 0x08 +#define BITMASK_ERROR_OTHER 0x04 +#define BITMASK_ERROR_SYNC_POINT_ENDED 0x02 +#define BITMASK_ERROR_VENDOR_SPECIFIC 0x01 + #pragma pack(push,1) // Ensure sizeof() is useable for (most) network byte lengths @@ -311,6 +346,7 @@ static ISOMessageReturnValue decodeISOFooter(const char *MessageBuffer, const si static HeaderType buildISOHeader(ISOMessageID id, uint32_t messageLength, const char debug); static FooterType buildISOFooter(const void *message, const size_t sizeExclFooter, const char debug); static char isValidMessageID(const uint16_t id); +static void convertMONRToHostRepresentation(const MONRType * MONRData, ObjectMonitorType * monitorData); // ************************** function definitions **************************************************************** @@ -883,137 +919,261 @@ ssize_t encodeHEABMessage(const ControlCenterStatusType status, char *heabDataBu * \param debug Flag for enabling of debugging * \return value according to ::ISOMessageReturnValue */ -ISOMessageReturnValue decodeMONRMessage(const char *MonrData, const size_t length, MONRType * MONRData, +ISOMessageReturnValue decodeMONRMessage(const char *monrDataBuffer, const size_t bufferLength, ObjectMonitorType * monitorData, const char debug) { - const char *p = MonrData; - const uint16_t ExpectedMONRStructSize = (uint16_t) (sizeof (*MONRData) - sizeof (MONRData->header) - - sizeof (MONRData->footer.Crc) - - sizeof (MONRData->monrStructValueID) - - sizeof (MONRData->monrStructContentLength)); + MONRType MONRData; + const char *p = monrDataBuffer; + const uint16_t ExpectedMONRStructSize = (uint16_t) (sizeof (MONRData) - sizeof (MONRData.header) + - sizeof (MONRData.footer.Crc) - + sizeof (MONRData.monrStructValueID) + - sizeof (MONRData.monrStructContentLength)); ISOMessageReturnValue retval = MESSAGE_OK; + memset(monitorData, 0, sizeof (*monitorData)); + // Decode ISO header - if ((retval = decodeISOHeader(p, length, &MONRData->header, debug)) != MESSAGE_OK) { - memset(MONRData, 0, sizeof (*MONRData)); + if ((retval = decodeISOHeader(p, bufferLength, &MONRData.header, debug)) != MESSAGE_OK) { + memset(monitorData, 0, sizeof (*monitorData)); return retval; } - p += sizeof (MONRData->header); + p += sizeof (MONRData.header); // If message is not a MONR message, generate an error - if (MONRData->header.MessageIdU16 != MESSAGE_ID_MONR) { - memset(MONRData, 0, sizeof (*MONRData)); + if (MONRData.header.MessageIdU16 != MESSAGE_ID_MONR) { + LogMessage(LOG_LEVEL_ERROR, "Attempted to pass non-MONR message into MONR parsing function"); return MESSAGE_TYPE_ERROR; } // Decode content header - memcpy(&MONRData->monrStructValueID, p, sizeof (MONRData->monrStructValueID)); - p += sizeof (MONRData->monrStructValueID); - MONRData->monrStructValueID = le16toh(MONRData->monrStructValueID); + memcpy(&MONRData.monrStructValueID, p, sizeof (MONRData.monrStructValueID)); + p += sizeof (MONRData.monrStructValueID); + MONRData.monrStructValueID = le16toh(MONRData.monrStructValueID); // If content is not a MONR struct or an unexpected size, generate an error - if (MONRData->monrStructValueID != VALUE_ID_MONR_STRUCT) { + if (MONRData.monrStructValueID != VALUE_ID_MONR_STRUCT) { LogMessage(LOG_LEVEL_ERROR, "Attempted to pass non-MONR struct into MONR parsing function"); - memset(MONRData, 0, sizeof (*MONRData)); return MESSAGE_VALUE_ID_ERROR; } - memcpy(&MONRData->monrStructContentLength, p, sizeof (MONRData->monrStructContentLength)); - p += sizeof (MONRData->monrStructContentLength); - MONRData->monrStructContentLength = le16toh(MONRData->monrStructContentLength); + memcpy(&MONRData.monrStructContentLength, p, sizeof (MONRData.monrStructContentLength)); + p += sizeof (MONRData.monrStructContentLength); + MONRData.monrStructContentLength = le16toh(MONRData.monrStructContentLength); - if (MONRData->monrStructContentLength != ExpectedMONRStructSize) { + if (MONRData.monrStructContentLength != ExpectedMONRStructSize) { LogMessage(LOG_LEVEL_ERROR, "MONR content length %u differs from the expected length %u", - MONRData->monrStructContentLength, ExpectedMONRStructSize); - memset(MONRData, 0, sizeof (*MONRData)); + MONRData.monrStructContentLength, ExpectedMONRStructSize); return MESSAGE_LENGTH_ERROR; } // Decode content - memcpy(&MONRData->gpsQmsOfWeek, p, sizeof (MONRData->gpsQmsOfWeek)); - p += sizeof (MONRData->gpsQmsOfWeek); - MONRData->gpsQmsOfWeek = le32toh(MONRData->gpsQmsOfWeek); + memcpy(&MONRData.gpsQmsOfWeek, p, sizeof (MONRData.gpsQmsOfWeek)); + p += sizeof (MONRData.gpsQmsOfWeek); + MONRData.gpsQmsOfWeek = le32toh(MONRData.gpsQmsOfWeek); - memcpy(&MONRData->xPosition, p, sizeof (MONRData->xPosition)); - p += sizeof (MONRData->xPosition); - MONRData->xPosition = (int32_t) le32toh(MONRData->xPosition); + memcpy(&MONRData.xPosition, p, sizeof (MONRData.xPosition)); + p += sizeof (MONRData.xPosition); + MONRData.xPosition = (int32_t) le32toh(MONRData.xPosition); - memcpy(&MONRData->yPosition, p, sizeof (MONRData->yPosition)); - p += sizeof (MONRData->yPosition); - MONRData->yPosition = (int32_t) le32toh(MONRData->yPosition); + memcpy(&MONRData.yPosition, p, sizeof (MONRData.yPosition)); + p += sizeof (MONRData.yPosition); + MONRData.yPosition = (int32_t) le32toh(MONRData.yPosition); - memcpy(&MONRData->zPosition, p, sizeof (MONRData->zPosition)); - p += sizeof (MONRData->zPosition); - MONRData->zPosition = (int32_t) le32toh(MONRData->zPosition); + memcpy(&MONRData.zPosition, p, sizeof (MONRData.zPosition)); + p += sizeof (MONRData.zPosition); + MONRData.zPosition = (int32_t) le32toh(MONRData.zPosition); - memcpy(&MONRData->heading, p, sizeof (MONRData->heading)); - p += sizeof (MONRData->heading); - MONRData->heading = le16toh(MONRData->heading); + memcpy(&MONRData.heading, p, sizeof (MONRData.heading)); + p += sizeof (MONRData.heading); + MONRData.heading = le16toh(MONRData.heading); - memcpy(&MONRData->longitudinalSpeed, p, sizeof (MONRData->longitudinalSpeed)); - p += sizeof (MONRData->longitudinalSpeed); - MONRData->longitudinalSpeed = (int16_t) le16toh(MONRData->longitudinalSpeed); + memcpy(&MONRData.longitudinalSpeed, p, sizeof (MONRData.longitudinalSpeed)); + p += sizeof (MONRData.longitudinalSpeed); + MONRData.longitudinalSpeed = (int16_t) le16toh(MONRData.longitudinalSpeed); - memcpy(&MONRData->lateralSpeed, p, sizeof (MONRData->lateralSpeed)); - p += sizeof (MONRData->lateralSpeed); - MONRData->lateralSpeed = (int16_t) le16toh(MONRData->lateralSpeed); + memcpy(&MONRData.lateralSpeed, p, sizeof (MONRData.lateralSpeed)); + p += sizeof (MONRData.lateralSpeed); + MONRData.lateralSpeed = (int16_t) le16toh(MONRData.lateralSpeed); - memcpy(&MONRData->longitudinalAcc, p, sizeof (MONRData->longitudinalAcc)); - p += sizeof (MONRData->longitudinalAcc); - MONRData->longitudinalAcc = (int16_t) le16toh(MONRData->longitudinalAcc); + memcpy(&MONRData.longitudinalAcc, p, sizeof (MONRData.longitudinalAcc)); + p += sizeof (MONRData.longitudinalAcc); + MONRData.longitudinalAcc = (int16_t) le16toh(MONRData.longitudinalAcc); - memcpy(&MONRData->lateralAcc, p, sizeof (MONRData->lateralAcc)); - p += sizeof (MONRData->lateralAcc); - MONRData->lateralAcc = (int16_t) le16toh(MONRData->lateralAcc); + memcpy(&MONRData.lateralAcc, p, sizeof (MONRData.lateralAcc)); + p += sizeof (MONRData.lateralAcc); + MONRData.lateralAcc = (int16_t) le16toh(MONRData.lateralAcc); - memcpy(&MONRData->driveDirection, p, sizeof (MONRData->driveDirection)); - p += sizeof (MONRData->driveDirection); + memcpy(&MONRData.driveDirection, p, sizeof (MONRData.driveDirection)); + p += sizeof (MONRData.driveDirection); - memcpy(&MONRData->state, p, sizeof (MONRData->state)); - p += sizeof (MONRData->state); + memcpy(&MONRData.state, p, sizeof (MONRData.state)); + p += sizeof (MONRData.state); - memcpy(&MONRData->readyToArm, p, sizeof (MONRData->readyToArm)); - p += sizeof (MONRData->readyToArm); + memcpy(&MONRData.readyToArm, p, sizeof (MONRData.readyToArm)); + p += sizeof (MONRData.readyToArm); - memcpy(&MONRData->errorStatus, p, sizeof (MONRData->errorStatus)); - p += sizeof (MONRData->errorStatus); + memcpy(&MONRData.errorStatus, p, sizeof (MONRData.errorStatus)); + p += sizeof (MONRData.errorStatus); // Decode footer if ((retval = - decodeISOFooter(p, length - (size_t) (p - MonrData), &MONRData->footer, debug)) != MESSAGE_OK) { - memset(MONRData, 0, sizeof (*MONRData)); + decodeISOFooter(p, bufferLength - (size_t) (p - monrDataBuffer), &MONRData.footer, debug)) != MESSAGE_OK) { + LogMessage(LOG_LEVEL_ERROR, "Error decoding MONR footer"); return retval; } - if (debug == 1) { + if (debug) { LogPrint("MONR:"); - LogPrint("SyncWord = %x", MONRData->header.SyncWordU16); - LogPrint("TransmitterId = %d", MONRData->header.TransmitterIdU8); - LogPrint("PackageCounter = %d", MONRData->header.MessageCounterU8); - LogPrint("AckReq = %d", MONRData->header.AckReqProtVerU8); - LogPrint("MessageId = %d", MONRData->header.MessageIdU16); - LogPrint("MessageLength = %d", MONRData->header.MessageLengthU32); - LogPrint("ValueId = %d", MONRData->monrStructValueID); - LogPrint("ContentLength = %d", MONRData->monrStructContentLength); - LogPrint("GPSSOW = %d", MONRData->gpsQmsOfWeek); - LogPrint("XPosition = %d", MONRData->xPosition); - LogPrint("YPosition = %d", MONRData->yPosition); - LogPrint("ZPosition = %d", MONRData->zPosition); - LogPrint("Heading = %d", MONRData->heading); - LogPrint("LongitudinalSpeed = %d", MONRData->longitudinalSpeed); - LogPrint("LateralSpeed = %d", MONRData->lateralSpeed); - LogPrint("LongitudinalAcc = %d", MONRData->longitudinalAcc); - LogPrint("LateralAcc = %d", MONRData->lateralAcc); - LogPrint("DriveDirection = %d", MONRData->driveDirection); - LogPrint("State = %d", MONRData->state); - LogPrint("ReadyToArm = %d", MONRData->readyToArm); - LogPrint("ErrorStatus = %d", MONRData->errorStatus); + LogPrint("SyncWord = %x", MONRData.header.SyncWordU16); + LogPrint("TransmitterId = %d", MONRData.header.TransmitterIdU8); + LogPrint("PackageCounter = %d", MONRData.header.MessageCounterU8); + LogPrint("AckReq = %d", MONRData.header.AckReqProtVerU8); + LogPrint("MessageId = %d", MONRData.header.MessageIdU16); + LogPrint("MessageLength = %d", MONRData.header.MessageLengthU32); + LogPrint("ValueId = %d", MONRData.monrStructValueID); + LogPrint("ContentLength = %d", MONRData.monrStructContentLength); + LogPrint("GPSSOW = %d", MONRData.gpsQmsOfWeek); + LogPrint("XPosition = %d", MONRData.xPosition); + LogPrint("YPosition = %d", MONRData.yPosition); + LogPrint("ZPosition = %d", MONRData.zPosition); + LogPrint("Heading = %d", MONRData.heading); + LogPrint("LongitudinalSpeed = %d", MONRData.longitudinalSpeed); + LogPrint("LateralSpeed = %d", MONRData.lateralSpeed); + LogPrint("LongitudinalAcc = %d", MONRData.longitudinalAcc); + LogPrint("LateralAcc = %d", MONRData.lateralAcc); + LogPrint("DriveDirection = %d", MONRData.driveDirection); + LogPrint("State = %d", MONRData.state); + LogPrint("ReadyToArm = %d", MONRData.readyToArm); + LogPrint("ErrorStatus = %d", MONRData.errorStatus); } + // Fill output struct with parsed data + convertMONRToHostRepresentation(&MONRData, monitorData); + return retval; } +/*! + * \brief convertMONRToHostRepresentation Converts a MONR message to the internal representation for + * object monitoring data + * \param MONRData MONR message to be converted + * \param monitorData Monitor data in which result is to be placed + */ +void convertMONRToHostRepresentation(const MONRType * MONRData, ObjectMonitorType * monitorData) { + + // Timestamp + monitorData->isTimestampValid = MONRData->gpsQmsOfWeek != GPS_SECOND_OF_WEEK_UNAVAILABLE_VALUE; + if (monitorData->isTimestampValid) { + struct timeval currentTime; + TimeSetToCurrentSystemTime(¤tTime); + TimeSetToGPStime(&monitorData->timestamp, TimeGetAsGPSweek(¤tTime), MONRData->gpsQmsOfWeek); + } + + // Position / heading + monitorData->position.xCoord_m = (double) (MONRData->xPosition) / POSITION_ONE_METER_VALUE; + monitorData->position.yCoord_m = (double) (MONRData->yPosition) / POSITION_ONE_METER_VALUE; + monitorData->position.zCoord_m = (double) (MONRData->zPosition) / POSITION_ONE_METER_VALUE; + monitorData->position.isPositionValid = true; + monitorData->position.isHeadingValid = MONRData->heading != HEADING_UNAVAILABLE_VALUE; + if (monitorData->position.isHeadingValid) { + // TODO: convert to host representation + monitorData->position.heading_deg = (double) (MONRData->heading) / 100; + } + + // Velocity + monitorData->speed.isValid = true; + if (MONRData->longitudinalSpeed == SPEED_UNAVAILABLE_VALUE) + monitorData->speed.isValid = false; + else + monitorData->speed.longitudinal_m_s = + (double) (MONRData->longitudinalSpeed) / SPEED_ONE_METER_PER_SECOND_VALUE; + if (MONRData->lateralSpeed == SPEED_UNAVAILABLE_VALUE) + monitorData->speed.isValid = false; + else + monitorData->speed.lateral_m_s = + (double) (MONRData->lateralSpeed) / SPEED_ONE_METER_PER_SECOND_VALUE; + + // Acceleration + monitorData->acceleration.isValid = true; + if (MONRData->longitudinalAcc == ACCELERATION_UNAVAILABLE_VALUE) + monitorData->acceleration.isValid = false; + else + monitorData->acceleration.longitudinal_m_s2 = + (double) (MONRData->longitudinalAcc) / ACCELERATION_ONE_METER_PER_SECOND_SQUARED_VALUE; + if (MONRData->lateralAcc == ACCELERATION_UNAVAILABLE_VALUE) + monitorData->acceleration.isValid = false; + else + monitorData->acceleration.lateral_m_s2 = + (double) (MONRData->lateralAcc) / ACCELERATION_ONE_METER_PER_SECOND_SQUARED_VALUE; + + // Drive direction + switch (MONRData->driveDirection) { + case ISO_DRIVE_DIRECTION_FORWARD: + monitorData->drivingDirection = OBJECT_DRIVE_DIRECTION_FORWARD; + break; + case ISO_DRIVE_DIRECTION_BACKWARD: + monitorData->drivingDirection = OBJECT_DRIVE_DIRECTION_BACKWARD; + break; + case ISO_DRIVE_DIRECTION_UNAVAILABLE: + default: + monitorData->drivingDirection = OBJECT_DRIVE_DIRECTION_UNAVAILABLE; + } + + // State + switch (MONRData->state) { + case ISO_OBJECT_STATE_DISARMED: + monitorData->state = OBJECT_STATE_DISARMED; + break; + case ISO_OBJECT_STATE_ARMED: + monitorData->state = OBJECT_STATE_ARMED; + break; + case ISO_OBJECT_STATE_RUNNING: + monitorData->state = OBJECT_STATE_ACTIVE; + break; + case ISO_OBJECT_STATE_POSTRUN: + monitorData->state = OBJECT_STATE_POSTRUN; + break; + case ISO_OBJECT_STATE_ABORTING: + monitorData->state = OBJECT_STATE_ABORTING; + break; + case ISO_OBJECT_STATE_REMOTE_CONTROLLED: + monitorData->state = OBJECT_STATE_REMOTE_CONTROL; + break; + case ISO_OBJECT_STATE_OFF: + case ISO_OBJECT_STATE_INIT: + default: + monitorData->state = OBJECT_STATE_UNKNOWN; + break; + } + + // Ready to arm + switch (MONRData->readyToArm) { + case ISO_READY_TO_ARM: + monitorData->armReadiness = OBJECT_READY_TO_ARM; + break; + case ISO_NOT_READY_TO_ARM: + monitorData->armReadiness = OBJECT_NOT_READY_TO_ARM; + break; + case ISO_READY_TO_ARM_UNAVAILABLE: + default: + monitorData->armReadiness = OBJECT_READY_TO_ARM_UNAVAILABLE; + } + + // Error status + monitorData->error.engineFault = MONRData->errorStatus & BITMASK_ERROR_ENGINE_FAULT; + monitorData->error.abortRequest = MONRData->errorStatus & BITMASK_ERROR_ABORT_REQUEST; + monitorData->error.batteryFault = MONRData->errorStatus & BITMASK_ERROR_BATTERY_FAULT; + monitorData->error.unknownError = MONRData->errorStatus & BITMASK_ERROR_OTHER || MONRData->errorStatus & BITMASK_ERROR_VENDOR_SPECIFIC; + monitorData->error.syncPointEnded = MONRData->errorStatus & BITMASK_ERROR_SYNC_POINT_ENDED; + monitorData->error.outsideGeofence = MONRData->errorStatus & BITMASK_ERROR_OUTSIDE_GEOFENCE; + monitorData->error.badPositioningAccuracy = MONRData->errorStatus & BITMASK_ERROR_BAD_POSITIONING_ACCURACY; + + return; +} + + + /*! * \brief MONRToASCII Converts a MONR struct into human readable ASCII text * \param MONRData Struct containing MONR data From 5b737a1bfbae13b242037c22f42107ff21d54a8f Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 4 Feb 2020 13:21:01 +0100 Subject: [PATCH 206/523] Commented all the typedefs --- server/inc/positioning.h | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/server/inc/positioning.h b/server/inc/positioning.h index 0903e0291..6dbe92c38 100644 --- a/server/inc/positioning.h +++ b/server/inc/positioning.h @@ -8,6 +8,7 @@ extern "C" { #include #include +/*! Struct describing a position and orientation in a local cartesian coordinate system */ typedef struct { double xCoord_m; double yCoord_m; @@ -17,24 +18,28 @@ typedef struct { bool isHeadingValid; } CartesianPosition; // TODO: rename +/*! Struct describing longitudinal and lateral speed of an object */ typedef struct { double longitudinal_m_s; double lateral_m_s; bool isValid; } SpeedType; +/*! Struct describing longitudinal and lateral acceleration of an object */ typedef struct { double longitudinal_m_s2; double lateral_m_s2; bool isValid; } AccelerationType; +/*! Enumeration of object driving direction descriptions */ typedef enum { OBJECT_DRIVE_DIRECTION_FORWARD, OBJECT_DRIVE_DIRECTION_BACKWARD, OBJECT_DRIVE_DIRECTION_UNAVAILABLE } DriveDirectionType; +/*! Enumeration of observable object states */ typedef enum { OBJECT_STATE_UNKNOWN, OBJECT_STATE_DISARMED, @@ -45,12 +50,14 @@ typedef enum { OBJECT_STATE_REMOTE_CONTROL } ObjectStateType; +/*! Enumeration of ready to arm statuses of an object */ typedef enum { OBJECT_NOT_READY_TO_ARM, OBJECT_READY_TO_ARM, OBJECT_READY_TO_ARM_UNAVAILABLE } ObjectArmReadinessType; +/*! Struct describing a range of possible errors reported by an object */ typedef struct { bool abortRequest; bool outsideGeofence; @@ -61,6 +68,7 @@ typedef struct { bool unknownError; } ObjectErrorType; +/*! Struct containing measured object data from a single point in time */ typedef struct { bool isTimestampValid; struct timeval timestamp; From 17943245ce5d4be594a27724a11bd3dff998f6f6 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 4 Feb 2020 16:28:23 +0100 Subject: [PATCH 207/523] Remade some string parsing functions to use monitor data rather than MONR --- server/CMakeLists.txt | 1 + server/inc/positioning.h | 12 +- server/inc/util.h | 28 +-- server/src/iso22133.c | 139 +-------------- server/src/positioning.c | 362 +++++++++++++++++++++++++++++++++++++++ server/src/util.c | 34 +--- 6 files changed, 384 insertions(+), 192 deletions(-) create mode 100644 server/src/positioning.c diff --git a/server/CMakeLists.txt b/server/CMakeLists.txt index 881398d95..a239c38f4 100644 --- a/server/CMakeLists.txt +++ b/server/CMakeLists.txt @@ -84,6 +84,7 @@ add_library(util inc/iso22133.h src/iso22133.c inc/positioning.h + src/positioning.c ) # Create library diff --git a/server/inc/positioning.h b/server/inc/positioning.h index 6dbe92c38..703d12cdc 100644 --- a/server/inc/positioning.h +++ b/server/inc/positioning.h @@ -7,6 +7,7 @@ extern "C" { #include #include #include +#include /*! Struct describing a position and orientation in a local cartesian coordinate system */ typedef struct { @@ -44,7 +45,7 @@ typedef enum { OBJECT_STATE_UNKNOWN, OBJECT_STATE_DISARMED, OBJECT_STATE_ARMED, - OBJECT_STATE_ACTIVE, + OBJECT_STATE_RUNNING, OBJECT_STATE_POSTRUN, OBJECT_STATE_ABORTING, OBJECT_STATE_REMOTE_CONTROL @@ -81,6 +82,15 @@ typedef struct { ObjectErrorType error; } ObjectMonitorType; + +int objectMonitorDataToASCII(const ObjectMonitorType * monitorData, char *asciiBuffer, const size_t bufferLength); +int ASCIIToObjectMonitorData(const char *asciiBuffer, ObjectMonitorType * monitorData); +bool hasError(const ObjectErrorType error); +const char * objectStateToASCII(const ObjectStateType state); +ObjectStateType ASCIIToObjectState(const char * asciiString); +void errorStatusToASCII(const ObjectErrorType error, char * asciiBuffer, const size_t bufferLength); +ObjectErrorType ASCIIToErrorStatus(const char * asciiString); + #ifdef __cplusplus } #endif diff --git a/server/inc/util.h b/server/inc/util.h index dce866e33..9186d990e 100644 --- a/server/inc/util.h +++ b/server/inc/util.h @@ -33,6 +33,7 @@ extern "C"{ #include "mqbus.h" #include "iso22133.h" #include "logging.h" +#include "positioning.h" /*------------------------------------------------------------ -- Defines @@ -257,17 +258,7 @@ typedef struct typedef struct { - double xCoord_m; - double yCoord_m; - double zCoord_m; - double heading_deg; -} CartesianPosition; - - - -typedef struct -{ - MONRType MONR; + ObjectMonitorType data; in_addr_t ClientIP; } MonitorDataType; @@ -567,18 +558,6 @@ typedef struct } VOILType; -typedef struct -{ - U16 MessageIdU16; - U32 ObjectIPU32; - U32 GPSQmsOfWeekU32; - I32 XPositionI32; - I32 YPositionI32; - I32 ZPositionI32; - U16 HeadingU16; - I16 SpeedI16; -} ObjectMonitorType; - #define HTTP_HEADER_MAX_LENGTH 64 typedef struct { char AcceptCharset[HTTP_HEADER_MAX_LENGTH]; @@ -702,8 +681,7 @@ void UtilGetGeofenceDirectoryPath(char* path, size_t pathLen); // File parsing functions int UtilCheckTrajectoryFileFormat(const char *path, size_t pathLen); -// -CartesianPosition MONRToCartesianPosition(MonitorDataType MONR); + int UtilMonitorDataToString(MonitorDataType monrData, char* monrString, size_t stringLength); int UtilStringToMonitorData(const char* monrString, size_t stringLength, MonitorDataType * monrData); uint8_t UtilIsPositionNearTarget(CartesianPosition position, CartesianPosition target, double tolerance_m); diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 682459886..e1b209565 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -1129,7 +1129,7 @@ void convertMONRToHostRepresentation(const MONRType * MONRData, ObjectMonitorTyp monitorData->state = OBJECT_STATE_ARMED; break; case ISO_OBJECT_STATE_RUNNING: - monitorData->state = OBJECT_STATE_ACTIVE; + monitorData->state = OBJECT_STATE_RUNNING; break; case ISO_OBJECT_STATE_POSTRUN: monitorData->state = OBJECT_STATE_POSTRUN; @@ -1173,143 +1173,6 @@ void convertMONRToHostRepresentation(const MONRType * MONRData, ObjectMonitorTyp } - -/*! - * \brief MONRToASCII Converts a MONR struct into human readable ASCII text - * \param MONRData Struct containing MONR data - * \param asciiBuffer Buffer in which to print ASCII text representation - * \param bufferLength Length of ASCII buffer - * \param debug Flag for enabling debugging - * \return value according to ::ISOMessageReturnValue - */ -ISOMessageReturnValue MONRToASCII(const MONRType * MONRData, char *asciiBuffer, const size_t bufferLength, - const char debug) { - - memset(asciiBuffer, 0, bufferLength); - if (MONRData->header.MessageIdU16 != MESSAGE_ID_MONR) { - LogMessage(LOG_LEVEL_ERROR, "Attempted to pass non-MONR struct into MONR parsing function"); - return MESSAGE_TYPE_ERROR; - } - - sprintf(asciiBuffer + strlen(asciiBuffer), "%u;", MONRData->gpsQmsOfWeek); - sprintf(asciiBuffer + strlen(asciiBuffer), "%d;%d;%d;%u;", - MONRData->xPosition, MONRData->yPosition, MONRData->zPosition, MONRData->heading); - sprintf(asciiBuffer + strlen(asciiBuffer), "%d;%d;%d;%d;", MONRData->longitudinalSpeed, - MONRData->lateralSpeed, MONRData->longitudinalAcc, MONRData->lateralAcc); - sprintf(asciiBuffer + strlen(asciiBuffer), "%u;%u;%u;%u;", MONRData->driveDirection, - MONRData->state, MONRData->readyToArm, MONRData->errorStatus); - - if (debug) { - LogPrint("Converted MONR to string:\n\tMONR:\n\t\t" - "Value ID = 0x%x\n\t\t" - "Content length = %u\n\t\t" - "GPS quarter millisecond of week = %u\n\t\t" - "X position = %d\n\t\t" - "Y position = %d\n\t\t" - "Z position = %d\n\t\t" - "Heading = %d\n\t\t" - "Longitudinal speed = %d\n\t\t" - "Lateral speed = %d\n\t\t" - "Longitudinal acceleration = %d\n\t\t" - "Lateral acceleration = %d\n\t\t" - "Drive direction = %d\n\t\t" - "State = %d\n\t\t" - "Ready to arm = %d\n\t\t" - "Error status = %d\n\t" - "String:\n\t\t<%s>", MONRData->monrStructValueID, MONRData->monrStructContentLength, - MONRData->gpsQmsOfWeek, MONRData->xPosition, MONRData->yPosition, MONRData->zPosition, - MONRData->heading, MONRData->longitudinalSpeed, MONRData->lateralSpeed, - MONRData->longitudinalAcc, MONRData->lateralAcc, MONRData->driveDirection, MONRData->state, - MONRData->readyToArm, MONRData->errorStatus, asciiBuffer); - } - - return MESSAGE_OK; -} - - -/*! - * \brief MONRToASCII Converts an ASCII string into a MONR struct - * \param asciiBuffer Buffer containing ASCII text representation - * \param MONRData Struct containing MONR data - * \param debug Flag for enabling debugging - * \return value according to ::ISOMessageReturnValue - */ -ISOMessageReturnValue ASCIIToMONR(const char *asciiBuffer, MONRType * MONRData, const char debug) { - - const char *token; - const char delim[] = ";"; - const int NumberBaseDecimal = 10; - char *copy = strdup(asciiBuffer); - - memset(MONRData, 0, sizeof (*MONRData)); - - token = strtok(copy, delim); - - token = strtok(NULL, delim); - MONRData->gpsQmsOfWeek = (uint32_t) strtoul(token, NULL, NumberBaseDecimal); - - token = strtok(NULL, delim); - MONRData->xPosition = (int32_t) strtol(token, NULL, NumberBaseDecimal); - - token = strtok(NULL, delim); - MONRData->yPosition = (int32_t) strtol(token, NULL, NumberBaseDecimal); - - token = strtok(NULL, delim); - MONRData->zPosition = (int32_t) strtol(token, NULL, NumberBaseDecimal); - - token = strtok(NULL, delim); - MONRData->heading = (uint16_t) strtoul(token, NULL, NumberBaseDecimal); - - token = strtok(NULL, delim); - MONRData->longitudinalSpeed = (int16_t) strtol(token, NULL, NumberBaseDecimal); - - token = strtok(NULL, delim); - MONRData->lateralSpeed = (int16_t) strtol(token, NULL, NumberBaseDecimal); - - token = strtok(NULL, delim); - MONRData->longitudinalAcc = (int16_t) strtol(token, NULL, NumberBaseDecimal); - - token = strtok(NULL, delim); - MONRData->lateralAcc = (int16_t) strtol(token, NULL, NumberBaseDecimal); - - token = strtok(NULL, delim); - MONRData->driveDirection = (uint8_t) strtoul(token, NULL, NumberBaseDecimal); - - token = strtok(NULL, delim); - MONRData->state = (uint8_t) strtoul(token, NULL, NumberBaseDecimal); - - token = strtok(NULL, delim); - MONRData->readyToArm = (uint8_t) strtoul(token, NULL, NumberBaseDecimal); - - token = strtok(NULL, delim); - MONRData->errorStatus = (uint8_t) strtoul(token, NULL, NumberBaseDecimal); - - if (debug) { - LogPrint("Converted string to MONR:\n\tString:\n\t\t<%s>\n\tMONR:\n\t\t" - "Value ID = 0x%x\n\t\t" - "Content length = %u\n\t\t" - "GPS quarter millisecond of week = %u\n\t\t" - "X position = %d\n\t\t" - "Y position = %d\n\t\t" - "Z position = %d\n\t\t" - "Heading = %d\n\t\t" - "Longitudinal speed = %d\n\t\t" - "Lateral speed = %d\n\t\t" - "Longitudinal acceleration = %d\n\t\t" - "Lateral acceleration = %d\n\t\t" - "Drive direction = %d\n\t\t" - "State = %d\n\t\t" - "Ready to arm = %d\n\t\t" - "Error status = %d", asciiBuffer, MONRData->monrStructValueID, - MONRData->monrStructContentLength, MONRData->gpsQmsOfWeek, MONRData->xPosition, - MONRData->yPosition, MONRData->zPosition, MONRData->heading, MONRData->longitudinalSpeed, - MONRData->lateralSpeed, MONRData->longitudinalAcc, MONRData->lateralAcc, - MONRData->driveDirection, MONRData->state, MONRData->readyToArm, MONRData->errorStatus); - } - - return MESSAGE_OK; -} - /*! * \brief encodeSYPMMessage Fills an ISO SYPM struct with relevant data fields, and corresponding value IDs and content lengths * \param synchronizationTime Time along trajectory at which objects are to be synchronized diff --git a/server/src/positioning.c b/server/src/positioning.c new file mode 100644 index 000000000..5aff418a2 --- /dev/null +++ b/server/src/positioning.c @@ -0,0 +1,362 @@ +#include +#include +#include + +#include "positioning.h" +#include "maestroTime.h" + +static const char unknownStateString[] = "UNKNOWN"; +static const char disarmedStateString[] = "DISARMED"; +static const char armedStateString[] = "ARMED"; +static const char runningStateString[] = "RUNNING"; +static const char postrunStateString[] = "POSTRUN"; +static const char abortingStateString[] = "ABORTING"; +static const char remoteControlStateString[] = "REMOTECTRL"; + +static const char noErrorString[] = "OK"; +static const char engineFaultString[] = "ENGINE_FAULT"; +static const char batteryFaultString[] = "BATTERY_FAULT"; +static const char abortRequestString[] = "ABORT_REQUEST"; +static const char otherErrorString[] = "OTHER_ERROR"; +static const char syncPointEndedString[] = "SYNC_PT_ENDED"; +static const char outsideGeofenceString[] = "OUTSIDE_GEOFENCE"; +static const char badPositioningAccuracyString[] = "BAD_POS_ACC"; + +static const char driveDirectionForwardString[] = "FORWARD"; +static const char driveDirectionReverseString[] = "REVERSE"; + +static const char readyToArmString[] = "READY"; +static const char notReadyToArmString[] = "NOT_READY"; + +/*! + * \brief objectStateToASCII Converts an enumerated object state into a printable string + * \param state Variable to be converted + * \return Pointer to a string of the state + */ +const char * objectStateToASCII(const ObjectStateType state) { + switch (state) { + case OBJECT_STATE_DISARMED: + return disarmedStateString; + case OBJECT_STATE_ARMED: + return armedStateString; + case OBJECT_STATE_RUNNING: + return runningStateString; + case OBJECT_STATE_POSTRUN: + return postrunStateString; + case OBJECT_STATE_ABORTING: + return abortingStateString; + case OBJECT_STATE_REMOTE_CONTROL: + return remoteControlStateString; + case OBJECT_STATE_UNKNOWN: + default: + return unknownStateString; + } +} + +/*! + * \brief ASCIIToObjectState Converts a string into an enumerated state + * \param asciiString String to convert + * \return Value according to ::ObjectStateType + */ +ObjectStateType ASCIIToObjectState(const char * asciiString) { + if (strstr(asciiString, disarmedStateString) != NULL) + return OBJECT_STATE_DISARMED; + if (strstr(asciiString, armedStateString) != NULL) + return OBJECT_STATE_ARMED; + if (strstr(asciiString, runningStateString) != NULL) + return OBJECT_STATE_RUNNING; + if (strstr(asciiString, postrunStateString) != NULL) + return OBJECT_STATE_POSTRUN; + if (strstr(asciiString, abortingStateString) != NULL) + return OBJECT_STATE_ABORTING; + if (strstr(asciiString, remoteControlStateString) != NULL) + return OBJECT_STATE_REMOTE_CONTROL; + return OBJECT_STATE_UNKNOWN; +} + + +/*! + * \brief hasError Returns whether or not an ObjectErrorType contains an error + * \param error Struct containing possible errors + * \return True if any of the errors are set + */ +bool hasError(const ObjectErrorType error) { + return error.engineFault || error.abortRequest || error.batteryFault || error.unknownError + || error.syncPointEnded || error.outsideGeofence || error.badPositioningAccuracy; +} + +/*! + * \brief errorStatusToASCII Converts an object error struct to ASCII text and prints it to a buffer + * \param error Struct to be converted + * \param asciiBuffer Buffer to which text is to be written + * \param bufferLength Size of buffer to which text is to be written + */ +void errorStatusToASCII(const ObjectErrorType error, char* asciiBuffer, const size_t bufferLength) { + + memset(asciiBuffer, 0, bufferLength); + + if (!hasError(error)) { + snprintf(asciiBuffer, bufferLength, noErrorString); + return; + } + if (error.engineFault) + snprintf(asciiBuffer, bufferLength, "%s,", engineFaultString); + if (error.batteryFault) + snprintf(asciiBuffer, bufferLength - strlen(asciiBuffer), + "%s,", batteryFaultString); + if (error.unknownError) + snprintf(asciiBuffer, bufferLength - strlen(asciiBuffer), + "%s,", otherErrorString); + if (error.syncPointEnded) + snprintf(asciiBuffer, bufferLength - strlen(asciiBuffer), + "%s,", syncPointEndedString); + if (error.outsideGeofence) + snprintf(asciiBuffer, bufferLength - strlen(asciiBuffer), + "%s,", outsideGeofenceString); + if (error.badPositioningAccuracy) + snprintf(asciiBuffer, bufferLength - strlen(asciiBuffer), + "%s,", badPositioningAccuracyString); + if (error.abortRequest) + snprintf(asciiBuffer, bufferLength - strlen(asciiBuffer), + "%s,", abortRequestString); + + if (asciiBuffer[strlen(asciiBuffer)] == ',') + asciiBuffer[strlen(asciiBuffer)] = '\0'; + + return; +} + +/*! + * \brief ASCIIToErrorStatus Converts a string with ASCII representation of object errors + * into a corresponding struct + * \param asciiString String to be parsed + * \return Value according to ::ObjectErrorType + */ +ObjectErrorType ASCIIToErrorStatus(const char * asciiString) { + ObjectErrorType error; + memset(&error, 0, sizeof (error)); + + // First check if string starts with the "all ok" string + if(strstr(asciiString, noErrorString) == asciiString) + return error; + + // Check against all error string representations + if(strstr(asciiString, batteryFaultString) != NULL) + error.batteryFault = true; + if(strstr(asciiString, engineFaultString) != NULL) + error.engineFault = true; + if(strstr(asciiString, abortRequestString) != NULL) + error.abortRequest = true; + if(strstr(asciiString, otherErrorString) != NULL) + error.unknownError = true; + if(strstr(asciiString, syncPointEndedString) != NULL) + error.syncPointEnded = true; + if(strstr(asciiString, outsideGeofenceString) != NULL) + error.outsideGeofence = true; + if(strstr(asciiString, badPositioningAccuracyString) != NULL) + error.badPositioningAccuracy = true; + + // Error unmatched against all strings casted into unknown type + if (!hasError(error)) + error.unknownError = true; + + return error; +} + +/*! + * \brief monitorDataToASCII Converts a monitor data struct into human readable ASCII text + * \param MONRData Struct containing monitor data + * \param asciiBuffer Buffer in which to print ASCII text representation + * \param bufferLength Length of ASCII buffer + * \return value according to ::ISOMessageReturnValue + */ +int monitorDataToASCII(const ObjectMonitorType * monitorData, char *asciiBuffer, const size_t bufferLength) { + + memset(asciiBuffer, 0, bufferLength); + + if (monitorData->isTimestampValid) + snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), + "%u;", TimeGetAsGPSqmsOfWeek(&monitorData->timestamp)); + else + snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), + "NaN;"); + + if (monitorData->position.isPositionValid) + snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), + "%.3f;%.3f;%.3f;", monitorData->position.xCoord_m, monitorData->position.yCoord_m, + monitorData->position.zCoord_m); + else + snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), + "NaN;NaN;NaN;"); + + if (monitorData->position.isHeadingValid) + snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), + "%.2f;", monitorData->position.heading_deg); + else + snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), + "NaN;"); + + if (monitorData->speed.isValid) + snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), + "%.2f;%.2f;", monitorData->speed.longitudinal_m_s, monitorData->speed.lateral_m_s); + else + snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), + "NaN;NaN;"); + + if (monitorData->acceleration.isValid) + snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), + "%.3f;%.3f;", monitorData->acceleration.longitudinal_m_s2, monitorData->acceleration.lateral_m_s2); + else + snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), + "NaN;NaN;"); + + if (monitorData->drivingDirection != OBJECT_DRIVE_DIRECTION_UNAVAILABLE) + snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), + "%s;", monitorData->drivingDirection == OBJECT_DRIVE_DIRECTION_FORWARD ? + driveDirectionForwardString : driveDirectionReverseString); + else + snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), + "UNKNOWN;"); + + snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), + "%s;", objectStateToASCII(monitorData->state)); + + if (monitorData->armReadiness != OBJECT_READY_TO_ARM_UNAVAILABLE) + snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), + "%s;", monitorData->armReadiness == OBJECT_READY_TO_ARM ? + readyToArmString : notReadyToArmString); + else + snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), + "UNKNOWN;"); + + errorStatusToASCII(monitorData->error, asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer)); + + snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), + ";"); + + return 0; +} + + + + +/*! + * \brief ASCIIToMonitorData Converts an ASCII string into a monitor data struct + * \param asciiBuffer Buffer containing ASCII text representation + * \param MONRData Struct containing monitor data + * \return 0 on success, -1 otherwise + */ +int ASCIIToObjectMonitorData(const char *asciiBuffer, ObjectMonitorType * monitorData) { + + const char *token; + char *endPtr; + const char delim[] = ";"; + const int NumberBaseDecimal = 10; + char *copy = strdup(asciiBuffer); + struct timeval currentTime; + TimeSetToCurrentSystemTime(¤tTime); + + memset(monitorData, 0, sizeof (*monitorData)); + + token = strtok(copy, delim); + + // Timestamp + token = strtok(NULL, delim); + TimeSetToGPStime(&monitorData->timestamp, TimeGetAsGPSweek(¤tTime), + (uint32_t) strtoul(token, &endPtr, NumberBaseDecimal)); + if (endPtr == token) { + monitorData->isTimestampValid = false; + memset(&monitorData->timestamp, 0, sizeof (monitorData->timestamp)); + } + + // Position + monitorData->position.isPositionValid = true; + + token = strtok(NULL, delim); + monitorData->position.xCoord_m = (int32_t) strtol(token, &endPtr, NumberBaseDecimal); + if (endPtr == token) { + monitorData->position.isPositionValid = false; + } + + token = strtok(NULL, delim); + monitorData->position.yCoord_m = (int32_t) strtol(token, &endPtr, NumberBaseDecimal); + if (endPtr == token) { + monitorData->position.isPositionValid = false; + } + + token = strtok(NULL, delim); + monitorData->position.zCoord_m = (int32_t) strtol(token, &endPtr, NumberBaseDecimal); + if (endPtr == token) { + monitorData->position.isPositionValid = false; + } + + token = strtok(NULL, delim); + monitorData->position.heading_deg = (uint16_t) strtoul(token, &endPtr, NumberBaseDecimal); + if (endPtr == token) { + monitorData->position.isHeadingValid = false; + } + + // Velocity + monitorData->speed.isValid = true; + + token = strtok(NULL, delim); + monitorData->speed.longitudinal_m_s = (int16_t) strtol(token, &endPtr, NumberBaseDecimal); + if (endPtr == token) { + monitorData->speed.isValid = false; + } + + token = strtok(NULL, delim); + monitorData->speed.lateral_m_s = (int16_t) strtol(token, &endPtr, NumberBaseDecimal); + if (endPtr == token) { + monitorData->speed.isValid = false; + } + + // Acceleration + monitorData->acceleration.isValid = true; + + token = strtok(NULL, delim); + monitorData->acceleration.longitudinal_m_s2 = (int16_t) strtol(token, &endPtr, NumberBaseDecimal); + if (endPtr == token) { + monitorData->acceleration.isValid = false; + } + + token = strtok(NULL, delim); + monitorData->acceleration.lateral_m_s2 = (int16_t) strtol(token, &endPtr, NumberBaseDecimal); + if (endPtr == token) { + monitorData->acceleration.isValid = false; + } + + // Drive direction + token = strtok(NULL, delim); + if (strstr(token, driveDirectionForwardString) != NULL) { + monitorData->drivingDirection = OBJECT_DRIVE_DIRECTION_FORWARD; + } + else if (strstr(token, driveDirectionReverseString) != NULL) { + monitorData->drivingDirection = OBJECT_DRIVE_DIRECTION_BACKWARD; + } + else { + monitorData->drivingDirection = OBJECT_DRIVE_DIRECTION_UNAVAILABLE; + } + + // State + token = strtok(NULL, delim); + monitorData->state = ASCIIToObjectState(token); + + // Ready to arm + token = strtok(NULL, delim); + if (strstr(token, readyToArmString) != NULL) { + monitorData->armReadiness = OBJECT_READY_TO_ARM; + } + else if (strstr(token, notReadyToArmString) != NULL) { + monitorData->armReadiness = OBJECT_NOT_READY_TO_ARM; + } + else { + monitorData->armReadiness = OBJECT_READY_TO_ARM_UNAVAILABLE; + } + + // Error status + token = strtok(NULL, delim); + monitorData->error = ASCIIToErrorStatus(token); + + return 0; +} diff --git a/server/src/util.c b/server/src/util.c index 9d093b44e..0486dd61e 100644 --- a/server/src/util.c +++ b/server/src/util.c @@ -689,27 +689,6 @@ int UtilSetSlaveObject(ObjectPosition * OP, char *Filename, char debug) { } -/*! - * \brief MONRToCartesianPosition Extracts a CartesianPosition from MONR - * \param MONR Struct containing MONR data - * \return CartesianPosition struct containing the point represented by MONR - */ -CartesianPosition MONRToCartesianPosition(MonitorDataType MONR) { - CartesianPosition retval; - - retval.xCoord_m = MONR.MONR.xPosition / 1000.0; - retval.yCoord_m = MONR.MONR.yPosition / 1000.0; - retval.zCoord_m = MONR.MONR.zPosition / 1000.0; - if (MONR.MONR.heading == 36001) { // 36001: unavailable - LogMessage(LOG_LEVEL_DEBUG, "MONR heading unavailable, assuming 0"); - retval.heading_deg = 0.0; - } - else { - retval.heading_deg = MONR.MONR.heading / 100.0; - } - return retval; -} - /*! * \brief UtilMonitorDataToString Converts the data from a message queue monitor data struct into ASCII format * \param monrData Struct containing relevant monitor data @@ -717,14 +696,13 @@ CartesianPosition MONRToCartesianPosition(MonitorDataType MONR) { * \param stringLength Length of string in which converted data is to be placed * \return 0 upon success, -1 otherwise */ -int UtilMonitorDataToString(MonitorDataType monrData, char *monrString, size_t stringLength) { - memset(monrString, 0, stringLength); - inet_ntop(AF_INET, &monrData.ClientIP, monrString, +int UtilMonitorDataToString(MonitorDataType monitorData, char *monitorDataString, size_t stringLength) { + memset(monitorDataString, 0, stringLength); + inet_ntop(AF_INET, &monitorData.ClientIP, monitorDataString, (stringLength > UINT_MAX) ? UINT_MAX : (socklen_t) stringLength); - strcat(monrString, ";0;"); - if (MONRToASCII(&monrData.MONR, monrString + strlen(monrString), stringLength - strlen(monrString), 0) != - MESSAGE_OK) { - memset(monrString, 0, stringLength); + strcat(monitorDataString, ";0;"); + if (monitorDataToASCII(&monitorData.data, monitorDataString + strlen(monitorDataString), stringLength - strlen(monitorDataString)) != 0) { + memset(monitorDataString, 0, stringLength); return -1; } return 0; From 6ff017be03e102b4db4d14b7a9bfefb0d37d9b79 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 4 Feb 2020 16:29:12 +0100 Subject: [PATCH 208/523] Missed removing declarations in iso22133 header --- server/inc/iso22133.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/server/inc/iso22133.h b/server/inc/iso22133.h index d1f357831..474688901 100644 --- a/server/inc/iso22133.h +++ b/server/inc/iso22133.h @@ -229,8 +229,6 @@ ssize_t decodeTREOMessage(); ssize_t encodeACCMMessage(const uint16_t* actionID, const ActionType_t* actionType, const ActionTypeParameter_t* param1, const ActionTypeParameter_t* param2, const 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 encodeINSUPMessage(const SupervisorCommandType, char * insupDataBuffer, const size_t bufferLength, const char debug); -ISOMessageReturnValue MONRToASCII(const MONRType * MONRData, char * asciiBuffer, const size_t bufferLength, const char debug); -ISOMessageReturnValue ASCIIToMONR(const char * asciiBuffer, MONRType * MONRData, const char debug); ISOMessageID getISOMessageType(const char * messageData, const size_t length, const char debug); From 50e82791b899d941cee19ab2773cf8d9e9692254 Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Thu, 6 Feb 2020 08:53:42 +0100 Subject: [PATCH 209/523] Fixes --- server/src/datadictionary.c | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/server/src/datadictionary.c b/server/src/datadictionary.c index 7e133fd77..7a982f62f 100644 --- a/server/src/datadictionary.c +++ b/server/src/datadictionary.c @@ -95,9 +95,6 @@ ReadWriteAccess_t DataDictionaryConstructor(GSDType * GSD) { DataDictionarySetOBCStateU8(GSD, OBC_STATE_UNDEFINED); - //Allocate memory in data dict - - return Res; } @@ -1372,7 +1369,7 @@ ReadWriteAccess_t DataDictionaryInitSupervisorTCPPortU16(GSDType * GSD) { pthread_mutex_lock(&SupervisorTCPPortMutex); GSD->SupervisorTCPPortU16 = atoi(ResultBufferC8); pthread_mutex_unlock(&SupervisorTCPPortMutex); - } + } else { Res = PARAMETER_NOTFOUND; LogMessage(LOG_LEVEL_ERROR, "SupervisorTCPPort not found!"); @@ -1720,7 +1717,7 @@ ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, const MONRType * MONR, co Res = WRITE_OK; pthread_mutex_lock(&MONRMutex); if (GSD->MonrMessages != NULL && transmitterId < GSD->numberOfObjects) { - GSD->MonrMessages[0] = *MONR; + GSD->MonrMessages[transmitterId] = *MONR; } pthread_mutex_unlock(&MONRMutex); From a7810d7d122959736d2cce4d3d0280ce0445e10d Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Thu, 6 Feb 2020 09:00:12 +0100 Subject: [PATCH 210/523] Update server/src/datadictionary.c Co-Authored-By: LukasWikander --- server/src/datadictionary.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/server/src/datadictionary.c b/server/src/datadictionary.c index 7a982f62f..357028942 100644 --- a/server/src/datadictionary.c +++ b/server/src/datadictionary.c @@ -1686,7 +1686,7 @@ ReadWriteAccess_t DataDictionaryInitMONR(GSDType * GSD) { stat(filePath, &st); //printf("File size on open: %ld\n", st.st_size); - +// this memory does not change size as more MONR messages are added, and it is unclear where in the memory stuff is being written lseek(fd, (sizeof (MONRType)) - 1, SEEK_SET); write(fd, "", 1); From 88c7a0d5d7841e6fca8edce91070bb75dbb8ec2d Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Thu, 6 Feb 2020 09:43:09 +0100 Subject: [PATCH 211/523] removed number --- server/src/datadictionary.c | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/server/src/datadictionary.c b/server/src/datadictionary.c index 7a982f62f..2fc337ade 100644 --- a/server/src/datadictionary.c +++ b/server/src/datadictionary.c @@ -1681,17 +1681,14 @@ ReadWriteAccess_t DataDictionaryInitMONR(GSDType * GSD) { struct stat st; pthread_mutex_lock(&MONRMutex); - sprintf(filePath, "%smonrMessageMemory%d.mem", SHARED_MEMORY_PATH, 5); + sprintf(filePath, "%smonrMessageMemory.mem", SHARED_MEMORY_PATH); fd = open(filePath, O_RDWR | O_CREAT, S_IRUSR | S_IWUSR); stat(filePath, &st); - //printf("File size on open: %ld\n", st.st_size); - lseek(fd, (sizeof (MONRType)) - 1, SEEK_SET); write(fd, "", 1); stat(filePath, &st); - //printf("File size after writing: %ld\n", st.st_size); // Map memory to created file GSD->MonrMessages = @@ -1719,6 +1716,7 @@ ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, const MONRType * MONR, co if (GSD->MonrMessages != NULL && transmitterId < GSD->numberOfObjects) { GSD->MonrMessages[transmitterId] = *MONR; } + pthread_mutex_unlock(&MONRMutex); return Res; From 55ace78c5c2fed408544961a22284a7537c19923 Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Thu, 6 Feb 2020 10:13:18 +0100 Subject: [PATCH 212/523] Fixes --- server/src/datadictionary.c | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/server/src/datadictionary.c b/server/src/datadictionary.c index 958fd8a77..92cdfb846 100644 --- a/server/src/datadictionary.c +++ b/server/src/datadictionary.c @@ -1717,6 +1717,10 @@ ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, const MONRType * MONR, co if (GSD->MonrMessages != NULL && transmitterId < GSD->numberOfObjects) { GSD->MonrMessages[transmitterId] = *MONR; } + else{ + Res = UNDEFINED; + LogPrint(LOG_LEVEL_ERROR, "Unable to write MonrMessage in DataDictionary"); + } pthread_mutex_unlock(&MONRMutex); @@ -1731,12 +1735,19 @@ ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, const MONRType * MONR, co * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetMONR(GSDType * GSD, MONRType * MONR, const U32 transmitterId) { - pthread_mutex_lock(&MONRMutex); + ReadWriteAccess_t Res; + Res = READ_OK; + + pthread_mutex_lock(&MONRMutex); if (GSD->MonrMessages != NULL && transmitterId < GSD->numberOfObjects) { *MONR = GSD->MonrMessages[transmitterId]; - } + } + else{ + Res = UNDEFINED; + LogPrint(LOG_LEVEL_ERROR, "Unable to read MonrMessage in DataDictionary"); + } pthread_mutex_unlock(&MONRMutex); - return READ_OK; + return Res } /*! From 0555970d6e08e9396b6956a7710cc729c40e19ac Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Thu, 6 Feb 2020 10:25:19 +0100 Subject: [PATCH 213/523] Added error handling for init --- server/src/datadictionary.c | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/server/src/datadictionary.c b/server/src/datadictionary.c index 92cdfb846..0960df74c 100644 --- a/server/src/datadictionary.c +++ b/server/src/datadictionary.c @@ -1694,10 +1694,15 @@ ReadWriteAccess_t DataDictionaryInitMONR(GSDType * GSD) { // Map memory to created file GSD->MonrMessages = (MONRType *) mmap(NULL, (sizeof (MONRType)), PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0); - close(fd); - - LogMessage(LOG_LEVEL_INFO, "Init MonrMessage memory"); + if (GSD->MonrMessages == MAP_FAILED) { + LogPrint(LOG_LEVEL_ERROR, "mmap failed: %s", strerror(errno)); + close(fd); + } + else{ + LogMessage(LOG_LEVEL_INFO, "Init MonrMessage memory"); + } + close(fd); pthread_mutex_unlock(&MONRMutex); return Res; } @@ -1747,7 +1752,7 @@ ReadWriteAccess_t DataDictionaryGetMONR(GSDType * GSD, MONRType * MONR, const U3 LogPrint(LOG_LEVEL_ERROR, "Unable to read MonrMessage in DataDictionary"); } pthread_mutex_unlock(&MONRMutex); - return Res + return Res; } /*! From 84d854b51a4649571a679c8cf6df4b3d9fff040e Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 6 Feb 2020 16:50:36 +0100 Subject: [PATCH 214/523] Removed ScenarioControl from runServer --- runServer.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/runServer.sh b/runServer.sh index fd9f36d3e..26cd7120c 100755 --- a/runServer.sh +++ b/runServer.sh @@ -3,7 +3,7 @@ #### User settings # Modify this array by adding more modules to include them in the execution -MODULES=(ScenarioControl) +MODULES=() #### # Save top directory From 8f43742093124dd7f530407f32325105feac0b93 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 6 Feb 2020 16:51:40 +0100 Subject: [PATCH 215/523] Added positioning to CMakeLists.txt for modules --- modules/ScenarioControl/CMakeLists.txt | 2 ++ modules/Supervision/CMakeLists.txt | 2 ++ modules/Visualization/CMakeLists.txt | 2 ++ server/src/positioning.c | 12 ++++++------ 4 files changed, 12 insertions(+), 6 deletions(-) diff --git a/modules/ScenarioControl/CMakeLists.txt b/modules/ScenarioControl/CMakeLists.txt index 11ab4eb91..12b38bb57 100644 --- a/modules/ScenarioControl/CMakeLists.txt +++ b/modules/ScenarioControl/CMakeLists.txt @@ -39,6 +39,8 @@ add_library(util ../../server/inc/util.h ../../server/src/iso22133.c ../../server/inc/iso22133.h + ../../server/src/positioning.c + ../../server/inc/positioning.h ) add_library(Trigger diff --git a/modules/Supervision/CMakeLists.txt b/modules/Supervision/CMakeLists.txt index a6af6ab81..47d226e1a 100644 --- a/modules/Supervision/CMakeLists.txt +++ b/modules/Supervision/CMakeLists.txt @@ -38,6 +38,8 @@ add_library(util ../../server/inc/util.h ../../server/src/iso22133.c ../../server/inc/iso22133.h + ../../server/inc/positioning.h + ../../server/src/positioning.c ) add_executable(Supervision diff --git a/modules/Visualization/CMakeLists.txt b/modules/Visualization/CMakeLists.txt index 79b2b1b0c..655b3711a 100644 --- a/modules/Visualization/CMakeLists.txt +++ b/modules/Visualization/CMakeLists.txt @@ -39,6 +39,8 @@ add_library(util ../../server/inc/util.h ../../server/inc/iso22133.h ../../server/src/iso22133.c + ../../server/inc/positioning.h + ../../server/src/positioning.c ) add_executable(Visualization diff --git a/server/src/positioning.c b/server/src/positioning.c index 5aff418a2..ffd63089d 100644 --- a/server/src/positioning.c +++ b/server/src/positioning.c @@ -164,13 +164,13 @@ ObjectErrorType ASCIIToErrorStatus(const char * asciiString) { } /*! - * \brief monitorDataToASCII Converts a monitor data struct into human readable ASCII text + * \brief objectMonitorDataToASCII Converts a monitor data struct into human readable ASCII text * \param MONRData Struct containing monitor data * \param asciiBuffer Buffer in which to print ASCII text representation * \param bufferLength Length of ASCII buffer - * \return value according to ::ISOMessageReturnValue + * \return Number of bytes printed */ -int monitorDataToASCII(const ObjectMonitorType * monitorData, char *asciiBuffer, const size_t bufferLength) { +int objectMonitorDataToASCII(const ObjectMonitorType * monitorData, char *asciiBuffer, const size_t bufferLength) { memset(asciiBuffer, 0, bufferLength); @@ -234,16 +234,16 @@ int monitorDataToASCII(const ObjectMonitorType * monitorData, char *asciiBuffer, snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), ";"); - return 0; + return (int) strlen(asciiBuffer); } /*! - * \brief ASCIIToMonitorData Converts an ASCII string into a monitor data struct + * \brief ASCIIToObjectMonitorData Converts an ASCII string into a monitor data struct * \param asciiBuffer Buffer containing ASCII text representation - * \param MONRData Struct containing monitor data + * \param monitorData Struct containing monitor data * \return 0 on success, -1 otherwise */ int ASCIIToObjectMonitorData(const char *asciiBuffer, ObjectMonitorType * monitorData) { From aa4cc10c14cbff4d6bb55345c56684e403e6532c Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 6 Feb 2020 16:53:16 +0100 Subject: [PATCH 216/523] Made visualization use monitor data struct --- modules/Visualization/src/main.c | 51 ++++++++++++++++---------------- 1 file changed, 26 insertions(+), 25 deletions(-) diff --git a/modules/Visualization/src/main.c b/modules/Visualization/src/main.c index fb3b87868..b10095135 100644 --- a/modules/Visualization/src/main.c +++ b/modules/Visualization/src/main.c @@ -10,6 +10,7 @@ #include #include #include "logging.h" +#include "maestroTime.h" #include "util.h" /*------------------------------------------------------------ @@ -47,26 +48,25 @@ void vCreateVisualizationMessage(MonitorDataType * _monitorData, char *_visualiz inet_ntop(AF_INET, &_monitorData->ClientIP, ipStringBuffer, sizeof (ipStringBuffer))); char GPSMsOfWeekString[ENOUGH_BUFFER_SIZE]; - sprintf(GPSMsOfWeekString, "%u", _monitorData->MONR.gpsQmsOfWeek); + sprintf(GPSMsOfWeekString, "%u", TimeGetAsGPSqmsOfWeek(&_monitorData->data.timestamp)); char xPosString[ENOUGH_BUFFER_SIZE]; - sprintf(xPosString, "%d", _monitorData->MONR.xPosition); + sprintf(xPosString, "%.3f", _monitorData->data.position.xCoord_m); char yPosString[ENOUGH_BUFFER_SIZE]; - sprintf(yPosString, "%d", _monitorData->MONR.yPosition); + sprintf(yPosString, "%.3f", _monitorData->data.position.yCoord_m); char zPosString[ENOUGH_BUFFER_SIZE]; - sprintf(zPosString, "%d", _monitorData->MONR.zPosition); + sprintf(zPosString, "%.3f", _monitorData->data.position.zCoord_m); char headingString[ENOUGH_BUFFER_SIZE]; - sprintf(headingString, "%u", _monitorData->MONR.heading); + sprintf(headingString, "%.2f", _monitorData->data.position.heading_deg); char longSpeedString[ENOUGH_BUFFER_SIZE]; - sprintf(longSpeedString, "%d", _monitorData->MONR.longitudinalSpeed); + sprintf(longSpeedString, "%.3f", _monitorData->data.speed.longitudinal_m_s); char stateString[ENOUGH_BUFFER_SIZE]; - sprintf(stateString, "%u", _monitorData->MONR.state); - + sprintf(stateString, "%s", objectStateToASCII(_monitorData->data.state)); //Build message from MonitorStruct snprintf(_visualizationMessage, _sizeOfVisualizationMessage, "%s;%s;%s;%s;%s;%s;%s;%s;", @@ -77,15 +77,15 @@ void vCreateVisualizationMessage(MonitorDataType * _monitorData, char *_visualiz if (_debug) { //LogMessage(LOG_LEVEL_INFO, "%s", _visualizationMessage); - LogMessage(LOG_LEVEL_INFO, "IP: %s", ipStringBuffer); - LogMessage(LOG_LEVEL_INFO, "GPSQmsOfWeek: %u", _monitorData->MONR.gpsQmsOfWeek); - LogMessage(LOG_LEVEL_INFO, "X: %d", _monitorData->MONR.xPosition); - LogMessage(LOG_LEVEL_INFO, "Y: %d", _monitorData->MONR.yPosition); - LogMessage(LOG_LEVEL_INFO, "Z: %d", _monitorData->MONR.zPosition); - LogMessage(LOG_LEVEL_INFO, "Heading: %u", _monitorData->MONR.heading); - LogMessage(LOG_LEVEL_INFO, "LongSpeed: %d", _monitorData->MONR.longitudinalSpeed); - LogMessage(LOG_LEVEL_INFO, "State: %u", _monitorData->MONR.state); - LogMessage(LOG_LEVEL_INFO, "MESSAGE-SIZE = %d", _sizeOfVisualizationMessage); + LogPrint("IP: %s", ipStringBuffer); + LogPrint("GPSQmsOfWeek: %s", GPSMsOfWeekString); + LogPrint("X: %s", xPosString); + LogPrint("Y: %s", yPosString); + LogPrint("Z: %s", zPosString); + LogPrint("Heading: %s", headingString); + LogPrint("LongSpeed: %s", longSpeedString); + LogPrint("State: %s", stateString); + LogPrint("MESSAGE-SIZE = %d", _sizeOfVisualizationMessage); } } @@ -146,19 +146,20 @@ int main() { { //Populate the monitorType - UtilPopulateMonitorDataStruct(mqRecvData, (size_t) (sizeof (mqRecvData)), &monitorData, 0); + UtilPopulateMonitorDataStruct(mqRecvData, (size_t) (sizeof (mqRecvData)), &monitorData); char dummy[1]; int sizeOfVisualizationMessage; //Calculate size of incoming buffer - sizeOfVisualizationMessage = snprintf(dummy, sizeof (dummy), "%u;%d;%d;%d;%u;%d;%u;", - monitorData.MONR.gpsQmsOfWeek, - monitorData.MONR.xPosition, - monitorData.MONR.yPosition, - monitorData.MONR.zPosition, - monitorData.MONR.heading, - monitorData.MONR.longitudinalSpeed, monitorData.MONR.state); + sizeOfVisualizationMessage = snprintf(dummy, sizeof (dummy), "%u;%.3f;%.3f;%.3f;%.2f;%.2f;%s;", + TimeGetAsGPSqmsOfWeek(&monitorData.data.timestamp), + monitorData.data.position.xCoord_m, + monitorData.data.position.yCoord_m, + monitorData.data.position.zCoord_m, + monitorData.data.position.heading_deg, + monitorData.data.speed.longitudinal_m_s, + objectStateToASCII(monitorData.data.state)); sizeOfVisualizationMessage += INET_ADDRSTRLEN; sizeOfVisualizationMessage += 8; //(;) From 6500ef64ab4594482d0b5f6dd84d2bf7b8d558da Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 6 Feb 2020 16:54:01 +0100 Subject: [PATCH 217/523] Added object ID as return value for MONR parsing function --- server/inc/iso22133.h | 2 +- server/src/iso22133.c | 4 +++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/server/inc/iso22133.h b/server/inc/iso22133.h index 474688901..e7678458b 100644 --- a/server/inc/iso22133.h +++ b/server/inc/iso22133.h @@ -217,7 +217,7 @@ typedef enum { SUPERVISOR_COMMAND_DEBUG = 2 //!< Place supervisor in debug mode } SupervisorCommandType; -ISOMessageReturnValue decodeMONRMessage(const char * monrDataBuffer, const size_t bufferLength, ObjectMonitorType * MonitorData, const char debug); +ISOMessageReturnValue decodeMONRMessage(const char * monrDataBuffer, const size_t bufferLength, uint32_t * objectID, ObjectMonitorType * MonitorData, const char debug); ssize_t encodeSTRTMessage(const struct timeval* timeOfStart, char * strtDataBuffer, const size_t bufferLength, const char debug); ssize_t encodeOSEMMessage(const double * latitude_deg, const double * longitude_deg, const float * altitude_m, const float * maxPositionDeviation_m, const float * maxLateralDeviation_m, const float * minimumPositioningAccuracy_m, char * osemDataBuffer, const size_t bufferLength, const char debug); ssize_t encodeOSTMMessage(const ObjectCommandType command, char * ostmDataBuffer, const size_t bufferLength, const char debug); diff --git a/server/src/iso22133.c b/server/src/iso22133.c index e1b209565..0264ac0dd 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -919,7 +919,7 @@ ssize_t encodeHEABMessage(const ControlCenterStatusType status, char *heabDataBu * \param debug Flag for enabling of debugging * \return value according to ::ISOMessageReturnValue */ -ISOMessageReturnValue decodeMONRMessage(const char *monrDataBuffer, const size_t bufferLength, ObjectMonitorType * monitorData, +ISOMessageReturnValue decodeMONRMessage(const char *monrDataBuffer, const size_t bufferLength, uint32_t* objectID, ObjectMonitorType * monitorData, const char debug) { MONRType MONRData; @@ -931,6 +931,7 @@ ISOMessageReturnValue decodeMONRMessage(const char *monrDataBuffer, const size_t ISOMessageReturnValue retval = MESSAGE_OK; memset(monitorData, 0, sizeof (*monitorData)); + *objectID = 0; // Decode ISO header if ((retval = decodeISOHeader(p, bufferLength, &MONRData.header, debug)) != MESSAGE_OK) { @@ -938,6 +939,7 @@ ISOMessageReturnValue decodeMONRMessage(const char *monrDataBuffer, const size_t return retval; } p += sizeof (MONRData.header); + *objectID = MONRData.header.TransmitterIdU8; // If message is not a MONR message, generate an error if (MONRData.header.MessageIdU16 != MESSAGE_ID_MONR) { From b0174e53d1ef5e55b0405b5682a1f161602fb7b5 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 6 Feb 2020 16:54:39 +0100 Subject: [PATCH 218/523] Made scenariocontrol use new monitor data struct --- modules/ScenarioControl/src/main.cpp | 2 +- modules/ScenarioControl/src/scenario.cpp | 15 ++++++++++----- 2 files changed, 11 insertions(+), 6 deletions(-) diff --git a/modules/ScenarioControl/src/main.cpp b/modules/ScenarioControl/src/main.cpp index 821dc8be1..840be4d45 100644 --- a/modules/ScenarioControl/src/main.cpp +++ b/modules/ScenarioControl/src/main.cpp @@ -127,7 +127,7 @@ int main() break; case COMM_MONR: // Update triggers - UtilPopulateMonitorDataStruct(reinterpret_cast(mqRecvData), sizeof(mqRecvData), &monr, 0); + UtilPopulateMonitorDataStruct(mqRecvData, sizeof(mqRecvData), &monr); scenario.updateTrigger(monr); break; case COMM_DISCONNECT: diff --git a/modules/ScenarioControl/src/scenario.cpp b/modules/ScenarioControl/src/scenario.cpp index 3b159042d..1a4abfe0c 100644 --- a/modules/ScenarioControl/src/scenario.cpp +++ b/modules/ScenarioControl/src/scenario.cpp @@ -432,11 +432,16 @@ Scenario::ScenarioReturnCode_t Scenario::updateTrigger(const MonitorDataType &mo { switch (tp->getTypeCode()) { - case Trigger::TriggerTypeCode_t::TRIGGER_BRAKE: - struct timeval monrTime, currentTime; - TimeSetToCurrentSystemTime(¤tTime); - TimeSetToGPStime(&monrTime, TimeGetAsGPSweek(¤tTime), monr.MONR.gpsQmsOfWeek); - tp->update(static_cast(monr.MONR.longitudinalSpeed)/100.0, monrTime); + case Trigger::TriggerTypeCode_t::TRIGGER_BRAKE: + if (monr.data.speed.isValid && monr.data.isTimestampValid) + { + tp->update(monr.data.speed.longitudinal_m_s, monr.data.timestamp); + } + else + { + LogMessage(LOG_LEVEL_WARNING, "Could not update trigger type %s due to invalid monitor data values", + tp->getTypeAsString(tp->getTypeCode()).c_str()); + } break; default: LogMessage(LOG_LEVEL_WARNING, "Unhandled trigger type in update: %s", From 3631326cf52e78fa7800007443adfc8602cdb7da Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 6 Feb 2020 16:54:58 +0100 Subject: [PATCH 219/523] Made supervision use monitor data struct --- modules/Supervision/src/main.cpp | 52 ++++++++++++-------------- modules/Supervision/src/trajectory.cpp | 2 + 2 files changed, 26 insertions(+), 28 deletions(-) diff --git a/modules/Supervision/src/main.cpp b/modules/Supervision/src/main.cpp index b451d4624..758beeaf3 100644 --- a/modules/Supervision/src/main.cpp +++ b/modules/Supervision/src/main.cpp @@ -31,7 +31,7 @@ typedef enum { /*------------------------------------------------------------ -- Private functions ------------------------------------------------------------*/ -static bool isViolatingGeofence(const MonitorDataType &MONRdata, std::vector geofences); +static bool isViolatingGeofence(const MonitorDataType &monitorData, std::vector geofences); static void loadGeofenceFiles(std::vector &geofences); static void loadTrajectoryFiles(std::vector &trajectories); static Geofence parseGeofenceFile(const std::string geofenceFile); @@ -107,26 +107,26 @@ int main() break; case COMM_MONR: - MonitorDataType MONRMessage; - UtilPopulateMonitorDataStruct((unsigned char *)mqRecvData, sizeof (mqRecvData), &MONRMessage, 0); + MonitorDataType monitorMessage; + UtilPopulateMonitorDataStruct(mqRecvData, sizeof (mqRecvData), &monitorMessage); - if (state.get() == SupervisionState::RUNNING && isViolatingGeofence(MONRMessage, geofences)) { + if (state.get() == SupervisionState::RUNNING && isViolatingGeofence(monitorMessage, geofences)) { LogMessage(LOG_LEVEL_WARNING, "Object with IP %s is violating a geofence: sending ABORT", - inet_ntop(AF_INET, &MONRMessage.ClientIP, ipString, sizeof (ipString))); + inet_ntop(AF_INET, &monitorMessage.ClientIP, ipString, sizeof (ipString))); iCommSend(COMM_ABORT, nullptr, 0); state.set(SupervisionState::READY); } if (state.get() == SupervisionState::VERIFYING_ARM) { - if (isViolatingGeofence(MONRMessage, geofences)) { + if (isViolatingGeofence(monitorMessage, geofences)) { LogMessage(LOG_LEVEL_INFO, "Arm not approved: object with IP address %s is violating a geofence", - inet_ntop(AF_INET, &MONRMessage.ClientIP, ipString, sizeof (ipString))); + inet_ntop(AF_INET, &monitorMessage.ClientIP, ipString, sizeof (ipString))); iCommSend(COMM_DISARM, nullptr, 0); state.set(SupervisionState::READY); break; } - switch (updateNearStartingPositionStatus(MONRMessage, armVerified)) { + switch (updateNearStartingPositionStatus(monitorMessage, armVerified)) { case SINGLE_OBJECT_NOT_NEAR_START: // Object not near start: disarm LogMessage(LOG_LEVEL_INFO, "Arm not approved: sending disarm"); iCommSend(COMM_DISARM, nullptr, 0); @@ -436,23 +436,19 @@ Geofence parseGeofenceFile(const std::string geofenceFile) { /*! * \brief SupervisionCheckGeofences Checks all geofences to verify that the point represented by the MONR data lies within all permitted geofences and outside all forbidden geofences - * \param MONRdata MONR struct containing the object coordinate data + * \param monitorData monitor data struct containing the object coordinate data * \param geofences Vector containing all geofences * \return True if MONR coordinate violates a geofence, false if not. */ -bool isViolatingGeofence(const MonitorDataType &MONRdata, std::vector geofences) { +bool isViolatingGeofence(const MonitorDataType &monitorData, std::vector geofences) { - const CartesianPosition monrPoint = - { - MONRdata.MONR.xPosition / 1000.0, MONRdata.MONR.yPosition / 1000.0, - MONRdata.MONR.zPosition / 1000.0, 0.0 - }; + const CartesianPosition monitorPoint = monitorData.data.position; char isInPolygon = 0; int retval = false; for (Geofence geofence : geofences) { - isInPolygon = UtilIsPointInPolygon(monrPoint, geofence.polygonPoints.data(), + isInPolygon = UtilIsPointInPolygon(monitorPoint, geofence.polygonPoints.data(), static_cast(geofence.polygonPoints.size())); if (isInPolygon == -1) { LogMessage(LOG_LEVEL_WARNING, "No points in polygon"); @@ -466,12 +462,12 @@ bool isViolatingGeofence(const MonitorDataType &MONRdata, std::vector else { if (geofence.isPermitted) LogMessage(LOG_LEVEL_WARNING, - "Object with MONR transmitter ID %u is outside a permitted area %s", - MONRdata.MONR.header.TransmitterIdU8, geofence.name.c_str()); + "Object with ID %u is outside a permitted area %s", + monitorData.ClientID, geofence.name.c_str()); else LogMessage(LOG_LEVEL_WARNING, - "Object with MONR transmitter ID %u is inside a forbidden area %s", - MONRdata.MONR.header.TransmitterIdU8, geofence.name.c_str()); + "Object with ID %u is inside a forbidden area %s", + monitorData.ClientID, geofence.name.c_str()); retval = true; } } @@ -480,31 +476,31 @@ bool isViolatingGeofence(const MonitorDataType &MONRdata, std::vector } /*! - * \brief updateNearStartingPositionStatus Loops through the armVerified vector for a trajectory that matches the input MONR packet, + * \brief updateNearStartingPositionStatus Loops through the armVerified vector for a trajectory that matches the input monitor data packet, * and sets the armVerified boolean to true if the object is near its starting position. It also performs a check on the entire vector * to determine whether all objects have been verified near starting position. - * \param MONRdata MONR struct containing the object coordinate data + * \param monitorData Monitor data struct containing the object coordinate data * \param armVerified Vector containing trajectories paired with a boolean showing whether or not the associated object has been previously verified to be near its starting position * \return A value according to ::PositionStatus */ -PositionStatus updateNearStartingPositionStatus(const MonitorDataType &MONRdata, std::vector> armVerified) { +PositionStatus updateNearStartingPositionStatus(const MonitorDataType &monitorData, std::vector> armVerified) { char ipString[INET_ADDRSTRLEN]; for (std::pair &element : armVerified) { - if (element.first.ip == MONRdata.ClientIP) { + if (element.first.ip == monitorData.ClientIP) { if (element.first.points.empty()) { element.second = true; return OBJECT_HAS_NO_TRAJECTORY; } CartesianPosition trajectoryPoint = element.first.points.front().getCartesianPosition(); - CartesianPosition objectPosition = MONRToCartesianPosition(MONRdata); + CartesianPosition objectPosition = monitorData.data.position; if (UtilIsPositionNearTarget(objectPosition, trajectoryPoint, ARM_MAX_DISTANCE_TO_START_M) && UtilIsAngleNearTarget(objectPosition, trajectoryPoint, ARM_MAX_ANGLE_TO_START_DEG)) { if (element.second == false) { LogMessage(LOG_LEVEL_INFO, "Object with IP %s and position (%.2f, %.2f, %.2f) detected within %.2f m and %.2f degrees of the first point (%.2f, %.2f, %.2f) in trajectory %s", - inet_ntop(AF_INET, &MONRdata.ClientIP, ipString, sizeof (ipString)), + inet_ntop(AF_INET, &monitorData.ClientIP, ipString, sizeof (ipString)), objectPosition.xCoord_m, objectPosition.yCoord_m, objectPosition.zCoord_m, ARM_MAX_DISTANCE_TO_START_M, ARM_MAX_ANGLE_TO_START_DEG, trajectoryPoint.xCoord_m, trajectoryPoint.yCoord_m, trajectoryPoint.zCoord_m, @@ -523,14 +519,14 @@ PositionStatus updateNearStartingPositionStatus(const MonitorDataType &MONRdata, else { if (!UtilIsPositionNearTarget(objectPosition, trajectoryPoint, ARM_MAX_DISTANCE_TO_START_M)) { LogMessage(LOG_LEVEL_INFO, "Object with IP %s and position (%.2f, %.2f, %.2f) farther than %.2f m from first point (%.2f, %.2f, %.2f) in trajectory %s", - inet_ntop(AF_INET, &MONRdata.ClientIP, ipString, sizeof (ipString)), + inet_ntop(AF_INET, &monitorData.ClientIP, ipString, sizeof (ipString)), objectPosition.xCoord_m, objectPosition.yCoord_m, objectPosition.zCoord_m, ARM_MAX_DISTANCE_TO_START_M, trajectoryPoint.xCoord_m, trajectoryPoint.yCoord_m, trajectoryPoint.zCoord_m, element.first.points.front().getZCoord(), element.first.name.c_str()); } else { LogMessage(LOG_LEVEL_INFO, "Object with IP %s (heading: %.2f degrees) not facing direction specified by first point (heading: %.2f degrees) in trajectory %s (tolerance: %.2f degrees)", - inet_ntop(AF_INET, &MONRdata.ClientIP, ipString, sizeof (ipString)), objectPosition.heading_deg, + inet_ntop(AF_INET, &monitorData.ClientIP, ipString, sizeof (ipString)), objectPosition.heading_deg, trajectoryPoint.heading_deg, element.first.name.c_str(), ARM_MAX_ANGLE_TO_START_DEG); } element.second = false; diff --git a/modules/Supervision/src/trajectory.cpp b/modules/Supervision/src/trajectory.cpp index f0ea311ea..a7196dda2 100644 --- a/modules/Supervision/src/trajectory.cpp +++ b/modules/Supervision/src/trajectory.cpp @@ -130,5 +130,7 @@ CartesianPosition Trajectory::TrajectoryPoint::getCartesianPosition() { retval.zCoord_m = 0.0; } retval.heading_deg = this->getHeading() * 180.0 / M_PI; + retval.isHeadingValid = true; + retval.isPositionValid = true; return retval; } From a7c6640a28b71b57bb0794a70272fd1a503c5cb5 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 6 Feb 2020 16:57:19 +0100 Subject: [PATCH 220/523] Added client ID as member of monitor data struct --- server/inc/util.h | 1 + 1 file changed, 1 insertion(+) diff --git a/server/inc/util.h b/server/inc/util.h index 9186d990e..04fd4b91b 100644 --- a/server/inc/util.h +++ b/server/inc/util.h @@ -260,6 +260,7 @@ typedef struct { ObjectMonitorType data; in_addr_t ClientIP; + uint32_t ClientID; } MonitorDataType; typedef struct From f739510d21d1826e1d886abcca8b5bc689ac2b7f Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 6 Feb 2020 16:58:00 +0100 Subject: [PATCH 221/523] Rewrote toString functions for monitor data --- server/inc/util.h | 2 +- server/src/util.c | 163 +++++----------------------------------------- 2 files changed, 19 insertions(+), 146 deletions(-) diff --git a/server/inc/util.h b/server/inc/util.h index 04fd4b91b..35ef6157f 100644 --- a/server/inc/util.h +++ b/server/inc/util.h @@ -683,7 +683,7 @@ void UtilGetGeofenceDirectoryPath(char* path, size_t pathLen); int UtilCheckTrajectoryFileFormat(const char *path, size_t pathLen); -int UtilMonitorDataToString(MonitorDataType monrData, char* monrString, size_t stringLength); +int UtilMonitorDataToString(const MonitorDataType monrData, char* monrString, size_t stringLength); int UtilStringToMonitorData(const char* monrString, size_t stringLength, MonitorDataType * monrData); uint8_t UtilIsPositionNearTarget(CartesianPosition position, CartesianPosition target, double tolerance_m); uint8_t UtilIsAngleNearTarget(CartesianPosition position, CartesianPosition target, double tolerance_deg); diff --git a/server/src/util.c b/server/src/util.c index 0486dd61e..4b0ce747b 100644 --- a/server/src/util.c +++ b/server/src/util.c @@ -696,12 +696,13 @@ int UtilSetSlaveObject(ObjectPosition * OP, char *Filename, char debug) { * \param stringLength Length of string in which converted data is to be placed * \return 0 upon success, -1 otherwise */ -int UtilMonitorDataToString(MonitorDataType monitorData, char *monitorDataString, size_t stringLength) { +int UtilMonitorDataToString(const MonitorDataType monitorData, char *monitorDataString, size_t stringLength) { memset(monitorDataString, 0, stringLength); inet_ntop(AF_INET, &monitorData.ClientIP, monitorDataString, (stringLength > UINT_MAX) ? UINT_MAX : (socklen_t) stringLength); strcat(monitorDataString, ";0;"); - if (monitorDataToASCII(&monitorData.data, monitorDataString + strlen(monitorDataString), stringLength - strlen(monitorDataString)) != 0) { + + if (objectMonitorDataToASCII(&monitorData.data, monitorDataString + strlen(monitorDataString), stringLength - strlen(monitorDataString)) < 0) { memset(monitorDataString, 0, stringLength); return -1; } @@ -715,24 +716,24 @@ int UtilMonitorDataToString(MonitorDataType monitorData, char *monitorDataString * \param monrData Struct containing relevant monitor data * \return 0 upon success, -1 otherwise */ -int UtilStringToMonitorData(const char *monrString, size_t stringLength, MonitorDataType * monrData) { +int UtilStringToMonitorData(const char *monitorString, size_t stringLength, MonitorDataType * monitorData) { const char *token; const char delim[] = ";"; struct in_addr addr; - char *copy = strdup(monrString); + char *copy = strdup(monitorString); token = strtok(copy, delim); // IP address inet_pton(AF_INET, token, &addr); - monrData->ClientIP = addr.s_addr; + monitorData->ClientIP = addr.s_addr; // Skip the 0 token = strtok(NULL, delim); // MONR data token = strtok(NULL, delim); - if (ASCIIToMONR(token, &monrData->MONR, 0) == MESSAGE_OK) + if (ASCIIToObjectMonitorData(token, &monitorData->data) == 0) return 0; else return -1; @@ -3584,150 +3585,22 @@ I32 UtilWriteConfigurationParameter(C8 * ParameterName, C8 * NewValue, U8 Debug) } /*! - * \brief UtilPopulateMonitorDataStruct Takes an array of raw MONR data and fills a MONRType struct with the content - * \param rawMONR Array of raw MONR data - * \param rawMONRsize Size of raw MONR data array - * \param MONR Struct where MONR data should be placed - * \param debug Boolean enabling debug printouts + * \brief UtilPopulateMonitorDataStruct Takes an array of raw monitor data and fills a monitor data struct with the content + * \param rawData Array of raw monitor data + * \param rawDataSize Size of raw monitor data array + * \param MONR Struct where monitor data should be placed * \return -1 on failure, 0 on success */ -I32 UtilPopulateMonitorDataStruct(C8 * rawMONR, size_t rawMONRsize, MonitorDataType * monitorData, U8 debug) { - U16 Crc = 0, U16Data = 0; - I16 I16Data = 0; - U32 U32Data = 0; - I32 I32Data = 0; - U64 U64Data = 0; - C8 *rdPtr = rawMONR, *monrStruct; // Pointer to keep track of where in rawMONR we are currently reading - U16 contentLength = 0; - in_addr_t IPData = 0; - const size_t monrPacketSize = sizeof (monitorData->MONR) - sizeof (monitorData->MONR.header) - - sizeof (monitorData->MONR.footer.Crc) - sizeof (monitorData->MONR.monrStructValueID) - - sizeof (monitorData->MONR.monrStructContentLength); - - if (rawMONRsize < sizeof (MONRType)) { - LogMessage(LOG_LEVEL_ERROR, "Raw MONR array too small to hold all necessary MONR data, %d < %d.", - rawMONRsize, sizeof (MONRType)); - return -1; - } - - // ISO message header - memcpy(&U16Data, rdPtr, sizeof (U16Data)); - monitorData->MONR.header.SyncWordU16 = U16Data; - rdPtr += sizeof (U16Data); - U16Data = 0; - - monitorData->MONR.header.TransmitterIdU8 = *(rdPtr++); - monitorData->MONR.header.MessageCounterU8 = *(rdPtr++); - monitorData->MONR.header.AckReqProtVerU8 = *(rdPtr++); - - memcpy(&U16Data, rdPtr, sizeof (U16Data)); - monitorData->MONR.header.MessageIdU16 = U16Data; - rdPtr += sizeof (U16Data); - U16Data = 0; - - memcpy(&U32Data, rdPtr, sizeof (U32Data)); - monitorData->MONR.header.MessageLengthU32 = U32Data; - rdPtr += sizeof (U32Data); - U32Data = 0; - - // ISO content header? - memcpy(&U16Data, rdPtr, sizeof (U16Data)); - if (U16Data == VALUE_ID_MONR_STRUCT) { - rdPtr += sizeof (U16Data); - - memcpy(&contentLength, rdPtr, sizeof (contentLength)); - rdPtr += sizeof (contentLength); - - - if (contentLength < monrPacketSize) { - LogMessage(LOG_LEVEL_ERROR, - "Content length %u too small to hold necessary MONR data (expected %u)", contentLength, - monrPacketSize); - return -1; - } - else if (contentLength > monrPacketSize) { - LogMessage(LOG_LEVEL_ERROR, - "Content length %u too large to follow MONR data specification (expected %u)", - contentLength, monrPacketSize); - return -1; - } +int UtilPopulateMonitorDataStruct(const char * rawData, const size_t rawDataSize, MonitorDataType * monitorData) { + if (rawDataSize != sizeof (MonitorDataType)) { + errno = EMSGSIZE; + LogMessage(LOG_LEVEL_ERROR, "Raw monitor data array wrong size, %d != %d", + rawDataSize, sizeof (MonitorDataType)); + return -1; } - else if (debug) { - LogPrint("Received MONR message without content header: corrupt data may result"); - } - - U16Data = 0; - - memcpy(&U32Data, rdPtr, sizeof (U32Data)); - monitorData->MONR.gpsQmsOfWeek = U32Data; - rdPtr += sizeof (U32Data); - U32Data = 0; - - memcpy(&I32Data, rdPtr, sizeof (I32Data)); - monitorData->MONR.xPosition = I32Data; - rdPtr += sizeof (I32Data); - I32Data = 0; - - memcpy(&I32Data, rdPtr, sizeof (I32Data)); - monitorData->MONR.yPosition = I32Data; - rdPtr += sizeof (I32Data); - I32Data = 0; - - memcpy(&I32Data, rdPtr, sizeof (I32Data)); - monitorData->MONR.zPosition = I32Data; - rdPtr += sizeof (I32Data); - I32Data = 0; - - memcpy(&U16Data, rdPtr, sizeof (U16Data)); - monitorData->MONR.heading = U16Data; - rdPtr += sizeof (U16Data); - U16Data = 0; - - memcpy(&I16Data, rdPtr, sizeof (I16Data)); - monitorData->MONR.longitudinalSpeed = I16Data; - rdPtr += sizeof (I16Data); - I16Data = 0; - - memcpy(&I16Data, rdPtr, sizeof (I16Data)); - monitorData->MONR.lateralSpeed = I16Data; - rdPtr += sizeof (I16Data); - I16Data = 0; - - memcpy(&I16Data, rdPtr, sizeof (I16Data)); - monitorData->MONR.longitudinalAcc = I16Data; - rdPtr += sizeof (I16Data); - I16Data = 0; - - memcpy(&I16Data, rdPtr, sizeof (I16Data)); - monitorData->MONR.lateralAcc = I16Data; - rdPtr += sizeof (I16Data); - I16Data = 0; - - monitorData->MONR.driveDirection = *(rdPtr++); - monitorData->MONR.state = *(rdPtr++); - monitorData->MONR.readyToArm = *(rdPtr++); - monitorData->MONR.errorStatus = *(rdPtr++); - - memcpy(&U16Data, rdPtr, sizeof (U16Data)); - monitorData->MONR.footer.Crc = U16Data; - rdPtr += sizeof (U16Data); - U16Data = 0; - memcpy(&IPData, rdPtr, sizeof (IPData)); - monitorData->ClientIP = IPData; - rdPtr += sizeof (IPData); - IPData = 0; - - if (debug == 1) { - LogPrint("MONR:"); - LogPrint("SyncWord = %d", monitorData->MONR.header.SyncWordU16); - LogPrint("TransmitterId = %d", monitorData->MONR.header.TransmitterIdU8); - LogPrint("PackageCounter = %d", monitorData->MONR.header.MessageCounterU8); - LogPrint("AckReq = %d", monitorData->MONR.header.AckReqProtVerU8); - LogPrint("MessageLength = %d", monitorData->MONR.header.MessageLengthU32); - LogPrint("GPSQMSOW = %u", monitorData->MONR.gpsQmsOfWeek); - } + memcpy(monitorData, rawData, rawDataSize); return 0; } From a9cab4a78ed18fa75fb1629f2a0672074f321e1f Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 6 Feb 2020 16:58:32 +0100 Subject: [PATCH 222/523] missed committing something --- server/inc/util.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/server/inc/util.h b/server/inc/util.h index 35ef6157f..18e3cf20a 100644 --- a/server/inc/util.h +++ b/server/inc/util.h @@ -747,7 +747,7 @@ I32 UtilISOBuildTRAJMessage(C8 *MessageBuffer, C8 *DTMData, I32 RowCount, DOTMTy I32 UtilISOBuildTRAJInfo(C8* MessageBuffer, TRAJInfoType *TRAJInfoData, U8 debug); I32 UtilWriteConfigurationParameter(C8 *ParameterName, C8 *NewValue, U8 Debug); -I32 UtilPopulateMonitorDataStruct(C8* rawMONR, size_t rawMONRsize, MonitorDataType *monitorData, U8 debug); +int UtilPopulateMonitorDataStruct(const char * rawMONR, const size_t rawMONRsize, MonitorDataType *monitorData); I32 UtilPopulateTREODataStructFromMQ(C8* rawTREO, size_t rawTREOsize, TREOData *treoData); I32 UtilPopulateEXACDataStructFromMQ(C8* rawEXAC, size_t rawEXACsize, EXACData *exacData); I32 UtilPopulateTRCMDataStructFromMQ(C8* rawTRCM, size_t rawTRCMsize, TRCMData *trcmData); From 54f9dd99ae12d157aee508b899afb5f60dc4b7e1 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 6 Feb 2020 16:59:01 +0100 Subject: [PATCH 223/523] Added check on position validity for near-target check --- server/src/util.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/server/src/util.c b/server/src/util.c index 4b0ce747b..b94eb7011 100644 --- a/server/src/util.c +++ b/server/src/util.c @@ -749,7 +749,8 @@ int UtilStringToMonitorData(const char *monitorString, size_t stringLength, Moni */ uint8_t UtilIsPositionNearTarget(CartesianPosition position, CartesianPosition target, double tolerance_m) { double distance = 0.0; - + if (!position.isPositionValid || target.isPositionValid) + return 0; distance = sqrt(pow(position.xCoord_m - target.xCoord_m, 2) + pow(position.yCoord_m - target.yCoord_m, 2) + pow(position.zCoord_m - target.zCoord_m, 2)); From 8698f341ec441e8227682e056832b990c579f3ef Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 6 Feb 2020 16:59:27 +0100 Subject: [PATCH 224/523] Made iCommRecv return correct size of data it has read --- server/src/util.c | 1 + 1 file changed, 1 insertion(+) diff --git a/server/src/util.c b/server/src/util.c index b94eb7011..8167cf1f7 100644 --- a/server/src/util.c +++ b/server/src/util.c @@ -1880,6 +1880,7 @@ ssize_t iCommRecv(enum COMMAND * command, char *data, const size_t messageSize, return 0; } memcpy(data, message + headerSize, dataLength - headerSize); + result -= headerSize; } } else if (result > 0) { From a2f7a44ed85fe5ae161b09e6c8c6df6a6a4abaf8 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 6 Feb 2020 16:59:54 +0100 Subject: [PATCH 225/523] Made logger use monitor data struct --- server/src/logger.c | 59 +++++++++++++++++++++++++++++---------------- 1 file changed, 38 insertions(+), 21 deletions(-) diff --git a/server/src/logger.c b/server/src/logger.c index 4ebaf2702..f17e67074 100644 --- a/server/src/logger.c +++ b/server/src/logger.c @@ -24,6 +24,7 @@ #include #include +#include "positioning.h" #include "maestroTime.h" #include "logger.h" @@ -237,7 +238,7 @@ void logger_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { MonitorDataType monrData; UtilStringToMonitorData(pcReadBuffer, sizeof (pcReadBuffer), &monrData); - NewTimestamp = monrData.MONR.gpsQmsOfWeek; + NewTimestamp = TimeGetAsGPSqmsOfWeek(&monrData.data.timestamp); if (!FirstIteration) { /* Wait a little bit */ sleep_time.tv_sec = 0; @@ -565,39 +566,55 @@ void vLogMonitorData(char *commandData, ssize_t commandDatalen, struct timeval r FILE *filefd; char DateBuffer[MAX_DATE_STRLEN]; char ipStringBuffer[INET_ADDRSTRLEN]; + char printBuffer[MAX_LOG_ROW_LENGTH]; MonitorDataType monitorData; - struct timeval monrTime, systemTime; - const int debug = 0; + int printedBytes = 0; + int totalPrintedBytes = 0; if (commandDatalen < 0) return; - TimeSetToCurrentSystemTime(&systemTime); - - UtilPopulateMonitorDataStruct(commandData, (size_t) (commandDatalen), &monitorData, debug); - TimeSetToGPStime(&monrTime, TimeGetAsGPSweek(&systemTime), monitorData.MONR.gpsQmsOfWeek); + UtilPopulateMonitorDataStruct(commandData, (size_t) commandDatalen, &monitorData); - bzero(DateBuffer, sizeof (DateBuffer)); + memset(DateBuffer, 0, sizeof (DateBuffer)); TimeGetAsDateTime(&recvTime, "%Y;%m;%d;%H;%M;%S;%q", DateBuffer, sizeof (DateBuffer)); + printedBytes = snprintf(printBuffer, sizeof (printBuffer), "%s;%ld;%ld;%d;", DateBuffer, + TimeGetAsUTCms(&monitorData.data.timestamp), TimeGetAsGPSms(&monitorData.data.timestamp), + (unsigned char)COMM_MONR); + + totalPrintedBytes += printedBytes; + if (printedBytes < 0 || (size_t) totalPrintedBytes > sizeof (printBuffer)) { + LogMessage(LOG_LEVEL_ERROR, "Error printing data to string"); + return; + } + + printedBytes = snprintf(printBuffer + totalPrintedBytes, sizeof (printBuffer) - (size_t) totalPrintedBytes, "%s;", + inet_ntop(AF_INET, &monitorData.ClientIP, ipStringBuffer, sizeof (ipStringBuffer))); + + totalPrintedBytes += printedBytes; + if (printedBytes < 0 || (size_t) totalPrintedBytes > sizeof (printBuffer)) { + LogMessage(LOG_LEVEL_ERROR, "Error printing data to string"); + return; + } + + printedBytes = snprintf(printBuffer + totalPrintedBytes, sizeof (printBuffer) - (size_t) totalPrintedBytes, "%u;", + monitorData.ClientID); + + totalPrintedBytes += printedBytes; + if (printedBytes < 0 || (size_t) totalPrintedBytes > sizeof (printBuffer)) { + LogMessage(LOG_LEVEL_ERROR, "Error printing data to string"); + return; + } + + objectMonitorDataToASCII(&monitorData.data, printBuffer + totalPrintedBytes, sizeof (printBuffer) - (size_t) totalPrintedBytes); + filefd = fopen(pcLogFile, ACCESS_MODE_APPEND_AND_READ); if (filefd == NULL) { LogMessage(LOG_LEVEL_ERROR, "Unable to open file <%s>", pcLogFile); return; } - - fprintf(filefd, "%s;%ld;%ld;%d;", DateBuffer, TimeGetAsUTCms(&monrTime), TimeGetAsGPSms(&monrTime), - (unsigned char)COMM_MONR); - fprintf(filefd, "%s;", - inet_ntop(AF_INET, &monitorData.ClientIP, ipStringBuffer, sizeof (ipStringBuffer))); - fprintf(filefd, "%u;%u;", monitorData.MONR.header.TransmitterIdU8, monitorData.MONR.gpsQmsOfWeek); - fprintf(filefd, "%d;%d;%d;%u;", monitorData.MONR.xPosition, monitorData.MONR.yPosition, - monitorData.MONR.zPosition, monitorData.MONR.heading); - fprintf(filefd, "%d;%d;", monitorData.MONR.longitudinalSpeed, monitorData.MONR.lateralSpeed); - fprintf(filefd, "%d;%d;", monitorData.MONR.longitudinalAcc, monitorData.MONR.lateralAcc); - fprintf(filefd, "%u;%u;%u;%u;\n", monitorData.MONR.driveDirection, monitorData.MONR.state, - monitorData.MONR.readyToArm, monitorData.MONR.errorStatus); - + fprintf(filefd, "%s\n", printBuffer); fclose(filefd); } From a0c0726eceffef4e25e316649fc8c4030a25cb23 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 6 Feb 2020 17:02:07 +0100 Subject: [PATCH 226/523] RVSS use new monitor data stuff --- server/src/systemcontrol.c | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index cf8f816a7..a10cf0e00 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -570,9 +570,8 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { //SystemControlSendLog(pcRecvBuffer, &ClientSocket, 0); break; case COMM_MONR: - // TODO: Decode if (RVSSChannelSocket != 0 && RVSSConfigU32 & RVSS_MONR_CHANNEL && bytesReceived >= 0) { - UtilPopulateMonitorDataStruct(pcRecvBuffer, (size_t) bytesReceived, &monrData, 0); + UtilPopulateMonitorDataStruct(pcRecvBuffer, (size_t) bytesReceived, &monrData); SystemControlBuildRVSSMONRChannelMessage(RVSSData, &RVSSMessageLengthU32, monrData, 0); UtilSendUDPData("SystemControl", &RVSSChannelSocket, &RVSSChannelAddr, RVSSData, RVSSMessageLengthU32, 0); @@ -2345,7 +2344,7 @@ I32 SystemControlBuildRVSSMaestroChannelMessage(C8 * RVSSData, U32 * RVSSDataLen -#define MAX_MONR_STRING_LENGTH 116 +#define MAX_MONR_STRING_LENGTH 1024 /* SystemControlBuildRVSSMONRChannelMessage builds a message from data in *MonrData. The message is stored in *RVSSData. See the architecture document for the protocol of RVSS. @@ -2362,7 +2361,12 @@ I32 SystemControlBuildRVSSMONRChannelMessage(C8 * RVSSData, U32 * RVSSDataLength char MonrDataString[MAX_MONR_STRING_LENGTH]; // TODO: Convert MonrData to string - UtilMonitorDataToString(MonrData, MonrDataString, sizeof (MonrDataString)); + if (UtilMonitorDataToString(MonrData, MonrDataString, sizeof (MonrDataString)) == -1) { + // TODO memset rvssdata to 0 + LogMessage(LOG_LEVEL_ERROR, "Error building monitor data string"); + *RVSSDataLengthU32 = 0; + return -1; + } MessageLength = strlen(MonrDataString) + 8; bzero(RVSSData, MessageLength); From bdbb47dcabb359ec39c4dab6200f93ed1d438128 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 6 Feb 2020 17:03:38 +0100 Subject: [PATCH 227/523] Made object control use monitor data over monr --- server/src/objectcontrol.c | 51 +++++++++++++++----------------------- 1 file changed, 20 insertions(+), 31 deletions(-) diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 2b6f9c19f..9ee86c403 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -229,7 +229,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { C8 pcSendBuffer[MBUS_MAX_DATALEN]; C8 ObjectPort[SMALL_BUFFER_SIZE_0]; HeaderType HeaderData; - MONRType MONRData; + MonitorDataType monitorData; DOTMType DOTMData; TRAJInfoType TRAJInfoData; VOILType VOILData; @@ -387,12 +387,13 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { memset(buffer, 0, sizeof (buffer)); receivedMONRData = uiRecvMonitor(&safety_socket_fd[iIndex], buffer, sizeof (buffer)); - if (receivedMONRData > 0) { - LogMessage(LOG_LEVEL_DEBUG, "Recieved new data from %s %d %d: %s", - object_address_name[iIndex], object_udp_port[iIndex], receivedMONRData, + if (receivedMONRData > 0 && getISOMessageType(buffer, receivedMONRData, 0) == MESSAGE_ID_MONR) { + LogMessage(LOG_LEVEL_DEBUG, "Recieved %d bytes of new data from %s %d: %s", + receivedMONRData, object_address_name[iIndex], object_udp_port[iIndex], buffer); - if (decodeMONRMessage(buffer, receivedMONRData, &MONRData, 0) != MESSAGE_OK) { + monitorData.ClientIP = safety_object_addr[iIndex].sin_addr.s_addr; + if (decodeMONRMessage(buffer, receivedMONRData, &monitorData.ClientID, &monitorData.data, 0) != MESSAGE_OK) { LogMessage(LOG_LEVEL_INFO, "Error decoding MONR from %s: disconnecting object", object_address_name[iIndex]); vDisconnectObject(&safety_socket_fd[iIndex]); @@ -401,12 +402,10 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { } if (ObjectcontrolExecutionMode == OBJECT_CONTROL_CONTROL_MODE) { - // Append IP to buffer - memcpy(&buffer[receivedMONRData], &safety_object_addr[iIndex].sin_addr.s_addr, - sizeof (in_addr_t)); + // Place struct in buffer + memcpy(&buffer, &monitorData, sizeof (monitorData)); // Send MONR message as bytes - - if (iCommSend(COMM_MONR, buffer, (size_t) (receivedMONRData) + sizeof (in_addr_t)) < + if (iCommSend(COMM_MONR, buffer, sizeof (monitorData)) < 0) { LogMessage(LOG_LEVEL_ERROR, "Fatal communication fault when sending MONR command - entering error state"); @@ -417,20 +416,11 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { //Store MONR in GSD - //UtilSendUDPData("ObjectControl", &ObjectControlUDPSocketfdI32, &simulator_addr, &MONRData, sizeof(MONRData), 0); - for (i = 0; - i < - (MONRData.header.MessageLengthU32 + sizeof (MONRData.header) + - sizeof (MONRData.footer)); i++) - GSD->MONRData[i] = buffer[i]; - GSD->MONRSizeU8 = - MONRData.header.MessageLengthU32 + sizeof (MONRData.header) + - sizeof (MONRData.footer); - memset(buffer, 0, sizeof (buffer)); - memcpy(buffer, object_address_name[iIndex], strlen(object_address_name[iIndex])); - strcat(buffer, ";0;"); - MONRToASCII(&MONRData, buffer + strlen(buffer), sizeof (buffer) - strlen(buffer), 0); + memcpy(GSD->MONRData, buffer, receivedMONRData); + GSD->MONRSizeU8 = (unsigned char) receivedMONRData; + memset(buffer, 0, sizeof (buffer)); + UtilMonitorDataToString(monitorData, buffer, sizeof (buffer)); if (ASPData.MTSPU32 != 0) { //Add MTSP to MONR if not 0 @@ -450,14 +440,14 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { TimeCap1 = (uint64_t) CurrentTimeStruct.tv_sec * 1000 + (uint64_t) CurrentTimeStruct.tv_usec / 1000; //Calculate initial timestamp - OP[iIndex].x = ((double)MONRData.xPosition) / 1000; //Set x and y on OP (ObjectPosition) - OP[iIndex].y = ((double)MONRData.yPosition) / 1000; + OP[iIndex].x = monitorData.data.position.xCoord_m; //Set x and y on OP (ObjectPosition) + OP[iIndex].y = monitorData.data.position.yCoord_m; //OP[iIndex].OrigoDistance = sqrt(pow(OP[iIndex].x,2) + pow(OP[iIndex].y,2)); //Calculate hypotenuse // TODO: check use of this function since it should take two lat/long points but is here used with x/y UtilCalcPositionDelta(OriginLatitudeDbl, OriginLongitudeDbl, - MONRData.xPosition / 1e7, MONRData.yPosition / 1e7, + monitorData.data.position.xCoord_m, monitorData.data.position.yCoord_m, &OP[iIndex]); if (OP[iIndex].BestFoundTrajectoryIndex <= OP[iIndex].SyncIndex) { @@ -507,7 +497,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { //ObjectControlBuildASPMessage(buffer, &ASPData, 0); DataDictionarySetRVSSAsp(GSD, &ASPData); - if (MONRData.gpsQmsOfWeek % ASPDebugRate == 0) { + if (TimeGetAsGPSqmsOfWeek(&monitorData.data.timestamp) % ASPDebugRate == 0) { printf("%d, %d, %3.3f, %s, %s\n", CurrentTimeU32, StartTimeU32, ASPData.TimeToSyncPointDbl, object_address_name[iIndex], ASP[i].MasterIP); @@ -525,11 +515,10 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { } } OP[iIndex].Speed = - sqrt(pow(MONRData.lateralSpeed, 2) + pow(MONRData.longitudinalSpeed, 2)); + (float) sqrt(pow(monitorData.data.speed.lateral_m_s, 2) + pow(monitorData.data.speed.longitudinal_m_s, 2)); } else if (receivedMONRData > 0) - LogMessage(LOG_LEVEL_INFO, "MONR length error (should be %d but is %ld) from %s.", - sizeof (MONRType), object_address_name[iIndex]); + LogMessage(LOG_LEVEL_WARNING, "Received unhandled message on monitoring socket"); } } @@ -1880,7 +1869,7 @@ int ObjectControlSendUDPData(int *sockfd, struct sockaddr_in *addr, char *SendDa } -static size_t uiRecvMonitor(int *sockfd, char *buffer, size_t length) { +size_t uiRecvMonitor(int *sockfd, char *buffer, size_t length) { ssize_t result = 0; size_t recvDataSize = 0; From 36751356de3a93f53fbd934eec0cf0fadbb9b107 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 6 Feb 2020 17:34:09 +0100 Subject: [PATCH 228/523] Made positioning use radians internally --- server/src/positioning.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/server/src/positioning.c b/server/src/positioning.c index ffd63089d..fc7f84e0e 100644 --- a/server/src/positioning.c +++ b/server/src/positioning.c @@ -1,6 +1,7 @@ #include #include #include +#include #include "positioning.h" #include "maestroTime.h" @@ -191,7 +192,7 @@ int objectMonitorDataToASCII(const ObjectMonitorType * monitorData, char *asciiB if (monitorData->position.isHeadingValid) snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), - "%.2f;", monitorData->position.heading_deg); + "%.2f;", monitorData->position.heading_rad * 180.0 / M_PI); else snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), "NaN;"); From bdde7f3666674ec781c71785dfb24ece14a9a4e5 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 6 Feb 2020 17:34:53 +0100 Subject: [PATCH 229/523] Made string interpreters for xyz positions and speed not cast to ints --- server/src/positioning.c | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/server/src/positioning.c b/server/src/positioning.c index fc7f84e0e..31179ce08 100644 --- a/server/src/positioning.c +++ b/server/src/positioning.c @@ -274,25 +274,25 @@ int ASCIIToObjectMonitorData(const char *asciiBuffer, ObjectMonitorType * monito monitorData->position.isPositionValid = true; token = strtok(NULL, delim); - monitorData->position.xCoord_m = (int32_t) strtol(token, &endPtr, NumberBaseDecimal); + monitorData->position.xCoord_m = strtod(token, &endPtr); if (endPtr == token) { monitorData->position.isPositionValid = false; } token = strtok(NULL, delim); - monitorData->position.yCoord_m = (int32_t) strtol(token, &endPtr, NumberBaseDecimal); + monitorData->position.yCoord_m = strtod(token, &endPtr); if (endPtr == token) { monitorData->position.isPositionValid = false; } token = strtok(NULL, delim); - monitorData->position.zCoord_m = (int32_t) strtol(token, &endPtr, NumberBaseDecimal); + monitorData->position.zCoord_m = strtod(token, &endPtr); if (endPtr == token) { monitorData->position.isPositionValid = false; } token = strtok(NULL, delim); - monitorData->position.heading_deg = (uint16_t) strtoul(token, &endPtr, NumberBaseDecimal); + monitorData->position.heading_rad = strtod(token, &endPtr) * M_PI / 180.0; if (endPtr == token) { monitorData->position.isHeadingValid = false; } @@ -301,13 +301,13 @@ int ASCIIToObjectMonitorData(const char *asciiBuffer, ObjectMonitorType * monito monitorData->speed.isValid = true; token = strtok(NULL, delim); - monitorData->speed.longitudinal_m_s = (int16_t) strtol(token, &endPtr, NumberBaseDecimal); + monitorData->speed.longitudinal_m_s = strtod(token, &endPtr); if (endPtr == token) { monitorData->speed.isValid = false; } token = strtok(NULL, delim); - monitorData->speed.lateral_m_s = (int16_t) strtol(token, &endPtr, NumberBaseDecimal); + monitorData->speed.lateral_m_s = strtod(token, &endPtr); if (endPtr == token) { monitorData->speed.isValid = false; } @@ -316,13 +316,13 @@ int ASCIIToObjectMonitorData(const char *asciiBuffer, ObjectMonitorType * monito monitorData->acceleration.isValid = true; token = strtok(NULL, delim); - monitorData->acceleration.longitudinal_m_s2 = (int16_t) strtol(token, &endPtr, NumberBaseDecimal); + monitorData->acceleration.longitudinal_m_s2 = strtod(token, &endPtr); if (endPtr == token) { monitorData->acceleration.isValid = false; } token = strtok(NULL, delim); - monitorData->acceleration.lateral_m_s2 = (int16_t) strtol(token, &endPtr, NumberBaseDecimal); + monitorData->acceleration.lateral_m_s2 = strtod(token, &endPtr); if (endPtr == token) { monitorData->acceleration.isValid = false; } From 912d500e290bca5a3111341408dfdb1e50d0f2de Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 6 Feb 2020 17:44:59 +0100 Subject: [PATCH 230/523] Made supervision use right heading --- modules/Supervision/src/main.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/modules/Supervision/src/main.cpp b/modules/Supervision/src/main.cpp index 758beeaf3..4ceb45af1 100644 --- a/modules/Supervision/src/main.cpp +++ b/modules/Supervision/src/main.cpp @@ -405,7 +405,9 @@ Geofence parseGeofenceFile(const std::string geofenceFile) { pos.xCoord_m = stod(match[1]); pos.yCoord_m = stod(match[2]); pos.zCoord_m = (geofence.maxHeight + geofence.minHeight) / 2.0; - pos.heading_deg = 0; + pos.isPositionValid = true; + pos.heading_rad = 0; + pos.isHeadingValid = false; LogMessage(LOG_LEVEL_DEBUG, "Point: (%.3f, %.3f, %.3f)", pos.xCoord_m, @@ -497,7 +499,7 @@ PositionStatus updateNearStartingPositionStatus(const MonitorDataType &monitorDa CartesianPosition trajectoryPoint = element.first.points.front().getCartesianPosition(); CartesianPosition objectPosition = monitorData.data.position; if (UtilIsPositionNearTarget(objectPosition, trajectoryPoint, ARM_MAX_DISTANCE_TO_START_M) - && UtilIsAngleNearTarget(objectPosition, trajectoryPoint, ARM_MAX_ANGLE_TO_START_DEG)) { + && UtilIsAngleNearTarget(objectPosition, trajectoryPoint, ARM_MAX_ANGLE_TO_START_DEG * M_PI / 180.0)) { if (element.second == false) { LogMessage(LOG_LEVEL_INFO, "Object with IP %s and position (%.2f, %.2f, %.2f) detected within %.2f m and %.2f degrees of the first point (%.2f, %.2f, %.2f) in trajectory %s", inet_ntop(AF_INET, &monitorData.ClientIP, ipString, sizeof (ipString)), @@ -526,8 +528,8 @@ PositionStatus updateNearStartingPositionStatus(const MonitorDataType &monitorDa } else { LogMessage(LOG_LEVEL_INFO, "Object with IP %s (heading: %.2f degrees) not facing direction specified by first point (heading: %.2f degrees) in trajectory %s (tolerance: %.2f degrees)", - inet_ntop(AF_INET, &monitorData.ClientIP, ipString, sizeof (ipString)), objectPosition.heading_deg, - trajectoryPoint.heading_deg, element.first.name.c_str(), ARM_MAX_ANGLE_TO_START_DEG); + inet_ntop(AF_INET, &monitorData.ClientIP, ipString, sizeof (ipString)), objectPosition.heading_rad * 180.0 / M_PI, + trajectoryPoint.heading_rad * 180.0 / M_PI, element.first.name.c_str(), ARM_MAX_ANGLE_TO_START_DEG); } element.second = false; return SINGLE_OBJECT_NOT_NEAR_START; From 3f4ca7af30b2adbef53faf30b256ff142f152c8b Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 6 Feb 2020 17:45:21 +0100 Subject: [PATCH 231/523] Made util use correct heading --- server/inc/util.h | 2 +- server/src/util.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/server/inc/util.h b/server/inc/util.h index 18e3cf20a..6496d3884 100644 --- a/server/inc/util.h +++ b/server/inc/util.h @@ -686,7 +686,7 @@ int UtilCheckTrajectoryFileFormat(const char *path, size_t pathLen); int UtilMonitorDataToString(const MonitorDataType monrData, char* monrString, size_t stringLength); int UtilStringToMonitorData(const char* monrString, size_t stringLength, MonitorDataType * monrData); uint8_t UtilIsPositionNearTarget(CartesianPosition position, CartesianPosition target, double tolerance_m); -uint8_t UtilIsAngleNearTarget(CartesianPosition position, CartesianPosition target, double tolerance_deg); +uint8_t UtilIsAngleNearTarget(CartesianPosition position, CartesianPosition target, double tolerance); double UtilCalcPositionDelta(double P1Lat, double P1Long, double P2Lat, double P2Long, ObjectPosition *OP); int UtilVincentyDirect(double refLat, double refLon, double a1, double distance, double *resLat, double *resLon, double *a2); double UtilDegToRad(double Deg); diff --git a/server/src/util.c b/server/src/util.c index 8167cf1f7..d7d6d5944 100644 --- a/server/src/util.c +++ b/server/src/util.c @@ -766,7 +766,7 @@ uint8_t UtilIsPositionNearTarget(CartesianPosition position, CartesianPosition t */ uint8_t UtilIsAngleNearTarget(CartesianPosition position, CartesianPosition target, double tolerance_deg) { - const double oneRotation = 360.0; + const double oneRotation = 2.0 * M_PI; double posHeading = position.heading_deg, tarHeading = target.heading_deg; while (posHeading < 0) { From 4afe09ec54b09eb1c9db628f0312d458c0f4a627 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 6 Feb 2020 17:47:17 +0100 Subject: [PATCH 232/523] Missed committing --- server/src/util.c | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/server/src/util.c b/server/src/util.c index d7d6d5944..96ce607c1 100644 --- a/server/src/util.c +++ b/server/src/util.c @@ -758,16 +758,19 @@ uint8_t UtilIsPositionNearTarget(CartesianPosition position, CartesianPosition t } /*! - * \brief UtilIsAngleNearTarget Checks if angle is within tolerence_deg of target angle + * \brief UtilIsAngleNearTarget Checks if angle is within tolerence of target angle * \param position Position with angle to verify * \param target Target position with angle - * \param tolerance_deg Angle tolerance defining "near" - * \return true if position is within tolerance_deg of target, false otherwise + * \param tolerance Angle tolerance defining "near" + * \return true if position is within tolerance of target, false otherwise */ -uint8_t UtilIsAngleNearTarget(CartesianPosition position, CartesianPosition target, double tolerance_deg) { +uint8_t UtilIsAngleNearTarget(CartesianPosition position, CartesianPosition target, double tolerance) { const double oneRotation = 2.0 * M_PI; - double posHeading = position.heading_deg, tarHeading = target.heading_deg; + double posHeading = position.heading_rad, tarHeading = target.heading_rad; + + if (!position.isHeadingValid || !target.isHeadingValid) + return 0; while (posHeading < 0) { posHeading += oneRotation; @@ -776,7 +779,7 @@ uint8_t UtilIsAngleNearTarget(CartesianPosition position, CartesianPosition targ tarHeading += oneRotation; } - return fabs(posHeading - tarHeading) <= tolerance_deg; + return fabs(posHeading - tarHeading) <= tolerance; } double UtilCalcPositionDelta(double P1Lat, double P1Long, double P2Lat, double P2Long, ObjectPosition * OP) { From 1fb02257bf251f5421a69df75f7127bf1d36e061 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 6 Feb 2020 18:00:03 +0100 Subject: [PATCH 233/523] Made supervision use correct heading --- modules/Supervision/src/trajectory.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/Supervision/src/trajectory.cpp b/modules/Supervision/src/trajectory.cpp index a7196dda2..f2d487e46 100644 --- a/modules/Supervision/src/trajectory.cpp +++ b/modules/Supervision/src/trajectory.cpp @@ -129,7 +129,7 @@ CartesianPosition Trajectory::TrajectoryPoint::getCartesianPosition() { LogMessage(LOG_LEVEL_WARNING, "Casting trajectory point to cartesian position: optional z value assumed to be 0"); retval.zCoord_m = 0.0; } - retval.heading_deg = this->getHeading() * 180.0 / M_PI; + retval.heading_rad = this->getHeading(); retval.isHeadingValid = true; retval.isPositionValid = true; return retval; From b02bcdb3d69cf1abbe9d725acd2ac8795045afea Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 6 Feb 2020 18:00:25 +0100 Subject: [PATCH 234/523] Made visualization use correct heading --- modules/Visualization/src/main.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/modules/Visualization/src/main.c b/modules/Visualization/src/main.c index b10095135..80f900121 100644 --- a/modules/Visualization/src/main.c +++ b/modules/Visualization/src/main.c @@ -60,7 +60,7 @@ void vCreateVisualizationMessage(MonitorDataType * _monitorData, char *_visualiz sprintf(zPosString, "%.3f", _monitorData->data.position.zCoord_m); char headingString[ENOUGH_BUFFER_SIZE]; - sprintf(headingString, "%.2f", _monitorData->data.position.heading_deg); + sprintf(headingString, "%.2f", _monitorData->data.position.heading_rad * 180.0 / M_PI); char longSpeedString[ENOUGH_BUFFER_SIZE]; sprintf(longSpeedString, "%.3f", _monitorData->data.speed.longitudinal_m_s); @@ -157,7 +157,7 @@ int main() { monitorData.data.position.xCoord_m, monitorData.data.position.yCoord_m, monitorData.data.position.zCoord_m, - monitorData.data.position.heading_deg, + monitorData.data.position.heading_rad * 180.0 / M_PI, monitorData.data.speed.longitudinal_m_s, objectStateToASCII(monitorData.data.state)); sizeOfVisualizationMessage += INET_ADDRSTRLEN; From bd65319c7659846b4fd5a72674b0fc17c13ebf92 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 6 Feb 2020 18:25:14 +0100 Subject: [PATCH 235/523] Fixed COMM_OSEM being sent with strange data --- server/src/objectcontrol.c | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 9ee86c403..1d9b24d6b 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -801,6 +801,11 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { DataDictionaryGetOriginLongitudeC8(GSD, OriginLongitude, SMALL_BUFFER_SIZE_0); DataDictionaryGetOriginAltitudeC8(GSD, OriginAltitude, SMALL_BUFFER_SIZE_0); + memset(pcSendBuffer, 0, sizeof(pcSendBuffer)); + snprintf(pcSendBuffer, sizeof (pcSendBuffer), "%u;", TimeGetAsGPSweek(¤tTime)); + snprintf(pcSendBuffer + strlen(pcSendBuffer), sizeof (pcSendBuffer) - strlen(pcSendBuffer), "%s;%s;%s;%s;", + OriginLatitude, OriginLongitude, OriginAltitude, OriginHeading); + if (iCommSend(COMM_OSEM, pcSendBuffer, strlen(pcSendBuffer) + 1) < 0) { LogMessage(LOG_LEVEL_ERROR, "Fatal communication fault when sending OSEM command - entering error state"); From 688e0845728f2f27c80690495b9e2373213a9ba3 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 6 Feb 2020 18:28:01 +0100 Subject: [PATCH 236/523] Made systemcontrol not send COMM_OSEM on MQ when usercontrol asks what the origin is --- server/src/systemcontrol.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index a10cf0e00..605b65f12 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -622,7 +622,7 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { DataDictionaryGetOriginAltitudeC8(GSD, TextBuffer20, SMALL_BUFFER_SIZE_20); strcat(ControlResponseBuffer, TextBuffer20); strcat(ControlResponseBuffer, ";"); - iCommSend(COMM_OSEM, ControlResponseBuffer, sizeof (ControlResponseBuffer)); + SystemControlSendControlResponse(strlen(ParameterListC8) > 0 ? SYSTEM_CONTROL_RESPONSE_CODE_OK : SYSTEM_CONTROL_RESPONSE_CODE_NO_DATA, "GetTestOrigin:", From 70bfea471af81f7316dcce228fcaf481e01b97c4 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 6 Feb 2020 18:30:15 +0100 Subject: [PATCH 237/523] Added null pointer guard in logger --- server/src/logger.c | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/server/src/logger.c b/server/src/logger.c index f17e67074..26feddf61 100644 --- a/server/src/logger.c +++ b/server/src/logger.c @@ -176,8 +176,9 @@ void logger_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { break; case COMM_MONR: - if (!isFirstInit) + if (!isFirstInit) { vLogMonitorData(busReceiveBuffer, receivedBytes, recvTime, pcLogFile, pcLogFileComp); + } else LogMessage(LOG_LEVEL_WARNING, "Received command %u while log uninitialized", command); break; @@ -188,9 +189,14 @@ void logger_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { // Returns first datapoint of OSEM (GPSWeek) char *token = strtok(busReceiveBuffer, ";"); - GPSweek = atoi(token); + if (token != NULL) { + GPSweek = atoi(token); + LogMessage(LOG_LEVEL_INFO, "GPS week of OSEM: %d", GPSweek); + } + else { + LogMessage(LOG_LEVEL_WARNING, "Could not read GPS week in OSEM"); + } - LogMessage(LOG_LEVEL_INFO, "GPS week of OSEM: %d", GPSweek); // Rest of OSEM if needed /* @@ -506,7 +512,6 @@ void vInitializeLog(char *logFilePath, unsigned int filePathLength, char *csvLog COMM_VIOP, COMM_INV); (void)fwrite(pcBuffer, 1, strlen(pcBuffer), filefd); - fclose(filefd); } From 14012dcd0d8575b332080288a80fbfbd290ae820 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 6 Feb 2020 18:30:50 +0100 Subject: [PATCH 238/523] Ran code formatter --- server/src/iso22133.c | 47 +++++++++++--------- server/src/logger.c | 17 +++++--- server/src/objectcontrol.c | 43 ++++++++++++------- server/src/positioning.c | 87 +++++++++++++++++--------------------- server/src/util.c | 8 +++- 5 files changed, 109 insertions(+), 93 deletions(-) diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 0264ac0dd..5ea2e57ba 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -60,6 +60,7 @@ typedef enum { ISO_READY_TO_ARM = 1, ISO_READY_TO_ARM_UNAVAILABLE = 2 } ArmReadinessValues; + #define BITMASK_ERROR_ABORT_REQUEST 0x80 #define BITMASK_ERROR_OUTSIDE_GEOFENCE 0x40 #define BITMASK_ERROR_BAD_POSITIONING_ACCURACY 0x20 @@ -71,7 +72,7 @@ typedef enum { -#pragma pack(push,1) // Ensure sizeof() is useable for (most) network byte lengths +#pragma pack(push,1) // Ensure sizeof() is useable for (most) network byte lengths /*! OSEM message */ typedef struct { HeaderType header; @@ -919,7 +920,8 @@ ssize_t encodeHEABMessage(const ControlCenterStatusType status, char *heabDataBu * \param debug Flag for enabling of debugging * \return value according to ::ISOMessageReturnValue */ -ISOMessageReturnValue decodeMONRMessage(const char *monrDataBuffer, const size_t bufferLength, uint32_t* objectID, ObjectMonitorType * monitorData, +ISOMessageReturnValue decodeMONRMessage(const char *monrDataBuffer, const size_t bufferLength, + uint32_t * objectID, ObjectMonitorType * monitorData, const char debug) { MONRType MONRData; @@ -1019,7 +1021,8 @@ ISOMessageReturnValue decodeMONRMessage(const char *monrDataBuffer, const size_t // Decode footer if ((retval = - decodeISOFooter(p, bufferLength - (size_t) (p - monrDataBuffer), &MONRData.footer, debug)) != MESSAGE_OK) { + decodeISOFooter(p, bufferLength - (size_t) (p - monrDataBuffer), &MONRData.footer, + debug)) != MESSAGE_OK) { LogMessage(LOG_LEVEL_ERROR, "Error decoding MONR footer"); return retval; } @@ -1068,19 +1071,20 @@ void convertMONRToHostRepresentation(const MONRType * MONRData, ObjectMonitorTyp monitorData->isTimestampValid = MONRData->gpsQmsOfWeek != GPS_SECOND_OF_WEEK_UNAVAILABLE_VALUE; if (monitorData->isTimestampValid) { struct timeval currentTime; + TimeSetToCurrentSystemTime(¤tTime); TimeSetToGPStime(&monitorData->timestamp, TimeGetAsGPSweek(¤tTime), MONRData->gpsQmsOfWeek); } // Position / heading - monitorData->position.xCoord_m = (double) (MONRData->xPosition) / POSITION_ONE_METER_VALUE; - monitorData->position.yCoord_m = (double) (MONRData->yPosition) / POSITION_ONE_METER_VALUE; - monitorData->position.zCoord_m = (double) (MONRData->zPosition) / POSITION_ONE_METER_VALUE; + monitorData->position.xCoord_m = (double)(MONRData->xPosition) / POSITION_ONE_METER_VALUE; + monitorData->position.yCoord_m = (double)(MONRData->yPosition) / POSITION_ONE_METER_VALUE; + monitorData->position.zCoord_m = (double)(MONRData->zPosition) / POSITION_ONE_METER_VALUE; monitorData->position.isPositionValid = true; monitorData->position.isHeadingValid = MONRData->heading != HEADING_UNAVAILABLE_VALUE; if (monitorData->position.isHeadingValid) { // TODO: convert to host representation - monitorData->position.heading_deg = (double) (MONRData->heading) / 100; + monitorData->position.heading_deg = (double)(MONRData->heading) / 100; } // Velocity @@ -1089,12 +1093,11 @@ void convertMONRToHostRepresentation(const MONRType * MONRData, ObjectMonitorTyp monitorData->speed.isValid = false; else monitorData->speed.longitudinal_m_s = - (double) (MONRData->longitudinalSpeed) / SPEED_ONE_METER_PER_SECOND_VALUE; + (double)(MONRData->longitudinalSpeed) / SPEED_ONE_METER_PER_SECOND_VALUE; if (MONRData->lateralSpeed == SPEED_UNAVAILABLE_VALUE) monitorData->speed.isValid = false; else - monitorData->speed.lateral_m_s = - (double) (MONRData->lateralSpeed) / SPEED_ONE_METER_PER_SECOND_VALUE; + monitorData->speed.lateral_m_s = (double)(MONRData->lateralSpeed) / SPEED_ONE_METER_PER_SECOND_VALUE; // Acceleration monitorData->acceleration.isValid = true; @@ -1102,12 +1105,12 @@ void convertMONRToHostRepresentation(const MONRType * MONRData, ObjectMonitorTyp monitorData->acceleration.isValid = false; else monitorData->acceleration.longitudinal_m_s2 = - (double) (MONRData->longitudinalAcc) / ACCELERATION_ONE_METER_PER_SECOND_SQUARED_VALUE; + (double)(MONRData->longitudinalAcc) / ACCELERATION_ONE_METER_PER_SECOND_SQUARED_VALUE; if (MONRData->lateralAcc == ACCELERATION_UNAVAILABLE_VALUE) monitorData->acceleration.isValid = false; else monitorData->acceleration.lateral_m_s2 = - (double) (MONRData->lateralAcc) / ACCELERATION_ONE_METER_PER_SECOND_SQUARED_VALUE; + (double)(MONRData->lateralAcc) / ACCELERATION_ONE_METER_PER_SECOND_SQUARED_VALUE; // Drive direction switch (MONRData->driveDirection) { @@ -1166,10 +1169,12 @@ void convertMONRToHostRepresentation(const MONRType * MONRData, ObjectMonitorTyp monitorData->error.engineFault = MONRData->errorStatus & BITMASK_ERROR_ENGINE_FAULT; monitorData->error.abortRequest = MONRData->errorStatus & BITMASK_ERROR_ABORT_REQUEST; monitorData->error.batteryFault = MONRData->errorStatus & BITMASK_ERROR_BATTERY_FAULT; - monitorData->error.unknownError = MONRData->errorStatus & BITMASK_ERROR_OTHER || MONRData->errorStatus & BITMASK_ERROR_VENDOR_SPECIFIC; + monitorData->error.unknownError = MONRData->errorStatus & BITMASK_ERROR_OTHER + || MONRData->errorStatus & BITMASK_ERROR_VENDOR_SPECIFIC; monitorData->error.syncPointEnded = MONRData->errorStatus & BITMASK_ERROR_SYNC_POINT_ENDED; monitorData->error.outsideGeofence = MONRData->errorStatus & BITMASK_ERROR_OUTSIDE_GEOFENCE; - monitorData->error.badPositioningAccuracy = MONRData->errorStatus & BITMASK_ERROR_BAD_POSITIONING_ACCURACY; + monitorData->error.badPositioningAccuracy = + MONRData->errorStatus & BITMASK_ERROR_BAD_POSITIONING_ACCURACY; return; } @@ -1184,8 +1189,8 @@ void convertMONRToHostRepresentation(const MONRType * MONRData, ObjectMonitorTyp * \param debug Flag for enabling debugging * \return Number of bytes written to buffer, or -1 in case of an error */ -ssize_t encodeSYPMMessage(const struct timeval synchronizationTime, const struct timeval freezeTime, char * sypmDataBuffer, - const size_t bufferLength, const char debug) { +ssize_t encodeSYPMMessage(const struct timeval synchronizationTime, const struct timeval freezeTime, + char *sypmDataBuffer, const size_t bufferLength, const char debug) { SYPMType SYPMData; @@ -1215,8 +1220,7 @@ ssize_t encodeSYPMMessage(const struct timeval synchronizationTime, const struct "Freeze time content length: %u\n\t" "Freeze time: %u [ms]", SYPMData.syncPointTimeValueID, SYPMData.syncPointTimeContentLength, SYPMData.syncPointTime, - SYPMData.freezeTimeValueID, SYPMData.freezeTimeContentLength, - SYPMData.freezeTime); + SYPMData.freezeTimeValueID, SYPMData.freezeTimeContentLength, SYPMData.freezeTime); } // Switch from host endianness to little endian @@ -1243,7 +1247,8 @@ ssize_t encodeSYPMMessage(const struct timeval synchronizationTime, const struct * \param debug Flag for enabling debugging * \return Number of bytes written to buffer, or -1 in case of an error */ -ssize_t encodeMTSPMessage(const struct timeval * estSyncPointTime, char * mtspDataBuffer, const size_t bufferLength, const char debug) { +ssize_t encodeMTSPMessage(const struct timeval *estSyncPointTime, char *mtspDataBuffer, + const size_t bufferLength, const char debug) { MTSPType MTSPData; @@ -1261,7 +1266,9 @@ ssize_t encodeMTSPMessage(const struct timeval * estSyncPointTime, char * mtspDa // Fill contents MTSPData.estSyncPointTimeValueID = VALUE_ID_MTSP_EST_SYNC_POINT_TIME; MTSPData.estSyncPointTimeContentLength = sizeof (MTSPData.estSyncPointTime); - MTSPData.estSyncPointTime = estSyncPointTime == NULL ? GPS_SECOND_OF_WEEK_UNAVAILABLE_VALUE : TimeGetAsGPSqmsOfWeek(estSyncPointTime); + MTSPData.estSyncPointTime = + estSyncPointTime == + NULL ? GPS_SECOND_OF_WEEK_UNAVAILABLE_VALUE : TimeGetAsGPSqmsOfWeek(estSyncPointTime); if (debug) { LogPrint("MTSP message:\n\t" diff --git a/server/src/logger.c b/server/src/logger.c index 26feddf61..5cab5f4ec 100644 --- a/server/src/logger.c +++ b/server/src/logger.c @@ -585,8 +585,8 @@ void vLogMonitorData(char *commandData, ssize_t commandDatalen, struct timeval r TimeGetAsDateTime(&recvTime, "%Y;%m;%d;%H;%M;%S;%q", DateBuffer, sizeof (DateBuffer)); printedBytes = snprintf(printBuffer, sizeof (printBuffer), "%s;%ld;%ld;%d;", DateBuffer, - TimeGetAsUTCms(&monitorData.data.timestamp), TimeGetAsGPSms(&monitorData.data.timestamp), - (unsigned char)COMM_MONR); + TimeGetAsUTCms(&monitorData.data.timestamp), + TimeGetAsGPSms(&monitorData.data.timestamp), (unsigned char)COMM_MONR); totalPrintedBytes += printedBytes; if (printedBytes < 0 || (size_t) totalPrintedBytes > sizeof (printBuffer)) { @@ -594,8 +594,9 @@ void vLogMonitorData(char *commandData, ssize_t commandDatalen, struct timeval r return; } - printedBytes = snprintf(printBuffer + totalPrintedBytes, sizeof (printBuffer) - (size_t) totalPrintedBytes, "%s;", - inet_ntop(AF_INET, &monitorData.ClientIP, ipStringBuffer, sizeof (ipStringBuffer))); + printedBytes = + snprintf(printBuffer + totalPrintedBytes, sizeof (printBuffer) - (size_t) totalPrintedBytes, "%s;", + inet_ntop(AF_INET, &monitorData.ClientIP, ipStringBuffer, sizeof (ipStringBuffer))); totalPrintedBytes += printedBytes; if (printedBytes < 0 || (size_t) totalPrintedBytes > sizeof (printBuffer)) { @@ -603,8 +604,9 @@ void vLogMonitorData(char *commandData, ssize_t commandDatalen, struct timeval r return; } - printedBytes = snprintf(printBuffer + totalPrintedBytes, sizeof (printBuffer) - (size_t) totalPrintedBytes, "%u;", - monitorData.ClientID); + printedBytes = + snprintf(printBuffer + totalPrintedBytes, sizeof (printBuffer) - (size_t) totalPrintedBytes, "%u;", + monitorData.ClientID); totalPrintedBytes += printedBytes; if (printedBytes < 0 || (size_t) totalPrintedBytes > sizeof (printBuffer)) { @@ -612,7 +614,8 @@ void vLogMonitorData(char *commandData, ssize_t commandDatalen, struct timeval r return; } - objectMonitorDataToASCII(&monitorData.data, printBuffer + totalPrintedBytes, sizeof (printBuffer) - (size_t) totalPrintedBytes); + objectMonitorDataToASCII(&monitorData.data, printBuffer + totalPrintedBytes, + sizeof (printBuffer) - (size_t) totalPrintedBytes); filefd = fopen(pcLogFile, ACCESS_MODE_APPEND_AND_READ); if (filefd == NULL) { diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 1d9b24d6b..3c158b4c3 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -353,12 +353,13 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { ((GPSTime->GPSSecondsOfWeekU32 * 1000 + (U32) TimeControlGetMillisecond(GPSTime)) << 2) + GPSTime->MicroSecondU16; - /*MTSP*/ - if (timercmp(¤tTime, &nextAdaptiveSyncMessageTime, >)) { + /*MTSP*/ if (timercmp(¤tTime, &nextAdaptiveSyncMessageTime, >)) { - timeradd(&nextAdaptiveSyncMessageTime, &adaptiveSyncMessagePeriod, &nextAdaptiveSyncMessageTime); + timeradd(&nextAdaptiveSyncMessageTime, &adaptiveSyncMessagePeriod, + &nextAdaptiveSyncMessageTime); struct timeval estSyncPointTime; + TimeSetToGPStime(&estSyncPointTime, TimeGetAsGPSweek(¤tTime), ASPData.MTSPU32); for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { @@ -368,7 +369,9 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { && ASPData.MTSPU32 > 0 && ASPData.TimeToSyncPointDbl > -1) { /*Send Master time to adaptive sync point */ - MessageLength = encodeMTSPMessage(&estSyncPointTime, MessageBuffer, sizeof (MessageBuffer), 0); + MessageLength = + encodeMTSPMessage(&estSyncPointTime, MessageBuffer, sizeof (MessageBuffer), + 0); UtilSendUDPData(MODULE_NAME, &safety_socket_fd[iIndex], &safety_object_addr[iIndex], MessageBuffer, MessageLength, 0); } @@ -393,7 +396,9 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { buffer); monitorData.ClientIP = safety_object_addr[iIndex].sin_addr.s_addr; - if (decodeMONRMessage(buffer, receivedMONRData, &monitorData.ClientID, &monitorData.data, 0) != MESSAGE_OK) { + if (decodeMONRMessage + (buffer, receivedMONRData, &monitorData.ClientID, &monitorData.data, + 0) != MESSAGE_OK) { LogMessage(LOG_LEVEL_INFO, "Error decoding MONR from %s: disconnecting object", object_address_name[iIndex]); vDisconnectObject(&safety_socket_fd[iIndex]); @@ -405,8 +410,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { // Place struct in buffer memcpy(&buffer, &monitorData, sizeof (monitorData)); // Send MONR message as bytes - if (iCommSend(COMM_MONR, buffer, sizeof (monitorData)) < - 0) { + if (iCommSend(COMM_MONR, buffer, sizeof (monitorData)) < 0) { LogMessage(LOG_LEVEL_ERROR, "Fatal communication fault when sending MONR command - entering error state"); vSetState(OBC_STATE_ERROR, GSD); @@ -417,7 +421,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { //Store MONR in GSD memcpy(GSD->MONRData, buffer, receivedMONRData); - GSD->MONRSizeU8 = (unsigned char) receivedMONRData; + GSD->MONRSizeU8 = (unsigned char)receivedMONRData; memset(buffer, 0, sizeof (buffer)); UtilMonitorDataToString(monitorData, buffer, sizeof (buffer)); @@ -447,8 +451,8 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { // TODO: check use of this function since it should take two lat/long points but is here used with x/y UtilCalcPositionDelta(OriginLatitudeDbl, OriginLongitudeDbl, - monitorData.data.position.xCoord_m, monitorData.data.position.yCoord_m, - &OP[iIndex]); + monitorData.data.position.xCoord_m, + monitorData.data.position.yCoord_m, &OP[iIndex]); if (OP[iIndex].BestFoundTrajectoryIndex <= OP[iIndex].SyncIndex) { ASPData.CurrentTimeU32 = CurrentTimeU32; @@ -515,7 +519,8 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { } } OP[iIndex].Speed = - (float) sqrt(pow(monitorData.data.speed.lateral_m_s, 2) + pow(monitorData.data.speed.longitudinal_m_s, 2)); + (float)sqrt(pow(monitorData.data.speed.lateral_m_s, 2) + + pow(monitorData.data.speed.longitudinal_m_s, 2)); } else if (receivedMONRData > 0) LogMessage(LOG_LEVEL_WARNING, "Received unhandled message on monitoring socket"); @@ -801,10 +806,11 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { DataDictionaryGetOriginLongitudeC8(GSD, OriginLongitude, SMALL_BUFFER_SIZE_0); DataDictionaryGetOriginAltitudeC8(GSD, OriginAltitude, SMALL_BUFFER_SIZE_0); - memset(pcSendBuffer, 0, sizeof(pcSendBuffer)); + memset(pcSendBuffer, 0, sizeof (pcSendBuffer)); snprintf(pcSendBuffer, sizeof (pcSendBuffer), "%u;", TimeGetAsGPSweek(¤tTime)); - snprintf(pcSendBuffer + strlen(pcSendBuffer), sizeof (pcSendBuffer) - strlen(pcSendBuffer), "%s;%s;%s;%s;", - OriginLatitude, OriginLongitude, OriginAltitude, OriginHeading); + snprintf(pcSendBuffer + strlen(pcSendBuffer), + sizeof (pcSendBuffer) - strlen(pcSendBuffer), "%s;%s;%s;%s;", OriginLatitude, + OriginLongitude, OriginAltitude, OriginHeading); if (iCommSend(COMM_OSEM, pcSendBuffer, strlen(pcSendBuffer) + 1) < 0) { LogMessage(LOG_LEVEL_ERROR, @@ -869,18 +875,23 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { LogMessage(LOG_LEVEL_INFO, "Sync point counts: %d", SyncPointCount); for (i = 0; i < SyncPointCount; i++) { struct timeval syncPointTime, syncStopTime; + TimeSetToUTCms(&syncPointTime, (int64_t) (ASP[i].SlaveTrajSyncTime * 1000.0f)); TimeSetToUTCms(&syncStopTime, (int64_t) (ASP[i].SlaveSyncStopTime * 1000.0f)); if (TEST_SYNC_POINTS == 1 && iIndex == 1) { /*Send SYPM to slave */ - MessageLength = encodeSYPMMessage(syncPointTime, syncStopTime, MessageBuffer, sizeof (MessageBuffer), 0); + MessageLength = + encodeSYPMMessage(syncPointTime, syncStopTime, MessageBuffer, + sizeof (MessageBuffer), 0); UtilSendTCPData(MODULE_NAME, MessageBuffer, MessageLength, &socket_fds[iIndex], 0); } else if (TEST_SYNC_POINTS == 0 && strstr(object_address_name[iIndex], ASP[i].SlaveIP) != NULL) { /*Send SYPM to slave */ - MessageLength = encodeSYPMMessage(syncPointTime, syncStopTime, MessageBuffer, sizeof (MessageBuffer), 0); + MessageLength = + encodeSYPMMessage(syncPointTime, syncStopTime, MessageBuffer, + sizeof (MessageBuffer), 0); UtilSendTCPData(MODULE_NAME, MessageBuffer, MessageLength, &socket_fds[iIndex], 0); } diff --git a/server/src/positioning.c b/server/src/positioning.c index ffd63089d..2599902a4 100644 --- a/server/src/positioning.c +++ b/server/src/positioning.c @@ -33,7 +33,7 @@ static const char notReadyToArmString[] = "NOT_READY"; * \param state Variable to be converted * \return Pointer to a string of the state */ -const char * objectStateToASCII(const ObjectStateType state) { +const char *objectStateToASCII(const ObjectStateType state) { switch (state) { case OBJECT_STATE_DISARMED: return disarmedStateString; @@ -58,7 +58,7 @@ const char * objectStateToASCII(const ObjectStateType state) { * \param asciiString String to convert * \return Value according to ::ObjectStateType */ -ObjectStateType ASCIIToObjectState(const char * asciiString) { +ObjectStateType ASCIIToObjectState(const char *asciiString) { if (strstr(asciiString, disarmedStateString) != NULL) return OBJECT_STATE_DISARMED; if (strstr(asciiString, armedStateString) != NULL) @@ -82,7 +82,7 @@ ObjectStateType ASCIIToObjectState(const char * asciiString) { */ bool hasError(const ObjectErrorType error) { return error.engineFault || error.abortRequest || error.batteryFault || error.unknownError - || error.syncPointEnded || error.outsideGeofence || error.badPositioningAccuracy; + || error.syncPointEnded || error.outsideGeofence || error.badPositioningAccuracy; } /*! @@ -91,7 +91,7 @@ bool hasError(const ObjectErrorType error) { * \param asciiBuffer Buffer to which text is to be written * \param bufferLength Size of buffer to which text is to be written */ -void errorStatusToASCII(const ObjectErrorType error, char* asciiBuffer, const size_t bufferLength) { +void errorStatusToASCII(const ObjectErrorType error, char *asciiBuffer, const size_t bufferLength) { memset(asciiBuffer, 0, bufferLength); @@ -102,23 +102,17 @@ void errorStatusToASCII(const ObjectErrorType error, char* asciiBuffer, const si if (error.engineFault) snprintf(asciiBuffer, bufferLength, "%s,", engineFaultString); if (error.batteryFault) - snprintf(asciiBuffer, bufferLength - strlen(asciiBuffer), - "%s,", batteryFaultString); + snprintf(asciiBuffer, bufferLength - strlen(asciiBuffer), "%s,", batteryFaultString); if (error.unknownError) - snprintf(asciiBuffer, bufferLength - strlen(asciiBuffer), - "%s,", otherErrorString); + snprintf(asciiBuffer, bufferLength - strlen(asciiBuffer), "%s,", otherErrorString); if (error.syncPointEnded) - snprintf(asciiBuffer, bufferLength - strlen(asciiBuffer), - "%s,", syncPointEndedString); + snprintf(asciiBuffer, bufferLength - strlen(asciiBuffer), "%s,", syncPointEndedString); if (error.outsideGeofence) - snprintf(asciiBuffer, bufferLength - strlen(asciiBuffer), - "%s,", outsideGeofenceString); + snprintf(asciiBuffer, bufferLength - strlen(asciiBuffer), "%s,", outsideGeofenceString); if (error.badPositioningAccuracy) - snprintf(asciiBuffer, bufferLength - strlen(asciiBuffer), - "%s,", badPositioningAccuracyString); + snprintf(asciiBuffer, bufferLength - strlen(asciiBuffer), "%s,", badPositioningAccuracyString); if (error.abortRequest) - snprintf(asciiBuffer, bufferLength - strlen(asciiBuffer), - "%s,", abortRequestString); + snprintf(asciiBuffer, bufferLength - strlen(asciiBuffer), "%s,", abortRequestString); if (asciiBuffer[strlen(asciiBuffer)] == ',') asciiBuffer[strlen(asciiBuffer)] = '\0'; @@ -132,28 +126,29 @@ void errorStatusToASCII(const ObjectErrorType error, char* asciiBuffer, const si * \param asciiString String to be parsed * \return Value according to ::ObjectErrorType */ -ObjectErrorType ASCIIToErrorStatus(const char * asciiString) { +ObjectErrorType ASCIIToErrorStatus(const char *asciiString) { ObjectErrorType error; + memset(&error, 0, sizeof (error)); // First check if string starts with the "all ok" string - if(strstr(asciiString, noErrorString) == asciiString) + if (strstr(asciiString, noErrorString) == asciiString) return error; // Check against all error string representations - if(strstr(asciiString, batteryFaultString) != NULL) + if (strstr(asciiString, batteryFaultString) != NULL) error.batteryFault = true; - if(strstr(asciiString, engineFaultString) != NULL) + if (strstr(asciiString, engineFaultString) != NULL) error.engineFault = true; - if(strstr(asciiString, abortRequestString) != NULL) + if (strstr(asciiString, abortRequestString) != NULL) error.abortRequest = true; - if(strstr(asciiString, otherErrorString) != NULL) + if (strstr(asciiString, otherErrorString) != NULL) error.unknownError = true; - if(strstr(asciiString, syncPointEndedString) != NULL) + if (strstr(asciiString, syncPointEndedString) != NULL) error.syncPointEnded = true; - if(strstr(asciiString, outsideGeofenceString) != NULL) + if (strstr(asciiString, outsideGeofenceString) != NULL) error.outsideGeofence = true; - if(strstr(asciiString, badPositioningAccuracyString) != NULL) + if (strstr(asciiString, badPositioningAccuracyString) != NULL) error.badPositioningAccuracy = true; // Error unmatched against all strings casted into unknown type @@ -170,7 +165,8 @@ ObjectErrorType ASCIIToErrorStatus(const char * asciiString) { * \param bufferLength Length of ASCII buffer * \return Number of bytes printed */ -int objectMonitorDataToASCII(const ObjectMonitorType * monitorData, char *asciiBuffer, const size_t bufferLength) { +int objectMonitorDataToASCII(const ObjectMonitorType * monitorData, char *asciiBuffer, + const size_t bufferLength) { memset(asciiBuffer, 0, bufferLength); @@ -178,45 +174,40 @@ int objectMonitorDataToASCII(const ObjectMonitorType * monitorData, char *asciiB snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), "%u;", TimeGetAsGPSqmsOfWeek(&monitorData->timestamp)); else - snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), - "NaN;"); + snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), "NaN;"); if (monitorData->position.isPositionValid) snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), "%.3f;%.3f;%.3f;", monitorData->position.xCoord_m, monitorData->position.yCoord_m, monitorData->position.zCoord_m); else - snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), - "NaN;NaN;NaN;"); + snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), "NaN;NaN;NaN;"); if (monitorData->position.isHeadingValid) snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), "%.2f;", monitorData->position.heading_deg); else - snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), - "NaN;"); + snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), "NaN;"); if (monitorData->speed.isValid) snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), - "%.2f;%.2f;", monitorData->speed.longitudinal_m_s, monitorData->speed.lateral_m_s); + "%.2f;%.2f;", monitorData->speed.longitudinal_m_s, monitorData->speed.lateral_m_s); else - snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), - "NaN;NaN;"); + snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), "NaN;NaN;"); if (monitorData->acceleration.isValid) snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), - "%.3f;%.3f;", monitorData->acceleration.longitudinal_m_s2, monitorData->acceleration.lateral_m_s2); + "%.3f;%.3f;", monitorData->acceleration.longitudinal_m_s2, + monitorData->acceleration.lateral_m_s2); else - snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), - "NaN;NaN;"); + snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), "NaN;NaN;"); if (monitorData->drivingDirection != OBJECT_DRIVE_DIRECTION_UNAVAILABLE) snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), "%s;", monitorData->drivingDirection == OBJECT_DRIVE_DIRECTION_FORWARD ? - driveDirectionForwardString : driveDirectionReverseString); + driveDirectionForwardString : driveDirectionReverseString); else - snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), - "UNKNOWN;"); + snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), "UNKNOWN;"); snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), "%s;", objectStateToASCII(monitorData->state)); @@ -224,17 +215,16 @@ int objectMonitorDataToASCII(const ObjectMonitorType * monitorData, char *asciiB if (monitorData->armReadiness != OBJECT_READY_TO_ARM_UNAVAILABLE) snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), "%s;", monitorData->armReadiness == OBJECT_READY_TO_ARM ? - readyToArmString : notReadyToArmString); + readyToArmString : notReadyToArmString); else - snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), - "UNKNOWN;"); + snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), "UNKNOWN;"); - errorStatusToASCII(monitorData->error, asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer)); + errorStatusToASCII(monitorData->error, asciiBuffer + strlen(asciiBuffer), + bufferLength - strlen(asciiBuffer)); - snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), - ";"); + snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), ";"); - return (int) strlen(asciiBuffer); + return (int)strlen(asciiBuffer); } @@ -254,6 +244,7 @@ int ASCIIToObjectMonitorData(const char *asciiBuffer, ObjectMonitorType * monito const int NumberBaseDecimal = 10; char *copy = strdup(asciiBuffer); struct timeval currentTime; + TimeSetToCurrentSystemTime(¤tTime); memset(monitorData, 0, sizeof (*monitorData)); diff --git a/server/src/util.c b/server/src/util.c index 8167cf1f7..d51685773 100644 --- a/server/src/util.c +++ b/server/src/util.c @@ -702,7 +702,9 @@ int UtilMonitorDataToString(const MonitorDataType monitorData, char *monitorData (stringLength > UINT_MAX) ? UINT_MAX : (socklen_t) stringLength); strcat(monitorDataString, ";0;"); - if (objectMonitorDataToASCII(&monitorData.data, monitorDataString + strlen(monitorDataString), stringLength - strlen(monitorDataString)) < 0) { + if (objectMonitorDataToASCII + (&monitorData.data, monitorDataString + strlen(monitorDataString), + stringLength - strlen(monitorDataString)) < 0) { memset(monitorDataString, 0, stringLength); return -1; } @@ -749,6 +751,7 @@ int UtilStringToMonitorData(const char *monitorString, size_t stringLength, Moni */ uint8_t UtilIsPositionNearTarget(CartesianPosition position, CartesianPosition target, double tolerance_m) { double distance = 0.0; + if (!position.isPositionValid || target.isPositionValid) return 0; distance = sqrt(pow(position.xCoord_m - target.xCoord_m, 2) @@ -3593,7 +3596,8 @@ I32 UtilWriteConfigurationParameter(C8 * ParameterName, C8 * NewValue, U8 Debug) * \param MONR Struct where monitor data should be placed * \return -1 on failure, 0 on success */ -int UtilPopulateMonitorDataStruct(const char * rawData, const size_t rawDataSize, MonitorDataType * monitorData) { +int UtilPopulateMonitorDataStruct(const char *rawData, const size_t rawDataSize, + MonitorDataType * monitorData) { if (rawDataSize != sizeof (MonitorDataType)) { errno = EMSGSIZE; From da9d1141cc01d0429b22095bdb6993edc598c93a Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 6 Feb 2020 18:36:51 +0100 Subject: [PATCH 239/523] Ran code formatter --- server/src/iso22133.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 80902bee1..5c50dd45f 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -1085,7 +1085,8 @@ void convertMONRToHostRepresentation(const MONRType * MONRData, ObjectMonitorTyp monitorData->position.isPositionValid = true; monitorData->position.isHeadingValid = MONRData->heading != HEADING_UNAVAILABLE_VALUE; if (monitorData->position.isHeadingValid) { - monitorData->position.heading_rad = mapISOHeadingToHostHeading(MONRData->heading / 100.0 * M_PI / 180.0); + monitorData->position.heading_rad = + mapISOHeadingToHostHeading(MONRData->heading / 100.0 * M_PI / 180.0); } // Velocity @@ -1611,6 +1612,7 @@ double mapISOHeadingToHostHeading(const double isoHeading_rad) { // TODO: Reevaluate this when ISO specification is updated with new heading and rotated coordinate system double retval = isoHeading_rad; + // Host heading is CCW while ISO is CW retval = -retval; // Host heading is measured from the x axis while ISO is measured from the y axis @@ -1624,4 +1626,3 @@ double mapISOHeadingToHostHeading(const double isoHeading_rad) { } return retval; } - From 973c2cb3011fe637d2116d906f0f553ef0d8a9c0 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 11 Feb 2020 09:38:58 +0100 Subject: [PATCH 240/523] Implemented TRAJ functions in iso22133 --- server/inc/iso22133.h | 9 +- server/src/iso22133.c | 403 +++++++++++++++++++++++++++++++++++++++++- 2 files changed, 404 insertions(+), 8 deletions(-) diff --git a/server/inc/iso22133.h b/server/inc/iso22133.h index 1599676b1..702ab4d44 100644 --- a/server/inc/iso22133.h +++ b/server/inc/iso22133.h @@ -20,6 +20,7 @@ extern "C" { #include #include #include +#include #include "positioning.h" @@ -45,6 +46,7 @@ typedef struct uint16_t Crc; } FooterType; //2 bytes +#pragma pack(pop) /*! OSTM commands */ typedef enum { @@ -67,8 +69,6 @@ typedef enum { #define MONR_EXPECTED_FREQUENCY_HZ 100 -#pragma pack(pop) - typedef enum { TRIGGER_UNDEFINED = 0x0000, TRIGGER_TYPE_1 = 0x0001, @@ -217,8 +217,11 @@ typedef enum { } SupervisorCommandType; ISOMessageReturnValue decodeMONRMessage(const char * monrDataBuffer, const size_t bufferLength, uint32_t * objectID, ObjectMonitorType * MonitorData, const char debug); +ssize_t encodeTRAJMessageHeader(const uint16_t trajectoryID, const uint16_t trajectoryVersion, const char * trajectoryName, const size_t nameLength, const uint32_t numberOfPointsInTraj, char * trajDataBuffer, const size_t bufferLength, const char debug); +ssize_t encodeTRAJMessagePoint(const struct timeval * pointTimeFromStart, const CartesianPosition position, const SpeedType speed, const AccelerationType acceleration, const float curvature, char * trajDataBufferPointer, const size_t remainingBufferLength, const char debug); +ssize_t encodeTRAJMessageFooter(char * trajDataBuffer, const char * trajDataBufferStart, const size_t bufferLength, const char debug); ssize_t encodeSTRTMessage(const struct timeval* timeOfStart, char * strtDataBuffer, const size_t bufferLength, const char debug); -ssize_t encodeOSEMMessage(const double * latitude_deg, const double * longitude_deg, const float * altitude_m, const float * maxPositionDeviation_m, const float * maxLateralDeviation_m, const float * minimumPositioningAccuracy_m, char * osemDataBuffer, const size_t bufferLength, const char debug); +ssize_t encodeOSEMMessage(const double_t * latitude_deg, const double_t * longitude_deg, const float * altitude_m, const float * maxPositionDeviation_m, const float * maxLateralDeviation_m, const float * minimumPositioningAccuracy_m, char * osemDataBuffer, const size_t bufferLength, const char debug); ssize_t encodeOSTMMessage(const ObjectCommandType command, char * ostmDataBuffer, const size_t bufferLength, const char debug); ssize_t encodeHEABMessage(const ControlCenterStatusType status, char * heabDataBuffer, const size_t bufferLength, const char debug); ssize_t encodeSYPMMessage(const struct timeval synchronizationTime, const struct timeval freezeTime, char * sypmDataBuffer, const size_t bufferLength, const char debug); diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 5c50dd45f..6eb1f9096 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -5,6 +5,7 @@ #include #include #include +#include #include // ************************* ISO protocol versions supported by functions in this file *************************** @@ -37,10 +38,13 @@ static const uint8_t SupportedProtocolVersions[] = { 2 }; #define ACTION_TYPE_PARAMETER_UNAVAILABLE 4294967295 #define POSITION_ONE_METER_VALUE 1000 #define HEADING_UNAVAILABLE_VALUE 36001 +#define HEADING_ONE_DEGREE_VALUE 100 #define SPEED_UNAVAILABLE_VALUE (-32768) #define SPEED_ONE_METER_PER_SECOND_VALUE 100 #define ACCELERATION_UNAVAILABLE_VALUE 32001 #define ACCELERATION_ONE_METER_PER_SECOND_SQUARED_VALUE 1000 +#define RELATIVE_TIME_ONE_SECOND_VALUE 1000 + typedef enum { ISO_DRIVE_DIRECTION_FORWARD = 0, ISO_DRIVE_DIRECTION_BACKWARD = 1, @@ -74,6 +78,73 @@ typedef enum { #pragma pack(push,1) // Ensure sizeof() is useable for (most) network byte lengths +/*! TRAJ message */ +#define TRAJ_NAME_STRING_MAX_LENGTH 64 +typedef struct { + HeaderType header; + uint16_t trajectoryIDValueID; + uint16_t trajectoryIDContentLength; + uint16_t trajectoryID; + uint16_t trajectoryNameValueID; + uint16_t trajectoryNameContentLength; + char trajectoryName[TRAJ_NAME_STRING_MAX_LENGTH]; + uint16_t trajectoryVersionValueID; + uint16_t trajectoryVersionContentLength; + uint16_t trajectoryVersion; +} TRAJHeaderType; + +typedef struct { + uint16_t relativeTimeValueID; + uint16_t relativeTimeContentLength; + uint32_t relativeTime; + uint16_t xPositionValueID; + uint16_t xPositionContentLength; + int32_t xPosition; + uint16_t yPositionValueID; + uint16_t yPositionContentLength; + int32_t yPosition; + uint16_t zPositionValueID; + uint16_t zPositionContentLength; + int32_t zPosition; + uint16_t headingValueID; + uint16_t headingContentLength; + uint16_t heading; + uint16_t longitudinalSpeedValueID; + uint16_t longitudinalSpeedContentLength; + int16_t longitudinalSpeed; + uint16_t lateralSpeedValueID; + uint16_t lateralSpeedContentLength; + int16_t lateralSpeed; + uint16_t longitudinalAccelerationValueID; + uint16_t longitudinalAccelerationContentLength; + int16_t longitudinalAcceleration; + uint16_t lateralAccelerationValueID; + uint16_t lateralAccelerationContentLength; + int16_t lateralAcceleration; + uint16_t curvatureValueID; + uint16_t curvatureContentLength; + float_t curvature; +} TRAJPointType; + +typedef struct { + FooterType footer; +} TRAJFooterType; + +//! TRAJ value IDs +#define VALUE_ID_TRAJ_TRAJECTORY_IDENTIFIER 0x0101 +#define VALUE_ID_TRAJ_TRAJECTORY_NAME 0x0101 +#define VALUE_ID_TRAJ_TRAJECTORY_VERSION 0x0101 +#define VALUE_ID_TRAJ_RELATIVE_TIME 0x0001 +#define VALUE_ID_TRAJ_X_POSITION 0x0010 +#define VALUE_ID_TRAJ_Y_POSITION 0x0011 +#define VALUE_ID_TRAJ_Z_POSITION 0x0012 +#define VALUE_ID_TRAJ_HEADING 0x0030 +#define VALUE_ID_TRAJ_LONGITUDINAL_SPEED 0x0040 +#define VALUE_ID_TRAJ_LATERAL_SPEED 0x0041 +#define VALUE_ID_TRAJ_LONGITUDINAL_ACCELERATION 0x0050 +#define VALUE_ID_TRAJ_LATERAL_ACCELERATION 0x0051 +#define VALUE_ID_TRAJ_CURVATURE 0x0052 + /*! OSEM message */ typedef struct { HeaderType header; @@ -331,13 +402,18 @@ typedef struct { // ************************* Non-ISO type definitions and defines ************************************************ -// Byte swapper definitions for 6 byte values +// Byte swapper definitions for 6 byte values and floats #if __BYTE_ORDER == __LITTLE_ENDIAN #define le48toh(x) (x) #define htole48(x) (x) +#define htolef_a(x) \ + htole32((union { uint32_t i; float f; }){ .f = (x) }.i) +#define htolef(x) \ + ((union { uint32_t i; float f; }){ .i = htolef_a(x) }.f) #else #define le48toh(x) (le64toh(x) >> 16) #define htole48(x) (htole64(x) >> 16) +#define htolef(x) (x) #endif // ************************** static function declarations ******************************************************** @@ -348,7 +424,8 @@ static ISOMessageReturnValue decodeISOFooter(const char *MessageBuffer, const si static HeaderType buildISOHeader(ISOMessageID id, uint32_t messageLength, const char debug); static FooterType buildISOFooter(const void *message, const size_t sizeExclFooter, const char debug); static char isValidMessageID(const uint16_t id); -static double mapISOHeadingToHostHeading(const double isoHeading_rad); +static double_t mapISOHeadingToHostHeading(const double_t isoHeading_rad); +static double_t mapHostHeadingToISOHeading(const double_t hostHeading_rad); static void convertMONRToHostRepresentation(const MONRType * MONRData, ObjectMonitorType * monitorData); // ************************** function definitions **************************************************************** @@ -565,6 +642,298 @@ ISOMessageID getISOMessageType(const char *messageData, const size_t length, con } } +ssize_t encodeTRAJMessageHeader(const uint16_t trajectoryID, const uint16_t trajectoryVersion, + const char * trajectoryName, const size_t nameLength, + const uint32_t numberOfPointsInTraj, char *trajDataBuffer, + const size_t bufferLength, const char debug){ + + TRAJHeaderType TRAJData; + + memset(trajDataBuffer, 0, bufferLength); + + // Error guarding + if (trajectoryName == NULL && nameLength > 0) { + errno = EINVAL; + LogMessage(LOG_LEVEL_ERROR, "Trajectory name length and pointer mismatch"); + return -1; + } + else if (trajDataBuffer == NULL) { + errno = EINVAL; + LogMessage(LOG_LEVEL_ERROR, "Trajectory data buffer invalid"); + return -1; + } + else if (bufferLength < sizeof (TRAJHeaderType)) { + errno = ENOBUFS; + LogMessage(LOG_LEVEL_ERROR, "Buffer too small to hold necessary TRAJ header data"); + return -1; + } + else if (nameLength > sizeof (TRAJData.trajectoryName)) { + errno = EMSGSIZE; + LogMessage(LOG_LEVEL_ERROR, "Trajectory name <%s> too long for TRAJ message", trajectoryName); + return -1; + } + + // Construct ISO header + TRAJData.header = buildISOHeader(MESSAGE_ID_TRAJ, sizeof (TRAJHeaderType) + + numberOfPointsInTraj * sizeof (TRAJPointType) + sizeof (TRAJFooterType), debug); + + // Fill contents + TRAJData.trajectoryIDValueID = VALUE_ID_TRAJ_TRAJECTORY_IDENTIFIER; + TRAJData.trajectoryIDContentLength = sizeof (TRAJData.trajectoryID); + TRAJData.trajectoryID = trajectoryID; + + TRAJData.trajectoryVersionValueID = VALUE_ID_TRAJ_TRAJECTORY_VERSION; + TRAJData.trajectoryVersionContentLength = sizeof (TRAJData.trajectoryVersion); + TRAJData.trajectoryVersion = trajectoryVersion; + + TRAJData.trajectoryNameValueID = VALUE_ID_TRAJ_TRAJECTORY_NAME; + TRAJData.trajectoryNameContentLength = sizeof (TRAJData.trajectoryName); + memset(TRAJData.trajectoryName, 0, sizeof (TRAJData.trajectoryName)); + if (trajectoryName != NULL) { + memcpy(&TRAJData.trajectoryName, trajectoryName, nameLength); + } + + if (debug) { + LogPrint("TRAJ message header:\n\t" + "Trajectory ID value ID: 0x%x\n\t" + "Trajectory ID content length: %u\n\t" + "Trajectory ID: %u\n\t" + "Trajectory name value ID: 0x%x\n\t" + "Trajectory name content length: %u\n\t" + "Trajectory name: %s\n\t" + "Trajectory version value ID: 0x%x\n\t" + "Trajectory version content length: %u\n\t" + "Trajectory version: %u", TRAJData.trajectoryIDValueID, + TRAJData.trajectoryIDContentLength, TRAJData.trajectoryID, + TRAJData.trajectoryNameValueID, TRAJData.trajectoryNameContentLength, + TRAJData.trajectoryName, TRAJData.trajectoryVersionValueID, + TRAJData.trajectoryVersionContentLength, TRAJData.trajectoryVersion); + } + + // Switch endianness to little endian for all fields + TRAJData.trajectoryIDValueID = htole16(TRAJData.trajectoryIDValueID); + TRAJData.trajectoryIDContentLength = htole16(TRAJData.trajectoryIDContentLength); + TRAJData.trajectoryID = htole16(TRAJData.trajectoryID); + TRAJData.trajectoryVersionValueID = htole16(TRAJData.trajectoryVersionValueID); + TRAJData.trajectoryVersionContentLength = htole16(TRAJData.trajectoryVersionContentLength); + TRAJData.trajectoryVersion = htole16(TRAJData.trajectoryVersion); + TRAJData.trajectoryNameValueID = htole16(TRAJData.trajectoryNameValueID); + TRAJData.trajectoryNameContentLength = htole16(TRAJData.trajectoryNameContentLength); + + memcpy(trajDataBuffer, &TRAJData, sizeof (TRAJData)); + + return sizeof (TRAJHeaderType); +} + +ssize_t encodeTRAJMessagePoint(const struct timeval * pointTimeFromStart, const CartesianPosition position, const SpeedType speed, + const AccelerationType acceleration, const float curvature, char * trajDataBufferPointer, + const size_t remainingBufferLength, const char debug) { + + TRAJPointType TRAJData; + + if (remainingBufferLength < sizeof (TRAJPointType)) { + errno = ENOBUFS; + LogMessage(LOG_LEVEL_ERROR, "Buffer too small to hold necessary TRAJ point data"); + return -1; + } + else if (trajDataBufferPointer == NULL) { + errno = EINVAL; + LogMessage(LOG_LEVEL_ERROR, "Trajectory data buffer invalid"); + return -1; + } + + // Fill contents + TRAJData.relativeTimeValueID = VALUE_ID_TRAJ_RELATIVE_TIME; + TRAJData.relativeTimeContentLength = sizeof (TRAJData.relativeTime); + TRAJData.relativeTime = (uint32_t) TimeGetAsUTCms(pointTimeFromStart) / (1000 / RELATIVE_TIME_ONE_SECOND_VALUE); + + TRAJData.xPositionValueID = VALUE_ID_TRAJ_X_POSITION; + TRAJData.xPositionContentLength = sizeof (TRAJData.xPosition); + TRAJData.yPositionValueID = VALUE_ID_TRAJ_Y_POSITION; + TRAJData.yPositionContentLength = sizeof (TRAJData.yPosition); + TRAJData.zPositionValueID = VALUE_ID_TRAJ_Z_POSITION; + TRAJData.zPositionContentLength = sizeof (TRAJData.zPosition); + if (position.isPositionValid) { + TRAJData.xPosition = (int32_t) (position.xCoord_m * POSITION_ONE_METER_VALUE); + TRAJData.yPosition = (int32_t) (position.yCoord_m * POSITION_ONE_METER_VALUE); + TRAJData.zPosition = (int32_t) (position.zCoord_m * POSITION_ONE_METER_VALUE); + } + else { + errno = EINVAL; + LogMessage(LOG_LEVEL_ERROR, "Position is a required field in TRAJ messages"); + return -1; + } + + TRAJData.headingValueID = VALUE_ID_TRAJ_HEADING; + TRAJData.headingContentLength = sizeof (TRAJData.heading); + if (position.isHeadingValid) { + TRAJData.heading = (uint16_t) (mapHostHeadingToISOHeading(position.heading_rad) + * 180.0 / M_PI * HEADING_ONE_DEGREE_VALUE); + } + else { + errno = EINVAL; + LogMessage(LOG_LEVEL_ERROR, "Heading is a required field in TRAJ messages"); + return -1; + } + + TRAJData.longitudinalSpeedValueID = VALUE_ID_TRAJ_LONGITUDINAL_SPEED; + TRAJData.longitudinalSpeedContentLength = sizeof (TRAJData.longitudinalSpeed); + TRAJData.lateralSpeedValueID = VALUE_ID_TRAJ_LATERAL_SPEED; + TRAJData.lateralSpeedContentLength = sizeof (TRAJData.lateralSpeed); + if (speed.isValid) { + TRAJData.longitudinalSpeed = (int16_t) (speed.longitudinal_m_s * SPEED_ONE_METER_PER_SECOND_VALUE); + TRAJData.lateralSpeed = (int16_t) (speed.lateral_m_s * SPEED_ONE_METER_PER_SECOND_VALUE); + } + else { + LogMessage(LOG_LEVEL_WARNING, "TRAJ speed supplied not valid: assuming longitudinal still valid"); + TRAJData.longitudinalSpeed = (int16_t) (speed.longitudinal_m_s * SPEED_ONE_METER_PER_SECOND_VALUE); + TRAJData.lateralSpeed = SPEED_UNAVAILABLE_VALUE; + } + + TRAJData.longitudinalAccelerationValueID = VALUE_ID_TRAJ_LONGITUDINAL_ACCELERATION; + TRAJData.longitudinalAccelerationContentLength = sizeof (TRAJData.longitudinalAcceleration); + TRAJData.lateralAccelerationValueID = VALUE_ID_TRAJ_LATERAL_ACCELERATION; + TRAJData.lateralAccelerationContentLength = sizeof (TRAJData.lateralAcceleration); + if (acceleration.isValid) { + TRAJData.longitudinalAcceleration = (int16_t) (acceleration.longitudinal_m_s2 * ACCELERATION_ONE_METER_PER_SECOND_SQUARED_VALUE); + TRAJData.lateralAcceleration = (int16_t) (acceleration.lateral_m_s2 * ACCELERATION_ONE_METER_PER_SECOND_SQUARED_VALUE); + } + else { + LogMessage(LOG_LEVEL_WARNING, "TRAJ acceleration supplied not valid: assuming longitudinal still valid"); + TRAJData.longitudinalAcceleration = (int16_t) (acceleration.longitudinal_m_s2 * ACCELERATION_ONE_METER_PER_SECOND_SQUARED_VALUE); + TRAJData.lateralAcceleration = ACCELERATION_UNAVAILABLE_VALUE; + } + + TRAJData.curvatureValueID = VALUE_ID_TRAJ_CURVATURE; + TRAJData.curvatureContentLength = sizeof (TRAJData.curvature); + TRAJData.curvature = curvature; + + if (debug) { + LogPrint("TRAJ message point:\n\t" + "Relative time value ID: 0x%x\n\t" + "Relative time content length: %u\n\t" + "Relative time: %u\n\t" + "x position value ID: 0x%x\n\t" + "x position content length: %u\n\t" + "x position: %d\n\t" + "y position value ID: 0x%x\n\t" + "y position content length: %u\n\t" + "y position: %d\n\t" + "z position value ID: 0x%x\n\t" + "z position content length: %u\n\t" + "z position: %d\n\t" + "Heading value ID: 0x%x\n\t" + "Heading content length: %u\n\t" + "Heading: %u\n\t" + "Longitudinal speed value ID: 0x%x\n\t" + "Longitudinal speed content length: %u\n\t" + "Longitudinal speed: %d\n\t" + "Lateral speed value ID: 0x%x\n\t" + "Lateral speed content length: %u\n\t" + "Lateral speed: %d\n\t" + "Longitudinal acceleration value ID: 0x%x\n\t" + "Longitudinal acceleration content length: %u\n\t" + "Longitudinal acceleration: %d\n\t" + "Lateral acceleration value ID: 0x%x\n\t" + "Lateral acceleration content length: %u\n\t" + "Lateral acceleration: %d\n\t" + "Curvature value ID: 0x%x\n\t" + "Curvature content length: %u\n\t" + "Curvature: %.6f", + TRAJData.relativeTimeValueID, TRAJData.relativeTimeContentLength, + TRAJData.relativeTime, TRAJData.xPositionValueID, TRAJData.xPositionContentLength, + TRAJData.xPosition, TRAJData.yPositionValueID, TRAJData.yPositionContentLength, + TRAJData.yPosition, TRAJData.zPositionValueID, TRAJData.zPositionContentLength, + TRAJData.zPosition, TRAJData.headingValueID, TRAJData.headingContentLength, + TRAJData.heading, TRAJData.longitudinalSpeedValueID, TRAJData.longitudinalSpeedContentLength, + TRAJData.longitudinalSpeed, TRAJData.lateralSpeedValueID, TRAJData.lateralSpeedContentLength, + TRAJData.lateralSpeed, TRAJData.longitudinalAccelerationValueID, + TRAJData.longitudinalAccelerationContentLength, TRAJData.longitudinalAcceleration, + TRAJData.lateralAccelerationValueID, TRAJData.lateralAccelerationContentLength, + TRAJData.lateralAcceleration, TRAJData.curvatureValueID, TRAJData.curvatureContentLength, + (double_t) TRAJData.curvature); + } + + // Convert from host endianness to little endian + TRAJData.relativeTimeValueID = htole16(TRAJData.relativeTimeValueID); + TRAJData.relativeTimeContentLength = htole16(TRAJData.relativeTimeContentLength); + TRAJData.relativeTime = htole32(TRAJData.relativeTime); + TRAJData.xPositionValueID = htole16(TRAJData.xPositionValueID); + TRAJData.xPositionContentLength = htole16(TRAJData.xPositionContentLength); + TRAJData.xPosition = (int32_t) htole32(TRAJData.xPosition); + TRAJData.yPositionValueID = htole16(TRAJData.yPositionValueID); + TRAJData.yPositionContentLength = htole16(TRAJData.yPositionContentLength); + TRAJData.yPosition = (int32_t) htole32(TRAJData.yPosition); + TRAJData.zPositionValueID = htole16(TRAJData.zPositionValueID); + TRAJData.zPositionContentLength = htole16(TRAJData.zPositionContentLength); + TRAJData.zPosition = (int32_t) htole32(TRAJData.zPosition); + TRAJData.headingValueID = htole16(TRAJData.headingValueID); + TRAJData.headingContentLength = htole16(TRAJData.headingContentLength); + TRAJData.heading = htole16(TRAJData.heading); + TRAJData.longitudinalSpeedValueID = htole16(TRAJData.longitudinalSpeedValueID); + TRAJData.longitudinalSpeedContentLength = htole16(TRAJData.longitudinalSpeedContentLength); + TRAJData.longitudinalSpeed = (int16_t) htole16(TRAJData.longitudinalSpeed); + TRAJData.lateralSpeedValueID = htole16(TRAJData.lateralSpeedValueID); + TRAJData.lateralSpeedContentLength = htole16(TRAJData.lateralSpeedContentLength); + TRAJData.lateralSpeed = (int16_t) htole16(TRAJData.lateralSpeed); + TRAJData.longitudinalAccelerationValueID = htole16(TRAJData.longitudinalAccelerationValueID); + TRAJData.longitudinalAccelerationContentLength = htole16(TRAJData.longitudinalAccelerationContentLength); + TRAJData.longitudinalAcceleration = (int16_t) htole16(TRAJData.longitudinalAcceleration); + TRAJData.lateralAccelerationValueID = htole16(TRAJData.lateralAccelerationValueID); + TRAJData.lateralAccelerationContentLength = htole16(TRAJData.lateralAccelerationContentLength); + TRAJData.lateralAcceleration = (int16_t) htole16(TRAJData.lateralAcceleration); + TRAJData.curvatureValueID = htole16(TRAJData.curvatureValueID); + TRAJData.curvatureContentLength = htole16(TRAJData.curvatureContentLength); + TRAJData.curvature = htolef(TRAJData.curvature); + + memcpy(trajDataBufferPointer, &TRAJData, sizeof (TRAJData)); + + return sizeof (TRAJPointType); +} + + +ssize_t encodeTRAJMessageFooter(char * trajDataBuffer, const char * trajDataBufferStart, const size_t remainingBufferLength, const char debug) { + + ssize_t messageSize; + TRAJFooterType TRAJData; + + if (trajDataBufferStart == NULL) { + messageSize = -1; + } + else { + messageSize = trajDataBuffer - trajDataBufferStart; + + if (messageSize < (ssize_t) sizeof (TRAJHeaderType)) { + errno = EINVAL; + LogMessage(LOG_LEVEL_ERROR, "TRAJ message too short to contain header"); + return -1; + } + } + + if (remainingBufferLength < sizeof (TRAJFooterType)) { + errno = ENOBUFS; + LogMessage(LOG_LEVEL_ERROR, "Buffer too small to hold TRAJ footer data"); + return -1; + } + else if (trajDataBuffer == NULL) { + errno = EINVAL; + LogMessage(LOG_LEVEL_ERROR, "Invalid trajectory data buffer supplied"); + return -1; + } + + if (messageSize > 0) { + TRAJData.footer = buildISOFooter(trajDataBufferStart, (size_t) messageSize, debug); + } + else { + TRAJData.footer.Crc = 0; + } + + memcpy(trajDataBuffer, &TRAJData, sizeof (TRAJData)); + + return sizeof (TRAJFooterType); +} + + /*! * \brief encodeOSEMMessage Creates an OSEM message and writes it into a buffer based on supplied values. All values are passed as pointers and * passing them as NULL causes the OSEM message to contain a default value for that field (a value representing "unavailable" or similar). @@ -579,7 +948,7 @@ ISOMessageID getISOMessageType(const char *messageData, const size_t length, con * \param debug Flag for enabling debugging * \return Number of bytes written to the buffer, or -1 in case of an error */ -ssize_t encodeOSEMMessage(const double *latitude_deg, const double *longitude_deg, const float *altitude_m, +ssize_t encodeOSEMMessage(const double_t *latitude_deg, const double_t *longitude_deg, const float *altitude_m, const float *maxWayDeviation_m, const float *maxLateralDeviation_m, const float *minimumPositioningAccuracy_m, char *osemDataBuffer, const size_t bufferLength, const char debug) { @@ -1608,10 +1977,34 @@ ssize_t encodeINSUPMessage(const SupervisorCommandType command, char *insupDataB * \param isoHeading_rad Heading measured according to ISO specification, in radians * \return Heading, in radians, measured from the test x axis */ -double mapISOHeadingToHostHeading(const double isoHeading_rad) { +double_t mapISOHeadingToHostHeading(const double_t isoHeading_rad) { + // TODO: Reevaluate this when ISO specification is updated with new heading and rotated coordinate system + + double_t retval = isoHeading_rad; + + // Host heading is CCW while ISO is CW + retval = -retval; + // Host heading is measured from the x axis while ISO is measured from the y axis + retval = retval + M_PI / 2.0; + // Ensure angle lies between 0 and 2pi + while (retval < 0.0) { + retval += 2.0 * M_PI; + } + while (retval >= 2.0 * M_PI) { + retval -= 2.0 * M_PI; + } + return retval; +} + +/*! + * \brief mapHostHeadingToISOHeading Converts between internal heading measured from the test x axis to ISO NED heading + * \param isoHeading_rad Heading measured form test x axis, in radians + * \return Heading, in radians, measured as specified by ISO 22133 + */ +double_t mapHostHeadingToISOHeading(const double_t hostHeading_rad) { // TODO: Reevaluate this when ISO specification is updated with new heading and rotated coordinate system - double retval = isoHeading_rad; + double_t retval = hostHeading_rad; // Host heading is CCW while ISO is CW retval = -retval; From a8fa50041b5c40bbb0767be795b19e3221a70158 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 12 Feb 2020 09:21:16 +0100 Subject: [PATCH 241/523] Made footer builder function not take start of message buffer as input --- server/inc/iso22133.h | 2 +- server/src/iso22133.c | 28 +++++----------------------- 2 files changed, 6 insertions(+), 24 deletions(-) diff --git a/server/inc/iso22133.h b/server/inc/iso22133.h index 702ab4d44..4ff9f626b 100644 --- a/server/inc/iso22133.h +++ b/server/inc/iso22133.h @@ -219,7 +219,7 @@ typedef enum { ISOMessageReturnValue decodeMONRMessage(const char * monrDataBuffer, const size_t bufferLength, uint32_t * objectID, ObjectMonitorType * MonitorData, const char debug); ssize_t encodeTRAJMessageHeader(const uint16_t trajectoryID, const uint16_t trajectoryVersion, const char * trajectoryName, const size_t nameLength, const uint32_t numberOfPointsInTraj, char * trajDataBuffer, const size_t bufferLength, const char debug); ssize_t encodeTRAJMessagePoint(const struct timeval * pointTimeFromStart, const CartesianPosition position, const SpeedType speed, const AccelerationType acceleration, const float curvature, char * trajDataBufferPointer, const size_t remainingBufferLength, const char debug); -ssize_t encodeTRAJMessageFooter(char * trajDataBuffer, const char * trajDataBufferStart, const size_t bufferLength, const char debug); +ssize_t encodeTRAJMessageFooter(char * trajDataBuffer, const size_t bufferLength, const char debug); ssize_t encodeSTRTMessage(const struct timeval* timeOfStart, char * strtDataBuffer, const size_t bufferLength, const char debug); ssize_t encodeOSEMMessage(const double_t * latitude_deg, const double_t * longitude_deg, const float * altitude_m, const float * maxPositionDeviation_m, const float * maxLateralDeviation_m, const float * minimumPositioningAccuracy_m, char * osemDataBuffer, const size_t bufferLength, const char debug); ssize_t encodeOSTMMessage(const ObjectCommandType command, char * ostmDataBuffer, const size_t bufferLength, const char debug); diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 6eb1f9096..8c77d90d0 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -733,7 +733,7 @@ ssize_t encodeTRAJMessagePoint(const struct timeval * pointTimeFromStart, const if (remainingBufferLength < sizeof (TRAJPointType)) { errno = ENOBUFS; - LogMessage(LOG_LEVEL_ERROR, "Buffer too small to hold necessary TRAJ point data"); + LogMessage(LOG_LEVEL_DEBUG, "Buffer too small to hold necessary TRAJ point data"); return -1; } else if (trajDataBufferPointer == NULL) { @@ -892,27 +892,13 @@ ssize_t encodeTRAJMessagePoint(const struct timeval * pointTimeFromStart, const } -ssize_t encodeTRAJMessageFooter(char * trajDataBuffer, const char * trajDataBufferStart, const size_t remainingBufferLength, const char debug) { +ssize_t encodeTRAJMessageFooter(char * trajDataBuffer, const size_t remainingBufferLength, const char debug) { - ssize_t messageSize; TRAJFooterType TRAJData; - if (trajDataBufferStart == NULL) { - messageSize = -1; - } - else { - messageSize = trajDataBuffer - trajDataBufferStart; - - if (messageSize < (ssize_t) sizeof (TRAJHeaderType)) { - errno = EINVAL; - LogMessage(LOG_LEVEL_ERROR, "TRAJ message too short to contain header"); - return -1; - } - } - if (remainingBufferLength < sizeof (TRAJFooterType)) { errno = ENOBUFS; - LogMessage(LOG_LEVEL_ERROR, "Buffer too small to hold TRAJ footer data"); + LogMessage(LOG_LEVEL_DEBUG, "Buffer too small to hold TRAJ footer data"); return -1; } else if (trajDataBuffer == NULL) { @@ -921,12 +907,8 @@ ssize_t encodeTRAJMessageFooter(char * trajDataBuffer, const char * trajDataBuff return -1; } - if (messageSize > 0) { - TRAJData.footer = buildISOFooter(trajDataBufferStart, (size_t) messageSize, debug); - } - else { - TRAJData.footer.Crc = 0; - } + // TODO TRAJData.footer = real CRC + TRAJData.footer.Crc = 0; memcpy(trajDataBuffer, &TRAJData, sizeof (TRAJData)); From 97758d758b1bff6aebd1bdf4f70e894a5f853ff3 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 12 Feb 2020 09:23:26 +0100 Subject: [PATCH 242/523] Removed old TRAJ builder functions from object control --- server/inc/objectcontrol.h | 3 -- server/src/objectcontrol.c | 98 +------------------------------------- 2 files changed, 2 insertions(+), 99 deletions(-) diff --git a/server/inc/objectcontrol.h b/server/inc/objectcontrol.h index 51ab97491..fcd616b58 100644 --- a/server/inc/objectcontrol.h +++ b/server/inc/objectcontrol.h @@ -22,8 +22,5 @@ void objectcontrol_task(TimeType *GPSTime, GSDType *GSD, LOG_LEVEL logLevel); int ObjectControlBuildLLCMMessage(char* MessageBuffer, unsigned short Speed, unsigned short Curvature, unsigned char Mode, char debug); -I32 ObjectControlBuildTRAJMessageHeader(C8* MessageBuffer, I32 *RowCount, HeaderType *HeaderData, TRAJInfoType *TRAJInfoData, C8 *TrajFileHeader, U8 debug); -I32 ObjectControlBuildTRAJMessage(C8* MessageBuffer, FILE *fd, I32 RowCount, DOTMType *DOTMType, U8 debug); I32 ObjectControlBuildVOILMessage(C8* MessageBuffer, VOILType *VOILData, C8* SimData, U8 debug); -I32 ObjectControlSendTRAJMessage(C8* Filename, I32 *Socket, I32 RowCount, C8 *IP, U32 Port, DOTMType *DOTMData, U8 debug); #endif //__OBJECTCONTROL_H_INCLUDED__ diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 3c158b4c3..bd6986708 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -47,7 +47,6 @@ #define RECV_MESSAGE_BUFFER 6200 #define BUFFER_SIZE_3100 3100 #define TRAJ_FILE_HEADER_ROW 256 -#define OBJECT_MESS_BUFFER_SIZE 1024 #define OC_SLEEP_TIME_EMPTY_MQ_S 0 #define OC_SLEEP_TIME_EMPTY_MQ_NS 1000000 @@ -129,19 +128,13 @@ static void signalHandler(int signo); int ObjectControlBuildLLCMMessage(char *MessageBuffer, unsigned short Speed, unsigned short Curvature, unsigned char Mode, char debug); -I32 ObjectControlBuildTRAJMessageHeader(C8 * MessageBuffer, I32 * RowCount, HeaderType * HeaderData, - TRAJInfoType * TRAJInfoData, C8 * TrajFileHeader, U8 debug); -I32 ObjectControlBuildTRAJMessage(C8 * MessageBuffer, FILE * fd, I32 RowCount, DOTMType * DOTMData, U8 debug); -I32 ObjectControlSendTRAJMessage(C8 * Filename, I32 * Socket, I32 RowCount, C8 * IP, U32 Port, - DOTMType * DOTMData, U8 debug); +static ssize_t ObjectControlSendTRAJMessage(const char * Filename, int * Socket, const char debug); int ObjectControlSendUDPData(int *sockfd, struct sockaddr_in *addr, char *SendData, int Length, char debug); I32 ObjectControlBuildVOILMessage(C8 * MessageBuffer, VOILType * VOILData, C8 * SimData, U8 debug); I32 ObjectControlSendDTMMessage(C8 * DTMData, I32 * Socket, I32 RowCount, C8 * IP, U32 Port, DOTMType * DOTMData, U8 debug); I32 ObjectControlBuildDTMMessage(C8 * MessageBuffer, C8 * DTMData, I32 RowCount, DOTMType * DOTMData, U8 debug); -I32 ObjectControlBuildASPMessage(C8 * MessageBuffer, ASPType * ASPData, U8 debug); -I32 ObjectControlSendEXACMessage(EXACData * EXAC, I32 * socket, U8 debug); static int iFindObjectsInfo(C8 object_traj_file[MAX_OBJECTS][MAX_FILE_PATH], C8 object_address_name[MAX_OBJECTS][MAX_FILE_PATH], @@ -1202,94 +1195,7 @@ int ObjectControlBuildLLCMMessage(char *MessageBuffer, unsigned short Speed, uns -I32 ObjectControlBuildTRAJMessageHeader(C8 * MessageBuffer, I32 * RowCount, HeaderType * HeaderData, - TRAJInfoType * TRAJInfoData, C8 * TrajFileHeader, U8 debug) { - I32 MessageIndex = 0, i, j; - U16 Crc = 0; - C8 *p; - C8 *token; - - - if (strlen(TrajFileHeader) >= 1) { - j = 0; - token = strtok(TrajFileHeader, ";"); - while (token != NULL) { - if (j == 1) { - TRAJInfoData->TrajectoryIDValueIdU16 = VALUE_ID_TRAJECTORY_ID; - TRAJInfoData->TrajectoryIDContentLengthU16 = 2; - TRAJInfoData->TrajectoryIDU16 = atoi(token); - } - else if (j == 2) { - TRAJInfoData->TrajectoryNameValueIdU16 = VALUE_ID_TRAJECTORY_NAME; - TRAJInfoData->TrajectoryNameContentLengthU16 = 64; - bzero(TRAJInfoData->TrajectoryNameC8, 64); - strncpy(TRAJInfoData->TrajectoryNameC8, token, strlen(token)); - } - else if (j == 3) { - TRAJInfoData->TrajectoryVersionValueIdU16 = VALUE_ID_TRAJECTORY_VERSION; - TRAJInfoData->TrajectoryVersionContentLengthU16 = 2; - TRAJInfoData->TrajectoryVersionU16 = atoi(token); - } - else if (j == 4) { - *RowCount = atoi(token); - } - - j++; - token = strtok(NULL, ";"); - } - } - - TRAJInfoData->IpAddressValueIdU16 = 0xA000; - TRAJInfoData->IpAddressContentLengthU16 = 4; - TRAJInfoData->IpAddressU32 = 0; - - bzero(MessageBuffer, COMMAND_MESSAGE_HEADER_LENGTH + COMMAND_TRAJ_INFO_ROW_MESSAGE_LENGTH); - - HeaderData->SyncWordU16 = ISO_SYNC_WORD; - HeaderData->TransmitterIdU8 = 0; - HeaderData->MessageCounterU8 = 0; - HeaderData->AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; - HeaderData->MessageIdU16 = COMMAND_DOTM_CODE; - HeaderData->MessageLengthU32 = - *RowCount * COMMAND_DOTM_ROW_MESSAGE_LENGTH + COMMAND_TRAJ_INFO_ROW_MESSAGE_LENGTH; - - p = (C8 *) HeaderData; - for (i = 0; i < COMMAND_MESSAGE_HEADER_LENGTH; i++) - *(MessageBuffer + i) = *p++; - - p = (C8 *) TRAJInfoData; - for (; i < COMMAND_MESSAGE_HEADER_LENGTH + COMMAND_TRAJ_INFO_ROW_MESSAGE_LENGTH; i++) - *(MessageBuffer + i) = *p++; - - MessageIndex = i; - - - if (debug) { - // TODO: Change to log printout when byte thingy has been implemented - printf("Header + TRAJInfo total length = %d bytes\n", - (int)(COMMAND_MESSAGE_HEADER_LENGTH + COMMAND_TRAJ_INFO_ROW_MESSAGE_LENGTH)); - printf("----HEADER + TRAJInfo----\n"); - for (i = 0; i < sizeof (HeaderType) + sizeof (TRAJInfoType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n"); - printf("DOTM message total length = %d bytes.\n", (int)HeaderData->MessageLengthU32); - printf("Traj file header = %s\n", TrajFileHeader); - printf("TrajectoryID = %d\n", TRAJInfoData->TrajectoryIDU16); - printf("TrajectoryName = %s\n", TRAJInfoData->TrajectoryNameC8); - printf("TrajectoryVersion = %d\n", TRAJInfoData->TrajectoryVersionU16); - printf("RowCount = %d\n", *RowCount); - printf("IpAddress = %d\n", TRAJInfoData->IpAddressU32); - - printf("\n----MESSAGE----\n"); - } - - return MessageIndex; //Total number of bytes = COMMAND_MESSAGE_HEADER_LENGTH -} - - - -I32 ObjectControlSendTRAJMessage(C8 * Filename, I32 * Socket, I32 RowCount, C8 * IP, U32 Port, - DOTMType * DOTMData, U8 debug) { +ssize_t ObjectControlSendTRAJMessage(const char * Filename, int * Socket, const char debug) { FILE *fd; // Save socket settings and set it to blocking From 9fb9b94057e4a4ac61d143c118fdac979a9a75f9 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 12 Feb 2020 09:24:24 +0100 Subject: [PATCH 243/523] Made trajectory file parser functions in util not static --- server/inc/util.h | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/server/inc/util.h b/server/inc/util.h index 6496d3884..29aac84cc 100644 --- a/server/inc/util.h +++ b/server/inc/util.h @@ -315,6 +315,27 @@ typedef struct } TRAJInfoType; +typedef struct { + unsigned int ID; + char name[128]; + unsigned short majorVersion; + unsigned short minorVersion; + unsigned int numberOfLines; +} TrajectoryFileHeader; + +typedef struct { + double time; + double xCoord; + double yCoord; + double *zCoord; + double heading; + double *longitudinalVelocity; + double *lateralVelocity; + double *longitudinalAcceleration; + double *lateralAcceleration; + double curvature; + uint8_t mode; +} TrajectoryFileLine; typedef struct { @@ -755,6 +776,9 @@ I32 UtilPopulateACCMDataStructFromMQ(C8* rawACCM, size_t rawACCMsize, ACCMData * double UtilGetDistance(double lat1, double lon1, double lat2, double lon2); +int UtilParseTrajectoryFileHeader(char *headerLine, TrajectoryFileHeader * header); +int UtilParseTrajectoryFileFooter(char *footerLine); +int UtilParseTrajectoryFileLine(char *fileLine, TrajectoryFileLine * line); typedef struct { uint64_t timestamp; From 92a01162307e12ab0ff7d863cfce6aceb16a721e Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 12 Feb 2020 09:24:47 +0100 Subject: [PATCH 244/523] Missed committing something --- server/src/util.c | 27 +-------------------------- 1 file changed, 1 insertion(+), 26 deletions(-) diff --git a/server/src/util.c b/server/src/util.c index d217e5804..cda713fbc 100644 --- a/server/src/util.c +++ b/server/src/util.c @@ -95,27 +95,6 @@ /*------------------------------------------------------------ -- Local type definitions ------------------------------------------------------------*/ -typedef struct { - unsigned int ID; - char name[SMALL_BUFFER_SIZE_128]; - unsigned short majorVersion; - unsigned short minorVersion; - unsigned int numberOfLines; -} TrajectoryFileHeader; - -typedef struct { - double time; - double xCoord; - double yCoord; - double *zCoord; - double heading; - double *longitudinalVelocity; - double *lateralVelocity; - double *longitudinalAcceleration; - double *lateralAcceleration; - double curvature; - uint8_t mode; -} TrajectoryFileLine; /*------------------------------------------------------------ -- Public variables @@ -136,11 +115,6 @@ static char rayFromPointIntersectsLine(double pointX, double pointY, double poly double polyPointBX, double polyPointBY); -static int UtilParseTrajectoryFileHeader(char *headerLine, TrajectoryFileHeader * header); -static int UtilParseTrajectoryFileFooter(char *footerLine); -static int UtilParseTrajectoryFileLine(char *fileLine, TrajectoryFileLine * line); - - void CopyHTTPHeaderField(char *request, char *targetContainer, size_t targetContainerSize, const char *fieldName) { char *firstPos, *lastPos; @@ -2474,6 +2448,7 @@ int UtilParseTrajectoryFileLine(char *line, TrajectoryFileLine * fileLine) { free(fileLine->longitudinalVelocity); if (fileLine->lateralAcceleration) free(fileLine->longitudinalAcceleration); + memset(fileLine, 0, sizeof (*fileLine)); // strtok() does not handle double delimiters well, more complicated parsing necessary From 5bd996f7d5f8af478f858542ec6fe77f7d062e67 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 12 Feb 2020 09:26:02 +0100 Subject: [PATCH 245/523] Started using traj builder functions from iso22133 --- server/src/objectcontrol.c | 404 +++++++++++++------------------------ 1 file changed, 143 insertions(+), 261 deletions(-) diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index bd6986708..a12b2f3b8 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -65,7 +65,8 @@ #define COMMAND_CODE_INDEX 0 #define COMMAND_MESSAGE_LENGTH_INDEX 1 -#define COMMAND_DOTM_CODE 1 +#define TRAJECTORY_TX_BUFFER_SIZE 2048 + #define COMMAND_DOTM_ROW_MESSAGE_LENGTH sizeof(DOTMType) #define COMMAND_TRAJ_INFO_ROW_MESSAGE_LENGTH sizeof(TRAJInfoType) #define COMMAND_DOTM_ROWS_IN_TRANSMISSION 40 @@ -811,51 +812,19 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { vSetState(OBC_STATE_ERROR, GSD); objectControlServerStatus = CONTROL_CENTER_STATUS_ABORT; } + LogPrint("OSEM msglen: %ld", MessageLength); UtilSendTCPData("Object Control", MessageBuffer, MessageLength, &socket_fds[iIndex], - 0); + 1); - /*Here we send TRAJ, if the IP-address not is found */ + /* Here we send TRAJ, if the IP-address is not operating with a dynamic trajectory */ if (strstr(DTMReceivers, object_address_name[iIndex]) == NULL) { - - fd = fopen(object_traj_file[iIndex], "r"); - - if (fd != NULL) { - //RowCount = UtilCountFileRows(fd); - //printf("RowCount: %d\n", RowCount); - //fclose(fd); - - //fd = fopen(object_traj_file[iIndex], "r"); - //printf("Open file: %s\n", object_traj_file[iIndex]); - UtilReadLineCntSpecChars(fd, FileHeaderBufferC8); - fclose(fd); - - /*TRAJ*/ - MessageLength = ObjectControlBuildTRAJMessageHeader(TrajBuffer, - &RowCount, - &HeaderData, - &TRAJInfoData, - FileHeaderBufferC8, - 0); - - //printf("RowCount: %d\n", RowCount); - - /*Send TRAJ header */ - UtilSendTCPData("Object Control", TrajBuffer, MessageLength, - &socket_fds[iIndex], 0); - - /*Send TRAJ data */ - ObjectControlSendTRAJMessage(object_traj_file[iIndex], &socket_fds[iIndex], - RowCount, - (char *)&object_address_name[iIndex], - object_tcp_port[iIndex], &DOTMData, 0); - + LogMessage(LOG_LEVEL_INFO, "Sending TRAJ to %s", object_address_name[iIndex]); + if (ObjectControlSendTRAJMessage(object_traj_file[iIndex], &socket_fds[iIndex], 0) == -1) { + LogMessage(LOG_LEVEL_ERROR, "Failed to send TRAJ to %s", object_address_name[iIndex]); + continue; } - else - LogMessage(LOG_LEVEL_WARNING, "Could not open file <%s>", - object_traj_file[iIndex]); } - /* Adaptive Sync Points object configuration start... */ if (TEST_SYNC_POINTS == 1) printf("Trajfile: %s\n", object_traj_file[iIndex]); @@ -1197,6 +1166,25 @@ int ObjectControlBuildLLCMMessage(char *MessageBuffer, unsigned short Speed, uns ssize_t ObjectControlSendTRAJMessage(const char * Filename, int * Socket, const char debug) { FILE *fd; + char* line; + size_t len = 0; + ssize_t read; + ssize_t printedBytes = 0, totalPrintedBytes = 0; + int socketOptions; + TrajectoryFileHeader fileHeader; + TrajectoryFileLine fileLine; + char messageBuffer[TRAJECTORY_TX_BUFFER_SIZE]; + size_t remainingBufferSpace = sizeof (messageBuffer); + char* messageBufferPosition = messageBuffer; + + memset(&fileHeader, 0, sizeof (fileHeader)); + memset(&fileLine, 0, sizeof (fileLine)); + + // Check file format before doing anything + if (UtilCheckTrajectoryFileFormat(Filename, strlen(Filename)) == -1) { + LogMessage(LOG_LEVEL_ERROR, "Incorrect trajectory file format - cannot proceed to send message"); + return -1; + } // Save socket settings and set it to blocking int retval = fcntl(*Socket, F_GETFL); @@ -1205,7 +1193,7 @@ ssize_t ObjectControlSendTRAJMessage(const char * Filename, int * Socket, const LogMessage(LOG_LEVEL_ERROR, "Error getting socket options with fcntl"); return -1; } - int socketOptions = retval; + socketOptions = retval; retval = fcntl(*Socket, F_SETFL, socketOptions & ~O_NONBLOCK); if (retval < 0) { @@ -1213,53 +1201,135 @@ ssize_t ObjectControlSendTRAJMessage(const char * Filename, int * Socket, const return -1; } - + // Open the file and parse header fd = fopen(Filename, "r"); if (fd == NULL) { LogMessage(LOG_LEVEL_ERROR, "Unable to open file <%s>", Filename); return -1; } - UtilReadLineCntSpecChars(fd, TrajBuffer); //Read first line - int Rest = 0, i = 0, MessageLength = 0, SumMessageLength = 0, Modulo = 0, Transmissions = 0; - - Transmissions = RowCount / COMMAND_DOTM_ROWS_IN_TRANSMISSION; - Rest = RowCount % COMMAND_DOTM_ROWS_IN_TRANSMISSION; - U16 CrcU16 = 0; + read = getline(&line, &len, fd); + if (read == -1 || (retval = UtilParseTrajectoryFileHeader(line, &fileHeader)) == -1) { + LogMessage(LOG_LEVEL_ERROR, "Failed to parse header of file <%s>", Filename); + fclose(fd); + return -1; + } + // Generate ISO trajectory message header + if ((printedBytes = encodeTRAJMessageHeader(fileHeader.ID, fileHeader.majorVersion, + fileHeader.name, strlen(fileHeader.name), + fileHeader.numberOfLines, messageBufferPosition, + remainingBufferSpace, debug)) == -1) { + LogMessage(LOG_LEVEL_ERROR, "Unable to encode trajectory message"); + fclose(fd); + return -1; + } - for (i = 0; i < Transmissions; i++) { - MessageLength = - ObjectControlBuildTRAJMessage(TrajBuffer, fd, COMMAND_DOTM_ROWS_IN_TRANSMISSION, DOTMData, debug); + totalPrintedBytes += printedBytes; + messageBufferPosition += printedBytes; + remainingBufferSpace -= (size_t) printedBytes; + + read = getline(&line, &len, fd); + for (unsigned int i = 0; i < fileHeader.numberOfLines && read != -1; + ++i, read = getline(&line, &len, fd)) { + + // Parse file line + struct timeval relTime; + CartesianPosition position; + SpeedType speed; + AccelerationType acceleration; + + if (UtilParseTrajectoryFileLine(line, &fileLine) == -1) { + // TODO: how to terminate an ISO message when an error has occurred? + LogMessage(LOG_LEVEL_ERROR, "Unable to parse line %u of trajectory file <%s>", i+1, Filename); + fclose(fd); + return -1; + } - if (i == Transmissions && Rest == 0) { - TrajBuffer[MessageLength] = (U8) (CrcU16); - TrajBuffer[MessageLength + 1] = (U8) (CrcU16 >> 8); - MessageLength = MessageLength + 2; - UtilSendTCPData("Object Control", TrajBuffer, MessageLength, Socket, 0); - SumMessageLength = SumMessageLength + MessageLength; + relTime.tv_sec = (time_t) fileLine.time; + relTime.tv_usec = (time_t) ((fileLine.time - relTime.tv_sec) * 1000000); + position.xCoord_m = fileLine.xCoord; + position.yCoord_m = fileLine.yCoord; + position.isPositionValid = fileLine.zCoord != NULL; + position.zCoord_m = position.isPositionValid ? *fileLine.zCoord : 0; + position.heading_rad = fileLine.heading; + position.isHeadingValid = true; + speed.isValid = fileLine.longitudinalVelocity != NULL && fileLine.lateralVelocity != NULL; + speed.longitudinal_m_s = fileLine.longitudinalVelocity != NULL ? *fileLine.longitudinalVelocity : 0; + speed.lateral_m_s = fileLine.lateralVelocity != NULL ? *fileLine.lateralVelocity : 0; + acceleration.isValid = fileLine.longitudinalAcceleration != NULL && fileLine.lateralAcceleration != NULL; + acceleration.longitudinal_m_s2 = fileLine.longitudinalAcceleration != NULL ? *fileLine.longitudinalAcceleration : 0; + acceleration.lateral_m_s2 = fileLine.lateralAcceleration != NULL ? *fileLine.lateralAcceleration : 0; + + // Print to buffer + if ((printedBytes = encodeTRAJMessagePoint(&relTime, position, speed, acceleration, + fileLine.curvature, messageBufferPosition, + remainingBufferSpace, debug)) == -1) { + + if (errno == ENOBUFS) { + // Reached the end of buffer, send buffered data and + // try again + UtilSendTCPData(MODULE_NAME, messageBuffer, messageBufferPosition - messageBuffer, Socket, 1); + + messageBufferPosition = messageBuffer; + remainingBufferSpace = sizeof (messageBuffer); + if ((printedBytes = encodeTRAJMessagePoint(&relTime, position, speed, acceleration, fileLine.curvature, + messageBufferPosition, remainingBufferSpace, debug)) == -1) { + // TODO how to terminate an ISO message when an error has occurred? + LogMessage(LOG_LEVEL_ERROR, "Error encoding trajectory message point"); + fclose(fd); + return -1; + } + messageBufferPosition += printedBytes; + totalPrintedBytes += printedBytes; + remainingBufferSpace -= (size_t) printedBytes; + } + else { + // TODO how to terminate an ISO message when an error has occurred? + LogMessage(LOG_LEVEL_ERROR, "Error encoding trajectory message point"); + fclose(fd); + return -1; + } } else { - UtilSendTCPData("Object Control", TrajBuffer, MessageLength, Socket, 0); - SumMessageLength = SumMessageLength + MessageLength; + totalPrintedBytes += printedBytes; + messageBufferPosition += printedBytes; + remainingBufferSpace -= (size_t) printedBytes; } - - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "Transmission %d: %d bytes sent", i, MessageLength); } - if (Rest > 0) { - MessageLength = ObjectControlBuildTRAJMessage(TrajBuffer, fd, Rest, DOTMData, debug); - TrajBuffer[MessageLength] = (U8) (CrcU16); - TrajBuffer[MessageLength + 1] = (U8) (CrcU16 >> 8); - MessageLength = MessageLength + 2; - UtilSendTCPData("Object Control", TrajBuffer, MessageLength, Socket, 0); - SumMessageLength = SumMessageLength + MessageLength; - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "Transmission %d: %d bytes sent.\n", i, MessageLength); + // Trajectory message footer + if ((printedBytes = encodeTRAJMessageFooter(messageBufferPosition, remainingBufferSpace, debug)) == -1) { + if (errno == ENOBUFS) { + // Buffer was filled: send the data and try to encode it again + UtilSendTCPData(MODULE_NAME, messageBuffer, messageBufferPosition - messageBuffer, Socket, debug); + messageBufferPosition = messageBuffer; + remainingBufferSpace = sizeof (messageBuffer); + if ((printedBytes = encodeTRAJMessageFooter(messageBufferPosition, remainingBufferSpace, debug)) == -1) { + // TODO how to terminate an ISO message when an error has occurred? + LogMessage(LOG_LEVEL_ERROR, "Error encoding trajectory message footer"); + fclose(fd); + return -1; + } + messageBufferPosition += printedBytes; + totalPrintedBytes += printedBytes; + remainingBufferSpace -= (size_t) printedBytes; + } + else { + // TODO how to terminate an ISO message when an error has occurred? + LogMessage(LOG_LEVEL_ERROR, "Error encoding trajectory message footer"); + fclose(fd); + return -1; + } + } + else { + totalPrintedBytes += printedBytes; + messageBufferPosition += printedBytes; + remainingBufferSpace -= (size_t) printedBytes; } - LogMessage(LOG_LEVEL_INFO, "%d DOTM bytes sent to %s:%d", SumMessageLength, IP, Port); + UtilSendTCPData(MODULE_NAME, messageBuffer, messageBufferPosition - messageBuffer, Socket, debug); + fclose(fd); // Reset socket settings @@ -1269,195 +1339,7 @@ ssize_t ObjectControlSendTRAJMessage(const char * Filename, int * Socket, const return -1; } - - return 0; -} - -I32 ObjectControlBuildTRAJMessage(C8 * MessageBuffer, FILE * fd, I32 RowCount, DOTMType * DOTMData, U8 debug) { - I32 MessageIndex = 0; - C8 RowBuffer[100]; - C8 DataBuffer[20]; - dbl Data; - C8 *src, *p; - U16 Crc = 0; - flt curv = 0; - C8 *pc; - - bzero(MessageBuffer, COMMAND_DOTM_ROW_MESSAGE_LENGTH * RowCount); - - I32 i = 0, j = 0, n = 0; - - for (i = 0; i < RowCount; i++) { - bzero(RowBuffer, 100); - UtilReadLineCntSpecChars(fd, RowBuffer); - - //Read to ';' in row = LINE;0.00;21.239000;39.045000;0.000000;-1.240001;0.029103;0.004005;0.000000;3;ENDLINE; - //Time - src = strchr(RowBuffer, ';'); - bzero(DataBuffer, 20); - strncpy(DataBuffer, src + 1, (U64) strchr(src + 1, ';') - (U64) src - 1); - Data = atof(DataBuffer) * 1e3; - DOTMData->RelativeTimeValueIdU16 = VALUE_ID_RELATIVE_TIME; - DOTMData->RelativeTimeContentLengthU16 = 4; - DOTMData->RelativeTimeU32 = (U32) Data; - if (debug) - printf("Time DataBuffer=%s float=%3.6f\n", DataBuffer, Data); - - //x - src = strchr(src + 1, ';'); - bzero(DataBuffer, 20); - strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); - Data = atof(DataBuffer) * 1e3; - DOTMData->XPositionValueIdU16 = VALUE_ID_X_POSITION; - DOTMData->XPositionContentLengthU16 = 4; - DOTMData->XPositionI32 = (I32) Data; - if (debug) - printf("X DataBuffer=%s float=%3.6f\n", DataBuffer, Data); - - //y - src = strchr(src + 1, ';'); - bzero(DataBuffer, 20); - strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); - Data = atof(DataBuffer) * 1e3; - DOTMData->YPositionValueIdU16 = VALUE_ID_Y_POSITION; - DOTMData->YPositionContentLengthU16 = 4; - DOTMData->YPositionI32 = (I32) Data; - if (debug) - printf("Y DataBuffer=%s float=%3.6f\n", DataBuffer, Data); - - //z - src = strchr(src + 1, ';'); - bzero(DataBuffer, 20); - strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); - Data = atof(DataBuffer) * 1e3; - DOTMData->ZPositionValueIdU16 = VALUE_ID_Z_POSITION; - DOTMData->ZPositionContentLengthU16 = 4; - DOTMData->ZPositionI32 = (I32) Data; - if (debug) - printf("Z DataBuffer=%s float=%3.6f\n", DataBuffer, Data); - - //Heading - src = strchr(src + 1, ';'); - bzero(DataBuffer, 20); - strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); - Data = UtilRadToDeg(atof(DataBuffer)); - Data = 450 - Data; //Turn heading back pi/2 - while (Data < 0) - Data += 360.0; - while (Data > 360) - Data -= 360.0; - DOTMData->HeadingValueIdU16 = VALUE_ID_HEADING; - DOTMData->HeadingContentLengthU16 = 2; - DOTMData->HeadingU16 = (U16) (Data * 1e2); - if (debug) - printf("Heading DataBuffer=%s float=%3.6f\n", DataBuffer, Data); - - //Longitudinal speed - src = strchr(src + 1, ';'); - bzero(DataBuffer, 20); - strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); - Data = atof(DataBuffer) * 1e2; - DOTMData->LongitudinalSpeedValueIdU16 = VALUE_ID_LONGITUDINAL_SPEED; - DOTMData->LongitudinalSpeedContentLengthU16 = 2; - DOTMData->LongitudinalSpeedI16 = (I16) Data; - if (debug) - printf("Long speed DataBuffer=%s float=%3.6f\n", DataBuffer, Data); - - //Lateral speed - src = strchr(src + 1, ';'); - bzero(DataBuffer, 20); - strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); - Data = atof(DataBuffer) * 1e2; - DOTMData->LateralSpeedValueIdU16 = VALUE_ID_LATERAL_SPEED; - DOTMData->LateralSpeedContentLengthU16 = 2; - DOTMData->LateralSpeedI16 = (I16) Data; - if (debug) - printf("Lat speed DataBuffer=%s float=%3.6f\n", DataBuffer, Data); - - //Longitudinal acceleration - src = strchr(src + 1, ';'); - bzero(DataBuffer, 20); - strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); - Data = atof(DataBuffer) * 1e3; - DOTMData->LongitudinalAccValueIdU16 = VALUE_ID_LONGITUDINAL_ACCELERATION; - DOTMData->LongitudinalAccContentLengthU16 = 2; - DOTMData->LongitudinalAccI16 = (I16) Data; - if (debug) - printf("Long acc DataBuffer=%s float=%3.6f\n", DataBuffer, Data); - - //Lateral acceleration - src = strchr(src + 1, ';'); - bzero(DataBuffer, 20); - strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); - Data = atof(DataBuffer) * 1e3; - DOTMData->LateralAccValueIdU16 = VALUE_ID_LATERAL_ACCELERATION; - DOTMData->LateralAccContentLengthU16 = 2; - DOTMData->LateralAccI16 = (I16) Data; - if (debug) - printf("Lat accDataBuffer=%s float=%3.6f\n", DataBuffer, Data); - - //Curvature - src = strchr(src + 1, ';'); - bzero(DataBuffer, 20); - strncpy(DataBuffer, src + 1, (uint64_t) strchr(src + 1, ';') - (uint64_t) src - 1); - //Data = atof(DataBuffer) * 3e4; - curv = atof(DataBuffer); - pc = (C8 *) & curv; - DOTMData->CurvatureValueIdU16 = VALUE_ID_CURVATURE; - DOTMData->CurvatureContentLengthU16 = 4; - //DOTMData->CurvatureI32 = (I32) Data; - DOTMData->CurvatureI32 = pc[0]; - DOTMData->CurvatureI32 = DOTMData->CurvatureI32 | ((I32) pc[1]) << 8; - DOTMData->CurvatureI32 = DOTMData->CurvatureI32 | ((I32) pc[2]) << 16; - DOTMData->CurvatureI32 = DOTMData->CurvatureI32 | ((I32) pc[3]) << 24; - - if (debug) - printf("Curv DataBuffer=%s float=%3.6f\n", DataBuffer, Data); - - p = (C8 *) DOTMData; - for (j = 0; j < sizeof (DOTMType); j++, n++) - *(MessageBuffer + n) = *p++; - MessageIndex = n; - } - - - if (debug) { - int i = 0; - - for (i = 0; i < MessageIndex; i++) { - // TODO: Write to log when bytes thingy has been implemented - if ((unsigned char)MessageBuffer[i] >= 0 && (unsigned char)MessageBuffer[i] <= 15) - printf("0"); - printf("%x-", (unsigned char)MessageBuffer[i]); - } - printf("\n"); - } - - return MessageIndex; //Total number of bytes - -} - - -I32 ObjectControlBuildASPMessage(C8 * MessageBuffer, ASPType * ASPData, U8 debug) { - I32 MessageIndex = 0, i; - C8 *p; - - memset(MessageBuffer, 0, sizeof (ASPType)); - p = (C8 *) ASPData; - for (i = 0; i < sizeof (ASPType); i++) - *(MessageBuffer + i) = *p++; - MessageIndex = i; - - if (debug) { - // TODO: Write to log when bytes thingy has been implemented - printf("ASP total length = %d bytes \n", (int)(sizeof (ASPType))); - printf("\n----MESSAGE----\n"); - for (i = 0; i < sizeof (ASPType); i++) - printf("%x ", (C8) MessageBuffer[i]); - printf("\n"); - } - - return MessageIndex; //Total number of bytes + return totalPrintedBytes; } From 9eaaf1662d4dd145d9d688c40ae360e823ba1f30 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 12 Feb 2020 10:23:47 +0100 Subject: [PATCH 246/523] Added function descriptions and added a static variable for keeping track of CRC between TRAJ message parts --- server/src/iso22133.c | 63 ++++++++++++++++++++++++++++++++++++++----- 1 file changed, 56 insertions(+), 7 deletions(-) diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 8c77d90d0..5ae4cce8b 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -406,14 +406,14 @@ typedef struct { #if __BYTE_ORDER == __LITTLE_ENDIAN #define le48toh(x) (x) #define htole48(x) (x) +#define htolef(x) (x) +#else +#define le48toh(x) (le64toh(x) >> 16) +#define htole48(x) (htole64(x) >> 16) #define htolef_a(x) \ htole32((union { uint32_t i; float f; }){ .f = (x) }.i) #define htolef(x) \ ((union { uint32_t i; float f; }){ .i = htolef_a(x) }.f) -#else -#define le48toh(x) (le64toh(x) >> 16) -#define htole48(x) (htole64(x) >> 16) -#define htolef(x) (x) #endif // ************************** static function declarations ******************************************************** @@ -428,6 +428,9 @@ static double_t mapISOHeadingToHostHeading(const double_t isoHeading_rad); static double_t mapHostHeadingToISOHeading(const double_t hostHeading_rad); static void convertMONRToHostRepresentation(const MONRType * MONRData, ObjectMonitorType * monitorData); +// ************************** static variables ******************************************************************** +static uint16_t trajectoryMessageCrc = 0; + // ************************** function definitions **************************************************************** /*! @@ -642,6 +645,23 @@ ISOMessageID getISOMessageType(const char *messageData, const size_t length, con } } + +/*! + * \brief encodeTRAJMessageHeader Creates a TRAJ message header based on supplied values and resets + * an internal CRC to be used in the corresponding footer. The header is printed to a buffer. + * \param trajectoryID ID of the trajectory + * \param trajectoryVersion Version of the trajectory + * \param trajectoryName A string of maximum length 63 excluding the null terminator + * \param nameLength Length of the name string excluding the null terminator + * \param numberOfPointsInTraj Number of points in the subsequent trajectory + * \param trajDataBuffer Buffer to which TRAJ header is to be printed + * \param bufferLength Length of buffer to which TRAJ header is to be printed + * \param debug Flag for enabling debugging + * \return Number of bytes printed, or -1 in case of error with the following errnos: + * EINVAL if one of the input parameters are invalid + * ENOBUFS if supplied buffer is too small to hold header + * EMSGSIZE if trajectory name is too long + */ ssize_t encodeTRAJMessageHeader(const uint16_t trajectoryID, const uint16_t trajectoryVersion, const char * trajectoryName, const size_t nameLength, const uint32_t numberOfPointsInTraj, char *trajDataBuffer, @@ -667,7 +687,7 @@ ssize_t encodeTRAJMessageHeader(const uint16_t trajectoryID, const uint16_t traj LogMessage(LOG_LEVEL_ERROR, "Buffer too small to hold necessary TRAJ header data"); return -1; } - else if (nameLength > sizeof (TRAJData.trajectoryName)) { + else if (nameLength >= sizeof (TRAJData.trajectoryName)) { errno = EMSGSIZE; LogMessage(LOG_LEVEL_ERROR, "Trajectory name <%s> too long for TRAJ message", trajectoryName); return -1; @@ -720,11 +740,30 @@ ssize_t encodeTRAJMessageHeader(const uint16_t trajectoryID, const uint16_t traj TRAJData.trajectoryNameValueID = htole16(TRAJData.trajectoryNameValueID); TRAJData.trajectoryNameContentLength = htole16(TRAJData.trajectoryNameContentLength); + trajectoryMessageCrc = 0; + memcpy(trajDataBuffer, &TRAJData, sizeof (TRAJData)); + // TODO update CRC based on header data return sizeof (TRAJHeaderType); } + +/*! + * \brief encodeTRAJMessagePoint Creates a TRAJ message point based on supplied values and updates an internal + * CRC to be used in the footer. Also prints the TRAJ point to a buffer. + * \param pointTimeFromStart Time from start of the trajectory point + * \param position Position of the point + * \param speed Speed at the point + * \param acceleration Acceleration at the point + * \param curvature Curvature of the trajectory at the point + * \param trajDataBufferPointer Buffer to which the message is to be printed + * \param remainingBufferLength Remaining bytes in the buffer to which the message is to be printed + * \param debug Flag for enabling debugging + * \return Number of bytes printed, or -1 in case of error with the following errnos: + * EINVAL if one of the input parameters are invalid + * ENOBUFS if supplied buffer is too small to hold point + */ ssize_t encodeTRAJMessagePoint(const struct timeval * pointTimeFromStart, const CartesianPosition position, const SpeedType speed, const AccelerationType acceleration, const float curvature, char * trajDataBufferPointer, const size_t remainingBufferLength, const char debug) { @@ -888,10 +927,21 @@ ssize_t encodeTRAJMessagePoint(const struct timeval * pointTimeFromStart, const memcpy(trajDataBufferPointer, &TRAJData, sizeof (TRAJData)); + // TODO update CRC based on printed data return sizeof (TRAJPointType); } +/*! + * \brief encodeTRAJMessageFooter Creates a TRAJ message footer based on an internal CRC from previous header + * and points, and prints it to a buffer. + * \param trajDataBuffer Buffer to which TRAJ message is to be printed + * \param remainingBufferLength Remaining bytes in the buffer to which the message is to be printed + * \param debug Flag for enabling debugging + * \return Number of bytes printed, or -1 in case of error with the following errnos: + * EINVAL if one of the input parameters are invalid + * ENOBUFS if supplied buffer is too small to hold footer + */ ssize_t encodeTRAJMessageFooter(char * trajDataBuffer, const size_t remainingBufferLength, const char debug) { TRAJFooterType TRAJData; @@ -907,8 +957,7 @@ ssize_t encodeTRAJMessageFooter(char * trajDataBuffer, const size_t remainingBuf return -1; } - // TODO TRAJData.footer = real CRC - TRAJData.footer.Crc = 0; + TRAJData.footer.Crc = trajectoryMessageCrc; memcpy(trajDataBuffer, &TRAJData, sizeof (TRAJData)); From 16a02b8afe6c4f694250539d7268caa883e3871d Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 12 Feb 2020 10:40:42 +0100 Subject: [PATCH 247/523] Removed old unused stuff in util.h --- server/inc/util.h | 99 +--------------- server/src/util.c | 284 ---------------------------------------------- 2 files changed, 3 insertions(+), 380 deletions(-) diff --git a/server/inc/util.h b/server/inc/util.h index 29aac84cc..7a702c473 100644 --- a/server/inc/util.h +++ b/server/inc/util.h @@ -132,32 +132,6 @@ extern "C"{ #define STRUCT_CODE 254 #define RESERVED_CODE 255 -#define VALUE_ID_NOT_DEF 0 -#define VALUE_ID_RELATIVE_TIME 0x1 -#define VALUE_ID_GPS_SECOND_OF_WEEK 0x2 -#define VALUE_ID_GPS_WEEK 0x3 -#define VALUE_ID_DATE_ISO8601 0x4 -#define VALUE_ID_X_POSITION 0x10 -#define VALUE_ID_Y_POSITION 0x11 -#define VALUE_ID_Z_POSITION 0x12 -#define VALUE_ID_LATITUDE 0x20 -#define VALUE_ID_LONGITUDE 0x21 -#define VALUE_ID_ALTITUDE 0x22 -#define VALUE_ID_HEADING 0x30 -#define VALUE_ID_LONGITUDINAL_SPEED 0x40 -#define VALUE_ID_LATERAL_SPEED 0x41 -#define VALUE_ID_LONGITUDINAL_ACCELERATION 0x50 -#define VALUE_ID_LATERAL_ACCELERATION 0x51 -#define VALUE_ID_STATE_CHANGE_REQUEST 0x64 -#define VALUE_ID_MAX_WAY_DEVIATION 0x70 -#define VALUE_ID_MAX_LATERAL_DEVIATION 0x72 -#define VALUE_ID_MIN_POS_ACCURACY 0x74 -#define VALUE_ID_CURVATURE 0x52 -#define VALUE_ID_TRAJECTORY_ID 0x101 -#define VALUE_ID_TRAJECTORY_NAME 0x102 -#define VALUE_ID_TRAJECTORY_VERSION 0x103 -#define VALUE_ID_INSUP_MODE 0x200 - #define C8 uint8_t #define U8 uint8_t #define I8 int8_t @@ -196,19 +170,6 @@ extern "C"{ #define GetCurrentDir getcwd #define MAX_PATH_LENGTH 255 -#define ISO_MESSAGE_HEADER_LENGTH sizeof(HeaderType) - -#define ISO_TRAJ_CODE 1 -#define ISO_DTM_ROWS_IN_TRANSMISSION 40 -#define ISO_DTM_ROW_MESSAGE_LENGTH sizeof(DOTMType) - -#define ISO_TRAJ_INFO_ROW_MESSAGE_LENGTH sizeof(TRAJInfoType) -#define SIM_TRAJ_BYTES_IN_ROW 30 - - - -#define ISO_MESSAGE_FOOTER_LENGTH sizeof(FooterType) - #define DD_CONTROL_BUFFER_SIZE_1024 1024 #define DD_CONTROL_BUFFER_SIZE_20 20 #define DD_CONTROL_BUFFER_SIZE_52 52 @@ -263,57 +224,6 @@ typedef struct uint32_t ClientID; } MonitorDataType; -typedef struct -{ - U16 RelativeTimeValueIdU16; - U16 RelativeTimeContentLengthU16; - U32 RelativeTimeU32; - U16 XPositionValueIdU16; - U16 XPositionContentLengthU16; - I32 XPositionI32; - U16 YPositionValueIdU16; - U16 YPositionContentLengthU16; - I32 YPositionI32; - U16 ZPositionValueIdU16; - U16 ZPositionContentLengthU16; - I32 ZPositionI32; - U16 HeadingValueIdU16; - U16 HeadingContentLengthU16; - U16 HeadingU16; - U16 LongitudinalSpeedValueIdU16; - U16 LongitudinalSpeedContentLengthU16; - I16 LongitudinalSpeedI16; - U16 LateralSpeedValueIdU16; - U16 LateralSpeedContentLengthU16; - I16 LateralSpeedI16; - U16 LongitudinalAccValueIdU16; - U16 LongitudinalAccContentLengthU16; - I16 LongitudinalAccI16; - U16 LateralAccValueIdU16; - U16 LateralAccContentLengthU16; - I16 LateralAccI16; - U16 CurvatureValueIdU16; - U16 CurvatureContentLengthU16; - I32 CurvatureI32; -} DOTMType; //70 bytes - - -typedef struct -{ - U16 TrajectoryIDValueIdU16; - U16 TrajectoryIDContentLengthU16; - U16 TrajectoryIDU16; - U16 TrajectoryNameValueIdU16; - U16 TrajectoryNameContentLengthU16; - C8 TrajectoryNameC8[64]; - U16 TrajectoryVersionValueIdU16; - U16 TrajectoryVersionContentLengthU16; - U16 TrajectoryVersionU16; - U16 IpAddressValueIdU16; - U16 IpAddressContentLengthU16; - I32 IpAddressU32; - -} TRAJInfoType; typedef struct { unsigned int ID; @@ -702,6 +612,9 @@ void UtilGetGeofenceDirectoryPath(char* path, size_t pathLen); // File parsing functions int UtilCheckTrajectoryFileFormat(const char *path, size_t pathLen); +int UtilParseTrajectoryFileHeader(char *headerLine, TrajectoryFileHeader * header); +int UtilParseTrajectoryFileFooter(char *footerLine); +int UtilParseTrajectoryFileLine(char *fileLine, TrajectoryFileLine * line); int UtilMonitorDataToString(const MonitorDataType monrData, char* monrString, size_t stringLength); @@ -763,9 +676,6 @@ U32 UtilHexTextToBinary(U32 DataLength, C8 *Text, C8 *Binary, U8 Debug); U32 UtilCreateDirContent(C8* DirPath, C8* TempPath); U16 UtilGetMillisecond(TimeType *GPSTime); -I32 UtilISOBuildTRAJMessageHeader(C8* MessageBuffer, I32 RowCount, HeaderType *HeaderData, TRAJInfoType *TRAJInfoData, U8 Debug); -I32 UtilISOBuildTRAJMessage(C8 *MessageBuffer, C8 *DTMData, I32 RowCount, DOTMType *DOTMData, U8 debug); -I32 UtilISOBuildTRAJInfo(C8* MessageBuffer, TRAJInfoType *TRAJInfoData, U8 debug); I32 UtilWriteConfigurationParameter(C8 *ParameterName, C8 *NewValue, U8 Debug); int UtilPopulateMonitorDataStruct(const char * rawMONR, const size_t rawMONRsize, MonitorDataType *monitorData); @@ -776,9 +686,6 @@ I32 UtilPopulateACCMDataStructFromMQ(C8* rawACCM, size_t rawACCMsize, ACCMData * double UtilGetDistance(double lat1, double lon1, double lat2, double lon2); -int UtilParseTrajectoryFileHeader(char *headerLine, TrajectoryFileHeader * header); -int UtilParseTrajectoryFileFooter(char *footerLine); -int UtilParseTrajectoryFileLine(char *fileLine, TrajectoryFileLine * line); typedef struct { uint64_t timestamp; diff --git a/server/src/util.c b/server/src/util.c index cda713fbc..59409768b 100644 --- a/server/src/util.c +++ b/server/src/util.c @@ -3191,290 +3191,6 @@ U16 UtilGetMillisecond(TimeType * GPSTime) { return MilliU16; } -I32 UtilISOBuildTRAJInfo(C8 * MessageBuffer, TRAJInfoType * TRAJInfoData, U8 debug) { - I32 MessageIndex = 0, i; - - U16 Crc = 0, U16Data = 0; - I16 I16Data = 0; - U32 U32Data = 0; - I32 I32Data = 0; - U64 U64Data = 0; - C8 *p; - - U16Data = 0; - U16Data = (U16Data | *(MessageBuffer + 1)) << 8; - U16Data = U16Data | *MessageBuffer; - TRAJInfoData->TrajectoryIDValueIdU16 = U16Data; - U16Data = 0; - U16Data = (U16Data | *(MessageBuffer + 3)) << 8; - U16Data = U16Data | *(MessageBuffer + 2); - TRAJInfoData->TrajectoryIDContentLengthU16 = U16Data; - U16Data = 0; - U16Data = (U16Data | *(MessageBuffer + 5)) << 8; - U16Data = U16Data | *(MessageBuffer + 4); - TRAJInfoData->TrajectoryIDU16 = U16Data; - - U16Data = 0; - U16Data = (U16Data | *(MessageBuffer + 7)) << 8; - U16Data = U16Data | *(MessageBuffer + 6); - TRAJInfoData->TrajectoryNameValueIdU16 = U16Data; - U16Data = 0; - U16Data = (U16Data | *(MessageBuffer + 9)) << 8; - U16Data = U16Data | *(MessageBuffer + 8); - TRAJInfoData->TrajectoryNameContentLengthU16 = U16Data; - for (i = 0; i < TRAJInfoData->TrajectoryNameContentLengthU16; i++) - TRAJInfoData->TrajectoryNameC8[i] = *(MessageBuffer + 10 + i); - - U16Data = 0; - U16Data = (U16Data | *(MessageBuffer + 65)) << 8; - U16Data = U16Data | *(MessageBuffer + 64); - TRAJInfoData->TrajectoryVersionValueIdU16 = U16Data; - U16Data = 0; - U16Data = (U16Data | *(MessageBuffer + 67)) << 8; - U16Data = U16Data | *(MessageBuffer + 66); - TRAJInfoData->TrajectoryVersionContentLengthU16 = U16Data; - U16Data = 0; - U16Data = (U16Data | *(MessageBuffer + 69)) << 8; - U16Data = U16Data | *(MessageBuffer + 68); - TRAJInfoData->TrajectoryVersionU16 = U16Data; - - U16Data = 0; - U16Data = (U16Data | *(MessageBuffer + 71)) << 8; - U16Data = U16Data | *(MessageBuffer + 70); - TRAJInfoData->IpAddressValueIdU16 = U16Data; - U16Data = 0; - U16Data = (U16Data | *(MessageBuffer + 73)) << 8; - U16Data = U16Data | *(MessageBuffer + 72); - TRAJInfoData->IpAddressContentLengthU16 = U16Data; - U32Data = 0; - U32Data = (U32Data | *(MessageBuffer + 77)) << 8; - U32Data = (U32Data | *(MessageBuffer + 76)) << 8; - U32Data = (U32Data | *(MessageBuffer + 75)) << 8; - U32Data = U32Data | *(MessageBuffer + 74); - TRAJInfoData->IpAddressU32 = U32Data; - - if (debug) { - // TODO: Change this when bytes thingy has been implemented in logging - printf("TRAJInfo total length = %d bytes\n", (int)(ISO_TRAJ_INFO_ROW_MESSAGE_LENGTH)); - printf("----TRAJInfo----\n"); - for (i = 0; i < sizeof (TRAJInfoType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n"); - printf("TrajectoryID = %d\n", TRAJInfoData->TrajectoryIDU16); - printf("TrajectoryName = %s\n", TRAJInfoData->TrajectoryNameC8); - printf("TrajectoryVersion = %d\n", TRAJInfoData->TrajectoryVersionU16); - printf("IpAddress = %d\n", TRAJInfoData->IpAddressU32); - printf("\n----MESSAGE----\n"); - } - - return 0; -} - - -I32 UtilISOBuildTRAJMessageHeader(C8 * MessageBuffer, I32 RowCount, HeaderType * HeaderData, - TRAJInfoType * TRAJInfoData, U8 debug) { - I32 MessageIndex = 0, i; - U16 Crc = 0; - C8 *p; - - bzero(MessageBuffer, ISO_MESSAGE_HEADER_LENGTH + ISO_TRAJ_INFO_ROW_MESSAGE_LENGTH); - - HeaderData->SyncWordU16 = ISO_SYNC_WORD; - HeaderData->TransmitterIdU8 = 0; - HeaderData->MessageCounterU8 = 0; - HeaderData->AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; - HeaderData->MessageIdU16 = ISO_TRAJ_CODE; - HeaderData->MessageLengthU32 = ISO_DTM_ROW_MESSAGE_LENGTH * RowCount + ISO_TRAJ_INFO_ROW_MESSAGE_LENGTH; - - p = (C8 *) HeaderData; - for (i = 0; i < ISO_MESSAGE_HEADER_LENGTH; i++) - *(MessageBuffer + i) = *p++; - - - TRAJInfoData->TrajectoryIDValueIdU16 = VALUE_ID_TRAJECTORY_ID; - TRAJInfoData->TrajectoryIDContentLengthU16 = 2; - - TRAJInfoData->TrajectoryNameValueIdU16 = VALUE_ID_TRAJECTORY_NAME; - TRAJInfoData->TrajectoryNameContentLengthU16 = 64; - - TRAJInfoData->TrajectoryVersionValueIdU16 = VALUE_ID_TRAJECTORY_VERSION; - TRAJInfoData->TrajectoryVersionContentLengthU16 = 2; - - TRAJInfoData->IpAddressValueIdU16 = 0xA000; - TRAJInfoData->IpAddressContentLengthU16 = 4; - - p = (C8 *) TRAJInfoData; - for (; i < ISO_MESSAGE_HEADER_LENGTH + ISO_TRAJ_INFO_ROW_MESSAGE_LENGTH; i++) - *(MessageBuffer + i) = *p++; - - MessageIndex = i; - - - if (debug) { - // TODO: Change this when bytes thingy has been implemented in logging - printf("Header + TRAJInfo total length = %d bytes\n", - (int)(ISO_MESSAGE_HEADER_LENGTH + ISO_TRAJ_INFO_ROW_MESSAGE_LENGTH)); - printf("----HEADER + TRAJInfo----\n"); - for (i = 0; i < sizeof (HeaderType) + sizeof (TRAJInfoType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n"); - printf("DOTM message total length = %d bytes.\n", (int)HeaderData->MessageLengthU32); - printf("TrajectoryID = %d\n", TRAJInfoData->TrajectoryIDU16); - printf("TrajectoryName = %s\n", TRAJInfoData->TrajectoryNameC8); - printf("TrajectoryVersion = %d\n", TRAJInfoData->TrajectoryVersionU16); - printf("IpAddress = %d\n", TRAJInfoData->IpAddressU32); - printf("\n----MESSAGE----\n"); - } - - return MessageIndex; //Total number of bytes = ISO_MESSAGE_HEADER_LENGTH -} - -I32 UtilISOBuildTRAJMessage(C8 * MessageBuffer, C8 * DTMData, I32 RowCount, DOTMType * DOTMData, U8 debug) { - I32 MessageIndex = 0; - U32 Data; - C8 *src, *p; - U16 Crc = 0; - - bzero(MessageBuffer, ISO_DTM_ROW_MESSAGE_LENGTH * RowCount); - - I32 i = 0, j = 0, n = 0; - - for (i = 0; i < RowCount; i++) { - //Time - Data = 0; - Data = *(DTMData + SIM_TRAJ_BYTES_IN_ROW * i + 3); - //if(debug) printf("%x-",Data); - Data = (Data << 8) | *(DTMData + SIM_TRAJ_BYTES_IN_ROW * i + 2); - //if(debug) printf("%x-",Data); - Data = (Data << 8) | *(DTMData + SIM_TRAJ_BYTES_IN_ROW * i + 1); - //if(debug) printf("%x-",Data); - Data = (Data << 8) | *(DTMData + SIM_TRAJ_BYTES_IN_ROW * i + 0); - //if(debug) printf("%x- ",Data); - DOTMData->RelativeTimeValueIdU16 = VALUE_ID_RELATIVE_TIME; - DOTMData->RelativeTimeContentLengthU16 = 4; - DOTMData->RelativeTimeU32 = SwapU32((U32) Data); - - //x - Data = 0; - Data = *(DTMData + SIM_TRAJ_BYTES_IN_ROW * i + 7); - Data = (Data << 8) | *(DTMData + SIM_TRAJ_BYTES_IN_ROW * i + 6); - Data = (Data << 8) | *(DTMData + SIM_TRAJ_BYTES_IN_ROW * i + 5); - Data = (Data << 8) | *(DTMData + SIM_TRAJ_BYTES_IN_ROW * i + 4); - DOTMData->XPositionValueIdU16 = VALUE_ID_X_POSITION; - DOTMData->XPositionContentLengthU16 = 4; - DOTMData->XPositionI32 = SwapI32((I32) Data); - - //y - Data = 0; - Data = *(DTMData + SIM_TRAJ_BYTES_IN_ROW * i + 11); - Data = (Data << 8) | *(DTMData + SIM_TRAJ_BYTES_IN_ROW * i + 10); - Data = (Data << 8) | *(DTMData + SIM_TRAJ_BYTES_IN_ROW * i + 9); - Data = (Data << 8) | *(DTMData + SIM_TRAJ_BYTES_IN_ROW * i + 8); - DOTMData->YPositionValueIdU16 = VALUE_ID_Y_POSITION; - DOTMData->YPositionContentLengthU16 = 4; - DOTMData->YPositionI32 = SwapI32((I32) Data); - - //z - Data = 0; - Data = *(DTMData + SIM_TRAJ_BYTES_IN_ROW * i + 15); - Data = (Data << 8) | *(DTMData + SIM_TRAJ_BYTES_IN_ROW * i + 14); - Data = (Data << 8) | *(DTMData + SIM_TRAJ_BYTES_IN_ROW * i + 13); - Data = (Data << 8) | *(DTMData + SIM_TRAJ_BYTES_IN_ROW * i + 12); - DOTMData->ZPositionValueIdU16 = VALUE_ID_Z_POSITION; - DOTMData->ZPositionContentLengthU16 = 4; - DOTMData->ZPositionI32 = SwapI32((I32) Data); - - //Heading - Data = 0; - Data = *(DTMData + SIM_TRAJ_BYTES_IN_ROW * i + 17); - Data = (Data << 8) | *(DTMData + SIM_TRAJ_BYTES_IN_ROW * i + 16); - //Data = UtilRadToDeg(Data); - //Data = 4500 - Data; //Turn heading back pi/2 - //while(Data<0) Data+=360.0; - //while(Data>3600) Data-=360.0; - DOTMData->HeadingValueIdU16 = VALUE_ID_HEADING; - DOTMData->HeadingContentLengthU16 = 2; - DOTMData->HeadingU16 = SwapU16((U16) (Data)); - - //Longitudinal speed - Data = 0; - Data = *(DTMData + SIM_TRAJ_BYTES_IN_ROW * i + 19); - Data = (Data << 8) | *(DTMData + SIM_TRAJ_BYTES_IN_ROW * i + 18); - DOTMData->LongitudinalSpeedValueIdU16 = VALUE_ID_LONGITUDINAL_SPEED; - DOTMData->LongitudinalSpeedContentLengthU16 = 2; - DOTMData->LongitudinalSpeedI16 = SwapI16((I16) Data); - - //Lateral speed - Data = 0; - Data = *(DTMData + SIM_TRAJ_BYTES_IN_ROW * i + 21); - Data = (Data << 8) | *(DTMData + SIM_TRAJ_BYTES_IN_ROW * i + 20); - DOTMData->LateralSpeedValueIdU16 = VALUE_ID_LATERAL_SPEED; - DOTMData->LateralSpeedContentLengthU16 = 2; - DOTMData->LateralSpeedI16 = SwapI16((I16) Data); - - //Longitudinal acceleration - Data = 0; - Data = *(DTMData + SIM_TRAJ_BYTES_IN_ROW * i + 23); - Data = (Data << 8) | *(DTMData + SIM_TRAJ_BYTES_IN_ROW * i + 22); - DOTMData->LongitudinalAccValueIdU16 = VALUE_ID_LONGITUDINAL_ACCELERATION; - DOTMData->LongitudinalAccContentLengthU16 = 2; - DOTMData->LongitudinalAccI16 = SwapI16((I16) Data); - - //Lateral acceleration - Data = 0; - Data = *(DTMData + SIM_TRAJ_BYTES_IN_ROW * i + 25); - Data = (Data << 8) | *(DTMData + SIM_TRAJ_BYTES_IN_ROW * i + 24); - DOTMData->LateralAccValueIdU16 = VALUE_ID_LATERAL_ACCELERATION; - DOTMData->LateralAccContentLengthU16 = 2; - DOTMData->LateralAccI16 = SwapI16((I16) Data); - - //Curvature - Data = 0; - Data = *(DTMData + SIM_TRAJ_BYTES_IN_ROW * i + 29); - Data = (Data << 8) | *(DTMData + SIM_TRAJ_BYTES_IN_ROW * i + 28); - Data = (Data << 8) | *(DTMData + SIM_TRAJ_BYTES_IN_ROW * i + 27); - Data = (Data << 8) | *(DTMData + SIM_TRAJ_BYTES_IN_ROW * i + 26); - DOTMData->CurvatureValueIdU16 = VALUE_ID_CURVATURE; - DOTMData->CurvatureContentLengthU16 = 4; - DOTMData->CurvatureI32 = SwapI32((I32) Data); - - p = (C8 *) DOTMData; - for (j = 0; j < sizeof (DOTMType); j++, n++) - *(MessageBuffer + n) = *p++; - MessageIndex = n; - - if (debug) { - LogPrint - ("%d. Time=%d, X=%d, Y=%d, Z=%d, Heading=%d, LongitudinalSpeedI16=%d, LateralSpeedI16=%d, LongitudinalAccI16=%d, LateralAccI16=%d, CurvatureI32=%d", - i, DOTMData->RelativeTimeU32, DOTMData->XPositionI32, DOTMData->YPositionI32, - DOTMData->ZPositionI32, DOTMData->HeadingU16, DOTMData->LongitudinalSpeedI16, - DOTMData->LateralSpeedI16, DOTMData->LongitudinalAccI16, DOTMData->LateralAccI16, - DOTMData->CurvatureI32); - } - } - - - Crc = crc_16((const C8 *)MessageBuffer, sizeof (DOTMType)); - Crc = 0; - *(MessageBuffer + MessageIndex++) = (U8) (Crc); - *(MessageBuffer + MessageIndex++) = (U8) (Crc >> 8); - - - if (debug == 2) { - // TODO: Replace this when bytes thingy has been implemented - int i = 0; - - for (i = 0; i < MessageIndex; i++) { - if ((unsigned char)MessageBuffer[i] >= 0 && (unsigned char)MessageBuffer[i] <= 15) - printf("0"); - printf("%x-", (unsigned char)MessageBuffer[i]); - } - printf("\n"); - } - - return MessageIndex; //Total number of bytes -} - /* UtilWriteConfigurationParameter updates parameters in the file test.conf. From 244b70f981a301cb71a340bae68e4eb7cf162a0f Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 12 Feb 2020 10:44:44 +0100 Subject: [PATCH 248/523] Removed some unsupported code --- server/src/objectcontrol.c | 374 ------------------------------------- 1 file changed, 374 deletions(-) diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index a12b2f3b8..5bcd576a4 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -60,25 +60,8 @@ #define OC_STATE_REPORT_PERIOD_S 1 #define OC_STATE_REPORT_PERIOD_US 0 -#define COMMAND_MESSAGE_HEADER_LENGTH sizeof(HeaderType) -#define COMMAND_MESSAGE_FOOTER_LENGTH sizeof(FooterType) -#define COMMAND_CODE_INDEX 0 -#define COMMAND_MESSAGE_LENGTH_INDEX 1 - #define TRAJECTORY_TX_BUFFER_SIZE 2048 -#define COMMAND_DOTM_ROW_MESSAGE_LENGTH sizeof(DOTMType) -#define COMMAND_TRAJ_INFO_ROW_MESSAGE_LENGTH sizeof(TRAJInfoType) -#define COMMAND_DOTM_ROWS_IN_TRANSMISSION 40 -#define COMMAND_DTM_BYTES_IN_ROW 30 - -#define COMMAND_VOIL_CODE 0xA100 -//#define COMMAND_VOIL_NOFV 2 -#define COMMAND_VOIL_MESSAGE_LENGTH (16 * sizeof(Sim1Type) + sizeof(HeaderType) + 5) - -#define COMMAND_LLCM_CODE 8 -#define COMMAND_LLCM_MESSAGE_LENGTH 5 - #define SMALL_BUFFER_SIZE_0 20 #define SMALL_BUFFER_SIZE_1 2 #define SMALL_BUFFER_SIZE_2 1 @@ -109,9 +92,6 @@ typedef enum { */ typedef StateTransitionResult(*StateTransition) (OBCState_t * currentState, OBCState_t requestedState); -C8 TrajBuffer[COMMAND_DOTM_ROWS_IN_TRANSMISSION * COMMAND_DOTM_ROW_MESSAGE_LENGTH + - COMMAND_MESSAGE_HEADER_LENGTH + COMMAND_TRAJ_INFO_ROW_MESSAGE_LENGTH]; - /*------------------------------------------------------------ -- Function declarations. @@ -127,15 +107,8 @@ static size_t uiRecvMonitor(int *sockfd, char *buffer, size_t length); static int iGetObjectIndexFromObjectIP(in_addr_t ipAddr, in_addr_t objectIPs[], unsigned int numberOfObjects); static void signalHandler(int signo); -int ObjectControlBuildLLCMMessage(char *MessageBuffer, unsigned short Speed, unsigned short Curvature, - unsigned char Mode, char debug); static ssize_t ObjectControlSendTRAJMessage(const char * Filename, int * Socket, const char debug); int ObjectControlSendUDPData(int *sockfd, struct sockaddr_in *addr, char *SendData, int Length, char debug); -I32 ObjectControlBuildVOILMessage(C8 * MessageBuffer, VOILType * VOILData, C8 * SimData, U8 debug); -I32 ObjectControlSendDTMMessage(C8 * DTMData, I32 * Socket, I32 RowCount, C8 * IP, U32 Port, - DOTMType * DOTMData, U8 debug); -I32 ObjectControlBuildDTMMessage(C8 * MessageBuffer, C8 * DTMData, I32 RowCount, DOTMType * DOTMData, - U8 debug); static int iFindObjectsInfo(C8 object_traj_file[MAX_OBJECTS][MAX_FILE_PATH], C8 object_address_name[MAX_OBJECTS][MAX_FILE_PATH], @@ -222,10 +195,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { dbl OriginHeadingDbl; C8 pcSendBuffer[MBUS_MAX_DATALEN]; C8 ObjectPort[SMALL_BUFFER_SIZE_0]; - HeaderType HeaderData; MonitorDataType monitorData; - DOTMType DOTMData; - TRAJInfoType TRAJInfoData; VOILType VOILData; ACCMData mqACCMData; EXACData mqEXACData; @@ -995,173 +965,6 @@ void signalHandler(int signo) { } } -I32 ObjectControlBuildVOILMessage(C8 * MessageBuffer, VOILType * VOILData, C8 * SimData, U8 debug) { - I32 MessageIndex = 0, i; - U16 Crc = 0; - C8 *p; - U16 U16Data = 0; - I16 I16Data = 0; - U32 U32Data = 0; - I32 I32Data = 0; - - if (debug) { - printf("Length: %d\n", *(SimData + 3)); - for (i = 0; i < *(SimData + 3) + 4; i++) - printf("%x-", *(SimData + i)); - printf("\n"); - } - - U16Data = (U16Data | *(SimData + 5)) << 8; - U16Data = (U16Data | *(SimData + 4)); - U16 MessageId = U16Data; - - //printf("MessageId = %x\n", MessageId); - - U32Data = (U32Data | *(SimData + 6)) << 8; - U32Data = (U32Data | *(SimData + 7)) << 8; - U32Data = (U32Data | *(SimData + 8)) << 8; - U32Data = (U32Data | *(SimData + 9)); - U32 GPSSOW = U32Data; - - //printf("GPSSOW = %x\n", GPSSOW); - U8 DynamicWorldState = *(SimData + 10); - U8 ObjectCount = *(SimData + 11); - - //printf("ObjectCount = %d\n", ObjectCount); - - U8 ObjectId = *(SimData + 12); - U8 ObjectState = *(SimData + 13); - - I32Data = (I32Data | *(SimData + 14)) << 8; - I32Data = (I32Data | *(SimData + 15)) << 8; - I32Data = (I32Data | *(SimData + 16)) << 8; - I32Data = (I32Data | *(SimData + 17)); - I32 XPosition = I32Data; - - I32Data = 0; - I32Data = (I32Data | *(SimData + 18)) << 8; - I32Data = (I32Data | *(SimData + 19)) << 8; - I32Data = (I32Data | *(SimData + 20)) << 8; - I32Data = (I32Data | *(SimData + 21)); - I32 YPosition = I32Data; - - I32Data = 0; - I32Data = (I32Data | *(SimData + 22)) << 8; - I32Data = (I32Data | *(SimData + 23)) << 8; - I32Data = (I32Data | *(SimData + 24)) << 8; - I32Data = (I32Data | *(SimData + 25)); - I32 ZPosition = I32Data; - - U16Data = 0; - U16Data = (U16Data | *(SimData + 26)) << 8; - U16Data = (U16Data | *(SimData + 27)); - U16 Heading = U16Data; - - U16Data = 0; - U16Data = (U16Data | *(SimData + 28)) << 8; - U16Data = (U16Data | *(SimData + 29)); - U16 Pitch = U16Data; - - U16Data = 0; - U16Data = (U16Data | *(SimData + 30)) << 8; - U16Data = (U16Data | *(SimData + 31)); - U16 Roll = U16Data; - - //printf("Roll = %d\n", Roll); - I16Data = 0; - I16Data = (I16Data | *(SimData + 32)) << 8; - I16Data = (I16Data | *(SimData + 33)); - I16 Speed = I16Data; - - //printf("Speed = %d\n", Speed); - - - bzero(MessageBuffer, - ObjectCount * sizeof (Sim1Type) + 6 + COMMAND_MESSAGE_FOOTER_LENGTH + - COMMAND_MESSAGE_HEADER_LENGTH); - - - VOILData->Header.SyncWordU16 = ISO_SYNC_WORD; - VOILData->Header.TransmitterIdU8 = 0; - VOILData->Header.MessageCounterU8 = 0; - VOILData->Header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; - VOILData->Header.MessageIdU16 = COMMAND_VOIL_CODE; - VOILData->Header.MessageLengthU32 = ObjectCount * sizeof (Sim1Type) + 6 - COMMAND_MESSAGE_HEADER_LENGTH; - VOILData->GPSQmsOfWeekU32 = GPSSOW; - VOILData->WorldStateU8 = DynamicWorldState; - VOILData->ObjectCountU8 = ObjectCount; - VOILData->SimObjects[0].ObjectIdU8 = ObjectId; - VOILData->SimObjects[0].ObjectStateU8 = ObjectState; - VOILData->SimObjects[0].XPositionI32 = XPosition; - VOILData->SimObjects[0].YPositionI32 = YPosition; - VOILData->SimObjects[0].ZPositionI32 = ZPosition; - VOILData->SimObjects[0].HeadingU16 = Heading; - VOILData->SimObjects[0].PitchU16 = Pitch; - VOILData->SimObjects[0].RollU16 = Roll; - VOILData->SimObjects[0].SpeedI16 = Speed; - - - p = (C8 *) VOILData; - for (i = 0; i < ObjectCount * sizeof (Sim1Type) + 6 + COMMAND_MESSAGE_HEADER_LENGTH; i++) - *(MessageBuffer + i) = *p++; - //Crc = crc_16((const C8*)MessageBuffer, sizeof(VOILData)); - Crc = 0; - *(MessageBuffer + i++) = (U8) (Crc); - *(MessageBuffer + i++) = (U8) (Crc >> 8); - MessageIndex = i; - - if (debug) { - // TODO: use byte printout from logging when it has been implemented - printf("VOILData total length = %d bytes (header+message+footer)\n", - (int)(ObjectCount * sizeof (Sim1Type) + 6 + COMMAND_MESSAGE_FOOTER_LENGTH + - COMMAND_MESSAGE_HEADER_LENGTH)); - printf("----HEADER----\n"); - for (i = 0; i < sizeof (HeaderType); i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----MESSAGE----\n"); - for (; i < sizeof (Sim1Type) * ObjectCount + 6 + COMMAND_MESSAGE_HEADER_LENGTH; i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n----FOOTER----\n"); - for (; i < MessageIndex; i++) - printf("%x ", (unsigned char)MessageBuffer[i]); - printf("\n"); - } - - - return ObjectCount * sizeof (Sim1Type) + 6 + COMMAND_MESSAGE_HEADER_LENGTH + COMMAND_MESSAGE_FOOTER_LENGTH; //Total number of bytes - -} - - -int ObjectControlBuildLLCMMessage(char *MessageBuffer, unsigned short Speed, unsigned short Curvature, - unsigned char Mode, char debug) { - int MessageIndex = 0; - - bzero(MessageBuffer, COMMAND_LLCM_MESSAGE_LENGTH + COMMAND_MESSAGE_HEADER_LENGTH); - - UtilAddOneByteMessageData(MessageBuffer, COMMAND_CODE_INDEX, COMMAND_LLCM_CODE); - - MessageIndex = - UtilAddTwoBytesMessageData(MessageBuffer, MessageIndex + COMMAND_MESSAGE_HEADER_LENGTH, Speed); - - MessageIndex = UtilAddTwoBytesMessageData(MessageBuffer, MessageIndex, Curvature); - - MessageIndex = UtilAddOneByteMessageData(MessageBuffer, MessageIndex, Mode); - - UtilAddFourBytesMessageData(MessageBuffer, COMMAND_MESSAGE_LENGTH_INDEX, - (unsigned int)MessageIndex - COMMAND_MESSAGE_HEADER_LENGTH); - - if (debug) { - int i = 0; - - LogMessage(LOG_LEVEL_DEBUG, "LLCM:"); - for (i = 0; i < MessageIndex; i++) - LogMessage(LOG_LEVEL_DEBUG, "[%d]= %x", i, (unsigned char)MessageBuffer[i]); - } - - return MessageIndex; //Total number of bytes = COMMAND_MESSAGE_HEADER_LENGTH + message data count -} - ssize_t ObjectControlSendTRAJMessage(const char * Filename, int * Socket, const char debug) { @@ -1343,183 +1146,6 @@ ssize_t ObjectControlSendTRAJMessage(const char * Filename, int * Socket, const } - -I32 ObjectControlSendDTMMessage(C8 * DTMData, I32 * Socket, I32 RowCount, C8 * IP, U32 Port, - DOTMType * DOTMData, U8 debug) { - - U32 Rest = 0, i = 0, MessageLength = 0, SumMessageLength = 0, Modulo = 0, Transmissions = 0; - U16 CrcU16 = 0; - - MessageLength = - ObjectControlBuildDTMMessage(TrajBuffer, DTMData, COMMAND_DOTM_ROWS_IN_TRANSMISSION, DOTMData, 0); - - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "Transmission %d: %d bytes sent", i, MessageLength); - - LogMessage(LOG_LEVEL_INFO, "%d DTM bytes sent to %s:%d", SumMessageLength, IP, Port); - - return 0; -} - - -I32 ObjectControlBuildDTMMessage(C8 * MessageBuffer, C8 * DTMData, I32 RowCount, DOTMType * DOTMData, - U8 debug) { - I32 MessageIndex = 0; - U32 Data; - C8 *src, *p; - U16 Crc = 0; - - bzero(MessageBuffer, COMMAND_DOTM_ROW_MESSAGE_LENGTH * RowCount); - - I32 i = 0, j = 0, n = 0; - - for (i = 0; i < RowCount; i++) { - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "DOTM row:"); - //Time - Data = 0; - Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 3); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 2); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 1); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 0); - DOTMData->RelativeTimeValueIdU16 = VALUE_ID_RELATIVE_TIME; - DOTMData->RelativeTimeContentLengthU16 = 4; - DOTMData->RelativeTimeU32 = SwapU32((U32) Data); - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "Time=%d", DOTMData->RelativeTimeU32); - - //x - Data = 0; - Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 7); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 6); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 5); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 4); - DOTMData->XPositionValueIdU16 = VALUE_ID_X_POSITION; - DOTMData->XPositionContentLengthU16 = 4; - DOTMData->XPositionI32 = SwapI32((I32) Data); - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "X=%d", DOTMData->XPositionI32); - - //y - Data = 0; - Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 11); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 10); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 9); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 8); - DOTMData->YPositionValueIdU16 = VALUE_ID_Y_POSITION; - DOTMData->YPositionContentLengthU16 = 4; - DOTMData->YPositionI32 = SwapI32((I32) Data); - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "Y=%d", DOTMData->YPositionI32); - - //z - Data = 0; - Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 15); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 14); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 13); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 12); - DOTMData->ZPositionValueIdU16 = VALUE_ID_Z_POSITION; - DOTMData->ZPositionContentLengthU16 = 4; - DOTMData->ZPositionI32 = SwapI32((I32) Data); - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "Z=%d", DOTMData->ZPositionI32); - - //Heading - Data = 0; - Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 17); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 16); - //Data = UtilRadToDeg(Data); - //Data = 4500 - Data; //Turn heading back pi/2 - //while(Data<0) Data+=360.0; - //while(Data>3600) Data-=360.0; - DOTMData->HeadingValueIdU16 = VALUE_ID_HEADING; - DOTMData->HeadingContentLengthU16 = 2; - DOTMData->HeadingU16 = SwapU16((U16) (Data)); - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "Heading=%d", DOTMData->HeadingU16); - - //Longitudinal speed - Data = 0; - Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 19); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 18); - DOTMData->LongitudinalSpeedValueIdU16 = VALUE_ID_LONGITUDINAL_SPEED; - DOTMData->LongitudinalSpeedContentLengthU16 = 2; - DOTMData->LongitudinalSpeedI16 = SwapI16((I16) Data); - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "LongitudinalSpeedI16=%d", DOTMData->LongitudinalSpeedI16); - - //Lateral speed - Data = 0; - Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 21); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 20); - DOTMData->LateralSpeedValueIdU16 = VALUE_ID_LATERAL_SPEED; - DOTMData->LateralSpeedContentLengthU16 = 2; - DOTMData->LateralSpeedI16 = SwapI16((I16) Data); - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "LateralSpeedI16=%d", DOTMData->LateralSpeedI16); - - //Longitudinal acceleration - Data = 0; - Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 23); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 22); - DOTMData->LongitudinalAccValueIdU16 = VALUE_ID_LONGITUDINAL_ACCELERATION; - DOTMData->LongitudinalAccContentLengthU16 = 2; - DOTMData->LongitudinalAccI16 = SwapI16((I16) Data); - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "LongitudinalAccI16=%d", DOTMData->LongitudinalAccI16); - - //Lateral acceleration - Data = 0; - Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 25); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 24); - DOTMData->LateralAccValueIdU16 = VALUE_ID_LATERAL_ACCELERATION; - DOTMData->LateralAccContentLengthU16 = 2; - DOTMData->LateralAccI16 = SwapI16((I16) Data); - if (debug) - LogMessage(LOG_LEVEL_DEBUG, "LateralAccI16=%d", DOTMData->LateralAccI16); - - //Curvature - Data = 0; - Data = *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 29); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 28); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 27); - Data = (Data << 8) | *(DTMData + COMMAND_DTM_BYTES_IN_ROW * i + 26); - DOTMData->CurvatureValueIdU16 = VALUE_ID_CURVATURE; - DOTMData->CurvatureContentLengthU16 = 4; - DOTMData->CurvatureI32 = SwapI32((I32) Data); - if (debug) - printf("CurvatureI32=%d \n", DOTMData->CurvatureI32); - - p = (C8 *) DOTMData; - for (j = 0; j < sizeof (DOTMType); j++, n++) - *(MessageBuffer + n) = *p++; - MessageIndex = n; - } - - - Crc = crc_16((const C8 *)MessageBuffer, sizeof (DOTMType)); - Crc = 0; - *(MessageBuffer + MessageIndex++) = (U8) (Crc); - *(MessageBuffer + MessageIndex++) = (U8) (Crc >> 8); - - - if (debug) { - int i = 0; - - for (i = 0; i < MessageIndex; i++) { - // TODO: Write to log when bytes thingy has been implemented - if ((unsigned char)MessageBuffer[i] >= 0 && (unsigned char)MessageBuffer[i] <= 15) - printf("0"); - printf("%x-", (unsigned char)MessageBuffer[i]); - } - printf("\n"); - } - - return MessageIndex; //Total number of bytes - -} - - static int iGetObjectIndexFromObjectIP(in_addr_t ipAddr, in_addr_t objectIPs[], unsigned int numberOfObjects) { for (unsigned int i = 0; i < numberOfObjects; ++i) { if (objectIPs[i] == ipAddr) From 26d40364e7a8c72ef9f72494c6a0cad50f1e2a68 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 12 Feb 2020 10:58:02 +0100 Subject: [PATCH 249/523] Moved ISO header definition to .c file --- server/inc/iso22133.h | 24 ------------------------ server/src/iso22133.c | 26 +++++++++++++++++++++++++- 2 files changed, 25 insertions(+), 25 deletions(-) diff --git a/server/inc/iso22133.h b/server/inc/iso22133.h index 4ff9f626b..faae10143 100644 --- a/server/inc/iso22133.h +++ b/server/inc/iso22133.h @@ -24,30 +24,6 @@ extern "C" { #include "positioning.h" -// TODO: move to .c-file -#define ISO_PROTOCOL_VERSION 2 -#define ACK_REQ 0 - -#define ISO_SYNC_WORD 0x7E7E - -#pragma pack(push,1) -typedef struct -{ - uint16_t SyncWordU16; - uint8_t TransmitterIdU8; - uint8_t MessageCounterU8; - uint8_t AckReqProtVerU8; - uint16_t MessageIdU16; - uint32_t MessageLengthU32; -} HeaderType; //11 bytes - -typedef struct -{ - uint16_t Crc; -} FooterType; //2 bytes - -#pragma pack(pop) - /*! OSTM commands */ typedef enum { OBJECT_COMMAND_ARM = 0x02, //!< Request to arm the target object diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 5ae4cce8b..8a5e1ea82 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -8,13 +8,16 @@ #include #include -// ************************* ISO protocol versions supported by functions in this file *************************** +// ************************* Global ISO protocol settings ******************************************************** static const uint8_t SupportedProtocolVersions[] = { 2 }; +#define ISO_PROTOCOL_VERSION 2 //!< ISO protocol version of messages sent +#define ACK_REQ 0 // ************************* Type definitions according ISO protocol specification ******************************* //! Predefined integer values with special meaning +#define ISO_SYNC_WORD 0x7E7E #define LATITUDE_UNAVAILABLE_VALUE 900000000001 #define LATITUDE_ONE_DEGREE_VALUE 10000000000 #define LONGITUDE_UNAVAILABLE_VALUE 1800000000001 @@ -78,6 +81,25 @@ typedef enum { #pragma pack(push,1) // Ensure sizeof() is useable for (most) network byte lengths +/*! ISO message header */ +typedef struct +{ + uint16_t SyncWordU16; + uint8_t TransmitterIdU8; + uint8_t MessageCounterU8; + uint8_t AckReqProtVerU8; + uint16_t MessageIdU16; + uint32_t MessageLengthU32; +} HeaderType; + + +/*! ISO message footer */ +typedef struct +{ + uint16_t Crc; +} FooterType; + + /*! TRAJ message */ #define TRAJ_NAME_STRING_MAX_LENGTH 64 typedef struct { @@ -145,6 +167,7 @@ typedef struct { #define VALUE_ID_TRAJ_LATERAL_ACCELERATION 0x0051 #define VALUE_ID_TRAJ_CURVATURE 0x0052 + /*! OSEM message */ typedef struct { HeaderType header; @@ -385,6 +408,7 @@ typedef struct { //! CATA value IDs // TODO + /*! INSUP message */ typedef struct { HeaderType header; From b927a2fc61a0b6b332092d4e8fc11947b3fe1234 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 12 Feb 2020 10:58:44 +0100 Subject: [PATCH 250/523] Removed more unsupported code --- server/inc/objectcontrol.h | 2 -- server/inc/util.h | 11 ----------- 2 files changed, 13 deletions(-) diff --git a/server/inc/objectcontrol.h b/server/inc/objectcontrol.h index fcd616b58..b794ff533 100644 --- a/server/inc/objectcontrol.h +++ b/server/inc/objectcontrol.h @@ -21,6 +21,4 @@ ------------------------------------------------------------*/ void objectcontrol_task(TimeType *GPSTime, GSDType *GSD, LOG_LEVEL logLevel); -int ObjectControlBuildLLCMMessage(char* MessageBuffer, unsigned short Speed, unsigned short Curvature, unsigned char Mode, char debug); -I32 ObjectControlBuildVOILMessage(C8* MessageBuffer, VOILType *VOILData, C8* SimData, U8 debug); #endif //__OBJECTCONTROL_H_INCLUDED__ diff --git a/server/inc/util.h b/server/inc/util.h index 7a702c473..4b4f1cce5 100644 --- a/server/inc/util.h +++ b/server/inc/util.h @@ -479,17 +479,6 @@ typedef struct } Sim1Type; -typedef struct -{ - HeaderType Header; - U32 GPSQmsOfWeekU32; - U8 WorldStateU8; - U8 ObjectCountU8; - Sim1Type SimObjects[16]; - -} VOILType; - - #define HTTP_HEADER_MAX_LENGTH 64 typedef struct { char AcceptCharset[HTTP_HEADER_MAX_LENGTH]; From 8a53ed45e1ad5f041f5078deec075cc8778551d1 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 12 Feb 2020 10:58:59 +0100 Subject: [PATCH 251/523] Added function description --- server/src/objectcontrol.c | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 5bcd576a4..080e34eac 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -196,7 +196,6 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { C8 pcSendBuffer[MBUS_MAX_DATALEN]; C8 ObjectPort[SMALL_BUFFER_SIZE_0]; MonitorDataType monitorData; - VOILType VOILData; ACCMData mqACCMData; EXACData mqEXACData; TRCMData mqTRCMData; @@ -966,7 +965,13 @@ void signalHandler(int signo) { } - +/*! + * \brief ObjectControlSendTRAJMessage Sends a trajectory message over a socket based on a trajectory file. + * \param Filename Path and name of the file containing the trajectory + * \param Socket TCP socket over which the trajectory is to be sent + * \param debug Flag for enabling debugging + * \return Number of bytes sent, or -1 in case of an error + */ ssize_t ObjectControlSendTRAJMessage(const char * Filename, int * Socket, const char debug) { FILE *fd; char* line; @@ -1019,8 +1024,8 @@ ssize_t ObjectControlSendTRAJMessage(const char * Filename, int * Socket, const } // Generate ISO trajectory message header - if ((printedBytes = encodeTRAJMessageHeader(fileHeader.ID, fileHeader.majorVersion, - fileHeader.name, strlen(fileHeader.name), + if ((printedBytes = encodeTRAJMessageHeader(fileHeader.ID > UINT16_MAX ? 0 : (uint16_t) fileHeader.ID, + fileHeader.majorVersion, fileHeader.name, strlen(fileHeader.name), fileHeader.numberOfLines, messageBufferPosition, remainingBufferSpace, debug)) == -1) { LogMessage(LOG_LEVEL_ERROR, "Unable to encode trajectory message"); @@ -1066,7 +1071,7 @@ ssize_t ObjectControlSendTRAJMessage(const char * Filename, int * Socket, const // Print to buffer if ((printedBytes = encodeTRAJMessagePoint(&relTime, position, speed, acceleration, - fileLine.curvature, messageBufferPosition, + (float) fileLine.curvature, messageBufferPosition, remainingBufferSpace, debug)) == -1) { if (errno == ENOBUFS) { From a55d1532a5ed64b763c1675593a20f2700c7d2ec Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 12 Feb 2020 11:00:20 +0100 Subject: [PATCH 252/523] Removed some unused variables --- server/src/objectcontrol.c | 2 -- 1 file changed, 2 deletions(-) diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 080e34eac..d9dc1cdc4 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -228,8 +228,6 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { C8 VOILReceivers[SMALL_BUFFER_SIZE_254]; C8 DTMReceivers[SMALL_BUFFER_SIZE_254]; U32 RowCount; - U32 DTMIpU32; - U32 DTMLengthU32; U8 DisconnectU8 = 0; I32 iResult; From 85a27ac1cd2d9f173100e64b85b2812092744e05 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 12 Feb 2020 11:03:18 +0100 Subject: [PATCH 253/523] Removed the pragma pack instruction in util.h (yay) --- server/inc/util.h | 17 ----------------- 1 file changed, 17 deletions(-) diff --git a/server/inc/util.h b/server/inc/util.h index 4b4f1cce5..e024adf06 100644 --- a/server/inc/util.h +++ b/server/inc/util.h @@ -144,9 +144,6 @@ extern "C"{ #define dbl double #define flt float -// Why do we need this memory efficiency? There is a risk that this breaks included code which isn't using pragma pack -#pragma pack(1) // #pragma pack ( 1 ) directive can be used for arranging memory for structure members very next to the end of other structure members. - #define SERVER_PREPARED 0x01 #define SERVER_PREPARED_BIG_PACKET_SIZE 0x02 #define PATH_INVALID_MISSING 0x03 @@ -465,20 +462,6 @@ typedef struct } ServiceSessionType; //9 bytes -typedef struct -{ - U8 ObjectIdU8; - U8 ObjectStateU8; - I32 XPositionI32; - I32 YPositionI32; - I32 ZPositionI32; - U16 HeadingU16; - U16 PitchU16; - U16 RollU16; - I16 SpeedI16; -} Sim1Type; - - #define HTTP_HEADER_MAX_LENGTH 64 typedef struct { char AcceptCharset[HTTP_HEADER_MAX_LENGTH]; From 3cc63f2cf3c965339d5ff98ef58b23e01633b289 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 12 Feb 2020 12:43:19 +0100 Subject: [PATCH 254/523] Updated util ref --- util | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/util b/util index 13d1481a9..5171c2cb4 160000 --- a/util +++ b/util @@ -1 +1 @@ -Subproject commit 13d1481a9be4ba2e851717c650f7ff884467ce1f +Subproject commit 5171c2cb43b958fb4eb5b3df18a92b0e2e9e518f From f13bc8d1719cf45aa6a35496d7fc4de1f2c6b04b Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 12 Feb 2020 13:51:38 +0100 Subject: [PATCH 255/523] Made time control set system time --- server/src/timecontrol.c | 26 +++++++++++++++++++++++++- 1 file changed, 25 insertions(+), 1 deletion(-) diff --git a/server/src/timecontrol.c b/server/src/timecontrol.c index 93717e0df..92e078b2e 100644 --- a/server/src/timecontrol.c +++ b/server/src/timecontrol.c @@ -193,8 +193,32 @@ void timecontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { bzero(TimeBuffer, TIME_CONTROL_RECEIVE_BUFFER_SIZE); TimeControlRecvTime(&SocketfdI32, TimeBuffer, TIME_CONTROL_RECEIVE_BUFFER_SIZE, &ReceivedNewData); - if (ReceivedNewData) + if (ReceivedNewData) { TimeControlDecodeTimeBuffer(GPSTime, TimeBuffer, 0); + + if (GPSTime->GPSMillisecondsU64 < INT64_MAX) { + struct timespec newSystemTime; + TimeSetToGPSms(&tv, (int64_t) GPSTime->GPSMillisecondsU64); + newSystemTime.tv_sec = tv.tv_sec; + newSystemTime.tv_nsec = tv.tv_usec * 1000; + if (clock_settime(CLOCK_REALTIME, &newSystemTime) == -1) { + switch (errno) { + case EPERM: + LogMessage(LOG_LEVEL_ERROR, "Unable to set system time - ensure this program has the correct capabilities"); + break; + case EINVAL: + LogMessage(LOG_LEVEL_ERROR, "Clock type not supported on this system"); + break; + default: + LogMessage(LOG_LEVEL_ERROR, "Error setting system time"); + break; + } + } + } + else { + LogMessage(LOG_LEVEL_ERROR, "Current GPS time exceeds limit and would be interpreted as negative"); + } + } } else if (!GPSTime->isGPSenabled) { gettimeofday(&tv, NULL); From c78508e1310aeb9638a5ffd05d148aa9b948285f Mon Sep 17 00:00:00 2001 From: alfaro01 Date: Wed, 12 Feb 2020 16:42:24 +0100 Subject: [PATCH 256/523] Added manual test for dissector. Added functions to MSCP.py for more MSCP commands. --- server/integration-tests/MSCPManualTest.py | 111 +++++++++++++++++++++ server/integration-tests/tools/MSCP.py | 52 +++++++++- 2 files changed, 162 insertions(+), 1 deletion(-) create mode 100644 server/integration-tests/MSCPManualTest.py diff --git a/server/integration-tests/MSCPManualTest.py b/server/integration-tests/MSCPManualTest.py new file mode 100644 index 000000000..524535102 --- /dev/null +++ b/server/integration-tests/MSCPManualTest.py @@ -0,0 +1,111 @@ +from tools.MSCP import MSCP +from tools.Executable import Executable +import time +import subprocess +import sys + + +tests = ["INIT", + "CONNECTOBJECT", + "ARM", + "START", + "ABORT", + "GETSERVERSTATUS", + "CREATEOBJECTS", + "DISARM", + "GETSERVERTIME", + "UPLOADFILE", + "DOWNLOADFILE", + "DELETEFILE", + "CHECKFILEEXISTS", + "REMOTECONTROL", + "SETSERVERPARAMETER", + "GETSERVERPARAMETERS", + "GETDIRECTORYCONTENT", + "DISCONNECTOBJECT" ] + + +if __name__ == "__main__": + + S = Executable("../build/TEServer",["-m","0"]) + time.sleep(0.05) + M = MSCP("127.0.0.1") + time.sleep(1) + + print("Starting test") + + + for t in tests: + print("\n Press enter to test {}".format(t)) + s = input() + + if(t == "INIT"): + M.Init() + + if(t == "CONNECTOBJECT"): + M.Connect() + + if(t == "ARM"): + M.Arm() + + if(t == "START"): + M.Start(0) + + if(t == "ABORT"): + M.Abort() + + if(t == "GETSERVERSTATUS"): + M.GetStatus() + + if(t == "CREATEOBJECTS"): + M.CreateObjects(1) + + if(t == "DISARM"): + M.Disarm() + + if(t == "GETSERVERTIME"): + M.GetServerTime() + + #if(t == "UPLOADFILE"): + # M.UploadFile("","") + + if(t == "DOWNLOADFILE"): + M.DownloadFile() + + if(t == "DELETEFILE"): + M.DeleteFile() + + if(t == "CHECKFILEEXISTS"): + M.CheckFileExists() + + if(t == "REMOTECONTROL"): + M.RemoteControl() + + if(t == "SETSERVERPARAMETER"): + M.SetServerParameter() + + if(t == "GETSERVERPARAMETERS"): + M.GetServerParameters() + + if(t == "GETDIRECTORYCONTENT"): + M.GetDirectoryContent() + + if(t == "DISCONNECTOBJECT"): + M.Disconnect() + + + print("Checking server...") + time.sleep(1) + if S.poll(): + S.stop() + M.shutdown() + sys.exit(1) + + + S.stop() + M.shutdown() + sys.exit(1) + + + + diff --git a/server/integration-tests/tools/MSCP.py b/server/integration-tests/tools/MSCP.py index efd87d0fc..95d2de3c8 100644 --- a/server/integration-tests/tools/MSCP.py +++ b/server/integration-tests/tools/MSCP.py @@ -168,7 +168,7 @@ def listen(self): print("=== Unable to match against data: " + str(data)) def GetStatus(self): - message = "POST /maestro HTTP/1.1\r\nHost: " + self.host + "\r\n\r\nGetServerStatus();\r\n\r\n" + message = "POST /maestro HTTP/1.1\r\nHost:" + self.host + "\r\n\r\nGetServerStatus();\r\n\r\n" self.Send(message) print("=== GetServerStatus() sent") @@ -182,6 +182,11 @@ def Arm(self): self.Send(message) print("=== ArmScenario() sent") + def Disarm(self): + message = "POST /maestro HTTP/1.1\r\nHost:" + self.host + "\r\n\r\nDisarmScenario();\r\n\r\n" + self.Send(message) + print("=== DisarmScenario() sent") + def Init(self): message = "POST /maestro HTTP/1.1\r\nHost:" + self.host + "\r\n\r\nInitializeScenario();\r\n\r\n" self.Send(message) @@ -191,6 +196,11 @@ def Connect(self): message = "POST /maestro HTTP/1.1\r\nHost:" + self.host + "\r\n\r\nConnectObject();\r\n\r\n" self.Send(message) print("=== Connect() sent") + + def CreateObjects(self, count): + message = "POST /maestro HTTP/1.1\r\nHost:" + self.host + "\r\n\r\nCreateObjects(" + str(count) + ");\r\n\r\n" + self.Send(message) + print("=== CreateObjects() sent") def Disconnect(self): message = "POST /maestro HTTP/1.1\r\nHost:" + self.host + "\r\n\r\nDisconnectObject();\r\n\r\n" @@ -221,6 +231,46 @@ def UploadFile(self,targetPath,fileContents): self.waitForUploadReply("UPLOAD_SUCCESS") print("=== File upload verified") + def DownloadFile(self): #TODO + message = "POST /maestro HTTP/1.1\r\nHost:" + self.host + "\r\n\r\nDownloadFile();\r\n\r\n" + self.Send(message) + print("=== DownloadFile() sent") + + def DeleteFile(self): #TODO + message = "POST /maestro HTTP/1.1\r\nHost:" + self.host + "\r\n\r\nDeleteFile();\r\n\r\n" + self.Send(message) + print("=== DeleteFile() sent") + + def CheckFileExists(self): #TODO + message = "POST /maestro HTTP/1.1\r\nHost:" + self.host + "\r\n\r\nCheckFileExists();\r\n\r\n" + self.Send(message) + print("=== CheckFileExists() sent") + + def RemoteControl(self): #TODO + message = "POST /maestro HTTP/1.1\r\nHost:" + self.host + "\r\n\r\nRemoteControl();\r\n\r\n" + self.Send(message) + print("=== RemoteControl() sent") + + def SetServerParameter(self): #TODO + message = "POST /maestro HTTP/1.1\r\nHost:" + self.host + "\r\n\r\nSetServerParameter();\r\n\r\n" + self.Send(message) + print("=== SetServerParameter() sent") + + def GetServerParameters(self): #TODO + message = "POST /maestro HTTP/1.1\r\nHost:" + self.host + "\r\n\r\nGetServerParameters();\r\n\r\n" + self.Send(message) + print("=== GetServerParameters() sent") + + def GetDirectoryContent(self): #TODO + message = "POST /maestro HTTP/1.1\r\nHost:" + self.host + "\r\n\r\nGetDirectoryContent();\r\n\r\n" + self.Send(message) + print("=== GetDirectoryContent() sent") + + def GetServerTime(self): + message = "POST /maestro HTTP/1.1\r\nHost:" + self.host + "\r\n\r\nGetServerTime();\r\n\r\n" + self.Send(message) + print("=== GetServerTime() sent") + def Send(self,message): self.socket.send(message.encode()) From ed65d7db162c27c354959484dc8cd474a6454e8d Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 13 Feb 2020 13:52:13 +0100 Subject: [PATCH 257/523] Ran code formatter --- server/src/timecontrol.c | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/server/src/timecontrol.c b/server/src/timecontrol.c index 92e078b2e..b5d8f372e 100644 --- a/server/src/timecontrol.c +++ b/server/src/timecontrol.c @@ -198,13 +198,15 @@ void timecontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { if (GPSTime->GPSMillisecondsU64 < INT64_MAX) { struct timespec newSystemTime; + TimeSetToGPSms(&tv, (int64_t) GPSTime->GPSMillisecondsU64); newSystemTime.tv_sec = tv.tv_sec; newSystemTime.tv_nsec = tv.tv_usec * 1000; if (clock_settime(CLOCK_REALTIME, &newSystemTime) == -1) { switch (errno) { case EPERM: - LogMessage(LOG_LEVEL_ERROR, "Unable to set system time - ensure this program has the correct capabilities"); + LogMessage(LOG_LEVEL_ERROR, + "Unable to set system time - ensure this program has the correct capabilities"); break; case EINVAL: LogMessage(LOG_LEVEL_ERROR, "Clock type not supported on this system"); @@ -216,7 +218,8 @@ void timecontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { } } else { - LogMessage(LOG_LEVEL_ERROR, "Current GPS time exceeds limit and would be interpreted as negative"); + LogMessage(LOG_LEVEL_ERROR, + "Current GPS time exceeds limit and would be interpreted as negative"); } } } From 553ba2d645daf19b3f8f4c990233f6ad1de0d7cb Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 13 Feb 2020 16:32:11 +0100 Subject: [PATCH 258/523] Removed unused send UDP data function --- server/src/objectcontrol.c | 23 ----------------------- 1 file changed, 23 deletions(-) diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index d9dc1cdc4..69792bda8 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -108,7 +108,6 @@ static int iGetObjectIndexFromObjectIP(in_addr_t ipAddr, in_addr_t objectIPs[], static void signalHandler(int signo); static ssize_t ObjectControlSendTRAJMessage(const char * Filename, int * Socket, const char debug); -int ObjectControlSendUDPData(int *sockfd, struct sockaddr_in *addr, char *SendData, int Length, char debug); static int iFindObjectsInfo(C8 object_traj_file[MAX_OBJECTS][MAX_FILE_PATH], C8 object_address_name[MAX_OBJECTS][MAX_FILE_PATH], @@ -1279,28 +1278,6 @@ static I32 vCheckRemoteDisconnected(int *sockfd) { return 1; } -int ObjectControlSendUDPData(int *sockfd, struct sockaddr_in *addr, char *SendData, int Length, char debug) { - ssize_t result; - - // TODO: Change to log write when bytes thingy has been implemented - if (debug) { - printf("Bytes sent: "); - int i = 0; - - for (i = 0; i < Length; i++) - printf("%x-", (unsigned char)*(SendData + i)); - printf("\n"); - } - - result = sendto(*sockfd, SendData, Length, 0, (const struct sockaddr *)addr, sizeof (struct sockaddr_in)); - - if (result < 0) { - util_error("ERR: Failed to send on monitor socket"); - } - - return 0; -} - size_t uiRecvMonitor(int *sockfd, char *buffer, size_t length) { ssize_t result = 0; From 1b4586d6ebe1b368cde23892ad2a97aec4175920 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 13 Feb 2020 16:32:28 +0100 Subject: [PATCH 259/523] Disabled annoying debug printout --- server/src/objectcontrol.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 69792bda8..87e596b6e 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -1074,7 +1074,7 @@ ssize_t ObjectControlSendTRAJMessage(const char * Filename, int * Socket, const if (errno == ENOBUFS) { // Reached the end of buffer, send buffered data and // try again - UtilSendTCPData(MODULE_NAME, messageBuffer, messageBufferPosition - messageBuffer, Socket, 1); + UtilSendTCPData(MODULE_NAME, messageBuffer, messageBufferPosition - messageBuffer, Socket, debug); messageBufferPosition = messageBuffer; remainingBufferSpace = sizeof (messageBuffer); From 08c183017d35aeea2f6fbc78a87b661db013a732 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 13 Feb 2020 17:02:01 +0100 Subject: [PATCH 260/523] Added an isValid field separate for longitudinal and lateral speed and acceleration --- modules/ScenarioControl/src/scenario.cpp | 2 +- server/inc/positioning.h | 6 ++- server/src/iso22133.c | 60 +++++++++--------------- server/src/objectcontrol.c | 6 ++- server/src/positioning.c | 45 +++++++++--------- 5 files changed, 53 insertions(+), 66 deletions(-) diff --git a/modules/ScenarioControl/src/scenario.cpp b/modules/ScenarioControl/src/scenario.cpp index 1a4abfe0c..cad299a6d 100644 --- a/modules/ScenarioControl/src/scenario.cpp +++ b/modules/ScenarioControl/src/scenario.cpp @@ -433,7 +433,7 @@ Scenario::ScenarioReturnCode_t Scenario::updateTrigger(const MonitorDataType &mo switch (tp->getTypeCode()) { case Trigger::TriggerTypeCode_t::TRIGGER_BRAKE: - if (monr.data.speed.isValid && monr.data.isTimestampValid) + if (monr.data.speed.isLongitudinalValid && monr.data.isTimestampValid) { tp->update(monr.data.speed.longitudinal_m_s, monr.data.timestamp); } diff --git a/server/inc/positioning.h b/server/inc/positioning.h index 9cc965eec..8874cd995 100644 --- a/server/inc/positioning.h +++ b/server/inc/positioning.h @@ -23,14 +23,16 @@ typedef struct { typedef struct { double longitudinal_m_s; double lateral_m_s; - bool isValid; + bool isLongitudinalValid; + bool isLateralValid; } SpeedType; /*! Struct describing longitudinal and lateral acceleration of an object */ typedef struct { double longitudinal_m_s2; double lateral_m_s2; - bool isValid; + bool isLongitudinalValid; + bool isLateralValid; } AccelerationType; /*! Enumeration of object driving direction descriptions */ diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 8a5e1ea82..6da1f35ba 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -834,38 +834,31 @@ ssize_t encodeTRAJMessagePoint(const struct timeval * pointTimeFromStart, const * 180.0 / M_PI * HEADING_ONE_DEGREE_VALUE); } else { - errno = EINVAL; - LogMessage(LOG_LEVEL_ERROR, "Heading is a required field in TRAJ messages"); - return -1; + TRAJData.heading = HEADING_UNAVAILABLE_VALUE; } TRAJData.longitudinalSpeedValueID = VALUE_ID_TRAJ_LONGITUDINAL_SPEED; TRAJData.longitudinalSpeedContentLength = sizeof (TRAJData.longitudinalSpeed); TRAJData.lateralSpeedValueID = VALUE_ID_TRAJ_LATERAL_SPEED; TRAJData.lateralSpeedContentLength = sizeof (TRAJData.lateralSpeed); - if (speed.isValid) { + if (speed.isLongitudinalValid) { TRAJData.longitudinalSpeed = (int16_t) (speed.longitudinal_m_s * SPEED_ONE_METER_PER_SECOND_VALUE); - TRAJData.lateralSpeed = (int16_t) (speed.lateral_m_s * SPEED_ONE_METER_PER_SECOND_VALUE); } else { - LogMessage(LOG_LEVEL_WARNING, "TRAJ speed supplied not valid: assuming longitudinal still valid"); - TRAJData.longitudinalSpeed = (int16_t) (speed.longitudinal_m_s * SPEED_ONE_METER_PER_SECOND_VALUE); - TRAJData.lateralSpeed = SPEED_UNAVAILABLE_VALUE; + errno = EINVAL; + LogMessage(LOG_LEVEL_ERROR, "Longitudinal speed is a required field in TRAJ messages"); + return -1; } + TRAJData.lateralSpeed = speed.isLateralValid ? (int16_t) (speed.lateral_m_s * SPEED_ONE_METER_PER_SECOND_VALUE) : SPEED_UNAVAILABLE_VALUE; TRAJData.longitudinalAccelerationValueID = VALUE_ID_TRAJ_LONGITUDINAL_ACCELERATION; TRAJData.longitudinalAccelerationContentLength = sizeof (TRAJData.longitudinalAcceleration); TRAJData.lateralAccelerationValueID = VALUE_ID_TRAJ_LATERAL_ACCELERATION; TRAJData.lateralAccelerationContentLength = sizeof (TRAJData.lateralAcceleration); - if (acceleration.isValid) { - TRAJData.longitudinalAcceleration = (int16_t) (acceleration.longitudinal_m_s2 * ACCELERATION_ONE_METER_PER_SECOND_SQUARED_VALUE); - TRAJData.lateralAcceleration = (int16_t) (acceleration.lateral_m_s2 * ACCELERATION_ONE_METER_PER_SECOND_SQUARED_VALUE); - } - else { - LogMessage(LOG_LEVEL_WARNING, "TRAJ acceleration supplied not valid: assuming longitudinal still valid"); - TRAJData.longitudinalAcceleration = (int16_t) (acceleration.longitudinal_m_s2 * ACCELERATION_ONE_METER_PER_SECOND_SQUARED_VALUE); - TRAJData.lateralAcceleration = ACCELERATION_UNAVAILABLE_VALUE; - } + TRAJData.longitudinalAcceleration = acceleration.isLongitudinalValid ? + (int16_t) (acceleration.longitudinal_m_s2 * ACCELERATION_ONE_METER_PER_SECOND_SQUARED_VALUE) : ACCELERATION_UNAVAILABLE_VALUE; + TRAJData.lateralAcceleration = acceleration.isLateralValid ? + (int16_t) (acceleration.lateral_m_s2 * ACCELERATION_ONE_METER_PER_SECOND_SQUARED_VALUE) : ACCELERATION_UNAVAILABLE_VALUE; TRAJData.curvatureValueID = VALUE_ID_TRAJ_CURVATURE; TRAJData.curvatureContentLength = sizeof (TRAJData.curvature); @@ -1514,29 +1507,20 @@ void convertMONRToHostRepresentation(const MONRType * MONRData, ObjectMonitorTyp } // Velocity - monitorData->speed.isValid = true; - if (MONRData->longitudinalSpeed == SPEED_UNAVAILABLE_VALUE) - monitorData->speed.isValid = false; - else - monitorData->speed.longitudinal_m_s = - (double)(MONRData->longitudinalSpeed) / SPEED_ONE_METER_PER_SECOND_VALUE; - if (MONRData->lateralSpeed == SPEED_UNAVAILABLE_VALUE) - monitorData->speed.isValid = false; - else - monitorData->speed.lateral_m_s = (double)(MONRData->lateralSpeed) / SPEED_ONE_METER_PER_SECOND_VALUE; + monitorData->speed.isLongitudinalValid = MONRData->longitudinalSpeed != SPEED_UNAVAILABLE_VALUE; + monitorData->speed.longitudinal_m_s = monitorData->speed.isLongitudinalValid ? + (double) (MONRData->longitudinalSpeed) / SPEED_ONE_METER_PER_SECOND_VALUE : 0; + monitorData->speed.isLateralValid = MONRData->lateralSpeed != SPEED_UNAVAILABLE_VALUE; + monitorData->speed.lateral_m_s = monitorData->speed.isLateralValid ? + (double) (MONRData->lateralSpeed) / SPEED_ONE_METER_PER_SECOND_VALUE : 0; // Acceleration - monitorData->acceleration.isValid = true; - if (MONRData->longitudinalAcc == ACCELERATION_UNAVAILABLE_VALUE) - monitorData->acceleration.isValid = false; - else - monitorData->acceleration.longitudinal_m_s2 = - (double)(MONRData->longitudinalAcc) / ACCELERATION_ONE_METER_PER_SECOND_SQUARED_VALUE; - if (MONRData->lateralAcc == ACCELERATION_UNAVAILABLE_VALUE) - monitorData->acceleration.isValid = false; - else - monitorData->acceleration.lateral_m_s2 = - (double)(MONRData->lateralAcc) / ACCELERATION_ONE_METER_PER_SECOND_SQUARED_VALUE; + monitorData->acceleration.isLongitudinalValid = MONRData->longitudinalAcc != ACCELERATION_UNAVAILABLE_VALUE; + monitorData->acceleration.longitudinal_m_s2 = monitorData->acceleration.isLongitudinalValid ? + (double) (MONRData->longitudinalAcc) / ACCELERATION_ONE_METER_PER_SECOND_SQUARED_VALUE : 0; + monitorData->acceleration.isLateralValid = MONRData->lateralAcc != ACCELERATION_UNAVAILABLE_VALUE; + monitorData->acceleration.lateral_m_s2 = monitorData->acceleration.isLateralValid ? + (double) (MONRData->lateralAcc) / ACCELERATION_ONE_METER_PER_SECOND_SQUARED_VALUE : 0; // Drive direction switch (MONRData->driveDirection) { diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 87e596b6e..7a82a61b6 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -1059,10 +1059,12 @@ ssize_t ObjectControlSendTRAJMessage(const char * Filename, int * Socket, const position.zCoord_m = position.isPositionValid ? *fileLine.zCoord : 0; position.heading_rad = fileLine.heading; position.isHeadingValid = true; - speed.isValid = fileLine.longitudinalVelocity != NULL && fileLine.lateralVelocity != NULL; + speed.isLongitudinalValid = fileLine.longitudinalVelocity != NULL; + speed.isLateralValid = fileLine.lateralVelocity != NULL; speed.longitudinal_m_s = fileLine.longitudinalVelocity != NULL ? *fileLine.longitudinalVelocity : 0; speed.lateral_m_s = fileLine.lateralVelocity != NULL ? *fileLine.lateralVelocity : 0; - acceleration.isValid = fileLine.longitudinalAcceleration != NULL && fileLine.lateralAcceleration != NULL; + acceleration.isLongitudinalValid = fileLine.longitudinalAcceleration != NULL; + acceleration.isLateralValid = fileLine.lateralAcceleration != NULL; acceleration.longitudinal_m_s2 = fileLine.longitudinalAcceleration != NULL ? *fileLine.longitudinalAcceleration : 0; acceleration.lateral_m_s2 = fileLine.lateralAcceleration != NULL ? *fileLine.lateralAcceleration : 0; diff --git a/server/src/positioning.c b/server/src/positioning.c index 3c24b54b2..9fb3f1e12 100644 --- a/server/src/positioning.c +++ b/server/src/positioning.c @@ -190,18 +190,29 @@ int objectMonitorDataToASCII(const ObjectMonitorType * monitorData, char *asciiB else snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), "NaN;"); - if (monitorData->speed.isValid) + if (monitorData->speed.isLongitudinalValid) snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), - "%.2f;%.2f;", monitorData->speed.longitudinal_m_s, monitorData->speed.lateral_m_s); + "%.2f;", monitorData->speed.longitudinal_m_s); else - snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), "NaN;NaN;"); + snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), "NaN;"); + + if (monitorData->speed.isLateralValid) + snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), + "%.2f;", monitorData->speed.lateral_m_s); + else + snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), "NaN;"); - if (monitorData->acceleration.isValid) + if (monitorData->acceleration.isLongitudinalValid) snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), - "%.3f;%.3f;", monitorData->acceleration.longitudinal_m_s2, - monitorData->acceleration.lateral_m_s2); + "%.3f;", monitorData->acceleration.longitudinal_m_s2); else - snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), "NaN;NaN;"); + snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), "NaN;"); + + if (monitorData->acceleration.isLateralValid) + snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), + "%.3f;", monitorData->acceleration.lateral_m_s2); + else + snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), "NaN;"); if (monitorData->drivingDirection != OBJECT_DRIVE_DIRECTION_UNAVAILABLE) snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), @@ -289,34 +300,22 @@ int ASCIIToObjectMonitorData(const char *asciiBuffer, ObjectMonitorType * monito } // Velocity - monitorData->speed.isValid = true; - token = strtok(NULL, delim); monitorData->speed.longitudinal_m_s = strtod(token, &endPtr); - if (endPtr == token) { - monitorData->speed.isValid = false; - } + monitorData->speed.isLongitudinalValid = endPtr != token; token = strtok(NULL, delim); monitorData->speed.lateral_m_s = strtod(token, &endPtr); - if (endPtr == token) { - monitorData->speed.isValid = false; - } + monitorData->speed.isLateralValid = endPtr != token; // Acceleration - monitorData->acceleration.isValid = true; - token = strtok(NULL, delim); monitorData->acceleration.longitudinal_m_s2 = strtod(token, &endPtr); - if (endPtr == token) { - monitorData->acceleration.isValid = false; - } + monitorData->acceleration.isLongitudinalValid = endPtr != token; token = strtok(NULL, delim); monitorData->acceleration.lateral_m_s2 = strtod(token, &endPtr); - if (endPtr == token) { - monitorData->acceleration.isValid = false; - } + monitorData->acceleration.isLateralValid = endPtr != token; // Drive direction token = strtok(NULL, delim); From fd0207d3ab99d26d0ee87985bc3f752b39427b36 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 13 Feb 2020 17:03:55 +0100 Subject: [PATCH 261/523] Removed debug printout --- server/src/objectcontrol.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 7a82a61b6..f0762f59e 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -779,8 +779,8 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { objectControlServerStatus = CONTROL_CENTER_STATUS_ABORT; } LogPrint("OSEM msglen: %ld", MessageLength); - UtilSendTCPData("Object Control", MessageBuffer, MessageLength, &socket_fds[iIndex], - 1); + UtilSendTCPData(MODULE_NAME, MessageBuffer, MessageLength, &socket_fds[iIndex], + 0); /* Here we send TRAJ, if the IP-address is not operating with a dynamic trajectory */ if (strstr(DTMReceivers, object_address_name[iIndex]) == NULL) { From 7b670e36489496b87102e94947abd891b7925f59 Mon Sep 17 00:00:00 2001 From: alfaro01 Date: Fri, 14 Feb 2020 10:29:09 +0100 Subject: [PATCH 262/523] Changed location for MSCPTool so Jenkins won't bug it --- server/integration-tests/{ => manual tests}/MSCPManualTest.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename server/integration-tests/{ => manual tests}/MSCPManualTest.py (100%) diff --git a/server/integration-tests/MSCPManualTest.py b/server/integration-tests/manual tests/MSCPManualTest.py similarity index 100% rename from server/integration-tests/MSCPManualTest.py rename to server/integration-tests/manual tests/MSCPManualTest.py From e72f467c502fcf37c2781c212db19c795a72f011 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 14 Feb 2020 16:06:07 +0100 Subject: [PATCH 263/523] Loop over data --- server/integration-tests/tools/MSCP.py | 217 +++++++++++++------------ 1 file changed, 110 insertions(+), 107 deletions(-) diff --git a/server/integration-tests/tools/MSCP.py b/server/integration-tests/tools/MSCP.py index efd87d0fc..bc0233ac9 100644 --- a/server/integration-tests/tools/MSCP.py +++ b/server/integration-tests/tools/MSCP.py @@ -53,119 +53,122 @@ def listen(self): if not self.quit: raise e - if len(data) > 0: + while len(data) > 0: print("=== Received " + str(len(data)) + " bytes:") print("=== " + str(data)) - for replyPattern in replyPatterns: - match = re.search(replyPattern["regex"],data) - if match is not None: - matchPattern = replyPattern - self.responseCodeLock.acquire() - self.lastResponseCode = self.interpretResponseCode(match.group(1)) - self.responseCodeLock.release() - break - if match is not None: - if matchPattern["command"] == "init": - print("=== Init reply received") - if matchPattern["command"] == "status": - print("=== Status reply received") - num = int.from_bytes(match.group(2),byteorder='big') - self.statusReplyLock.acquire() - if num == 1: - self.lastStatusReply["systemControlState"] = "INITIALIZED" - elif num == 2: - self.lastStatusReply["systemControlState"] = "IDLE" - elif num == 5: - self.lastStatusReply["systemControlState"] = "INWORK" - elif num == 6: - self.lastStatusReply["systemControlState"] = "ERROR" + for replyPattern in replyPatterns: + match = re.search(replyPattern["regex"],data) + if match is not None: + matchPattern = replyPattern + self.responseCodeLock.acquire() + self.lastResponseCode = self.interpretResponseCode(match.group(1)) + self.responseCodeLock.release() + data = data[match.end():] + break else: - self.lastStatusReply["systemControlState"] = "UNKNOWN" + print("=== Unable to match against data") + data = [] + if match is not None: + if matchPattern["command"] == "init": + print("=== Init reply received") + if matchPattern["command"] == "status": + print("=== Status reply received") + num = int.from_bytes(match.group(2),byteorder='big') + self.statusReplyLock.acquire() + if num == 1: + self.lastStatusReply["systemControlState"] = "INITIALIZED" + elif num == 2: + self.lastStatusReply["systemControlState"] = "IDLE" + elif num == 5: + self.lastStatusReply["systemControlState"] = "INWORK" + elif num == 6: + self.lastStatusReply["systemControlState"] = "ERROR" + else: + self.lastStatusReply["systemControlState"] = "UNKNOWN" - num = int.from_bytes(match.group(3),byteorder='big') - if num == 1: - self.lastStatusReply["objectControlState"] = "IDLE" - elif num == 2: - self.lastStatusReply["objectControlState"] = "INITIALIZED" - elif num == 3: - self.lastStatusReply["objectControlState"] = "CONNECTED" - elif num == 4: - self.lastStatusReply["objectControlState"] = "ARMED" - elif num == 5: - self.lastStatusReply["objectControlState"] = "RUNNING" - elif num == 6: - self.lastStatusReply["objectControlState"] = "ERROR" - else: - self.lastStatusReply["objectControlState"] = "UNKNOWN" + num = int.from_bytes(match.group(3),byteorder='big') + if num == 1: + self.lastStatusReply["objectControlState"] = "IDLE" + elif num == 2: + self.lastStatusReply["objectControlState"] = "INITIALIZED" + elif num == 3: + self.lastStatusReply["objectControlState"] = "CONNECTED" + elif num == 4: + self.lastStatusReply["objectControlState"] = "ARMED" + elif num == 5: + self.lastStatusReply["objectControlState"] = "RUNNING" + elif num == 6: + self.lastStatusReply["objectControlState"] = "ERROR" + else: + self.lastStatusReply["objectControlState"] = "UNKNOWN" + + if match.group(4) is not None: + self.lastStatusReply["systemControlErrorCode"] = int.from_bytes(match.group(4),byteorder='big') + else: + self.lastStatusReply["systemControlErrorCode"] = 0 + if match.group(5) is not None: + self.lastStatusReply["objectControlErrorCode"] = int.from_bytes(match.group(5),byteorder='big') + else: + self.lastStatusReply["objectControlErrorCode"] = 0 + self.statusReplyLock.release() + if matchPattern["command"] == "abort": + print("=== Abort reply received") + num = int.from_bytes(match.group(2),byteorder='big') + self.abortReplyLock.acquire() + if num == 0: + self.lastAbortReply["scenarioActive"] = "NOT_ACTIVE" + elif num == 1: + self.lastAbortReply["scenarioActive"] = "ACTIVE" + else: + self.lastAbortReply["scenarioActive"] = "UNKNOWN" + self.abortReplyLock.release() + if matchPattern["command"] == "arm": + print("=== Arm reply received") + if matchPattern["command"] == "start": + print("=== Start reply received") + num = int.from_bytes(match.group(2),byteorder='big') + self.startReplyLock.acquire() + if num == 0: + self.lastStartReply["scenarioActive"] = "NOT_ACTIVE" + elif num == 1: + self.lastStartReply["scenarioActive"] = "ACTIVE" + else: + self.lastStartReply["scenarioActive"] = "UNKNOWN" + self.startReplyLock.release() + if matchPattern["command"] == "connect": + print("=== Connect reply received") + if matchPattern["command"] == "disconnect": + print("=== Disconnect reply received") + if matchPattern["command"] == "upload": + print("=== Upload reply 1 received") + num = int.from_bytes(match.group(2),byteorder='big') + self.uploadReplyLock.acquire() + if num == 0x01: + self.lastUploadReply["status"] = "SERVER_PREPARED" + elif num == 0x02: + self.lastUploadReply["status"] = "PACKET_SIZE_ERROR" + elif num == 0x03: + self.lastUploadReply["status"] = "INVALID_PATH" + elif num == 0x04: + self.lastUploadReply["status"] = "UPLOAD_SUCCESS" + elif num == 0x05: + self.lastUploadReply["status"] = "UPLOAD_FAILURE" + else: + self.lastUploadReply["status"] = "UNKNOWN" + self.uploadReplyLock.release() + if matchPattern["command"] == "subupload": + print("=== Upload reply 2 received") + num = int.from_bytes(match.group(2),byteorder='big') + self.uploadReplyLock.acquire() + if num == 0x04: + self.lastUploadReply["status"] = "UPLOAD_SUCCESS" + elif num == 0x05: + self.lastUploadReply["status"] = "UPLOAD_FAILURE" + else: + self.lastUploadReply["status"] = "UNKNOWN" + self.uploadReplyLock.release() - if match.group(4) is not None: - self.lastStatusReply["systemControlErrorCode"] = int.from_bytes(match.group(4),byteorder='big') - else: - self.lastStatusReply["systemControlErrorCode"] = 0 - if match.group(5) is not None: - self.lastStatusReply["objectControlErrorCode"] = int.from_bytes(match.group(5),byteorder='big') - else: - self.lastStatusReply["objectControlErrorCode"] = 0 - self.statusReplyLock.release() - if matchPattern["command"] == "abort": - print("=== Abort reply received") - num = int.from_bytes(match.group(2),byteorder='big') - self.abortReplyLock.acquire() - if num == 0: - self.lastAbortReply["scenarioActive"] = "NOT_ACTIVE" - elif num == 1: - self.lastAbortReply["scenarioActive"] = "ACTIVE" - else: - self.lastAbortReply["scenarioActive"] = "UNKNOWN" - self.abortReplyLock.release() - if matchPattern["command"] == "arm": - print("=== Arm reply received") - if matchPattern["command"] == "start": - print("=== Start reply received") - num = int.from_bytes(match.group(2),byteorder='big') - self.startReplyLock.acquire() - if num == 0: - self.lastStartReply["scenarioActive"] = "NOT_ACTIVE" - elif num == 1: - self.lastStartReply["scenarioActive"] = "ACTIVE" - else: - self.lastStartReply["scenarioActive"] = "UNKNOWN" - self.startReplyLock.release() - if matchPattern["command"] == "connect": - print("=== Connect reply received") - if matchPattern["command"] == "disconnect": - print("=== Disconnect reply received") - if matchPattern["command"] == "upload": - print("=== Upload reply 1 received") - num = int.from_bytes(match.group(2),byteorder='big') - self.uploadReplyLock.acquire() - if num == 0x01: - self.lastUploadReply["status"] = "SERVER_PREPARED" - elif num == 0x02: - self.lastUploadReply["status"] = "PACKET_SIZE_ERROR" - elif num == 0x03: - self.lastUploadReply["status"] = "INVALID_PATH" - elif num == 0x04: - self.lastUploadReply["status"] = "UPLOAD_SUCCESS" - elif num == 0x05: - self.lastUploadReply["status"] = "UPLOAD_FAILURE" - else: - self.lastUploadReply["status"] = "UNKNOWN" - self.uploadReplyLock.release() - if matchPattern["command"] == "subupload": - print("=== Upload reply 2 received") - num = int.from_bytes(match.group(2),byteorder='big') - self.uploadReplyLock.acquire() - if num == 0x04: - self.lastUploadReply["status"] = "UPLOAD_SUCCESS" - elif num == 0x05: - self.lastUploadReply["status"] = "UPLOAD_FAILURE" - else: - self.lastUploadReply["status"] = "UNKNOWN" - self.uploadReplyLock.release() - elif len(data) > 0: - print("=== Unable to match against data: " + str(data)) def GetStatus(self): message = "POST /maestro HTTP/1.1\r\nHost: " + self.host + "\r\n\r\nGetServerStatus();\r\n\r\n" From 3184b60dcd2f387d129c63693c91ecc13afcdc02 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 14 Feb 2020 16:28:38 +0100 Subject: [PATCH 264/523] Made received data into bytearray --- server/integration-tests/tools/MSCP.py | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/server/integration-tests/tools/MSCP.py b/server/integration-tests/tools/MSCP.py index bc0233ac9..f4ca4a1f2 100644 --- a/server/integration-tests/tools/MSCP.py +++ b/server/integration-tests/tools/MSCP.py @@ -48,7 +48,8 @@ def listen(self): print("=== Starting listener on " + str(self.host) + ":" + str(self.port)) while not self.quit: try: - data = self.socket.recv(2048) + dat = self.socket.recv(2048) + data = bytearray(dat) except ConnectionResetError as e: if not self.quit: raise e @@ -56,7 +57,7 @@ def listen(self): while len(data) > 0: print("=== Received " + str(len(data)) + " bytes:") print("=== " + str(data)) - + for replyPattern in replyPatterns: match = re.search(replyPattern["regex"],data) if match is not None: @@ -66,9 +67,6 @@ def listen(self): self.responseCodeLock.release() data = data[match.end():] break - else: - print("=== Unable to match against data") - data = [] if match is not None: if matchPattern["command"] == "init": print("=== Init reply received") @@ -168,6 +166,9 @@ def listen(self): else: self.lastUploadReply["status"] = "UNKNOWN" self.uploadReplyLock.release() + else: + print("=== Unable to match against data") + data = bytearray([]) def GetStatus(self): From 23d9b7125c635eac3041b3ee8e14ffed2f50a858 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 18 Feb 2020 14:25:41 +0100 Subject: [PATCH 265/523] WIP --- CMakeLists.txt | 6 +++ modules/ScenarioControl/CMakeLists.txt | 65 ++++++++++++----------- server/CMakeLists.txt | 72 +++++++++++++------------- 3 files changed, 76 insertions(+), 67 deletions(-) create mode 100644 CMakeLists.txt diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 000000000..cd4894586 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,6 @@ +cmake_minimum_required (VERSION 3.1) + +add_subdirectory(util/C) + +add_subdirectory(server) +add_subdirectory(modules/ScenarioControl) diff --git a/modules/ScenarioControl/CMakeLists.txt b/modules/ScenarioControl/CMakeLists.txt index 12b38bb57..444fe2b4e 100644 --- a/modules/ScenarioControl/CMakeLists.txt +++ b/modules/ScenarioControl/CMakeLists.txt @@ -10,38 +10,43 @@ project(ScenarioControl) include_directories(inc) include_directories(src) -include_directories(../../util/C/logging) -include_directories(../../util/C/time) -include_directories(../../util/C/MQBus) -include_directories(../../server/inc) include(GNUInstallDirs) - -# Create library -add_library(MaestroLogging - ../../util/C/logging/logging.h - ../../util/C/logging/logging.c -) - -add_library(MaestroTime - ../../util/C/time/maestroTime.h - ../../util/C/time/maestroTime.c -) - -add_library(MQBus - ../../util/C/MQBus/mqbus.h - ../../util/C/MQBus/mqbus.c -) - -# Create library -add_library(util - ../../server/src/util.c - ../../server/inc/util.h - ../../server/src/iso22133.c - ../../server/inc/iso22133.h - ../../server/src/positioning.c - ../../server/inc/positioning.h -) +if (NOT TARGET MaestroLogging) + include_directories(../../util/C/logging) + add_library(MaestroLogging + ../util/C/logging/logging.h + ../util/C/logging/logging.c + ) +endif() + +if (NOT TARGET MaestroTime) + include_directories(../../util/C/time) + add_library(MaestroTime + ../util/C/time/maestroTime.h + ../util/C/time/maestroTime.c + ) +endif() + +if (NOT TARGET MQBus) + include_directories(../../util/C/MQBus) + add_library(MQBus + ../util/C/MQBus/mqbus.c + ../util/C/MQBus/mqbus.h + ) +endif() + +if (NOT TARGET util) + include_directories(../../server/inc) + add_library(util + ../../server/src/util.c + ../../server/inc/util.h + ../../server/src/iso22133.c + ../../server/inc/iso22133.h + ../../server/src/positioning.c + ../../server/inc/positioning.h + ) +endif() add_library(Trigger inc/trigger.h diff --git a/server/CMakeLists.txt b/server/CMakeLists.txt index a239c38f4..eb9be870a 100644 --- a/server/CMakeLists.txt +++ b/server/CMakeLists.txt @@ -22,11 +22,41 @@ SET(GCC_COMPILE_FLAGS_ALL "${GCC_COMPILE_FLAGS_WARNINGS}") # add include directories include_directories(inc) -include_directories(../util/C/logging) -include_directories(../util/C/time) -include_directories(../util/C/MQBus) -include_directories(../util/ASN1/generatedfiles) include(GNUInstallDirs) +if (NOT TARGET MaestroLogging) + include_directories(../util/C/logging) + add_library(MaestroLogging + ../util/C/logging/logging.h + ../util/C/logging/logging.c + ) +endif() + +if (NOT TARGET MaestroTime) + include_directories(../util/C/time) + add_library(MaestroTime + ../util/C/time/maestroTime.h + ../util/C/time/maestroTime.c + ) +endif() + +if (NOT TARGET MQBus) + include_directories(../util/C/MQBus) + add_library(MQBus + ../util/C/MQBus/mqbus.c + ../util/C/MQBus/mqbus.h + ) +endif() + +if (NOT TARGET util) + add_library(util + inc/util.h + src/util.c + inc/iso22133.h + src/iso22133.c + inc/positioning.h + src/positioning.c + ) +endif() # Add logging library (hint at some locations) #find_library(MAESTRO_LOGGING_PATH @@ -43,6 +73,7 @@ SET(USE_CITS FALSE CACHE BOOL "Flag to indicate the use of CITS.") # MQTT PAHO if(USE_CITS) + include_directories(../util/ASN1/generatedfiles) find_package(OpenSSL REQUIRED) find_library(paho-mqtt3c NAMES libpaho-mqtt3c.so REQUIRED) add_library(pahomqtt3c SHARED IMPORTED) @@ -61,39 +92,6 @@ add_definitions(-DCITS_ENABLED) endif() -# Create library -add_library(MaestroLogging - ../util/C/logging/logging.h - ../util/C/logging/logging.c -) - -add_library(MaestroTime - ../util/C/time/maestroTime.h - ../util/C/time/maestroTime.c -) - -add_library(MQBus - ../util/C/MQBus/mqbus.h - ../util/C/MQBus/mqbus.c -) - -# Create library -add_library(util - inc/util.h - src/util.c - inc/iso22133.h - src/iso22133.c - inc/positioning.h - src/positioning.c -) - -# Create library -add_library(objctrl - src/objectcontrol.c - inc/objectcontrol.h -) - - # add the executable add_executable(TEServer src/main.c From 3fcf3f919ee7d9f46fd5314a9a1c14b395d7acd6 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 18 Feb 2020 14:26:59 +0100 Subject: [PATCH 266/523] Ran code formatter --- server/src/objectcontrol.c | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 30920ebdd..e0af87a90 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -304,11 +304,10 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { objectControlServerStatus = CONTROL_CENTER_STATUS_ABORT; MessageLength = encodeHEABMessage(objectControlServerStatus, MessageBuffer, sizeof (MessageBuffer), 0); - for(iIndex = 0; iIndex < nbr_objects; ++iIndex) - { - UtilSendUDPData("Object Control", &safety_socket_fd[iIndex], &safety_object_addr[iIndex], - MessageBuffer, MessageLength, 0); - } + for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { + UtilSendUDPData("Object Control", &safety_socket_fd[iIndex], &safety_object_addr[iIndex], + MessageBuffer, MessageLength, 0); + } } // Heartbeat From 599cfa83fdc031e927a5fa7ff2caa42f0f6b4d56 Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Wed, 19 Feb 2020 09:05:52 +0100 Subject: [PATCH 267/523] merged with dev, fixed conflicts --- server/src/timecontrol.c | 47 +++++++++++++++++++++++++++++++--------- 1 file changed, 37 insertions(+), 10 deletions(-) diff --git a/server/src/timecontrol.c b/server/src/timecontrol.c index 5b48a07d4..7c8a4a673 100644 --- a/server/src/timecontrol.c +++ b/server/src/timecontrol.c @@ -89,9 +89,9 @@ void timecontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { U16 ServerPortU16; I32 SocketfdI32 = -1; struct sockaddr_in time_addr; - const struct timespec mqEmptyPollPeriod = { TC_SLEEP_TIME_EMPTY_MQ_S, TC_SLEEP_TIME_EMPTY_MQ_NS }; - const struct timespec mqNonEmptyPollPeriod = - { TC_SLEEP_TIME_NONEMPTY_MQ_S, TC_SLEEP_TIME_NONEMPTY_MQ_NS }; + const struct timespec mqEmptyPollPeriod = { TC_SLEEP_TIME_EMPTY_MQ_S, TC_SLEEP_TIME_EMPTY_MQ_NS }; + const struct timespec mqNonEmptyPollPeriod = + { TC_SLEEP_TIME_NONEMPTY_MQ_S, TC_SLEEP_TIME_NONEMPTY_MQ_NS }; I32 result; C8 TimeBuffer[TIME_CONTROL_RECEIVE_BUFFER_SIZE]; @@ -173,9 +173,9 @@ void timecontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { while (!iExit) { // Ignore any commands received, just empty the bus - do { - iCommRecv(&command, busReceiveBuffer, sizeof (busReceiveBuffer), NULL); - } while (command != COMM_INV); + do { + iCommRecv(&command, busReceiveBuffer, sizeof (busReceiveBuffer), NULL); + } while (command != COMM_INV); gettimeofday(&ExecTime, NULL); CurrentMilliSecondU16 = (U16) (ExecTime.tv_usec / 1000); @@ -193,8 +193,35 @@ void timecontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { bzero(TimeBuffer, TIME_CONTROL_RECEIVE_BUFFER_SIZE); TimeControlRecvTime(&SocketfdI32, TimeBuffer, TIME_CONTROL_RECEIVE_BUFFER_SIZE, &ReceivedNewData); - if (ReceivedNewData) + if (ReceivedNewData) { TimeControlDecodeTimeBuffer(GPSTime, TimeBuffer, 0); + + if (GPSTime->GPSMillisecondsU64 < INT64_MAX) { + struct timespec newSystemTime; + + TimeSetToGPSms(&tv, (int64_t) GPSTime->GPSMillisecondsU64); + newSystemTime.tv_sec = tv.tv_sec; + newSystemTime.tv_nsec = tv.tv_usec * 1000; + if (clock_settime(CLOCK_REALTIME, &newSystemTime) == -1) { + switch (errno) { + case EPERM: + LogMessage(LOG_LEVEL_ERROR, + "Unable to set system time - ensure this program has the correct capabilities"); + break; + case EINVAL: + LogMessage(LOG_LEVEL_ERROR, "Clock type not supported on this system"); + break; + default: + LogMessage(LOG_LEVEL_ERROR, "Error setting system time"); + break; + } + } + } + else { + LogMessage(LOG_LEVEL_ERROR, + "Current GPS time exceeds limit and would be interpreted as negative"); + } + } } else if (!GPSTime->isGPSenabled) { gettimeofday(&tv, NULL); @@ -248,9 +275,9 @@ void timecontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { } - sleep_time = command == COMM_INV ? mqEmptyPollPeriod : mqNonEmptyPollPeriod; - nanosleep(&sleep_time, &ref_time); - } + sleep_time = command == COMM_INV ? mqEmptyPollPeriod : mqNonEmptyPollPeriod; + nanosleep(&sleep_time, &ref_time); + } LogMessage(LOG_LEVEL_INFO, "Time control exiting"); } From de305883c1659b2d419f4ae284234ce6914e7f5a Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Wed, 19 Feb 2020 10:22:14 +0100 Subject: [PATCH 268/523] updated util ref --- util | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/util b/util index 5171c2cb4..b7265abf3 160000 --- a/util +++ b/util @@ -1 +1 @@ -Subproject commit 5171c2cb43b958fb4eb5b3df18a92b0e2e9e518f +Subproject commit b7265abf3715cc2abaa77c256e4b3d51f0ab8d6c From f9b18073347046a61ced17af05aa2ad5106eb002 Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Wed, 19 Feb 2020 11:11:58 +0100 Subject: [PATCH 269/523] Changed from MONRType to MonitorDataType --- server/inc/datadictionary.h | 4 ++-- server/inc/util.h | 3 +-- server/src/datadictionary.c | 11 ++++++----- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/server/inc/datadictionary.h b/server/inc/datadictionary.h index 3321b640a..1d8a23d57 100644 --- a/server/inc/datadictionary.h +++ b/server/inc/datadictionary.h @@ -123,8 +123,8 @@ OBCState_t DataDictionaryGetOBCStateU8(GSDType *GSD); ReadWriteAccess_t DataDictionaryFreeMONR(GSDType *GSD); ReadWriteAccess_t DataDictionaryInitMONR(GSDType *GSD); -ReadWriteAccess_t DataDictionarySetMONR(GSDType *GSD, const MONRType * MONR, const U32 TransmitterId); -ReadWriteAccess_t DataDictionaryGetMONR(GSDType *GSD, MONRType * MONR, const U32 TransmitterId); +ReadWriteAccess_t DataDictionarySetMONR(GSDType *GSD, const MonitorDataType * MONR, const U32 TransmitterId); +ReadWriteAccess_t DataDictionaryGetMONR(GSDType *GSD, MonitorDataType * MONR, const U32 TransmitterId); ReadWriteAccess_t DataDictionarySetNumberOfObjectsU8(GSDType * GSD, U32 *numberOfObjects); ReadWriteAccess_t DataDictionaryGetNumberOfObjectsU8(GSDType * GSD, U32 *numberOfObjects); diff --git a/server/inc/util.h b/server/inc/util.h index e2f021e45..2c5b7737f 100644 --- a/server/inc/util.h +++ b/server/inc/util.h @@ -385,14 +385,13 @@ typedef struct U8 ASPDebugDataU8[sizeof(ASPType)]; U32 SupChunkSize; U8 SupChunk[6200]; - + MonitorDataType* MonrMessages; U8 MONRSizeU8; U8 MONRData[100]; U8 HEABSizeU8; U8 HEABData[100]; U8 numberOfObjects; - MONRType *MonrMessages; char *memory; //U8 OSTMSizeU8; //U8 OSTMData[100]; diff --git a/server/src/datadictionary.c b/server/src/datadictionary.c index 0960df74c..8230ed46d 100644 --- a/server/src/datadictionary.c +++ b/server/src/datadictionary.c @@ -17,6 +17,7 @@ #include #include #include "datadictionary.h" +#include "iso22133.h" #include "logging.h" // Parameters and variables @@ -1686,14 +1687,14 @@ ReadWriteAccess_t DataDictionaryInitMONR(GSDType * GSD) { stat(filePath, &st); // this memory does not change size as more MONR messages are added, and it is unclear where in the memory stuff is being written - lseek(fd, (sizeof (MONRType)) - 1, SEEK_SET); + lseek(fd, (sizeof (MonitorDataType)) - 1, SEEK_SET); write(fd, "", 1); stat(filePath, &st); // Map memory to created file GSD->MonrMessages = - (MONRType *) mmap(NULL, (sizeof (MONRType)), PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0); + (MonitorDataType *) mmap(NULL, (sizeof (MonitorDataType)), PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0); if (GSD->MonrMessages == MAP_FAILED) { LogPrint(LOG_LEVEL_ERROR, "mmap failed: %s", strerror(errno)); @@ -1714,7 +1715,7 @@ ReadWriteAccess_t DataDictionaryInitMONR(GSDType * GSD) { * \param transmitterId requested object transmitterId * \return Result according to ::ReadWriteAccess_t */ -ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, const MONRType * MONR, const U32 transmitterId) { +ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, const MonitorDataType * MONR, const U32 transmitterId) { ReadWriteAccess_t Res; Res = WRITE_OK; @@ -1739,7 +1740,7 @@ ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, const MONRType * MONR, co * \param transmitterId requested object transmitterId * \return Result according to ::ReadWriteAccess_t */ -ReadWriteAccess_t DataDictionaryGetMONR(GSDType * GSD, MONRType * MONR, const U32 transmitterId) { +ReadWriteAccess_t DataDictionaryGetMONR(GSDType * GSD, MonitorDataType * MONR, const U32 transmitterId) { ReadWriteAccess_t Res; Res = READ_OK; @@ -1765,7 +1766,7 @@ ReadWriteAccess_t DataDictionaryFreeMONR(GSDType * GSD) { Res = WRITE_OK; pthread_mutex_lock(&MONRMutex); - int res = munmap(GSD->MonrMessages, sizeof (MONRType)); + int res = munmap(GSD->MonrMessages, sizeof (MonitorDataType)); if (res < 0) { util_error("Unable to unmap monrMessages file!"); From 94f30bc6b9b3b3e9c06f426b05b03dd4af10a394 Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Wed, 19 Feb 2020 12:21:10 +0100 Subject: [PATCH 270/523] ran code formatter --- server/src/citscontrol.c | 222 +++++++++++++++++------------------- server/src/datadictionary.c | 59 +++++----- server/src/objectcontrol.c | 9 +- server/src/systemcontrol.c | 2 +- 4 files changed, 138 insertions(+), 154 deletions(-) diff --git a/server/src/citscontrol.c b/server/src/citscontrol.c index 05f45faeb..f206e4f4c 100644 --- a/server/src/citscontrol.c +++ b/server/src/citscontrol.c @@ -645,18 +645,16 @@ CAM_t *allocateCAMStruct(void) { cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.accelerationControl = NULL; // TODO: Allocate memory for this cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.lanePosition = calloc(1, sizeof (LanePosition_t)); - cam->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.steeringWheelAngle = calloc(1, sizeof (SteeringWheelAngle_t)); - cam->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.lateralAcceleration = - calloc(1, sizeof (LateralAcceleration_t)); - cam->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.verticalAcceleration = - calloc(1, sizeof (VerticalAcceleration_t)); - cam->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.performanceClass = calloc(1, sizeof (PerformanceClass_t)); - cam->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.cenDsrcTollingZone = calloc(1, sizeof (CenDsrcTollingZone_t)); + cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + steeringWheelAngle = calloc(1, sizeof (SteeringWheelAngle_t)); + cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + lateralAcceleration = calloc(1, sizeof (LateralAcceleration_t)); + cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + verticalAcceleration = calloc(1, sizeof (VerticalAcceleration_t)); + cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + performanceClass = calloc(1, sizeof (PerformanceClass_t)); + cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + cenDsrcTollingZone = calloc(1, sizeof (CenDsrcTollingZone_t)); cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.cenDsrcTollingZone->cenDsrcTollingZoneID = NULL; // TODO: Allocate memory for this BasicVehicleContainerHighFrequency_t *bvc = @@ -700,71 +698,67 @@ void initializeCAMStruct(CAM_t * cam) { cam->cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_basicVehicleContainerHighFrequency; - cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - heading.headingValue = HeadingValue_unavailable; - cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - heading.headingConfidence = HeadingConfidence_unavailable; + cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading. + headingValue = HeadingValue_unavailable; + cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading. + headingConfidence = HeadingConfidence_unavailable; cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.speed.speedValue = SpeedValue_unavailable; - cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - speed.speedConfidence = SpeedConfidence_unavailable; + cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.speed. + speedConfidence = SpeedConfidence_unavailable; cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.driveDirection = DriveDirection_unavailable; - cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - vehicleLength.vehicleLengthValue = VehicleLengthValue_unavailable; - cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - vehicleLength.vehicleLengthConfidenceIndication = VehicleLengthConfidenceIndication_unavailable; + cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.vehicleLength. + vehicleLengthValue = VehicleLengthValue_unavailable; + cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.vehicleLength. + vehicleLengthConfidenceIndication = VehicleLengthConfidenceIndication_unavailable; cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.vehicleWidth = VehicleWidth_unavailable; - cam->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.longitudinalAcceleration.longitudinalAccelerationValue = - LongitudinalAccelerationValue_unavailable; - cam->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.longitudinalAcceleration.longitudinalAccelerationConfidence = - AccelerationConfidence_unavailable; cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - curvature.curvatureValue = CurvatureValue_unavailable; + longitudinalAcceleration.longitudinalAccelerationValue = LongitudinalAccelerationValue_unavailable; cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - curvature.curvatureConfidence = CurvatureConfidence_unavailable; - cam->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.curvatureCalculationMode = CurvatureCalculationMode_unavailable; + longitudinalAcceleration.longitudinalAccelerationConfidence = AccelerationConfidence_unavailable; + cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.curvature. + curvatureValue = CurvatureValue_unavailable; + cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.curvature. + curvatureConfidence = CurvatureConfidence_unavailable; cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - yawRate.yawRateValue = YawRateValue_unavailable; + curvatureCalculationMode = CurvatureCalculationMode_unavailable; + cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.yawRate. + yawRateValue = YawRateValue_unavailable; + cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.yawRate. + yawRateConfidence = YawRateConfidence_unavailable; cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - yawRate.yawRateConfidence = YawRateConfidence_unavailable; - cam->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.lateralAcceleration->lateralAccelerationValue = - LateralAccelerationValue_unavailable; - cam->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.lateralAcceleration->lateralAccelerationConfidence = - AccelerationConfidence_unavailable; + lateralAcceleration->lateralAccelerationValue = LateralAccelerationValue_unavailable; + cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + lateralAcceleration->lateralAccelerationConfidence = AccelerationConfidence_unavailable; // Unused highFrequencyContainer optional fields (null their pointers to show unused) // TODO: Modify here to once relevant information can be used - free(cam->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.accelerationControl); - free(cam->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.lanePosition); - free(cam->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.steeringWheelAngle); - free(cam->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.verticalAcceleration); - free(cam->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.performanceClass); - free(cam->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.cenDsrcTollingZone); - cam->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.accelerationControl = NULL; + free(cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + accelerationControl); + free(cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + lanePosition); + free(cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + steeringWheelAngle); + free(cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + verticalAcceleration); + free(cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + performanceClass); + free(cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + cenDsrcTollingZone); + cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + accelerationControl = NULL; cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.lanePosition = NULL; - cam->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.steeringWheelAngle = NULL; - cam->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.verticalAcceleration = NULL; + cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + steeringWheelAngle = NULL; + cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + verticalAcceleration = NULL; cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.performanceClass = NULL; - cam->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.cenDsrcTollingZone = NULL; + cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + cenDsrcTollingZone = NULL; // Low frequency container and special vehicle containers unused for now (TODO) free(cam->cam.camParameters.lowFrequencyContainer); @@ -927,12 +921,12 @@ I32 generateCAMMessage(MONRType * MONRData, CAM_t * cam) { tempCAM->cam.camParameters.basicContainer.referencePosition.longitude = (long)(longitude * 10000000.0); - tempCAM->cam.camParameters.basicContainer.referencePosition. - positionConfidenceEllipse.semiMajorConfidence = SemiAxisLength_unavailable; - tempCAM->cam.camParameters.basicContainer.referencePosition. - positionConfidenceEllipse.semiMinorConfidence = SemiAxisLength_unavailable; - tempCAM->cam.camParameters.basicContainer.referencePosition. - positionConfidenceEllipse.semiMajorOrientation = 0; + tempCAM->cam.camParameters.basicContainer.referencePosition.positionConfidenceEllipse. + semiMajorConfidence = SemiAxisLength_unavailable; + tempCAM->cam.camParameters.basicContainer.referencePosition.positionConfidenceEllipse. + semiMinorConfidence = SemiAxisLength_unavailable; + tempCAM->cam.camParameters.basicContainer.referencePosition.positionConfidenceEllipse. + semiMajorOrientation = 0; tempCAM->cam.camParameters.basicContainer.referencePosition.altitude.altitudeValue = AltitudeValue_unavailable; @@ -945,80 +939,70 @@ I32 generateCAMMessage(MONRType * MONRData, CAM_t * cam) { tempCAM->cam.camParameters.basicContainer.stationType = StationType_roadSideUnit; if (tempCAM->cam.camParameters.highFrequencyContainer.present == HighFrequencyContainer_PR_basicVehicleContainerHighFrequency) { - tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - speed.speedValue = + tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.speed. + speedValue = (long)(sqrt (pow((double)(MONRData->LongitudinalSpeedI16), 2) + pow((double)(MONRData->LateralSpeedI16), 2))); - tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - speed.speedConfidence = SpeedConfidence_unavailable; + tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.speed. + speedConfidence = SpeedConfidence_unavailable; - tempCAM->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.driveDirection = MONRData->DriveDirectionU8; tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - heading.headingValue = MONRData->HeadingU16 / 10; - tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - heading.headingConfidence = HeadingConfidence_unavailable; + driveDirection = MONRData->DriveDirectionU8; + tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading. + headingValue = MONRData->HeadingU16 / 10; + tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading. + headingConfidence = HeadingConfidence_unavailable; if (MONRData->LongitudinalAccI16 == 32001) - tempCAM->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.longitudinalAcceleration.longitudinalAccelerationValue = + tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + longitudinalAcceleration.longitudinalAccelerationValue = LongitudinalAccelerationValue_unavailable; else if (MONRData->LongitudinalAccI16 > 16000) - tempCAM->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.longitudinalAcceleration.longitudinalAccelerationValue = - 160; + tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + longitudinalAcceleration.longitudinalAccelerationValue = 160; else if (MONRData->LongitudinalAccI16 < -16000) - tempCAM->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.longitudinalAcceleration.longitudinalAccelerationValue = - -160; + tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + longitudinalAcceleration.longitudinalAccelerationValue = -160; else - tempCAM->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.longitudinalAcceleration.longitudinalAccelerationValue = - MONRData->LongitudinalAccI16 / 100; - tempCAM->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.longitudinalAcceleration.longitudinalAccelerationConfidence = - AccelerationConfidence_unavailable; + tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + longitudinalAcceleration.longitudinalAccelerationValue = MONRData->LongitudinalAccI16 / 100; + tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + longitudinalAcceleration.longitudinalAccelerationConfidence = AccelerationConfidence_unavailable; if (MONRData->LateralAccI16 == 32001) - tempCAM->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.lateralAcceleration->lateralAccelerationValue = - LateralAccelerationValue_unavailable; + tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + lateralAcceleration->lateralAccelerationValue = LateralAccelerationValue_unavailable; else if (MONRData->LateralAccI16 > 16000) - tempCAM->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.lateralAcceleration->lateralAccelerationValue = 160; + tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + lateralAcceleration->lateralAccelerationValue = 160; else if (MONRData->LateralAccI16 < -16000) - tempCAM->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.lateralAcceleration->lateralAccelerationValue = -160; + tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + lateralAcceleration->lateralAccelerationValue = -160; else - tempCAM->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.lateralAcceleration->lateralAccelerationValue = - MONRData->LateralAccI16 / 100; - tempCAM->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.lateralAcceleration->lateralAccelerationConfidence = - AccelerationConfidence_unavailable; - + tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + lateralAcceleration->lateralAccelerationValue = MONRData->LateralAccI16 / 100; tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - yawRate.yawRateValue = YawRateValue_unavailable; + lateralAcceleration->lateralAccelerationConfidence = AccelerationConfidence_unavailable; + + tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.yawRate. + yawRateValue = YawRateValue_unavailable; + tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.yawRate. + yawRateConfidence = YawRateConfidence_unavailable; + + tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.curvature. + curvatureValue = CurvatureValue_unavailable; + tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.curvature. + curvatureConfidence = CurvatureConfidence_unavailable; tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - yawRate.yawRateConfidence = YawRateConfidence_unavailable; + curvatureCalculationMode = CurvatureCalculationMode_unavailable; tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - curvature.curvatureValue = CurvatureValue_unavailable; + vehicleWidth = VehicleWidth_unavailable; + tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + vehicleLength.vehicleLengthValue = VehicleLengthValue_unavailable; tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - curvature.curvatureConfidence = CurvatureConfidence_unavailable; - tempCAM->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.curvatureCalculationMode = - CurvatureCalculationMode_unavailable; - - tempCAM->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.vehicleWidth = VehicleWidth_unavailable; - tempCAM->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.vehicleLength.vehicleLengthValue = - VehicleLengthValue_unavailable; - tempCAM->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.vehicleLength.vehicleLengthConfidenceIndication = - VehicleLengthConfidenceIndication_unavailable; + vehicleLength.vehicleLengthConfidenceIndication = VehicleLengthConfidenceIndication_unavailable; } diff --git a/server/src/datadictionary.c b/server/src/datadictionary.c index 8230ed46d..86ed9b401 100644 --- a/server/src/datadictionary.c +++ b/server/src/datadictionary.c @@ -1370,7 +1370,7 @@ ReadWriteAccess_t DataDictionaryInitSupervisorTCPPortU16(GSDType * GSD) { pthread_mutex_lock(&SupervisorTCPPortMutex); GSD->SupervisorTCPPortU16 = atoi(ResultBufferC8); pthread_mutex_unlock(&SupervisorTCPPortMutex); - } + } else { Res = PARAMETER_NOTFOUND; LogMessage(LOG_LEVEL_ERROR, "SupervisorTCPPort not found!"); @@ -1682,28 +1682,28 @@ ReadWriteAccess_t DataDictionaryInitMONR(GSDType * GSD) { struct stat st; pthread_mutex_lock(&MONRMutex); - sprintf(filePath, "%smonrMessageMemory.mem", SHARED_MEMORY_PATH); + sprintf(filePath, "%smonrMessageMemory.mem", SHARED_MEMORY_PATH); fd = open(filePath, O_RDWR | O_CREAT, S_IRUSR | S_IWUSR); stat(filePath, &st); // this memory does not change size as more MONR messages are added, and it is unclear where in the memory stuff is being written - lseek(fd, (sizeof (MonitorDataType)) - 1, SEEK_SET); + lseek(fd, (sizeof (MonitorDataType)) - 1, SEEK_SET); write(fd, "", 1); stat(filePath, &st); // Map memory to created file GSD->MonrMessages = - (MonitorDataType *) mmap(NULL, (sizeof (MonitorDataType)), PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0); - - if (GSD->MonrMessages == MAP_FAILED) { - LogPrint(LOG_LEVEL_ERROR, "mmap failed: %s", strerror(errno)); - close(fd); - } - else{ - LogMessage(LOG_LEVEL_INFO, "Init MonrMessage memory"); - } - close(fd); + (MonitorDataType *) mmap(NULL, (sizeof (MonitorDataType)), PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0); + + if (GSD->MonrMessages == MAP_FAILED) { + LogPrint(LOG_LEVEL_ERROR, "mmap failed: %s", strerror(errno)); + close(fd); + } + else { + LogMessage(LOG_LEVEL_INFO, "Init MonrMessage memory"); + } + close(fd); pthread_mutex_unlock(&MONRMutex); return Res; } @@ -1721,12 +1721,12 @@ ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, const MonitorDataType * M Res = WRITE_OK; pthread_mutex_lock(&MONRMutex); if (GSD->MonrMessages != NULL && transmitterId < GSD->numberOfObjects) { - GSD->MonrMessages[transmitterId] = *MONR; + GSD->MonrMessages[transmitterId] = *MONR; + } + else { + Res = UNDEFINED; + LogPrint(LOG_LEVEL_ERROR, "Unable to write MonrMessage in DataDictionary"); } - else{ - Res = UNDEFINED; - LogPrint(LOG_LEVEL_ERROR, "Unable to write MonrMessage in DataDictionary"); - } pthread_mutex_unlock(&MONRMutex); @@ -1741,19 +1741,20 @@ ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, const MonitorDataType * M * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryGetMONR(GSDType * GSD, MonitorDataType * MONR, const U32 transmitterId) { - ReadWriteAccess_t Res; - Res = READ_OK; + ReadWriteAccess_t Res; - pthread_mutex_lock(&MONRMutex); - if (GSD->MonrMessages != NULL && transmitterId < GSD->numberOfObjects) { + Res = READ_OK; + + pthread_mutex_lock(&MONRMutex); + if (GSD->MonrMessages != NULL && transmitterId < GSD->numberOfObjects) { *MONR = GSD->MonrMessages[transmitterId]; - } - else{ - Res = UNDEFINED; - LogPrint(LOG_LEVEL_ERROR, "Unable to read MonrMessage in DataDictionary"); - } + } + else { + Res = UNDEFINED; + LogPrint(LOG_LEVEL_ERROR, "Unable to read MonrMessage in DataDictionary"); + } pthread_mutex_unlock(&MONRMutex); - return Res; + return Res; } /*! @@ -1766,7 +1767,7 @@ ReadWriteAccess_t DataDictionaryFreeMONR(GSDType * GSD) { Res = WRITE_OK; pthread_mutex_lock(&MONRMutex); - int res = munmap(GSD->MonrMessages, sizeof (MonitorDataType)); + int res = munmap(GSD->MonrMessages, sizeof (MonitorDataType)); if (res < 0) { util_error("Unable to unmap monrMessages file!"); diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 30920ebdd..e0af87a90 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -304,11 +304,10 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { objectControlServerStatus = CONTROL_CENTER_STATUS_ABORT; MessageLength = encodeHEABMessage(objectControlServerStatus, MessageBuffer, sizeof (MessageBuffer), 0); - for(iIndex = 0; iIndex < nbr_objects; ++iIndex) - { - UtilSendUDPData("Object Control", &safety_socket_fd[iIndex], &safety_object_addr[iIndex], - MessageBuffer, MessageLength, 0); - } + for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { + UtilSendUDPData("Object Control", &safety_socket_fd[iIndex], &safety_object_addr[iIndex], + MessageBuffer, MessageLength, 0); + } } // Heartbeat diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index cb7f62cbb..35c128dc5 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -577,7 +577,7 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { SystemControlBuildRVSSMONRChannelMessage(RVSSData, &RVSSMessageLengthU32, monrData, 0); UtilSendUDPData("SystemControl", &RVSSChannelSocket, &RVSSChannelAddr, RVSSData, RVSSMessageLengthU32, 0); - } + } break; case COMM_INV: break; From b2419608486773c67caf4a5d266a3fbbf021ac81 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 19 Feb 2020 13:13:15 +0100 Subject: [PATCH 271/523] Modified the CMakeLists of modules and core --- CMakeLists.txt | 13 +++- modules/ScenarioControl/CMakeLists.txt | 102 +++++-------------------- modules/Supervision/CMakeLists.txt | 72 ++++------------- modules/Visualization/CMakeLists.txt | 56 +++----------- 4 files changed, 56 insertions(+), 187 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index cd4894586..1074a9483 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,13 @@ -cmake_minimum_required (VERSION 3.1) +cmake_minimum_required(VERSION 3.10) -add_subdirectory(util/C) +project(Maestro) + +set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib) +set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib) +set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin) -add_subdirectory(server) +add_subdirectory(util/C) +add_subdirectory(core) add_subdirectory(modules/ScenarioControl) +add_subdirectory(modules/Supervision) +add_subdirectory(modules/Visualization) diff --git a/modules/ScenarioControl/CMakeLists.txt b/modules/ScenarioControl/CMakeLists.txt index 444fe2b4e..988b9d959 100644 --- a/modules/ScenarioControl/CMakeLists.txt +++ b/modules/ScenarioControl/CMakeLists.txt @@ -1,88 +1,28 @@ cmake_minimum_required(VERSION 3.1) -SET(CMAKE_CXX_STANDARD 11) -SET(CMAKE_CXX_STANDARD_REQUIRED ON) -SET(CMAKE_C_STANDARD 11) -SET(CMAKE_C_STANDARD_REQUIRED ON) +set(CMAKE_CXX_STANDARD 11) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_C_STANDARD 11) +set(CMAKE_C_STANDARD_REQUIRED ON) project(ScenarioControl) -# This module is an example of how to set up a new module external to the Maestro executable - -include_directories(inc) -include_directories(src) -include(GNUInstallDirs) - -if (NOT TARGET MaestroLogging) - include_directories(../../util/C/logging) - add_library(MaestroLogging - ../util/C/logging/logging.h - ../util/C/logging/logging.c - ) -endif() - -if (NOT TARGET MaestroTime) - include_directories(../../util/C/time) - add_library(MaestroTime - ../util/C/time/maestroTime.h - ../util/C/time/maestroTime.c - ) -endif() - -if (NOT TARGET MQBus) - include_directories(../../util/C/MQBus) - add_library(MQBus - ../util/C/MQBus/mqbus.c - ../util/C/MQBus/mqbus.h - ) -endif() - -if (NOT TARGET util) - include_directories(../../server/inc) - add_library(util - ../../server/src/util.c - ../../server/inc/util.h - ../../server/src/iso22133.c - ../../server/inc/iso22133.h - ../../server/src/positioning.c - ../../server/inc/positioning.h - ) -endif() - -add_library(Trigger - inc/trigger.h - src/trigger.cpp - inc/isotrigger.h - src/isotrigger.cpp - inc/booleantrigger.h - src/booleantrigger.cpp - inc/braketrigger.h - src/braketrigger.cpp -) - -add_library(Action - inc/action.h - src/action.cpp - inc/externalaction.h - src/externalaction.cpp +add_executable(ScenarioControl + src/action.cpp + src/booleantrigger.cpp + src/braketrigger.cpp + src/causality.cpp + src/externalaction.cpp + src/isotrigger.cpp + src/main.cpp + src/scenario.cpp + src/trigger.cpp ) -add_library(Scenario - inc/scenario.h - src/scenario.cpp - inc/causality.h - src/causality.cpp +target_link_libraries(ScenarioControl LINK_PUBLIC MaestroTime MaestroLogging MaestroMQ MaestroUtil) +target_include_directories(ScenarioControl PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/inc + ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C/time + ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C/logging + ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C/mqbus + ${CMAKE_CURRENT_SOURCE_DIR}/../../core/inc ) - - -add_executable(ScenarioControl src/main.cpp) - -install(TARGETS ScenarioControl DESTINATION bin) - -target_link_libraries(ScenarioControl MaestroTime MaestroLogging util Scenario) -target_link_libraries(Scenario Trigger Action) -target_link_libraries(Action MaestroTime) -target_link_libraries(util MQBus MaestroTime MaestroLogging) -target_link_libraries(MQBus rt m) - - - diff --git a/modules/Supervision/CMakeLists.txt b/modules/Supervision/CMakeLists.txt index 47d226e1a..167e580a1 100644 --- a/modules/Supervision/CMakeLists.txt +++ b/modules/Supervision/CMakeLists.txt @@ -1,63 +1,23 @@ cmake_minimum_required(VERSION 3.1) - -SET(CMAKE_CXX_STANDARD 11) -SET(CMAKE_CXX_STANDARD_REQUIRED ON) -SET(CMAKE_C_STANDARD 11) -SET(CMAKE_C_STANDARD_REQUIRED ON) +set(CMAKE_CXX_STANDARD 11) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_C_STANDARD 11) +set(CMAKE_C_STANDARD_REQUIRED ON) project(Supervision) - -include_directories(inc) -include_directories(../../util/C/logging) -include_directories(../../util/C/time) -include_directories(../../util/C/MQBus) -include_directories(../../server/inc) -include(GNUInstallDirs) - - -# Create library -add_library(MaestroLogging - ../../util/C/logging/logging.h - ../../util/C/logging/logging.c -) - -add_library(MaestroTime - ../../util/C/time/maestroTime.h - ../../util/C/time/maestroTime.c -) - -add_library(MQBus - ../../util/C/MQBus/mqbus.h - ../../util/C/MQBus/mqbus.c -) - -# Create library -add_library(util - ../../server/src/util.c - ../../server/inc/util.h - ../../server/src/iso22133.c - ../../server/inc/iso22133.h - ../../server/inc/positioning.h - ../../server/src/positioning.c -) - add_executable(Supervision - src/main.cpp - src/geofence.cpp - inc/geofence.h - src/trajectory.cpp - inc/trajectory.h - inc/regexpatterns.h - src/supervisionstate.cpp - inc/supervisionstate.h + src/geofence.cpp + src/main.cpp + src/supervisionstate.cpp + src/trajectory.cpp ) -install(TARGETS Supervision DESTINATION bin) - -target_link_libraries(Supervision MaestroTime MaestroLogging util) -target_link_libraries(util MQBus MaestroTime MaestroLogging) -target_link_libraries(MQBus rt m) - - - +target_link_libraries(Supervision LINK_PUBLIC MaestroTime MaestroLogging MaestroMQ MaestroUtil) +target_include_directories(Supervision PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/inc + ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C/time + ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C/logging + ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C/mqbus + ${CMAKE_CURRENT_SOURCE_DIR}/../../core/inc +) diff --git a/modules/Visualization/CMakeLists.txt b/modules/Visualization/CMakeLists.txt index 655b3711a..02ec16c28 100644 --- a/modules/Visualization/CMakeLists.txt +++ b/modules/Visualization/CMakeLists.txt @@ -1,56 +1,18 @@ cmake_minimum_required(VERSION 3.1) project(Visualization) -# This module is an example of how to set up a new module external to the Maestro executable # Set C and C++ dialects -SET(CMAKE_C_STANDARD 11) -SET(CMAKE_C_STANDARD_REQUIRED ON) -SET(CMAKE_CXX_STANDARD 11) -SET(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_C_STANDARD 11) +set(CMAKE_C_STANDARD_REQUIRED ON) -include_directories(inc) -include_directories(../../util/C/logging) -include_directories(../../util/C/time) -include_directories(../../util/C/MQBus) -include_directories(../../server/inc) -include(GNUInstallDirs) - - -# Create library -add_library(MaestroLogging - ../../util/C/logging/logging.h - ../../util/C/logging/logging.c -) - -add_library(MaestroTime - ../../util/C/time/maestroTime.h - ../../util/C/time/maestroTime.c -) - -add_library(MQBus - ../../util/C/MQBus/mqbus.h - ../../util/C/MQBus/mqbus.c +add_executable(Visualization + src/main.c ) -# Create library -add_library(util - ../../server/src/util.c - ../../server/inc/util.h - ../../server/inc/iso22133.h - ../../server/src/iso22133.c - ../../server/inc/positioning.h - ../../server/src/positioning.c +target_link_libraries(Visualization LINK_PUBLIC MaestroTime MaestroLogging MaestroUtil) +target_include_directories(Visualization PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C/time + ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C/logging + ${CMAKE_CURRENT_SOURCE_DIR}/../../core/inc ) - -add_executable(Visualization - src/main.c) - -install(TARGETS Visualization DESTINATION bin) - -target_link_libraries(Visualization MaestroTime MaestroLogging util) -target_link_libraries(util MQBus MaestroLogging MaestroTime) -target_link_libraries(MQBus rt m) - - - From c30913ba59edb2f6b4329c9fd736d7406cdf25d6 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 19 Feb 2020 13:28:51 +0100 Subject: [PATCH 272/523] Renamed server dir into core --- core/CMakeLists.txt | 53 +++++++ {server => core}/conf/adaptivesync.conf | 0 {server => core}/conf/test.conf | 0 {server => core}/conf/triggeraction.conf | 0 .../geofence/GaragePlanForbidden.geofence | 0 .../HalvaGarageplanPermitted.geofence | 0 .../LitenGarageplanForbidden.geofence | 0 {server => core}/inc/citscontrol.h | 0 {server => core}/inc/datadictionary.h | 0 {server => core}/inc/iso22133.h | 0 {server => core}/inc/logger.h | 0 {server => core}/inc/objectcontrol.h | 0 {server => core}/inc/positioning.h | 0 {server => core}/inc/remotecontrol.h | 0 {server => core}/inc/simulatorcontrol.h | 0 {server => core}/inc/supervisorcontrol.h | 0 {server => core}/inc/systemcontrol.h | 0 {server => core}/inc/timecontrol.h | 0 {server => core}/inc/util.h | 0 .../000-StartupAndKillMainExecutable.sh | 0 .../005-UserControlConnect.py | 0 .../100-SingleTypicalTest.py | 0 .../249-uploadRowMismatchingTrajectories.py | 0 .../manual tests/MSCPManualTest.py | 0 .../resources/trajectories/0.traj | 0 .../resources/trajectories/1.traj | 0 .../trajectories/GarageLastbilRektangel2.traj | 0 .../trajectories/GarageRektangelInre3.traj | 0 .../trajectories/GarageRektangelYttre3.traj | 0 .../trajectories/GarageplanInnerring.traj | 0 .../trajectories/GarageplanYtterring.traj | 0 .../trajectories/RuralRoadTruck1.traj | 0 .../resources/trajectories/aliv_rect.traj | 0 ...geplanInnerring_lessRowsThanSpecified.traj | 0 ...geplanInnerring_moreRowsThanSpecified.traj | 0 .../trajectories/fengco_drivefile_ego.traj | 0 .../resources/trajectories/rectangle1.traj | 0 .../resources/trajectories/rural2.traj | 0 .../resources/trajectories/ruralmid2.traj | 0 .../resources/trajectories/ruralright2.traj | 0 .../resources/trajectories/safety.traj | 0 .../integration-tests/tools/Executable.py | 0 .../integration-tests/tools/MSCP.py | 0 .../integration-tests/tools/PortChecker.py | 0 .../integration-tests/tools/TrajectoryFile.py | 0 .../integration-tests/tools/__init__.py | 0 {server => core}/src/citscontrol.c | 0 {server => core}/src/datadictionary.c | 0 {server => core}/src/icon/MaestroICON.png | Bin {server => core}/src/icon/MaestroICON.svg | 0 {server => core}/src/iso22133.c | 0 {server => core}/src/logger.c | 0 {server => core}/src/main.c | 0 {server => core}/src/objectcontrol.c | 0 {server => core}/src/positioning.c | 0 {server => core}/src/remotecontrol.c | 0 {server => core}/src/simulatorcontrol.c | 0 {server => core}/src/supervisorcontrol.c | 0 {server => core}/src/systemcontrol.c | 0 {server => core}/src/timecontrol.c | 0 {server => core}/src/util.c | 0 {server => core}/traj/0.traj | 0 {server => core}/traj/1.traj | 0 {server => core}/traj/192.168.0.17 | 0 .../traj/DriveFiles_Curve_v3/DriveFile_1.csv | 0 .../traj/DriveFiles_Curve_v3/DriveFile_2.csv | 0 .../traj/DriveFiles_Curve_v3/DriveFile_3.csv | 0 .../traj/DriveFiles_Curve_v3/DriveFile_4.csv | 0 .../traj/DriveFiles_Curve_v3/DriveFile_5.csv | 0 .../DriveFile_1.csv | 0 .../DriveFile_2.csv | 0 .../DriveFile_3.csv | 0 .../DriveFile_4.csv | 0 .../DriveFile_5.csv | 0 .../traj/GarageLastbilRektangel2.traj | 0 .../traj/GarageRektangelInre3.traj | 0 .../traj/GarageRektangelYttre3.traj | 0 .../traj/GarageplanInnerring.traj | 0 .../traj/GarageplanYtterring.traj | 0 {server => core}/traj/README.txt | 0 {server => core}/traj/RuralRoadTruck1.traj | 0 .../TestScenario_Intersection/192.168.0.1 | 0 .../TestScenario_Intersection/192.168.0.2 | 0 .../TestScenario_Intersection/192.168.0.3 | 0 .../TestScenario_Intersection/192.168.0.4 | 0 .../TestScenario_Intersection/192.168.0.5 | 0 {server => core}/traj/aliv_rect.traj | 0 .../traj/fengco_drivefile_ego.traj | 0 {server => core}/traj/garageh.chronos | 0 {server => core}/traj/garagev.chronos | 0 {server => core}/traj/rectangle1.traj | 0 {server => core}/traj/rural2.traj | 0 {server => core}/traj/ruralmid2.traj | 0 {server => core}/traj/ruralright2.traj | 0 {server => core}/traj/safety.traj | 0 .../traj/straightLineAccelDecel_30kmh.traj | 0 server/CMakeLists.txt | 129 ------------------ server/ubuntuCleanServer | 10 -- 98 files changed, 53 insertions(+), 139 deletions(-) create mode 100644 core/CMakeLists.txt rename {server => core}/conf/adaptivesync.conf (100%) rename {server => core}/conf/test.conf (100%) rename {server => core}/conf/triggeraction.conf (100%) rename {server => core}/geofence/GaragePlanForbidden.geofence (100%) rename {server => core}/geofence/HalvaGarageplanPermitted.geofence (100%) rename {server => core}/geofence/LitenGarageplanForbidden.geofence (100%) rename {server => core}/inc/citscontrol.h (100%) rename {server => core}/inc/datadictionary.h (100%) rename {server => core}/inc/iso22133.h (100%) rename {server => core}/inc/logger.h (100%) rename {server => core}/inc/objectcontrol.h (100%) rename {server => core}/inc/positioning.h (100%) rename {server => core}/inc/remotecontrol.h (100%) rename {server => core}/inc/simulatorcontrol.h (100%) rename {server => core}/inc/supervisorcontrol.h (100%) rename {server => core}/inc/systemcontrol.h (100%) rename {server => core}/inc/timecontrol.h (100%) rename {server => core}/inc/util.h (100%) rename {server => core}/integration-tests/000-StartupAndKillMainExecutable.sh (100%) rename {server => core}/integration-tests/005-UserControlConnect.py (100%) rename {server => core}/integration-tests/100-SingleTypicalTest.py (100%) rename {server => core}/integration-tests/249-uploadRowMismatchingTrajectories.py (100%) rename {server => core}/integration-tests/manual tests/MSCPManualTest.py (100%) rename {server => core}/integration-tests/resources/trajectories/0.traj (100%) rename {server => core}/integration-tests/resources/trajectories/1.traj (100%) rename {server => core}/integration-tests/resources/trajectories/GarageLastbilRektangel2.traj (100%) rename {server => core}/integration-tests/resources/trajectories/GarageRektangelInre3.traj (100%) rename {server => core}/integration-tests/resources/trajectories/GarageRektangelYttre3.traj (100%) rename {server => core}/integration-tests/resources/trajectories/GarageplanInnerring.traj (100%) rename {server => core}/integration-tests/resources/trajectories/GarageplanYtterring.traj (100%) rename {server => core}/integration-tests/resources/trajectories/RuralRoadTruck1.traj (100%) rename {server => core}/integration-tests/resources/trajectories/aliv_rect.traj (100%) rename {server => core}/integration-tests/resources/trajectories/faulty/GarageplanInnerring_lessRowsThanSpecified.traj (100%) rename {server => core}/integration-tests/resources/trajectories/faulty/GarageplanInnerring_moreRowsThanSpecified.traj (100%) rename {server => core}/integration-tests/resources/trajectories/fengco_drivefile_ego.traj (100%) rename {server => core}/integration-tests/resources/trajectories/rectangle1.traj (100%) rename {server => core}/integration-tests/resources/trajectories/rural2.traj (100%) rename {server => core}/integration-tests/resources/trajectories/ruralmid2.traj (100%) rename {server => core}/integration-tests/resources/trajectories/ruralright2.traj (100%) rename {server => core}/integration-tests/resources/trajectories/safety.traj (100%) rename {server => core}/integration-tests/tools/Executable.py (100%) rename {server => core}/integration-tests/tools/MSCP.py (100%) rename {server => core}/integration-tests/tools/PortChecker.py (100%) rename {server => core}/integration-tests/tools/TrajectoryFile.py (100%) rename {server => core}/integration-tests/tools/__init__.py (100%) rename {server => core}/src/citscontrol.c (100%) rename {server => core}/src/datadictionary.c (100%) rename {server => core}/src/icon/MaestroICON.png (100%) rename {server => core}/src/icon/MaestroICON.svg (100%) rename {server => core}/src/iso22133.c (100%) rename {server => core}/src/logger.c (100%) rename {server => core}/src/main.c (100%) rename {server => core}/src/objectcontrol.c (100%) rename {server => core}/src/positioning.c (100%) rename {server => core}/src/remotecontrol.c (100%) rename {server => core}/src/simulatorcontrol.c (100%) rename {server => core}/src/supervisorcontrol.c (100%) rename {server => core}/src/systemcontrol.c (100%) rename {server => core}/src/timecontrol.c (100%) rename {server => core}/src/util.c (100%) rename {server => core}/traj/0.traj (100%) rename {server => core}/traj/1.traj (100%) rename {server => core}/traj/192.168.0.17 (100%) rename {server => core}/traj/DriveFiles_Curve_v3/DriveFile_1.csv (100%) rename {server => core}/traj/DriveFiles_Curve_v3/DriveFile_2.csv (100%) rename {server => core}/traj/DriveFiles_Curve_v3/DriveFile_3.csv (100%) rename {server => core}/traj/DriveFiles_Curve_v3/DriveFile_4.csv (100%) rename {server => core}/traj/DriveFiles_Curve_v3/DriveFile_5.csv (100%) rename {server => core}/traj/DriveFiles_DriveStraight_v6/DriveFile_1.csv (100%) rename {server => core}/traj/DriveFiles_DriveStraight_v6/DriveFile_2.csv (100%) rename {server => core}/traj/DriveFiles_DriveStraight_v6/DriveFile_3.csv (100%) rename {server => core}/traj/DriveFiles_DriveStraight_v6/DriveFile_4.csv (100%) rename {server => core}/traj/DriveFiles_DriveStraight_v6/DriveFile_5.csv (100%) rename {server => core}/traj/GarageLastbilRektangel2.traj (100%) rename {server => core}/traj/GarageRektangelInre3.traj (100%) rename {server => core}/traj/GarageRektangelYttre3.traj (100%) rename {server => core}/traj/GarageplanInnerring.traj (100%) rename {server => core}/traj/GarageplanYtterring.traj (100%) rename {server => core}/traj/README.txt (100%) rename {server => core}/traj/RuralRoadTruck1.traj (100%) rename {server => core}/traj/TestScenario_Intersection/192.168.0.1 (100%) rename {server => core}/traj/TestScenario_Intersection/192.168.0.2 (100%) rename {server => core}/traj/TestScenario_Intersection/192.168.0.3 (100%) rename {server => core}/traj/TestScenario_Intersection/192.168.0.4 (100%) rename {server => core}/traj/TestScenario_Intersection/192.168.0.5 (100%) rename {server => core}/traj/aliv_rect.traj (100%) rename {server => core}/traj/fengco_drivefile_ego.traj (100%) rename {server => core}/traj/garageh.chronos (100%) rename {server => core}/traj/garagev.chronos (100%) rename {server => core}/traj/rectangle1.traj (100%) rename {server => core}/traj/rural2.traj (100%) rename {server => core}/traj/ruralmid2.traj (100%) rename {server => core}/traj/ruralright2.traj (100%) rename {server => core}/traj/safety.traj (100%) rename {server => core}/traj/straightLineAccelDecel_30kmh.traj (100%) delete mode 100644 server/CMakeLists.txt delete mode 100755 server/ubuntuCleanServer diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt new file mode 100644 index 000000000..5c7c1df60 --- /dev/null +++ b/core/CMakeLists.txt @@ -0,0 +1,53 @@ +cmake_minimum_required(VERSION 3.1) + +project(Core) + +add_executable(Core + src/objectcontrol.c + src/logger.c + src/main.c + src/systemcontrol.c + src/timecontrol.c +) + +add_library(MaestroUtil + src/util.c + src/iso22133.c + src/positioning.c + src/datadictionary.c +) + +if(NOT TARGET MaestroTime) + add_library(MaestroTime STATIC + ${CMAKE_CURRENT_SOURCE_DIR}/../util/C/time/maestroTime.c + ) +endif() + +if(NOT TARGET MaestroLogging) + add_library(MaestroLogging STATIC + ${CMAKE_CURRENT_SOURCE_DIR}/../util/C/logging/logging.c + ) +endif() + +if(NOT TARGET MaestroMQ) + add_library(MaestroMQ STATIC + ${CMAKE_CURRENT_SOURCE_DIR}/../util/C/mqbus/mqbus.c + ) + target_link_libraries(MaestroMQ rt) +endif() + +target_link_libraries(Core LINK_PUBLIC MaestroTime MaestroLogging MaestroUtil) +target_include_directories(Core PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/inc + ${CMAKE_CURRENT_SOURCE_DIR}/../util/C/time + ${CMAKE_CURRENT_SOURCE_DIR}/../util/C/logging +) + +target_link_libraries(MaestroUtil LINK_PUBLIC MaestroTime MaestroLogging MaestroMQ) +target_link_libraries(MaestroUtil LINK_PUBLIC m) +target_include_directories(MaestroUtil PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/inc + ${CMAKE_CURRENT_SOURCE_DIR}/../util/C/time + ${CMAKE_CURRENT_SOURCE_DIR}/../util/C/logging + ${CMAKE_CURRENT_SOURCE_DIR}/../util/C/mqbus +) diff --git a/server/conf/adaptivesync.conf b/core/conf/adaptivesync.conf similarity index 100% rename from server/conf/adaptivesync.conf rename to core/conf/adaptivesync.conf diff --git a/server/conf/test.conf b/core/conf/test.conf similarity index 100% rename from server/conf/test.conf rename to core/conf/test.conf diff --git a/server/conf/triggeraction.conf b/core/conf/triggeraction.conf similarity index 100% rename from server/conf/triggeraction.conf rename to core/conf/triggeraction.conf diff --git a/server/geofence/GaragePlanForbidden.geofence b/core/geofence/GaragePlanForbidden.geofence similarity index 100% rename from server/geofence/GaragePlanForbidden.geofence rename to core/geofence/GaragePlanForbidden.geofence diff --git a/server/geofence/HalvaGarageplanPermitted.geofence b/core/geofence/HalvaGarageplanPermitted.geofence similarity index 100% rename from server/geofence/HalvaGarageplanPermitted.geofence rename to core/geofence/HalvaGarageplanPermitted.geofence diff --git a/server/geofence/LitenGarageplanForbidden.geofence b/core/geofence/LitenGarageplanForbidden.geofence similarity index 100% rename from server/geofence/LitenGarageplanForbidden.geofence rename to core/geofence/LitenGarageplanForbidden.geofence diff --git a/server/inc/citscontrol.h b/core/inc/citscontrol.h similarity index 100% rename from server/inc/citscontrol.h rename to core/inc/citscontrol.h diff --git a/server/inc/datadictionary.h b/core/inc/datadictionary.h similarity index 100% rename from server/inc/datadictionary.h rename to core/inc/datadictionary.h diff --git a/server/inc/iso22133.h b/core/inc/iso22133.h similarity index 100% rename from server/inc/iso22133.h rename to core/inc/iso22133.h diff --git a/server/inc/logger.h b/core/inc/logger.h similarity index 100% rename from server/inc/logger.h rename to core/inc/logger.h diff --git a/server/inc/objectcontrol.h b/core/inc/objectcontrol.h similarity index 100% rename from server/inc/objectcontrol.h rename to core/inc/objectcontrol.h diff --git a/server/inc/positioning.h b/core/inc/positioning.h similarity index 100% rename from server/inc/positioning.h rename to core/inc/positioning.h diff --git a/server/inc/remotecontrol.h b/core/inc/remotecontrol.h similarity index 100% rename from server/inc/remotecontrol.h rename to core/inc/remotecontrol.h diff --git a/server/inc/simulatorcontrol.h b/core/inc/simulatorcontrol.h similarity index 100% rename from server/inc/simulatorcontrol.h rename to core/inc/simulatorcontrol.h diff --git a/server/inc/supervisorcontrol.h b/core/inc/supervisorcontrol.h similarity index 100% rename from server/inc/supervisorcontrol.h rename to core/inc/supervisorcontrol.h diff --git a/server/inc/systemcontrol.h b/core/inc/systemcontrol.h similarity index 100% rename from server/inc/systemcontrol.h rename to core/inc/systemcontrol.h diff --git a/server/inc/timecontrol.h b/core/inc/timecontrol.h similarity index 100% rename from server/inc/timecontrol.h rename to core/inc/timecontrol.h diff --git a/server/inc/util.h b/core/inc/util.h similarity index 100% rename from server/inc/util.h rename to core/inc/util.h diff --git a/server/integration-tests/000-StartupAndKillMainExecutable.sh b/core/integration-tests/000-StartupAndKillMainExecutable.sh similarity index 100% rename from server/integration-tests/000-StartupAndKillMainExecutable.sh rename to core/integration-tests/000-StartupAndKillMainExecutable.sh diff --git a/server/integration-tests/005-UserControlConnect.py b/core/integration-tests/005-UserControlConnect.py similarity index 100% rename from server/integration-tests/005-UserControlConnect.py rename to core/integration-tests/005-UserControlConnect.py diff --git a/server/integration-tests/100-SingleTypicalTest.py b/core/integration-tests/100-SingleTypicalTest.py similarity index 100% rename from server/integration-tests/100-SingleTypicalTest.py rename to core/integration-tests/100-SingleTypicalTest.py diff --git a/server/integration-tests/249-uploadRowMismatchingTrajectories.py b/core/integration-tests/249-uploadRowMismatchingTrajectories.py similarity index 100% rename from server/integration-tests/249-uploadRowMismatchingTrajectories.py rename to core/integration-tests/249-uploadRowMismatchingTrajectories.py diff --git a/server/integration-tests/manual tests/MSCPManualTest.py b/core/integration-tests/manual tests/MSCPManualTest.py similarity index 100% rename from server/integration-tests/manual tests/MSCPManualTest.py rename to core/integration-tests/manual tests/MSCPManualTest.py diff --git a/server/integration-tests/resources/trajectories/0.traj b/core/integration-tests/resources/trajectories/0.traj similarity index 100% rename from server/integration-tests/resources/trajectories/0.traj rename to core/integration-tests/resources/trajectories/0.traj diff --git a/server/integration-tests/resources/trajectories/1.traj b/core/integration-tests/resources/trajectories/1.traj similarity index 100% rename from server/integration-tests/resources/trajectories/1.traj rename to core/integration-tests/resources/trajectories/1.traj diff --git a/server/integration-tests/resources/trajectories/GarageLastbilRektangel2.traj b/core/integration-tests/resources/trajectories/GarageLastbilRektangel2.traj similarity index 100% rename from server/integration-tests/resources/trajectories/GarageLastbilRektangel2.traj rename to core/integration-tests/resources/trajectories/GarageLastbilRektangel2.traj diff --git a/server/integration-tests/resources/trajectories/GarageRektangelInre3.traj b/core/integration-tests/resources/trajectories/GarageRektangelInre3.traj similarity index 100% rename from server/integration-tests/resources/trajectories/GarageRektangelInre3.traj rename to core/integration-tests/resources/trajectories/GarageRektangelInre3.traj diff --git a/server/integration-tests/resources/trajectories/GarageRektangelYttre3.traj b/core/integration-tests/resources/trajectories/GarageRektangelYttre3.traj similarity index 100% rename from server/integration-tests/resources/trajectories/GarageRektangelYttre3.traj rename to core/integration-tests/resources/trajectories/GarageRektangelYttre3.traj diff --git a/server/integration-tests/resources/trajectories/GarageplanInnerring.traj b/core/integration-tests/resources/trajectories/GarageplanInnerring.traj similarity index 100% rename from server/integration-tests/resources/trajectories/GarageplanInnerring.traj rename to core/integration-tests/resources/trajectories/GarageplanInnerring.traj diff --git a/server/integration-tests/resources/trajectories/GarageplanYtterring.traj b/core/integration-tests/resources/trajectories/GarageplanYtterring.traj similarity index 100% rename from server/integration-tests/resources/trajectories/GarageplanYtterring.traj rename to core/integration-tests/resources/trajectories/GarageplanYtterring.traj diff --git a/server/integration-tests/resources/trajectories/RuralRoadTruck1.traj b/core/integration-tests/resources/trajectories/RuralRoadTruck1.traj similarity index 100% rename from server/integration-tests/resources/trajectories/RuralRoadTruck1.traj rename to core/integration-tests/resources/trajectories/RuralRoadTruck1.traj diff --git a/server/integration-tests/resources/trajectories/aliv_rect.traj b/core/integration-tests/resources/trajectories/aliv_rect.traj similarity index 100% rename from server/integration-tests/resources/trajectories/aliv_rect.traj rename to core/integration-tests/resources/trajectories/aliv_rect.traj diff --git a/server/integration-tests/resources/trajectories/faulty/GarageplanInnerring_lessRowsThanSpecified.traj b/core/integration-tests/resources/trajectories/faulty/GarageplanInnerring_lessRowsThanSpecified.traj similarity index 100% rename from server/integration-tests/resources/trajectories/faulty/GarageplanInnerring_lessRowsThanSpecified.traj rename to core/integration-tests/resources/trajectories/faulty/GarageplanInnerring_lessRowsThanSpecified.traj diff --git a/server/integration-tests/resources/trajectories/faulty/GarageplanInnerring_moreRowsThanSpecified.traj b/core/integration-tests/resources/trajectories/faulty/GarageplanInnerring_moreRowsThanSpecified.traj similarity index 100% rename from server/integration-tests/resources/trajectories/faulty/GarageplanInnerring_moreRowsThanSpecified.traj rename to core/integration-tests/resources/trajectories/faulty/GarageplanInnerring_moreRowsThanSpecified.traj diff --git a/server/integration-tests/resources/trajectories/fengco_drivefile_ego.traj b/core/integration-tests/resources/trajectories/fengco_drivefile_ego.traj similarity index 100% rename from server/integration-tests/resources/trajectories/fengco_drivefile_ego.traj rename to core/integration-tests/resources/trajectories/fengco_drivefile_ego.traj diff --git a/server/integration-tests/resources/trajectories/rectangle1.traj b/core/integration-tests/resources/trajectories/rectangle1.traj similarity index 100% rename from server/integration-tests/resources/trajectories/rectangle1.traj rename to core/integration-tests/resources/trajectories/rectangle1.traj diff --git a/server/integration-tests/resources/trajectories/rural2.traj b/core/integration-tests/resources/trajectories/rural2.traj similarity index 100% rename from server/integration-tests/resources/trajectories/rural2.traj rename to core/integration-tests/resources/trajectories/rural2.traj diff --git a/server/integration-tests/resources/trajectories/ruralmid2.traj b/core/integration-tests/resources/trajectories/ruralmid2.traj similarity index 100% rename from server/integration-tests/resources/trajectories/ruralmid2.traj rename to core/integration-tests/resources/trajectories/ruralmid2.traj diff --git a/server/integration-tests/resources/trajectories/ruralright2.traj b/core/integration-tests/resources/trajectories/ruralright2.traj similarity index 100% rename from server/integration-tests/resources/trajectories/ruralright2.traj rename to core/integration-tests/resources/trajectories/ruralright2.traj diff --git a/server/integration-tests/resources/trajectories/safety.traj b/core/integration-tests/resources/trajectories/safety.traj similarity index 100% rename from server/integration-tests/resources/trajectories/safety.traj rename to core/integration-tests/resources/trajectories/safety.traj diff --git a/server/integration-tests/tools/Executable.py b/core/integration-tests/tools/Executable.py similarity index 100% rename from server/integration-tests/tools/Executable.py rename to core/integration-tests/tools/Executable.py diff --git a/server/integration-tests/tools/MSCP.py b/core/integration-tests/tools/MSCP.py similarity index 100% rename from server/integration-tests/tools/MSCP.py rename to core/integration-tests/tools/MSCP.py diff --git a/server/integration-tests/tools/PortChecker.py b/core/integration-tests/tools/PortChecker.py similarity index 100% rename from server/integration-tests/tools/PortChecker.py rename to core/integration-tests/tools/PortChecker.py diff --git a/server/integration-tests/tools/TrajectoryFile.py b/core/integration-tests/tools/TrajectoryFile.py similarity index 100% rename from server/integration-tests/tools/TrajectoryFile.py rename to core/integration-tests/tools/TrajectoryFile.py diff --git a/server/integration-tests/tools/__init__.py b/core/integration-tests/tools/__init__.py similarity index 100% rename from server/integration-tests/tools/__init__.py rename to core/integration-tests/tools/__init__.py diff --git a/server/src/citscontrol.c b/core/src/citscontrol.c similarity index 100% rename from server/src/citscontrol.c rename to core/src/citscontrol.c diff --git a/server/src/datadictionary.c b/core/src/datadictionary.c similarity index 100% rename from server/src/datadictionary.c rename to core/src/datadictionary.c diff --git a/server/src/icon/MaestroICON.png b/core/src/icon/MaestroICON.png similarity index 100% rename from server/src/icon/MaestroICON.png rename to core/src/icon/MaestroICON.png diff --git a/server/src/icon/MaestroICON.svg b/core/src/icon/MaestroICON.svg similarity index 100% rename from server/src/icon/MaestroICON.svg rename to core/src/icon/MaestroICON.svg diff --git a/server/src/iso22133.c b/core/src/iso22133.c similarity index 100% rename from server/src/iso22133.c rename to core/src/iso22133.c diff --git a/server/src/logger.c b/core/src/logger.c similarity index 100% rename from server/src/logger.c rename to core/src/logger.c diff --git a/server/src/main.c b/core/src/main.c similarity index 100% rename from server/src/main.c rename to core/src/main.c diff --git a/server/src/objectcontrol.c b/core/src/objectcontrol.c similarity index 100% rename from server/src/objectcontrol.c rename to core/src/objectcontrol.c diff --git a/server/src/positioning.c b/core/src/positioning.c similarity index 100% rename from server/src/positioning.c rename to core/src/positioning.c diff --git a/server/src/remotecontrol.c b/core/src/remotecontrol.c similarity index 100% rename from server/src/remotecontrol.c rename to core/src/remotecontrol.c diff --git a/server/src/simulatorcontrol.c b/core/src/simulatorcontrol.c similarity index 100% rename from server/src/simulatorcontrol.c rename to core/src/simulatorcontrol.c diff --git a/server/src/supervisorcontrol.c b/core/src/supervisorcontrol.c similarity index 100% rename from server/src/supervisorcontrol.c rename to core/src/supervisorcontrol.c diff --git a/server/src/systemcontrol.c b/core/src/systemcontrol.c similarity index 100% rename from server/src/systemcontrol.c rename to core/src/systemcontrol.c diff --git a/server/src/timecontrol.c b/core/src/timecontrol.c similarity index 100% rename from server/src/timecontrol.c rename to core/src/timecontrol.c diff --git a/server/src/util.c b/core/src/util.c similarity index 100% rename from server/src/util.c rename to core/src/util.c diff --git a/server/traj/0.traj b/core/traj/0.traj similarity index 100% rename from server/traj/0.traj rename to core/traj/0.traj diff --git a/server/traj/1.traj b/core/traj/1.traj similarity index 100% rename from server/traj/1.traj rename to core/traj/1.traj diff --git a/server/traj/192.168.0.17 b/core/traj/192.168.0.17 similarity index 100% rename from server/traj/192.168.0.17 rename to core/traj/192.168.0.17 diff --git a/server/traj/DriveFiles_Curve_v3/DriveFile_1.csv b/core/traj/DriveFiles_Curve_v3/DriveFile_1.csv similarity index 100% rename from server/traj/DriveFiles_Curve_v3/DriveFile_1.csv rename to core/traj/DriveFiles_Curve_v3/DriveFile_1.csv diff --git a/server/traj/DriveFiles_Curve_v3/DriveFile_2.csv b/core/traj/DriveFiles_Curve_v3/DriveFile_2.csv similarity index 100% rename from server/traj/DriveFiles_Curve_v3/DriveFile_2.csv rename to core/traj/DriveFiles_Curve_v3/DriveFile_2.csv diff --git a/server/traj/DriveFiles_Curve_v3/DriveFile_3.csv b/core/traj/DriveFiles_Curve_v3/DriveFile_3.csv similarity index 100% rename from server/traj/DriveFiles_Curve_v3/DriveFile_3.csv rename to core/traj/DriveFiles_Curve_v3/DriveFile_3.csv diff --git a/server/traj/DriveFiles_Curve_v3/DriveFile_4.csv b/core/traj/DriveFiles_Curve_v3/DriveFile_4.csv similarity index 100% rename from server/traj/DriveFiles_Curve_v3/DriveFile_4.csv rename to core/traj/DriveFiles_Curve_v3/DriveFile_4.csv diff --git a/server/traj/DriveFiles_Curve_v3/DriveFile_5.csv b/core/traj/DriveFiles_Curve_v3/DriveFile_5.csv similarity index 100% rename from server/traj/DriveFiles_Curve_v3/DriveFile_5.csv rename to core/traj/DriveFiles_Curve_v3/DriveFile_5.csv diff --git a/server/traj/DriveFiles_DriveStraight_v6/DriveFile_1.csv b/core/traj/DriveFiles_DriveStraight_v6/DriveFile_1.csv similarity index 100% rename from server/traj/DriveFiles_DriveStraight_v6/DriveFile_1.csv rename to core/traj/DriveFiles_DriveStraight_v6/DriveFile_1.csv diff --git a/server/traj/DriveFiles_DriveStraight_v6/DriveFile_2.csv b/core/traj/DriveFiles_DriveStraight_v6/DriveFile_2.csv similarity index 100% rename from server/traj/DriveFiles_DriveStraight_v6/DriveFile_2.csv rename to core/traj/DriveFiles_DriveStraight_v6/DriveFile_2.csv diff --git a/server/traj/DriveFiles_DriveStraight_v6/DriveFile_3.csv b/core/traj/DriveFiles_DriveStraight_v6/DriveFile_3.csv similarity index 100% rename from server/traj/DriveFiles_DriveStraight_v6/DriveFile_3.csv rename to core/traj/DriveFiles_DriveStraight_v6/DriveFile_3.csv diff --git a/server/traj/DriveFiles_DriveStraight_v6/DriveFile_4.csv b/core/traj/DriveFiles_DriveStraight_v6/DriveFile_4.csv similarity index 100% rename from server/traj/DriveFiles_DriveStraight_v6/DriveFile_4.csv rename to core/traj/DriveFiles_DriveStraight_v6/DriveFile_4.csv diff --git a/server/traj/DriveFiles_DriveStraight_v6/DriveFile_5.csv b/core/traj/DriveFiles_DriveStraight_v6/DriveFile_5.csv similarity index 100% rename from server/traj/DriveFiles_DriveStraight_v6/DriveFile_5.csv rename to core/traj/DriveFiles_DriveStraight_v6/DriveFile_5.csv diff --git a/server/traj/GarageLastbilRektangel2.traj b/core/traj/GarageLastbilRektangel2.traj similarity index 100% rename from server/traj/GarageLastbilRektangel2.traj rename to core/traj/GarageLastbilRektangel2.traj diff --git a/server/traj/GarageRektangelInre3.traj b/core/traj/GarageRektangelInre3.traj similarity index 100% rename from server/traj/GarageRektangelInre3.traj rename to core/traj/GarageRektangelInre3.traj diff --git a/server/traj/GarageRektangelYttre3.traj b/core/traj/GarageRektangelYttre3.traj similarity index 100% rename from server/traj/GarageRektangelYttre3.traj rename to core/traj/GarageRektangelYttre3.traj diff --git a/server/traj/GarageplanInnerring.traj b/core/traj/GarageplanInnerring.traj similarity index 100% rename from server/traj/GarageplanInnerring.traj rename to core/traj/GarageplanInnerring.traj diff --git a/server/traj/GarageplanYtterring.traj b/core/traj/GarageplanYtterring.traj similarity index 100% rename from server/traj/GarageplanYtterring.traj rename to core/traj/GarageplanYtterring.traj diff --git a/server/traj/README.txt b/core/traj/README.txt similarity index 100% rename from server/traj/README.txt rename to core/traj/README.txt diff --git a/server/traj/RuralRoadTruck1.traj b/core/traj/RuralRoadTruck1.traj similarity index 100% rename from server/traj/RuralRoadTruck1.traj rename to core/traj/RuralRoadTruck1.traj diff --git a/server/traj/TestScenario_Intersection/192.168.0.1 b/core/traj/TestScenario_Intersection/192.168.0.1 similarity index 100% rename from server/traj/TestScenario_Intersection/192.168.0.1 rename to core/traj/TestScenario_Intersection/192.168.0.1 diff --git a/server/traj/TestScenario_Intersection/192.168.0.2 b/core/traj/TestScenario_Intersection/192.168.0.2 similarity index 100% rename from server/traj/TestScenario_Intersection/192.168.0.2 rename to core/traj/TestScenario_Intersection/192.168.0.2 diff --git a/server/traj/TestScenario_Intersection/192.168.0.3 b/core/traj/TestScenario_Intersection/192.168.0.3 similarity index 100% rename from server/traj/TestScenario_Intersection/192.168.0.3 rename to core/traj/TestScenario_Intersection/192.168.0.3 diff --git a/server/traj/TestScenario_Intersection/192.168.0.4 b/core/traj/TestScenario_Intersection/192.168.0.4 similarity index 100% rename from server/traj/TestScenario_Intersection/192.168.0.4 rename to core/traj/TestScenario_Intersection/192.168.0.4 diff --git a/server/traj/TestScenario_Intersection/192.168.0.5 b/core/traj/TestScenario_Intersection/192.168.0.5 similarity index 100% rename from server/traj/TestScenario_Intersection/192.168.0.5 rename to core/traj/TestScenario_Intersection/192.168.0.5 diff --git a/server/traj/aliv_rect.traj b/core/traj/aliv_rect.traj similarity index 100% rename from server/traj/aliv_rect.traj rename to core/traj/aliv_rect.traj diff --git a/server/traj/fengco_drivefile_ego.traj b/core/traj/fengco_drivefile_ego.traj similarity index 100% rename from server/traj/fengco_drivefile_ego.traj rename to core/traj/fengco_drivefile_ego.traj diff --git a/server/traj/garageh.chronos b/core/traj/garageh.chronos similarity index 100% rename from server/traj/garageh.chronos rename to core/traj/garageh.chronos diff --git a/server/traj/garagev.chronos b/core/traj/garagev.chronos similarity index 100% rename from server/traj/garagev.chronos rename to core/traj/garagev.chronos diff --git a/server/traj/rectangle1.traj b/core/traj/rectangle1.traj similarity index 100% rename from server/traj/rectangle1.traj rename to core/traj/rectangle1.traj diff --git a/server/traj/rural2.traj b/core/traj/rural2.traj similarity index 100% rename from server/traj/rural2.traj rename to core/traj/rural2.traj diff --git a/server/traj/ruralmid2.traj b/core/traj/ruralmid2.traj similarity index 100% rename from server/traj/ruralmid2.traj rename to core/traj/ruralmid2.traj diff --git a/server/traj/ruralright2.traj b/core/traj/ruralright2.traj similarity index 100% rename from server/traj/ruralright2.traj rename to core/traj/ruralright2.traj diff --git a/server/traj/safety.traj b/core/traj/safety.traj similarity index 100% rename from server/traj/safety.traj rename to core/traj/safety.traj diff --git a/server/traj/straightLineAccelDecel_30kmh.traj b/core/traj/straightLineAccelDecel_30kmh.traj similarity index 100% rename from server/traj/straightLineAccelDecel_30kmh.traj rename to core/traj/straightLineAccelDecel_30kmh.traj diff --git a/server/CMakeLists.txt b/server/CMakeLists.txt deleted file mode 100644 index eb9be870a..000000000 --- a/server/CMakeLists.txt +++ /dev/null @@ -1,129 +0,0 @@ -cmake_minimum_required (VERSION 3.1) -project (Maestro) - -# Set C and C++ dialects -SET(CMAKE_C_STANDARD 11) -SET(CMAKE_C_STANDARD_REQUIRED ON) -SET(CMAKE_CXX_STANDARD 11) -SET(CMAKE_CXX_STANDARD_REQUIRED ON) - -# Standard compiler flags -SET(GCC_COMPILE_FLAGS_WARNINGS "-Wall") # Use -Wall to make all warnings appear, and -Werror to make all of them into errors. To select individual warnings use e.g. -Werror=pointer-sign -SET(GCC_COMPILE_FLAGS_COVERAGE "-fprofile-arcs -ftest-coverage") -SET(GCC_COMPILE_FLAGS_OPTIMISATION "-O0") # Use -O2 for final build - -# Set which compiler flags are used for the build -SET(GCC_COMPILE_FLAGS_ALL "${GCC_COMPILE_FLAGS_WARNINGS}") - -# copy needed file -#configure_file(traj/0.traj traj/192.168.0.1 COPYONLY) -#configure_file(traj/1.traj traj/192.168.0.2 COPYONLY) -#configure_file(conf/test.conf conf/test.conf COPYONLY) - -# add include directories -include_directories(inc) -include(GNUInstallDirs) -if (NOT TARGET MaestroLogging) - include_directories(../util/C/logging) - add_library(MaestroLogging - ../util/C/logging/logging.h - ../util/C/logging/logging.c - ) -endif() - -if (NOT TARGET MaestroTime) - include_directories(../util/C/time) - add_library(MaestroTime - ../util/C/time/maestroTime.h - ../util/C/time/maestroTime.c - ) -endif() - -if (NOT TARGET MQBus) - include_directories(../util/C/MQBus) - add_library(MQBus - ../util/C/MQBus/mqbus.c - ../util/C/MQBus/mqbus.h - ) -endif() - -if (NOT TARGET util) - add_library(util - inc/util.h - src/util.c - inc/iso22133.h - src/iso22133.c - inc/positioning.h - src/positioning.c - ) -endif() - -# Add logging library (hint at some locations) -#find_library(MAESTRO_LOGGING_PATH -# NAMES MaestroLogging -# PATHS "../util/C/logging" -# "../util/C" -# ${CMAKE_INSTALL_LIBDIR} -# ${CMAKE_INSTALL_INCLUDEDIR}) -#add_library(MaestroLogging SHARED IMPORTED) -#set_property(TARGET MaestroLogging PROPERTY IMPORTED_LOCATION ${MAESTRO_LOGGING_PATH}) - -SET(USE_CITS FALSE CACHE BOOL "Flag to indicate the use of CITS.") - - -# MQTT PAHO -if(USE_CITS) - include_directories(../util/ASN1/generatedfiles) - find_package(OpenSSL REQUIRED) - find_library(paho-mqtt3c NAMES libpaho-mqtt3c.so REQUIRED) - add_library(pahomqtt3c SHARED IMPORTED) - set_property(TARGET pahomqtt3c PROPERTY IMPORTED_LOCATION ${paho-mqtt3c}) - add_library(cits - src/citscontrol.c - inc/citscontrol.h - ) - file(GLOB ASN1_HEADERS "../util/ASN1/generatedfiles/*.h") - file(GLOB ASN1_SOURCES "../util/ASN1/generatedfiles/*.c") - add_library(ASN1_ITS - ${ASN1_HEADERS} - ${ASN1_SOURCES} - ) -add_definitions(-DCITS_ENABLED) -endif() - - -# add the executable -add_executable(TEServer - src/main.c - src/logger.c - src/objectcontrol.c - src/systemcontrol.c - src/timecontrol.c - src/datadictionary.c - inc/logger.h - inc/objectcontrol.h - inc/systemcontrol.h - inc/timecontrol.h - inc/datadictionary.h -) - -# add compiler flags for all targets -set_target_properties(TEServer PROPERTIES COMPILE_FLAGS ${GCC_COMPILE_FLAGS_ALL}) - -# add the install targets -install (TARGETS TEServer DESTINATION bin) - -# add linking to message queue -target_link_libraries(util rt m) -target_link_libraries(util m) -target_link_libraries(util rt) -target_link_libraries(util MaestroLogging MaestroTime MQBus) - -target_link_libraries(TEServer util) - -if(USE_CITS) - target_link_libraries(cits pahomqtt3c MaestroTime ASN1_ITS) - target_link_libraries(TEServer cits) -endif() - -target_link_libraries(TEServer MaestroLogging MaestroTime MQBus) diff --git a/server/ubuntuCleanServer b/server/ubuntuCleanServer deleted file mode 100755 index 48eb9f8fb..000000000 --- a/server/ubuntuCleanServer +++ /dev/null @@ -1,10 +0,0 @@ -#!/bin/bash - -echo "Cleaning server ..." -sudo netstat -anp | grep 53251 -sudo rm -rf /dev/mqueue/* -kill $(pidof TEServer) -kill $(pidof DefaultVisualizationAdapter) -kill $(pidof UserControl) -echo "Cleaning server done." - From 9970f0730ec716cce57dd7b56192a6b368350ae8 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 19 Feb 2020 13:29:20 +0100 Subject: [PATCH 273/523] Updated util to ref corresponding commit with new cmakelists structure --- util | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/util b/util index 5171c2cb4..99ccf4b7e 160000 --- a/util +++ b/util @@ -1 +1 @@ -Subproject commit 5171c2cb43b958fb4eb5b3df18a92b0e2e9e518f +Subproject commit 99ccf4b7edbfeb4f675847b379c316eb42d365d6 From 2ceac55f13737718dbe6ad1f1e562b2c601d93ce Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 19 Feb 2020 16:23:35 +0100 Subject: [PATCH 274/523] Updated CMakeLists for core to be more flexible --- core/CMakeLists.txt | 52 +++++++++++++++++++++++++-------------------- 1 file changed, 29 insertions(+), 23 deletions(-) diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index 5c7c1df60..e2d350143 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -2,50 +2,56 @@ cmake_minimum_required(VERSION 3.1) project(Core) -add_executable(Core - src/objectcontrol.c - src/logger.c - src/main.c - src/systemcontrol.c - src/timecontrol.c +set(CORE_TARGET ${CMAKE_PROJECT_NAME}) +set(UTIL_TARGET MaestroUtil) +set(TIME_TARGET MaestroTime) +set(LOGGING_TARGET MaestroLogging) +set(MESSAGE_BUS_TARGET MaestroMQ) + +add_executable(${CORE_TARGET} + ${CMAKE_CURRENT_SOURCE_DIR}/src/objectcontrol.c + ${CMAKE_CURRENT_SOURCE_DIR}/src/logger.c + ${CMAKE_CURRENT_SOURCE_DIR}/src/main.c + ${CMAKE_CURRENT_SOURCE_DIR}/src/systemcontrol.c + ${CMAKE_CURRENT_SOURCE_DIR}/src/timecontrol.c ) -add_library(MaestroUtil - src/util.c - src/iso22133.c - src/positioning.c - src/datadictionary.c +add_library(${UTIL_TARGET} + ${CMAKE_CURRENT_SOURCE_DIR}/src/util.c + ${CMAKE_CURRENT_SOURCE_DIR}/src/iso22133.c + ${CMAKE_CURRENT_SOURCE_DIR}/src/positioning.c + ${CMAKE_CURRENT_SOURCE_DIR}/src/datadictionary.c ) -if(NOT TARGET MaestroTime) - add_library(MaestroTime STATIC +if(NOT TARGET ${TIME_TARGET}) + add_library(${TIME_TARGET} STATIC ${CMAKE_CURRENT_SOURCE_DIR}/../util/C/time/maestroTime.c ) endif() -if(NOT TARGET MaestroLogging) - add_library(MaestroLogging STATIC +if(NOT TARGET ${LOGGING_TARGET}) + add_library(${LOGGING_TARGET} STATIC ${CMAKE_CURRENT_SOURCE_DIR}/../util/C/logging/logging.c ) endif() -if(NOT TARGET MaestroMQ) - add_library(MaestroMQ STATIC +if(NOT TARGET ${MESSAGE_BUS_TARGET}) + add_library(${MESSAGE_BUS_TARGET} STATIC ${CMAKE_CURRENT_SOURCE_DIR}/../util/C/mqbus/mqbus.c ) - target_link_libraries(MaestroMQ rt) + target_link_libraries(${MESSAGE_BUS_TARGET} rt) endif() -target_link_libraries(Core LINK_PUBLIC MaestroTime MaestroLogging MaestroUtil) -target_include_directories(Core PUBLIC +target_link_libraries(${CORE_TARGET} LINK_PUBLIC ${TIME_TARGET} ${LOGGING_TARGET} ${UTIL_TARGET}) +target_include_directories(${CORE_TARGET} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/inc ${CMAKE_CURRENT_SOURCE_DIR}/../util/C/time ${CMAKE_CURRENT_SOURCE_DIR}/../util/C/logging ) -target_link_libraries(MaestroUtil LINK_PUBLIC MaestroTime MaestroLogging MaestroMQ) -target_link_libraries(MaestroUtil LINK_PUBLIC m) -target_include_directories(MaestroUtil PUBLIC +target_link_libraries(${UTIL_TARGET} LINK_PUBLIC ${TIME_TARGET} ${LOGGING_TARGET} ${MESSAGE_BUS_TARGET}) +target_link_libraries(${UTIL_TARGET} LINK_PUBLIC m) +target_include_directories(${UTIL_TARGET} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/inc ${CMAKE_CURRENT_SOURCE_DIR}/../util/C/time ${CMAKE_CURRENT_SOURCE_DIR}/../util/C/logging From fc9e304609e820240eb9b96eebf520369db34034 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 19 Feb 2020 16:56:22 +0100 Subject: [PATCH 275/523] Updated ScenarioControl CMakeLists to be more flexible --- modules/ScenarioControl/CMakeLists.txt | 66 +++++++++++++++++++++----- 1 file changed, 53 insertions(+), 13 deletions(-) diff --git a/modules/ScenarioControl/CMakeLists.txt b/modules/ScenarioControl/CMakeLists.txt index 988b9d959..848f000bf 100644 --- a/modules/ScenarioControl/CMakeLists.txt +++ b/modules/ScenarioControl/CMakeLists.txt @@ -6,23 +6,63 @@ set(CMAKE_C_STANDARD_REQUIRED ON) project(ScenarioControl) -add_executable(ScenarioControl - src/action.cpp - src/booleantrigger.cpp - src/braketrigger.cpp - src/causality.cpp - src/externalaction.cpp - src/isotrigger.cpp - src/main.cpp - src/scenario.cpp - src/trigger.cpp +set(SCENARIO_CONTROL_TARGET ${CMAKE_PROJECT_NAME}) +set(UTIL_TARGET MaestroUtil) +set(TIME_TARGET MaestroTime) +set(LOGGING_TARGET MaestroLogging) +set(MESSAGE_BUS_TARGET MaestroMQ) + +add_executable(${SCENARIO_CONTROL_TARGET} + ${CMAKE_CURRENT_SOURCE_DIR}/src/action.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/booleantrigger.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/braketrigger.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/causality.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/externalaction.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/isotrigger.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/main.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/scenario.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/trigger.cpp ) -target_link_libraries(ScenarioControl LINK_PUBLIC MaestroTime MaestroLogging MaestroMQ MaestroUtil) -target_include_directories(ScenarioControl PUBLIC +if(NOT TARGET ${TIME_TARGET}) + add_library(${TIME_TARGET} STATIC + ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C/time/maestroTime.c + ) +endif() + +if(NOT TARGET ${LOGGING_TARGET}) + add_library(${LOGGING_TARGET} STATIC + ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C/logging/logging.c + ) +endif() + +if(NOT TARGET ${MESSAGE_BUS_TARGET}) + add_library(${MESSAGE_BUS_TARGET} STATIC + ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C/mqbus/mqbus.c + ) + target_link_libraries(${MESSAGE_BUS_TARGET} rt) +endif() + +if(NOT TARGET ${UTIL_TARGET}) + add_library(${UTIL_TARGET} STATIC + ${CMAKE_CURRENT_SOURCE_DIR}/../../core/src/util.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../core/src/iso22133.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../core/src/positioning.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../core/src/datadictionary.c + ) + target_link_libraries(${UTIL_TARGET} ${MESSAGE_BUS_TARGET} m) + target_include_directories(${UTIL_TARGET} PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/../../core/inc + ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C/mqbus + ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C/logging + ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C/time + ) +endif() + +target_link_libraries(${SCENARIO_CONTROL_TARGET} LINK_PUBLIC ${TIME_TARGET} ${UTIL_TARGET} ${LOGGING_TARGET}) +target_include_directories(${SCENARIO_CONTROL_TARGET} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/inc ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C/time ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C/logging - ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C/mqbus ${CMAKE_CURRENT_SOURCE_DIR}/../../core/inc ) From 7e96adb70d7604917a54d2f40419c16e98a2fe8d Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 19 Feb 2020 17:00:53 +0100 Subject: [PATCH 276/523] Made CMakeLists for supervision more flexible --- modules/Supervision/CMakeLists.txt | 53 ++++++++++++++++++++++++++---- 1 file changed, 47 insertions(+), 6 deletions(-) diff --git a/modules/Supervision/CMakeLists.txt b/modules/Supervision/CMakeLists.txt index 167e580a1..cc02658c2 100644 --- a/modules/Supervision/CMakeLists.txt +++ b/modules/Supervision/CMakeLists.txt @@ -6,15 +6,56 @@ set(CMAKE_C_STANDARD_REQUIRED ON) project(Supervision) +set(SUPERVISION_TARGET ${CMAKE_PROJECT_NAME}) +set(UTIL_TARGET MaestroUtil) +set(TIME_TARGET MaestroTime) +set(LOGGING_TARGET MaestroLogging) +set(MESSAGE_BUS_TARGET MaestroMQ) + add_executable(Supervision - src/geofence.cpp - src/main.cpp - src/supervisionstate.cpp - src/trajectory.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/geofence.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/main.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/supervisionstate.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/trajectory.cpp ) -target_link_libraries(Supervision LINK_PUBLIC MaestroTime MaestroLogging MaestroMQ MaestroUtil) -target_include_directories(Supervision PUBLIC +if(NOT TARGET ${TIME_TARGET}) + add_library(${TIME_TARGET} STATIC + ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C/time/maestroTime.c + ) +endif() + +if(NOT TARGET ${LOGGING_TARGET}) + add_library(${LOGGING_TARGET} STATIC + ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C/logging/logging.c + ) +endif() + +if(NOT TARGET ${MESSAGE_BUS_TARGET}) + add_library(${MESSAGE_BUS_TARGET} STATIC + ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C/mqbus/mqbus.c + ) + target_link_libraries(${MESSAGE_BUS_TARGET} rt) +endif() + +if(NOT TARGET ${UTIL_TARGET}) + add_library(${UTIL_TARGET} STATIC + ${CMAKE_CURRENT_SOURCE_DIR}/../../core/src/util.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../core/src/iso22133.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../core/src/positioning.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../core/src/datadictionary.c + ) + target_link_libraries(${UTIL_TARGET} ${MESSAGE_BUS_TARGET} m) + target_include_directories(${UTIL_TARGET} PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/../../core/inc + ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C/mqbus + ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C/logging + ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C/time + ) +endif() + +target_link_libraries(${SUPERVISION_TARGET} LINK_PUBLIC ${TIME_TARGET} ${UTIL_TARGET} ${LOGGING_TARGET}) +target_include_directories(${SUPERVISION_TARGET} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/inc ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C/time ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C/logging From bc806dcb88908acfe20bdce310c15161b61f438a Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 19 Feb 2020 17:02:45 +0100 Subject: [PATCH 277/523] Linked util target to maestro time and logging --- modules/Supervision/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/Supervision/CMakeLists.txt b/modules/Supervision/CMakeLists.txt index cc02658c2..e69d5c51b 100644 --- a/modules/Supervision/CMakeLists.txt +++ b/modules/Supervision/CMakeLists.txt @@ -45,7 +45,7 @@ if(NOT TARGET ${UTIL_TARGET}) ${CMAKE_CURRENT_SOURCE_DIR}/../../core/src/positioning.c ${CMAKE_CURRENT_SOURCE_DIR}/../../core/src/datadictionary.c ) - target_link_libraries(${UTIL_TARGET} ${MESSAGE_BUS_TARGET} m) + target_link_libraries(${UTIL_TARGET} ${LOGGING_TARGET} ${TIME_TARGET} ${MESSAGE_BUS_TARGET} m) target_include_directories(${UTIL_TARGET} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/../../core/inc ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C/mqbus From faaa914de62f3335e1aa070a5c5166bd6398514c Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 19 Feb 2020 17:03:34 +0100 Subject: [PATCH 278/523] Another linking thing --- modules/ScenarioControl/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/ScenarioControl/CMakeLists.txt b/modules/ScenarioControl/CMakeLists.txt index 848f000bf..fd96678dd 100644 --- a/modules/ScenarioControl/CMakeLists.txt +++ b/modules/ScenarioControl/CMakeLists.txt @@ -50,7 +50,7 @@ if(NOT TARGET ${UTIL_TARGET}) ${CMAKE_CURRENT_SOURCE_DIR}/../../core/src/positioning.c ${CMAKE_CURRENT_SOURCE_DIR}/../../core/src/datadictionary.c ) - target_link_libraries(${UTIL_TARGET} ${MESSAGE_BUS_TARGET} m) + target_link_libraries(${UTIL_TARGET} ${LOGGING_TARGET} ${TIME_TARGET} ${MESSAGE_BUS_TARGET} m) target_include_directories(${UTIL_TARGET} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/../../core/inc ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C/mqbus From ffc2f7abb543df128d7af48985b5e26aa36a1f30 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 19 Feb 2020 17:33:03 +0100 Subject: [PATCH 279/523] Made paths in CMakeLists into variables to reduce repetition --- modules/ScenarioControl/CMakeLists.txt | 43 +++++++++++++++-------- modules/Supervision/CMakeLists.txt | 47 ++++++++++++++++---------- 2 files changed, 58 insertions(+), 32 deletions(-) diff --git a/modules/ScenarioControl/CMakeLists.txt b/modules/ScenarioControl/CMakeLists.txt index fd96678dd..ec0a08b32 100644 --- a/modules/ScenarioControl/CMakeLists.txt +++ b/modules/ScenarioControl/CMakeLists.txt @@ -6,12 +6,23 @@ set(CMAKE_C_STANDARD_REQUIRED ON) project(ScenarioControl) -set(SCENARIO_CONTROL_TARGET ${CMAKE_PROJECT_NAME}) +# Define target names +set(SCENARIO_CONTROL_TARGET ${PROJECT_NAME}) set(UTIL_TARGET MaestroUtil) set(TIME_TARGET MaestroTime) set(LOGGING_TARGET MaestroLogging) set(MESSAGE_BUS_TARGET MaestroMQ) +# Define paths to related files +set(CORE_ROOT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/../../core) +set(CORE_INCLUDE_PATH ${CORE_ROOT_PATH}/inc) +set(CORE_SOURCE_PATH ${CORE_ROOT_PATH}/src) +set(UTIL_ROOT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C) +set(TIME_PATH ${UTIL_ROOT_PATH}/time) +set(LOGGING_PATH ${UTIL_ROOT_PATH}/logging) +set(MESSAGE_BUS_PATH ${UTIL_ROOT_PATH}/mqbus) + +# Create project main executable target add_executable(${SCENARIO_CONTROL_TARGET} ${CMAKE_CURRENT_SOURCE_DIR}/src/action.cpp ${CMAKE_CURRENT_SOURCE_DIR}/src/booleantrigger.cpp @@ -24,45 +35,47 @@ add_executable(${SCENARIO_CONTROL_TARGET} ${CMAKE_CURRENT_SOURCE_DIR}/src/trigger.cpp ) +# If building locally, it is necessary to add relevant util targets if(NOT TARGET ${TIME_TARGET}) add_library(${TIME_TARGET} STATIC - ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C/time/maestroTime.c + ${TIME_PATH}/maestroTime.c ) endif() if(NOT TARGET ${LOGGING_TARGET}) add_library(${LOGGING_TARGET} STATIC - ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C/logging/logging.c + ${LOGGING_PATH}/logging.c ) endif() if(NOT TARGET ${MESSAGE_BUS_TARGET}) add_library(${MESSAGE_BUS_TARGET} STATIC - ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C/mqbus/mqbus.c + ${MESSAGE_BUS_PATH}/mqbus.c ) target_link_libraries(${MESSAGE_BUS_TARGET} rt) endif() if(NOT TARGET ${UTIL_TARGET}) add_library(${UTIL_TARGET} STATIC - ${CMAKE_CURRENT_SOURCE_DIR}/../../core/src/util.c - ${CMAKE_CURRENT_SOURCE_DIR}/../../core/src/iso22133.c - ${CMAKE_CURRENT_SOURCE_DIR}/../../core/src/positioning.c - ${CMAKE_CURRENT_SOURCE_DIR}/../../core/src/datadictionary.c + ${CORE_SOURCE_PATH}/util.c + ${CORE_SOURCE_PATH}/iso22133.c + ${CORE_SOURCE_PATH}/positioning.c + ${CORE_SOURCE_PATH}/datadictionary.c ) target_link_libraries(${UTIL_TARGET} ${LOGGING_TARGET} ${TIME_TARGET} ${MESSAGE_BUS_TARGET} m) target_include_directories(${UTIL_TARGET} PUBLIC - ${CMAKE_CURRENT_SOURCE_DIR}/../../core/inc - ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C/mqbus - ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C/logging - ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C/time + ${CORE_INCLUDE_PATH} + ${MESSAGE_BUS_PATH} + ${LOGGING_PATH} + ${TIME_PATH} ) endif() +# Link project executable to util libraries target_link_libraries(${SCENARIO_CONTROL_TARGET} LINK_PUBLIC ${TIME_TARGET} ${UTIL_TARGET} ${LOGGING_TARGET}) target_include_directories(${SCENARIO_CONTROL_TARGET} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/inc - ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C/time - ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C/logging - ${CMAKE_CURRENT_SOURCE_DIR}/../../core/inc + ${LOGGING_PATH} + ${TIME_PATH} + ${CORE_INCLUDE_PATH} ) diff --git a/modules/Supervision/CMakeLists.txt b/modules/Supervision/CMakeLists.txt index e69d5c51b..48d949c32 100644 --- a/modules/Supervision/CMakeLists.txt +++ b/modules/Supervision/CMakeLists.txt @@ -6,59 +6,72 @@ set(CMAKE_C_STANDARD_REQUIRED ON) project(Supervision) -set(SUPERVISION_TARGET ${CMAKE_PROJECT_NAME}) +# Define target names +set(SUPERVISION_TARGET ${PROJECT_NAME}) set(UTIL_TARGET MaestroUtil) set(TIME_TARGET MaestroTime) set(LOGGING_TARGET MaestroLogging) set(MESSAGE_BUS_TARGET MaestroMQ) -add_executable(Supervision +# Define paths to related files +set(CORE_ROOT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/../../core) +set(CORE_INCLUDE_PATH ${CORE_ROOT_PATH}/inc) +set(CORE_SOURCE_PATH ${CORE_ROOT_PATH}/src) +set(UTIL_ROOT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C) +set(TIME_PATH ${UTIL_ROOT_PATH}/time) +set(LOGGING_PATH ${UTIL_ROOT_PATH}/logging) +set(MESSAGE_BUS_PATH ${UTIL_ROOT_PATH}/mqbus) + +# Create project main executable target +add_executable(${SUPERVISION_TARGET} ${CMAKE_CURRENT_SOURCE_DIR}/src/geofence.cpp ${CMAKE_CURRENT_SOURCE_DIR}/src/main.cpp ${CMAKE_CURRENT_SOURCE_DIR}/src/supervisionstate.cpp ${CMAKE_CURRENT_SOURCE_DIR}/src/trajectory.cpp ) +# If building locally, it is necessary to add relevant util targets if(NOT TARGET ${TIME_TARGET}) add_library(${TIME_TARGET} STATIC - ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C/time/maestroTime.c + ${TIME_PATH}/maestroTime.c ) endif() if(NOT TARGET ${LOGGING_TARGET}) add_library(${LOGGING_TARGET} STATIC - ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C/logging/logging.c + ${LOGGING_PATH}/logging.c ) endif() if(NOT TARGET ${MESSAGE_BUS_TARGET}) add_library(${MESSAGE_BUS_TARGET} STATIC - ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C/mqbus/mqbus.c + ${MESSAGE_BUS_PATH}/mqbus.c ) target_link_libraries(${MESSAGE_BUS_TARGET} rt) endif() if(NOT TARGET ${UTIL_TARGET}) add_library(${UTIL_TARGET} STATIC - ${CMAKE_CURRENT_SOURCE_DIR}/../../core/src/util.c - ${CMAKE_CURRENT_SOURCE_DIR}/../../core/src/iso22133.c - ${CMAKE_CURRENT_SOURCE_DIR}/../../core/src/positioning.c - ${CMAKE_CURRENT_SOURCE_DIR}/../../core/src/datadictionary.c + ${CORE_SOURCE_PATH}/util.c + ${CORE_SOURCE_PATH}/iso22133.c + ${CORE_SOURCE_PATH}/positioning.c + ${CORE_SOURCE_PATH}/datadictionary.c ) target_link_libraries(${UTIL_TARGET} ${LOGGING_TARGET} ${TIME_TARGET} ${MESSAGE_BUS_TARGET} m) target_include_directories(${UTIL_TARGET} PUBLIC - ${CMAKE_CURRENT_SOURCE_DIR}/../../core/inc - ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C/mqbus - ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C/logging - ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C/time + ${CORE_INCLUDE_PATH} + ${MESSAGE_BUS_PATH} + ${LOGGING_PATH} + ${TIME_PATH} ) endif() +# Link project executable to util libraries target_link_libraries(${SUPERVISION_TARGET} LINK_PUBLIC ${TIME_TARGET} ${UTIL_TARGET} ${LOGGING_TARGET}) target_include_directories(${SUPERVISION_TARGET} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/inc - ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C/time - ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C/logging - ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C/mqbus - ${CMAKE_CURRENT_SOURCE_DIR}/../../core/inc + ${TIME_PATH} + ${LOGGING_PATH} + ${MESSAGE_BUS_PATH} + ${CORE_INCLUDE_PATH} ) From 78eab674ec92caa2f1c20d2a037d38b336e7afb3 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 19 Feb 2020 17:51:10 +0100 Subject: [PATCH 280/523] Fixed up visualization and dummy similarly --- modules/Visualization/CMakeLists.txt | 72 +++++++++++++++++--- modules/dummy/CMakeLists.txt | 99 ++++++++++++++++++---------- 2 files changed, 125 insertions(+), 46 deletions(-) diff --git a/modules/Visualization/CMakeLists.txt b/modules/Visualization/CMakeLists.txt index 02ec16c28..56156e740 100644 --- a/modules/Visualization/CMakeLists.txt +++ b/modules/Visualization/CMakeLists.txt @@ -1,18 +1,70 @@ cmake_minimum_required(VERSION 3.1) +set(CMAKE_C_STANDARD 11) +set(CMAKE_C_STANDARD_REQUIRED ON) project(Visualization) -# Set C and C++ dialects -set(CMAKE_C_STANDARD 11) -set(CMAKE_C_STANDARD_REQUIRED ON) +# Define target names +set(VISUALIZATION_TARGET ${PROJECT_NAME}) +set(UTIL_TARGET MaestroUtil) +set(TIME_TARGET MaestroTime) +set(LOGGING_TARGET MaestroLogging) +set(MESSAGE_BUS_TARGET MaestroMQ) + +# Define paths to related files +set(CORE_ROOT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/../../core) +set(CORE_INCLUDE_PATH ${CORE_ROOT_PATH}/inc) +set(CORE_SOURCE_PATH ${CORE_ROOT_PATH}/src) +set(UTIL_ROOT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C) +set(TIME_PATH ${UTIL_ROOT_PATH}/time) +set(LOGGING_PATH ${UTIL_ROOT_PATH}/logging) +set(MESSAGE_BUS_PATH ${UTIL_ROOT_PATH}/mqbus) -add_executable(Visualization - src/main.c +# Create project main executable target +add_executable(${VISUALIZATION_TARGET} + ${CMAKE_CURRENT_SOURCE_DIR}/src/main.c ) -target_link_libraries(Visualization LINK_PUBLIC MaestroTime MaestroLogging MaestroUtil) -target_include_directories(Visualization PUBLIC - ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C/time - ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C/logging - ${CMAKE_CURRENT_SOURCE_DIR}/../../core/inc +# If building locally, it is necessary to add relevant util targets +if(NOT TARGET ${TIME_TARGET}) + add_library(${TIME_TARGET} STATIC + ${TIME_PATH}/maestroTime.c + ) +endif() + +if(NOT TARGET ${LOGGING_TARGET}) + add_library(${LOGGING_TARGET} STATIC + ${LOGGING_PATH}/logging.c + ) +endif() + +if(NOT TARGET ${MESSAGE_BUS_TARGET}) + add_library(${MESSAGE_BUS_TARGET} STATIC + ${MESSAGE_BUS_PATH}/mqbus.c + ) + target_link_libraries(${MESSAGE_BUS_TARGET} rt) +endif() + +if(NOT TARGET ${UTIL_TARGET}) + add_library(${UTIL_TARGET} STATIC + ${CORE_SOURCE_PATH}/util.c + ${CORE_SOURCE_PATH}/iso22133.c + ${CORE_SOURCE_PATH}/positioning.c + ${CORE_SOURCE_PATH}/datadictionary.c + ) + target_link_libraries(${UTIL_TARGET} ${LOGGING_TARGET} ${TIME_TARGET} ${MESSAGE_BUS_TARGET} m) + target_include_directories(${UTIL_TARGET} PUBLIC + ${CORE_INCLUDE_PATH} + ${MESSAGE_BUS_PATH} + ${LOGGING_PATH} + ${TIME_PATH} + ) +endif() + +# Link project executable to util libraries +target_link_libraries(${VISUALIZATION_TARGET} LINK_PUBLIC ${TIME_TARGET} ${LOGGING_TARGET} ${UTIL_TARGET}) +target_include_directories(${VISUALIZATION_TARGET} PUBLIC + ${TIME_PATH} + ${LOGGING_PATH} + ${CORE_INCLUDE_PATH} ) diff --git a/modules/dummy/CMakeLists.txt b/modules/dummy/CMakeLists.txt index 580c7141f..d6a7499bc 100644 --- a/modules/dummy/CMakeLists.txt +++ b/modules/dummy/CMakeLists.txt @@ -1,46 +1,73 @@ -cmake_minimum_required(VERSION 2.8) +cmake_minimum_required(VERSION 3.1) +set(CMAKE_CXX_STANDARD 11) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_C_STANDARD 11) +set(CMAKE_C_STANDARD_REQUIRED ON) -project(DummyModule) -# This module is an example of how to set up a new module external to the Maestro executable +project(dummy) +# Define target names +set(DUMMY_TARGET ${PROJECT_NAME}) +set(UTIL_TARGET MaestroUtil) +set(TIME_TARGET MaestroTime) +set(LOGGING_TARGET MaestroLogging) +set(MESSAGE_BUS_TARGET MaestroMQ) -include_directories(inc) -include_directories(../../util/C/logging) -include_directories(../../util/C/time) -include_directories(../../util/C/MQBus) -include_directories(../../server/inc) -include(GNUInstallDirs) +# Define paths to related files +set(CORE_ROOT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/../../core) +set(CORE_INCLUDE_PATH ${CORE_ROOT_PATH}/inc) +set(CORE_SOURCE_PATH ${CORE_ROOT_PATH}/src) +set(UTIL_ROOT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C) +set(TIME_PATH ${UTIL_ROOT_PATH}/time) +set(LOGGING_PATH ${UTIL_ROOT_PATH}/logging) +set(MESSAGE_BUS_PATH ${UTIL_ROOT_PATH}/mqbus) - -# Create library -add_library(MaestroLogging - ../../util/C/logging/logging.h - ../../util/C/logging/logging.c +# Create project main executable target +add_executable(${DUMMY_TARGET} + ${CMAKE_CURRENT_SOURCE_DIR}/src/main.cpp ) -add_library(MaestroTime - ../../util/C/time/maestroTime.h - ../../util/C/time/maestroTime.c -) +# If building locally, it is necessary to add relevant util targets +if(NOT TARGET ${TIME_TARGET}) + add_library(${TIME_TARGET} STATIC + ${TIME_PATH}/maestroTime.c + ) +endif() -add_library(MQBus - ../../util/C/MQBus/mqbus.h - ../../util/C/MQBus/mqbus.c -) - -# Create library -add_library(util - ../../server/src/util.c - ../../server/inc/util.h -) - -add_executable(dummy src/main.cpp) - -install(TARGETS dummy DESTINATION bin) - -target_link_libraries(dummy MaestroTime MaestroLogging util) -target_link_libraries(util MQBus MaestroTime MaestroLogging) -target_link_libraries(MQBus rt m) +if(NOT TARGET ${LOGGING_TARGET}) + add_library(${LOGGING_TARGET} STATIC + ${LOGGING_PATH}/logging.c + ) +endif() +if(NOT TARGET ${MESSAGE_BUS_TARGET}) + add_library(${MESSAGE_BUS_TARGET} STATIC + ${MESSAGE_BUS_PATH}/mqbus.c + ) + target_link_libraries(${MESSAGE_BUS_TARGET} rt) +endif() +if(NOT TARGET ${UTIL_TARGET}) + add_library(${UTIL_TARGET} STATIC + ${CORE_SOURCE_PATH}/util.c + ${CORE_SOURCE_PATH}/iso22133.c + ${CORE_SOURCE_PATH}/positioning.c + ${CORE_SOURCE_PATH}/datadictionary.c + ) + target_link_libraries(${UTIL_TARGET} ${LOGGING_TARGET} ${TIME_TARGET} ${MESSAGE_BUS_TARGET} m) + target_include_directories(${UTIL_TARGET} PUBLIC + ${CORE_INCLUDE_PATH} + ${MESSAGE_BUS_PATH} + ${LOGGING_PATH} + ${TIME_PATH} + ) +endif() +# Link project executable to util libraries +target_link_libraries(${DUMMY_TARGET} LINK_PUBLIC ${TIME_TARGET} ${UTIL_TARGET} ${LOGGING_TARGET}) +target_include_directories(${DUMMY_TARGET} PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/inc + ${LOGGING_PATH} + ${TIME_PATH} + ${CORE_INCLUDE_PATH} +) From b1ea0113cce3d9f6af273f3f3d127bc4a6a7a015 Mon Sep 17 00:00:00 2001 From: Viktor Johansson Date: Thu, 20 Feb 2020 07:42:26 +0100 Subject: [PATCH 281/523] Added a missing command from Architecture document. --- server/src/systemcontrol.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index 457b863ce..137300883 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -783,7 +783,7 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { ControlResponseBuffer, 0); } - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "UploadFile:", + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "SubUploadFile:", ControlResponseBuffer, 1, &ClientSocket, 0); } From 2371be34ceff923380c8a9f702af01254547fe8e Mon Sep 17 00:00:00 2001 From: Viktor Johansson Date: Thu, 20 Feb 2020 08:21:02 +0100 Subject: [PATCH 282/523] Added a no-parameter GetDirectoryContent. --- server/src/systemcontrol.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index 137300883..149334a10 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -120,7 +120,7 @@ typedef enum { Idle_0, GetServerStatus_0, ArmScenario_0, DisarmScenario_0, StartScenario_1, stop_0, AbortScenario_0, InitializeScenario_0, ConnectObject_0, DisconnectObject_0, GetServerParameterList_0, SetServerParameter_2, GetServerParameter_1, - DownloadFile_1, UploadFile_3, CheckFileDirectoryExist_1, GetDirectoryContent_1, + DownloadFile_1, UploadFile_3, CheckFileDirectoryExist_1, GetDirectoryContent_0, GetDirectoryContent_1, DeleteFileDirectory_1, CreateDirectory_1, GetTestOrigin_0, replay_1, control_0, Exit_0, start_ext_trigg_1, nocommand } SystemControlCommand_t; @@ -130,7 +130,7 @@ const char *SystemControlCommandsArr[] = { "AbortScenario_0", "InitializeScenario_0", "ConnectObject_0", "DisconnectObject_0", "GetServerParameterList_0", "SetServerParameter_2", "GetServerParameter_1", "DownloadFile_1", "UploadFile_3", "CheckFileDirectoryExist_1", - "GetDirectoryContent_1", + "GetDirectoryContent_0", "GetDirectoryContent_1", "DeleteFileDirectory_1", "CreateDirectory_1", "GetTestOrigin_0", "replay_1", "control_0", "Exit_0", "start_ext_trigg_1" }; @@ -706,6 +706,7 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { SystemControlCommand = Idle_0; } break; + case GetDirectoryContent_0: case GetDirectoryContent_1: if (CurrentInputArgCount == CommandArgCount) { SystemControlCommand = Idle_0; From 36927f2100b90a76506499d9899f01c3cc17114c Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 20 Feb 2020 09:32:54 +0100 Subject: [PATCH 283/523] Ran code formatter --- server/src/iso22133.c | 105 +++++++++++++++++++------------------ server/src/objectcontrol.c | 53 ++++++++++--------- 2 files changed, 84 insertions(+), 74 deletions(-) diff --git a/server/src/iso22133.c b/server/src/iso22133.c index 8a5e1ea82..74cfb331f 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -82,8 +82,7 @@ typedef enum { #pragma pack(push,1) // Ensure sizeof() is useable for (most) network byte lengths /*! ISO message header */ -typedef struct -{ +typedef struct { uint16_t SyncWordU16; uint8_t TransmitterIdU8; uint8_t MessageCounterU8; @@ -94,8 +93,7 @@ typedef struct /*! ISO message footer */ -typedef struct -{ +typedef struct { uint16_t Crc; } FooterType; @@ -687,9 +685,9 @@ ISOMessageID getISOMessageType(const char *messageData, const size_t length, con * EMSGSIZE if trajectory name is too long */ ssize_t encodeTRAJMessageHeader(const uint16_t trajectoryID, const uint16_t trajectoryVersion, - const char * trajectoryName, const size_t nameLength, + const char *trajectoryName, const size_t nameLength, const uint32_t numberOfPointsInTraj, char *trajDataBuffer, - const size_t bufferLength, const char debug){ + const size_t bufferLength, const char debug) { TRAJHeaderType TRAJData; @@ -719,7 +717,8 @@ ssize_t encodeTRAJMessageHeader(const uint16_t trajectoryID, const uint16_t traj // Construct ISO header TRAJData.header = buildISOHeader(MESSAGE_ID_TRAJ, sizeof (TRAJHeaderType) - + numberOfPointsInTraj * sizeof (TRAJPointType) + sizeof (TRAJFooterType), debug); + + numberOfPointsInTraj * sizeof (TRAJPointType) + + sizeof (TRAJFooterType), debug); // Fill contents TRAJData.trajectoryIDValueID = VALUE_ID_TRAJ_TRAJECTORY_IDENTIFIER; @@ -788,8 +787,9 @@ ssize_t encodeTRAJMessageHeader(const uint16_t trajectoryID, const uint16_t traj * EINVAL if one of the input parameters are invalid * ENOBUFS if supplied buffer is too small to hold point */ -ssize_t encodeTRAJMessagePoint(const struct timeval * pointTimeFromStart, const CartesianPosition position, const SpeedType speed, - const AccelerationType acceleration, const float curvature, char * trajDataBufferPointer, +ssize_t encodeTRAJMessagePoint(const struct timeval *pointTimeFromStart, const CartesianPosition position, + const SpeedType speed, const AccelerationType acceleration, + const float curvature, char *trajDataBufferPointer, const size_t remainingBufferLength, const char debug) { TRAJPointType TRAJData; @@ -808,7 +808,8 @@ ssize_t encodeTRAJMessagePoint(const struct timeval * pointTimeFromStart, const // Fill contents TRAJData.relativeTimeValueID = VALUE_ID_TRAJ_RELATIVE_TIME; TRAJData.relativeTimeContentLength = sizeof (TRAJData.relativeTime); - TRAJData.relativeTime = (uint32_t) TimeGetAsUTCms(pointTimeFromStart) / (1000 / RELATIVE_TIME_ONE_SECOND_VALUE); + TRAJData.relativeTime = + (uint32_t) TimeGetAsUTCms(pointTimeFromStart) / (1000 / RELATIVE_TIME_ONE_SECOND_VALUE); TRAJData.xPositionValueID = VALUE_ID_TRAJ_X_POSITION; TRAJData.xPositionContentLength = sizeof (TRAJData.xPosition); @@ -831,7 +832,7 @@ ssize_t encodeTRAJMessagePoint(const struct timeval * pointTimeFromStart, const TRAJData.headingContentLength = sizeof (TRAJData.heading); if (position.isHeadingValid) { TRAJData.heading = (uint16_t) (mapHostHeadingToISOHeading(position.heading_rad) - * 180.0 / M_PI * HEADING_ONE_DEGREE_VALUE); + * 180.0 / M_PI * HEADING_ONE_DEGREE_VALUE); } else { errno = EINVAL; @@ -858,12 +859,16 @@ ssize_t encodeTRAJMessagePoint(const struct timeval * pointTimeFromStart, const TRAJData.lateralAccelerationValueID = VALUE_ID_TRAJ_LATERAL_ACCELERATION; TRAJData.lateralAccelerationContentLength = sizeof (TRAJData.lateralAcceleration); if (acceleration.isValid) { - TRAJData.longitudinalAcceleration = (int16_t) (acceleration.longitudinal_m_s2 * ACCELERATION_ONE_METER_PER_SECOND_SQUARED_VALUE); - TRAJData.lateralAcceleration = (int16_t) (acceleration.lateral_m_s2 * ACCELERATION_ONE_METER_PER_SECOND_SQUARED_VALUE); + TRAJData.longitudinalAcceleration = + (int16_t) (acceleration.longitudinal_m_s2 * ACCELERATION_ONE_METER_PER_SECOND_SQUARED_VALUE); + TRAJData.lateralAcceleration = + (int16_t) (acceleration.lateral_m_s2 * ACCELERATION_ONE_METER_PER_SECOND_SQUARED_VALUE); } else { - LogMessage(LOG_LEVEL_WARNING, "TRAJ acceleration supplied not valid: assuming longitudinal still valid"); - TRAJData.longitudinalAcceleration = (int16_t) (acceleration.longitudinal_m_s2 * ACCELERATION_ONE_METER_PER_SECOND_SQUARED_VALUE); + LogMessage(LOG_LEVEL_WARNING, + "TRAJ acceleration supplied not valid: assuming longitudinal still valid"); + TRAJData.longitudinalAcceleration = + (int16_t) (acceleration.longitudinal_m_s2 * ACCELERATION_ONE_METER_PER_SECOND_SQUARED_VALUE); TRAJData.lateralAcceleration = ACCELERATION_UNAVAILABLE_VALUE; } @@ -918,36 +923,36 @@ ssize_t encodeTRAJMessagePoint(const struct timeval * pointTimeFromStart, const } // Convert from host endianness to little endian - TRAJData.relativeTimeValueID = htole16(TRAJData.relativeTimeValueID); - TRAJData.relativeTimeContentLength = htole16(TRAJData.relativeTimeContentLength); - TRAJData.relativeTime = htole32(TRAJData.relativeTime); - TRAJData.xPositionValueID = htole16(TRAJData.xPositionValueID); - TRAJData.xPositionContentLength = htole16(TRAJData.xPositionContentLength); - TRAJData.xPosition = (int32_t) htole32(TRAJData.xPosition); - TRAJData.yPositionValueID = htole16(TRAJData.yPositionValueID); - TRAJData.yPositionContentLength = htole16(TRAJData.yPositionContentLength); - TRAJData.yPosition = (int32_t) htole32(TRAJData.yPosition); - TRAJData.zPositionValueID = htole16(TRAJData.zPositionValueID); - TRAJData.zPositionContentLength = htole16(TRAJData.zPositionContentLength); - TRAJData.zPosition = (int32_t) htole32(TRAJData.zPosition); - TRAJData.headingValueID = htole16(TRAJData.headingValueID); - TRAJData.headingContentLength = htole16(TRAJData.headingContentLength); - TRAJData.heading = htole16(TRAJData.heading); - TRAJData.longitudinalSpeedValueID = htole16(TRAJData.longitudinalSpeedValueID); - TRAJData.longitudinalSpeedContentLength = htole16(TRAJData.longitudinalSpeedContentLength); - TRAJData.longitudinalSpeed = (int16_t) htole16(TRAJData.longitudinalSpeed); - TRAJData.lateralSpeedValueID = htole16(TRAJData.lateralSpeedValueID); - TRAJData.lateralSpeedContentLength = htole16(TRAJData.lateralSpeedContentLength); - TRAJData.lateralSpeed = (int16_t) htole16(TRAJData.lateralSpeed); - TRAJData.longitudinalAccelerationValueID = htole16(TRAJData.longitudinalAccelerationValueID); - TRAJData.longitudinalAccelerationContentLength = htole16(TRAJData.longitudinalAccelerationContentLength); - TRAJData.longitudinalAcceleration = (int16_t) htole16(TRAJData.longitudinalAcceleration); - TRAJData.lateralAccelerationValueID = htole16(TRAJData.lateralAccelerationValueID); - TRAJData.lateralAccelerationContentLength = htole16(TRAJData.lateralAccelerationContentLength); - TRAJData.lateralAcceleration = (int16_t) htole16(TRAJData.lateralAcceleration); - TRAJData.curvatureValueID = htole16(TRAJData.curvatureValueID); - TRAJData.curvatureContentLength = htole16(TRAJData.curvatureContentLength); - TRAJData.curvature = htolef(TRAJData.curvature); + TRAJData.relativeTimeValueID = htole16(TRAJData.relativeTimeValueID); + TRAJData.relativeTimeContentLength = htole16(TRAJData.relativeTimeContentLength); + TRAJData.relativeTime = htole32(TRAJData.relativeTime); + TRAJData.xPositionValueID = htole16(TRAJData.xPositionValueID); + TRAJData.xPositionContentLength = htole16(TRAJData.xPositionContentLength); + TRAJData.xPosition = (int32_t) htole32(TRAJData.xPosition); + TRAJData.yPositionValueID = htole16(TRAJData.yPositionValueID); + TRAJData.yPositionContentLength = htole16(TRAJData.yPositionContentLength); + TRAJData.yPosition = (int32_t) htole32(TRAJData.yPosition); + TRAJData.zPositionValueID = htole16(TRAJData.zPositionValueID); + TRAJData.zPositionContentLength = htole16(TRAJData.zPositionContentLength); + TRAJData.zPosition = (int32_t) htole32(TRAJData.zPosition); + TRAJData.headingValueID = htole16(TRAJData.headingValueID); + TRAJData.headingContentLength = htole16(TRAJData.headingContentLength); + TRAJData.heading = htole16(TRAJData.heading); + TRAJData.longitudinalSpeedValueID = htole16(TRAJData.longitudinalSpeedValueID); + TRAJData.longitudinalSpeedContentLength = htole16(TRAJData.longitudinalSpeedContentLength); + TRAJData.longitudinalSpeed = (int16_t) htole16(TRAJData.longitudinalSpeed); + TRAJData.lateralSpeedValueID = htole16(TRAJData.lateralSpeedValueID); + TRAJData.lateralSpeedContentLength = htole16(TRAJData.lateralSpeedContentLength); + TRAJData.lateralSpeed = (int16_t) htole16(TRAJData.lateralSpeed); + TRAJData.longitudinalAccelerationValueID = htole16(TRAJData.longitudinalAccelerationValueID); + TRAJData.longitudinalAccelerationContentLength = htole16(TRAJData.longitudinalAccelerationContentLength); + TRAJData.longitudinalAcceleration = (int16_t) htole16(TRAJData.longitudinalAcceleration); + TRAJData.lateralAccelerationValueID = htole16(TRAJData.lateralAccelerationValueID); + TRAJData.lateralAccelerationContentLength = htole16(TRAJData.lateralAccelerationContentLength); + TRAJData.lateralAcceleration = (int16_t) htole16(TRAJData.lateralAcceleration); + TRAJData.curvatureValueID = htole16(TRAJData.curvatureValueID); + TRAJData.curvatureContentLength = htole16(TRAJData.curvatureContentLength); + TRAJData.curvature = htolef(TRAJData.curvature); memcpy(trajDataBufferPointer, &TRAJData, sizeof (TRAJData)); @@ -966,7 +971,7 @@ ssize_t encodeTRAJMessagePoint(const struct timeval * pointTimeFromStart, const * EINVAL if one of the input parameters are invalid * ENOBUFS if supplied buffer is too small to hold footer */ -ssize_t encodeTRAJMessageFooter(char * trajDataBuffer, const size_t remainingBufferLength, const char debug) { +ssize_t encodeTRAJMessageFooter(char *trajDataBuffer, const size_t remainingBufferLength, const char debug) { TRAJFooterType TRAJData; @@ -1003,10 +1008,10 @@ ssize_t encodeTRAJMessageFooter(char * trajDataBuffer, const size_t remainingBuf * \param debug Flag for enabling debugging * \return Number of bytes written to the buffer, or -1 in case of an error */ -ssize_t encodeOSEMMessage(const double_t *latitude_deg, const double_t *longitude_deg, const float *altitude_m, - const float *maxWayDeviation_m, const float *maxLateralDeviation_m, - const float *minimumPositioningAccuracy_m, char *osemDataBuffer, - const size_t bufferLength, const char debug) { +ssize_t encodeOSEMMessage(const double_t * latitude_deg, const double_t * longitude_deg, + const float *altitude_m, const float *maxWayDeviation_m, + const float *maxLateralDeviation_m, const float *minimumPositioningAccuracy_m, + char *osemDataBuffer, const size_t bufferLength, const char debug) { const char SizeDifference64bitTo48bit = 2; OSEMType OSEMData; diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 60009e2c3..ddc1a1a38 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -107,7 +107,7 @@ static size_t uiRecvMonitor(int *sockfd, char *buffer, size_t length); static int iGetObjectIndexFromObjectIP(in_addr_t ipAddr, in_addr_t objectIPs[], unsigned int numberOfObjects); static void signalHandler(int signo); -static ssize_t ObjectControlSendTRAJMessage(const char * Filename, int * Socket, const char debug); +static ssize_t ObjectControlSendTRAJMessage(const char *Filename, int *Socket, const char debug); static int iFindObjectsInfo(C8 object_traj_file[MAX_OBJECTS][MAX_FILE_PATH], C8 object_address_name[MAX_OBJECTS][MAX_FILE_PATH], @@ -264,11 +264,10 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { objectControlServerStatus = CONTROL_CENTER_STATUS_ABORT; MessageLength = encodeHEABMessage(objectControlServerStatus, MessageBuffer, sizeof (MessageBuffer), 0); - for(iIndex = 0; iIndex < nbr_objects; ++iIndex) - { - UtilSendUDPData("Object Control", &safety_socket_fd[iIndex], &safety_object_addr[iIndex], - MessageBuffer, MessageLength, 0); - } + for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { + UtilSendUDPData("Object Control", &safety_socket_fd[iIndex], &safety_object_addr[iIndex], + MessageBuffer, MessageLength, 0); + } } // Heartbeat @@ -788,8 +787,10 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { /* Here we send TRAJ, if the IP-address is not operating with a dynamic trajectory */ if (strstr(DTMReceivers, object_address_name[iIndex]) == NULL) { LogMessage(LOG_LEVEL_INFO, "Sending TRAJ to %s", object_address_name[iIndex]); - if (ObjectControlSendTRAJMessage(object_traj_file[iIndex], &socket_fds[iIndex], 0) == -1) { - LogMessage(LOG_LEVEL_ERROR, "Failed to send TRAJ to %s", object_address_name[iIndex]); + if (ObjectControlSendTRAJMessage(object_traj_file[iIndex], &socket_fds[iIndex], 0) + == -1) { + LogMessage(LOG_LEVEL_ERROR, "Failed to send TRAJ to %s", + object_address_name[iIndex]); continue; } } @@ -972,9 +973,9 @@ void signalHandler(int signo) { * \param debug Flag for enabling debugging * \return Number of bytes sent, or -1 in case of an error */ -ssize_t ObjectControlSendTRAJMessage(const char * Filename, int * Socket, const char debug) { +ssize_t ObjectControlSendTRAJMessage(const char *Filename, int *Socket, const char debug) { FILE *fd; - char* line; + char *line; size_t len = 0; ssize_t read; ssize_t printedBytes = 0, totalPrintedBytes = 0; @@ -983,7 +984,7 @@ ssize_t ObjectControlSendTRAJMessage(const char * Filename, int * Socket, const TrajectoryFileLine fileLine; char messageBuffer[TRAJECTORY_TX_BUFFER_SIZE]; size_t remainingBufferSpace = sizeof (messageBuffer); - char* messageBufferPosition = messageBuffer; + char *messageBufferPosition = messageBuffer; memset(&fileHeader, 0, sizeof (fileHeader)); memset(&fileLine, 0, sizeof (fileLine)); @@ -1025,9 +1026,9 @@ ssize_t ObjectControlSendTRAJMessage(const char * Filename, int * Socket, const // Generate ISO trajectory message header if ((printedBytes = encodeTRAJMessageHeader(fileHeader.ID > UINT16_MAX ? 0 : (uint16_t) fileHeader.ID, - fileHeader.majorVersion, fileHeader.name, strlen(fileHeader.name), - fileHeader.numberOfLines, messageBufferPosition, - remainingBufferSpace, debug)) == -1) { + fileHeader.majorVersion, fileHeader.name, + strlen(fileHeader.name), fileHeader.numberOfLines, + messageBufferPosition, remainingBufferSpace, debug)) == -1) { LogMessage(LOG_LEVEL_ERROR, "Unable to encode trajectory message"); fclose(fd); return -1; @@ -1038,8 +1039,7 @@ ssize_t ObjectControlSendTRAJMessage(const char * Filename, int * Socket, const remainingBufferSpace -= (size_t) printedBytes; read = getline(&line, &len, fd); - for (unsigned int i = 0; i < fileHeader.numberOfLines && read != -1; - ++i, read = getline(&line, &len, fd)) { + for (unsigned int i = 0; i < fileHeader.numberOfLines && read != -1; ++i, read = getline(&line, &len, fd)) { // Parse file line struct timeval relTime; @@ -1049,7 +1049,7 @@ ssize_t ObjectControlSendTRAJMessage(const char * Filename, int * Socket, const if (UtilParseTrajectoryFileLine(line, &fileLine) == -1) { // TODO: how to terminate an ISO message when an error has occurred? - LogMessage(LOG_LEVEL_ERROR, "Unable to parse line %u of trajectory file <%s>", i+1, Filename); + LogMessage(LOG_LEVEL_ERROR, "Unable to parse line %u of trajectory file <%s>", i + 1, Filename); fclose(fd); return -1; } @@ -1065,24 +1065,28 @@ ssize_t ObjectControlSendTRAJMessage(const char * Filename, int * Socket, const speed.isValid = fileLine.longitudinalVelocity != NULL && fileLine.lateralVelocity != NULL; speed.longitudinal_m_s = fileLine.longitudinalVelocity != NULL ? *fileLine.longitudinalVelocity : 0; speed.lateral_m_s = fileLine.lateralVelocity != NULL ? *fileLine.lateralVelocity : 0; - acceleration.isValid = fileLine.longitudinalAcceleration != NULL && fileLine.lateralAcceleration != NULL; - acceleration.longitudinal_m_s2 = fileLine.longitudinalAcceleration != NULL ? *fileLine.longitudinalAcceleration : 0; + acceleration.isValid = fileLine.longitudinalAcceleration != NULL + && fileLine.lateralAcceleration != NULL; + acceleration.longitudinal_m_s2 = + fileLine.longitudinalAcceleration != NULL ? *fileLine.longitudinalAcceleration : 0; acceleration.lateral_m_s2 = fileLine.lateralAcceleration != NULL ? *fileLine.lateralAcceleration : 0; // Print to buffer if ((printedBytes = encodeTRAJMessagePoint(&relTime, position, speed, acceleration, - (float) fileLine.curvature, messageBufferPosition, + (float)fileLine.curvature, messageBufferPosition, remainingBufferSpace, debug)) == -1) { if (errno == ENOBUFS) { // Reached the end of buffer, send buffered data and // try again - UtilSendTCPData(MODULE_NAME, messageBuffer, messageBufferPosition - messageBuffer, Socket, debug); + UtilSendTCPData(MODULE_NAME, messageBuffer, messageBufferPosition - messageBuffer, Socket, + debug); messageBufferPosition = messageBuffer; remainingBufferSpace = sizeof (messageBuffer); - if ((printedBytes = encodeTRAJMessagePoint(&relTime, position, speed, acceleration, fileLine.curvature, - messageBufferPosition, remainingBufferSpace, debug)) == -1) { + if ((printedBytes = + encodeTRAJMessagePoint(&relTime, position, speed, acceleration, fileLine.curvature, + messageBufferPosition, remainingBufferSpace, debug)) == -1) { // TODO how to terminate an ISO message when an error has occurred? LogMessage(LOG_LEVEL_ERROR, "Error encoding trajectory message point"); fclose(fd); @@ -1113,7 +1117,8 @@ ssize_t ObjectControlSendTRAJMessage(const char * Filename, int * Socket, const UtilSendTCPData(MODULE_NAME, messageBuffer, messageBufferPosition - messageBuffer, Socket, debug); messageBufferPosition = messageBuffer; remainingBufferSpace = sizeof (messageBuffer); - if ((printedBytes = encodeTRAJMessageFooter(messageBufferPosition, remainingBufferSpace, debug)) == -1) { + if ((printedBytes = + encodeTRAJMessageFooter(messageBufferPosition, remainingBufferSpace, debug)) == -1) { // TODO how to terminate an ISO message when an error has occurred? LogMessage(LOG_LEVEL_ERROR, "Error encoding trajectory message footer"); fclose(fd); From e1498193afd562f1cc3812e3904d68bef13b64a8 Mon Sep 17 00:00:00 2001 From: LukasWikander Date: Thu, 20 Feb 2020 09:42:29 +0100 Subject: [PATCH 284/523] Update MSCP.py --- server/integration-tests/tools/MSCP.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/server/integration-tests/tools/MSCP.py b/server/integration-tests/tools/MSCP.py index d9ede9ac2..05c3c369e 100644 --- a/server/integration-tests/tools/MSCP.py +++ b/server/integration-tests/tools/MSCP.py @@ -48,8 +48,8 @@ def listen(self): print("=== Starting listener on " + str(self.host) + ":" + str(self.port)) while not self.quit: try: - dat = self.socket.recv(2048) - data = bytearray(dat) + byteData = self.socket.recv(2048) + data = bytearray(byteData) except ConnectionResetError as e: if not self.quit: raise e From 7dcc00b91a5b4aab1ea587132616d1ceba830eb0 Mon Sep 17 00:00:00 2001 From: alfaro01 Date: Thu, 20 Feb 2020 12:38:50 +0100 Subject: [PATCH 285/523] Added a library for sending ISO messages. Only contains the ones in use right now so need expanding. --- server/integration-tests/tools/ISO.py | 100 ++++++++++++++++++++++++++ 1 file changed, 100 insertions(+) create mode 100644 server/integration-tests/tools/ISO.py diff --git a/server/integration-tests/tools/ISO.py b/server/integration-tests/tools/ISO.py new file mode 100644 index 000000000..e98a8819b --- /dev/null +++ b/server/integration-tests/tools/ISO.py @@ -0,0 +1,100 @@ +import socket +import sys, select +import threading +import re +import time +import encodings + +class ISO: + + udpPort = 57074 + tcpPort = 54242 + + def __init__(self,host="127.0.0.1",port=54241): + self.host = host + self.port = port + + try: + self.tcpSocket = socket.socket() + print("=== ISO connecting to " + str(self.host) + ":" + str(self.port)) + self.tcpSocket.connect((self.host,self.port)) + except: + print("TCP port: {} is already in use. Continuing...".format(self.tcpPort)) + + try: + print("=== Creating a UDP socket") + self.udpSocket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + self.udpSocket.bind((self.host, 53240)) + except: + print("UDP port: {} is already in use. Continuing...".format(self.udpPort)) + + + + + def StringTest(self): + message = bytearray.fromhex("48454c4c4f") + header = bytearray.fromhex("7e7e00000213002200000080001e00") + #messageLength = bytearray.fromhex(hex(len(message))) + #header.extend(messageLength) + header.extend(message) + print(header) + self.SendRawUDP(header) + print("=== TREO() sent") + + def MONR(self): + message = bytearray.fromhex("7e7e00460206002200000080001e008140384600710000f983000000000000f93e0000000000000000000301000000") + self.SendRawUDP(message) + print("=== MONR() sent") + + def HEAB(self): + message = bytearray.fromhex("7e7e0000020500090000009000050083c00d4a010000") + self.SendRawUDP(message) + print("=== HEAB() sent") + + def OSEM(self): + message = bytearray.fromhex("7e7e00000202004400000020000600d084fd85860021000600f82fc1c11d00220004006f4b0000040004001b3b3401030002002d0802000400629f0d4a70000200ffff72000200ffff7400020000000000") + self.SendRawUDP(message) + print("=== OSEM() sent") + + def OSTM(self): + message = bytearray.fromhex("7e7e00000203000500000064000100020000") + self.SendRawUDP(message) + print("=== OSTM() sent") + + def STRT(self): + message = bytearray.fromhex("7e7e00000204000e0000000200040038131e4a030002002d080000") + self.SendRawUDP(message) + print("=== STRT() sent") + + def TRAJ(self): + message = bytearray.fromhex("7e7e0000020100387f00000101020000000201400047617261676552656b74616e67656c496e726500000000000000000000000000000000000000000000000000000000000000000000000000000000000000000003010200000000a0040000000000") + self.SendRawUDP(message) + print("=== TRAJ() sent") + + def TRCM(self): + message = bytearray.fromhex("7e7e00000211002400000001000200000002000200e000110004002200000012000400ffffffff13000400ffffffff0000") + self.SendRawUDP(message) + print("=== TRCM() sent") + + def ACCM(self): + message = bytearray.fromhex("7e7e00000212002400000002000200000003000200e000a100040001000000a2000400ffffffffa3000400ffffffff0000") + self.SendRawUDP(message) + print("=== ACCM() sent") + + def TREO(self): + message = bytearray.fromhex("7e7e0000020100387f00000101020000000201400047617261676552656b74616e67656c496e726500000000000000000000000000000000000000000000000000000000000000000000000000000000000000000003010200000000a0040000000000") + self.SendRawUDP(message) + print("=== TREO() sent") + + def SendTCP(self,message): + self.tcpSocket.send(message.encode()) + + def SendUDP(self, message): + self.udpSocket.sendto(bytes(message, "utf-8"), (self.host, self.udpPort)) + + def SendRawUDP(self, message): + self.udpSocket.sendto(message, (self.host, self.udpPort)) + + def shutdown(self): + self.quit = True + self.tcpSocket.close() \ No newline at end of file From 8f26c62510eed0d148c63d3ec917c75106ff1f2f Mon Sep 17 00:00:00 2001 From: alfaro01 Date: Thu, 20 Feb 2020 12:40:49 +0100 Subject: [PATCH 286/523] Updated the tool for testing both MSCP and ISO. --- server/integration-tests/MSCPManualTest.py | 164 +++++++++++++++++++++ 1 file changed, 164 insertions(+) create mode 100644 server/integration-tests/MSCPManualTest.py diff --git a/server/integration-tests/MSCPManualTest.py b/server/integration-tests/MSCPManualTest.py new file mode 100644 index 000000000..f25aaa5f7 --- /dev/null +++ b/server/integration-tests/MSCPManualTest.py @@ -0,0 +1,164 @@ +from tools.MSCP import MSCP +from tools.ISO import ISO +from tools.Executable import Executable +import time +import subprocess +import sys + + +MSCPTests = ["INIT", + "CONNECTOBJECT", + "ARM", + "START", + "ABORT", + "GETSERVERSTATUS", + "CREATEOBJECTS", + "DISARM", + "GETSERVERTIME", + "UPLOADFILE", + "DOWNLOADFILE", + "DELETEFILE", + "CHECKFILEEXISTS", + "REMOTECONTROL", + "SETSERVERPARAMETER", + "GETSERVERPARAMETERS", + "GETDIRECTORYCONTENT", + "DISCONNECTOBJECT" ] + +ISOTests = ["TRAJ", + "OSEM", + "OSTM", + "STRT", + "HEAB", + "MONR", + "MONR2", + "SOWM", + "INFO", + "TRCM", + "ACCM", + "TREO", + "EXAC", + "CATA", + "SYPM", + "MTSP"] + + +if __name__ == "__main__": + + S = Executable("../build/TEServer",["-m","0"]) + time.sleep(0.05) + print("Choose what you would like to test: \n MSCP(1) \n ISO(2) \n:") + ans = input() + + if(ans == "1" or ans == "MSCP"): + + M = MSCP("127.0.0.1") + time.sleep(1) + + for t in MSCPTests: + print("\n Press enter to test {}".format(t)) + s = input() + + if(t == "INIT"): + M.Init() + + if(t == "CONNECTOBJECT"): + M.Connect() + + if(t == "ARM"): + M.Arm() + + if(t == "START"): + M.Start(0) + + if(t == "ABORT"): + M.Abort() + + if(t == "GETSERVERSTATUS"): + M.GetStatus() + + if(t == "CREATEOBJECTS"): + M.CreateObjects(1) + + if(t == "DISARM"): + M.Disarm() + + if(t == "GETSERVERTIME"): + M.GetServerTime() + + #if(t == "UPLOADFILE"): + # M.UploadFile("","") + + if(t == "DOWNLOADFILE"): + M.DownloadFile() + + if(t == "DELETEFILE"): + M.DeleteFile() + + if(t == "CHECKFILEEXISTS"): + M.CheckFileExists() + + if(t == "REMOTECONTROL"): + M.RemoteControl() + + if(t == "SETSERVERPARAMETER"): + M.SetServerParameter() + + if(t == "GETSERVERPARAMETERS"): + M.GetServerParameters() + + if(t == "GETDIRECTORYCONTENT"): + M.GetDirectoryContent() + + if(t == "DISCONNECTOBJECT"): + M.Disconnect() + + + print("Checking server...") + time.sleep(1) + if S.poll(): + S.stop() + M.shutdown() + sys.exit(1) + + M.shutdown() + + elif (ans == "2" or ans == "ISO"): + + I = ISO("127.0.0.1") + + for t in ISOTests: + + print("\n Press enter to test {}".format(t)) + s = input() + + if(t == "MONR"): + I.MONR() + if(t == "TREO"): + I.StringTest() + if(t == "OSEM"): + I.OSEM() + if(t == "OSTM"): + I.OSTM() + if(t == "STRT"): + I.STRT() + if(t == "HEAB"): + I.HEAB() + if(t == "TRCM"): + I.TRCM() + if(t == "ACCM"): + I.ACCM() + if(t == "TRAJ"): + I.TRAJ() + + + I.shutdown() + + + + S.stop() + sys.exit(1) + + + + From e0a458eebfdfbc99ccdba2cb3740c95ab65a85b8 Mon Sep 17 00:00:00 2001 From: alfaro01 Date: Thu, 20 Feb 2020 12:47:27 +0100 Subject: [PATCH 287/523] Changed name --- server/integration-tests/ManualMessageTest.py | 164 ++++++++++++++++++ 1 file changed, 164 insertions(+) create mode 100644 server/integration-tests/ManualMessageTest.py diff --git a/server/integration-tests/ManualMessageTest.py b/server/integration-tests/ManualMessageTest.py new file mode 100644 index 000000000..f25aaa5f7 --- /dev/null +++ b/server/integration-tests/ManualMessageTest.py @@ -0,0 +1,164 @@ +from tools.MSCP import MSCP +from tools.ISO import ISO +from tools.Executable import Executable +import time +import subprocess +import sys + + +MSCPTests = ["INIT", + "CONNECTOBJECT", + "ARM", + "START", + "ABORT", + "GETSERVERSTATUS", + "CREATEOBJECTS", + "DISARM", + "GETSERVERTIME", + "UPLOADFILE", + "DOWNLOADFILE", + "DELETEFILE", + "CHECKFILEEXISTS", + "REMOTECONTROL", + "SETSERVERPARAMETER", + "GETSERVERPARAMETERS", + "GETDIRECTORYCONTENT", + "DISCONNECTOBJECT" ] + +ISOTests = ["TRAJ", + "OSEM", + "OSTM", + "STRT", + "HEAB", + "MONR", + "MONR2", + "SOWM", + "INFO", + "TRCM", + "ACCM", + "TREO", + "EXAC", + "CATA", + "SYPM", + "MTSP"] + + +if __name__ == "__main__": + + S = Executable("../build/TEServer",["-m","0"]) + time.sleep(0.05) + print("Choose what you would like to test: \n MSCP(1) \n ISO(2) \n:") + ans = input() + + if(ans == "1" or ans == "MSCP"): + + M = MSCP("127.0.0.1") + time.sleep(1) + + for t in MSCPTests: + print("\n Press enter to test {}".format(t)) + s = input() + + if(t == "INIT"): + M.Init() + + if(t == "CONNECTOBJECT"): + M.Connect() + + if(t == "ARM"): + M.Arm() + + if(t == "START"): + M.Start(0) + + if(t == "ABORT"): + M.Abort() + + if(t == "GETSERVERSTATUS"): + M.GetStatus() + + if(t == "CREATEOBJECTS"): + M.CreateObjects(1) + + if(t == "DISARM"): + M.Disarm() + + if(t == "GETSERVERTIME"): + M.GetServerTime() + + #if(t == "UPLOADFILE"): + # M.UploadFile("","") + + if(t == "DOWNLOADFILE"): + M.DownloadFile() + + if(t == "DELETEFILE"): + M.DeleteFile() + + if(t == "CHECKFILEEXISTS"): + M.CheckFileExists() + + if(t == "REMOTECONTROL"): + M.RemoteControl() + + if(t == "SETSERVERPARAMETER"): + M.SetServerParameter() + + if(t == "GETSERVERPARAMETERS"): + M.GetServerParameters() + + if(t == "GETDIRECTORYCONTENT"): + M.GetDirectoryContent() + + if(t == "DISCONNECTOBJECT"): + M.Disconnect() + + + print("Checking server...") + time.sleep(1) + if S.poll(): + S.stop() + M.shutdown() + sys.exit(1) + + M.shutdown() + + elif (ans == "2" or ans == "ISO"): + + I = ISO("127.0.0.1") + + for t in ISOTests: + + print("\n Press enter to test {}".format(t)) + s = input() + + if(t == "MONR"): + I.MONR() + if(t == "TREO"): + I.StringTest() + if(t == "OSEM"): + I.OSEM() + if(t == "OSTM"): + I.OSTM() + if(t == "STRT"): + I.STRT() + if(t == "HEAB"): + I.HEAB() + if(t == "TRCM"): + I.TRCM() + if(t == "ACCM"): + I.ACCM() + if(t == "TRAJ"): + I.TRAJ() + + + I.shutdown() + + + + S.stop() + sys.exit(1) + + + + From e78326bcf6372bb6857c9259f6bc14533db96842 Mon Sep 17 00:00:00 2001 From: Viktor Johansson Date: Fri, 21 Feb 2020 15:06:34 +0100 Subject: [PATCH 288/523] Begun the reconstruction of build directory content. --- server/src/systemcontrol.c | 63 +++++++++++++++++++++++++++++++++----- 1 file changed, 56 insertions(+), 7 deletions(-) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index 149334a10..e93518ed0 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -14,6 +14,7 @@ #include #include #include +#include #include #include #include @@ -177,7 +178,7 @@ I32 SystemControlUploadFile(C8 * Path, C8 * FileSize, C8 * PacketSize, C8 * Retu I32 SystemControlReceiveRxData(I32 * sockfd, C8 * Path, C8 * FileSize, C8 * PacketSize, C8 * ReturnValue, U8 Debug); I32 SystemControlDeleteFileDirectory(C8 * Path, C8 * ReturnValue, U8 Debug); -I32 SystemControlBuildFileContentInfo(C8 * Path, C8 * ReturnValue, U8 Debug); +I32 SystemControlBuildFileContentInfo(C8 * Path, C8 ** bufferPointer, U8 Debug); I32 SystemControlSendFileContent(I32 * sockfd, C8 * Path, C8 * PacketSize, C8 * ReturnValue, U8 Remove, U8 Debug); I32 SystemControlCreateDirectory(C8 * Path, C8 * ReturnValue, U8 Debug); @@ -717,11 +718,22 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { if (ControlResponseBuffer[0] == FOLDER_EXIST) { UtilCreateDirContent(SystemControlArgument[0], "dir.info"); bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlBuildFileContentInfo("dir.info", ControlResponseBuffer, 0); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "GetDirectoryContent:", - ControlResponseBuffer, 4, &ClientSocket, 0); + + char * fileBufferPointer = NULL; + char * traversingPointer = NULL; + I32 file_len = SystemControlBuildFileContentInfo("dir.info",&fileBufferPointer,0); + traversingPointer = fileBufferPointer; + for (int i = 0; i < file_len; i++) { + printf("0x%X\n", *traversingPointer); + traversingPointer++; + } + /* + SystemControlBuildFileContentInfo("dir.info", ControlResponseBuffer, 0); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "SubGetDirectoryContent:", + ControlResponseBuffer, 4, &ClientSocket, 0); SystemControlSendFileContent(&ClientSocket, "dir.info", STR_SYSTEM_CONTROL_TX_PACKET_SIZE, ControlResponseBuffer, REMOVE_FILE, 0); + */ } } @@ -1294,7 +1306,7 @@ void SystemControlSendControlResponse(U16 ResponseStatus, C8 * ResponseString, C C8 Data[SYSTEM_CONTROL_SEND_BUFFER_SIZE]; bzero(Data, SYSTEM_CONTROL_SEND_BUFFER_SIZE); - n = 2 + strlen(ResponseString) + ResponseDataLength; + n = 2 + strlen(ResponseString) + ResponseDataLength; Length[0] = (C8) (n >> 24); Length[1] = (C8) (n >> 16); Length[2] = (C8) (n >> 8); @@ -1910,8 +1922,9 @@ I32 SystemControlReadServerParameterList(C8 * ParameterList, U8 Debug) { return strlen(ParameterList); } -I32 SystemControlBuildFileContentInfo(C8 * Path, C8 * ReturnValue, U8 Debug) { +I32 SystemControlBuildFileContentInfo(C8 * Path, C8 ** bufferPlace, U8 Debug) { + /* struct stat st; C8 CompletePath[MAX_FILE_PATH]; @@ -1929,7 +1942,43 @@ I32 SystemControlBuildFileContentInfo(C8 * Path, C8 * ReturnValue, U8 Debug) { if (Debug) LogMessage(LOG_LEVEL_DEBUG, "Filesize %d of %s", (I32) st.st_size, CompletePath); - return 0; + return st.st_size; + */ + struct stat st; + C8 CompletePath[MAX_FILE_PATH]; + C8 temporaryCompletePath[MAX_FILE_PATH]; + bzero(CompletePath, MAX_FILE_PATH); + UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); + strcat(CompletePath, Path); + stat(CompletePath, &st); + // Create new temporary file, containing the length of the current file in hex + the rest of the document + strcat(temporaryCompletePath,".temp"); + FILE * comp_fd = fopen(CompletePath,"r"); + FILE * temp_fd = fopen(temporaryCompletePath,"w"); + + fprintf(temp_fd,"%c%c%c%c", + (U8) (st.st_size >> 24), + (U8) (st.st_size >> 16), + (U8) (st.st_size >> 8), + (U8) (st.st_size) + ); + + while (!feof(comp_fd)) { + fputc(fgetc(comp_fd),temp_fd); + } + + fclose(comp_fd); + fclose(temp_fd); + + // Rename the temporary file to the name of the previous one + rename(temporaryCompletePath, CompletePath); + stat(CompletePath, &st); + // Create mmap of the file and return the length + int fd = open(CompletePath, O_RDWR); + char * buffer = mmap(NULL,st.st_size,PROT_READ | PROT_WRITE, MAP_PRIVATE, fd,0); + + *bufferPlace = buffer; + return st.st_size; } I32 SystemControlCheckFileDirectoryExist(C8 * ParameterName, C8 * ReturnValue, U8 Debug) { From c02e0bdf846986e08c60c24c1f3723499569ba37 Mon Sep 17 00:00:00 2001 From: Viktor Johansson Date: Sat, 22 Feb 2020 13:55:42 +0100 Subject: [PATCH 289/523] Begun integration of subdirectory fix --- server/src/systemcontrol.c | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index e93518ed0..3af248540 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -721,7 +721,8 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { char * fileBufferPointer = NULL; char * traversingPointer = NULL; - I32 file_len = SystemControlBuildFileContentInfo("dir.info",&fileBufferPointer,0); + int fd = 0; + I32 file_len = SystemControlBuildFileContentInfo("dir.info",&fileBufferPointer,&fd,0); traversingPointer = fileBufferPointer; for (int i = 0; i < file_len; i++) { printf("0x%X\n", *traversingPointer); @@ -1922,7 +1923,7 @@ I32 SystemControlReadServerParameterList(C8 * ParameterList, U8 Debug) { return strlen(ParameterList); } -I32 SystemControlBuildFileContentInfo(C8 * Path, C8 ** bufferPlace, U8 Debug) { +I32 SystemControlBuildFileContentInfo(C8 * Path, C8 ** bufferPlace, int *fd_pointer, U8 Debug) { /* struct stat st; @@ -1974,13 +1975,20 @@ I32 SystemControlBuildFileContentInfo(C8 * Path, C8 ** bufferPlace, U8 Debug) { rename(temporaryCompletePath, CompletePath); stat(CompletePath, &st); // Create mmap of the file and return the length - int fd = open(CompletePath, O_RDWR); - char * buffer = mmap(NULL,st.st_size,PROT_READ | PROT_WRITE, MAP_PRIVATE, fd,0); + *fd_pointer = open(CompletePath, O_RDWR); + char * buffer = mmap(NULL,st.st_size,PROT_READ | PROT_WRITE, MAP_PRIVATE, *fd_pointer,0); *bufferPlace = buffer; return st.st_size; } +I32 SystemControlDestroyFileContentInfo(C8 *path, C8 *bufferPointer, int *fd_pointer) +{ + char CompletePath[MAX_FILE_PATH]; + UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); + strcat(CompletePath, path); +} + I32 SystemControlCheckFileDirectoryExist(C8 * ParameterName, C8 * ReturnValue, U8 Debug) { DIR *pDir; From 333e54aa4e314006333183912fe266699ebf2fa5 Mon Sep 17 00:00:00 2001 From: Viktor Johansson Date: Sun, 23 Feb 2020 19:27:39 +0100 Subject: [PATCH 290/523] Made the building of files less general. --- server/src/systemcontrol.c | 46 +++++++++++++++++++++++++++----------- 1 file changed, 33 insertions(+), 13 deletions(-) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index 3af248540..edad6a865 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -52,6 +52,12 @@ typedef enum { SERVER_STATE_ERROR, } ServerState_t; +typedef struct { + int exist; + int fd; + char *info_buffer; + int size; +} content_dir_info; #define SYSTEM_CONTROL_SERVICE_POLL_TIME_MS 5000 #define SYSTEM_CONTROL_TASK_PERIOD_MS 1 @@ -149,6 +155,8 @@ char SystemControlArgument[SYSTEM_CONTROL_ARG_MAX_COUNT][SYSTEM_CONTROL_ARGUMENT C8 *STR_SYSTEM_CONTROL_RX_PACKET_SIZE = "1280"; C8 *STR_SYSTEM_CONTROL_TX_PACKET_SIZE = "1200"; +content_dir_info SystemControlDirectoryInfo = { 0, 0, NULL, 0}; + /*------------------------------------------------------------ -- Function declarations. ------------------------------------------------------------*/ @@ -178,7 +186,8 @@ I32 SystemControlUploadFile(C8 * Path, C8 * FileSize, C8 * PacketSize, C8 * Retu I32 SystemControlReceiveRxData(I32 * sockfd, C8 * Path, C8 * FileSize, C8 * PacketSize, C8 * ReturnValue, U8 Debug); I32 SystemControlDeleteFileDirectory(C8 * Path, C8 * ReturnValue, U8 Debug); -I32 SystemControlBuildFileContentInfo(C8 * Path, C8 ** bufferPointer, U8 Debug); +I32 SystemControlBuildFileContentInfo(C8 * Path, U8 Debug); +I32 SystemControlDestroyFileContentInfo(C8 *path); I32 SystemControlSendFileContent(I32 * sockfd, C8 * Path, C8 * PacketSize, C8 * ReturnValue, U8 Remove, U8 Debug); I32 SystemControlCreateDirectory(C8 * Path, C8 * ReturnValue, U8 Debug); @@ -719,15 +728,15 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { UtilCreateDirContent(SystemControlArgument[0], "dir.info"); bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - char * fileBufferPointer = NULL; - char * traversingPointer = NULL; - int fd = 0; - I32 file_len = SystemControlBuildFileContentInfo("dir.info",&fileBufferPointer,&fd,0); - traversingPointer = fileBufferPointer; + I32 file_len = SystemControlBuildFileContentInfo("dir.info",0); + if (file_len > 0) printf("file contred Created\n"); + char *traversingPointer = SystemControlDirectoryInfo.info_buffer; for (int i = 0; i < file_len; i++) { printf("0x%X\n", *traversingPointer); traversingPointer++; } + + if (!SystemControlDestroyFileContentInfo("dir.info")) printf("file content deleted \n"); /* SystemControlBuildFileContentInfo("dir.info", ControlResponseBuffer, 0); SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "SubGetDirectoryContent:", @@ -753,7 +762,7 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { if (ControlResponseBuffer[0] == FILE_EXIST) { UtilCreateDirContent(SystemControlArgument[0], SystemControlArgument[0]); bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlBuildFileContentInfo(SystemControlArgument[0], ControlResponseBuffer, 0); + SystemControlBuildFileContentInfo(SystemControlArgument[0], 0); SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "SubDownloadFile:", ControlResponseBuffer, 4, &ClientSocket, 0); SystemControlSendFileContent(&ClientSocket, SystemControlArgument[0], @@ -1923,7 +1932,7 @@ I32 SystemControlReadServerParameterList(C8 * ParameterList, U8 Debug) { return strlen(ParameterList); } -I32 SystemControlBuildFileContentInfo(C8 * Path, C8 ** bufferPlace, int *fd_pointer, U8 Debug) { +I32 SystemControlBuildFileContentInfo(C8 * Path, U8 Debug) { /* struct stat st; @@ -1949,6 +1958,9 @@ I32 SystemControlBuildFileContentInfo(C8 * Path, C8 ** bufferPlace, int *fd_poin C8 CompletePath[MAX_FILE_PATH]; C8 temporaryCompletePath[MAX_FILE_PATH]; bzero(CompletePath, MAX_FILE_PATH); + + if (SystemControlDirectoryInfo.exist) return -1; + UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); strcat(CompletePath, Path); stat(CompletePath, &st); @@ -1975,18 +1987,26 @@ I32 SystemControlBuildFileContentInfo(C8 * Path, C8 ** bufferPlace, int *fd_poin rename(temporaryCompletePath, CompletePath); stat(CompletePath, &st); // Create mmap of the file and return the length - *fd_pointer = open(CompletePath, O_RDWR); - char * buffer = mmap(NULL,st.st_size,PROT_READ | PROT_WRITE, MAP_PRIVATE, *fd_pointer,0); - - *bufferPlace = buffer; + SystemControlDirectoryInfo.fd = open(CompletePath, O_RDWR); + SystemControlDirectoryInfo.info_buffer = mmap(NULL,st.st_size,PROT_READ | PROT_WRITE, MAP_PRIVATE, SystemControlDirectoryInfo.fd,0); + SystemControlDirectoryInfo.size = st.st_size; + SystemControlDirectoryInfo.exist = 1; return st.st_size; } -I32 SystemControlDestroyFileContentInfo(C8 *path, C8 *bufferPointer, int *fd_pointer) +I32 SystemControlDestroyFileContentInfo(C8 *path) { char CompletePath[MAX_FILE_PATH]; + struct stat st; + if (SystemControlDirectoryInfo.exist != 0) return -1; UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); strcat(CompletePath, path); + + munmap(SystemControlDirectoryInfo.info_buffer, SystemControlDirectoryInfo.size); + close(SystemControlDirectoryInfo.fd); + SystemControlDirectoryInfo.exist = 0; + remove(CompletePath); + return 0; } I32 SystemControlCheckFileDirectoryExist(C8 * ParameterName, C8 * ReturnValue, U8 Debug) { From 145bf2f63ef5e75f808a06b7ddbd079bbce57a54 Mon Sep 17 00:00:00 2001 From: Viktor Johansson Date: Sun, 23 Feb 2020 22:12:07 +0100 Subject: [PATCH 291/523] Tested the getdirectory and it now works. --- server/src/systemcontrol.c | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index edad6a865..8a27295ab 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -729,18 +729,22 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); I32 file_len = SystemControlBuildFileContentInfo("dir.info",0); + + /* if (file_len > 0) printf("file contred Created\n"); char *traversingPointer = SystemControlDirectoryInfo.info_buffer; for (int i = 0; i < file_len; i++) { printf("0x%X\n", *traversingPointer); traversingPointer++; } + */ + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "SubGetDirectoryContent:", + SystemControlDirectoryInfo.info_buffer, file_len, &ClientSocket, 0); - if (!SystemControlDestroyFileContentInfo("dir.info")) printf("file content deleted \n"); + SystemControlDestroyFileContentInfo("dir.info"); /* SystemControlBuildFileContentInfo("dir.info", ControlResponseBuffer, 0); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "SubGetDirectoryContent:", - ControlResponseBuffer, 4, &ClientSocket, 0); + SystemControlSendFileContent(&ClientSocket, "dir.info", STR_SYSTEM_CONTROL_TX_PACKET_SIZE, ControlResponseBuffer, REMOVE_FILE, 0); */ @@ -1998,14 +2002,14 @@ I32 SystemControlDestroyFileContentInfo(C8 *path) { char CompletePath[MAX_FILE_PATH]; struct stat st; - if (SystemControlDirectoryInfo.exist != 0) return -1; + if (!SystemControlDirectoryInfo.exist) return -1; UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); strcat(CompletePath, path); munmap(SystemControlDirectoryInfo.info_buffer, SystemControlDirectoryInfo.size); close(SystemControlDirectoryInfo.fd); SystemControlDirectoryInfo.exist = 0; - remove(CompletePath); + //remove(CompletePath); return 0; } From 0a19365387e91c37d88b5aedf42d134067a5eb82 Mon Sep 17 00:00:00 2001 From: Viktor Johansson Date: Tue, 25 Feb 2020 17:05:53 +0100 Subject: [PATCH 292/523] Added GetRootDirectoryContent command to the server. --- server/src/systemcontrol.c | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index 8a27295ab..a5a4bbac4 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -127,7 +127,7 @@ typedef enum { Idle_0, GetServerStatus_0, ArmScenario_0, DisarmScenario_0, StartScenario_1, stop_0, AbortScenario_0, InitializeScenario_0, ConnectObject_0, DisconnectObject_0, GetServerParameterList_0, SetServerParameter_2, GetServerParameter_1, - DownloadFile_1, UploadFile_3, CheckFileDirectoryExist_1, GetDirectoryContent_0, GetDirectoryContent_1, + DownloadFile_1, UploadFile_3, CheckFileDirectoryExist_1, GetRootDirectoryContent_0, GetDirectoryContent_1, DeleteFileDirectory_1, CreateDirectory_1, GetTestOrigin_0, replay_1, control_0, Exit_0, start_ext_trigg_1, nocommand } SystemControlCommand_t; @@ -137,7 +137,7 @@ const char *SystemControlCommandsArr[] = { "AbortScenario_0", "InitializeScenario_0", "ConnectObject_0", "DisconnectObject_0", "GetServerParameterList_0", "SetServerParameter_2", "GetServerParameter_1", "DownloadFile_1", "UploadFile_3", "CheckFileDirectoryExist_1", - "GetDirectoryContent_0", "GetDirectoryContent_1", + "GetRootDirectoryContent_0", "GetDirectoryContent_1", "DeleteFileDirectory_1", "CreateDirectory_1", "GetTestOrigin_0", "replay_1", "control_0", "Exit_0", "start_ext_trigg_1" }; @@ -716,7 +716,8 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { SystemControlCommand = Idle_0; } break; - case GetDirectoryContent_0: + case GetRootDirectoryContent_0: + LogMessage(LOG_LEVEL_ERROR, "GetRootDirectory called"); case GetDirectoryContent_1: if (CurrentInputArgCount == CommandArgCount) { SystemControlCommand = Idle_0; @@ -752,7 +753,7 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { } else { - LogMessage(LOG_LEVEL_ERROR, "Wrong parameter count in GetDirectoryContent(path)!"); + LogMessage(LOG_LEVEL_ERROR, "Wrong parameter count in GetDirectoryContent(path)! got:%d, expected:%d",CurrentInputArgCount, CommandArgCount); SystemControlCommand = Idle_0; } break; From 4e30ac4f0edff07ef2e53a17feee698b0c21c0f5 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 27 Feb 2020 09:03:39 +0100 Subject: [PATCH 293/523] Moved configurations to top level dir --- {core/conf => conf}/adaptivesync.conf | 0 {core/conf => conf}/test.conf | 0 {core/conf => conf}/triggeraction.conf | 0 3 files changed, 0 insertions(+), 0 deletions(-) rename {core/conf => conf}/adaptivesync.conf (100%) rename {core/conf => conf}/test.conf (100%) rename {core/conf => conf}/triggeraction.conf (100%) diff --git a/core/conf/adaptivesync.conf b/conf/adaptivesync.conf similarity index 100% rename from core/conf/adaptivesync.conf rename to conf/adaptivesync.conf diff --git a/core/conf/test.conf b/conf/test.conf similarity index 100% rename from core/conf/test.conf rename to conf/test.conf diff --git a/core/conf/triggeraction.conf b/conf/triggeraction.conf similarity index 100% rename from core/conf/triggeraction.conf rename to conf/triggeraction.conf From 012c43ac015746dedc9a1878996e4caf79112de1 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 27 Feb 2020 09:04:35 +0100 Subject: [PATCH 294/523] Made top level CMakeLists create the home .maestro directory upon installation --- CMakeLists.txt | 19 +++++++++++++++++-- 1 file changed, 17 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1074a9483..3d2acec7c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,6 +2,10 @@ cmake_minimum_required(VERSION 3.10) project(Maestro) +set(CMAKE_VERBOSE_MAKEFILE ON) +set(CMAKE_COLOR_MAKEFILE ON) + +# Configure structure of output set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib) set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib) set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin) @@ -9,5 +13,16 @@ set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin) add_subdirectory(util/C) add_subdirectory(core) add_subdirectory(modules/ScenarioControl) -add_subdirectory(modules/Supervision) -add_subdirectory(modules/Visualization) +#add_subdirectory(modules/Supervision) +#add_subdirectory(modules/Visualization) + +# Create directory for test data in user home directory +install(DIRECTORY DESTINATION "$ENV{HOME}/.maestro" USE_SOURCE_PERMISSIONS) +install(DIRECTORY DESTINATION "$ENV{HOME}/.maestro/journal" USE_SOURCE_PERMISSIONS) +install(DIRECTORY DESTINATION "$ENV{HOME}/.maestro/traj" USE_SOURCE_PERMISSIONS) +install(DIRECTORY DESTINATION "$ENV{HOME}/.maestro/geofence" USE_SOURCE_PERMISSIONS) +install(DIRECTORY ${CMAKE_SOURCE_DIR}/conf + DESTINATION "$ENV{HOME}/.maestro" USE_SOURCE_PERMISSIONS) + +# Ensure libraries are reloaded after install +install(CODE "execute_process(COMMAND ldconfig)") From fa0126dbf7a3717e0971603c58f61deb05df73c8 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 27 Feb 2020 09:05:51 +0100 Subject: [PATCH 295/523] Added installation procedure to core CMakeLists --- core/CMakeLists.txt | 115 +++++++++++++++++++++++++++++++------------- 1 file changed, 81 insertions(+), 34 deletions(-) diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index e2d350143..cf81d2715 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -1,12 +1,24 @@ cmake_minimum_required(VERSION 3.1) -project(Core) +project(Core LANGUAGES C) + +# Specify locations for logs and shared memory +# WARNING: These are used for installation and could hose +# your system if e.g. specified to "/" +set(SYSTEM_SHARED_MEMORY_PATH "/dev/shm/maestro") set(CORE_TARGET ${CMAKE_PROJECT_NAME}) -set(UTIL_TARGET MaestroUtil) -set(TIME_TARGET MaestroTime) -set(LOGGING_TARGET MaestroLogging) -set(MESSAGE_BUS_TARGET MaestroMQ) +set(COREUTILS_TARGET MaestroCoreUtil) +set(ISO_22133_TARGET MaestroISO22133) +set(POSITIONING_TARGET MaestroPositioning) +set(DATA_DICTIONARY_TARGET MaestroDataDictionary) + +set(TIME_LIBRARY MaestroTime) +set(LOGGING_LIBRARY MaestroLogging) +set(MESSAGE_BUS_LIBRARY MaestroMQ) +set(MATH_LIBRARY m) + +include(GNUInstallDirs) add_executable(${CORE_TARGET} ${CMAKE_CURRENT_SOURCE_DIR}/src/objectcontrol.c @@ -16,44 +28,79 @@ add_executable(${CORE_TARGET} ${CMAKE_CURRENT_SOURCE_DIR}/src/timecontrol.c ) -add_library(${UTIL_TARGET} +add_library(${COREUTILS_TARGET} SHARED ${CMAKE_CURRENT_SOURCE_DIR}/src/util.c - ${CMAKE_CURRENT_SOURCE_DIR}/src/iso22133.c - ${CMAKE_CURRENT_SOURCE_DIR}/src/positioning.c - ${CMAKE_CURRENT_SOURCE_DIR}/src/datadictionary.c ) +set_target_properties(${COREUTILS_TARGET} PROPERTIES PUBLIC_HEADER + ${CMAKE_CURRENT_SOURCE_DIR}/inc/util.h +) +set_target_properties(${COREUTILS_TARGET} PROPERTIES POSITION_INDEPENDENT_CODE ON) -if(NOT TARGET ${TIME_TARGET}) - add_library(${TIME_TARGET} STATIC - ${CMAKE_CURRENT_SOURCE_DIR}/../util/C/time/maestroTime.c - ) -endif() +add_library(${ISO_22133_TARGET} SHARED + ${CMAKE_CURRENT_SOURCE_DIR}/src/iso22133.c +) +set_target_properties(${ISO_22133_TARGET} PROPERTIES PUBLIC_HEADER + ${CMAKE_CURRENT_SOURCE_DIR}/inc/iso22133.h +) +set_target_properties(${ISO_22133_TARGET} PROPERTIES POSITION_INDEPENDENT_CODE ON) -if(NOT TARGET ${LOGGING_TARGET}) - add_library(${LOGGING_TARGET} STATIC - ${CMAKE_CURRENT_SOURCE_DIR}/../util/C/logging/logging.c - ) -endif() +add_library(${POSITIONING_TARGET} SHARED + ${CMAKE_CURRENT_SOURCE_DIR}/src/positioning.c +) +set_target_properties(${POSITIONING_TARGET} PROPERTIES PUBLIC_HEADER + ${CMAKE_CURRENT_SOURCE_DIR}/inc/positioning.h +) +set_target_properties(${POSITIONING_TARGET} PROPERTIES POSITION_INDEPENDENT_CODE ON) -if(NOT TARGET ${MESSAGE_BUS_TARGET}) - add_library(${MESSAGE_BUS_TARGET} STATIC - ${CMAKE_CURRENT_SOURCE_DIR}/../util/C/mqbus/mqbus.c - ) - target_link_libraries(${MESSAGE_BUS_TARGET} rt) -endif() +add_library(${DATA_DICTIONARY_TARGET} SHARED + ${CMAKE_CURRENT_SOURCE_DIR}/src/datadictionary.c +) +set_target_properties(${DATA_DICTIONARY_TARGET} PROPERTIES PUBLIC_HEADER + ${CMAKE_CURRENT_SOURCE_DIR}/inc/datadictionary.h +) +set_target_properties(${DATA_DICTIONARY_TARGET} PROPERTIES POSITION_INDEPENDENT_CODE ON) -target_link_libraries(${CORE_TARGET} LINK_PUBLIC ${TIME_TARGET} ${LOGGING_TARGET} ${UTIL_TARGET}) +# Linking +target_link_libraries(${CORE_TARGET} LINK_PUBLIC + ${TIME_LIBRARY} + ${LOGGING_LIBRARY} + ${COREUTILS_TARGET} + ${DATA_DICTIONARY_TARGET} + ${ISO_22133_TARGET} + ${POSITIONING_TARGET} +) target_include_directories(${CORE_TARGET} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/inc - ${CMAKE_CURRENT_SOURCE_DIR}/../util/C/time - ${CMAKE_CURRENT_SOURCE_DIR}/../util/C/logging +) +target_include_directories(${COREUTILS_TARGET} PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/inc +) +target_include_directories(${DATA_DICTIONARY_TARGET} PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/inc +) +target_include_directories(${ISO_22133_TARGET} PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/inc +) +target_include_directories(${POSITIONING_TARGET} PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/inc ) -target_link_libraries(${UTIL_TARGET} LINK_PUBLIC ${TIME_TARGET} ${LOGGING_TARGET} ${MESSAGE_BUS_TARGET}) -target_link_libraries(${UTIL_TARGET} LINK_PUBLIC m) -target_include_directories(${UTIL_TARGET} PUBLIC +target_link_libraries(${COREUTILS_TARGET} LINK_PUBLIC ${TIME_LIBRARY} ${LOGGING_LIBRARY} ${MESSAGE_BUS_LIBRARY} ${MATH_LIBRARY}) + +target_include_directories(${COREUTILS_TARGET} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/inc - ${CMAKE_CURRENT_SOURCE_DIR}/../util/C/time - ${CMAKE_CURRENT_SOURCE_DIR}/../util/C/logging - ${CMAKE_CURRENT_SOURCE_DIR}/../util/C/mqbus ) + +install(TARGETS ${CORE_TARGET} ${COREUTILS_TARGET} + ${ISO_22133_TARGET} ${DATA_DICTIONARY_TARGET} + ${POSITIONING_TARGET} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} +) + +# Create shared memory directory and give user access +install(DIRECTORY DESTINATION ${SYSTEM_SHARED_MEMORY_PATH}) +install(CODE "execute_process(COMMAND chown $ENV{USER} ${SYSTEM_SHARED_MEMORY_PATH})") + From 60d754eb269798f3b621c03c576cad0e2e415785 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 27 Feb 2020 09:06:32 +0100 Subject: [PATCH 296/523] Added installation procedure to ScC CMakeLists --- modules/ScenarioControl/CMakeLists.txt | 77 ++++++++------------------ 1 file changed, 24 insertions(+), 53 deletions(-) diff --git a/modules/ScenarioControl/CMakeLists.txt b/modules/ScenarioControl/CMakeLists.txt index ec0a08b32..f3ec99a42 100644 --- a/modules/ScenarioControl/CMakeLists.txt +++ b/modules/ScenarioControl/CMakeLists.txt @@ -4,23 +4,19 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_C_STANDARD 11) set(CMAKE_C_STANDARD_REQUIRED ON) -project(ScenarioControl) +project(ScenarioControl LANGUAGES C CXX) # Define target names set(SCENARIO_CONTROL_TARGET ${PROJECT_NAME}) -set(UTIL_TARGET MaestroUtil) -set(TIME_TARGET MaestroTime) -set(LOGGING_TARGET MaestroLogging) -set(MESSAGE_BUS_TARGET MaestroMQ) -# Define paths to related files -set(CORE_ROOT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/../../core) -set(CORE_INCLUDE_PATH ${CORE_ROOT_PATH}/inc) -set(CORE_SOURCE_PATH ${CORE_ROOT_PATH}/src) -set(UTIL_ROOT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C) -set(TIME_PATH ${UTIL_ROOT_PATH}/time) -set(LOGGING_PATH ${UTIL_ROOT_PATH}/logging) -set(MESSAGE_BUS_PATH ${UTIL_ROOT_PATH}/mqbus) +set(COREUTILS_LIBRARY MaestroCoreUtil) +set(ISO_22133_LIBRARY MaestroISO22133) +set(POSITIONING_LIBRARY MaestroPositioning) +set(TIME_LIBRARY MaestroTime) +set(LOGGING_LIBRARY MaestroLogging) +set(MESSAGE_BUS_LIBRARY MaestroMQ) + +include(GNUInstallDirs) # Create project main executable target add_executable(${SCENARIO_CONTROL_TARGET} @@ -35,47 +31,22 @@ add_executable(${SCENARIO_CONTROL_TARGET} ${CMAKE_CURRENT_SOURCE_DIR}/src/trigger.cpp ) -# If building locally, it is necessary to add relevant util targets -if(NOT TARGET ${TIME_TARGET}) - add_library(${TIME_TARGET} STATIC - ${TIME_PATH}/maestroTime.c - ) -endif() - -if(NOT TARGET ${LOGGING_TARGET}) - add_library(${LOGGING_TARGET} STATIC - ${LOGGING_PATH}/logging.c - ) -endif() - -if(NOT TARGET ${MESSAGE_BUS_TARGET}) - add_library(${MESSAGE_BUS_TARGET} STATIC - ${MESSAGE_BUS_PATH}/mqbus.c - ) - target_link_libraries(${MESSAGE_BUS_TARGET} rt) -endif() - -if(NOT TARGET ${UTIL_TARGET}) - add_library(${UTIL_TARGET} STATIC - ${CORE_SOURCE_PATH}/util.c - ${CORE_SOURCE_PATH}/iso22133.c - ${CORE_SOURCE_PATH}/positioning.c - ${CORE_SOURCE_PATH}/datadictionary.c - ) - target_link_libraries(${UTIL_TARGET} ${LOGGING_TARGET} ${TIME_TARGET} ${MESSAGE_BUS_TARGET} m) - target_include_directories(${UTIL_TARGET} PUBLIC - ${CORE_INCLUDE_PATH} - ${MESSAGE_BUS_PATH} - ${LOGGING_PATH} - ${TIME_PATH} - ) -endif() - # Link project executable to util libraries -target_link_libraries(${SCENARIO_CONTROL_TARGET} LINK_PUBLIC ${TIME_TARGET} ${UTIL_TARGET} ${LOGGING_TARGET}) +target_link_libraries(${SCENARIO_CONTROL_TARGET} LINK_PUBLIC + ${TIME_LIBRARY} + ${COREUTILS_LIBRARY} + ${LOGGING_LIBRARY} + ${MESSAGE_BUS_LIBRARY} + ${POSITIONING_LIBRARY} + ${ISO_22133_LIBRARY} +) target_include_directories(${SCENARIO_CONTROL_TARGET} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/inc - ${LOGGING_PATH} - ${TIME_PATH} - ${CORE_INCLUDE_PATH} +) + +install(TARGETS ${SCENARIO_CONTROL_TARGET} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} ) From 581a8a222a597efa2e86b3df0ac1d425b52e51cf Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 27 Feb 2020 09:12:14 +0100 Subject: [PATCH 297/523] Updated util reference --- util | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/util b/util index 99ccf4b7e..21967d535 160000 --- a/util +++ b/util @@ -1 +1 @@ -Subproject commit 99ccf4b7edbfeb4f675847b379c316eb42d365d6 +Subproject commit 21967d53556bbffeb309492bfee1036a3b750b9d From cd206431958b78f632eedc185554777ae8ff50f2 Mon Sep 17 00:00:00 2001 From: Viktor Johansson Date: Mon, 2 Mar 2020 10:52:38 +0100 Subject: [PATCH 298/523] Format to corrent indent. --- server/src/systemcontrol.c | 195 +++++++++++++++++++------------------ 1 file changed, 100 insertions(+), 95 deletions(-) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index a5a4bbac4..6ed970823 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -53,10 +53,10 @@ typedef enum { } ServerState_t; typedef struct { - int exist; - int fd; - char *info_buffer; - int size; + int exist; + int fd; + char *info_buffer; + int size; } content_dir_info; #define SYSTEM_CONTROL_SERVICE_POLL_TIME_MS 5000 @@ -127,7 +127,7 @@ typedef enum { Idle_0, GetServerStatus_0, ArmScenario_0, DisarmScenario_0, StartScenario_1, stop_0, AbortScenario_0, InitializeScenario_0, ConnectObject_0, DisconnectObject_0, GetServerParameterList_0, SetServerParameter_2, GetServerParameter_1, - DownloadFile_1, UploadFile_3, CheckFileDirectoryExist_1, GetRootDirectoryContent_0, GetDirectoryContent_1, + DownloadFile_1, UploadFile_3, CheckFileDirectoryExist_1, GetRootDirectoryContent_0, GetDirectoryContent_1, DeleteFileDirectory_1, CreateDirectory_1, GetTestOrigin_0, replay_1, control_0, Exit_0, start_ext_trigg_1, nocommand } SystemControlCommand_t; @@ -137,7 +137,7 @@ const char *SystemControlCommandsArr[] = { "AbortScenario_0", "InitializeScenario_0", "ConnectObject_0", "DisconnectObject_0", "GetServerParameterList_0", "SetServerParameter_2", "GetServerParameter_1", "DownloadFile_1", "UploadFile_3", "CheckFileDirectoryExist_1", - "GetRootDirectoryContent_0", "GetDirectoryContent_1", + "GetRootDirectoryContent_0", "GetDirectoryContent_1", "DeleteFileDirectory_1", "CreateDirectory_1", "GetTestOrigin_0", "replay_1", "control_0", "Exit_0", "start_ext_trigg_1" }; @@ -155,7 +155,7 @@ char SystemControlArgument[SYSTEM_CONTROL_ARG_MAX_COUNT][SYSTEM_CONTROL_ARGUMENT C8 *STR_SYSTEM_CONTROL_RX_PACKET_SIZE = "1280"; C8 *STR_SYSTEM_CONTROL_TX_PACKET_SIZE = "1200"; -content_dir_info SystemControlDirectoryInfo = { 0, 0, NULL, 0}; +content_dir_info SystemControlDirectoryInfo = { 0, 0, NULL, 0 }; /*------------------------------------------------------------ -- Function declarations. @@ -187,7 +187,7 @@ I32 SystemControlReceiveRxData(I32 * sockfd, C8 * Path, C8 * FileSize, C8 * Pack U8 Debug); I32 SystemControlDeleteFileDirectory(C8 * Path, C8 * ReturnValue, U8 Debug); I32 SystemControlBuildFileContentInfo(C8 * Path, U8 Debug); -I32 SystemControlDestroyFileContentInfo(C8 *path); +I32 SystemControlDestroyFileContentInfo(C8 * path); I32 SystemControlSendFileContent(I32 * sockfd, C8 * Path, C8 * PacketSize, C8 * ReturnValue, U8 Remove, U8 Debug); I32 SystemControlCreateDirectory(C8 * Path, C8 * ReturnValue, U8 Debug); @@ -716,8 +716,8 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { SystemControlCommand = Idle_0; } break; - case GetRootDirectoryContent_0: - LogMessage(LOG_LEVEL_ERROR, "GetRootDirectory called"); + case GetRootDirectoryContent_0: + LogMessage(LOG_LEVEL_ERROR, "GetRootDirectory called"); case GetDirectoryContent_1: if (CurrentInputArgCount == CommandArgCount) { SystemControlCommand = Idle_0; @@ -729,31 +729,35 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { UtilCreateDirContent(SystemControlArgument[0], "dir.info"); bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - I32 file_len = SystemControlBuildFileContentInfo("dir.info",0); - - /* - if (file_len > 0) printf("file contred Created\n"); - char *traversingPointer = SystemControlDirectoryInfo.info_buffer; - for (int i = 0; i < file_len; i++) { - printf("0x%X\n", *traversingPointer); - traversingPointer++; - } - */ - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "SubGetDirectoryContent:", - SystemControlDirectoryInfo.info_buffer, file_len, &ClientSocket, 0); - - SystemControlDestroyFileContentInfo("dir.info"); - /* - SystemControlBuildFileContentInfo("dir.info", ControlResponseBuffer, 0); - - SystemControlSendFileContent(&ClientSocket, "dir.info", STR_SYSTEM_CONTROL_TX_PACKET_SIZE, - ControlResponseBuffer, REMOVE_FILE, 0); - */ + I32 file_len = SystemControlBuildFileContentInfo("dir.info", 0); + + /* + if (file_len > 0) printf("file contred Created\n"); + char *traversingPointer = SystemControlDirectoryInfo.info_buffer; + for (int i = 0; i < file_len; i++) { + printf("0x%X\n", *traversingPointer); + traversingPointer++; + } + */ + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, + "SubGetDirectoryContent:", + SystemControlDirectoryInfo.info_buffer, file_len, + &ClientSocket, 0); + + SystemControlDestroyFileContentInfo("dir.info"); + /* + SystemControlBuildFileContentInfo("dir.info", ControlResponseBuffer, 0); + + SystemControlSendFileContent(&ClientSocket, "dir.info", STR_SYSTEM_CONTROL_TX_PACKET_SIZE, + ControlResponseBuffer, REMOVE_FILE, 0); + */ } } else { - LogMessage(LOG_LEVEL_ERROR, "Wrong parameter count in GetDirectoryContent(path)! got:%d, expected:%d",CurrentInputArgCount, CommandArgCount); + LogMessage(LOG_LEVEL_ERROR, + "Wrong parameter count in GetDirectoryContent(path)! got:%d, expected:%d", + CurrentInputArgCount, CommandArgCount); SystemControlCommand = Idle_0; } break; @@ -767,7 +771,7 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { if (ControlResponseBuffer[0] == FILE_EXIST) { UtilCreateDirContent(SystemControlArgument[0], SystemControlArgument[0]); bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlBuildFileContentInfo(SystemControlArgument[0], 0); + SystemControlBuildFileContentInfo(SystemControlArgument[0], 0); SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "SubDownloadFile:", ControlResponseBuffer, 4, &ClientSocket, 0); SystemControlSendFileContent(&ClientSocket, SystemControlArgument[0], @@ -811,7 +815,7 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { ControlResponseBuffer, 0); } - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "SubUploadFile:", + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "SubUploadFile:", ControlResponseBuffer, 1, &ClientSocket, 0); } @@ -1321,7 +1325,7 @@ void SystemControlSendControlResponse(U16 ResponseStatus, C8 * ResponseString, C C8 Data[SYSTEM_CONTROL_SEND_BUFFER_SIZE]; bzero(Data, SYSTEM_CONTROL_SEND_BUFFER_SIZE); - n = 2 + strlen(ResponseString) + ResponseDataLength; + n = 2 + strlen(ResponseString) + ResponseDataLength; Length[0] = (C8) (n >> 24); Length[1] = (C8) (n >> 16); Length[2] = (C8) (n >> 8); @@ -1939,79 +1943,80 @@ I32 SystemControlReadServerParameterList(C8 * ParameterList, U8 Debug) { I32 SystemControlBuildFileContentInfo(C8 * Path, U8 Debug) { - /* + /* + struct stat st; + C8 CompletePath[MAX_FILE_PATH]; + + bzero(CompletePath, MAX_FILE_PATH); + UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); + strcat(CompletePath, Path); + + stat(CompletePath, &st); + *(ReturnValue + 0) = (U8) (st.st_size >> 24); + *(ReturnValue + 1) = (U8) (st.st_size >> 16); + *(ReturnValue + 2) = (U8) (st.st_size >> 8); + *(ReturnValue + 3) = (U8) st.st_size; + + + if (Debug) + LogMessage(LOG_LEVEL_DEBUG, "Filesize %d of %s", (I32) st.st_size, CompletePath); + + return st.st_size; + */ struct stat st; C8 CompletePath[MAX_FILE_PATH]; + C8 temporaryCompletePath[MAX_FILE_PATH]; bzero(CompletePath, MAX_FILE_PATH); + + if (SystemControlDirectoryInfo.exist) + return -1; + UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); strcat(CompletePath, Path); - stat(CompletePath, &st); - *(ReturnValue + 0) = (U8) (st.st_size >> 24); - *(ReturnValue + 1) = (U8) (st.st_size >> 16); - *(ReturnValue + 2) = (U8) (st.st_size >> 8); - *(ReturnValue + 3) = (U8) st.st_size; + // Create new temporary file, containing the length of the current file in hex + the rest of the document + strcat(temporaryCompletePath, ".temp"); + FILE *comp_fd = fopen(CompletePath, "r"); + FILE *temp_fd = fopen(temporaryCompletePath, "w"); + fprintf(temp_fd, "%c%c%c%c", + (U8) (st.st_size >> 24), (U8) (st.st_size >> 16), (U8) (st.st_size >> 8), (U8) (st.st_size) + ); - if (Debug) - LogMessage(LOG_LEVEL_DEBUG, "Filesize %d of %s", (I32) st.st_size, CompletePath); - - return st.st_size; - */ - struct stat st; - C8 CompletePath[MAX_FILE_PATH]; - C8 temporaryCompletePath[MAX_FILE_PATH]; - bzero(CompletePath, MAX_FILE_PATH); - - if (SystemControlDirectoryInfo.exist) return -1; - - UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); - strcat(CompletePath, Path); - stat(CompletePath, &st); - // Create new temporary file, containing the length of the current file in hex + the rest of the document - strcat(temporaryCompletePath,".temp"); - FILE * comp_fd = fopen(CompletePath,"r"); - FILE * temp_fd = fopen(temporaryCompletePath,"w"); - - fprintf(temp_fd,"%c%c%c%c", - (U8) (st.st_size >> 24), - (U8) (st.st_size >> 16), - (U8) (st.st_size >> 8), - (U8) (st.st_size) - ); - - while (!feof(comp_fd)) { - fputc(fgetc(comp_fd),temp_fd); - } + while (!feof(comp_fd)) { + fputc(fgetc(comp_fd), temp_fd); + } + + fclose(comp_fd); + fclose(temp_fd); - fclose(comp_fd); - fclose(temp_fd); - - // Rename the temporary file to the name of the previous one - rename(temporaryCompletePath, CompletePath); - stat(CompletePath, &st); - // Create mmap of the file and return the length - SystemControlDirectoryInfo.fd = open(CompletePath, O_RDWR); - SystemControlDirectoryInfo.info_buffer = mmap(NULL,st.st_size,PROT_READ | PROT_WRITE, MAP_PRIVATE, SystemControlDirectoryInfo.fd,0); - SystemControlDirectoryInfo.size = st.st_size; - SystemControlDirectoryInfo.exist = 1; - return st.st_size; + // Rename the temporary file to the name of the previous one + rename(temporaryCompletePath, CompletePath); + stat(CompletePath, &st); + // Create mmap of the file and return the length + SystemControlDirectoryInfo.fd = open(CompletePath, O_RDWR); + SystemControlDirectoryInfo.info_buffer = + mmap(NULL, st.st_size, PROT_READ | PROT_WRITE, MAP_PRIVATE, SystemControlDirectoryInfo.fd, 0); + SystemControlDirectoryInfo.size = st.st_size; + SystemControlDirectoryInfo.exist = 1; + return st.st_size; } -I32 SystemControlDestroyFileContentInfo(C8 *path) -{ - char CompletePath[MAX_FILE_PATH]; - struct stat st; - if (!SystemControlDirectoryInfo.exist) return -1; - UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); - strcat(CompletePath, path); - - munmap(SystemControlDirectoryInfo.info_buffer, SystemControlDirectoryInfo.size); - close(SystemControlDirectoryInfo.fd); - SystemControlDirectoryInfo.exist = 0; - //remove(CompletePath); - return 0; +I32 SystemControlDestroyFileContentInfo(C8 * path) { + char CompletePath[MAX_FILE_PATH]; + struct stat st; + + if (!SystemControlDirectoryInfo.exist) + return -1; + UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); + strcat(CompletePath, path); + + munmap(SystemControlDirectoryInfo.info_buffer, SystemControlDirectoryInfo.size); + close(SystemControlDirectoryInfo.fd); + SystemControlDirectoryInfo.exist = 0; + //remove(CompletePath); + return 0; } I32 SystemControlCheckFileDirectoryExist(C8 * ParameterName, C8 * ReturnValue, U8 Debug) { From 4ff0c28536537ea693f3a989cedf22a50021fa97 Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Mon, 2 Mar 2020 11:18:46 +0100 Subject: [PATCH 299/523] Ran formatting script --- server/src/citscontrol.c | 222 +++++++++++++++++++++------------------ server/src/util.c | 6 +- 2 files changed, 122 insertions(+), 106 deletions(-) diff --git a/server/src/citscontrol.c b/server/src/citscontrol.c index f206e4f4c..05f45faeb 100644 --- a/server/src/citscontrol.c +++ b/server/src/citscontrol.c @@ -645,16 +645,18 @@ CAM_t *allocateCAMStruct(void) { cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.accelerationControl = NULL; // TODO: Allocate memory for this cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.lanePosition = calloc(1, sizeof (LanePosition_t)); - cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - steeringWheelAngle = calloc(1, sizeof (SteeringWheelAngle_t)); - cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - lateralAcceleration = calloc(1, sizeof (LateralAcceleration_t)); - cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - verticalAcceleration = calloc(1, sizeof (VerticalAcceleration_t)); - cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - performanceClass = calloc(1, sizeof (PerformanceClass_t)); - cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - cenDsrcTollingZone = calloc(1, sizeof (CenDsrcTollingZone_t)); + cam->cam.camParameters.highFrequencyContainer.choice. + basicVehicleContainerHighFrequency.steeringWheelAngle = calloc(1, sizeof (SteeringWheelAngle_t)); + cam->cam.camParameters.highFrequencyContainer.choice. + basicVehicleContainerHighFrequency.lateralAcceleration = + calloc(1, sizeof (LateralAcceleration_t)); + cam->cam.camParameters.highFrequencyContainer.choice. + basicVehicleContainerHighFrequency.verticalAcceleration = + calloc(1, sizeof (VerticalAcceleration_t)); + cam->cam.camParameters.highFrequencyContainer.choice. + basicVehicleContainerHighFrequency.performanceClass = calloc(1, sizeof (PerformanceClass_t)); + cam->cam.camParameters.highFrequencyContainer.choice. + basicVehicleContainerHighFrequency.cenDsrcTollingZone = calloc(1, sizeof (CenDsrcTollingZone_t)); cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.cenDsrcTollingZone->cenDsrcTollingZoneID = NULL; // TODO: Allocate memory for this BasicVehicleContainerHighFrequency_t *bvc = @@ -698,67 +700,71 @@ void initializeCAMStruct(CAM_t * cam) { cam->cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_basicVehicleContainerHighFrequency; - cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading. - headingValue = HeadingValue_unavailable; - cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading. - headingConfidence = HeadingConfidence_unavailable; + cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + heading.headingValue = HeadingValue_unavailable; + cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + heading.headingConfidence = HeadingConfidence_unavailable; cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.speed.speedValue = SpeedValue_unavailable; - cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.speed. - speedConfidence = SpeedConfidence_unavailable; + cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + speed.speedConfidence = SpeedConfidence_unavailable; cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.driveDirection = DriveDirection_unavailable; - cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.vehicleLength. - vehicleLengthValue = VehicleLengthValue_unavailable; - cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.vehicleLength. - vehicleLengthConfidenceIndication = VehicleLengthConfidenceIndication_unavailable; + cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + vehicleLength.vehicleLengthValue = VehicleLengthValue_unavailable; + cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + vehicleLength.vehicleLengthConfidenceIndication = VehicleLengthConfidenceIndication_unavailable; cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.vehicleWidth = VehicleWidth_unavailable; + cam->cam.camParameters.highFrequencyContainer.choice. + basicVehicleContainerHighFrequency.longitudinalAcceleration.longitudinalAccelerationValue = + LongitudinalAccelerationValue_unavailable; + cam->cam.camParameters.highFrequencyContainer.choice. + basicVehicleContainerHighFrequency.longitudinalAcceleration.longitudinalAccelerationConfidence = + AccelerationConfidence_unavailable; cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - longitudinalAcceleration.longitudinalAccelerationValue = LongitudinalAccelerationValue_unavailable; + curvature.curvatureValue = CurvatureValue_unavailable; cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - longitudinalAcceleration.longitudinalAccelerationConfidence = AccelerationConfidence_unavailable; - cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.curvature. - curvatureValue = CurvatureValue_unavailable; - cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.curvature. - curvatureConfidence = CurvatureConfidence_unavailable; + curvature.curvatureConfidence = CurvatureConfidence_unavailable; + cam->cam.camParameters.highFrequencyContainer.choice. + basicVehicleContainerHighFrequency.curvatureCalculationMode = CurvatureCalculationMode_unavailable; cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - curvatureCalculationMode = CurvatureCalculationMode_unavailable; - cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.yawRate. - yawRateValue = YawRateValue_unavailable; - cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.yawRate. - yawRateConfidence = YawRateConfidence_unavailable; + yawRate.yawRateValue = YawRateValue_unavailable; cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - lateralAcceleration->lateralAccelerationValue = LateralAccelerationValue_unavailable; - cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - lateralAcceleration->lateralAccelerationConfidence = AccelerationConfidence_unavailable; + yawRate.yawRateConfidence = YawRateConfidence_unavailable; + cam->cam.camParameters.highFrequencyContainer.choice. + basicVehicleContainerHighFrequency.lateralAcceleration->lateralAccelerationValue = + LateralAccelerationValue_unavailable; + cam->cam.camParameters.highFrequencyContainer.choice. + basicVehicleContainerHighFrequency.lateralAcceleration->lateralAccelerationConfidence = + AccelerationConfidence_unavailable; // Unused highFrequencyContainer optional fields (null their pointers to show unused) // TODO: Modify here to once relevant information can be used - free(cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - accelerationControl); - free(cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - lanePosition); - free(cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - steeringWheelAngle); - free(cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - verticalAcceleration); - free(cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - performanceClass); - free(cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - cenDsrcTollingZone); - cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - accelerationControl = NULL; + free(cam->cam.camParameters.highFrequencyContainer.choice. + basicVehicleContainerHighFrequency.accelerationControl); + free(cam->cam.camParameters.highFrequencyContainer.choice. + basicVehicleContainerHighFrequency.lanePosition); + free(cam->cam.camParameters.highFrequencyContainer.choice. + basicVehicleContainerHighFrequency.steeringWheelAngle); + free(cam->cam.camParameters.highFrequencyContainer.choice. + basicVehicleContainerHighFrequency.verticalAcceleration); + free(cam->cam.camParameters.highFrequencyContainer.choice. + basicVehicleContainerHighFrequency.performanceClass); + free(cam->cam.camParameters.highFrequencyContainer.choice. + basicVehicleContainerHighFrequency.cenDsrcTollingZone); + cam->cam.camParameters.highFrequencyContainer.choice. + basicVehicleContainerHighFrequency.accelerationControl = NULL; cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.lanePosition = NULL; - cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - steeringWheelAngle = NULL; - cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - verticalAcceleration = NULL; + cam->cam.camParameters.highFrequencyContainer.choice. + basicVehicleContainerHighFrequency.steeringWheelAngle = NULL; + cam->cam.camParameters.highFrequencyContainer.choice. + basicVehicleContainerHighFrequency.verticalAcceleration = NULL; cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.performanceClass = NULL; - cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - cenDsrcTollingZone = NULL; + cam->cam.camParameters.highFrequencyContainer.choice. + basicVehicleContainerHighFrequency.cenDsrcTollingZone = NULL; // Low frequency container and special vehicle containers unused for now (TODO) free(cam->cam.camParameters.lowFrequencyContainer); @@ -921,12 +927,12 @@ I32 generateCAMMessage(MONRType * MONRData, CAM_t * cam) { tempCAM->cam.camParameters.basicContainer.referencePosition.longitude = (long)(longitude * 10000000.0); - tempCAM->cam.camParameters.basicContainer.referencePosition.positionConfidenceEllipse. - semiMajorConfidence = SemiAxisLength_unavailable; - tempCAM->cam.camParameters.basicContainer.referencePosition.positionConfidenceEllipse. - semiMinorConfidence = SemiAxisLength_unavailable; - tempCAM->cam.camParameters.basicContainer.referencePosition.positionConfidenceEllipse. - semiMajorOrientation = 0; + tempCAM->cam.camParameters.basicContainer.referencePosition. + positionConfidenceEllipse.semiMajorConfidence = SemiAxisLength_unavailable; + tempCAM->cam.camParameters.basicContainer.referencePosition. + positionConfidenceEllipse.semiMinorConfidence = SemiAxisLength_unavailable; + tempCAM->cam.camParameters.basicContainer.referencePosition. + positionConfidenceEllipse.semiMajorOrientation = 0; tempCAM->cam.camParameters.basicContainer.referencePosition.altitude.altitudeValue = AltitudeValue_unavailable; @@ -939,70 +945,80 @@ I32 generateCAMMessage(MONRType * MONRData, CAM_t * cam) { tempCAM->cam.camParameters.basicContainer.stationType = StationType_roadSideUnit; if (tempCAM->cam.camParameters.highFrequencyContainer.present == HighFrequencyContainer_PR_basicVehicleContainerHighFrequency) { - tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.speed. - speedValue = + tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + speed.speedValue = (long)(sqrt (pow((double)(MONRData->LongitudinalSpeedI16), 2) + pow((double)(MONRData->LateralSpeedI16), 2))); - tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.speed. - speedConfidence = SpeedConfidence_unavailable; + tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + speed.speedConfidence = SpeedConfidence_unavailable; + tempCAM->cam.camParameters.highFrequencyContainer.choice. + basicVehicleContainerHighFrequency.driveDirection = MONRData->DriveDirectionU8; tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - driveDirection = MONRData->DriveDirectionU8; - tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading. - headingValue = MONRData->HeadingU16 / 10; - tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading. - headingConfidence = HeadingConfidence_unavailable; + heading.headingValue = MONRData->HeadingU16 / 10; + tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + heading.headingConfidence = HeadingConfidence_unavailable; if (MONRData->LongitudinalAccI16 == 32001) - tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - longitudinalAcceleration.longitudinalAccelerationValue = + tempCAM->cam.camParameters.highFrequencyContainer.choice. + basicVehicleContainerHighFrequency.longitudinalAcceleration.longitudinalAccelerationValue = LongitudinalAccelerationValue_unavailable; else if (MONRData->LongitudinalAccI16 > 16000) - tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - longitudinalAcceleration.longitudinalAccelerationValue = 160; + tempCAM->cam.camParameters.highFrequencyContainer.choice. + basicVehicleContainerHighFrequency.longitudinalAcceleration.longitudinalAccelerationValue = + 160; else if (MONRData->LongitudinalAccI16 < -16000) - tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - longitudinalAcceleration.longitudinalAccelerationValue = -160; + tempCAM->cam.camParameters.highFrequencyContainer.choice. + basicVehicleContainerHighFrequency.longitudinalAcceleration.longitudinalAccelerationValue = + -160; else - tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - longitudinalAcceleration.longitudinalAccelerationValue = MONRData->LongitudinalAccI16 / 100; - tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - longitudinalAcceleration.longitudinalAccelerationConfidence = AccelerationConfidence_unavailable; + tempCAM->cam.camParameters.highFrequencyContainer.choice. + basicVehicleContainerHighFrequency.longitudinalAcceleration.longitudinalAccelerationValue = + MONRData->LongitudinalAccI16 / 100; + tempCAM->cam.camParameters.highFrequencyContainer.choice. + basicVehicleContainerHighFrequency.longitudinalAcceleration.longitudinalAccelerationConfidence = + AccelerationConfidence_unavailable; if (MONRData->LateralAccI16 == 32001) - tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - lateralAcceleration->lateralAccelerationValue = LateralAccelerationValue_unavailable; + tempCAM->cam.camParameters.highFrequencyContainer.choice. + basicVehicleContainerHighFrequency.lateralAcceleration->lateralAccelerationValue = + LateralAccelerationValue_unavailable; else if (MONRData->LateralAccI16 > 16000) - tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - lateralAcceleration->lateralAccelerationValue = 160; + tempCAM->cam.camParameters.highFrequencyContainer.choice. + basicVehicleContainerHighFrequency.lateralAcceleration->lateralAccelerationValue = 160; else if (MONRData->LateralAccI16 < -16000) - tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - lateralAcceleration->lateralAccelerationValue = -160; + tempCAM->cam.camParameters.highFrequencyContainer.choice. + basicVehicleContainerHighFrequency.lateralAcceleration->lateralAccelerationValue = -160; else - tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - lateralAcceleration->lateralAccelerationValue = MONRData->LateralAccI16 / 100; - tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - lateralAcceleration->lateralAccelerationConfidence = AccelerationConfidence_unavailable; - - tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.yawRate. - yawRateValue = YawRateValue_unavailable; - tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.yawRate. - yawRateConfidence = YawRateConfidence_unavailable; + tempCAM->cam.camParameters.highFrequencyContainer.choice. + basicVehicleContainerHighFrequency.lateralAcceleration->lateralAccelerationValue = + MONRData->LateralAccI16 / 100; + tempCAM->cam.camParameters.highFrequencyContainer.choice. + basicVehicleContainerHighFrequency.lateralAcceleration->lateralAccelerationConfidence = + AccelerationConfidence_unavailable; - tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.curvature. - curvatureValue = CurvatureValue_unavailable; - tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.curvature. - curvatureConfidence = CurvatureConfidence_unavailable; tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - curvatureCalculationMode = CurvatureCalculationMode_unavailable; - + yawRate.yawRateValue = YawRateValue_unavailable; tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - vehicleWidth = VehicleWidth_unavailable; + yawRate.yawRateConfidence = YawRateConfidence_unavailable; + tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - vehicleLength.vehicleLengthValue = VehicleLengthValue_unavailable; + curvature.curvatureValue = CurvatureValue_unavailable; tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - vehicleLength.vehicleLengthConfidenceIndication = VehicleLengthConfidenceIndication_unavailable; + curvature.curvatureConfidence = CurvatureConfidence_unavailable; + tempCAM->cam.camParameters.highFrequencyContainer.choice. + basicVehicleContainerHighFrequency.curvatureCalculationMode = + CurvatureCalculationMode_unavailable; + + tempCAM->cam.camParameters.highFrequencyContainer.choice. + basicVehicleContainerHighFrequency.vehicleWidth = VehicleWidth_unavailable; + tempCAM->cam.camParameters.highFrequencyContainer.choice. + basicVehicleContainerHighFrequency.vehicleLength.vehicleLengthValue = + VehicleLengthValue_unavailable; + tempCAM->cam.camParameters.highFrequencyContainer.choice. + basicVehicleContainerHighFrequency.vehicleLength.vehicleLengthConfidenceIndication = + VehicleLengthConfidenceIndication_unavailable; } diff --git a/server/src/util.c b/server/src/util.c index f1f1de02e..59409768b 100644 --- a/server/src/util.c +++ b/server/src/util.c @@ -2012,9 +2012,9 @@ int iCommSend(const enum COMMAND iCommand, const char *cpData, size_t dataLength case MQBUS_OK: return 0; case MQBUS_MQ_FULL: - LogMessage(LOG_LEVEL_WARNING, - "Attempted to write to full message queue - message may be lost: <%d><%s>", iCommand, - cpData); + LogMessage(LOG_LEVEL_WARNING, + "Attempted to write to full message queue - message may be lost: <%d><%s>", iCommand, + cpData); return 1; case MQBUS_INVALID_INPUT_ARGUMENT: LogMessage(LOG_LEVEL_WARNING, "Invalid message queue message length"); From 16a9965f7f94becd7a2edb0de5eaf0c7d393e1ee Mon Sep 17 00:00:00 2001 From: alfaro01 Date: Mon, 2 Mar 2020 13:35:05 +0100 Subject: [PATCH 300/523] Updated integration test script to not run unnumbered tests --- allMaestroIntegrationTests.sh | 35 +++++++++++++++++++++-------------- 1 file changed, 21 insertions(+), 14 deletions(-) diff --git a/allMaestroIntegrationTests.sh b/allMaestroIntegrationTests.sh index 4c880c3f8..1cc6e63f0 100755 --- a/allMaestroIntegrationTests.sh +++ b/allMaestroIntegrationTests.sh @@ -3,25 +3,32 @@ MAESTRODIR=$(pwd) cd ${MAESTRODIR}/server/integration-tests FAILURES=0 NUM_TESTS=0 +NUMBER_REGEX='^[0-9]+' echo "Running integration tests" for f in $(pwd)/*; do rm -f /dev/mqueue/* - if [ ${f: -3} == ".py" ]; then - echo "Running ${f}" - python3 "$f" - if [ "$?" != "0" ]; then - echo "Failed test ${f}" - FAILURES=$((FAILURES+1)) + fname="$(basename -- ${f})" + if [[ ${fname:0:3} =~ $NUMBER_REGEX ]] ; then + echo "Running ${fname}" + if [ ${fname: -3} == ".py" ]; then + python3 "$f" + if [ "$?" != "0" ]; then + echo "Failed test ${fname}" + FAILURES=$((FAILURES+1)) + fi + NUM_TESTS=$((NUM_TESTS+1)) + elif [ ${fname: -3} == ".sh" ]; then + if [ $(sh "$f" -H > /dev/null 2>&1) ]; then + echo "Failed test ${fname}" + FAILURES=$((FAILURES+1)) + fi + NUM_TESTS=$((NUM_TESTS+1)) + else + echo "Skipping ${fname}" fi - NUM_TESTS=$((NUM_TESTS+1)) - elif [ ${f: -3} == ".sh" ]; then - echo "Running ${f}" - if [ $(sh "$f" -H > /dev/null 2>&1) ]; then - echo "Failed test ${f}" - FAILURES=$((FAILURES+1)) - fi - NUM_TESTS=$((NUM_TESTS+1)) + else + echo "Skipping ${fname}" fi done From 6f25e017cffa26a55267c958bf301033c0cb416f Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 2 Mar 2020 14:33:58 +0100 Subject: [PATCH 301/523] Updated supervision and visualization for installation --- modules/Supervision/CMakeLists.txt | 77 +++++++++------------------- modules/Visualization/CMakeLists.txt | 75 ++++++++------------------- 2 files changed, 44 insertions(+), 108 deletions(-) diff --git a/modules/Supervision/CMakeLists.txt b/modules/Supervision/CMakeLists.txt index 48d949c32..8ac598d33 100644 --- a/modules/Supervision/CMakeLists.txt +++ b/modules/Supervision/CMakeLists.txt @@ -4,23 +4,19 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_C_STANDARD 11) set(CMAKE_C_STANDARD_REQUIRED ON) -project(Supervision) +project(Supervision LANGUAGES C CXX) # Define target names set(SUPERVISION_TARGET ${PROJECT_NAME}) -set(UTIL_TARGET MaestroUtil) -set(TIME_TARGET MaestroTime) -set(LOGGING_TARGET MaestroLogging) -set(MESSAGE_BUS_TARGET MaestroMQ) -# Define paths to related files -set(CORE_ROOT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/../../core) -set(CORE_INCLUDE_PATH ${CORE_ROOT_PATH}/inc) -set(CORE_SOURCE_PATH ${CORE_ROOT_PATH}/src) -set(UTIL_ROOT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C) -set(TIME_PATH ${UTIL_ROOT_PATH}/time) -set(LOGGING_PATH ${UTIL_ROOT_PATH}/logging) -set(MESSAGE_BUS_PATH ${UTIL_ROOT_PATH}/mqbus) +set(COREUTILS_LIBRARY MaestroCoreUtil) +set(ISO_22133_LIBRARY MaestroISO22133) +set(POSITIONING_LIBRARY MaestroPositioning) +set(TIME_LIBRARY MaestroTime) +set(LOGGING_LIBRARY MaestroLogging) +set(MESSAGE_BUS_LIBRARY MaestroMQ) + +include(GNUInstallDirs) # Create project main executable target add_executable(${SUPERVISION_TARGET} @@ -30,48 +26,21 @@ add_executable(${SUPERVISION_TARGET} ${CMAKE_CURRENT_SOURCE_DIR}/src/trajectory.cpp ) -# If building locally, it is necessary to add relevant util targets -if(NOT TARGET ${TIME_TARGET}) - add_library(${TIME_TARGET} STATIC - ${TIME_PATH}/maestroTime.c - ) -endif() - -if(NOT TARGET ${LOGGING_TARGET}) - add_library(${LOGGING_TARGET} STATIC - ${LOGGING_PATH}/logging.c - ) -endif() - -if(NOT TARGET ${MESSAGE_BUS_TARGET}) - add_library(${MESSAGE_BUS_TARGET} STATIC - ${MESSAGE_BUS_PATH}/mqbus.c - ) - target_link_libraries(${MESSAGE_BUS_TARGET} rt) -endif() - -if(NOT TARGET ${UTIL_TARGET}) - add_library(${UTIL_TARGET} STATIC - ${CORE_SOURCE_PATH}/util.c - ${CORE_SOURCE_PATH}/iso22133.c - ${CORE_SOURCE_PATH}/positioning.c - ${CORE_SOURCE_PATH}/datadictionary.c - ) - target_link_libraries(${UTIL_TARGET} ${LOGGING_TARGET} ${TIME_TARGET} ${MESSAGE_BUS_TARGET} m) - target_include_directories(${UTIL_TARGET} PUBLIC - ${CORE_INCLUDE_PATH} - ${MESSAGE_BUS_PATH} - ${LOGGING_PATH} - ${TIME_PATH} - ) -endif() - # Link project executable to util libraries -target_link_libraries(${SUPERVISION_TARGET} LINK_PUBLIC ${TIME_TARGET} ${UTIL_TARGET} ${LOGGING_TARGET}) +target_link_libraries(${SUPERVISION_TARGET} LINK_PUBLIC + ${TIME_LIBRARY} + ${COREUTILS_LIBRARY} + ${ISO_22133_LIBRARY} + ${POSITIONING_LIBRARY} + ${LOGGING_LIBRARY} +) target_include_directories(${SUPERVISION_TARGET} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/inc - ${TIME_PATH} - ${LOGGING_PATH} - ${MESSAGE_BUS_PATH} - ${CORE_INCLUDE_PATH} +) + +install(TARGETS ${SUPERVISION_TARGET} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} ) diff --git a/modules/Visualization/CMakeLists.txt b/modules/Visualization/CMakeLists.txt index 56156e740..ac2832652 100644 --- a/modules/Visualization/CMakeLists.txt +++ b/modules/Visualization/CMakeLists.txt @@ -2,69 +2,36 @@ cmake_minimum_required(VERSION 3.1) set(CMAKE_C_STANDARD 11) set(CMAKE_C_STANDARD_REQUIRED ON) -project(Visualization) +project(Visualization LANGUAGES C) # Define target names set(VISUALIZATION_TARGET ${PROJECT_NAME}) -set(UTIL_TARGET MaestroUtil) -set(TIME_TARGET MaestroTime) -set(LOGGING_TARGET MaestroLogging) -set(MESSAGE_BUS_TARGET MaestroMQ) -# Define paths to related files -set(CORE_ROOT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/../../core) -set(CORE_INCLUDE_PATH ${CORE_ROOT_PATH}/inc) -set(CORE_SOURCE_PATH ${CORE_ROOT_PATH}/src) -set(UTIL_ROOT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C) -set(TIME_PATH ${UTIL_ROOT_PATH}/time) -set(LOGGING_PATH ${UTIL_ROOT_PATH}/logging) -set(MESSAGE_BUS_PATH ${UTIL_ROOT_PATH}/mqbus) +set(COREUTILS_LIBRARY MaestroCoreUtil) +set(ISO_22133_LIBRARY MaestroISO22133) +set(POSITIONING_LIBRARY MaestroPositioning) +set(TIME_LIBRARY MaestroTime) +set(LOGGING_LIBRARY MaestroLogging) +set(MESSAGE_BUS_LIBRARY MaestroMQ) # Create project main executable target add_executable(${VISUALIZATION_TARGET} ${CMAKE_CURRENT_SOURCE_DIR}/src/main.c ) -# If building locally, it is necessary to add relevant util targets -if(NOT TARGET ${TIME_TARGET}) - add_library(${TIME_TARGET} STATIC - ${TIME_PATH}/maestroTime.c - ) -endif() - -if(NOT TARGET ${LOGGING_TARGET}) - add_library(${LOGGING_TARGET} STATIC - ${LOGGING_PATH}/logging.c - ) -endif() - -if(NOT TARGET ${MESSAGE_BUS_TARGET}) - add_library(${MESSAGE_BUS_TARGET} STATIC - ${MESSAGE_BUS_PATH}/mqbus.c - ) - target_link_libraries(${MESSAGE_BUS_TARGET} rt) -endif() - -if(NOT TARGET ${UTIL_TARGET}) - add_library(${UTIL_TARGET} STATIC - ${CORE_SOURCE_PATH}/util.c - ${CORE_SOURCE_PATH}/iso22133.c - ${CORE_SOURCE_PATH}/positioning.c - ${CORE_SOURCE_PATH}/datadictionary.c - ) - target_link_libraries(${UTIL_TARGET} ${LOGGING_TARGET} ${TIME_TARGET} ${MESSAGE_BUS_TARGET} m) - target_include_directories(${UTIL_TARGET} PUBLIC - ${CORE_INCLUDE_PATH} - ${MESSAGE_BUS_PATH} - ${LOGGING_PATH} - ${TIME_PATH} - ) -endif() - # Link project executable to util libraries -target_link_libraries(${VISUALIZATION_TARGET} LINK_PUBLIC ${TIME_TARGET} ${LOGGING_TARGET} ${UTIL_TARGET}) -target_include_directories(${VISUALIZATION_TARGET} PUBLIC - ${TIME_PATH} - ${LOGGING_PATH} - ${CORE_INCLUDE_PATH} +target_link_libraries(${VISUALIZATION_TARGET} LINK_PUBLIC + ${COREUTILS_LIBRARY} + ${ISO_22133_LIBRARY} + ${POSITIONING_LIBRARY} + ${TIME_LIBRARY} + ${LOGGING_LIBRARY} + ${MESSAGE_BUS_LIBRARY} +) + +install(TARGETS ${VISUALIZATION_TARGET} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} ) From e6628b3a443a3cbb50f2010423fb5713e8c79c9f Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 2 Mar 2020 14:34:16 +0100 Subject: [PATCH 302/523] Enabled all modules in top level cmakelists --- CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3d2acec7c..2652bead7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -13,8 +13,8 @@ set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin) add_subdirectory(util/C) add_subdirectory(core) add_subdirectory(modules/ScenarioControl) -#add_subdirectory(modules/Supervision) -#add_subdirectory(modules/Visualization) +add_subdirectory(modules/Supervision) +add_subdirectory(modules/Visualization) # Create directory for test data in user home directory install(DIRECTORY DESTINATION "$ENV{HOME}/.maestro" USE_SOURCE_PERMISSIONS) From bdeb157a15c11ddddac2f0a306c970cb39407887 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 2 Mar 2020 14:34:50 +0100 Subject: [PATCH 303/523] Updated util reference --- util | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/util b/util index 21967d535..99ccf4b7e 160000 --- a/util +++ b/util @@ -1 +1 @@ -Subproject commit 21967d53556bbffeb309492bfee1036a3b750b9d +Subproject commit 99ccf4b7edbfeb4f675847b379c316eb42d365d6 From 861fed537edc56a1d4a3eebb314e648285f3cacc Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 2 Mar 2020 15:33:02 +0100 Subject: [PATCH 304/523] Set capabilities of built target --- core/CMakeLists.txt | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index cf81d2715..f8340e834 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -91,6 +91,7 @@ target_include_directories(${COREUTILS_TARGET} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/inc ) +install(CODE "MESSAGE(STATUS \"Installing target ${CORE_TARGET}\")") install(TARGETS ${CORE_TARGET} ${COREUTILS_TARGET} ${ISO_22133_TARGET} ${DATA_DICTIONARY_TARGET} ${POSITIONING_TARGET} @@ -100,7 +101,11 @@ install(TARGETS ${CORE_TARGET} ${COREUTILS_TARGET} PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} ) +install(CODE "MESSAGE(STATUS \"Giving system time setting capabilities to target ${CORE_TARGET}\")") +install(CODE "execute_process(COMMAND setcap CAP_SYS_TIME+ep ${CMAKE_INSTALL_FULL_BINDIR}/${CORE_TARGET})") + # Create shared memory directory and give user access +install(CODE "MESSAGE(STATUS \"Creating shared memory directory ${SYSTEM_SHARED_MEMORY_PATH} owned by user $ENV{USER}\")") install(DIRECTORY DESTINATION ${SYSTEM_SHARED_MEMORY_PATH}) install(CODE "execute_process(COMMAND chown $ENV{USER} ${SYSTEM_SHARED_MEMORY_PATH})") From 574c0d511913078d5a28cc7092ddc29091575581 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 2 Mar 2020 16:16:34 +0100 Subject: [PATCH 305/523] Added some printouts during the installation --- CMakeLists.txt | 1 + modules/ScenarioControl/CMakeLists.txt | 1 + modules/Supervision/CMakeLists.txt | 1 + modules/Visualization/CMakeLists.txt | 1 + 4 files changed, 4 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 2652bead7..704a94d0a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -17,6 +17,7 @@ add_subdirectory(modules/Supervision) add_subdirectory(modules/Visualization) # Create directory for test data in user home directory +install(CODE "MESSAGE(STATUS \"Creating home directory environment under $ENV{HOME}\")") install(DIRECTORY DESTINATION "$ENV{HOME}/.maestro" USE_SOURCE_PERMISSIONS) install(DIRECTORY DESTINATION "$ENV{HOME}/.maestro/journal" USE_SOURCE_PERMISSIONS) install(DIRECTORY DESTINATION "$ENV{HOME}/.maestro/traj" USE_SOURCE_PERMISSIONS) diff --git a/modules/ScenarioControl/CMakeLists.txt b/modules/ScenarioControl/CMakeLists.txt index f3ec99a42..1d28a65c2 100644 --- a/modules/ScenarioControl/CMakeLists.txt +++ b/modules/ScenarioControl/CMakeLists.txt @@ -44,6 +44,7 @@ target_include_directories(${SCENARIO_CONTROL_TARGET} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/inc ) +install(CODE "MESSAGE(STATUS \"Installing target ${SCENARIO_CONTROL_TARGET}\")") install(TARGETS ${SCENARIO_CONTROL_TARGET} RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} diff --git a/modules/Supervision/CMakeLists.txt b/modules/Supervision/CMakeLists.txt index 8ac598d33..fe8cf3dee 100644 --- a/modules/Supervision/CMakeLists.txt +++ b/modules/Supervision/CMakeLists.txt @@ -38,6 +38,7 @@ target_include_directories(${SUPERVISION_TARGET} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/inc ) +install(CODE "MESSAGE(STATUS \"Installing target ${SUPERVISION_TARGET}\")") install(TARGETS ${SUPERVISION_TARGET} RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} diff --git a/modules/Visualization/CMakeLists.txt b/modules/Visualization/CMakeLists.txt index ac2832652..3110b7bc6 100644 --- a/modules/Visualization/CMakeLists.txt +++ b/modules/Visualization/CMakeLists.txt @@ -29,6 +29,7 @@ target_link_libraries(${VISUALIZATION_TARGET} LINK_PUBLIC ${MESSAGE_BUS_LIBRARY} ) +install(CODE "MESSAGE(STATUS \"Installing target ${VISUALIZATION_TARGET}\")") install(TARGETS ${VISUALIZATION_TARGET} RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} From 5fcf6592b14c7fa6f61fde5782e66dd747104d48 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 3 Mar 2020 12:03:27 +0100 Subject: [PATCH 306/523] Updated jenkinsfile for parallel build and cmake --- Jenkinsfile | 60 +++++++++++++++++++++++++++++++---------------------- 1 file changed, 35 insertions(+), 25 deletions(-) diff --git a/Jenkinsfile b/Jenkinsfile index 71c65f554..762c14e76 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -1,30 +1,40 @@ pipeline { - agent any - - options { - timeout(time: 15, unit: 'MINUTES') - } - stages { - stage('Build') { - steps { - sh 'echo "Executing build script..."' - sh './buildMaestro.sh' - } - } - stage('Integration testing') { - steps { - sh 'echo "Running Maestro integration tests..."' - sh './allMaestroIntegrationTests.sh' - } - } - stage('Format check') { - steps { - sh 'echo "Running code formatting check..."' - sh './checkCodeFormat.sh' - } - } - } + agent any + + options { + timeout(time: 15, unit: 'MINUTES') + } + stages { + stage('Build') { + steps { + sh 'echo "Executing build steps..."' + cmake installation: 'InSearchPath' + cmakeBuild + buildDir: 'build', + installation: 'InSearchPath', + steps: [ + [args: 'all', envVars: 'DESTDIR=${WORKSPACE}/artifacts'] + ] + } + } + stage('Run tests') { + parallel { + stage('Integration tests') { + steps { + sh 'echo "Running Maestro integration tests..."' + sh './allMaestroIntegrationTests.sh' + } + } + stage('Format check') { + steps { + sh 'echo "Running code formatting check..."' + sh './checkCodeFormat.sh' + } + } + } + } + } } From a6a1a3ea560c3aae172c755dd3c45bde09f19525 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 3 Mar 2020 12:07:14 +0100 Subject: [PATCH 307/523] Updated Jenkinsfile --- Jenkinsfile | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/Jenkinsfile b/Jenkinsfile index 762c14e76..7e5ba320d 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -10,12 +10,7 @@ pipeline { steps { sh 'echo "Executing build steps..."' cmake installation: 'InSearchPath' - cmakeBuild - buildDir: 'build', - installation: 'InSearchPath', - steps: [ - [args: 'all', envVars: 'DESTDIR=${WORKSPACE}/artifacts'] - ] + cmakeBuild buildDir: 'build', installation: 'InSearchPath', steps: [[args: 'all', envVars: 'DESTDIR=${WORKSPACE}/artifacts', withCmake: true]] } } stage('Run tests') { From 91bc3b6a436e4024c86360a732b35a2ea086bfed Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Tue, 3 Mar 2020 12:08:15 +0100 Subject: [PATCH 308/523] read mq when not connected --- server/src/systemcontrol.c | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index 457b863ce..4de95c855 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -319,7 +319,7 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { } - while (!iExit) { + while (!iExit) { if (server_state == SERVER_STATE_ERROR) { iCommSend(COMM_ABORT, NULL, 0); continue; @@ -1111,6 +1111,8 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { nanosleep(&sleep_time, &ref_time); } + + (void)iCommClose(); LogMessage(LOG_LEVEL_INFO, "Exiting"); @@ -1403,6 +1405,11 @@ static I32 SystemControlInitServer(int *ClientSocket, int *ServerHandle, struct int result = 0; int sockFlags = 0; + enum COMMAND iCommand; + ssize_t bytesReceived = 0; + char pcRecvBuffer[SC_RECV_MESSAGE_BUFFER]; + + /* Init user control socket */ LogMessage(LOG_LEVEL_INFO, "Init control socket"); @@ -1439,7 +1446,7 @@ static I32 SystemControlInitServer(int *ClientSocket, int *ServerHandle, struct /* Set socket to nonblocking */ sockFlags = fcntl(*ServerHandle, F_GETFL, 0); - if (sockFlags == -1) + if (sockFlags == -1) util_error("Error calling fcntl"); sockFlags = sockFlags | O_NONBLOCK; @@ -1448,8 +1455,12 @@ static I32 SystemControlInitServer(int *ClientSocket, int *ServerHandle, struct do { *ClientSocket = accept(*ServerHandle, (struct sockaddr *)&cli_addr, &cli_length); - if ((*ClientSocket == -1 && errno != EAGAIN && errno != EWOULDBLOCK) || iExit) - util_error("Failed to establish connection"); + if ((*ClientSocket == -1 && errno != EAGAIN && errno != EWOULDBLOCK) || iExit) + util_error("Failed to establish connection"); + + bytesReceived = iCommRecv(&iCommand, pcRecvBuffer, SC_RECV_MESSAGE_BUFFER, NULL); + + } while (*ClientSocket == -1); LogMessage(LOG_LEVEL_INFO, "Connection established: %s:%i", inet_ntoa(cli_addr.sin_addr), From 60e546f56e179846143932b784a0e43d109b84d9 Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Tue, 3 Mar 2020 12:12:45 +0100 Subject: [PATCH 309/523] whitespacefix --- server/src/systemcontrol.c | 2 -- 1 file changed, 2 deletions(-) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index 4de95c855..8846dd0c2 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -1459,8 +1459,6 @@ static I32 SystemControlInitServer(int *ClientSocket, int *ServerHandle, struct util_error("Failed to establish connection"); bytesReceived = iCommRecv(&iCommand, pcRecvBuffer, SC_RECV_MESSAGE_BUFFER, NULL); - - } while (*ClientSocket == -1); LogMessage(LOG_LEVEL_INFO, "Connection established: %s:%i", inet_ntoa(cli_addr.sin_addr), From e3383989db1efcf15bfa44ff818ebd881c283111 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 3 Mar 2020 12:19:09 +0100 Subject: [PATCH 310/523] Removed cmake step from Jfile --- Jenkinsfile | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/Jenkinsfile b/Jenkinsfile index 7e5ba320d..51a662126 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -9,8 +9,7 @@ pipeline { stage('Build') { steps { sh 'echo "Executing build steps..."' - cmake installation: 'InSearchPath' - cmakeBuild buildDir: 'build', installation: 'InSearchPath', steps: [[args: 'all', envVars: 'DESTDIR=${WORKSPACE}/artifacts', withCmake: true]] + cmakeBuild cleanBuild: true, buildDir: 'build', installation: 'InSearchPath', steps: [[args: 'all', envVars: 'DESTDIR=${WORKSPACE}/artifacts', withCmake: true]] } } stage('Run tests') { From 412db43edac5fe7b3ac237f755d045ce9dcc47a3 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 3 Mar 2020 12:21:04 +0100 Subject: [PATCH 311/523] Removed all from cmake args --- Jenkinsfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Jenkinsfile b/Jenkinsfile index 51a662126..6596d68cc 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -9,7 +9,7 @@ pipeline { stage('Build') { steps { sh 'echo "Executing build steps..."' - cmakeBuild cleanBuild: true, buildDir: 'build', installation: 'InSearchPath', steps: [[args: 'all', envVars: 'DESTDIR=${WORKSPACE}/artifacts', withCmake: true]] + cmakeBuild cleanBuild: true, buildDir: 'build', installation: 'InSearchPath', steps: [[envVars: 'DESTDIR=${WORKSPACE}/artifacts', withCmake: true]] } } stage('Run tests') { From 2f0ef086317ba7ea1726fee4de51d7b8b83178bf Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 3 Mar 2020 12:38:11 +0100 Subject: [PATCH 312/523] Specify C dialect in core CMakeLists --- core/CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index f8340e834..10cf8dced 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -1,4 +1,6 @@ cmake_minimum_required(VERSION 3.1) +set(CMAKE_C_STANDARD 11) +set(CMAKE_C_STANDARD_REQUIRED ON) project(Core LANGUAGES C) From 0f0040d0c1389e82fc681a5ed61411c2f3f040a0 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 3 Mar 2020 12:41:54 +0100 Subject: [PATCH 313/523] Forced C/C++ standard in top level cmakelists --- CMakeLists.txt | 4 ++++ util | 2 +- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 704a94d0a..496f46472 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,4 +1,8 @@ cmake_minimum_required(VERSION 3.10) +set(CMAKE_C_STANDARD 11) +set(CMAKE_C_STANDARD_REQUIRED ON) +set(CMAKE_CXX_STANDARD 11) +set(CMAKE_CXX_STANDARD_REQUIRED ON) project(Maestro) diff --git a/util b/util index 99ccf4b7e..b8747a454 160000 --- a/util +++ b/util @@ -1 +1 @@ -Subproject commit 99ccf4b7edbfeb4f675847b379c316eb42d365d6 +Subproject commit b8747a454f7c05fbd0c89b7fcf338b2b5f1e2a6d From 0eb5e17aa92e1bc18e75baa78408f8f5dd5bdfc9 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 3 Mar 2020 18:43:42 +0100 Subject: [PATCH 314/523] Updated util ref --- util | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/util b/util index b8747a454..4e4fb9b30 160000 --- a/util +++ b/util @@ -1 +1 @@ -Subproject commit b8747a454f7c05fbd0c89b7fcf338b2b5f1e2a6d +Subproject commit 4e4fb9b30012351a2e77eb697530c15c9c783be6 From 79850000248b15185d26d0108bd9da6a6caa9e2f Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 3 Mar 2020 18:45:03 +0100 Subject: [PATCH 315/523] Updated install targets with explicit header separation --- CMakeLists.txt | 2 - core/CMakeLists.txt | 77 +++++++++++++++----------- modules/ScenarioControl/CMakeLists.txt | 3 + modules/Supervision/CMakeLists.txt | 3 + modules/Visualization/CMakeLists.txt | 4 ++ 5 files changed, 55 insertions(+), 34 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 496f46472..0ba8ac794 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -29,5 +29,3 @@ install(DIRECTORY DESTINATION "$ENV{HOME}/.maestro/geofence" USE_SOURCE_PERMISSI install(DIRECTORY ${CMAKE_SOURCE_DIR}/conf DESTINATION "$ENV{HOME}/.maestro" USE_SOURCE_PERMISSIONS) -# Ensure libraries are reloaded after install -install(CODE "execute_process(COMMAND ldconfig)") diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index 10cf8dced..7d02d6fe4 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -9,7 +9,7 @@ project(Core LANGUAGES C) # your system if e.g. specified to "/" set(SYSTEM_SHARED_MEMORY_PATH "/dev/shm/maestro") -set(CORE_TARGET ${CMAKE_PROJECT_NAME}) +set(CORE_TARGET ${PROJECT_NAME}) set(COREUTILS_TARGET MaestroCoreUtil) set(ISO_22133_TARGET MaestroISO22133) set(POSITIONING_TARGET MaestroPositioning) @@ -19,6 +19,9 @@ set(TIME_LIBRARY MaestroTime) set(LOGGING_LIBRARY MaestroLogging) set(MESSAGE_BUS_LIBRARY MaestroMQ) set(MATH_LIBRARY m) +get_target_property(TIME_HEADERS ${TIME_LIBRARY} INCLUDE_DIRECTORIES) +get_target_property(LOGGING_HEADERS ${LOGGING_LIBRARY} INCLUDE_DIRECTORIES) +get_target_property(MESSAGE_BUS_HEADERS ${MESSAGE_BUS_LIBRARY} INCLUDE_DIRECTORIES) include(GNUInstallDirs) @@ -33,34 +36,54 @@ add_executable(${CORE_TARGET} add_library(${COREUTILS_TARGET} SHARED ${CMAKE_CURRENT_SOURCE_DIR}/src/util.c ) -set_target_properties(${COREUTILS_TARGET} PROPERTIES PUBLIC_HEADER - ${CMAKE_CURRENT_SOURCE_DIR}/inc/util.h +target_include_directories(${COREUTILS_TARGET} PUBLIC + $ + $ + ${TIME_HEADERS} + ${LOGGING_HEADERS} + ${MESSAGE_BUS_HEADERS} +) +set_target_properties(${COREUTILS_TARGET} PROPERTIES + PUBLIC_HEADER ${CMAKE_CURRENT_SOURCE_DIR}/inc/util.h ) -set_target_properties(${COREUTILS_TARGET} PROPERTIES POSITION_INDEPENDENT_CODE ON) add_library(${ISO_22133_TARGET} SHARED ${CMAKE_CURRENT_SOURCE_DIR}/src/iso22133.c ) -set_target_properties(${ISO_22133_TARGET} PROPERTIES PUBLIC_HEADER - ${CMAKE_CURRENT_SOURCE_DIR}/inc/iso22133.h +target_include_directories(${ISO_22133_TARGET} PUBLIC + $ + $ + ${TIME_HEADERS} + ${LOGGING_HEADERS} +) +set_target_properties(${ISO_22133_TARGET} PROPERTIES + PUBLIC_HEADER ${CMAKE_CURRENT_SOURCE_DIR}/inc/iso22133.h ) -set_target_properties(${ISO_22133_TARGET} PROPERTIES POSITION_INDEPENDENT_CODE ON) add_library(${POSITIONING_TARGET} SHARED ${CMAKE_CURRENT_SOURCE_DIR}/src/positioning.c ) -set_target_properties(${POSITIONING_TARGET} PROPERTIES PUBLIC_HEADER - ${CMAKE_CURRENT_SOURCE_DIR}/inc/positioning.h +target_include_directories(${POSITIONING_TARGET} PUBLIC + $ + $ + ${TIME_HEADERS} +) +set_target_properties(${POSITIONING_TARGET} PROPERTIES + PUBLIC_HEADER ${CMAKE_CURRENT_SOURCE_DIR}/inc/positioning.h ) -set_target_properties(${POSITIONING_TARGET} PROPERTIES POSITION_INDEPENDENT_CODE ON) add_library(${DATA_DICTIONARY_TARGET} SHARED ${CMAKE_CURRENT_SOURCE_DIR}/src/datadictionary.c ) -set_target_properties(${DATA_DICTIONARY_TARGET} PROPERTIES PUBLIC_HEADER - ${CMAKE_CURRENT_SOURCE_DIR}/inc/datadictionary.h +target_include_directories(${DATA_DICTIONARY_TARGET} PUBLIC + $ + $ + ${LOGGING_HEADERS} + ${MESSAGE_BUS_HEADERS} +) +set_target_properties(${DATA_DICTIONARY_TARGET} PROPERTIES + PUBLIC_HEADER ${CMAKE_CURRENT_SOURCE_DIR}/inc/datadictionary.h ) -set_target_properties(${DATA_DICTIONARY_TARGET} PROPERTIES POSITION_INDEPENDENT_CODE ON) # Linking target_link_libraries(${CORE_TARGET} LINK_PUBLIC @@ -71,28 +94,15 @@ target_link_libraries(${CORE_TARGET} LINK_PUBLIC ${ISO_22133_TARGET} ${POSITIONING_TARGET} ) -target_include_directories(${CORE_TARGET} PUBLIC - ${CMAKE_CURRENT_SOURCE_DIR}/inc -) -target_include_directories(${COREUTILS_TARGET} PUBLIC - ${CMAKE_CURRENT_SOURCE_DIR}/inc -) -target_include_directories(${DATA_DICTIONARY_TARGET} PUBLIC - ${CMAKE_CURRENT_SOURCE_DIR}/inc -) -target_include_directories(${ISO_22133_TARGET} PUBLIC - ${CMAKE_CURRENT_SOURCE_DIR}/inc -) -target_include_directories(${POSITIONING_TARGET} PUBLIC - ${CMAKE_CURRENT_SOURCE_DIR}/inc -) -target_link_libraries(${COREUTILS_TARGET} LINK_PUBLIC ${TIME_LIBRARY} ${LOGGING_LIBRARY} ${MESSAGE_BUS_LIBRARY} ${MATH_LIBRARY}) - -target_include_directories(${COREUTILS_TARGET} PUBLIC - ${CMAKE_CURRENT_SOURCE_DIR}/inc +target_link_libraries(${COREUTILS_TARGET} LINK_PUBLIC + ${TIME_LIBRARY} + ${LOGGING_LIBRARY} + ${MESSAGE_BUS_LIBRARY} + ${MATH_LIBRARY} ) + install(CODE "MESSAGE(STATUS \"Installing target ${CORE_TARGET}\")") install(TARGETS ${CORE_TARGET} ${COREUTILS_TARGET} ${ISO_22133_TARGET} ${DATA_DICTIONARY_TARGET} @@ -103,6 +113,7 @@ install(TARGETS ${CORE_TARGET} ${COREUTILS_TARGET} PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} ) +# Give built target ability to set system clock install(CODE "MESSAGE(STATUS \"Giving system time setting capabilities to target ${CORE_TARGET}\")") install(CODE "execute_process(COMMAND setcap CAP_SYS_TIME+ep ${CMAKE_INSTALL_FULL_BINDIR}/${CORE_TARGET})") @@ -111,3 +122,5 @@ install(CODE "MESSAGE(STATUS \"Creating shared memory directory ${SYSTEM_SHARED_ install(DIRECTORY DESTINATION ${SYSTEM_SHARED_MEMORY_PATH}) install(CODE "execute_process(COMMAND chown $ENV{USER} ${SYSTEM_SHARED_MEMORY_PATH})") +# Ensure linkage is reloaded after install +install(CODE "execute_process(COMMAND ldconfig)") diff --git a/modules/ScenarioControl/CMakeLists.txt b/modules/ScenarioControl/CMakeLists.txt index 1d28a65c2..7e9013a2b 100644 --- a/modules/ScenarioControl/CMakeLists.txt +++ b/modules/ScenarioControl/CMakeLists.txt @@ -51,3 +51,6 @@ install(TARGETS ${SCENARIO_CONTROL_TARGET} ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} ) + +# Ensure linkage is reloaded after install +install(CODE "execute_process(COMMAND ldconfig)") diff --git a/modules/Supervision/CMakeLists.txt b/modules/Supervision/CMakeLists.txt index fe8cf3dee..415445660 100644 --- a/modules/Supervision/CMakeLists.txt +++ b/modules/Supervision/CMakeLists.txt @@ -45,3 +45,6 @@ install(TARGETS ${SUPERVISION_TARGET} ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} ) + +# Ensure linkage is reloaded after install +install(CODE "execute_process(COMMAND ldconfig)") diff --git a/modules/Visualization/CMakeLists.txt b/modules/Visualization/CMakeLists.txt index 3110b7bc6..227f49e97 100644 --- a/modules/Visualization/CMakeLists.txt +++ b/modules/Visualization/CMakeLists.txt @@ -36,3 +36,7 @@ install(TARGETS ${VISUALIZATION_TARGET} ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} ) + + +# Ensure linkage is reloaded after install +install(CODE "execute_process(COMMAND ldconfig)") From e5fb1fdbb199c86d468ee0f349990b2646436df1 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 3 Mar 2020 18:50:33 +0100 Subject: [PATCH 316/523] updated test script to new dir structure --- allMaestroIntegrationTests.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/allMaestroIntegrationTests.sh b/allMaestroIntegrationTests.sh index 4c880c3f8..c670c844b 100755 --- a/allMaestroIntegrationTests.sh +++ b/allMaestroIntegrationTests.sh @@ -1,6 +1,6 @@ #!/bin/bash MAESTRODIR=$(pwd) -cd ${MAESTRODIR}/server/integration-tests +cd ${MAESTRODIR}/core/integration-tests FAILURES=0 NUM_TESTS=0 From 2d428dc662959b4d41dc2d8cfaf0b9ed997ee433 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 3 Mar 2020 18:57:14 +0100 Subject: [PATCH 317/523] Updated test scripts with new executable name --- core/integration-tests/000-StartupAndKillMainExecutable.sh | 4 ++-- core/integration-tests/005-UserControlConnect.py | 2 +- core/integration-tests/100-SingleTypicalTest.py | 2 +- .../integration-tests/249-uploadRowMismatchingTrajectories.py | 2 +- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/core/integration-tests/000-StartupAndKillMainExecutable.sh b/core/integration-tests/000-StartupAndKillMainExecutable.sh index dbf04c2aa..95ea4f707 100755 --- a/core/integration-tests/000-StartupAndKillMainExecutable.sh +++ b/core/integration-tests/000-StartupAndKillMainExecutable.sh @@ -1,6 +1,6 @@ #!/bin/bash -EXECDIR=../build -EXECNAME=TEServer +EXECDIR=../build/bin +EXECNAME=Core SLEEP_TIME_START=3 SLEEP_TIME_KILL=1 RESULT=0 diff --git a/core/integration-tests/005-UserControlConnect.py b/core/integration-tests/005-UserControlConnect.py index d48382093..5997b44f3 100644 --- a/core/integration-tests/005-UserControlConnect.py +++ b/core/integration-tests/005-UserControlConnect.py @@ -6,7 +6,7 @@ if __name__ == "__main__": - S = Executable("../build/TEServer",["-m","0"]) + S = Executable("../build/bin/Core",["-m","0"]) time.sleep(0.05) M = MSCP("127.0.0.1") diff --git a/core/integration-tests/100-SingleTypicalTest.py b/core/integration-tests/100-SingleTypicalTest.py index c07b06082..769caa45b 100644 --- a/core/integration-tests/100-SingleTypicalTest.py +++ b/core/integration-tests/100-SingleTypicalTest.py @@ -29,7 +29,7 @@ def checkProgramStatus(failurePrintout): # Note: server does not close sockets properly so this fails frequently (cross fingers for now): #WaitForPortAvailable(54241,"TCP",timeout=0) - server = Executable("../build/TEServer",["-m","0"]) + server = Executable("../build/bin/Core",["-m","0"]) time.sleep(0.05) checkProgramStatus("=== Starting the server caused a problem") diff --git a/core/integration-tests/249-uploadRowMismatchingTrajectories.py b/core/integration-tests/249-uploadRowMismatchingTrajectories.py index 31c4dd9be..877624c5b 100644 --- a/core/integration-tests/249-uploadRowMismatchingTrajectories.py +++ b/core/integration-tests/249-uploadRowMismatchingTrajectories.py @@ -28,7 +28,7 @@ def checkProgramStatus(failurePrintout): try: # Note: server does not close sockets properly so this fails frequently (cross fingers for now): #WaitForPortAvailable(54241,"TCP",timeout=0) - server = Executable("../build/TEServer",["-m","0"]) + server = Executable("../build/bin/Core",["-m","0"]) time.sleep(0.05) checkProgramStatus("=== Starting the server caused a problem") From cc490b473d00798d243f51f2dfaa844df76ad325 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 3 Mar 2020 19:13:16 +0100 Subject: [PATCH 318/523] Fixed permissions on home dir maestro stuff --- CMakeLists.txt | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 0ba8ac794..075a93e2c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -22,10 +22,11 @@ add_subdirectory(modules/Visualization) # Create directory for test data in user home directory install(CODE "MESSAGE(STATUS \"Creating home directory environment under $ENV{HOME}\")") -install(DIRECTORY DESTINATION "$ENV{HOME}/.maestro" USE_SOURCE_PERMISSIONS) -install(DIRECTORY DESTINATION "$ENV{HOME}/.maestro/journal" USE_SOURCE_PERMISSIONS) -install(DIRECTORY DESTINATION "$ENV{HOME}/.maestro/traj" USE_SOURCE_PERMISSIONS) -install(DIRECTORY DESTINATION "$ENV{HOME}/.maestro/geofence" USE_SOURCE_PERMISSIONS) -install(DIRECTORY ${CMAKE_SOURCE_DIR}/conf - DESTINATION "$ENV{HOME}/.maestro" USE_SOURCE_PERMISSIONS) +install(DIRECTORY DESTINATION "$ENV{HOME}/.maestro") +install(DIRECTORY DESTINATION "$ENV{HOME}/.maestro/journal") +install(DIRECTORY DESTINATION "$ENV{HOME}/.maestro/traj") +install(DIRECTORY DESTINATION "$ENV{HOME}/.maestro/geofence") +install(DIRECTORY ${CMAKE_SOURCE_DIR}/conf DESTINATION "$ENV{HOME}/.maestro") + +install(CODE "execute_process(COMMAND chown -R $ENV{USER} $ENV{HOME}/.maestro)") From a2baee86ed822b54a28d731c922e185b8e3d0cec Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 3 Mar 2020 19:13:29 +0100 Subject: [PATCH 319/523] Fixed path to executable in tests --- core/integration-tests/000-StartupAndKillMainExecutable.sh | 2 +- core/integration-tests/005-UserControlConnect.py | 2 +- core/integration-tests/100-SingleTypicalTest.py | 2 +- core/integration-tests/249-uploadRowMismatchingTrajectories.py | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/core/integration-tests/000-StartupAndKillMainExecutable.sh b/core/integration-tests/000-StartupAndKillMainExecutable.sh index 95ea4f707..e5ec1a751 100755 --- a/core/integration-tests/000-StartupAndKillMainExecutable.sh +++ b/core/integration-tests/000-StartupAndKillMainExecutable.sh @@ -1,5 +1,5 @@ #!/bin/bash -EXECDIR=../build/bin +EXECDIR=../../build/bin EXECNAME=Core SLEEP_TIME_START=3 SLEEP_TIME_KILL=1 diff --git a/core/integration-tests/005-UserControlConnect.py b/core/integration-tests/005-UserControlConnect.py index 5997b44f3..266653c1a 100644 --- a/core/integration-tests/005-UserControlConnect.py +++ b/core/integration-tests/005-UserControlConnect.py @@ -6,7 +6,7 @@ if __name__ == "__main__": - S = Executable("../build/bin/Core",["-m","0"]) + S = Executable("../../build/bin/Core",["-m","0"]) time.sleep(0.05) M = MSCP("127.0.0.1") diff --git a/core/integration-tests/100-SingleTypicalTest.py b/core/integration-tests/100-SingleTypicalTest.py index 769caa45b..2eac32d49 100644 --- a/core/integration-tests/100-SingleTypicalTest.py +++ b/core/integration-tests/100-SingleTypicalTest.py @@ -29,7 +29,7 @@ def checkProgramStatus(failurePrintout): # Note: server does not close sockets properly so this fails frequently (cross fingers for now): #WaitForPortAvailable(54241,"TCP",timeout=0) - server = Executable("../build/bin/Core",["-m","0"]) + server = Executable("../../build/bin/Core",["-m","0"]) time.sleep(0.05) checkProgramStatus("=== Starting the server caused a problem") diff --git a/core/integration-tests/249-uploadRowMismatchingTrajectories.py b/core/integration-tests/249-uploadRowMismatchingTrajectories.py index 877624c5b..939d8ea2d 100644 --- a/core/integration-tests/249-uploadRowMismatchingTrajectories.py +++ b/core/integration-tests/249-uploadRowMismatchingTrajectories.py @@ -28,7 +28,7 @@ def checkProgramStatus(failurePrintout): try: # Note: server does not close sockets properly so this fails frequently (cross fingers for now): #WaitForPortAvailable(54241,"TCP",timeout=0) - server = Executable("../build/bin/Core",["-m","0"]) + server = Executable("../../build/bin/Core",["-m","0"]) time.sleep(0.05) checkProgramStatus("=== Starting the server caused a problem") From 1df9c1dd86ca022183ef70b62363c89696329548 Mon Sep 17 00:00:00 2001 From: Viktor Johansson Date: Wed, 4 Mar 2020 11:04:37 +0100 Subject: [PATCH 320/523] Added new clear trajectory command. Can now be accessed through GUC via calling ClearTrajectories(); --- server/inc/util.h | 2 ++ server/src/systemcontrol.c | 34 +++++++++++++++++++++++++---- server/src/util.c | 44 +++++++++++++++++++++++++++++++++++--- 3 files changed, 73 insertions(+), 7 deletions(-) diff --git a/server/inc/util.h b/server/inc/util.h index 8bdec3905..590ad1667 100644 --- a/server/inc/util.h +++ b/server/inc/util.h @@ -585,6 +585,8 @@ void UtilGetConfDirectoryPath(char* path, size_t pathLen); void UtilGetTrajDirectoryPath(char* path, size_t pathLen); void UtilGetGeofenceDirectoryPath(char* path, size_t pathLen); +int UtilDeleteTrajectoryFiles(); + // File parsing functions int UtilCheckTrajectoryFileFormat(const char *path, size_t pathLen); int UtilParseTrajectoryFileHeader(char *headerLine, TrajectoryFileHeader * header); diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index ff043df29..4e75f9c50 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -128,8 +128,8 @@ typedef enum { InitializeScenario_0, ConnectObject_0, DisconnectObject_0, GetServerParameterList_0, SetServerParameter_2, GetServerParameter_1, DownloadFile_1, UploadFile_3, CheckFileDirectoryExist_1, GetRootDirectoryContent_0, GetDirectoryContent_1, - DeleteFileDirectory_1, CreateDirectory_1, GetTestOrigin_0, replay_1, control_0, Exit_0, start_ext_trigg_1, - nocommand + ClearTrajectories_0, DeleteFileDirectory_1, CreateDirectory_1, GetTestOrigin_0, replay_1, control_0, Exit_0, + start_ext_trigg_1, nocommand } SystemControlCommand_t; const char *SystemControlCommandsArr[] = { @@ -138,8 +138,8 @@ const char *SystemControlCommandsArr[] = { "ConnectObject_0", "DisconnectObject_0", "GetServerParameterList_0", "SetServerParameter_2", "GetServerParameter_1", "DownloadFile_1", "UploadFile_3", "CheckFileDirectoryExist_1", "GetRootDirectoryContent_0", "GetDirectoryContent_1", - "DeleteFileDirectory_1", "CreateDirectory_1", "GetTestOrigin_0", "replay_1", "control_0", "Exit_0", - "start_ext_trigg_1" + "ClearTrajectories_0","DeleteFileDirectory_1", "CreateDirectory_1", "GetTestOrigin_0", "replay_1", "control_0", + "Exit_0", "start_ext_trigg_1" }; const char *SystemControlStatesArr[] = @@ -185,6 +185,7 @@ I32 SystemControlCheckFileDirectoryExist(C8 * ParameterName, C8 * ReturnValue, U I32 SystemControlUploadFile(C8 * Path, C8 * FileSize, C8 * PacketSize, C8 * ReturnValue, U8 Debug); I32 SystemControlReceiveRxData(I32 * sockfd, C8 * Path, C8 * FileSize, C8 * PacketSize, C8 * ReturnValue, U8 Debug); +I32 SystemControlClearTrajectories(C8 * ReturnValue, U8 Debug); I32 SystemControlDeleteFileDirectory(C8 * Path, C8 * ReturnValue, U8 Debug); I32 SystemControlBuildFileContentInfo(C8 * Path, U8 Debug); I32 SystemControlDestroyFileContentInfo(C8 * path); @@ -761,6 +762,15 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { SystemControlCommand = Idle_0; } break; + case ClearTrajectories_0: + if (CurrentInputArgCount == CommandArgCount) { + SystemControlCommand = Idle_0; + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlClearTrajectories(ControlResponseBuffer,0); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "ClearTrajectories:", + ControlResponseBuffer, 1, &ClientSocket, 0); + } + break; case DownloadFile_1: if (CurrentInputArgCount == CommandArgCount) { SystemControlCommand = Idle_0; @@ -2053,6 +2063,22 @@ I32 SystemControlCheckFileDirectoryExist(C8 * ParameterName, C8 * ReturnValue, U return 0; } +/*! + * \brief SystemControlClearTrajectories Clears the trajectory folder on the machine + * \param ReturnValue The status from the delete procedure is stored in the pointer location. + * \param Debug Used for debugging, at the moment there is no such information. + * \return Always 0 + */ +I32 SystemControlClearTrajectories(C8 * ReturnValue, U8 Debug) +{ + C8 returnResponse = SUCCEDED_DELETE; + int response; + if (UtilDeleteTrajectoryFiles() != 0) { + returnResponse = FAILED_DELETE; + } + *ReturnValue = returnResponse; + return 0; +} I32 SystemControlDeleteFileDirectory(C8 * Path, C8 * ReturnValue, U8 Debug) { diff --git a/server/src/util.c b/server/src/util.c index 59409768b..acb4768dc 100644 --- a/server/src/util.c +++ b/server/src/util.c @@ -113,7 +113,7 @@ static void CopyHTTPHeaderField(char *request, char *targetContainer, size_t tar const char *fieldName); static char rayFromPointIntersectsLine(double pointX, double pointY, double polyPointAX, double polyPointAY, double polyPointBX, double polyPointBY); - +static int deleteDirectoryContents(char* path, size_t pathLen); void CopyHTTPHeaderField(char *request, char *targetContainer, size_t targetContainerSize, const char *fieldName) { @@ -176,8 +176,35 @@ void CopyHTTPHeaderField(char *request, char *targetContainer, size_t targetCont } } - - +/*! + * \brief deleteDirectoryContents Deletes the directory given in the parameter ::path + * \param path The path to the directory on the machine. + * \param pathLen The length of ::the path string. + * \return 0 if it could successfully delete file, non-zero if it could not. + */ +int deleteDirectoryContents(char* path, size_t pathLen) { + if (pathLen > MAX_FILE_PATH) { + LogMessage(LOG_LEVEL_ERROR, "Path variable too large to handle"); + return -1; + } + // These are data types defined in the "dirent" header + DIR *theFolder = opendir(path); + if (theFolder == NULL) { + LogMessage(LOG_LEVEL_ERROR, "Path: %s could not be opened",path); + return -2; + } + struct dirent *next_file; + char filepath[MAX_FILE_PATH]; + + while ( (next_file = readdir(theFolder)) != NULL ) + { + // build the path for each file in the folder + sprintf(filepath, "%s/%s", path, next_file->d_name); + remove(filepath); + } + closedir(theFolder); + return 0; +} /*---------------------------------------------s--------------- -- Public functions ------------------------------------------------------------*/ @@ -2347,6 +2374,17 @@ void UtilGetGeofenceDirectoryPath(char *path, size_t pathLen) { strcat(path, "/"); } +/*! + * \brief UtilDeleteTrajectoryFiles finds the trajectory folder and deletes its contents + * \return returns 0 if succesfull if the trajectory folder now is empty. Non-zero values otherwise. + */ +int UtilDeleteTrajectoryFiles() { + char filePath[MAX_FILE_PATH] = {'\0'}; + UtilGetTrajDirectoryPath(filePath,MAX_FILE_PATH); + if (filePath[0] =='\0') return -1; + return deleteDirectoryContents(filePath,MAX_FILE_PATH); +} + /*! * \brief UtilParseTrajectoryFileHeader Attempts to parse a line into a trajectory header * \param line Line to be parsed From 9fcad9ad9db37f2a9b38bedeb777fa0d1d91b764 Mon Sep 17 00:00:00 2001 From: Viktor Johansson Date: Wed, 4 Mar 2020 11:07:45 +0100 Subject: [PATCH 321/523] Applied formating --- server/src/systemcontrol.c | 46 +++++++++++++++--------------- server/src/util.c | 57 ++++++++++++++++++++------------------ 2 files changed, 54 insertions(+), 49 deletions(-) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index 4e75f9c50..007525ae7 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -128,8 +128,9 @@ typedef enum { InitializeScenario_0, ConnectObject_0, DisconnectObject_0, GetServerParameterList_0, SetServerParameter_2, GetServerParameter_1, DownloadFile_1, UploadFile_3, CheckFileDirectoryExist_1, GetRootDirectoryContent_0, GetDirectoryContent_1, - ClearTrajectories_0, DeleteFileDirectory_1, CreateDirectory_1, GetTestOrigin_0, replay_1, control_0, Exit_0, - start_ext_trigg_1, nocommand + ClearTrajectories_0, DeleteFileDirectory_1, CreateDirectory_1, GetTestOrigin_0, replay_1, control_0, + Exit_0, + start_ext_trigg_1, nocommand } SystemControlCommand_t; const char *SystemControlCommandsArr[] = { @@ -138,8 +139,9 @@ const char *SystemControlCommandsArr[] = { "ConnectObject_0", "DisconnectObject_0", "GetServerParameterList_0", "SetServerParameter_2", "GetServerParameter_1", "DownloadFile_1", "UploadFile_3", "CheckFileDirectoryExist_1", "GetRootDirectoryContent_0", "GetDirectoryContent_1", - "ClearTrajectories_0","DeleteFileDirectory_1", "CreateDirectory_1", "GetTestOrigin_0", "replay_1", "control_0", - "Exit_0", "start_ext_trigg_1" + "ClearTrajectories_0", "DeleteFileDirectory_1", "CreateDirectory_1", "GetTestOrigin_0", "replay_1", + "control_0", + "Exit_0", "start_ext_trigg_1" }; const char *SystemControlStatesArr[] = @@ -762,15 +764,15 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { SystemControlCommand = Idle_0; } break; - case ClearTrajectories_0: - if (CurrentInputArgCount == CommandArgCount) { - SystemControlCommand = Idle_0; - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlClearTrajectories(ControlResponseBuffer,0); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "ClearTrajectories:", - ControlResponseBuffer, 1, &ClientSocket, 0); - } - break; + case ClearTrajectories_0: + if (CurrentInputArgCount == CommandArgCount) { + SystemControlCommand = Idle_0; + bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + SystemControlClearTrajectories(ControlResponseBuffer, 0); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "ClearTrajectories:", + ControlResponseBuffer, 1, &ClientSocket, 0); + } + break; case DownloadFile_1: if (CurrentInputArgCount == CommandArgCount) { SystemControlCommand = Idle_0; @@ -2069,15 +2071,15 @@ I32 SystemControlCheckFileDirectoryExist(C8 * ParameterName, C8 * ReturnValue, U * \param Debug Used for debugging, at the moment there is no such information. * \return Always 0 */ -I32 SystemControlClearTrajectories(C8 * ReturnValue, U8 Debug) -{ - C8 returnResponse = SUCCEDED_DELETE; - int response; - if (UtilDeleteTrajectoryFiles() != 0) { - returnResponse = FAILED_DELETE; - } - *ReturnValue = returnResponse; - return 0; +I32 SystemControlClearTrajectories(C8 * ReturnValue, U8 Debug) { + C8 returnResponse = SUCCEDED_DELETE; + int response; + + if (UtilDeleteTrajectoryFiles() != 0) { + returnResponse = FAILED_DELETE; + } + *ReturnValue = returnResponse; + return 0; } I32 SystemControlDeleteFileDirectory(C8 * Path, C8 * ReturnValue, U8 Debug) { diff --git a/server/src/util.c b/server/src/util.c index acb4768dc..4c926b3a4 100644 --- a/server/src/util.c +++ b/server/src/util.c @@ -113,7 +113,7 @@ static void CopyHTTPHeaderField(char *request, char *targetContainer, size_t tar const char *fieldName); static char rayFromPointIntersectsLine(double pointX, double pointY, double polyPointAX, double polyPointAY, double polyPointBX, double polyPointBY); -static int deleteDirectoryContents(char* path, size_t pathLen); +static int deleteDirectoryContents(char *path, size_t pathLen); void CopyHTTPHeaderField(char *request, char *targetContainer, size_t targetContainerSize, const char *fieldName) { @@ -176,35 +176,37 @@ void CopyHTTPHeaderField(char *request, char *targetContainer, size_t targetCont } } + /*! * \brief deleteDirectoryContents Deletes the directory given in the parameter ::path * \param path The path to the directory on the machine. * \param pathLen The length of ::the path string. * \return 0 if it could successfully delete file, non-zero if it could not. */ -int deleteDirectoryContents(char* path, size_t pathLen) { - if (pathLen > MAX_FILE_PATH) { - LogMessage(LOG_LEVEL_ERROR, "Path variable too large to handle"); - return -1; - } - // These are data types defined in the "dirent" header - DIR *theFolder = opendir(path); - if (theFolder == NULL) { - LogMessage(LOG_LEVEL_ERROR, "Path: %s could not be opened",path); - return -2; - } - struct dirent *next_file; - char filepath[MAX_FILE_PATH]; - - while ( (next_file = readdir(theFolder)) != NULL ) - { - // build the path for each file in the folder - sprintf(filepath, "%s/%s", path, next_file->d_name); - remove(filepath); - } - closedir(theFolder); - return 0; +int deleteDirectoryContents(char *path, size_t pathLen) { + if (pathLen > MAX_FILE_PATH) { + LogMessage(LOG_LEVEL_ERROR, "Path variable too large to handle"); + return -1; + } + // These are data types defined in the "dirent" header + DIR *theFolder = opendir(path); + + if (theFolder == NULL) { + LogMessage(LOG_LEVEL_ERROR, "Path: %s could not be opened", path); + return -2; + } + struct dirent *next_file; + char filepath[MAX_FILE_PATH]; + + while ((next_file = readdir(theFolder)) != NULL) { + // build the path for each file in the folder + sprintf(filepath, "%s/%s", path, next_file->d_name); + remove(filepath); + } + closedir(theFolder); + return 0; } + /*---------------------------------------------s--------------- -- Public functions ------------------------------------------------------------*/ @@ -2379,10 +2381,11 @@ void UtilGetGeofenceDirectoryPath(char *path, size_t pathLen) { * \return returns 0 if succesfull if the trajectory folder now is empty. Non-zero values otherwise. */ int UtilDeleteTrajectoryFiles() { - char filePath[MAX_FILE_PATH] = {'\0'}; - UtilGetTrajDirectoryPath(filePath,MAX_FILE_PATH); - if (filePath[0] =='\0') return -1; - return deleteDirectoryContents(filePath,MAX_FILE_PATH); + char filePath[MAX_FILE_PATH] = { '\0' }; + UtilGetTrajDirectoryPath(filePath, MAX_FILE_PATH); + if (filePath[0] == '\0') + return -1; + return deleteDirectoryContents(filePath, MAX_FILE_PATH); } /*! From f0668640a266e0ad22d53cea159ca0ce190b1f50 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 4 Mar 2020 12:12:29 +0100 Subject: [PATCH 322/523] Updated dummy CMakeLists --- core/CMakeLists.txt | 2 +- modules/ScenarioControl/CMakeLists.txt | 1 + modules/Supervision/CMakeLists.txt | 1 + modules/Visualization/CMakeLists.txt | 1 + modules/dummy/CMakeLists.txt | 79 +++++++++----------------- 5 files changed, 30 insertions(+), 54 deletions(-) diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index 7d02d6fe4..2e7a9c501 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -102,7 +102,7 @@ target_link_libraries(${COREUTILS_TARGET} LINK_PUBLIC ${MATH_LIBRARY} ) - +# Installation rules install(CODE "MESSAGE(STATUS \"Installing target ${CORE_TARGET}\")") install(TARGETS ${CORE_TARGET} ${COREUTILS_TARGET} ${ISO_22133_TARGET} ${DATA_DICTIONARY_TARGET} diff --git a/modules/ScenarioControl/CMakeLists.txt b/modules/ScenarioControl/CMakeLists.txt index 7e9013a2b..51b9cf51a 100644 --- a/modules/ScenarioControl/CMakeLists.txt +++ b/modules/ScenarioControl/CMakeLists.txt @@ -44,6 +44,7 @@ target_include_directories(${SCENARIO_CONTROL_TARGET} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/inc ) +# Installation rules install(CODE "MESSAGE(STATUS \"Installing target ${SCENARIO_CONTROL_TARGET}\")") install(TARGETS ${SCENARIO_CONTROL_TARGET} RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} diff --git a/modules/Supervision/CMakeLists.txt b/modules/Supervision/CMakeLists.txt index 415445660..d5589392e 100644 --- a/modules/Supervision/CMakeLists.txt +++ b/modules/Supervision/CMakeLists.txt @@ -38,6 +38,7 @@ target_include_directories(${SUPERVISION_TARGET} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/inc ) +# Installation rules install(CODE "MESSAGE(STATUS \"Installing target ${SUPERVISION_TARGET}\")") install(TARGETS ${SUPERVISION_TARGET} RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} diff --git a/modules/Visualization/CMakeLists.txt b/modules/Visualization/CMakeLists.txt index 227f49e97..cea98b06b 100644 --- a/modules/Visualization/CMakeLists.txt +++ b/modules/Visualization/CMakeLists.txt @@ -29,6 +29,7 @@ target_link_libraries(${VISUALIZATION_TARGET} LINK_PUBLIC ${MESSAGE_BUS_LIBRARY} ) +# Installation rules install(CODE "MESSAGE(STATUS \"Installing target ${VISUALIZATION_TARGET}\")") install(TARGETS ${VISUALIZATION_TARGET} RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} diff --git a/modules/dummy/CMakeLists.txt b/modules/dummy/CMakeLists.txt index d6a7499bc..ca5a5125e 100644 --- a/modules/dummy/CMakeLists.txt +++ b/modules/dummy/CMakeLists.txt @@ -8,66 +8,39 @@ project(dummy) # Define target names set(DUMMY_TARGET ${PROJECT_NAME}) -set(UTIL_TARGET MaestroUtil) -set(TIME_TARGET MaestroTime) -set(LOGGING_TARGET MaestroLogging) -set(MESSAGE_BUS_TARGET MaestroMQ) -# Define paths to related files -set(CORE_ROOT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/../../core) -set(CORE_INCLUDE_PATH ${CORE_ROOT_PATH}/inc) -set(CORE_SOURCE_PATH ${CORE_ROOT_PATH}/src) -set(UTIL_ROOT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/../../util/C) -set(TIME_PATH ${UTIL_ROOT_PATH}/time) -set(LOGGING_PATH ${UTIL_ROOT_PATH}/logging) -set(MESSAGE_BUS_PATH ${UTIL_ROOT_PATH}/mqbus) +set(COREUTILS_LIBRARY MaestroCoreUtil) +set(ISO_22133_LIBRARY MaestroISO22133) +set(POSITIONING_LIBRARY MaestroPositioning) +set(TIME_LIBRARY MaestroTime) +set(LOGGING_LIBRARY MaestroLogging) +set(MESSAGE_BUS_LIBRARY MaestroMQ) + +include(GNUInstallDirs) # Create project main executable target add_executable(${DUMMY_TARGET} ${CMAKE_CURRENT_SOURCE_DIR}/src/main.cpp ) - -# If building locally, it is necessary to add relevant util targets -if(NOT TARGET ${TIME_TARGET}) - add_library(${TIME_TARGET} STATIC - ${TIME_PATH}/maestroTime.c - ) -endif() - -if(NOT TARGET ${LOGGING_TARGET}) - add_library(${LOGGING_TARGET} STATIC - ${LOGGING_PATH}/logging.c - ) -endif() - -if(NOT TARGET ${MESSAGE_BUS_TARGET}) - add_library(${MESSAGE_BUS_TARGET} STATIC - ${MESSAGE_BUS_PATH}/mqbus.c - ) - target_link_libraries(${MESSAGE_BUS_TARGET} rt) -endif() - -if(NOT TARGET ${UTIL_TARGET}) - add_library(${UTIL_TARGET} STATIC - ${CORE_SOURCE_PATH}/util.c - ${CORE_SOURCE_PATH}/iso22133.c - ${CORE_SOURCE_PATH}/positioning.c - ${CORE_SOURCE_PATH}/datadictionary.c - ) - target_link_libraries(${UTIL_TARGET} ${LOGGING_TARGET} ${TIME_TARGET} ${MESSAGE_BUS_TARGET} m) - target_include_directories(${UTIL_TARGET} PUBLIC - ${CORE_INCLUDE_PATH} - ${MESSAGE_BUS_PATH} - ${LOGGING_PATH} - ${TIME_PATH} - ) -endif() - # Link project executable to util libraries -target_link_libraries(${DUMMY_TARGET} LINK_PUBLIC ${TIME_TARGET} ${UTIL_TARGET} ${LOGGING_TARGET}) +target_link_libraries(${DUMMY_TARGET} LINK_PUBLIC + ${TIME_LIBRARY} + ${COREUTILS_LIBRARY} + ${LOGGING_LIBRARY} +) target_include_directories(${DUMMY_TARGET} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/inc - ${LOGGING_PATH} - ${TIME_PATH} - ${CORE_INCLUDE_PATH} ) + +# Installation rules +install(CODE "MESSAGE(STATUS \"Installing target ${DUMMY_TARGET}\")") +install(TARGETS ${DUMMY_TARGET} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} +) + +# Ensure linkage is reloaded after install +install(CODE "execute_process(COMMAND ldconfig)") + From 9d7d8f6c126d0392f2a9f3106e240c5f1dd0342b Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 4 Mar 2020 12:41:57 +0100 Subject: [PATCH 323/523] Updated test to reduce risk of race conditions --- core/integration-tests/tools/MSCP.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/core/integration-tests/tools/MSCP.py b/core/integration-tests/tools/MSCP.py index 05c3c369e..1428f7206 100644 --- a/core/integration-tests/tools/MSCP.py +++ b/core/integration-tests/tools/MSCP.py @@ -227,11 +227,11 @@ def UploadFile(self,targetPath,fileContents): self.waitForUploadReply("SERVER_PREPARED") print("=== Sending file contents") # Send file - self.Send(fileContents) - print("=== Sent file contents") self.uploadReplyLock.acquire() self.lastUploadReply["status"] = "UNKNOWN" self.uploadReplyLock.release() + self.Send(fileContents) + print("=== Sent file contents") self.waitForUploadReply("UPLOAD_SUCCESS") print("=== File upload verified") From a40675728b0186818e094a48a2cbbcd2251fcfb4 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 4 Mar 2020 17:31:21 +0100 Subject: [PATCH 324/523] Ran code formatter --- server/src/iso22133.c | 27 +++++++++++++++++---------- server/src/objectcontrol.c | 6 +++--- 2 files changed, 20 insertions(+), 13 deletions(-) diff --git a/server/src/iso22133.c b/server/src/iso22133.c index ebf1a44ed..f314e82df 100644 --- a/server/src/iso22133.c +++ b/server/src/iso22133.c @@ -850,16 +850,21 @@ ssize_t encodeTRAJMessagePoint(const struct timeval *pointTimeFromStart, const C LogMessage(LOG_LEVEL_ERROR, "Longitudinal speed is a required field in TRAJ messages"); return -1; } - TRAJData.lateralSpeed = speed.isLateralValid ? (int16_t) (speed.lateral_m_s * SPEED_ONE_METER_PER_SECOND_VALUE) : SPEED_UNAVAILABLE_VALUE; + TRAJData.lateralSpeed = + speed.isLateralValid ? (int16_t) (speed.lateral_m_s * + SPEED_ONE_METER_PER_SECOND_VALUE) : SPEED_UNAVAILABLE_VALUE; TRAJData.longitudinalAccelerationValueID = VALUE_ID_TRAJ_LONGITUDINAL_ACCELERATION; TRAJData.longitudinalAccelerationContentLength = sizeof (TRAJData.longitudinalAcceleration); TRAJData.lateralAccelerationValueID = VALUE_ID_TRAJ_LATERAL_ACCELERATION; TRAJData.lateralAccelerationContentLength = sizeof (TRAJData.lateralAcceleration); TRAJData.longitudinalAcceleration = acceleration.isLongitudinalValid ? - (int16_t) (acceleration.longitudinal_m_s2 * ACCELERATION_ONE_METER_PER_SECOND_SQUARED_VALUE) : ACCELERATION_UNAVAILABLE_VALUE; - TRAJData.lateralAcceleration = acceleration.isLateralValid ? - (int16_t) (acceleration.lateral_m_s2 * ACCELERATION_ONE_METER_PER_SECOND_SQUARED_VALUE) : ACCELERATION_UNAVAILABLE_VALUE; + (int16_t) (acceleration.longitudinal_m_s2 * + ACCELERATION_ONE_METER_PER_SECOND_SQUARED_VALUE) : ACCELERATION_UNAVAILABLE_VALUE; + TRAJData.lateralAcceleration = + acceleration.isLateralValid ? (int16_t) (acceleration.lateral_m_s2 * + ACCELERATION_ONE_METER_PER_SECOND_SQUARED_VALUE) : + ACCELERATION_UNAVAILABLE_VALUE; TRAJData.curvatureValueID = VALUE_ID_TRAJ_CURVATURE; TRAJData.curvatureContentLength = sizeof (TRAJData.curvature); @@ -1510,18 +1515,20 @@ void convertMONRToHostRepresentation(const MONRType * MONRData, ObjectMonitorTyp // Velocity monitorData->speed.isLongitudinalValid = MONRData->longitudinalSpeed != SPEED_UNAVAILABLE_VALUE; monitorData->speed.longitudinal_m_s = monitorData->speed.isLongitudinalValid ? - (double) (MONRData->longitudinalSpeed) / SPEED_ONE_METER_PER_SECOND_VALUE : 0; + (double)(MONRData->longitudinalSpeed) / SPEED_ONE_METER_PER_SECOND_VALUE : 0; monitorData->speed.isLateralValid = MONRData->lateralSpeed != SPEED_UNAVAILABLE_VALUE; monitorData->speed.lateral_m_s = monitorData->speed.isLateralValid ? - (double) (MONRData->lateralSpeed) / SPEED_ONE_METER_PER_SECOND_VALUE : 0; + (double)(MONRData->lateralSpeed) / SPEED_ONE_METER_PER_SECOND_VALUE : 0; // Acceleration - monitorData->acceleration.isLongitudinalValid = MONRData->longitudinalAcc != ACCELERATION_UNAVAILABLE_VALUE; - monitorData->acceleration.longitudinal_m_s2 = monitorData->acceleration.isLongitudinalValid ? - (double) (MONRData->longitudinalAcc) / ACCELERATION_ONE_METER_PER_SECOND_SQUARED_VALUE : 0; + monitorData->acceleration.isLongitudinalValid = + MONRData->longitudinalAcc != ACCELERATION_UNAVAILABLE_VALUE; + monitorData->acceleration.longitudinal_m_s2 = + monitorData->acceleration.isLongitudinalValid ? (double)(MONRData->longitudinalAcc) / + ACCELERATION_ONE_METER_PER_SECOND_SQUARED_VALUE : 0; monitorData->acceleration.isLateralValid = MONRData->lateralAcc != ACCELERATION_UNAVAILABLE_VALUE; monitorData->acceleration.lateral_m_s2 = monitorData->acceleration.isLateralValid ? - (double) (MONRData->lateralAcc) / ACCELERATION_ONE_METER_PER_SECOND_SQUARED_VALUE : 0; + (double)(MONRData->lateralAcc) / ACCELERATION_ONE_METER_PER_SECOND_SQUARED_VALUE : 0; // Drive direction switch (MONRData->driveDirection) { diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c index 29e11bc92..3bd2c1904 100644 --- a/server/src/objectcontrol.c +++ b/server/src/objectcontrol.c @@ -781,8 +781,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { objectControlServerStatus = CONTROL_CENTER_STATUS_ABORT; } LogPrint("OSEM msglen: %ld", MessageLength); - UtilSendTCPData(MODULE_NAME, MessageBuffer, MessageLength, &socket_fds[iIndex], - 0); + UtilSendTCPData(MODULE_NAME, MessageBuffer, MessageLength, &socket_fds[iIndex], 0); /* Here we send TRAJ, if the IP-address is not operating with a dynamic trajectory */ if (strstr(DTMReceivers, object_address_name[iIndex]) == NULL) { @@ -1068,7 +1067,8 @@ ssize_t ObjectControlSendTRAJMessage(const char *Filename, int *Socket, const ch speed.lateral_m_s = fileLine.lateralVelocity != NULL ? *fileLine.lateralVelocity : 0; acceleration.isLongitudinalValid = fileLine.longitudinalAcceleration != NULL; acceleration.isLateralValid = fileLine.lateralAcceleration != NULL; - acceleration.longitudinal_m_s2 = fileLine.longitudinalAcceleration != NULL ? *fileLine.longitudinalAcceleration : 0; + acceleration.longitudinal_m_s2 = + fileLine.longitudinalAcceleration != NULL ? *fileLine.longitudinalAcceleration : 0; acceleration.lateral_m_s2 = fileLine.lateralAcceleration != NULL ? *fileLine.lateralAcceleration : 0; From 2fafe98a5e221acbaf89e63181a1466dd4fd94ea Mon Sep 17 00:00:00 2001 From: Sebastian Loh Lindholm Date: Thu, 5 Mar 2020 15:33:33 +0100 Subject: [PATCH 325/523] Adding filetype parameter in SystemControlUploadFile --- server/inc/util.h | 12 +++++++++++- server/src/systemcontrol.c | 22 +++++++++++++--------- 2 files changed, 24 insertions(+), 10 deletions(-) diff --git a/server/inc/util.h b/server/inc/util.h index 8bdec3905..1d68ee125 100644 --- a/server/inc/util.h +++ b/server/inc/util.h @@ -75,7 +75,7 @@ extern "C"{ #define MAX_ADAPTIVE_SYNC_POINTS 512 #define USE_LOCAL_USER_CONTROL 0 -#define LOCAL_USER_CONTROL_IP "10.168.212.5" +#define LOCAL_USER_CONTROL_IP "192.168.0.7" #define USE_TEST_HOST 0 #define TESTHOST_IP LOCAL_USER_CONTROL_IP #define TESTSERVER_IP LOCAL_USER_CONTROL_IP @@ -167,6 +167,16 @@ extern "C"{ #define GetCurrentDir getcwd #define MAX_PATH_LENGTH 255 +#define MAESTRO_GENERIC_FILE_TYPE 1 +#define MAESTRO_TRAJ_FILE_TYPE 2 +#define MAESTRO_CONF_FILE_TYPE 3 +#define MAESTRO_GEOFENCE_FILE_TYPE 4 + +#define MAESTRO_GENERIC_FILE_FOLDER "~/.maestro/" +#define MAESTRO_TRAJ_FILE_FOLDER "~/.maestro/traj/" +#define MAESTRO_CONF_FILE_FOLDER "~/.maestro/conf/" +#define MAESTRO_GEOFENCE_FILE_FOLDER "~/.maestro/geofence/" + #define DD_CONTROL_BUFFER_SIZE_1024 1024 #define DD_CONTROL_BUFFER_SIZE_20 20 #define DD_CONTROL_BUFFER_SIZE_52 52 diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index ff043df29..f6e272aa9 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -127,7 +127,7 @@ typedef enum { Idle_0, GetServerStatus_0, ArmScenario_0, DisarmScenario_0, StartScenario_1, stop_0, AbortScenario_0, InitializeScenario_0, ConnectObject_0, DisconnectObject_0, GetServerParameterList_0, SetServerParameter_2, GetServerParameter_1, - DownloadFile_1, UploadFile_3, CheckFileDirectoryExist_1, GetRootDirectoryContent_0, GetDirectoryContent_1, + DownloadFile_1, UploadFile_4, CheckFileDirectoryExist_1, GetRootDirectoryContent_0, GetDirectoryContent_1, DeleteFileDirectory_1, CreateDirectory_1, GetTestOrigin_0, replay_1, control_0, Exit_0, start_ext_trigg_1, nocommand } SystemControlCommand_t; @@ -136,7 +136,7 @@ const char *SystemControlCommandsArr[] = { "Idle_0", "GetServerStatus_0", "ArmScenario_0", "DisarmScenario_0", "StartScenario_1", "stop_0", "AbortScenario_0", "InitializeScenario_0", "ConnectObject_0", "DisconnectObject_0", "GetServerParameterList_0", "SetServerParameter_2", - "GetServerParameter_1", "DownloadFile_1", "UploadFile_3", "CheckFileDirectoryExist_1", + "GetServerParameter_1", "DownloadFile_1", "UploadFile_4", "CheckFileDirectoryExist_1", "GetRootDirectoryContent_0", "GetDirectoryContent_1", "DeleteFileDirectory_1", "CreateDirectory_1", "GetTestOrigin_0", "replay_1", "control_0", "Exit_0", "start_ext_trigg_1" @@ -182,7 +182,7 @@ I32 SystemControlReadServerParameter(C8 * ParameterName, C8 * ReturnValue, U8 De I32 SystemControlWriteServerParameter(C8 * ParameterName, C8 * NewValue, U8 Debug); I32 SystemControlSetServerParameter(GSDType * GSD, C8 * ParameterName, C8 * NewValue, U8 Debug); I32 SystemControlCheckFileDirectoryExist(C8 * ParameterName, C8 * ReturnValue, U8 Debug); -I32 SystemControlUploadFile(C8 * Path, C8 * FileSize, C8 * PacketSize, C8 * ReturnValue, U8 Debug); +I32 SystemControlUploadFile(C8 * Filename, C8 * FileSize, C8 * PacketSize, C8 * FileType, C8 * ReturnValue, U8 Debug); I32 SystemControlReceiveRxData(I32 * sockfd, C8 * Path, C8 * FileSize, C8 * PacketSize, C8 * ReturnValue, U8 Debug); I32 SystemControlDeleteFileDirectory(C8 * Path, C8 * ReturnValue, U8 Debug); @@ -785,12 +785,12 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { SystemControlCommand = Idle_0; } break; - case UploadFile_3: + case UploadFile_4: if (CurrentInputArgCount == CommandArgCount) { SystemControlCommand = Idle_0; bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); SystemControlUploadFile(SystemControlArgument[0], SystemControlArgument[1], - SystemControlArgument[2], ControlResponseBuffer, 0); + SystemControlArgument[2], SystemControlArgument[3], ControlResponseBuffer, 1); SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "UploadFile:", ControlResponseBuffer, 1, &ClientSocket, 0); LogMessage(LOG_LEVEL_DEBUG, "UploadFile filelength: %s", SystemControlArgument[1]); @@ -2147,18 +2147,22 @@ I32 SystemControlCreateDirectory(C8 * Path, C8 * ReturnValue, U8 Debug) { -I32 SystemControlUploadFile(C8 * Path, C8 * FileSize, C8 * PacketSize, C8 * ReturnValue, U8 Debug) { +I32 SystemControlUploadFile(C8 * Filename, C8 * FileSize, C8 * PacketSize, C8 * FileType, C8 * ReturnValue, U8 Debug) { FILE *fd; C8 CompletePath[MAX_FILE_PATH]; bzero(CompletePath, MAX_FILE_PATH); - UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); - strcat(CompletePath, Path); + //UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); + + if(atoi(FileType) == MAESTRO_GENERIC_FILE_TYPE) { strcat(CompletePath, MAESTRO_GENERIC_FILE_FOLDER); strcat(CompletePath, Filename);} + else if(atoi(FileType) == MAESTRO_TRAJ_FILE_TYPE) { strcat(CompletePath, MAESTRO_TRAJ_FILE_FOLDER); strcat(CompletePath, Filename); } + else if(atoi(FileType) == MAESTRO_CONF_FILE_TYPE) { strcat(CompletePath, MAESTRO_CONF_FILE_FOLDER); strcat(CompletePath, Filename); } + else if(atoi(FileType) == MAESTRO_GEOFENCE_FILE_TYPE) { strcat(CompletePath, MAESTRO_GEOFENCE_FILE_FOLDER); strcat(CompletePath, Filename); } if (Debug) { LogMessage(LOG_LEVEL_DEBUG, "Upload file:"); - LogMessage(LOG_LEVEL_DEBUG, "%s", Path); + LogMessage(LOG_LEVEL_DEBUG, "%s", Filename); LogMessage(LOG_LEVEL_DEBUG, "%s", FileSize); LogMessage(LOG_LEVEL_DEBUG, "%s", PacketSize); LogMessage(LOG_LEVEL_DEBUG, "%s", CompletePath); From da7d20e35e9f0ffec55b8139a92fd8e96c66a2b4 Mon Sep 17 00:00:00 2001 From: Sebastian Loh Lindholm Date: Thu, 5 Mar 2020 17:17:09 +0100 Subject: [PATCH 326/523] Upload file updated. --- server/inc/util.h | 9 +-- server/src/systemcontrol.c | 130 +++++++++++++++++++++++-------------- 2 files changed, 85 insertions(+), 54 deletions(-) diff --git a/server/inc/util.h b/server/inc/util.h index 1d68ee125..5bd2e6aac 100644 --- a/server/inc/util.h +++ b/server/inc/util.h @@ -30,6 +30,7 @@ extern "C"{ #include #include #include +#include #include "mqbus.h" #include "iso22133.h" #include "logging.h" @@ -172,10 +173,10 @@ extern "C"{ #define MAESTRO_CONF_FILE_TYPE 3 #define MAESTRO_GEOFENCE_FILE_TYPE 4 -#define MAESTRO_GENERIC_FILE_FOLDER "~/.maestro/" -#define MAESTRO_TRAJ_FILE_FOLDER "~/.maestro/traj/" -#define MAESTRO_CONF_FILE_FOLDER "~/.maestro/conf/" -#define MAESTRO_GEOFENCE_FILE_FOLDER "~/.maestro/geofence/" +#define MAESTRO_GENERIC_FILE_FOLDER "/.maestro/" +#define MAESTRO_TRAJ_FILE_FOLDER "/.maestro/traj/" +#define MAESTRO_CONF_FILE_FOLDER "/.maestro/conf/" +#define MAESTRO_GEOFENCE_FILE_FOLDER "/.maestro/geofence/" #define DD_CONTROL_BUFFER_SIZE_1024 1024 #define DD_CONTROL_BUFFER_SIZE_20 20 diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index f6e272aa9..8dc0f9f7b 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -2145,67 +2145,97 @@ I32 SystemControlCreateDirectory(C8 * Path, C8 * ReturnValue, U8 Debug) { } +I32 SystemControlUploadFile(C8 *Filename, C8 *FileSize, C8 *PacketSize, C8 * FileType, C8 *ReturnValue, U8 Debug) +{ + FILE *fd; + C8 CompletePath[MAX_FILE_PATH]; + bzero(CompletePath, MAX_FILE_PATH); + //GetCurrentDir(CompletePath, MAX_FILE_PATH); + //strcat(CompletePath, Filename); + + const char *homedir; -I32 SystemControlUploadFile(C8 * Filename, C8 * FileSize, C8 * PacketSize, C8 * FileType, C8 * ReturnValue, U8 Debug) { - - FILE *fd; - C8 CompletePath[MAX_FILE_PATH]; - - bzero(CompletePath, MAX_FILE_PATH); - //UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); + if((homedir = getenv("HOME")) == NULL) { + homedir = getpwuid(getuid())->pw_dir; + } - if(atoi(FileType) == MAESTRO_GENERIC_FILE_TYPE) { strcat(CompletePath, MAESTRO_GENERIC_FILE_FOLDER); strcat(CompletePath, Filename);} + strcat(CompletePath, homedir); + if(atoi(FileType) == MAESTRO_GENERIC_FILE_TYPE) { strcat(CompletePath, MAESTRO_GENERIC_FILE_FOLDER); strcat(CompletePath, Filename);} else if(atoi(FileType) == MAESTRO_TRAJ_FILE_TYPE) { strcat(CompletePath, MAESTRO_TRAJ_FILE_FOLDER); strcat(CompletePath, Filename); } else if(atoi(FileType) == MAESTRO_CONF_FILE_TYPE) { strcat(CompletePath, MAESTRO_CONF_FILE_FOLDER); strcat(CompletePath, Filename); } - else if(atoi(FileType) == MAESTRO_GEOFENCE_FILE_TYPE) { strcat(CompletePath, MAESTRO_GEOFENCE_FILE_FOLDER); strcat(CompletePath, Filename); } - - if (Debug) { - LogMessage(LOG_LEVEL_DEBUG, "Upload file:"); - LogMessage(LOG_LEVEL_DEBUG, "%s", Filename); - LogMessage(LOG_LEVEL_DEBUG, "%s", FileSize); - LogMessage(LOG_LEVEL_DEBUG, "%s", PacketSize); - LogMessage(LOG_LEVEL_DEBUG, "%s", CompletePath); - } - - if (atoi(PacketSize) > SYSTEM_CONTROL_RX_PACKET_SIZE) //Check packet size - { - *ReturnValue = SERVER_PREPARED_BIG_PACKET_SIZE; - return 0; - } - - fd = fopen(CompletePath, "r"); - if (fd != NULL) { - fclose(fd); - remove(CompletePath); //Remove file if exist - } + else if(atoi(FileType) == MAESTRO_GEOFENCE_FILE_TYPE) { strcat(CompletePath, MAESTRO_GEOFENCE_FILE_FOLDER); strcat(CompletePath, Filename);} + else + { + //ok, path invalid create temporary file + bzero(CompletePath, MAX_FILE_PATH); + GetCurrentDir(CompletePath, MAX_FILE_PATH); + strcat(CompletePath, "/file.tmp"); + fd = fopen(CompletePath, "r"); + if(fd != NULL) + { + fclose(fd); + remove(CompletePath); //Remove file if exist + } + fd = fopen(CompletePath, "w+"); //Create the temporary file + + *ReturnValue = PATH_INVALID_MISSING; + + return 0; + } - fd = fopen(CompletePath, "w+"); //Create the file - if (fd != NULL) { - *ReturnValue = SERVER_PREPARED; //Server prepared - fclose(fd); - return 0; - } - else { - //ok, path invalid create temporary file - bzero(CompletePath, MAX_FILE_PATH); - UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); - strcat(CompletePath, "file.tmp"); - fd = fopen(CompletePath, "r"); - if (fd != NULL) { - fclose(fd); - remove(CompletePath); //Remove file if exist - } - fd = fopen(CompletePath, "w+"); //Create the temporary file + if(Debug) + { + printf("Filename: %s\n", Filename); + printf("FileSize: %s\n", FileSize); + printf("PacketSize: %s\n", PacketSize); + printf("FileType: %s\n", FileType); + printf("CompletePath: %s\n", CompletePath); + } + + if(atoi(PacketSize) > SYSTEM_CONTROL_RX_PACKET_SIZE) //Check packet size + { + *ReturnValue = SERVER_PREPARED_BIG_PACKET_SIZE; + return 0; + } - *ReturnValue = PATH_INVALID_MISSING; + fd = fopen(CompletePath, "r"); + if(fd != NULL) + { + fclose(fd); + remove(CompletePath); //Remove file if exist + } - return 0; - } + fd = fopen(CompletePath, "w+"); //Create the file + if(fd != NULL) + { + *ReturnValue = SERVER_PREPARED;//Server prepared + fclose(fd); + return 0; + } + else + { + //Failed to open path create temporary file + bzero(CompletePath, MAX_FILE_PATH); + GetCurrentDir(CompletePath, MAX_FILE_PATH); + strcat(CompletePath, "/file.tmp"); + fd = fopen(CompletePath, "r"); + if(fd != NULL) + { + fclose(fd); + remove(CompletePath); //Remove file if exist + } + fd = fopen(CompletePath, "w+"); //Create the temporary file + + *ReturnValue = PATH_INVALID_MISSING; + + return 0; + } - return 0; + return 0; } + I32 SystemControlReceiveRxData(I32 * sockfd, C8 * Path, C8 * FileSize, C8 * PacketSize, C8 * ReturnValue, U8 Debug) { From bedecdc107f6d6101fdcda129cd29d508b344b11 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 5 Mar 2020 17:32:37 +0100 Subject: [PATCH 327/523] Removed execute permissions from configuration files --- conf/adaptivesync.conf | 0 conf/triggeraction.conf | 0 2 files changed, 0 insertions(+), 0 deletions(-) mode change 100755 => 100644 conf/adaptivesync.conf mode change 100755 => 100644 conf/triggeraction.conf diff --git a/conf/adaptivesync.conf b/conf/adaptivesync.conf old mode 100755 new mode 100644 diff --git a/conf/triggeraction.conf b/conf/triggeraction.conf old mode 100755 new mode 100644 From fc7f5f9f3487f0990f18dffa3c3474d3abb1bff7 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 5 Mar 2020 17:35:21 +0100 Subject: [PATCH 328/523] Added local .maestro dir to built directory --- CMakeLists.txt | 33 +++++++++++++++++++++++++-------- 1 file changed, 25 insertions(+), 8 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 075a93e2c..a454dd056 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,11 +3,15 @@ set(CMAKE_C_STANDARD 11) set(CMAKE_C_STANDARD_REQUIRED ON) set(CMAKE_CXX_STANDARD 11) set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_VERBOSE_MAKEFILE ON) +set(CMAKE_COLOR_MAKEFILE ON) project(Maestro) -set(CMAKE_VERBOSE_MAKEFILE ON) -set(CMAKE_COLOR_MAKEFILE ON) +set(MAESTRO_TEST_DIR ".maestro") +set(MAESTRO_JOURNAL_DIR "${MAESTRO_TEST_DIR}/journal") +set(MAESTRO_TRAJ_DIR "${MAESTRO_TEST_DIR}/traj") +set(MAESTRO_GEOFENCE_DIR "${MAESTRO_TEST_DIR}/geofence") # Configure structure of output set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib) @@ -20,13 +24,26 @@ add_subdirectory(modules/ScenarioControl) add_subdirectory(modules/Supervision) add_subdirectory(modules/Visualization) +# Ensure .maestro directory is created at build time in build directory +add_custom_target(configurationDirectory ALL + COMMAND ${CMAKE_COMMAND} -E copy_directory ${CMAKE_SOURCE_DIR}/conf + ${CMAKE_BINARY_DIR}/${MAESTRO_CONFIGURATION_DIR} + ) +add_custom_target(buildTimeDirectory ALL + COMMAND ${CMAKE_COMMAND} -E make_directory [ + ${CMAKE_BINARY_DIR}/${MAESTRO_JOURNAL_DIR} + ${CMAKE_BINARY_DIR}/${MAESTRO_TRAJ_DIR} + ${CMAKE_BINARY_DIR}/${MAESTRO_GEOFENCE_DIR} + ] +) + # Create directory for test data in user home directory install(CODE "MESSAGE(STATUS \"Creating home directory environment under $ENV{HOME}\")") -install(DIRECTORY DESTINATION "$ENV{HOME}/.maestro") -install(DIRECTORY DESTINATION "$ENV{HOME}/.maestro/journal") -install(DIRECTORY DESTINATION "$ENV{HOME}/.maestro/traj") -install(DIRECTORY DESTINATION "$ENV{HOME}/.maestro/geofence") -install(DIRECTORY ${CMAKE_SOURCE_DIR}/conf DESTINATION "$ENV{HOME}/.maestro") +install(DIRECTORY DESTINATION "$ENV{HOME}/${MAESTRO_TEST_DIR}") +install(DIRECTORY DESTINATION "$ENV{HOME}/${MAESTRO_JOURNAL_DIR}") +install(DIRECTORY DESTINATION "$ENV{HOME}/${MAESTRO_TRAJ_DIR}") +install(DIRECTORY DESTINATION "$ENV{HOME}/${MAESTRO_GEOFENCE_DIR}") +install(DIRECTORY ${CMAKE_SOURCE_DIR}/conf DESTINATION "$ENV{HOME}/${MAESTRO_TEST_DIR}") -install(CODE "execute_process(COMMAND chown -R $ENV{USER} $ENV{HOME}/.maestro)") +install(CODE "execute_process(COMMAND chown -R $ENV{USER} $ENV{HOME}/${MAESTRO_TEST_DIR})") From 1aadeb08228407c9c2008d5858913eeec7dbd906 Mon Sep 17 00:00:00 2001 From: viktorjo <31403385+viktorjo@users.noreply.github.com> Date: Fri, 6 Mar 2020 09:57:57 +0100 Subject: [PATCH 329/523] bzero replacement Co-Authored-By: LukasWikander --- server/src/systemcontrol.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index 007525ae7..b1fb37a1e 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -767,7 +767,7 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { case ClearTrajectories_0: if (CurrentInputArgCount == CommandArgCount) { SystemControlCommand = Idle_0; - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + memset(ControlResponseBuffer, 0, sizeof (ControlResponseBuffer)); SystemControlClearTrajectories(ControlResponseBuffer, 0); SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "ClearTrajectories:", ControlResponseBuffer, 1, &ClientSocket, 0); From 2fd3a32bd3be01f8a047c4678af1b2b800ef1916 Mon Sep 17 00:00:00 2001 From: viktorjo <31403385+viktorjo@users.noreply.github.com> Date: Fri, 6 Mar 2020 09:58:19 +0100 Subject: [PATCH 330/523] unused variable removal Co-Authored-By: LukasWikander --- server/src/systemcontrol.c | 1 - 1 file changed, 1 deletion(-) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index b1fb37a1e..134ee3c62 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -2073,7 +2073,6 @@ I32 SystemControlCheckFileDirectoryExist(C8 * ParameterName, C8 * ReturnValue, U */ I32 SystemControlClearTrajectories(C8 * ReturnValue, U8 Debug) { C8 returnResponse = SUCCEDED_DELETE; - int response; if (UtilDeleteTrajectoryFiles() != 0) { returnResponse = FAILED_DELETE; From a3526ac116b6de6240313336b071f925e4dcdbed Mon Sep 17 00:00:00 2001 From: Viktor Johansson Date: Fri, 6 Mar 2020 10:20:33 +0100 Subject: [PATCH 331/523] Made the clear trajectories function more compact. --- server/src/systemcontrol.c | 32 ++++++-------------------------- 1 file changed, 6 insertions(+), 26 deletions(-) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index 134ee3c62..d8be893e3 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -187,7 +187,7 @@ I32 SystemControlCheckFileDirectoryExist(C8 * ParameterName, C8 * ReturnValue, U I32 SystemControlUploadFile(C8 * Path, C8 * FileSize, C8 * PacketSize, C8 * ReturnValue, U8 Debug); I32 SystemControlReceiveRxData(I32 * sockfd, C8 * Path, C8 * FileSize, C8 * PacketSize, C8 * ReturnValue, U8 Debug); -I32 SystemControlClearTrajectories(C8 * ReturnValue, U8 Debug); +C8 SystemControlClearTrajectories(); I32 SystemControlDeleteFileDirectory(C8 * Path, C8 * ReturnValue, U8 Debug); I32 SystemControlBuildFileContentInfo(C8 * Path, U8 Debug); I32 SystemControlDestroyFileContentInfo(C8 * path); @@ -733,27 +733,12 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); I32 file_len = SystemControlBuildFileContentInfo("dir.info", 0); - - /* - if (file_len > 0) printf("file contred Created\n"); - char *traversingPointer = SystemControlDirectoryInfo.info_buffer; - for (int i = 0; i < file_len; i++) { - printf("0x%X\n", *traversingPointer); - traversingPointer++; - } - */ SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "SubGetDirectoryContent:", SystemControlDirectoryInfo.info_buffer, file_len, &ClientSocket, 0); SystemControlDestroyFileContentInfo("dir.info"); - /* - SystemControlBuildFileContentInfo("dir.info", ControlResponseBuffer, 0); - - SystemControlSendFileContent(&ClientSocket, "dir.info", STR_SYSTEM_CONTROL_TX_PACKET_SIZE, - ControlResponseBuffer, REMOVE_FILE, 0); - */ } } @@ -768,7 +753,7 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { if (CurrentInputArgCount == CommandArgCount) { SystemControlCommand = Idle_0; memset(ControlResponseBuffer, 0, sizeof (ControlResponseBuffer)); - SystemControlClearTrajectories(ControlResponseBuffer, 0); + *ControlResponseBuffer = SystemControlClearTrajectories(); SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "ClearTrajectories:", ControlResponseBuffer, 1, &ClientSocket, 0); } @@ -2067,18 +2052,13 @@ I32 SystemControlCheckFileDirectoryExist(C8 * ParameterName, C8 * ReturnValue, U /*! * \brief SystemControlClearTrajectories Clears the trajectory folder on the machine - * \param ReturnValue The status from the delete procedure is stored in the pointer location. - * \param Debug Used for debugging, at the moment there is no such information. - * \return Always 0 + * \return Returns ::SUCCEDED_DELETE upon successfully deleting a file, otherwise ::FAILED_DELETE. */ -I32 SystemControlClearTrajectories(C8 * ReturnValue, U8 Debug) { - C8 returnResponse = SUCCEDED_DELETE; - +C8 SystemControlClearTrajectories() { if (UtilDeleteTrajectoryFiles() != 0) { - returnResponse = FAILED_DELETE; + return FAILED_DELETE; } - *ReturnValue = returnResponse; - return 0; + return SUCCEDED_DELETE; } I32 SystemControlDeleteFileDirectory(C8 * Path, C8 * ReturnValue, U8 Debug) { From a8e3af56ebadb93adbac21a9ea8c5b58ce1e50b3 Mon Sep 17 00:00:00 2001 From: Viktor Johansson Date: Fri, 6 Mar 2020 10:32:01 +0100 Subject: [PATCH 332/523] Added errno and input additional argument check. --- server/src/util.c | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/server/src/util.c b/server/src/util.c index 4c926b3a4..00bb134b6 100644 --- a/server/src/util.c +++ b/server/src/util.c @@ -184,8 +184,14 @@ void CopyHTTPHeaderField(char *request, char *targetContainer, size_t targetCont * \return 0 if it could successfully delete file, non-zero if it could not. */ int deleteDirectoryContents(char *path, size_t pathLen) { + if (path == NULL) { + LogMessage(LOG_LEVEL_ERROR, "Path is null-pointer."); + errno = EINVAL; + return -1; + } if (pathLen > MAX_FILE_PATH) { LogMessage(LOG_LEVEL_ERROR, "Path variable too large to handle"); + errno = EINVAL; return -1; } // These are data types defined in the "dirent" header @@ -193,7 +199,8 @@ int deleteDirectoryContents(char *path, size_t pathLen) { if (theFolder == NULL) { LogMessage(LOG_LEVEL_ERROR, "Path: %s could not be opened", path); - return -2; + errno = ENOENT; + return -1; } struct dirent *next_file; char filepath[MAX_FILE_PATH]; From 79d098dab5728db4babb3337b7a9a11919fe2bb3 Mon Sep 17 00:00:00 2001 From: sepast Date: Fri, 6 Mar 2020 12:51:45 +0100 Subject: [PATCH 333/523] Update server/inc/util.h Co-Authored-By: LukasWikander --- server/inc/util.h | 4 ---- 1 file changed, 4 deletions(-) diff --git a/server/inc/util.h b/server/inc/util.h index 5bd2e6aac..e55f429aa 100644 --- a/server/inc/util.h +++ b/server/inc/util.h @@ -173,10 +173,6 @@ extern "C"{ #define MAESTRO_CONF_FILE_TYPE 3 #define MAESTRO_GEOFENCE_FILE_TYPE 4 -#define MAESTRO_GENERIC_FILE_FOLDER "/.maestro/" -#define MAESTRO_TRAJ_FILE_FOLDER "/.maestro/traj/" -#define MAESTRO_CONF_FILE_FOLDER "/.maestro/conf/" -#define MAESTRO_GEOFENCE_FILE_FOLDER "/.maestro/geofence/" #define DD_CONTROL_BUFFER_SIZE_1024 1024 #define DD_CONTROL_BUFFER_SIZE_20 20 From 2668ab494a0be352cef715f434e479eec326cc02 Mon Sep 17 00:00:00 2001 From: sepast Date: Fri, 6 Mar 2020 12:52:26 +0100 Subject: [PATCH 334/523] Update server/src/systemcontrol.c Co-Authored-By: LukasWikander --- server/src/systemcontrol.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index 8dc0f9f7b..cbe64cb6f 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -2150,7 +2150,7 @@ I32 SystemControlUploadFile(C8 *Filename, C8 *FileSize, C8 *PacketSize, C8 * Fil FILE *fd; C8 CompletePath[MAX_FILE_PATH]; - bzero(CompletePath, MAX_FILE_PATH); + memset(CompletePath, 0, sizeof (CompletePath)); //GetCurrentDir(CompletePath, MAX_FILE_PATH); //strcat(CompletePath, Filename); From 0dab626e6007f9953bb81b8868b8b25723761cec Mon Sep 17 00:00:00 2001 From: sepast Date: Fri, 6 Mar 2020 12:52:48 +0100 Subject: [PATCH 335/523] Update server/inc/util.h Co-Authored-By: LukasWikander --- server/inc/util.h | 1 - 1 file changed, 1 deletion(-) diff --git a/server/inc/util.h b/server/inc/util.h index e55f429aa..a6abd4cb9 100644 --- a/server/inc/util.h +++ b/server/inc/util.h @@ -30,7 +30,6 @@ extern "C"{ #include #include #include -#include #include "mqbus.h" #include "iso22133.h" #include "logging.h" From c290433636f09de5162dba12fba17071d93e9362 Mon Sep 17 00:00:00 2001 From: sepast Date: Fri, 6 Mar 2020 12:53:50 +0100 Subject: [PATCH 336/523] Update server/src/systemcontrol.c Co-Authored-By: LukasWikander --- server/src/systemcontrol.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index cbe64cb6f..d8b2ecd70 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -2145,8 +2145,7 @@ I32 SystemControlCreateDirectory(C8 * Path, C8 * ReturnValue, U8 Debug) { } -I32 SystemControlUploadFile(C8 *Filename, C8 *FileSize, C8 *PacketSize, C8 * FileType, C8 *ReturnValue, U8 Debug) -{ +I32 SystemControlUploadFile(C8 *Filename, C8 *FileSize, C8 *PacketSize, C8 * FileType, C8 *ReturnValue, U8 Debug) { FILE *fd; C8 CompletePath[MAX_FILE_PATH]; From 968050435d41ed1537eaf17a5e49ea4fcd859bd0 Mon Sep 17 00:00:00 2001 From: sepast Date: Fri, 6 Mar 2020 12:54:45 +0100 Subject: [PATCH 337/523] Update server/src/systemcontrol.c Co-Authored-By: LukasWikander --- server/src/systemcontrol.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index d8b2ecd70..89056bc9e 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -2185,11 +2185,11 @@ I32 SystemControlUploadFile(C8 *Filename, C8 *FileSize, C8 *PacketSize, C8 * Fil if(Debug) { - printf("Filename: %s\n", Filename); - printf("FileSize: %s\n", FileSize); - printf("PacketSize: %s\n", PacketSize); - printf("FileType: %s\n", FileType); - printf("CompletePath: %s\n", CompletePath); + LogPrint("Filename: %s\n", Filename); + LogPrint("FileSize: %s\n", FileSize); + LogPrint("PacketSize: %s\n", PacketSize); + LogPrint("FileType: %s\n", FileType); + LogPrint("CompletePath: %s\n", CompletePath); } if(atoi(PacketSize) > SYSTEM_CONTROL_RX_PACKET_SIZE) //Check packet size From 3c1327b71dacd4e143ad10d2ccb0d39819f08e59 Mon Sep 17 00:00:00 2001 From: sepast Date: Fri, 6 Mar 2020 12:55:06 +0100 Subject: [PATCH 338/523] Update server/src/systemcontrol.c Co-Authored-By: LukasWikander --- server/src/systemcontrol.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index 89056bc9e..ac834e6c6 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -790,7 +790,7 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { SystemControlCommand = Idle_0; bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); SystemControlUploadFile(SystemControlArgument[0], SystemControlArgument[1], - SystemControlArgument[2], SystemControlArgument[3], ControlResponseBuffer, 1); + SystemControlArgument[2], SystemControlArgument[3], ControlResponseBuffer, 0); SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "UploadFile:", ControlResponseBuffer, 1, &ClientSocket, 0); LogMessage(LOG_LEVEL_DEBUG, "UploadFile filelength: %s", SystemControlArgument[1]); From e0e2b5887b34033c4395a4d42cdc2a8e149b6d36 Mon Sep 17 00:00:00 2001 From: sepast Date: Fri, 6 Mar 2020 12:57:01 +0100 Subject: [PATCH 339/523] Update server/src/systemcontrol.c Co-Authored-By: LukasWikander --- server/src/systemcontrol.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index ac834e6c6..372fea1e1 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -2231,7 +2231,7 @@ I32 SystemControlUploadFile(C8 *Filename, C8 *FileSize, C8 *PacketSize, C8 * Fil return 0; } - return 0; + return -1; } From dca97485d652b3a07449e20cedf95a1bba747781 Mon Sep 17 00:00:00 2001 From: sepast Date: Fri, 6 Mar 2020 12:57:24 +0100 Subject: [PATCH 340/523] Update server/src/systemcontrol.c Co-Authored-By: LukasWikander --- server/src/systemcontrol.c | 22 ++++++++++++++++++---- 1 file changed, 18 insertions(+), 4 deletions(-) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index 372fea1e1..0d236b7e5 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -2160,10 +2160,24 @@ I32 SystemControlUploadFile(C8 *Filename, C8 *FileSize, C8 *PacketSize, C8 * Fil } strcat(CompletePath, homedir); - if(atoi(FileType) == MAESTRO_GENERIC_FILE_TYPE) { strcat(CompletePath, MAESTRO_GENERIC_FILE_FOLDER); strcat(CompletePath, Filename);} - else if(atoi(FileType) == MAESTRO_TRAJ_FILE_TYPE) { strcat(CompletePath, MAESTRO_TRAJ_FILE_FOLDER); strcat(CompletePath, Filename); } - else if(atoi(FileType) == MAESTRO_CONF_FILE_TYPE) { strcat(CompletePath, MAESTRO_CONF_FILE_FOLDER); strcat(CompletePath, Filename); } - else if(atoi(FileType) == MAESTRO_GEOFENCE_FILE_TYPE) { strcat(CompletePath, MAESTRO_GEOFENCE_FILE_FOLDER); strcat(CompletePath, Filename);} + switch (atoi(FileType)) { + case MAESTRO_GENERIC_FILE_TYPE: + UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); + break; + case MAESTRO_TRAJ_FILE_TYPE: + UtilGetTrajDirectoryPath(CompletePath, sizeof (CompletePath)); + break; + case MAESTRO_CONF_FILE_TYPE: + UtilGetConfDirectoryPath(CompletePath, sizeof (CompletePath)); + break; + case MAESTRO_GEOFENCE_FILE_TYPE: + UtilGetGeofenceDirectoryPath(CompletePath, sizeof (CompletePath)); + break; + default: + LogMessage(LOG_LEVEL_ERROR, "Received invalid file type upload request"); + return -1; + } + strcat(CompletePath, Filename); else { //ok, path invalid create temporary file From de3aa73f4b128ee32141890bef7e5b9f90dc5cc8 Mon Sep 17 00:00:00 2001 From: sepast Date: Fri, 6 Mar 2020 12:59:58 +0100 Subject: [PATCH 341/523] Update server/src/systemcontrol.c Co-Authored-By: LukasWikander --- server/src/systemcontrol.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index 0d236b7e5..335e85208 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -2197,8 +2197,7 @@ I32 SystemControlUploadFile(C8 *Filename, C8 *FileSize, C8 *PacketSize, C8 * Fil return 0; } - if(Debug) - { + if (Debug) { LogPrint("Filename: %s\n", Filename); LogPrint("FileSize: %s\n", FileSize); LogPrint("PacketSize: %s\n", PacketSize); From 5a46c9e9da90780e3d80dd0343a5d82d7016e38c Mon Sep 17 00:00:00 2001 From: sepast Date: Fri, 6 Mar 2020 13:00:19 +0100 Subject: [PATCH 342/523] Update server/src/systemcontrol.c Co-Authored-By: LukasWikander --- server/src/systemcontrol.c | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index 335e85208..fb51ff52c 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -2152,7 +2152,10 @@ I32 SystemControlUploadFile(C8 *Filename, C8 *FileSize, C8 *PacketSize, C8 * Fil memset(CompletePath, 0, sizeof (CompletePath)); //GetCurrentDir(CompletePath, MAX_FILE_PATH); //strcat(CompletePath, Filename); - + if (Filename == NULL || FileSize == NULL || PacketSize == NULL || FileType == NULL || ReturnValue == NULL) { + LogMessage(LOG_LEVEL_ERROR, "Invalid function parameter passed to upload file handler function"); + return -1; + } const char *homedir; if((homedir = getenv("HOME")) == NULL) { From fedba84c4706a43131b04eb449cd90e7f37d2fb0 Mon Sep 17 00:00:00 2001 From: sepast Date: Fri, 6 Mar 2020 13:00:43 +0100 Subject: [PATCH 343/523] Update server/src/systemcontrol.c Co-Authored-By: LukasWikander --- server/src/systemcontrol.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index fb51ff52c..339162446 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -2208,8 +2208,7 @@ I32 SystemControlUploadFile(C8 *Filename, C8 *FileSize, C8 *PacketSize, C8 * Fil LogPrint("CompletePath: %s\n", CompletePath); } - if(atoi(PacketSize) > SYSTEM_CONTROL_RX_PACKET_SIZE) //Check packet size - { + if (atoi(PacketSize) > SYSTEM_CONTROL_RX_PACKET_SIZE) { //Check packet size *ReturnValue = SERVER_PREPARED_BIG_PACKET_SIZE; return 0; } From 5836e90654b5e7e0b9c1551c663e050563b9b17b Mon Sep 17 00:00:00 2001 From: sepast Date: Fri, 6 Mar 2020 13:00:57 +0100 Subject: [PATCH 344/523] Update server/src/systemcontrol.c Co-Authored-By: LukasWikander --- server/src/systemcontrol.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index 339162446..6100b5277 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -2214,8 +2214,7 @@ I32 SystemControlUploadFile(C8 *Filename, C8 *FileSize, C8 *PacketSize, C8 * Fil } fd = fopen(CompletePath, "r"); - if(fd != NULL) - { + if (fd != NULL) { fclose(fd); remove(CompletePath); //Remove file if exist } From 7f413e0e2eea94b8fc0bb073a979ccca8d61412e Mon Sep 17 00:00:00 2001 From: sepast Date: Fri, 6 Mar 2020 13:01:24 +0100 Subject: [PATCH 345/523] Update server/src/systemcontrol.c Co-Authored-By: LukasWikander --- server/src/systemcontrol.c | 1 + 1 file changed, 1 insertion(+) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index 6100b5277..a6ece9f06 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -2217,6 +2217,7 @@ I32 SystemControlUploadFile(C8 *Filename, C8 *FileSize, C8 *PacketSize, C8 * Fil if (fd != NULL) { fclose(fd); remove(CompletePath); //Remove file if exist + LogMessage(LOG_LEVEL_INFO, "Deleted file <%s>", CompletePath); } fd = fopen(CompletePath, "w+"); //Create the file From f676842e3de46966f8044a216d69d49be576c18d Mon Sep 17 00:00:00 2001 From: Sebastian Loh Lindholm Date: Fri, 6 Mar 2020 13:29:20 +0100 Subject: [PATCH 346/523] File upload gets the directories from util functions. --- server/inc/util.h | 6 ------ server/src/systemcontrol.c | 30 +++++++++++------------------- 2 files changed, 11 insertions(+), 25 deletions(-) diff --git a/server/inc/util.h b/server/inc/util.h index a6abd4cb9..74a2c390f 100644 --- a/server/inc/util.h +++ b/server/inc/util.h @@ -167,12 +167,6 @@ extern "C"{ #define GetCurrentDir getcwd #define MAX_PATH_LENGTH 255 -#define MAESTRO_GENERIC_FILE_TYPE 1 -#define MAESTRO_TRAJ_FILE_TYPE 2 -#define MAESTRO_CONF_FILE_TYPE 3 -#define MAESTRO_GEOFENCE_FILE_TYPE 4 - - #define DD_CONTROL_BUFFER_SIZE_1024 1024 #define DD_CONTROL_BUFFER_SIZE_20 20 #define DD_CONTROL_BUFFER_SIZE_52 52 diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index a6ece9f06..39b829f8a 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -122,6 +122,11 @@ typedef struct { #define SC_SLEEP_TIME_NONEMPTY_MQ_S 0 #define SC_SLEEP_TIME_NONEMPTY_MQ_NS 0 +#define MAESTRO_GENERIC_FILE_TYPE 1 +#define MAESTRO_TRAJ_FILE_TYPE 2 +#define MAESTRO_CONF_FILE_TYPE 3 +#define MAESTRO_GEOFENCE_FILE_TYPE 4 + typedef enum { Idle_0, GetServerStatus_0, ArmScenario_0, DisarmScenario_0, StartScenario_1, stop_0, AbortScenario_0, @@ -2156,13 +2161,7 @@ I32 SystemControlUploadFile(C8 *Filename, C8 *FileSize, C8 *PacketSize, C8 * Fil LogMessage(LOG_LEVEL_ERROR, "Invalid function parameter passed to upload file handler function"); return -1; } - const char *homedir; - - if((homedir = getenv("HOME")) == NULL) { - homedir = getpwuid(getuid())->pw_dir; - } - strcat(CompletePath, homedir); switch (atoi(FileType)) { case MAESTRO_GENERIC_FILE_TYPE: UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); @@ -2178,14 +2177,8 @@ I32 SystemControlUploadFile(C8 *Filename, C8 *FileSize, C8 *PacketSize, C8 * Fil break; default: LogMessage(LOG_LEVEL_ERROR, "Received invalid file type upload request"); - return -1; - } - strcat(CompletePath, Filename); - else - { - //ok, path invalid create temporary file - bzero(CompletePath, MAX_FILE_PATH); - GetCurrentDir(CompletePath, MAX_FILE_PATH); + //Create temporary file for handling data anyway + UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); strcat(CompletePath, "/file.tmp"); fd = fopen(CompletePath, "r"); if(fd != NULL) @@ -2196,9 +2189,9 @@ I32 SystemControlUploadFile(C8 *Filename, C8 *FileSize, C8 *PacketSize, C8 * Fil fd = fopen(CompletePath, "w+"); //Create the temporary file *ReturnValue = PATH_INVALID_MISSING; - - return 0; - } + return -1; + } + strcat(CompletePath, Filename); if (Debug) { LogPrint("Filename: %s\n", Filename); @@ -2230,8 +2223,7 @@ I32 SystemControlUploadFile(C8 *Filename, C8 *FileSize, C8 *PacketSize, C8 * Fil else { //Failed to open path create temporary file - bzero(CompletePath, MAX_FILE_PATH); - GetCurrentDir(CompletePath, MAX_FILE_PATH); + UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); strcat(CompletePath, "/file.tmp"); fd = fopen(CompletePath, "r"); if(fd != NULL) From 50917fffb2fa3de34f49998890a3c1d72b96818c Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 6 Mar 2020 15:11:30 +0100 Subject: [PATCH 347/523] Ran code formatter --- server/src/systemcontrol.c | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index 5e2bb0322..3538489df 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -329,7 +329,7 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { } - while (!iExit) { + while (!iExit) { if (server_state == SERVER_STATE_ERROR) { iCommSend(COMM_ABORT, NULL, 0); continue; @@ -1438,9 +1438,9 @@ static I32 SystemControlInitServer(int *ClientSocket, int *ServerHandle, struct int result = 0; int sockFlags = 0; - enum COMMAND iCommand; - ssize_t bytesReceived = 0; - char pcRecvBuffer[SC_RECV_MESSAGE_BUFFER]; + enum COMMAND iCommand; + ssize_t bytesReceived = 0; + char pcRecvBuffer[SC_RECV_MESSAGE_BUFFER]; /* Init user control socket */ @@ -1479,7 +1479,7 @@ static I32 SystemControlInitServer(int *ClientSocket, int *ServerHandle, struct /* Set socket to nonblocking */ sockFlags = fcntl(*ServerHandle, F_GETFL, 0); - if (sockFlags == -1) + if (sockFlags == -1) util_error("Error calling fcntl"); sockFlags = sockFlags | O_NONBLOCK; @@ -1488,10 +1488,10 @@ static I32 SystemControlInitServer(int *ClientSocket, int *ServerHandle, struct do { *ClientSocket = accept(*ServerHandle, (struct sockaddr *)&cli_addr, &cli_length); - if ((*ClientSocket == -1 && errno != EAGAIN && errno != EWOULDBLOCK) || iExit) - util_error("Failed to establish connection"); + if ((*ClientSocket == -1 && errno != EAGAIN && errno != EWOULDBLOCK) || iExit) + util_error("Failed to establish connection"); - bytesReceived = iCommRecv(&iCommand, pcRecvBuffer, SC_RECV_MESSAGE_BUFFER, NULL); + bytesReceived = iCommRecv(&iCommand, pcRecvBuffer, SC_RECV_MESSAGE_BUFFER, NULL); } while (*ClientSocket == -1); LogMessage(LOG_LEVEL_INFO, "Connection established: %s:%i", inet_ntoa(cli_addr.sin_addr), From 2a10a9de921f852931e0a7d4cad166f1e35a4cd4 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 6 Mar 2020 15:23:48 +0100 Subject: [PATCH 348/523] Added function in system control for deleting geofence dir --- server/src/systemcontrol.c | 22 +++++++++++++++++----- 1 file changed, 17 insertions(+), 5 deletions(-) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index d8be893e3..82ec0b8fa 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -128,18 +128,18 @@ typedef enum { InitializeScenario_0, ConnectObject_0, DisconnectObject_0, GetServerParameterList_0, SetServerParameter_2, GetServerParameter_1, DownloadFile_1, UploadFile_3, CheckFileDirectoryExist_1, GetRootDirectoryContent_0, GetDirectoryContent_1, - ClearTrajectories_0, DeleteFileDirectory_1, CreateDirectory_1, GetTestOrigin_0, replay_1, control_0, + ClearTrajectories_0, ClearGeofences_0, DeleteFileDirectory_1, CreateDirectory_1, GetTestOrigin_0, replay_1, control_0, Exit_0, start_ext_trigg_1, nocommand } SystemControlCommand_t; -const char *SystemControlCommandsArr[] = { +static const char *SystemControlCommandsArr[] = { "Idle_0", "GetServerStatus_0", "ArmScenario_0", "DisarmScenario_0", "StartScenario_1", "stop_0", "AbortScenario_0", "InitializeScenario_0", "ConnectObject_0", "DisconnectObject_0", "GetServerParameterList_0", "SetServerParameter_2", "GetServerParameter_1", "DownloadFile_1", "UploadFile_3", "CheckFileDirectoryExist_1", "GetRootDirectoryContent_0", "GetDirectoryContent_1", - "ClearTrajectories_0", "DeleteFileDirectory_1", "CreateDirectory_1", "GetTestOrigin_0", "replay_1", + "ClearTrajectories_0", "ClearGeofences_0", "DeleteFileDirectory_1", "CreateDirectory_1", "GetTestOrigin_0", "replay_1", "control_0", "Exit_0", "start_ext_trigg_1" }; @@ -187,7 +187,8 @@ I32 SystemControlCheckFileDirectoryExist(C8 * ParameterName, C8 * ReturnValue, U I32 SystemControlUploadFile(C8 * Path, C8 * FileSize, C8 * PacketSize, C8 * ReturnValue, U8 Debug); I32 SystemControlReceiveRxData(I32 * sockfd, C8 * Path, C8 * FileSize, C8 * PacketSize, C8 * ReturnValue, U8 Debug); -C8 SystemControlClearTrajectories(); +C8 SystemControlClearTrajectories(void); +C8 SystemControlClearGeofences(void); I32 SystemControlDeleteFileDirectory(C8 * Path, C8 * ReturnValue, U8 Debug); I32 SystemControlBuildFileContentInfo(C8 * Path, U8 Debug); I32 SystemControlDestroyFileContentInfo(C8 * path); @@ -2051,7 +2052,7 @@ I32 SystemControlCheckFileDirectoryExist(C8 * ParameterName, C8 * ReturnValue, U } /*! - * \brief SystemControlClearTrajectories Clears the trajectory folder on the machine + * \brief SystemControlClearTrajectories Clears the trajectory directory on the machine * \return Returns ::SUCCEDED_DELETE upon successfully deleting a file, otherwise ::FAILED_DELETE. */ C8 SystemControlClearTrajectories() { @@ -2061,6 +2062,17 @@ C8 SystemControlClearTrajectories() { return SUCCEDED_DELETE; } +/*! + * \brief SystemControlClearGeofences Clears the geofence directory on the machine + * \return Returns ::SUCCEDED_DELETE upon successfully deleting a file, otherwise ::FAILED_DELETE. + */ +C8 SystemControlClearGeofences() { + if (UtilDeleteGeofenceFiles() != 0) { + return FAILED_DELETE; + } + return SUCCEDED_DELETE; +} + I32 SystemControlDeleteFileDirectory(C8 * Path, C8 * ReturnValue, U8 Debug) { DIR *pDir; From 337e8172c23b53f31ffc6f3e54213bae2a2d8918 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 6 Mar 2020 15:30:01 +0100 Subject: [PATCH 349/523] Corrected spelling error --- server/inc/util.h | 2 +- server/src/systemcontrol.c | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/server/inc/util.h b/server/inc/util.h index 590ad1667..23916914f 100644 --- a/server/inc/util.h +++ b/server/inc/util.h @@ -155,7 +155,7 @@ extern "C"{ #define FOLDER_EXISTED 0x02 #define SUCCEDED_CREATE_FOLDER 0x03 #define FAILED_CREATE_FOLDER 0x04 -#define SUCCEDED_DELETE 0x01 +#define SUCCEEDED_DELETE 0x01 #define FAILED_DELETE 0x02 #define FILE_TO_MUCH_DATA 0x06 diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index 82ec0b8fa..43fe77624 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -2059,7 +2059,7 @@ C8 SystemControlClearTrajectories() { if (UtilDeleteTrajectoryFiles() != 0) { return FAILED_DELETE; } - return SUCCEDED_DELETE; + return SUCCEEDED_DELETE; } /*! @@ -2070,7 +2070,7 @@ C8 SystemControlClearGeofences() { if (UtilDeleteGeofenceFiles() != 0) { return FAILED_DELETE; } - return SUCCEDED_DELETE; + return SUCCEEDED_DELETE; } I32 SystemControlDeleteFileDirectory(C8 * Path, C8 * ReturnValue, U8 Debug) { @@ -2094,7 +2094,7 @@ I32 SystemControlDeleteFileDirectory(C8 * Path, C8 * ReturnValue, U8 Debug) { else { if (0 == remove(CompletePath)) //Delete file { - *ReturnValue = SUCCEDED_DELETE; + *ReturnValue = SUCCEEDED_DELETE; } else { *ReturnValue = FAILED_DELETE; @@ -2104,14 +2104,14 @@ I32 SystemControlDeleteFileDirectory(C8 * Path, C8 * ReturnValue, U8 Debug) { else { if (0 == remove(CompletePath)) //Delete directory { - *ReturnValue = SUCCEDED_DELETE; + *ReturnValue = SUCCEEDED_DELETE; } else { *ReturnValue = FAILED_DELETE; } } - if (*ReturnValue == SUCCEDED_DELETE) + if (*ReturnValue == SUCCEEDED_DELETE) LogMessage(LOG_LEVEL_INFO, "Deleted %s", CompletePath); else if (*ReturnValue == FAILED_DELETE) LogMessage(LOG_LEVEL_INFO, "Failed to delete %s", CompletePath); From 657203799795896aeab99ef9c08d601c9e091415 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 6 Mar 2020 15:35:55 +0100 Subject: [PATCH 350/523] Implemented util function for clearing geofence dir --- server/inc/util.h | 3 ++- server/src/util.c | 18 +++++++++++++++--- 2 files changed, 17 insertions(+), 4 deletions(-) diff --git a/server/inc/util.h b/server/inc/util.h index 23916914f..d8ed5ef75 100644 --- a/server/inc/util.h +++ b/server/inc/util.h @@ -585,7 +585,8 @@ void UtilGetConfDirectoryPath(char* path, size_t pathLen); void UtilGetTrajDirectoryPath(char* path, size_t pathLen); void UtilGetGeofenceDirectoryPath(char* path, size_t pathLen); -int UtilDeleteTrajectoryFiles(); +int UtilDeleteTrajectoryFiles(void); +int UtilDeleteGeofenceFiles(void); // File parsing functions int UtilCheckTrajectoryFileFormat(const char *path, size_t pathLen); diff --git a/server/src/util.c b/server/src/util.c index 00bb134b6..e3cc1b9c7 100644 --- a/server/src/util.c +++ b/server/src/util.c @@ -2387,12 +2387,24 @@ void UtilGetGeofenceDirectoryPath(char *path, size_t pathLen) { * \brief UtilDeleteTrajectoryFiles finds the trajectory folder and deletes its contents * \return returns 0 if succesfull if the trajectory folder now is empty. Non-zero values otherwise. */ -int UtilDeleteTrajectoryFiles() { +int UtilDeleteTrajectoryFiles(void) { char filePath[MAX_FILE_PATH] = { '\0' }; - UtilGetTrajDirectoryPath(filePath, MAX_FILE_PATH); + UtilGetTrajDirectoryPath(filePath, sizeof (filePath)); if (filePath[0] == '\0') return -1; - return deleteDirectoryContents(filePath, MAX_FILE_PATH); + return deleteDirectoryContents(filePath, sizeof (filePath)); +} + +/*! + * \brief UtilDeleteGeofenceFiles finds the geofence folder and deletes its contents + * \return returns 0 if succesfull if the trajectory folder now is empty. Non-zero values otherwise. + */ +int UtilDeleteGeofenceFiles(void) { + char filePath[MAX_FILE_PATH] = { '\0' }; + UtilGetGeofenceDirectoryPath(filePath, sizeof (filePath)); + if (filePath[0] == '\0') + return -1; + return deleteDirectoryContents(filePath, sizeof (filePath)); } /*! From 8eb5227ca0d6eb2bb39e8fb4c13061a84881cd75 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 6 Mar 2020 15:36:14 +0100 Subject: [PATCH 351/523] refactoring --- server/src/systemcontrol.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index 43fe77624..fe7a8d1d4 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -2055,7 +2055,7 @@ I32 SystemControlCheckFileDirectoryExist(C8 * ParameterName, C8 * ReturnValue, U * \brief SystemControlClearTrajectories Clears the trajectory directory on the machine * \return Returns ::SUCCEDED_DELETE upon successfully deleting a file, otherwise ::FAILED_DELETE. */ -C8 SystemControlClearTrajectories() { +C8 SystemControlClearTrajectories(void) { if (UtilDeleteTrajectoryFiles() != 0) { return FAILED_DELETE; } @@ -2066,7 +2066,7 @@ C8 SystemControlClearTrajectories() { * \brief SystemControlClearGeofences Clears the geofence directory on the machine * \return Returns ::SUCCEDED_DELETE upon successfully deleting a file, otherwise ::FAILED_DELETE. */ -C8 SystemControlClearGeofences() { +C8 SystemControlClearGeofences(void) { if (UtilDeleteGeofenceFiles() != 0) { return FAILED_DELETE; } From 37c6f05834e66a2a187ec4e2645d1e2f13be38ec Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 6 Mar 2020 15:36:35 +0100 Subject: [PATCH 352/523] Ran code formatter --- server/src/systemcontrol.c | 15 +++++++++------ server/src/util.c | 16 ++++++++-------- 2 files changed, 17 insertions(+), 14 deletions(-) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index fe7a8d1d4..ff9d17b57 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -128,8 +128,9 @@ typedef enum { InitializeScenario_0, ConnectObject_0, DisconnectObject_0, GetServerParameterList_0, SetServerParameter_2, GetServerParameter_1, DownloadFile_1, UploadFile_3, CheckFileDirectoryExist_1, GetRootDirectoryContent_0, GetDirectoryContent_1, - ClearTrajectories_0, ClearGeofences_0, DeleteFileDirectory_1, CreateDirectory_1, GetTestOrigin_0, replay_1, control_0, - Exit_0, + ClearTrajectories_0, ClearGeofences_0, DeleteFileDirectory_1, CreateDirectory_1, GetTestOrigin_0, + replay_1, control_0, + Exit_0, start_ext_trigg_1, nocommand } SystemControlCommand_t; @@ -139,8 +140,9 @@ static const char *SystemControlCommandsArr[] = { "ConnectObject_0", "DisconnectObject_0", "GetServerParameterList_0", "SetServerParameter_2", "GetServerParameter_1", "DownloadFile_1", "UploadFile_3", "CheckFileDirectoryExist_1", "GetRootDirectoryContent_0", "GetDirectoryContent_1", - "ClearTrajectories_0", "ClearGeofences_0", "DeleteFileDirectory_1", "CreateDirectory_1", "GetTestOrigin_0", "replay_1", - "control_0", + "ClearTrajectories_0", "ClearGeofences_0", "DeleteFileDirectory_1", "CreateDirectory_1", + "GetTestOrigin_0", "replay_1", + "control_0", "Exit_0", "start_ext_trigg_1" }; @@ -734,6 +736,7 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); I32 file_len = SystemControlBuildFileContentInfo("dir.info", 0); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "SubGetDirectoryContent:", SystemControlDirectoryInfo.info_buffer, file_len, @@ -754,7 +757,7 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { if (CurrentInputArgCount == CommandArgCount) { SystemControlCommand = Idle_0; memset(ControlResponseBuffer, 0, sizeof (ControlResponseBuffer)); - *ControlResponseBuffer = SystemControlClearTrajectories(); + *ControlResponseBuffer = SystemControlClearTrajectories(); SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "ClearTrajectories:", ControlResponseBuffer, 1, &ClientSocket, 0); } @@ -2057,7 +2060,7 @@ I32 SystemControlCheckFileDirectoryExist(C8 * ParameterName, C8 * ReturnValue, U */ C8 SystemControlClearTrajectories(void) { if (UtilDeleteTrajectoryFiles() != 0) { - return FAILED_DELETE; + return FAILED_DELETE; } return SUCCEEDED_DELETE; } diff --git a/server/src/util.c b/server/src/util.c index e3cc1b9c7..7a7d285b3 100644 --- a/server/src/util.c +++ b/server/src/util.c @@ -184,14 +184,14 @@ void CopyHTTPHeaderField(char *request, char *targetContainer, size_t targetCont * \return 0 if it could successfully delete file, non-zero if it could not. */ int deleteDirectoryContents(char *path, size_t pathLen) { - if (path == NULL) { - LogMessage(LOG_LEVEL_ERROR, "Path is null-pointer."); - errno = EINVAL; - return -1; - } + if (path == NULL) { + LogMessage(LOG_LEVEL_ERROR, "Path is null-pointer."); + errno = EINVAL; + return -1; + } if (pathLen > MAX_FILE_PATH) { LogMessage(LOG_LEVEL_ERROR, "Path variable too large to handle"); - errno = EINVAL; + errno = EINVAL; return -1; } // These are data types defined in the "dirent" header @@ -199,8 +199,8 @@ int deleteDirectoryContents(char *path, size_t pathLen) { if (theFolder == NULL) { LogMessage(LOG_LEVEL_ERROR, "Path: %s could not be opened", path); - errno = ENOENT; - return -1; + errno = ENOENT; + return -1; } struct dirent *next_file; char filepath[MAX_FILE_PATH]; From daf9bfc3949df50f3ae8b4cea3a8a4f2dddf9155 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 6 Mar 2020 15:38:34 +0100 Subject: [PATCH 353/523] Added usage of the delete function --- server/src/systemcontrol.c | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index ff9d17b57..2f681a585 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -762,6 +762,15 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { ControlResponseBuffer, 1, &ClientSocket, 0); } break; + case ClearGeofences_0: + if (CurrentInputArgCount == CommandArgCount) { + SystemControlCommand = Idle_0; + memset(ControlResponseBuffer, 0, sizeof (ControlResponseBuffer)); + *ControlResponseBuffer = SystemControlClearGeofences(); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "ClearGeofences:", + ControlResponseBuffer, 1, &ClientSocket, 0); + } + break; case DownloadFile_1: if (CurrentInputArgCount == CommandArgCount) { SystemControlCommand = Idle_0; From 7c7a9330ee3b3f33566ccf9b5a893ed394a95ddd Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 6 Mar 2020 15:39:05 +0100 Subject: [PATCH 354/523] Ran code formatter --- server/src/systemcontrol.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/server/src/systemcontrol.c b/server/src/systemcontrol.c index 2f681a585..53a635024 100644 --- a/server/src/systemcontrol.c +++ b/server/src/systemcontrol.c @@ -129,7 +129,7 @@ typedef enum { ConnectObject_0, DisconnectObject_0, GetServerParameterList_0, SetServerParameter_2, GetServerParameter_1, DownloadFile_1, UploadFile_3, CheckFileDirectoryExist_1, GetRootDirectoryContent_0, GetDirectoryContent_1, ClearTrajectories_0, ClearGeofences_0, DeleteFileDirectory_1, CreateDirectory_1, GetTestOrigin_0, - replay_1, control_0, + replay_1, control_0, Exit_0, start_ext_trigg_1, nocommand } SystemControlCommand_t; @@ -141,7 +141,7 @@ static const char *SystemControlCommandsArr[] = { "GetServerParameter_1", "DownloadFile_1", "UploadFile_3", "CheckFileDirectoryExist_1", "GetRootDirectoryContent_0", "GetDirectoryContent_1", "ClearTrajectories_0", "ClearGeofences_0", "DeleteFileDirectory_1", "CreateDirectory_1", - "GetTestOrigin_0", "replay_1", + "GetTestOrigin_0", "replay_1", "control_0", "Exit_0", "start_ext_trigg_1" }; From 475f1a4d3c734825bd29aa633660d43efb3228cf Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 6 Mar 2020 15:53:02 +0100 Subject: [PATCH 355/523] Updated util ref --- util | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/util b/util index 80f2fd821..e4d3ad0e6 160000 --- a/util +++ b/util @@ -1 +1 @@ -Subproject commit 80f2fd821c909596e3caa6c23e8d4a1e3aedae82 +Subproject commit e4d3ad0e6ee5ca827578ca7c540fea411fdf36c8 From 52eb6fa3a99f22fa63c2d113f7c0a378feb15aeb Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 6 Mar 2020 16:32:09 +0100 Subject: [PATCH 356/523] Moved new tests to correct location --- {server => core}/integration-tests/MSCPManualTest.py | 0 {server => core}/integration-tests/ManualMessageTest.py | 0 {server => core}/integration-tests/tools/ISO.py | 0 3 files changed, 0 insertions(+), 0 deletions(-) rename {server => core}/integration-tests/MSCPManualTest.py (100%) rename {server => core}/integration-tests/ManualMessageTest.py (100%) rename {server => core}/integration-tests/tools/ISO.py (100%) diff --git a/server/integration-tests/MSCPManualTest.py b/core/integration-tests/MSCPManualTest.py similarity index 100% rename from server/integration-tests/MSCPManualTest.py rename to core/integration-tests/MSCPManualTest.py diff --git a/server/integration-tests/ManualMessageTest.py b/core/integration-tests/ManualMessageTest.py similarity index 100% rename from server/integration-tests/ManualMessageTest.py rename to core/integration-tests/ManualMessageTest.py diff --git a/server/integration-tests/tools/ISO.py b/core/integration-tests/tools/ISO.py similarity index 100% rename from server/integration-tests/tools/ISO.py rename to core/integration-tests/tools/ISO.py From d0c4fa179b7a32991ecb8bb147bb8b9a6273568e Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 6 Mar 2020 16:32:24 +0100 Subject: [PATCH 357/523] Removed buildMaestro.sh script --- buildMaestro.sh | 58 ------------------------------------------------- 1 file changed, 58 deletions(-) delete mode 100755 buildMaestro.sh diff --git a/buildMaestro.sh b/buildMaestro.sh deleted file mode 100755 index 37e0277ac..000000000 --- a/buildMaestro.sh +++ /dev/null @@ -1,58 +0,0 @@ -#!/bin/sh -export PATH=$PATH:/usr/bin - -if [ $USER = "jenkins" ] || [ $USER = "tomcat" ]; then - # Show jenkins environment - echo "Running as ${USER} with environment" - printenv - rm -rf ~/.maestro -fi - -MAESTRODIR=$(pwd) -git submodule update --init --recursive || exit 1 - -# Build util -cd util/C -echo "Building util" -cmake -G "Unix Makefiles" . && make || exit 1 - -# Build core modules -cd $MAESTRODIR/server -mkdir build -cd build -echo "Building core modules" -cmake -G "Unix Makefiles" -DUSE_CITS:BOOL=FALSE -DCMAKE_BUILD_TYPE=Debug .. && make || exit 1 - -# Build ScenarioControl module -mkdir $MAESTRODIR/modules/ScenarioControl/build -cd $MAESTRODIR/modules/ScenarioControl/build -echo "Building ScenarioControl" -cmake .. && make || exit 1 - -# Build Visualization module -mkdir $MAESTRODIR/modules/Visualization/build -cd $MAESTRODIR/modules/Visualization/build -echo "Building Visualization" -cmake .. && make || exit 1 - -# Build Supervision module -mkdir $MAESTRODIR/modules/Supervision/build -cd $MAESTRODIR/modules/Supervision/build -echo "Building Supervision" -cmake .. && make || exit 1 - -# Set up running directory in home -cd -echo "Setting up running directory" -if [ ! -d ".maestro" ]; then - mkdir .maestro - cd .maestro - mkdir journal - mkdir traj - mkdir conf - mkdir geofence - cp -R $MAESTRODIR/server/conf/ . -else - echo "Running directory already exists, nothing to do" -fi - From a79874bb3235eded7be3d6f8266f1f12694ffd9d Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 6 Mar 2020 16:32:43 +0100 Subject: [PATCH 358/523] Deleted execute permissions from configuration files --- conf/adaptivesync.conf | 0 conf/triggeraction.conf | 0 2 files changed, 0 insertions(+), 0 deletions(-) mode change 100755 => 100644 conf/adaptivesync.conf mode change 100755 => 100644 conf/triggeraction.conf diff --git a/conf/adaptivesync.conf b/conf/adaptivesync.conf old mode 100755 new mode 100644 diff --git a/conf/triggeraction.conf b/conf/triggeraction.conf old mode 100755 new mode 100644 From 193b9c55abac0f3dd0af1d36e72db0a4b519350b Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 6 Mar 2020 16:33:49 +0100 Subject: [PATCH 359/523] Modified runServer to work with new file structure --- runServer.sh | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/runServer.sh b/runServer.sh index 26cd7120c..fdc3a61ee 100755 --- a/runServer.sh +++ b/runServer.sh @@ -15,13 +15,13 @@ MODULE_LENGTH=${#MODULES[@]} echo "Starting server with $MODULE_LENGTH extra message queues." # Build string for executing server alongside modules -SERVER_EXEC_STRING="(cd $MAESTRODIR/server/build && ./TEServer -m $MODULE_LENGTH)" +SERVER_EXEC_STRING="(cd $MAESTRODIR/build/bin && ./Core -m $MODULE_LENGTH)" for i in "${MODULES[@]}" do - SERVER_EXEC_STRING="$SERVER_EXEC_STRING & (cd $MAESTRODIR/modules/$i/build && ./$i)" + SERVER_EXEC_STRING="$SERVER_EXEC_STRING & (cd $MAESTRODIR/build/bin && ./$i)" done # Run the generated string -# Each module in MODULES will be run in parallel with TEServer +# Each module in MODULES will be run in parallel with Core eval $SERVER_EXEC_STRING From f1b81f2112153eff38b9dfe447cd75727a49f8f7 Mon Sep 17 00:00:00 2001 From: LukasWikander Date: Fri, 6 Mar 2020 16:44:07 +0100 Subject: [PATCH 360/523] Update README.md --- README.md | 30 ++++++++++++++++-------------- 1 file changed, 16 insertions(+), 14 deletions(-) diff --git a/README.md b/README.md index 74609943b..b97367079 100644 --- a/README.md +++ b/README.md @@ -6,7 +6,7 @@ The Maestro server is a communication hub for all test objects. The server monit

-To build Maestro either usie the build script "buildMaestro.sh" or follow the guide below. +To build Maestro follow the guide below. ## How to build and run the server @@ -17,10 +17,9 @@ Clone the repo and make sure you run the following command to update all submodu git submodule update --init --recursive ``` -Navigate to the the repo and enter the build folder +Navigate to the the repo and enter the build directory ```sh -cd server mkdir build && cd build ``` create project @@ -34,21 +33,25 @@ cmake -G "Unix Makefiles" -DCMAKE_BUILD_TYPE=Debug .. ``` make the project -] ```sh make -cp -R ../conf/ . ``` -Create a folder for Trajectory files in /build and move one of the existing trajectory files to this folder. +Start the server ```sh -mkdir traj -cp ../traj/0.traj ./traj/192.168.0.1 +cd bin +./Core ``` -Start the server +### Installation +To install the server (recommended) navigate to the build directory ```sh -./TEServer +cd build +``` +then build and install the server (be aware that this requires superuser privileges) +```sh +cmake -G "Unix Makefiles" .. +make install ``` ## Building the server with CITS module and mqtt @@ -83,18 +86,17 @@ make sudo make install ``` -The server will not bu default build the CITS module. This is to prevent the use of the CITS module when it is not necessary. To enable building of the module, run `cmake` from the `build/` directory +The server will not build the CITS module by default. This is to prevent the use of the CITS module when it is not necessary. To enable building of the module, run `cmake` from the `build/` directory ```sh cmake "Unix Makefiles" -DUSE_CITS:BOOL=TRUE .. ``` then you can build and run the server as normal ```sh -make -./TEServer +make && cd bin +./Core ``` To disable the CITS module, remake the `cmake` procedure - ```sh cmake "Unix Makefiles" -DUSE_CITS:BOOL=FALSE .. ``` From b0898bcaed0eb4d8e2362762ab39365556773ccc Mon Sep 17 00:00:00 2001 From: LukasWikander Date: Fri, 6 Mar 2020 16:47:24 +0100 Subject: [PATCH 361/523] Update README.md --- README.md | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/README.md b/README.md index b97367079..21ca98ff6 100644 --- a/README.md +++ b/README.md @@ -43,6 +43,12 @@ cd bin ./Core ``` +To run one or several of the modules along with Core, modify the runServer.sh script by adding the modules you wish to execute in the variable near the top. Then run the script from the top level directory: +```sh +./runServer.sh +``` +To see which modules are available, check the build output inside the ```build/bin``` directory + ### Installation To install the server (recommended) navigate to the build directory ```sh From 2172d44b6560a33f41a69fdecbe5f9a9a00dfe6b Mon Sep 17 00:00:00 2001 From: LukasWikander Date: Fri, 6 Mar 2020 16:48:48 +0100 Subject: [PATCH 362/523] Update README.md --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 21ca98ff6..92b0b76b1 100644 --- a/README.md +++ b/README.md @@ -50,13 +50,13 @@ To run one or several of the modules along with Core, modify the runServer.sh sc To see which modules are available, check the build output inside the ```build/bin``` directory ### Installation -To install the server (recommended) navigate to the build directory +To install the server (recommended) navigate to the build directory and configure the project: ```sh cd build +cmake -G "Unix Makefiles" .. ``` then build and install the server (be aware that this requires superuser privileges) ```sh -cmake -G "Unix Makefiles" .. make install ``` From 6470e5bed31162f331752c5813af390f6d0c9f8c Mon Sep 17 00:00:00 2001 From: alfaro01 Date: Mon, 9 Mar 2020 15:59:51 +0100 Subject: [PATCH 363/523] Fixed values according the ISO standard. --- core/src/iso22133.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/core/src/iso22133.c b/core/src/iso22133.c index f314e82df..a46501dc7 100644 --- a/core/src/iso22133.c +++ b/core/src/iso22133.c @@ -152,8 +152,8 @@ typedef struct { //! TRAJ value IDs #define VALUE_ID_TRAJ_TRAJECTORY_IDENTIFIER 0x0101 -#define VALUE_ID_TRAJ_TRAJECTORY_NAME 0x0101 -#define VALUE_ID_TRAJ_TRAJECTORY_VERSION 0x0101 +#define VALUE_ID_TRAJ_TRAJECTORY_NAME 0x0102 +#define VALUE_ID_TRAJ_TRAJECTORY_VERSION 0x0103 #define VALUE_ID_TRAJ_RELATIVE_TIME 0x0001 #define VALUE_ID_TRAJ_X_POSITION 0x0010 #define VALUE_ID_TRAJ_Y_POSITION 0x0011 From 2905988486c6ae91668b883968787d06e3ab7639 Mon Sep 17 00:00:00 2001 From: Viktor Johansson Date: Fri, 13 Mar 2020 09:42:11 +0100 Subject: [PATCH 364/523] Format source files --- core/src/systemcontrol.c | 11 ++++++----- core/src/util.c | 16 ++++++++-------- 2 files changed, 14 insertions(+), 13 deletions(-) diff --git a/core/src/systemcontrol.c b/core/src/systemcontrol.c index b70bae8f0..5a048da16 100644 --- a/core/src/systemcontrol.c +++ b/core/src/systemcontrol.c @@ -129,7 +129,7 @@ typedef enum { ConnectObject_0, DisconnectObject_0, GetServerParameterList_0, SetServerParameter_2, GetServerParameter_1, DownloadFile_1, UploadFile_3, CheckFileDirectoryExist_1, GetRootDirectoryContent_0, GetDirectoryContent_1, ClearTrajectories_0, DeleteFileDirectory_1, CreateDirectory_1, GetTestOrigin_0, replay_1, control_0, - Exit_0, + Exit_0, start_ext_trigg_1, nocommand } SystemControlCommand_t; @@ -140,7 +140,7 @@ const char *SystemControlCommandsArr[] = { "GetServerParameter_1", "DownloadFile_1", "UploadFile_3", "CheckFileDirectoryExist_1", "GetRootDirectoryContent_0", "GetDirectoryContent_1", "ClearTrajectories_0", "DeleteFileDirectory_1", "CreateDirectory_1", "GetTestOrigin_0", "replay_1", - "control_0", + "control_0", "Exit_0", "start_ext_trigg_1" }; @@ -733,6 +733,7 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); I32 file_len = SystemControlBuildFileContentInfo("dir.info", 0); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "SubGetDirectoryContent:", SystemControlDirectoryInfo.info_buffer, file_len, @@ -753,7 +754,7 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { if (CurrentInputArgCount == CommandArgCount) { SystemControlCommand = Idle_0; memset(ControlResponseBuffer, 0, sizeof (ControlResponseBuffer)); - *ControlResponseBuffer = SystemControlClearTrajectories(); + *ControlResponseBuffer = SystemControlClearTrajectories(); SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "ClearTrajectories:", ControlResponseBuffer, 1, &ClientSocket, 0); } @@ -2065,9 +2066,9 @@ I32 SystemControlCheckFileDirectoryExist(C8 * ParameterName, C8 * ReturnValue, U */ C8 SystemControlClearTrajectories() { if (UtilDeleteTrajectoryFiles() != 0) { - return FAILED_DELETE; + return FAILED_DELETE; } - return SUCCEDED_DELETE; + return SUCCEDED_DELETE; } I32 SystemControlDeleteFileDirectory(C8 * Path, C8 * ReturnValue, U8 Debug) { diff --git a/core/src/util.c b/core/src/util.c index 00bb134b6..67f6cbe13 100644 --- a/core/src/util.c +++ b/core/src/util.c @@ -184,14 +184,14 @@ void CopyHTTPHeaderField(char *request, char *targetContainer, size_t targetCont * \return 0 if it could successfully delete file, non-zero if it could not. */ int deleteDirectoryContents(char *path, size_t pathLen) { - if (path == NULL) { - LogMessage(LOG_LEVEL_ERROR, "Path is null-pointer."); - errno = EINVAL; - return -1; - } + if (path == NULL) { + LogMessage(LOG_LEVEL_ERROR, "Path is null-pointer."); + errno = EINVAL; + return -1; + } if (pathLen > MAX_FILE_PATH) { LogMessage(LOG_LEVEL_ERROR, "Path variable too large to handle"); - errno = EINVAL; + errno = EINVAL; return -1; } // These are data types defined in the "dirent" header @@ -199,8 +199,8 @@ int deleteDirectoryContents(char *path, size_t pathLen) { if (theFolder == NULL) { LogMessage(LOG_LEVEL_ERROR, "Path: %s could not be opened", path); - errno = ENOENT; - return -1; + errno = ENOENT; + return -1; } struct dirent *next_file; char filepath[MAX_FILE_PATH]; From 1fdec3b95d10db42bb7be6d4a71ae1dacd89fc4f Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 13 Mar 2020 10:23:38 +0100 Subject: [PATCH 365/523] Code indentation --- core/src/systemcontrol.c | 128 +++++++++++++++++++-------------------- 1 file changed, 64 insertions(+), 64 deletions(-) diff --git a/core/src/systemcontrol.c b/core/src/systemcontrol.c index 76c4e1a5f..2fd72a19a 100644 --- a/core/src/systemcontrol.c +++ b/core/src/systemcontrol.c @@ -187,7 +187,8 @@ I32 SystemControlReadServerParameter(C8 * ParameterName, C8 * ReturnValue, U8 De I32 SystemControlWriteServerParameter(C8 * ParameterName, C8 * NewValue, U8 Debug); I32 SystemControlSetServerParameter(GSDType * GSD, C8 * ParameterName, C8 * NewValue, U8 Debug); I32 SystemControlCheckFileDirectoryExist(C8 * ParameterName, C8 * ReturnValue, U8 Debug); -I32 SystemControlUploadFile(C8 * Filename, C8 * FileSize, C8 * PacketSize, C8 * FileType, C8 * ReturnValue, U8 Debug); +I32 SystemControlUploadFile(C8 * Filename, C8 * FileSize, C8 * PacketSize, C8 * FileType, C8 * ReturnValue, + U8 Debug); I32 SystemControlReceiveRxData(I32 * sockfd, C8 * Path, C8 * FileSize, C8 * PacketSize, C8 * ReturnValue, U8 Debug); I32 SystemControlDeleteFileDirectory(C8 * Path, C8 * ReturnValue, U8 Debug); @@ -795,7 +796,8 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { SystemControlCommand = Idle_0; bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); SystemControlUploadFile(SystemControlArgument[0], SystemControlArgument[1], - SystemControlArgument[2], SystemControlArgument[3], ControlResponseBuffer, 0); + SystemControlArgument[2], SystemControlArgument[3], + ControlResponseBuffer, 0); SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "UploadFile:", ControlResponseBuffer, 1, &ClientSocket, 0); LogMessage(LOG_LEVEL_DEBUG, "UploadFile filelength: %s", SystemControlArgument[1]); @@ -2159,13 +2161,15 @@ I32 SystemControlCreateDirectory(C8 * Path, C8 * ReturnValue, U8 Debug) { } -I32 SystemControlUploadFile(C8 *Filename, C8 *FileSize, C8 *PacketSize, C8 * FileType, C8 *ReturnValue, U8 Debug) { +I32 SystemControlUploadFile(C8 * Filename, C8 * FileSize, C8 * PacketSize, C8 * FileType, C8 * ReturnValue, + U8 Debug) { - FILE *fd; - C8 CompletePath[MAX_FILE_PATH]; - memset(CompletePath, 0, sizeof (CompletePath)); - //GetCurrentDir(CompletePath, MAX_FILE_PATH); - //strcat(CompletePath, Filename); + FILE *fd; + C8 CompletePath[MAX_FILE_PATH]; + + memset(CompletePath, 0, sizeof (CompletePath)); + //GetCurrentDir(CompletePath, MAX_FILE_PATH); + //strcat(CompletePath, Filename); if (Filename == NULL || FileSize == NULL || PacketSize == NULL || FileType == NULL || ReturnValue == NULL) { LogMessage(LOG_LEVEL_ERROR, "Invalid function parameter passed to upload file handler function"); return -1; @@ -2186,68 +2190,64 @@ I32 SystemControlUploadFile(C8 *Filename, C8 *FileSize, C8 *PacketSize, C8 * Fil break; default: LogMessage(LOG_LEVEL_ERROR, "Received invalid file type upload request"); - //Create temporary file for handling data anyway - UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); - strcat(CompletePath, "/file.tmp"); - fd = fopen(CompletePath, "r"); - if(fd != NULL) - { - fclose(fd); - remove(CompletePath); //Remove file if exist - } - fd = fopen(CompletePath, "w+"); //Create the temporary file - - *ReturnValue = PATH_INVALID_MISSING; + //Create temporary file for handling data anyway + UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); + strcat(CompletePath, "/file.tmp"); + fd = fopen(CompletePath, "r"); + if (fd != NULL) { + fclose(fd); + remove(CompletePath); //Remove file if exist + } + fd = fopen(CompletePath, "w+"); //Create the temporary file + + *ReturnValue = PATH_INVALID_MISSING; return -1; } strcat(CompletePath, Filename); - if (Debug) { - LogPrint("Filename: %s\n", Filename); - LogPrint("FileSize: %s\n", FileSize); - LogPrint("PacketSize: %s\n", PacketSize); - LogPrint("FileType: %s\n", FileType); - LogPrint("CompletePath: %s\n", CompletePath); - } - - if (atoi(PacketSize) > SYSTEM_CONTROL_RX_PACKET_SIZE) { //Check packet size - *ReturnValue = SERVER_PREPARED_BIG_PACKET_SIZE; - return 0; - } + if (Debug) { + LogPrint("Filename: %s\n", Filename); + LogPrint("FileSize: %s\n", FileSize); + LogPrint("PacketSize: %s\n", PacketSize); + LogPrint("FileType: %s\n", FileType); + LogPrint("CompletePath: %s\n", CompletePath); + } - fd = fopen(CompletePath, "r"); - if (fd != NULL) { - fclose(fd); - remove(CompletePath); //Remove file if exist - LogMessage(LOG_LEVEL_INFO, "Deleted file <%s>", CompletePath); - } + if (atoi(PacketSize) > SYSTEM_CONTROL_RX_PACKET_SIZE) { //Check packet size + *ReturnValue = SERVER_PREPARED_BIG_PACKET_SIZE; + return 0; + } - fd = fopen(CompletePath, "w+"); //Create the file - if(fd != NULL) - { - *ReturnValue = SERVER_PREPARED;//Server prepared - fclose(fd); - return 0; - } - else - { - //Failed to open path create temporary file - UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); - strcat(CompletePath, "/file.tmp"); - fd = fopen(CompletePath, "r"); - if(fd != NULL) - { - fclose(fd); - remove(CompletePath); //Remove file if exist - } - fd = fopen(CompletePath, "w+"); //Create the temporary file - - *ReturnValue = PATH_INVALID_MISSING; - - return 0; - } - - return -1; + fd = fopen(CompletePath, "r"); + if (fd != NULL) { + fclose(fd); + remove(CompletePath); //Remove file if exist + LogMessage(LOG_LEVEL_INFO, "Deleted file <%s>", CompletePath); + } + + fd = fopen(CompletePath, "w+"); //Create the file + if (fd != NULL) { + *ReturnValue = SERVER_PREPARED; //Server prepared + fclose(fd); + return 0; + } + else { + //Failed to open path create temporary file + UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); + strcat(CompletePath, "/file.tmp"); + fd = fopen(CompletePath, "r"); + if (fd != NULL) { + fclose(fd); + remove(CompletePath); //Remove file if exist + } + fd = fopen(CompletePath, "w+"); //Create the temporary file + + *ReturnValue = PATH_INVALID_MISSING; + + return 0; + } + + return -1; } From dec9c53122e45b5374f8b5a6b840cb48a832fbdc Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 13 Mar 2020 10:28:37 +0100 Subject: [PATCH 366/523] Formatting --- core/src/systemcontrol.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/core/src/systemcontrol.c b/core/src/systemcontrol.c index 977650d37..39ac5b611 100644 --- a/core/src/systemcontrol.c +++ b/core/src/systemcontrol.c @@ -133,8 +133,9 @@ typedef enum { InitializeScenario_0, ConnectObject_0, DisconnectObject_0, GetServerParameterList_0, SetServerParameter_2, GetServerParameter_1, DownloadFile_1, UploadFile_4, CheckFileDirectoryExist_1, GetRootDirectoryContent_0, GetDirectoryContent_1, - ClearTrajectories_0, DeleteFileDirectory_1, CreateDirectory_1, GetTestOrigin_0, replay_1, control_0, Exit_0, - start_ext_trigg_1, nocommand + ClearTrajectories_0, DeleteFileDirectory_1, CreateDirectory_1, GetTestOrigin_0, replay_1, control_0, + Exit_0, + start_ext_trigg_1, nocommand } SystemControlCommand_t; const char *SystemControlCommandsArr[] = { From e2d6426dbc08eacdb2e555fb91eaf7df051ada2b Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 13 Mar 2020 15:04:06 +0100 Subject: [PATCH 367/523] Updated CMakeLists --- modules/ObjectMonitoring/CMakeLists.txt | 80 +++++++++++-------------- modules/ObjectMonitoring/src/main.cpp | 2 +- 2 files changed, 35 insertions(+), 47 deletions(-) diff --git a/modules/ObjectMonitoring/CMakeLists.txt b/modules/ObjectMonitoring/CMakeLists.txt index 171cb1933..3183793a6 100644 --- a/modules/ObjectMonitoring/CMakeLists.txt +++ b/modules/ObjectMonitoring/CMakeLists.txt @@ -4,62 +4,50 @@ SET(CMAKE_CXX_STANDARD_REQUIRED ON) SET(CMAKE_C_STANDARD 11) SET(CMAKE_C_STANDARD_REQUIRED ON) -project(ObjectMonitoring) -# This module is an example of how to set up a new module external to the Maestro executable +project(ObjectMonitoring LANGUAGES C CXX) +# Define target names +set(OBJECT_MONITORING_TARGET ${PROJECT_NAME}) -include_directories(inc) -include_directories(../../util/C/logging) -include_directories(../../util/C/time) -include_directories(../../util/C/MQBus) -include_directories(../../server/inc) -include(GNUInstallDirs) - -find_package(Threads) +set(COREUTILS_LIBRARY MaestroCoreUtil) +set(ISO_22133_LIBRARY MaestroISO22133) +set(POSITIONING_LIBRARY MaestroPositioning) +set(TIME_LIBRARY MaestroTime) +set(LOGGING_LIBRARY MaestroLogging) +set(MESSAGE_BUS_LIBRARY MaestroMQ) -# Create library -add_library(MaestroLogging - ../../util/C/logging/logging.h - ../../util/C/logging/logging.c -) +include(GNUInstallDirs) -add_library(MaestroTime - ../../util/C/time/maestroTime.h - ../../util/C/time/maestroTime.c -) +find_package(Threads REQUIRED) -add_library(MQBus - ../../util/C/MQBus/mqbus.h - ../../util/C/MQBus/mqbus.c +# Create project main executable target +add_executable(${OBJECT_MONITORING_TARGET} + ${CMAKE_CURRENT_SOURCE_DIR}/src/main.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/connectionhandler.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/protocoldata.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/ISO22133ProtocolData.cpp ) -add_library(util - ../../server/inc/util.h - ../../server/src/util.c +# Link project executable to util libraries +target_link_libraries(${OBJECT_MONITORING_TARGET} LINK_PUBLIC + ${TIME_LIBRARY} + ${LOGGING_LIBRARY} + ${COREUTILS_LIBRARY} + ${CMAKE_THREAD_LIBS_INIT} ) -add_executable(ObjectMonitoring - src/main.cpp +target_include_directories(${OBJECT_MONITORING_TARGET} PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/inc ) -add_library(ConnectionHandler - src/connectionhandler.cpp - inc/connectionhandler.h +# Installation rules +install(CODE "MESSAGE(STATUS \"Installing target ${OBJECT_MONITORING_TARGET}\")") +install(TARGETS ${OBJECT_MONITORING_TARGET} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} ) -add_library(ProtocolData - src/protocoldata.cpp - inc/protocoldata.h - src/ISO22133ProtocolData.cpp - inc/ISO22133ProtocolData.h -) - -install(TARGETS ObjectMonitoring DESTINATION bin) - -target_link_libraries(ObjectMonitoring MaestroTime MaestroLogging util ConnectionHandler) -target_link_libraries(ConnectionHandler ProtocolData ${CMAKE_THREAD_LIBS_INIT}) -target_link_libraries(util MQBus MaestroLogging MaestroTime) -target_link_libraries(MQBus rt m) - - - +# Ensure linkage is reloaded after install +install(CODE "execute_process(COMMAND ldconfig)") diff --git a/modules/ObjectMonitoring/src/main.cpp b/modules/ObjectMonitoring/src/main.cpp index afb5cbf4e..c42d7d86e 100644 --- a/modules/ObjectMonitoring/src/main.cpp +++ b/modules/ObjectMonitoring/src/main.cpp @@ -71,7 +71,7 @@ void pruneTerminatedConnectionHandlers(vector &handlers) { handlers.erase( std::remove(handlers.begin(), handlers.end(), nullptr), handlers.end() ); // Remove any connection handlers which have finished their tasks handlers.erase( std::remove_if(handlers.begin(), handlers.end(), - [](const ConnectionHandler* handler) { handler->isTerminated(); } + [](const ConnectionHandler* handler) { return handler->isTerminated(); } ), handlers.end() ); } From ab51928c23e39c74eff92d322f41c578573561b7 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 13 Mar 2020 15:05:04 +0100 Subject: [PATCH 368/523] Updated main CMakeLists --- CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index a454dd056..74df14435 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -23,6 +23,7 @@ add_subdirectory(core) add_subdirectory(modules/ScenarioControl) add_subdirectory(modules/Supervision) add_subdirectory(modules/Visualization) +add_subdirectory(modules/ObjectMonitoring) # Ensure .maestro directory is created at build time in build directory add_custom_target(configurationDirectory ALL From 1fad4b19f3737479d6bd0dc1404ab5eb9264be04 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 13 Mar 2020 15:07:39 +0100 Subject: [PATCH 369/523] Ran code formatter --- core/src/systemcontrol.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/core/src/systemcontrol.c b/core/src/systemcontrol.c index 39ac5b611..5cbe60e9e 100644 --- a/core/src/systemcontrol.c +++ b/core/src/systemcontrol.c @@ -134,7 +134,7 @@ typedef enum { ConnectObject_0, DisconnectObject_0, GetServerParameterList_0, SetServerParameter_2, GetServerParameter_1, DownloadFile_1, UploadFile_4, CheckFileDirectoryExist_1, GetRootDirectoryContent_0, GetDirectoryContent_1, ClearTrajectories_0, DeleteFileDirectory_1, CreateDirectory_1, GetTestOrigin_0, replay_1, control_0, - Exit_0, + Exit_0, start_ext_trigg_1, nocommand } SystemControlCommand_t; From c30221b7279b6c78d82a04874e664ec9238edf31 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 16 Mar 2020 13:54:02 +0100 Subject: [PATCH 370/523] Updated tests for the new uploadfile message format --- core/integration-tests/100-SingleTypicalTest.py | 2 +- .../249-uploadRowMismatchingTrajectories.py | 8 ++++---- core/integration-tests/tools/MSCP.py | 15 +++++++++++++-- 3 files changed, 18 insertions(+), 7 deletions(-) diff --git a/core/integration-tests/100-SingleTypicalTest.py b/core/integration-tests/100-SingleTypicalTest.py index 2eac32d49..f03cd805a 100644 --- a/core/integration-tests/100-SingleTypicalTest.py +++ b/core/integration-tests/100-SingleTypicalTest.py @@ -47,7 +47,7 @@ def checkProgramStatus(failurePrintout): obj = Executable("VirtualObject",["-nogui"]) # 4: Upload trajectory - userControl.UploadFile("traj/127.0.0.1",traj) + userControl.UploadFile("127.0.0.1",traj,"trajectory") # 5: Send init userControl.Init() diff --git a/core/integration-tests/249-uploadRowMismatchingTrajectories.py b/core/integration-tests/249-uploadRowMismatchingTrajectories.py index 939d8ea2d..6385af0f1 100644 --- a/core/integration-tests/249-uploadRowMismatchingTrajectories.py +++ b/core/integration-tests/249-uploadRowMismatchingTrajectories.py @@ -43,7 +43,7 @@ def checkProgramStatus(failurePrintout): normalTraj = ReadTrajectoryFile("resources/trajectories") # 4: Upload short trajectory - userControl.UploadFile("traj/127.0.0.1", fewRowTraj) + userControl.UploadFile("traj/127.0.0.1", fewRowTraj, "trajectory") # 5: Send init try: @@ -59,7 +59,7 @@ def checkProgramStatus(failurePrintout): time.sleep(0.05) # 6: Upload normal trajectory, to verify we can still initialise - userControl.UploadFile("traj/127.0.0.1", normalTraj) + userControl.UploadFile("traj/127.0.0.1", normalTraj, "trajectory") userControl.Init() userControl.waitForObjectControlState("INITIALIZED") @@ -70,7 +70,7 @@ def checkProgramStatus(failurePrintout): userControl.waitForObjectControlState("IDLE") # 7: Upload long trajectory - userControl.UploadFile("traj/127.0.0.1", manyRowTraj) + userControl.UploadFile("traj/127.0.0.1", manyRowTraj, "trajectory") # 8: Send init try: @@ -86,7 +86,7 @@ def checkProgramStatus(failurePrintout): time.sleep(0.05) # 9: Upload normal trajectory, to verify we can still initialise - userControl.UploadFile("traj/127.0.0.1", normalTraj) + userControl.UploadFile("traj/127.0.0.1", normalTraj,"trajectory") userControl.Init() userControl.waitForObjectControlState("INITIALIZED") diff --git a/core/integration-tests/tools/MSCP.py b/core/integration-tests/tools/MSCP.py index 1428f7206..3e3a0e748 100644 --- a/core/integration-tests/tools/MSCP.py +++ b/core/integration-tests/tools/MSCP.py @@ -216,9 +216,20 @@ def Start(self,delayTime_ms): self.Send(message) print("=== StartScenario() sent") - def UploadFile(self,targetPath,fileContents): + def UploadFile(self,fileName,fileContents,fileType): packetSize = 1200 - message = "POST /maestro HTTP/1.1\r\nHost:" + self.host + "\r\n\r\nUploadFile(" + targetPath + "," + str(len(fileContents)) + "," + str(packetSize) + ");\r\n\r\n" + message = "POST /maestro HTTP/1.1\r\nHost:" + self.host + "\r\n\r\nUploadFile(" + fileName + "," + str(len(fileContents)) + "," + str(packetSize) + "" + if fileType == "trajectory": + message = message + ",2" + elif fileType == "configuration": + message = message + ",3" + elif fileType == "geofence": + message = message + ",4" + elif fileType == "generic": + message = message + ",1" + else: + message = message + "," + fileType + message = message + ");\r\n\r\n" self.uploadReplyLock.acquire() self.lastUploadReply["status"] = "UNKNOWN" self.uploadReplyLock.release() From fc5af4945de3d85ae0397774513af83818a9a6b1 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 17 Mar 2020 15:29:07 +0100 Subject: [PATCH 371/523] Updated util reference --- util | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/util b/util index e4d3ad0e6..6a4532f55 160000 --- a/util +++ b/util @@ -1 +1 @@ -Subproject commit e4d3ad0e6ee5ca827578ca7c540fea411fdf36c8 +Subproject commit 6a4532f55a3da92637ce8c48de9e33d88eb29d7a From 7ca33aae7725cc3933c5235e1cc605a4a25030c1 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 19 Mar 2020 14:43:04 +0100 Subject: [PATCH 372/523] Updated util reference --- core/src/systemcontrol.c | 18 ++---------------- util | 2 +- 2 files changed, 3 insertions(+), 17 deletions(-) diff --git a/core/src/systemcontrol.c b/core/src/systemcontrol.c index 071a2257d..5ffb01c56 100644 --- a/core/src/systemcontrol.c +++ b/core/src/systemcontrol.c @@ -133,7 +133,7 @@ typedef enum { InitializeScenario_0, ConnectObject_0, DisconnectObject_0, GetServerParameterList_0, SetServerParameter_2, GetServerParameter_1, DownloadFile_1, UploadFile_4, CheckFileDirectoryExist_1, GetRootDirectoryContent_0, GetDirectoryContent_1, ClearTrajectories_0, ClearGeofences_0, - DeleteFileDirectory_1, CreateDirectory_1, GetTestOrigin_0, replay_1, control_0, Exit_0, + CreateDirectory_1, GetTestOrigin_0, replay_1, control_0, Exit_0, start_ext_trigg_1, nocommand } SystemControlCommand_t; @@ -143,7 +143,7 @@ static const char *SystemControlCommandsArr[] = { "ConnectObject_0", "DisconnectObject_0", "GetServerParameterList_0", "SetServerParameter_2", "GetServerParameter_1", "DownloadFile_1", "UploadFile_4", "CheckFileDirectoryExist_1", "GetRootDirectoryContent_0", "GetDirectoryContent_1", - "ClearTrajectories_0", "ClearGeofences_0", "DeleteFileDirectory_1", "CreateDirectory_1", + "ClearTrajectories_0", "ClearGeofences_0", "CreateDirectory_1", "GetTestOrigin_0", "replay_1", "control_0", "Exit_0", "start_ext_trigg_1" @@ -698,20 +698,6 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { SystemControlCommand = Idle_0; } break; - case DeleteFileDirectory_1: - if (CurrentInputArgCount == CommandArgCount) { - SystemControlCommand = Idle_0; - bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlDeleteFileDirectory(SystemControlArgument[0], ControlResponseBuffer, 0); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "DeleteFileDirectory:", - ControlResponseBuffer, 1, &ClientSocket, 0); - - } - else { - LogMessage(LOG_LEVEL_ERROR, "Wrong parameter count in DeleteFileDirectory(path)!"); - SystemControlCommand = Idle_0; - } - break; case CreateDirectory_1: if (CurrentInputArgCount == CommandArgCount) { SystemControlCommand = Idle_0; diff --git a/util b/util index e4d3ad0e6..19a42f2de 160000 --- a/util +++ b/util @@ -1 +1 @@ -Subproject commit e4d3ad0e6ee5ca827578ca7c540fea411fdf36c8 +Subproject commit 19a42f2dec489753d31322e0527de876c1b18660 From 3d26179f8107f5cc717545fd1bf4db1c42a359c3 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 19 Mar 2020 14:45:07 +0100 Subject: [PATCH 373/523] ran code formatter --- core/src/systemcontrol.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/core/src/systemcontrol.c b/core/src/systemcontrol.c index 5ffb01c56..5f4a6ad95 100644 --- a/core/src/systemcontrol.c +++ b/core/src/systemcontrol.c @@ -130,11 +130,11 @@ typedef struct { typedef enum { Idle_0, GetServerStatus_0, ArmScenario_0, DisarmScenario_0, StartScenario_1, stop_0, AbortScenario_0, - InitializeScenario_0, ConnectObject_0, DisconnectObject_0, GetServerParameterList_0, - SetServerParameter_2, GetServerParameter_1, DownloadFile_1, UploadFile_4, CheckFileDirectoryExist_1, - GetRootDirectoryContent_0, GetDirectoryContent_1, ClearTrajectories_0, ClearGeofences_0, - CreateDirectory_1, GetTestOrigin_0, replay_1, control_0, Exit_0, - start_ext_trigg_1, nocommand + InitializeScenario_0, ConnectObject_0, DisconnectObject_0, GetServerParameterList_0, + SetServerParameter_2, GetServerParameter_1, DownloadFile_1, UploadFile_4, CheckFileDirectoryExist_1, + GetRootDirectoryContent_0, GetDirectoryContent_1, ClearTrajectories_0, ClearGeofences_0, + CreateDirectory_1, GetTestOrigin_0, replay_1, control_0, Exit_0, + start_ext_trigg_1, nocommand } SystemControlCommand_t; static const char *SystemControlCommandsArr[] = { From b14fb321a8f59662017732a75c76dcb24d6a82b4 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 19 Mar 2020 16:02:25 +0100 Subject: [PATCH 374/523] Implemented deleteTraj and deleteGeofence commands --- core/inc/util.h | 3 ++ core/src/systemcontrol.c | 48 +++++++++++++++++++-- core/src/util.c | 90 ++++++++++++++++++++++++++++++++++++++++ 3 files changed, 137 insertions(+), 4 deletions(-) diff --git a/core/inc/util.h b/core/inc/util.h index 69918da0c..ef01969dd 100644 --- a/core/inc/util.h +++ b/core/inc/util.h @@ -588,6 +588,9 @@ void UtilGetGeofenceDirectoryPath(char* path, size_t pathLen); int UtilDeleteTrajectoryFiles(void); int UtilDeleteGeofenceFiles(void); +int UtilDeleteTrajectoryFile(const char * geofencePath, const size_t nameLen); +int UtilDeleteGeofenceFile(const char * geofencePath, const size_t nameLen); + // File parsing functions int UtilCheckTrajectoryFileFormat(const char *path, size_t pathLen); int UtilParseTrajectoryFileHeader(char *headerLine, TrajectoryFileHeader * header); diff --git a/core/src/systemcontrol.c b/core/src/systemcontrol.c index 5f4a6ad95..d1093fa72 100644 --- a/core/src/systemcontrol.c +++ b/core/src/systemcontrol.c @@ -132,7 +132,7 @@ typedef enum { Idle_0, GetServerStatus_0, ArmScenario_0, DisarmScenario_0, StartScenario_1, stop_0, AbortScenario_0, InitializeScenario_0, ConnectObject_0, DisconnectObject_0, GetServerParameterList_0, SetServerParameter_2, GetServerParameter_1, DownloadFile_1, UploadFile_4, CheckFileDirectoryExist_1, - GetRootDirectoryContent_0, GetDirectoryContent_1, ClearTrajectories_0, ClearGeofences_0, + GetRootDirectoryContent_0, GetDirectoryContent_1, DeleteTrajectory_1, DeleteGeofence_1, ClearTrajectories_0, ClearGeofences_0, CreateDirectory_1, GetTestOrigin_0, replay_1, control_0, Exit_0, start_ext_trigg_1, nocommand } SystemControlCommand_t; @@ -142,7 +142,7 @@ static const char *SystemControlCommandsArr[] = { "AbortScenario_0", "InitializeScenario_0", "ConnectObject_0", "DisconnectObject_0", "GetServerParameterList_0", "SetServerParameter_2", "GetServerParameter_1", "DownloadFile_1", "UploadFile_4", "CheckFileDirectoryExist_1", - "GetRootDirectoryContent_0", "GetDirectoryContent_1", + "GetRootDirectoryContent_0", "GetDirectoryContent_1", "DeleteTrajectory_1", "DeleteGeofence_1", "ClearTrajectories_0", "ClearGeofences_0", "CreateDirectory_1", "GetTestOrigin_0", "replay_1", "control_0", @@ -193,8 +193,10 @@ I32 SystemControlUploadFile(C8 * Filename, C8 * FileSize, C8 * PacketSize, C8 * U8 Debug); I32 SystemControlReceiveRxData(I32 * sockfd, C8 * Path, C8 * FileSize, C8 * PacketSize, C8 * ReturnValue, U8 Debug); -C8 SystemControlClearTrajectories(void); -C8 SystemControlClearGeofences(void); +static C8 SystemControlDeleteTrajectory(const C8 * trajectoryName, const size_t nameLen); +static C8 SystemControlDeleteGeofence(const C8 * geofenceName, const size_t nameLen); +static C8 SystemControlClearTrajectories(void); +static C8 SystemControlClearGeofences(void); I32 SystemControlDeleteFileDirectory(C8 * Path, C8 * ReturnValue, U8 Debug); I32 SystemControlBuildFileContentInfo(C8 * Path, U8 Debug); I32 SystemControlDestroyFileContentInfo(C8 * path); @@ -743,6 +745,24 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { SystemControlCommand = Idle_0; } break; + case DeleteTrajectory_1: + if (CurrentInputArgCount == CommandArgCount) { + SystemControlCommand = Idle_0; + memset(ControlResponseBuffer, 0, sizeof (ControlResponseBuffer)); + *ControlResponseBuffer = SystemControlDeleteTrajectory(SystemControlArgument[0], sizeof(SystemControlArgument[0])); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "DeleteTrajectory:", + ControlResponseBuffer, 1, &ClientSocket, 0); + } + break; + case DeleteGeofence_1: + if (CurrentInputArgCount == CommandArgCount) { + SystemControlCommand = Idle_0; + memset(ControlResponseBuffer, 0, sizeof (ControlResponseBuffer)); + *ControlResponseBuffer = SystemControlDeleteGeofence(SystemControlArgument[0], sizeof(SystemControlArgument[0])); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "DeleteGeofence:", + ControlResponseBuffer, 1, &ClientSocket, 0); + } + break; case ClearTrajectories_0: if (CurrentInputArgCount == CommandArgCount) { SystemControlCommand = Idle_0; @@ -2063,6 +2083,26 @@ I32 SystemControlCheckFileDirectoryExist(C8 * ParameterName, C8 * ReturnValue, U return 0; } +/*! + * \brief SystemControlDeleteTrajectory Deletes the chosen trajectory + * \param trajectoryName Name of the trajectory file + * \param nameLen Length of the name string + * \return Returns ::SUCCEDED_DELETE upon successfully deleting a file, otherwise ::FAILED_DELETE + */ +C8 SystemControlDeleteTrajectory(const C8 * trajectoryName, const size_t nameLen) { + return UtilDeleteTrajectoryFile(trajectoryName, nameLen) ? FAILED_DELETE : SUCCEEDED_DELETE; +} + +/*! + * \brief SystemControlDeleteGeofence Deletes the chosen geofence + * \param trajectoryName Name of the geofence file + * \param nameLen Length of the name string + * \return Returns ::SUCCEDED_DELETE upon successfully deleting a file, otherwise ::FAILED_DELETE + */ +C8 SystemControlDeleteGeofence(const C8 * geofenceName, const size_t nameLen) { + return UtilDeleteGeofenceFile(geofenceName, nameLen) ? FAILED_DELETE : SUCCEEDED_DELETE; +} + /*! * \brief SystemControlClearTrajectories Clears the trajectory directory on the machine * \return Returns ::SUCCEDED_DELETE upon successfully deleting a file, otherwise ::FAILED_DELETE. diff --git a/core/src/util.c b/core/src/util.c index 7a7d285b3..169fbb6ca 100644 --- a/core/src/util.c +++ b/core/src/util.c @@ -114,6 +114,7 @@ static void CopyHTTPHeaderField(char *request, char *targetContainer, size_t tar static char rayFromPointIntersectsLine(double pointX, double pointY, double polyPointAX, double polyPointAY, double polyPointBX, double polyPointBY); static int deleteDirectoryContents(char *path, size_t pathLen); +static int deleteFile(char *path, size_t pathLen); void CopyHTTPHeaderField(char *request, char *targetContainer, size_t targetContainerSize, const char *fieldName) { @@ -177,6 +178,33 @@ void CopyHTTPHeaderField(char *request, char *targetContainer, size_t targetCont } +/*! + * \brief deleteFile Deletes the file given in the parameter ::path + * \param path The path to the file on the machine. + * \param pathLen The length of ::the path string. + * \return 0 if it could successfully delete file, non-zero if it could not. + */ +int deleteFile(char *path, size_t pathLen) { + if (path == NULL) { + LogMessage(LOG_LEVEL_ERROR, "Path is null-pointer"); + errno = EINVAL; + return -1; + } + if (pathLen > MAX_FILE_PATH) { + LogMessage(LOG_LEVEL_ERROR, "Path variable too large to handle"); + errno = EINVAL; + return -1; + } + + FILE* fd = fopen(path, "a"); + + if (fd == NULL) { + LogMessage(LOG_LEVEL_ERROR, "Path: %s could not be opened", path); + return -1; + } + return 0; +} + /*! * \brief deleteDirectoryContents Deletes the directory given in the parameter ::path * \param path The path to the directory on the machine. @@ -2383,6 +2411,68 @@ void UtilGetGeofenceDirectoryPath(char *path, size_t pathLen) { strcat(path, "/"); } +/*! + * \brief UtilDeleteTrajectoryFile deletes the specified trajectory and deletes it + * \return returns 0 if the trajectory is now deleted. Non-zero values otherwise. + */ +int UtilDeleteTrajectoryFile(const char* name, const size_t nameLen) { + char filePath[MAX_FILE_PATH] = { '\0' }; + UtilGetTrajDirectoryPath(filePath, sizeof (filePath)); + + if (strstr(name, "..") != NULL || strstr(name, "/") != NULL) { + errno = EPERM; + LogMessage(LOG_LEVEL_ERROR, "Attempt to call delete on trajectory file and navigate out of directory"); + return -1; + } + if (name == NULL) { + errno = EINVAL; + LogMessage(LOG_LEVEL_ERROR, "Attempt to call delete on null trajectory file"); + return -1; + } + if (strlen(filePath) + nameLen > MAX_FILE_PATH) { + errno = ENOBUFS; + LogMessage(LOG_LEVEL_ERROR, "Trajectory file name too long"); + return -1; + } + + if (filePath[0] == '\0') + return -1; + + strcat(filePath, name); + return deleteFile(filePath, sizeof (filePath)); +} + +/*! + * \brief UtilDeleteGeofenceFile deletes the specified geofence and deletes it + * \return returns 0 if the geofence is now deleted. Non-zero values otherwise. + */ +int UtilDeleteGeofenceFile(const char* name, const size_t nameLen) { + char filePath[MAX_FILE_PATH] = { '\0' }; + UtilGetGeofenceDirectoryPath(filePath, sizeof (filePath)); + + if (strstr(name, "..") != NULL || strstr(name, "/") != NULL) { + errno = EPERM; + LogMessage(LOG_LEVEL_ERROR, "Attempt to call delete on geofence file and navigate out of directory"); + return -1; + } + if (name == NULL) { + errno = EINVAL; + LogMessage(LOG_LEVEL_ERROR, "Attempt to call delete on null geofence file"); + return -1; + } + if (strlen(filePath) + nameLen > MAX_FILE_PATH) { + errno = ENOBUFS; + LogMessage(LOG_LEVEL_ERROR, "Geofence file name too long"); + return -1; + } + + if (filePath[0] == '\0') + return -1; + + strcat(filePath, name); + return deleteFile(filePath, sizeof (filePath)); +} + /*! * \brief UtilDeleteTrajectoryFiles finds the trajectory folder and deletes its contents * \return returns 0 if succesfull if the trajectory folder now is empty. Non-zero values otherwise. From a7f8137828a415c72e208fbd336e3a0c0a0c88e5 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 19 Mar 2020 16:03:35 +0100 Subject: [PATCH 375/523] Updated util reference --- util | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/util b/util index 6a4532f55..19a42f2de 160000 --- a/util +++ b/util @@ -1 +1 @@ -Subproject commit 6a4532f55a3da92637ce8c48de9e33d88eb29d7a +Subproject commit 19a42f2dec489753d31322e0527de876c1b18660 From a0a7122df4bbd70274056679b1e9e2ae0cab2eef Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 19 Mar 2020 16:08:58 +0100 Subject: [PATCH 376/523] Actually delete file in the deleteFile function --- core/src/util.c | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/core/src/util.c b/core/src/util.c index 169fbb6ca..e7e4ef231 100644 --- a/core/src/util.c +++ b/core/src/util.c @@ -199,7 +199,13 @@ int deleteFile(char *path, size_t pathLen) { FILE* fd = fopen(path, "a"); if (fd == NULL) { - LogMessage(LOG_LEVEL_ERROR, "Path: %s could not be opened", path); + LogMessage(LOG_LEVEL_ERROR, "Path <%s> could not be opened", path); + return -1; + } + fclose(fd); + + if (remove(path) != 0) { + LogMessage(LOG_LEVEL_ERROR, "Path <%s> could not be deleted", path); return -1; } return 0; From 14f20033b53bd07cdd123c1dbad886ff85822902 Mon Sep 17 00:00:00 2001 From: Sebastian Loh Lindholm Date: Thu, 19 Mar 2020 16:56:44 +0100 Subject: [PATCH 377/523] Fixed the path for receiving the file. --- core/src/systemcontrol.c | 46 ++++++++++++++++++++++------------------ 1 file changed, 25 insertions(+), 21 deletions(-) diff --git a/core/src/systemcontrol.c b/core/src/systemcontrol.c index 071a2257d..5ebbe93e7 100644 --- a/core/src/systemcontrol.c +++ b/core/src/systemcontrol.c @@ -189,8 +189,7 @@ I32 SystemControlReadServerParameter(C8 * ParameterName, C8 * ReturnValue, U8 De I32 SystemControlWriteServerParameter(C8 * ParameterName, C8 * NewValue, U8 Debug); I32 SystemControlSetServerParameter(GSDType * GSD, C8 * ParameterName, C8 * NewValue, U8 Debug); I32 SystemControlCheckFileDirectoryExist(C8 * ParameterName, C8 * ReturnValue, U8 Debug); -I32 SystemControlUploadFile(C8 * Filename, C8 * FileSize, C8 * PacketSize, C8 * FileType, C8 * ReturnValue, - U8 Debug); +I32 SystemControlUploadFile(C8 * Filename, C8 * FileSize, C8 * PacketSize, C8 * FileType, C8 * ReturnValue, C8 * CompleteFilePath, U8 Debug); I32 SystemControlReceiveRxData(I32 * sockfd, C8 * Path, C8 * FileSize, C8 * PacketSize, C8 * ReturnValue, U8 Debug); C8 SystemControlClearTrajectories(void); @@ -300,6 +299,8 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { U32 RVSSConfigU32; U32 RVSSMessageLengthU32; U16 PCDMessageCodeU16; + C8 RxFilePath[MAX_FILE_PATH]; + LogInit(MODULE_NAME, logLevel); LogMessage(LOG_LEVEL_INFO, "System control task running with PID: %i", getpid()); @@ -803,29 +804,30 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { if (CurrentInputArgCount == CommandArgCount) { SystemControlCommand = Idle_0; bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); + bzero(RxFilePath, MAX_FILE_PATH); SystemControlUploadFile(SystemControlArgument[0], SystemControlArgument[1], SystemControlArgument[2], SystemControlArgument[3], - ControlResponseBuffer, 0); + ControlResponseBuffer, RxFilePath, 0); SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "UploadFile:", ControlResponseBuffer, 1, &ClientSocket, 0); LogMessage(LOG_LEVEL_DEBUG, "UploadFile filelength: %s", SystemControlArgument[1]); if (ControlResponseBuffer[0] == SERVER_PREPARED_BIG_PACKET_SIZE) //Server is ready to receive data { LogMessage(LOG_LEVEL_INFO, "Receiving file: %s", SystemControlArgument[0]); - SystemControlReceiveRxData(&ClientSocket, SystemControlArgument[0], + SystemControlReceiveRxData(&ClientSocket, RxFilePath, SystemControlArgument[1], STR_SYSTEM_CONTROL_RX_PACKET_SIZE, ControlResponseBuffer, 0); } else if (ControlResponseBuffer[0] == PATH_INVALID_MISSING) { LogMessage(LOG_LEVEL_INFO, "Failed receiving file: %s", SystemControlArgument[0]); - SystemControlReceiveRxData(&ClientSocket, "file.tmp", SystemControlArgument[1], + SystemControlReceiveRxData(&ClientSocket, RxFilePath, SystemControlArgument[1], STR_SYSTEM_CONTROL_RX_PACKET_SIZE, ControlResponseBuffer, 0); - SystemControlDeleteFileDirectory("file.tmp", ControlResponseBuffer, 0); + SystemControlDeleteFileDirectory(RxFilePath, ControlResponseBuffer, 0); ControlResponseBuffer[0] = PATH_INVALID_MISSING; } else { LogMessage(LOG_LEVEL_INFO, "Receiving file: %s", SystemControlArgument[0]); - SystemControlReceiveRxData(&ClientSocket, SystemControlArgument[0], + SystemControlReceiveRxData(&ClientSocket, RxFilePath, SystemControlArgument[1], SystemControlArgument[2], ControlResponseBuffer, 0); } @@ -2191,7 +2193,7 @@ I32 SystemControlCreateDirectory(C8 * Path, C8 * ReturnValue, U8 Debug) { I32 SystemControlUploadFile(C8 * Filename, C8 * FileSize, C8 * PacketSize, C8 * FileType, C8 * ReturnValue, - U8 Debug) { + C8 * CompleteFilePath, U8 Debug) { FILE *fd; C8 CompletePath[MAX_FILE_PATH]; @@ -2233,13 +2235,15 @@ I32 SystemControlUploadFile(C8 * Filename, C8 * FileSize, C8 * PacketSize, C8 * return -1; } strcat(CompletePath, Filename); + strcpy(CompleteFilePath, CompletePath); if (Debug) { - LogPrint("Filename: %s\n", Filename); - LogPrint("FileSize: %s\n", FileSize); - LogPrint("PacketSize: %s\n", PacketSize); - LogPrint("FileType: %s\n", FileType); - LogPrint("CompletePath: %s\n", CompletePath); + LogPrint("Filename: %s", Filename); + LogPrint("FileSize: %s", FileSize); + LogPrint("PacketSize: %s", PacketSize); + LogPrint("FileType: %s", FileType); + LogPrint("CompletePath: %s", CompletePath); + LogPrint("CompleteFilePath: %s", CompleteFilePath); } if (atoi(PacketSize) > SYSTEM_CONTROL_RX_PACKET_SIZE) { //Check packet size @@ -2251,7 +2255,7 @@ I32 SystemControlUploadFile(C8 * Filename, C8 * FileSize, C8 * PacketSize, C8 * if (fd != NULL) { fclose(fd); remove(CompletePath); //Remove file if exist - LogMessage(LOG_LEVEL_INFO, "Deleted file <%s>", CompletePath); + LogMessage(LOG_LEVEL_INFO, "Deleted existing file <%s>", CompletePath); } fd = fopen(CompletePath, "w+"); //Create the file @@ -2287,7 +2291,7 @@ I32 SystemControlReceiveRxData(I32 * sockfd, C8 * Path, C8 * FileSize, C8 * Pack C8 CompletePath[MAX_FILE_PATH]; bzero(CompletePath, MAX_FILE_PATH); - UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); + //UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); strcat(CompletePath, Path); U32 FileSizeU32 = atoi(FileSize); U16 PacketSizeU16 = atoi(PacketSize); @@ -2299,11 +2303,11 @@ I32 SystemControlReceiveRxData(I32 * sockfd, C8 * Path, C8 * FileSize, C8 * Pack if (Debug) { - LogMessage(LOG_LEVEL_DEBUG, "Receive Rx data:"); - LogMessage(LOG_LEVEL_DEBUG, "%s", Path); - LogMessage(LOG_LEVEL_DEBUG, "%s", FileSize); - LogMessage(LOG_LEVEL_DEBUG, "%s", PacketSize); - LogMessage(LOG_LEVEL_DEBUG, "%s", CompletePath); + LogPrint("Receive Rx data:"); + LogPrint("Path: %s", Path); + LogPrint("FileSize: %s", FileSize); + LogPrint("PacketSize: %s", PacketSize); + LogPrint("CompletePath: %s", CompletePath); } @@ -2362,7 +2366,7 @@ I32 SystemControlReceiveRxData(I32 * sockfd, C8 * Path, C8 * FileSize, C8 * Pack LogMessage(LOG_LEVEL_INFO, "CORRUPT FILE, REMOVING..."); } - LogMessage(LOG_LEVEL_DEBUG, "Rec count = %d, Req count = %d", TotalRxCount, FileSizeU32); + LogMessage(LOG_LEVEL_INFO, "Rec count = %d, Req count = %d", TotalRxCount, FileSizeU32); } From bf047d09e0f8f0c9b671eb41ca69a4d76c784c66 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 20 Mar 2020 09:49:47 +0100 Subject: [PATCH 378/523] Added error printouts for when the commands have insufficient parameters --- core/src/systemcontrol.c | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/core/src/systemcontrol.c b/core/src/systemcontrol.c index d1093fa72..02ba500a6 100644 --- a/core/src/systemcontrol.c +++ b/core/src/systemcontrol.c @@ -753,6 +753,12 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "DeleteTrajectory:", ControlResponseBuffer, 1, &ClientSocket, 0); } + else { + LogMessage(LOG_LEVEL_ERROR, + "Wrong parameter count in DeleteTrajectory(name)! got:%d, expected:%d", + CurrentInputArgCount, CommandArgCount); + SystemControlCommand = Idle_0; + } break; case DeleteGeofence_1: if (CurrentInputArgCount == CommandArgCount) { @@ -762,6 +768,12 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "DeleteGeofence:", ControlResponseBuffer, 1, &ClientSocket, 0); } + else { + LogMessage(LOG_LEVEL_ERROR, + "Wrong parameter count in DeleteGeofence(name)! got:%d, expected:%d", + CurrentInputArgCount, CommandArgCount); + SystemControlCommand = Idle_0; + } break; case ClearTrajectories_0: if (CurrentInputArgCount == CommandArgCount) { @@ -771,6 +783,12 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "ClearTrajectories:", ControlResponseBuffer, 1, &ClientSocket, 0); } + else { + LogMessage(LOG_LEVEL_ERROR, + "Wrong parameter count in ClearTrajectories()! got:%d, expected:%d", + CurrentInputArgCount, CommandArgCount); + SystemControlCommand = Idle_0; + } break; case ClearGeofences_0: if (CurrentInputArgCount == CommandArgCount) { @@ -780,6 +798,12 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "ClearGeofences:", ControlResponseBuffer, 1, &ClientSocket, 0); } + else { + LogMessage(LOG_LEVEL_ERROR, + "Wrong parameter count in ClearTrajectories()! got:%d, expected:%d", + CurrentInputArgCount, CommandArgCount); + SystemControlCommand = Idle_0; + } break; case DownloadFile_1: if (CurrentInputArgCount == CommandArgCount) { From e25525a8e7b59f5045df197fe943b6bfa2eb01b9 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 20 Mar 2020 10:09:20 +0100 Subject: [PATCH 379/523] Fixed typo --- core/src/systemcontrol.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/core/src/systemcontrol.c b/core/src/systemcontrol.c index 02ba500a6..5e68c3e09 100644 --- a/core/src/systemcontrol.c +++ b/core/src/systemcontrol.c @@ -800,7 +800,7 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { } else { LogMessage(LOG_LEVEL_ERROR, - "Wrong parameter count in ClearTrajectories()! got:%d, expected:%d", + "Wrong parameter count in ClearGeofences()! got:%d, expected:%d", CurrentInputArgCount, CommandArgCount); SystemControlCommand = Idle_0; } From d49497d9e89cbeaa4086fb322c8b4729fa6951f7 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 20 Mar 2020 10:28:05 +0100 Subject: [PATCH 380/523] Updated test script for uploadfile update --- .../249-uploadRowMismatchingTrajectories.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/core/integration-tests/249-uploadRowMismatchingTrajectories.py b/core/integration-tests/249-uploadRowMismatchingTrajectories.py index 6385af0f1..2c7435644 100644 --- a/core/integration-tests/249-uploadRowMismatchingTrajectories.py +++ b/core/integration-tests/249-uploadRowMismatchingTrajectories.py @@ -43,7 +43,7 @@ def checkProgramStatus(failurePrintout): normalTraj = ReadTrajectoryFile("resources/trajectories") # 4: Upload short trajectory - userControl.UploadFile("traj/127.0.0.1", fewRowTraj, "trajectory") + userControl.UploadFile("127.0.0.1", fewRowTraj, "trajectory") # 5: Send init try: @@ -59,7 +59,7 @@ def checkProgramStatus(failurePrintout): time.sleep(0.05) # 6: Upload normal trajectory, to verify we can still initialise - userControl.UploadFile("traj/127.0.0.1", normalTraj, "trajectory") + userControl.UploadFile("127.0.0.1", normalTraj, "trajectory") userControl.Init() userControl.waitForObjectControlState("INITIALIZED") @@ -70,7 +70,7 @@ def checkProgramStatus(failurePrintout): userControl.waitForObjectControlState("IDLE") # 7: Upload long trajectory - userControl.UploadFile("traj/127.0.0.1", manyRowTraj, "trajectory") + userControl.UploadFile("127.0.0.1", manyRowTraj, "trajectory") # 8: Send init try: @@ -86,7 +86,7 @@ def checkProgramStatus(failurePrintout): time.sleep(0.05) # 9: Upload normal trajectory, to verify we can still initialise - userControl.UploadFile("traj/127.0.0.1", normalTraj,"trajectory") + userControl.UploadFile("127.0.0.1", normalTraj,"trajectory") userControl.Init() userControl.waitForObjectControlState("INITIALIZED") From 211b130250feaba2368d0f5a89ad15815f5141ca Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 20 Mar 2020 11:44:57 +0100 Subject: [PATCH 381/523] Ran code formatter --- core/src/systemcontrol.c | 13 +++++++++---- core/src/util.c | 9 +++++---- 2 files changed, 14 insertions(+), 8 deletions(-) diff --git a/core/src/systemcontrol.c b/core/src/systemcontrol.c index 8ca3e051c..8f27173a2 100644 --- a/core/src/systemcontrol.c +++ b/core/src/systemcontrol.c @@ -132,7 +132,8 @@ typedef enum { Idle_0, GetServerStatus_0, ArmScenario_0, DisarmScenario_0, StartScenario_1, stop_0, AbortScenario_0, InitializeScenario_0, ConnectObject_0, DisconnectObject_0, GetServerParameterList_0, SetServerParameter_2, GetServerParameter_1, DownloadFile_1, UploadFile_4, CheckFileDirectoryExist_1, - GetRootDirectoryContent_0, GetDirectoryContent_1, DeleteTrajectory_1, DeleteGeofence_1, ClearTrajectories_0, ClearGeofences_0, + GetRootDirectoryContent_0, GetDirectoryContent_1, DeleteTrajectory_1, DeleteGeofence_1, + ClearTrajectories_0, ClearGeofences_0, CreateDirectory_1, GetTestOrigin_0, replay_1, control_0, Exit_0, start_ext_trigg_1, nocommand } SystemControlCommand_t; @@ -189,7 +190,8 @@ I32 SystemControlReadServerParameter(C8 * ParameterName, C8 * ReturnValue, U8 De I32 SystemControlWriteServerParameter(C8 * ParameterName, C8 * NewValue, U8 Debug); I32 SystemControlSetServerParameter(GSDType * GSD, C8 * ParameterName, C8 * NewValue, U8 Debug); I32 SystemControlCheckFileDirectoryExist(C8 * ParameterName, C8 * ReturnValue, U8 Debug); -I32 SystemControlUploadFile(C8 * Filename, C8 * FileSize, C8 * PacketSize, C8 * FileType, C8 * ReturnValue, C8 * CompleteFilePath, U8 Debug); +I32 SystemControlUploadFile(C8 * Filename, C8 * FileSize, C8 * PacketSize, C8 * FileType, C8 * ReturnValue, + C8 * CompleteFilePath, U8 Debug); I32 SystemControlReceiveRxData(I32 * sockfd, C8 * Path, C8 * FileSize, C8 * PacketSize, C8 * ReturnValue, U8 Debug); static C8 SystemControlDeleteTrajectory(const C8 * trajectoryName, const size_t nameLen); @@ -750,7 +752,9 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { if (CurrentInputArgCount == CommandArgCount) { SystemControlCommand = Idle_0; memset(ControlResponseBuffer, 0, sizeof (ControlResponseBuffer)); - *ControlResponseBuffer = SystemControlDeleteTrajectory(SystemControlArgument[0], sizeof(SystemControlArgument[0])); + *ControlResponseBuffer = + SystemControlDeleteTrajectory(SystemControlArgument[0], + sizeof (SystemControlArgument[0])); SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "DeleteTrajectory:", ControlResponseBuffer, 1, &ClientSocket, 0); } @@ -765,7 +769,8 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { if (CurrentInputArgCount == CommandArgCount) { SystemControlCommand = Idle_0; memset(ControlResponseBuffer, 0, sizeof (ControlResponseBuffer)); - *ControlResponseBuffer = SystemControlDeleteGeofence(SystemControlArgument[0], sizeof(SystemControlArgument[0])); + *ControlResponseBuffer = + SystemControlDeleteGeofence(SystemControlArgument[0], sizeof (SystemControlArgument[0])); SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "DeleteGeofence:", ControlResponseBuffer, 1, &ClientSocket, 0); } diff --git a/core/src/util.c b/core/src/util.c index e7e4ef231..bb6c7155b 100644 --- a/core/src/util.c +++ b/core/src/util.c @@ -196,7 +196,7 @@ int deleteFile(char *path, size_t pathLen) { return -1; } - FILE* fd = fopen(path, "a"); + FILE *fd = fopen(path, "a"); if (fd == NULL) { LogMessage(LOG_LEVEL_ERROR, "Path <%s> could not be opened", path); @@ -2421,13 +2421,14 @@ void UtilGetGeofenceDirectoryPath(char *path, size_t pathLen) { * \brief UtilDeleteTrajectoryFile deletes the specified trajectory and deletes it * \return returns 0 if the trajectory is now deleted. Non-zero values otherwise. */ -int UtilDeleteTrajectoryFile(const char* name, const size_t nameLen) { +int UtilDeleteTrajectoryFile(const char *name, const size_t nameLen) { char filePath[MAX_FILE_PATH] = { '\0' }; UtilGetTrajDirectoryPath(filePath, sizeof (filePath)); if (strstr(name, "..") != NULL || strstr(name, "/") != NULL) { errno = EPERM; - LogMessage(LOG_LEVEL_ERROR, "Attempt to call delete on trajectory file and navigate out of directory"); + LogMessage(LOG_LEVEL_ERROR, + "Attempt to call delete on trajectory file and navigate out of directory"); return -1; } if (name == NULL) { @@ -2452,7 +2453,7 @@ int UtilDeleteTrajectoryFile(const char* name, const size_t nameLen) { * \brief UtilDeleteGeofenceFile deletes the specified geofence and deletes it * \return returns 0 if the geofence is now deleted. Non-zero values otherwise. */ -int UtilDeleteGeofenceFile(const char* name, const size_t nameLen) { +int UtilDeleteGeofenceFile(const char *name, const size_t nameLen) { char filePath[MAX_FILE_PATH] = { '\0' }; UtilGetGeofenceDirectoryPath(filePath, sizeof (filePath)); From 06ff6197ed78db63a69d1a21b91cd71b15a6fcd4 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 20 Mar 2020 11:48:05 +0100 Subject: [PATCH 382/523] Ran code formatter --- core/src/systemcontrol.c | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/core/src/systemcontrol.c b/core/src/systemcontrol.c index 5ebbe93e7..7e571c5a3 100644 --- a/core/src/systemcontrol.c +++ b/core/src/systemcontrol.c @@ -130,11 +130,11 @@ typedef struct { typedef enum { Idle_0, GetServerStatus_0, ArmScenario_0, DisarmScenario_0, StartScenario_1, stop_0, AbortScenario_0, - InitializeScenario_0, ConnectObject_0, DisconnectObject_0, GetServerParameterList_0, - SetServerParameter_2, GetServerParameter_1, DownloadFile_1, UploadFile_4, CheckFileDirectoryExist_1, - GetRootDirectoryContent_0, GetDirectoryContent_1, ClearTrajectories_0, ClearGeofences_0, - DeleteFileDirectory_1, CreateDirectory_1, GetTestOrigin_0, replay_1, control_0, Exit_0, - start_ext_trigg_1, nocommand + InitializeScenario_0, ConnectObject_0, DisconnectObject_0, GetServerParameterList_0, + SetServerParameter_2, GetServerParameter_1, DownloadFile_1, UploadFile_4, CheckFileDirectoryExist_1, + GetRootDirectoryContent_0, GetDirectoryContent_1, ClearTrajectories_0, ClearGeofences_0, + DeleteFileDirectory_1, CreateDirectory_1, GetTestOrigin_0, replay_1, control_0, Exit_0, + start_ext_trigg_1, nocommand } SystemControlCommand_t; static const char *SystemControlCommandsArr[] = { @@ -189,7 +189,8 @@ I32 SystemControlReadServerParameter(C8 * ParameterName, C8 * ReturnValue, U8 De I32 SystemControlWriteServerParameter(C8 * ParameterName, C8 * NewValue, U8 Debug); I32 SystemControlSetServerParameter(GSDType * GSD, C8 * ParameterName, C8 * NewValue, U8 Debug); I32 SystemControlCheckFileDirectoryExist(C8 * ParameterName, C8 * ReturnValue, U8 Debug); -I32 SystemControlUploadFile(C8 * Filename, C8 * FileSize, C8 * PacketSize, C8 * FileType, C8 * ReturnValue, C8 * CompleteFilePath, U8 Debug); +I32 SystemControlUploadFile(C8 * Filename, C8 * FileSize, C8 * PacketSize, C8 * FileType, C8 * ReturnValue, + C8 * CompleteFilePath, U8 Debug); I32 SystemControlReceiveRxData(I32 * sockfd, C8 * Path, C8 * FileSize, C8 * PacketSize, C8 * ReturnValue, U8 Debug); C8 SystemControlClearTrajectories(void); From b5b1411779e83ff1a8c8429e7e8d3831acff3da8 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 20 Mar 2020 12:18:57 +0100 Subject: [PATCH 383/523] Ran code formatter --- core/src/systemcontrol.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/core/src/systemcontrol.c b/core/src/systemcontrol.c index 8f27173a2..aa8c88baa 100644 --- a/core/src/systemcontrol.c +++ b/core/src/systemcontrol.c @@ -133,7 +133,7 @@ typedef enum { InitializeScenario_0, ConnectObject_0, DisconnectObject_0, GetServerParameterList_0, SetServerParameter_2, GetServerParameter_1, DownloadFile_1, UploadFile_4, CheckFileDirectoryExist_1, GetRootDirectoryContent_0, GetDirectoryContent_1, DeleteTrajectory_1, DeleteGeofence_1, - ClearTrajectories_0, ClearGeofences_0, + ClearTrajectories_0, ClearGeofences_0, CreateDirectory_1, GetTestOrigin_0, replay_1, control_0, Exit_0, start_ext_trigg_1, nocommand } SystemControlCommand_t; From f768872e73a05ff370c694ce31fe5d01998db76c Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Mon, 23 Mar 2020 11:36:03 +0100 Subject: [PATCH 384/523] Added default values --- core/inc/util.h | 56 ++++++++++++++++++++++++++++------------ core/src/objectcontrol.c | 8 +++--- util | 2 +- 3 files changed, 44 insertions(+), 22 deletions(-) diff --git a/core/inc/util.h b/core/inc/util.h index 69918da0c..c73482e56 100644 --- a/core/inc/util.h +++ b/core/inc/util.h @@ -39,6 +39,28 @@ extern "C"{ -- Defines ------------------------------------------------------------*/ #define MaestroVersion "0.4.1" +#define DEFAULT_ORIGO_LAT 57.777073115 +#define DEFAULT_ORIGO_LOG 12.781295498333 +#define DEFAULT_ORIGO_ALT 193.114 +#define DEFAULT_VISUALISATION_SERVER_NAME 0 +#define DEFAULT_FORCE_OBJECT_TO_LOCALHOST 0 +#define DEFAULT_ASP_MAX_TIME_DIFF 2.5 +#define DEFAULT_ASP_MAX_TRAJ_DIFF 1.52 +#define DEFAULT_ASP_STEP_BACK_COUNT 0 +#define DEFAULT_ASP_FILTER_LEVEL 5 +#define DEFAULT_ASP_MAX_DELTA_TIME 0.05 +#define DEFAULT_TIMESERVER_IP 10.130.23.110 +#define DEFAULT_TIME_SERVER_PORT 53000 +#define DEFAULT_SIMULATOR_SIMULATOR_IP 0 +#define DEFAULT_SIMULATOR_TCP_PORT 53001 +#define DEFAULT_SIMULATOR_UDP_PORT 53002 +#define DEFAULT_SIMULATOR_MODE 1 +#define DEFAULT_VOIL_RECEIVERS 0 +#define DEFAULT_DTM_RECEIVERS 0 +#define DEFAULT_SUPERVISOR_IP 0 +#define DEFAULT_SUPERVISOR_TCP_PORT 53010 +#define DEFAULT_RVSS_CONF 3 +#define DEFAULT_RVSS_RATE 1 #define MBUS_MAX_DATALEN (MQ_MSG_SIZE-1) // Message queue data minus one byte for the command @@ -327,35 +349,35 @@ typedef struct //U8 STRTData[100]; //U8 OSEMSizeU8; //U8 OSEMData[100]; - volatile dbl OriginLatitudeDbl; + volatile dbl OriginLatitudeDbl = DEFAULT_ORIGO_LAT; C8 OriginLatitudeC8[DD_CONTROL_BUFFER_SIZE_20]; - volatile dbl OriginLongitudeDbl; + volatile dbl OriginLongitudeDbl = DEFAULT_ORIGO_LOG; C8 OriginLongitudeC8[DD_CONTROL_BUFFER_SIZE_20]; - volatile dbl OriginAltitudeDbl; + volatile dbl OriginAltitudeDbl = DEFAULT_ORIGO_ALT; C8 OriginAltitudeC8[DD_CONTROL_BUFFER_SIZE_20]; volatile U32 VisualizationServerU32; C8 VisualizationServerC8[DD_CONTROL_BUFFER_SIZE_20]; - volatile U8 ForceObjectToLocalhostU8; - volatile dbl ASPMaxTimeDiffDbl; - volatile dbl ASPMaxTrajDiffDbl; - volatile U32 ASPStepBackCountU32; - volatile dbl ASPFilterLevelDbl; - volatile dbl ASPMaxDeltaTimeDbl; + volatile U8 ForceObjectToLocalhostU8 = DEFAULT_FORCE_OBJECT_TO_LOCALHOST; + volatile dbl ASPMaxTimeDiffDbl = DEFAULT_ASP_MAX_TIME_DIFF; + volatile dbl ASPMaxTrajDiffDbl = DEFAULT_ASP_MAX_TRAJ_DIFF; + volatile U32 ASPStepBackCountU32 = DEFAULT_ASP_STEP_BACK_COUNT; + volatile dbl ASPFilterLevelDbl = DEFAULT_ASP_FILTER_LEVEL; + volatile dbl ASPMaxDeltaTimeDbl = DEFAULT_ASP_MAX_DELTA_TIME; volatile U32 TimeServerIPU32; C8 TimeServerIPC8[DD_CONTROL_BUFFER_SIZE_20]; - volatile U16 TimeServerPortU16; - volatile U32 SimulatorIPU32; + volatile U16 TimeServerPortU16 = DEFAULT_TIME_SERVER_PORT; + volatile U32 SimulatorIPU32 = DEFAULT_SIMULATOR_SIMULATOR_IP; C8 SimulatorIPC8[DD_CONTROL_BUFFER_SIZE_20]; - volatile U16 SimulatorTCPPortU16; - volatile U16 SimulatorUDPPortU16; - volatile U8 SimulatorModeU8; + volatile U16 SimulatorTCPPortU16 = DEFAULT_SIMULATOR_TCP_PORT; + volatile U16 SimulatorUDPPortU16 = DEFAULT_SIMULATOR_UDP_PORT; + volatile U8 SimulatorModeU8 = DEFAULT_SIMULATOR_MODE; C8 VOILReceiversC8[DD_CONTROL_BUFFER_SIZE_1024]; C8 DTMReceiversC8[DD_CONTROL_BUFFER_SIZE_1024]; volatile U32 ExternalSupervisorIPU32; C8 ExternalSupervisorIPC8[DD_CONTROL_BUFFER_SIZE_20]; - volatile U16 SupervisorTCPPortU16; - U32 DataDictionaryRVSSConfigU32; - U32 DataDictionaryRVSSRateU8; + volatile U16 SupervisorTCPPortU16 = DEFAULT_SUPERVISOR_TCP_PORT; + U32 DataDictionaryRVSSConfigU32 = DEFAULT_RVSS_CONF; + U32 DataDictionaryRVSSRateU8 = DEFAULT_RVSS_RATE; ASPType ASPData; C8 MiscDataC8[DD_CONTROL_BUFFER_SIZE_1024]; volatile OBCState_t OBCStateU8; diff --git a/core/src/objectcontrol.c b/core/src/objectcontrol.c index 3bd2c1904..cf253f1de 100644 --- a/core/src/objectcontrol.c +++ b/core/src/objectcontrol.c @@ -188,10 +188,10 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { C8 OriginLatitude[SMALL_BUFFER_SIZE_0], OriginLongitude[SMALL_BUFFER_SIZE_0], OriginAltitude[SMALL_BUFFER_SIZE_0], OriginHeading[SMALL_BUFFER_SIZE_0]; C8 TextBuffer[SMALL_BUFFER_SIZE_0]; - dbl OriginLatitudeDbl; - dbl OriginLongitudeDbl; - dbl OriginAltitudeDbl; - dbl OriginHeadingDbl; + dbl OriginLatitudeDbl = 0; + dbl OriginLongitudeDbl = 0; + dbl OriginAltitudeDbl = 0; + dbl OriginHeadingDbl = 0; C8 pcSendBuffer[MBUS_MAX_DATALEN]; C8 ObjectPort[SMALL_BUFFER_SIZE_0]; MonitorDataType monitorData; diff --git a/util b/util index 19a42f2de..80f2fd821 160000 --- a/util +++ b/util @@ -1 +1 @@ -Subproject commit 19a42f2dec489753d31322e0527de876c1b18660 +Subproject commit 80f2fd821c909596e3caa6c23e8d4a1e3aedae82 From 677eeeca6114095f432d8c3779fc9dff1564598b Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Mon, 23 Mar 2020 12:17:04 +0100 Subject: [PATCH 385/523] moved stuff --- core/inc/util.h | 35 ++++++++++++++++++----------------- 1 file changed, 18 insertions(+), 17 deletions(-) diff --git a/core/inc/util.h b/core/inc/util.h index c73482e56..e9a03874c 100644 --- a/core/inc/util.h +++ b/core/inc/util.h @@ -39,6 +39,7 @@ extern "C"{ -- Defines ------------------------------------------------------------*/ #define MaestroVersion "0.4.1" + #define DEFAULT_ORIGO_LAT 57.777073115 #define DEFAULT_ORIGO_LOG 12.781295498333 #define DEFAULT_ORIGO_ALT 193.114 @@ -349,35 +350,35 @@ typedef struct //U8 STRTData[100]; //U8 OSEMSizeU8; //U8 OSEMData[100]; - volatile dbl OriginLatitudeDbl = DEFAULT_ORIGO_LAT; + volatile dbl OriginLatitudeDbl; C8 OriginLatitudeC8[DD_CONTROL_BUFFER_SIZE_20]; - volatile dbl OriginLongitudeDbl = DEFAULT_ORIGO_LOG; + volatile dbl OriginLongitudeDbl; C8 OriginLongitudeC8[DD_CONTROL_BUFFER_SIZE_20]; - volatile dbl OriginAltitudeDbl = DEFAULT_ORIGO_ALT; + volatile dbl OriginAltitudeDbl; C8 OriginAltitudeC8[DD_CONTROL_BUFFER_SIZE_20]; volatile U32 VisualizationServerU32; C8 VisualizationServerC8[DD_CONTROL_BUFFER_SIZE_20]; - volatile U8 ForceObjectToLocalhostU8 = DEFAULT_FORCE_OBJECT_TO_LOCALHOST; - volatile dbl ASPMaxTimeDiffDbl = DEFAULT_ASP_MAX_TIME_DIFF; - volatile dbl ASPMaxTrajDiffDbl = DEFAULT_ASP_MAX_TRAJ_DIFF; - volatile U32 ASPStepBackCountU32 = DEFAULT_ASP_STEP_BACK_COUNT; - volatile dbl ASPFilterLevelDbl = DEFAULT_ASP_FILTER_LEVEL; - volatile dbl ASPMaxDeltaTimeDbl = DEFAULT_ASP_MAX_DELTA_TIME; + volatile U8 ForceObjectToLocalhostU8; + volatile dbl ASPMaxTimeDiffDbl; + volatile dbl ASPMaxTrajDiffDbl; + volatile U32 ASPStepBackCountU32; + volatile dbl ASPFilterLevelDbl; + volatile dbl ASPMaxDeltaTimeDbl; volatile U32 TimeServerIPU32; C8 TimeServerIPC8[DD_CONTROL_BUFFER_SIZE_20]; - volatile U16 TimeServerPortU16 = DEFAULT_TIME_SERVER_PORT; - volatile U32 SimulatorIPU32 = DEFAULT_SIMULATOR_SIMULATOR_IP; + volatile U16 TimeServerPortU16; + volatile U32 SimulatorIPU32; C8 SimulatorIPC8[DD_CONTROL_BUFFER_SIZE_20]; - volatile U16 SimulatorTCPPortU16 = DEFAULT_SIMULATOR_TCP_PORT; - volatile U16 SimulatorUDPPortU16 = DEFAULT_SIMULATOR_UDP_PORT; - volatile U8 SimulatorModeU8 = DEFAULT_SIMULATOR_MODE; + volatile U16 SimulatorTCPPortU16; + volatile U16 SimulatorUDPPortU16; + volatile U8 SimulatorModeU8; C8 VOILReceiversC8[DD_CONTROL_BUFFER_SIZE_1024]; C8 DTMReceiversC8[DD_CONTROL_BUFFER_SIZE_1024]; volatile U32 ExternalSupervisorIPU32; C8 ExternalSupervisorIPC8[DD_CONTROL_BUFFER_SIZE_20]; - volatile U16 SupervisorTCPPortU16 = DEFAULT_SUPERVISOR_TCP_PORT; - U32 DataDictionaryRVSSConfigU32 = DEFAULT_RVSS_CONF; - U32 DataDictionaryRVSSRateU8 = DEFAULT_RVSS_RATE; + volatile U16 SupervisorTCPPortU16; + U32 DataDictionaryRVSSConfigU32; + U32 DataDictionaryRVSSRateU8; ASPType ASPData; C8 MiscDataC8[DD_CONTROL_BUFFER_SIZE_1024]; volatile OBCState_t OBCStateU8; From 6b2c2d54f1617bb51d8dcec63c6297cb43369e8a Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Mon, 23 Mar 2020 12:55:17 +0100 Subject: [PATCH 386/523] adding default defines --- core/src/objectcontrol.c | 20 ++++++++++---------- core/src/timecontrol.c | 2 +- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/core/src/objectcontrol.c b/core/src/objectcontrol.c index cf253f1de..b1816e584 100644 --- a/core/src/objectcontrol.c +++ b/core/src/objectcontrol.c @@ -185,13 +185,13 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { flt SpaceArr[MAX_OBJECTS][TRAJECTORY_FILE_MAX_ROWS]; flt TimeArr[MAX_OBJECTS][TRAJECTORY_FILE_MAX_ROWS]; SpaceTime SpaceTimeArr[MAX_OBJECTS][TRAJECTORY_FILE_MAX_ROWS]; - C8 OriginLatitude[SMALL_BUFFER_SIZE_0], OriginLongitude[SMALL_BUFFER_SIZE_0], + C8 OriginLatitude[SMALL_BUFFER_SIZE_0], OriginLongitude[SMALL_BUFFER_SIZE_0], OriginAltitude[SMALL_BUFFER_SIZE_0], OriginHeading[SMALL_BUFFER_SIZE_0]; C8 TextBuffer[SMALL_BUFFER_SIZE_0]; - dbl OriginLatitudeDbl = 0; - dbl OriginLongitudeDbl = 0; - dbl OriginAltitudeDbl = 0; - dbl OriginHeadingDbl = 0; + dbl OriginLatitudeDbl = DEFAULT_ORIGO_LAT; + dbl OriginLongitudeDbl = DEFAULT_ORIGO_LOG; + dbl OriginAltitudeDbl = DEFAULT_ORIGO_ALT; + dbl OriginHeadingDbl; C8 pcSendBuffer[MBUS_MAX_DATALEN]; C8 ObjectPort[SMALL_BUFFER_SIZE_0]; MonitorDataType monitorData; @@ -208,12 +208,12 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { AdaptiveSyncPoint ASP[MAX_ADAPTIVE_SYNC_POINTS]; I32 SyncPointCount = 0; I32 SearchStartIndex = 0; - dbl ASPMaxTimeDiffDbl = 0; - dbl ASPMaxTrajDiffDbl = 0; - dbl ASPFilterLevelDbl = 0; - dbl ASPMaxDeltaTimeDbl = 0; + dbl ASPMaxTimeDiffDbl = DEFAULT_ASP_MAX_TIME_DIFF; + dbl ASPMaxTrajDiffDbl = DEFAULT_ASP_MAX_TRAJ_DIFF; + dbl ASPFilterLevelDbl = DEFAULT_ASP_FILTER_LEVEL; + dbl ASPMaxDeltaTimeDbl = DEFAULT_ASP_MAX_DELTA_TIME; I32 ASPDebugRate = 1; - I32 ASPStepBackCount = 0; + I32 ASPStepBackCount = 0; char confDirectoryPath[MAX_FILE_PATH]; ControlCenterStatusType objectControlServerStatus = CONTROL_CENTER_STATUS_INIT; diff --git a/core/src/timecontrol.c b/core/src/timecontrol.c index b5d8f372e..f69e76c01 100644 --- a/core/src/timecontrol.c +++ b/core/src/timecontrol.c @@ -86,7 +86,7 @@ void timecontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { C8 TextBufferC8[TIME_CONTROL_HOSTNAME_BUFFER_SIZE]; C8 ServerIPC8[TIME_CONTROL_HOSTNAME_BUFFER_SIZE]; - U16 ServerPortU16; + U16 ServerPortU16 = DEFAULT_TIME_SERVER_PORT; I32 SocketfdI32 = -1; struct sockaddr_in time_addr; const struct timespec mqEmptyPollPeriod = { TC_SLEEP_TIME_EMPTY_MQ_S, TC_SLEEP_TIME_EMPTY_MQ_NS }; From c0de6986df9eb9abd8fbb82a0a11aed74e19bb01 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 23 Mar 2020 14:10:10 +0100 Subject: [PATCH 387/523] Wrote shm file based init function --- core/inc/datadictionary.h | 2 +- core/src/datadictionary.c | 72 +++++++++++++++++++++++++-------------- 2 files changed, 47 insertions(+), 27 deletions(-) diff --git a/core/inc/datadictionary.h b/core/inc/datadictionary.h index 1d8a23d57..318173a22 100644 --- a/core/inc/datadictionary.h +++ b/core/inc/datadictionary.h @@ -122,7 +122,7 @@ ReadWriteAccess_t DataDictionarySetOBCStateU8(GSDType *GSD, OBCState_t OBCState) OBCState_t DataDictionaryGetOBCStateU8(GSDType *GSD); ReadWriteAccess_t DataDictionaryFreeMONR(GSDType *GSD); -ReadWriteAccess_t DataDictionaryInitMONR(GSDType *GSD); +ReadWriteAccess_t DataDictionaryInitMONR(); ReadWriteAccess_t DataDictionarySetMONR(GSDType *GSD, const MonitorDataType * MONR, const U32 TransmitterId); ReadWriteAccess_t DataDictionaryGetMONR(GSDType *GSD, MonitorDataType * MONR, const U32 TransmitterId); diff --git a/core/src/datadictionary.c b/core/src/datadictionary.c index 86ed9b401..af8ad525a 100644 --- a/core/src/datadictionary.c +++ b/core/src/datadictionary.c @@ -49,6 +49,10 @@ static pthread_mutex_t OBCStateMutex = PTHREAD_MUTEX_INITIALIZER; static pthread_mutex_t MONRMutex = PTHREAD_MUTEX_INITIALIZER; static pthread_mutex_t numberOfObjectsMutex = PTHREAD_MUTEX_INITIALIZER; +#define NUMBER_OF_OBJECTS_FILENAME "/NUMBER_OF_OBJECTS.mem" +#define MONR_DATA_FILENAME "/MONR.mem" +static unsigned int* numberOfObjectsMemory = NULL; +static MonitorDataType* MONRMemory = NULL; /*------------------------------------------------------------ @@ -1673,39 +1677,55 @@ OBCState_t DataDictionaryGetOBCStateU8(GSDType * GSD) { * \param GSD Pointer to shared allocated memory * \return Result according to ::ReadWriteAccess_t */ -ReadWriteAccess_t DataDictionaryInitMONR(GSDType * GSD) { - ReadWriteAccess_t Res; - - Res = WRITE_OK; - char filePath[PATH_MAX]; - int fd; - struct stat st; +ReadWriteAccess_t DataDictionaryInitMONR() { - pthread_mutex_lock(&MONRMutex); - sprintf(filePath, "%smonrMessageMemory.mem", SHARED_MEMORY_PATH); - fd = open(filePath, O_RDWR | O_CREAT, S_IRUSR | S_IWUSR); - stat(filePath, &st); + int fdCount, fdObjs; -// this memory does not change size as more MONR messages are added, and it is unclear where in the memory stuff is being written - lseek(fd, (sizeof (MonitorDataType)) - 1, SEEK_SET); - write(fd, "", 1); + fdCount = shm_open(NUMBER_OF_OBJECTS_FILENAME, O_RDWR | O_CREAT, S_IRUSR | S_IWUSR); + if (fdObjs == -1) { + LogMessage(LOG_LEVEL_ERROR, "Failed to create shared memory"); + return UNDEFINED; + } + fdObjs = shm_open(MONR_DATA_FILENAME, O_RDWR | O_CREAT, S_IRUSR | S_IWUSR); + if (fdObjs == -1) { + LogMessage(LOG_LEVEL_ERROR, "Failed to create shared memory"); + shm_unlink(NUMBER_OF_OBJECTS_FILENAME); + return UNDEFINED; + } - stat(filePath, &st); + if (ftruncate(fdCount, sizeof (unsigned int)) == -1) { + LogMessage(LOG_LEVEL_ERROR, "File truncation error"); + shm_unlink(NUMBER_OF_OBJECTS_FILENAME); + shm_unlink(MONR_DATA_FILENAME); + return UNDEFINED; + } - // Map memory to created file - GSD->MonrMessages = - (MonitorDataType *) mmap(NULL, (sizeof (MonitorDataType)), PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0); + if (ftruncate(fdObjs, sizeof (unsigned int)) == -1) { + LogMessage(LOG_LEVEL_ERROR, "File truncation error"); + shm_unlink(NUMBER_OF_OBJECTS_FILENAME); + shm_unlink(MONR_DATA_FILENAME); + return UNDEFINED; + } - if (GSD->MonrMessages == MAP_FAILED) { - LogPrint(LOG_LEVEL_ERROR, "mmap failed: %s", strerror(errno)); - close(fd); + numberOfObjectsMemory = mmap(NULL, sizeof (unsigned int), PROT_WRITE, MAP_SHARED, fdCount, 0); + if (numberOfObjectsMemory == MAP_FAILED) { + LogMessage(LOG_LEVEL_ERROR, "Memory mapping error"); + shm_unlink(NUMBER_OF_OBJECTS_FILENAME); + shm_unlink(MONR_DATA_FILENAME); + return UNDEFINED; } - else { - LogMessage(LOG_LEVEL_INFO, "Init MonrMessage memory"); + *numberOfObjectsMemory = 0; + + MONRMemory = mmap(NULL, sizeof (MonitorDataType) * (*numberOfObjectsMemory), + PROT_WRITE, MAP_SHARED, fdObjs, 0); + if (MONRMemory == MAP_FAILED) { + LogMessage(LOG_LEVEL_ERROR, "Memory mapping error"); + munmap(numberOfObjectsMemory, sizeof (unsigned int)); + shm_unlink(NUMBER_OF_OBJECTS_FILENAME); + shm_unlink(MONR_DATA_FILENAME); + return UNDEFINED; } - close(fd); - pthread_mutex_unlock(&MONRMutex); - return Res; + return WRITE_OK; } /*! From 02303837f11d3863ee6b689ad8609ceba50dce70 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 23 Mar 2020 14:26:27 +0100 Subject: [PATCH 388/523] Modified numberOfObject setter and getter functions --- core/inc/datadictionary.h | 4 +-- core/src/datadictionary.c | 53 ++++++++++++++++++++++++++------------- 2 files changed, 38 insertions(+), 19 deletions(-) diff --git a/core/inc/datadictionary.h b/core/inc/datadictionary.h index 318173a22..ef9380939 100644 --- a/core/inc/datadictionary.h +++ b/core/inc/datadictionary.h @@ -126,8 +126,8 @@ ReadWriteAccess_t DataDictionaryInitMONR(); ReadWriteAccess_t DataDictionarySetMONR(GSDType *GSD, const MonitorDataType * MONR, const U32 TransmitterId); ReadWriteAccess_t DataDictionaryGetMONR(GSDType *GSD, MonitorDataType * MONR, const U32 TransmitterId); -ReadWriteAccess_t DataDictionarySetNumberOfObjectsU8(GSDType * GSD, U32 *numberOfObjects); -ReadWriteAccess_t DataDictionaryGetNumberOfObjectsU8(GSDType * GSD, U32 *numberOfObjects); +ReadWriteAccess_t DataDictionarySetNumberOfObjectsU8(const uint32_t numberOfObjects); +ReadWriteAccess_t DataDictionaryGetNumberOfObjectsU8(uint32_t *numberOfObjects); ReadWriteAccess_t DataDictionaryConstructor(GSDType *GSD); diff --git a/core/src/datadictionary.c b/core/src/datadictionary.c index af8ad525a..f3e88cf2f 100644 --- a/core/src/datadictionary.c +++ b/core/src/datadictionary.c @@ -51,6 +51,8 @@ static pthread_mutex_t numberOfObjectsMutex = PTHREAD_MUTEX_INITIALIZER; #define NUMBER_OF_OBJECTS_FILENAME "/NUMBER_OF_OBJECTS.mem" #define MONR_DATA_FILENAME "/MONR.mem" + +static int fdObjectCount, fdObjectMONRData; static unsigned int* numberOfObjectsMemory = NULL; static MonitorDataType* MONRMemory = NULL; @@ -1679,35 +1681,33 @@ OBCState_t DataDictionaryGetOBCStateU8(GSDType * GSD) { */ ReadWriteAccess_t DataDictionaryInitMONR() { - int fdCount, fdObjs; - - fdCount = shm_open(NUMBER_OF_OBJECTS_FILENAME, O_RDWR | O_CREAT, S_IRUSR | S_IWUSR); - if (fdObjs == -1) { + fdObjectCount = shm_open(NUMBER_OF_OBJECTS_FILENAME, O_RDWR | O_CREAT, S_IRUSR | S_IWUSR); + if (fdObjectMONRData == -1) { LogMessage(LOG_LEVEL_ERROR, "Failed to create shared memory"); return UNDEFINED; } - fdObjs = shm_open(MONR_DATA_FILENAME, O_RDWR | O_CREAT, S_IRUSR | S_IWUSR); - if (fdObjs == -1) { + fdObjectMONRData = shm_open(MONR_DATA_FILENAME, O_RDWR | O_CREAT, S_IRUSR | S_IWUSR); + if (fdObjectMONRData == -1) { LogMessage(LOG_LEVEL_ERROR, "Failed to create shared memory"); shm_unlink(NUMBER_OF_OBJECTS_FILENAME); return UNDEFINED; } - if (ftruncate(fdCount, sizeof (unsigned int)) == -1) { + if (ftruncate(fdObjectCount, sizeof (unsigned int)) == -1) { LogMessage(LOG_LEVEL_ERROR, "File truncation error"); shm_unlink(NUMBER_OF_OBJECTS_FILENAME); shm_unlink(MONR_DATA_FILENAME); return UNDEFINED; } - if (ftruncate(fdObjs, sizeof (unsigned int)) == -1) { + if (ftruncate(fdObjectMONRData, sizeof (unsigned int)) == -1) { LogMessage(LOG_LEVEL_ERROR, "File truncation error"); shm_unlink(NUMBER_OF_OBJECTS_FILENAME); shm_unlink(MONR_DATA_FILENAME); return UNDEFINED; } - numberOfObjectsMemory = mmap(NULL, sizeof (unsigned int), PROT_WRITE, MAP_SHARED, fdCount, 0); + numberOfObjectsMemory = mmap(NULL, sizeof (unsigned int), PROT_WRITE, MAP_SHARED, fdObjectCount, 0); if (numberOfObjectsMemory == MAP_FAILED) { LogMessage(LOG_LEVEL_ERROR, "Memory mapping error"); shm_unlink(NUMBER_OF_OBJECTS_FILENAME); @@ -1717,7 +1717,7 @@ ReadWriteAccess_t DataDictionaryInitMONR() { *numberOfObjectsMemory = 0; MONRMemory = mmap(NULL, sizeof (MonitorDataType) * (*numberOfObjectsMemory), - PROT_WRITE, MAP_SHARED, fdObjs, 0); + PROT_WRITE, MAP_SHARED, fdObjectMONRData, 0); if (MONRMemory == MAP_FAILED) { LogMessage(LOG_LEVEL_ERROR, "Memory mapping error"); munmap(numberOfObjectsMemory, sizeof (unsigned int)); @@ -1808,14 +1808,26 @@ ReadWriteAccess_t DataDictionaryFreeMONR(GSDType * GSD) { * \param numberOfobjects number of objects in a test * \return Result according to ::ReadWriteAccess_t */ -ReadWriteAccess_t DataDictionarySetNumberOfObjectsU8(GSDType * GSD, U32 * numberOfObjects) { - ReadWriteAccess_t Res; +ReadWriteAccess_t DataDictionarySetNumberOfObjectsU8(const uint32_t numberOfObjects) { + ReadWriteAccess_t result = WRITE_OK; + if (numberOfObjectsMemory == NULL) { + errno = EINVAL; + LogMessage(LOG_LEVEL_ERROR, "Shared memory not initialized"); + return UNDEFINED; + } - Res = WRITE_OK; pthread_mutex_lock(&numberOfObjectsMutex); - GSD->numberOfObjects = *numberOfObjects; + if (ftruncate(fdObjectMONRData, numberOfObjects * sizeof (fdObjectMONRData)) == -1) { + LogMessage(LOG_LEVEL_ERROR, "File truncation error"); + result = UNDEFINED; + } + else { + *numberOfObjectsMemory = numberOfObjects; + LogMessage(LOG_LEVEL_DEBUG, "Modified shared memory to hold MONR data for %d objects", *numberOfObjectsMemory); + } pthread_mutex_unlock(&numberOfObjectsMutex); - return Res; + + return result; } /*! @@ -1824,10 +1836,17 @@ ReadWriteAccess_t DataDictionarySetNumberOfObjectsU8(GSDType * GSD, U32 * number * \param numberOfobjects number of objects in a test * \return Current object control state according to ::OBCState_t */ -ReadWriteAccess_t DataDictionaryGetNumberOfObjectsU8(GSDType * GSD, U32 * numberOfObjects) { +ReadWriteAccess_t DataDictionaryGetNumberOfObjectsU8(uint32_t * numberOfObjects) { + if (numberOfObjectsMemory == NULL) { + errno = EINVAL; + LogMessage(LOG_LEVEL_ERROR, "Shared memory not initialized"); + return UNDEFINED; + } + pthread_mutex_lock(&numberOfObjectsMutex); - *numberOfObjects = GSD->numberOfObjects; + *numberOfObjects = *numberOfObjectsMemory; pthread_mutex_unlock(&numberOfObjectsMutex); + return READ_OK; } From 82c0b5427df338c0bf86bee79d2bd3945304b76f Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 23 Mar 2020 16:09:34 +0100 Subject: [PATCH 389/523] Refactor --- core/src/main.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/core/src/main.c b/core/src/main.c index 7038d3746..20b2d5d02 100644 --- a/core/src/main.c +++ b/core/src/main.c @@ -129,7 +129,7 @@ int main(int argc, char *argv[]) { util_error("Unable to initialize shared memory space"); } else { - LogMessage(LOG_LEVEL_INFO, "Data dictionary succesfully initiated"); + LogMessage(LOG_LEVEL_INFO, "Data dictionary succesfully initialized"); } LogMessage(LOG_LEVEL_INFO, "About to enter mq init"); From 35574a6acaa4a253c8990e2cc17ef4849eef55e8 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 23 Mar 2020 16:10:14 +0100 Subject: [PATCH 390/523] Implemented auto-expanding of MONR shared memory --- core/inc/datadictionary.h | 6 +- core/src/datadictionary.c | 152 +++++++++++++++++++++++++++----------- 2 files changed, 113 insertions(+), 45 deletions(-) diff --git a/core/inc/datadictionary.h b/core/inc/datadictionary.h index ef9380939..15d4518e1 100644 --- a/core/inc/datadictionary.h +++ b/core/inc/datadictionary.h @@ -121,10 +121,10 @@ ReadWriteAccess_t DataDictionaryGetMiscDataC8(GSDType *GSD, C8 *MiscData, U32 Bu ReadWriteAccess_t DataDictionarySetOBCStateU8(GSDType *GSD, OBCState_t OBCState); OBCState_t DataDictionaryGetOBCStateU8(GSDType *GSD); -ReadWriteAccess_t DataDictionaryFreeMONR(GSDType *GSD); +ReadWriteAccess_t DataDictionaryFreeMONR(); ReadWriteAccess_t DataDictionaryInitMONR(); -ReadWriteAccess_t DataDictionarySetMONR(GSDType *GSD, const MonitorDataType * MONR, const U32 TransmitterId); -ReadWriteAccess_t DataDictionaryGetMONR(GSDType *GSD, MonitorDataType * MONR, const U32 TransmitterId); +ReadWriteAccess_t DataDictionarySetMONR(const MonitorDataType * MONR, const uint32_t TransmitterId); +ReadWriteAccess_t DataDictionaryGetMONR(MonitorDataType * MONR, const uint32_t TransmitterId); ReadWriteAccess_t DataDictionarySetNumberOfObjectsU8(const uint32_t numberOfObjects); ReadWriteAccess_t DataDictionaryGetNumberOfObjectsU8(uint32_t *numberOfObjects); diff --git a/core/src/datadictionary.c b/core/src/datadictionary.c index f3e88cf2f..ea8e32e48 100644 --- a/core/src/datadictionary.c +++ b/core/src/datadictionary.c @@ -99,6 +99,7 @@ ReadWriteAccess_t DataDictionaryConstructor(GSDType * GSD) { Res = Res == READ_OK ? DataDictionaryInitRVSSRateU8(GSD) : Res; Res = Res == READ_OK ? DataDictionaryInitSupervisorTCPPortU16(GSD) : Res; Res = Res == READ_OK ? DataDictionaryInitMiscDataC8(GSD) : Res; + Res = Res == READ_OK ? DataDictionaryInitMONR() : Res; DataDictionarySetOBCStateU8(GSD, OBC_STATE_UNDEFINED); @@ -1676,7 +1677,6 @@ OBCState_t DataDictionaryGetOBCStateU8(GSDType * GSD) { /*! * \brief DataDictionaryInitMONR inits a data structure for saving object monr - * \param GSD Pointer to shared allocated memory * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionaryInitMONR() { @@ -1714,7 +1714,6 @@ ReadWriteAccess_t DataDictionaryInitMONR() { shm_unlink(MONR_DATA_FILENAME); return UNDEFINED; } - *numberOfObjectsMemory = 0; MONRMemory = mmap(NULL, sizeof (MonitorDataType) * (*numberOfObjectsMemory), PROT_WRITE, MAP_SHARED, fdObjectMONRData, 0); @@ -1725,77 +1724,146 @@ ReadWriteAccess_t DataDictionaryInitMONR() { shm_unlink(MONR_DATA_FILENAME); return UNDEFINED; } - return WRITE_OK; + return READ_WRITE_OK; } /*! * \brief DataDictionarySetMONR Parses input variable and sets variable to corresponding value - * \param GSD Pointer to shared allocated memory * \param MONRdata Monitor data * \param transmitterId requested object transmitterId * \return Result according to ::ReadWriteAccess_t */ -ReadWriteAccess_t DataDictionarySetMONR(GSDType * GSD, const MonitorDataType * MONR, const U32 transmitterId) { - ReadWriteAccess_t Res; +ReadWriteAccess_t DataDictionarySetMONR(const MonitorDataType * MONR, const uint32_t transmitterId) { - Res = WRITE_OK; - pthread_mutex_lock(&MONRMutex); - if (GSD->MonrMessages != NULL && transmitterId < GSD->numberOfObjects) { - GSD->MonrMessages[transmitterId] = *MONR; + ReadWriteAccess_t result; + + if (MONRMemory == NULL || numberOfObjectsMemory == NULL) { + errno = EINVAL; + LogMessage(LOG_LEVEL_ERROR, "Shared memory not initialized"); + return UNDEFINED; } - else { - Res = UNDEFINED; - LogPrint(LOG_LEVEL_ERROR, "Unable to write MonrMessage in DataDictionary"); + if (MONR == NULL) { + errno = EINVAL; + LogMessage(LOG_LEVEL_ERROR, "Shared memory input pointer error"); + return UNDEFINED; + } + if (transmitterId == 0) { + errno = EINVAL; + LogMessage(LOG_LEVEL_ERROR, "Transmitter ID 0 is reserved"); + return UNDEFINED; } + pthread_mutex_lock(&numberOfObjectsMutex); + pthread_mutex_lock(&MONRMutex); + result = PARAMETER_NOTFOUND; + for (uint32_t i = 0; i < *numberOfObjectsMemory; ++i) { + if (MONRMemory[i].ClientID == transmitterId) { + memcpy(&MONRMemory[i], MONR, sizeof (MonitorDataType)); + result = WRITE_OK; + } + } + + if (result == PARAMETER_NOTFOUND) { + // Searched for unused memory space and place monitor data there + LogMessage(LOG_LEVEL_INFO, "Received first monitor data from transmitter ID %u", transmitterId); + for (uint32_t i = 0; i < *numberOfObjectsMemory; ++i) { + if (MONRMemory[i].ClientID == 0) { + memcpy(&MONRMemory[i], MONR, sizeof (MonitorDataType)); + result = WRITE_OK; + } + } + + // No uninitialized memory space found - create new + if (result == PARAMETER_NOTFOUND) { + if (ftruncate(fdObjectMONRData, (*numberOfObjectsMemory + 1) * sizeof (MonitorDataType)) == -1) { + LogMessage(LOG_LEVEL_ERROR, "File truncation error"); + result = UNDEFINED; + } + else { + *numberOfObjectsMemory += 1; + LogMessage(LOG_LEVEL_DEBUG, "Modified shared memory to hold MONR data for %d objects", *numberOfObjectsMemory); + } + } + } pthread_mutex_unlock(&MONRMutex); + pthread_mutex_unlock(&numberOfObjectsMutex); - return Res; + return result; } /*! * \brief DataDictionaryGetMONR Reads variable from shared memory - * \param GSD Pointer to shared allocated memory * \param MONRdata Return variable pointer * \param transmitterId requested object transmitterId * \return Result according to ::ReadWriteAccess_t */ -ReadWriteAccess_t DataDictionaryGetMONR(GSDType * GSD, MonitorDataType * MONR, const U32 transmitterId) { - ReadWriteAccess_t Res; +ReadWriteAccess_t DataDictionaryGetMONR(MonitorDataType * MONR, const uint32_t transmitterId) { + ReadWriteAccess_t result = PARAMETER_NOTFOUND; - Res = READ_OK; + if (MONR == NULL) { + errno = EINVAL; + LogMessage(LOG_LEVEL_ERROR, "Shared memory input pointer error"); + return UNDEFINED; + } + if (transmitterId == 0) { + errno = EINVAL; + LogMessage(LOG_LEVEL_ERROR, "Transmitter ID 0 is reserved"); + return UNDEFINED; + } + pthread_mutex_lock(&numberOfObjectsMutex); pthread_mutex_lock(&MONRMutex); - if (GSD->MonrMessages != NULL && transmitterId < GSD->numberOfObjects) { - *MONR = GSD->MonrMessages[transmitterId]; - } - else { - Res = UNDEFINED; - LogPrint(LOG_LEVEL_ERROR, "Unable to read MonrMessage in DataDictionary"); + for (uint32_t i = 0; i < *numberOfObjectsMemory; ++i) { + if (MONRMemory[i].ClientID == transmitterId) { + memcpy(MONR, &MONRMemory[i], sizeof (MonitorDataType)); + result = READ_OK; + } } pthread_mutex_unlock(&MONRMutex); - return Res; + pthread_mutex_unlock(&numberOfObjectsMutex); + + if (result == PARAMETER_NOTFOUND) { + LogMessage(LOG_LEVEL_WARNING, "Unable to find monitor data for transmitter ID %u", transmitterId); + } + + return result; } /*! - * \brief DataDictionaryInitMONR inits a data structure for saving object monr - * \param GSD Pointer to shared allocated memory + * \brief DataDictionaryInitMONR initializes a data structure for saving object monitor data * \return Result according to ::ReadWriteAccess_t */ -ReadWriteAccess_t DataDictionaryFreeMONR(GSDType * GSD) { - ReadWriteAccess_t Res; +ReadWriteAccess_t DataDictionaryFreeMONR() { + ReadWriteAccess_t result = WRITE_OK; - Res = WRITE_OK; - pthread_mutex_lock(&MONRMutex); - int res = munmap(GSD->MonrMessages, sizeof (MonitorDataType)); + if (numberOfObjectsMemory == NULL || MONRMemory == NULL) { + errno = EINVAL; + LogMessage(LOG_LEVEL_ERROR, "Attempt to free uninitialized memory"); + return UNDEFINED; + } - if (res < 0) { - util_error("Unable to unmap monrMessages file!"); + pthread_mutex_lock(&MONRMutex); + pthread_mutex_lock(&numberOfObjectsMutex); + if (munmap(MONRMemory, *numberOfObjectsMemory * sizeof (MonitorDataType)) == -1) { + LogMessage(LOG_LEVEL_ERROR, "Memory unmapping error"); + result = UNDEFINED; + } + if (shm_unlink(MONR_DATA_FILENAME) == -1) { + LogMessage(LOG_LEVEL_ERROR, "Memory unlinking error"); + result = UNDEFINED; + } + if (munmap(numberOfObjectsMemory, sizeof (*numberOfObjectsMemory)) == -1) { + LogMessage(LOG_LEVEL_ERROR, "Memory unmapping error"); + result = UNDEFINED; + } + if (shm_unlink(NUMBER_OF_OBJECTS_FILENAME) == -1) { + LogMessage(LOG_LEVEL_ERROR, "Memory unlinking error"); + result = UNDEFINED; } - free(GSD->MonrMessages); - DataDictionarySetNumberOfObjectsU8(GSD, 0); + pthread_mutex_unlock(&numberOfObjectsMutex); pthread_mutex_unlock(&MONRMutex); - return Res; + + return result; } /*END of MONR*/ @@ -1803,9 +1871,9 @@ ReadWriteAccess_t DataDictionaryFreeMONR(GSDType * GSD) { /*NbrOfObjects*/ /*! - * \brief DataDictionarySetOBCStateU8 Parses input variable and sets variable to corresponding value - * \param GSD Pointer to shared allocated memory - * \param numberOfobjects number of objects in a test + * \brief DataDictionarySetOBCStateU8 Sets the number of objects to the specified value and clears all + * monitor data currently present in the system + * \param numberOfobjects number of objects * \return Result according to ::ReadWriteAccess_t */ ReadWriteAccess_t DataDictionarySetNumberOfObjectsU8(const uint32_t numberOfObjects) { @@ -1817,12 +1885,13 @@ ReadWriteAccess_t DataDictionarySetNumberOfObjectsU8(const uint32_t numberOfObje } pthread_mutex_lock(&numberOfObjectsMutex); - if (ftruncate(fdObjectMONRData, numberOfObjects * sizeof (fdObjectMONRData)) == -1) { + if (ftruncate(fdObjectMONRData, numberOfObjects * sizeof (MonitorDataType)) == -1) { LogMessage(LOG_LEVEL_ERROR, "File truncation error"); result = UNDEFINED; } else { *numberOfObjectsMemory = numberOfObjects; + memset(MONRMemory, 0, *numberOfObjectsMemory * sizeof (MonitorDataType)); LogMessage(LOG_LEVEL_DEBUG, "Modified shared memory to hold MONR data for %d objects", *numberOfObjectsMemory); } pthread_mutex_unlock(&numberOfObjectsMutex); @@ -1832,7 +1901,6 @@ ReadWriteAccess_t DataDictionarySetNumberOfObjectsU8(const uint32_t numberOfObje /*! * \brief DataDictionaryGetOBCStateU8 Reads variable from shared memory - * \param GSD Pointer to shared allocated memory * \param numberOfobjects number of objects in a test * \return Current object control state according to ::OBCState_t */ From 16658508bce315921268f3f20a078060c2dc0fb9 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 23 Mar 2020 16:10:32 +0100 Subject: [PATCH 391/523] Updated util reference --- util | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/util b/util index 6a4532f55..19a42f2de 160000 --- a/util +++ b/util @@ -1 +1 @@ -Subproject commit 6a4532f55a3da92637ce8c48de9e33d88eb29d7a +Subproject commit 19a42f2dec489753d31322e0527de876c1b18660 From b5430bbba42d9db9e3c8933d81b4aac5447db116 Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Mon, 23 Mar 2020 16:48:19 +0100 Subject: [PATCH 392/523] Saving work --- core/inc/util.h | 1 + core/src/objectcontrol.c | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/core/inc/util.h b/core/inc/util.h index e9a03874c..5ceece7cf 100644 --- a/core/inc/util.h +++ b/core/inc/util.h @@ -43,6 +43,7 @@ extern "C"{ #define DEFAULT_ORIGO_LAT 57.777073115 #define DEFAULT_ORIGO_LOG 12.781295498333 #define DEFAULT_ORIGO_ALT 193.114 +#define DEFAULT_ORIGO_HEADING 0 #define DEFAULT_VISUALISATION_SERVER_NAME 0 #define DEFAULT_FORCE_OBJECT_TO_LOCALHOST 0 #define DEFAULT_ASP_MAX_TIME_DIFF 2.5 diff --git a/core/src/objectcontrol.c b/core/src/objectcontrol.c index b1816e584..194c6232b 100644 --- a/core/src/objectcontrol.c +++ b/core/src/objectcontrol.c @@ -191,7 +191,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { dbl OriginLatitudeDbl = DEFAULT_ORIGO_LAT; dbl OriginLongitudeDbl = DEFAULT_ORIGO_LOG; dbl OriginAltitudeDbl = DEFAULT_ORIGO_ALT; - dbl OriginHeadingDbl; + dbl OriginHeadingDbl = DEFAULT_ORIGO_HEADING; C8 pcSendBuffer[MBUS_MAX_DATALEN]; C8 ObjectPort[SMALL_BUFFER_SIZE_0]; MonitorDataType monitorData; From 296239d1b6a6e91482e72b50c403575a84fc725e Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 24 Mar 2020 08:46:42 +0100 Subject: [PATCH 393/523] Updated function to take transmitterID from MonitorData --- core/inc/datadictionary.h | 2 +- core/src/datadictionary.c | 13 +++++++------ 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/core/inc/datadictionary.h b/core/inc/datadictionary.h index 15d4518e1..3f3c0559c 100644 --- a/core/inc/datadictionary.h +++ b/core/inc/datadictionary.h @@ -123,7 +123,7 @@ OBCState_t DataDictionaryGetOBCStateU8(GSDType *GSD); ReadWriteAccess_t DataDictionaryFreeMONR(); ReadWriteAccess_t DataDictionaryInitMONR(); -ReadWriteAccess_t DataDictionarySetMONR(const MonitorDataType * MONR, const uint32_t TransmitterId); +ReadWriteAccess_t DataDictionarySetMONR(const MonitorDataType * MONR); ReadWriteAccess_t DataDictionaryGetMONR(MonitorDataType * MONR, const uint32_t TransmitterId); ReadWriteAccess_t DataDictionarySetNumberOfObjectsU8(const uint32_t numberOfObjects); diff --git a/core/src/datadictionary.c b/core/src/datadictionary.c index ea8e32e48..df630f0f6 100644 --- a/core/src/datadictionary.c +++ b/core/src/datadictionary.c @@ -53,7 +53,7 @@ static pthread_mutex_t numberOfObjectsMutex = PTHREAD_MUTEX_INITIALIZER; #define MONR_DATA_FILENAME "/MONR.mem" static int fdObjectCount, fdObjectMONRData; -static unsigned int* numberOfObjectsMemory = NULL; +static uint32_t* numberOfObjectsMemory = NULL; static MonitorDataType* MONRMemory = NULL; @@ -1686,6 +1686,7 @@ ReadWriteAccess_t DataDictionaryInitMONR() { LogMessage(LOG_LEVEL_ERROR, "Failed to create shared memory"); return UNDEFINED; } + fdObjectMONRData = shm_open(MONR_DATA_FILENAME, O_RDWR | O_CREAT, S_IRUSR | S_IWUSR); if (fdObjectMONRData == -1) { LogMessage(LOG_LEVEL_ERROR, "Failed to create shared memory"); @@ -1715,7 +1716,7 @@ ReadWriteAccess_t DataDictionaryInitMONR() { return UNDEFINED; } - MONRMemory = mmap(NULL, sizeof (MonitorDataType) * (*numberOfObjectsMemory), + MONRMemory = mmap(NULL, sizeof (MonitorDataType) * (*numberOfObjectsMemory + 1), PROT_WRITE, MAP_SHARED, fdObjectMONRData, 0); if (MONRMemory == MAP_FAILED) { LogMessage(LOG_LEVEL_ERROR, "Memory mapping error"); @@ -1733,7 +1734,7 @@ ReadWriteAccess_t DataDictionaryInitMONR() { * \param transmitterId requested object transmitterId * \return Result according to ::ReadWriteAccess_t */ -ReadWriteAccess_t DataDictionarySetMONR(const MonitorDataType * MONR, const uint32_t transmitterId) { +ReadWriteAccess_t DataDictionarySetMONR(const MonitorDataType * MONR) { ReadWriteAccess_t result; @@ -1747,7 +1748,7 @@ ReadWriteAccess_t DataDictionarySetMONR(const MonitorDataType * MONR, const uint LogMessage(LOG_LEVEL_ERROR, "Shared memory input pointer error"); return UNDEFINED; } - if (transmitterId == 0) { + if (MONR->ClientID == 0) { errno = EINVAL; LogMessage(LOG_LEVEL_ERROR, "Transmitter ID 0 is reserved"); return UNDEFINED; @@ -1757,7 +1758,7 @@ ReadWriteAccess_t DataDictionarySetMONR(const MonitorDataType * MONR, const uint pthread_mutex_lock(&MONRMutex); result = PARAMETER_NOTFOUND; for (uint32_t i = 0; i < *numberOfObjectsMemory; ++i) { - if (MONRMemory[i].ClientID == transmitterId) { + if (MONRMemory[i].ClientID == MONR->ClientID) { memcpy(&MONRMemory[i], MONR, sizeof (MonitorDataType)); result = WRITE_OK; } @@ -1765,7 +1766,7 @@ ReadWriteAccess_t DataDictionarySetMONR(const MonitorDataType * MONR, const uint if (result == PARAMETER_NOTFOUND) { // Searched for unused memory space and place monitor data there - LogMessage(LOG_LEVEL_INFO, "Received first monitor data from transmitter ID %u", transmitterId); + LogMessage(LOG_LEVEL_INFO, "Received first monitor data from transmitter ID %u", MONR->ClientID); for (uint32_t i = 0; i < *numberOfObjectsMemory; ++i) { if (MONRMemory[i].ClientID == 0) { memcpy(&MONRMemory[i], MONR, sizeof (MonitorDataType)); From 70a9089b8ef1664f8542c4a3fad98f7a22c7ff46 Mon Sep 17 00:00:00 2001 From: Sebastian Loh Lindholm Date: Wed, 25 Mar 2020 16:10:11 +0100 Subject: [PATCH 394/523] File download and Get directory content updated. Sending 4 bytes, after first MSCP response, containing the length of the file. The string SubDownload file is not sent. --- core/src/systemcontrol.c | 61 +++++++++++++++++++++++++++------------- 1 file changed, 42 insertions(+), 19 deletions(-) diff --git a/core/src/systemcontrol.c b/core/src/systemcontrol.c index 7e571c5a3..48e8f8e30 100644 --- a/core/src/systemcontrol.c +++ b/core/src/systemcontrol.c @@ -177,6 +177,7 @@ void SystemControlSendControlResponse(U16 ResponseStatus, C8 * ResponseString, C I32 ResponseDataLength, I32 * Sockfd, U8 Debug); I32 SystemControlBuildControlResponse(U16 ResponseStatus, C8 * ResponseString, C8 * ResponseData, I32 ResponseDataLength, U8 Debug); +void SystemControlFileDownloadResponse(I32 FileLength, I32 * Sockfd, U8 Debug); void SystemControlSendLog(C8 * LogString, I32 * Sockfd, U8 Debug); void SystemControlSendMONR(C8 * LogString, I32 * Sockfd, U8 Debug); static void SystemControlCreateProcessChannel(const C8 * name, const U32 port, I32 * sockfd, @@ -197,7 +198,7 @@ C8 SystemControlClearTrajectories(void); C8 SystemControlClearGeofences(void); I32 SystemControlDeleteFileDirectory(C8 * Path, C8 * ReturnValue, U8 Debug); I32 SystemControlBuildFileContentInfo(C8 * Path, U8 Debug); -I32 SystemControlDestroyFileContentInfo(C8 * path); +I32 SystemControlDestroyFileContentInfo(C8 * Path, U8 RemoveFile); I32 SystemControlSendFileContent(I32 * sockfd, C8 * Path, C8 * PacketSize, C8 * ReturnValue, U8 Remove, U8 Debug); I32 SystemControlCreateDirectory(C8 * Path, C8 * ReturnValue, U8 Debug); @@ -340,6 +341,7 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { } + I32 FileLengthI32 = 0; while (!iExit) { if (server_state == SERVER_STATE_ERROR) { @@ -740,15 +742,12 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { if (ControlResponseBuffer[0] == FOLDER_EXIST) { UtilCreateDirContent(SystemControlArgument[0], "dir.info"); bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - - I32 file_len = SystemControlBuildFileContentInfo("dir.info", 0); - - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, - "SubGetDirectoryContent:", - SystemControlDirectoryInfo.info_buffer, file_len, - &ClientSocket, 0); - - SystemControlDestroyFileContentInfo("dir.info"); + FileLengthI32 = SystemControlBuildFileContentInfo("dir.info", 0); + SystemControlFileDownloadResponse(FileLengthI32, &ClientSocket, 0); + SystemControlSendFileContent(&ClientSocket, "dir.info", + STR_SYSTEM_CONTROL_TX_PACKET_SIZE, SystemControlDirectoryInfo.info_buffer, + KEEP_FILE, 0); + SystemControlDestroyFileContentInfo("dir.info", 1); } } @@ -785,14 +784,13 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "DownloadFile:", ControlResponseBuffer, 1, &ClientSocket, 0); if (ControlResponseBuffer[0] == FILE_EXIST) { - UtilCreateDirContent(SystemControlArgument[0], SystemControlArgument[0]); bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); - SystemControlBuildFileContentInfo(SystemControlArgument[0], 0); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "SubDownloadFile:", - ControlResponseBuffer, 4, &ClientSocket, 0); + FileLengthI32 = SystemControlBuildFileContentInfo(SystemControlArgument[0], 0); + SystemControlFileDownloadResponse(FileLengthI32, &ClientSocket, 0); SystemControlSendFileContent(&ClientSocket, SystemControlArgument[0], - STR_SYSTEM_CONTROL_TX_PACKET_SIZE, ControlResponseBuffer, + STR_SYSTEM_CONTROL_TX_PACKET_SIZE, SystemControlDirectoryInfo.info_buffer, KEEP_FILE, 0); + SystemControlDestroyFileContentInfo(SystemControlArgument[0], 0); } } @@ -1379,6 +1377,26 @@ void SystemControlSendControlResponse(U16 ResponseStatus, C8 * ResponseString, C } +void SystemControlFileDownloadResponse(I32 FileLength, I32 * Sockfd, U8 Debug) { + I32 n, i; + C8 Length[4]; + n = FileLength; + Length[0] = (C8) (n >> 24); + Length[1] = (C8) (n >> 16); + Length[2] = (C8) (n >> 8); + Length[3] = (C8) n; + + if (Debug) { + for (i = 0; i < 4; i++) + printf("%x-", Length[i]); + printf("\n"); + } + + UtilSendTCPData("System Control", Length, 4, Sockfd, 0); +} + + + I32 SystemControlBuildControlResponse(U16 ResponseStatus, C8 * ResponseString, C8 * ResponseData, I32 ResponseDataLength, U8 Debug) { int i = 0, n = 0, j = 0, t = 0; @@ -1988,7 +2006,7 @@ I32 SystemControlBuildFileContentInfo(C8 * Path, U8 Debug) { if (Debug) LogMessage(LOG_LEVEL_DEBUG, "Filesize %d of %s", (I32) st.st_size, CompletePath); - + return st.st_size; */ struct stat st; @@ -2000,9 +2018,11 @@ I32 SystemControlBuildFileContentInfo(C8 * Path, U8 Debug) { if (SystemControlDirectoryInfo.exist) return -1; + UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); strcat(CompletePath, Path); stat(CompletePath, &st); + /* // Create new temporary file, containing the length of the current file in hex + the rest of the document strcat(temporaryCompletePath, ".temp"); FILE *comp_fd = fopen(CompletePath, "r"); @@ -2022,6 +2042,7 @@ I32 SystemControlBuildFileContentInfo(C8 * Path, U8 Debug) { // Rename the temporary file to the name of the previous one rename(temporaryCompletePath, CompletePath); stat(CompletePath, &st); + */ // Create mmap of the file and return the length SystemControlDirectoryInfo.fd = open(CompletePath, O_RDWR); SystemControlDirectoryInfo.info_buffer = @@ -2031,19 +2052,21 @@ I32 SystemControlBuildFileContentInfo(C8 * Path, U8 Debug) { return st.st_size; } -I32 SystemControlDestroyFileContentInfo(C8 * path) { +I32 SystemControlDestroyFileContentInfo(C8 * Path, U8 RemoveFile) { char CompletePath[MAX_FILE_PATH]; struct stat st; if (!SystemControlDirectoryInfo.exist) return -1; UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); - strcat(CompletePath, path); + strcat(CompletePath, Path); munmap(SystemControlDirectoryInfo.info_buffer, SystemControlDirectoryInfo.size); close(SystemControlDirectoryInfo.fd); SystemControlDirectoryInfo.exist = 0; - //remove(CompletePath); + if(RemoveFile == 1){ + remove(CompletePath); + } return 0; } From dfe5e5deec3b42d442348c5498c66aa33c4dd878 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 26 Mar 2020 17:28:49 +0100 Subject: [PATCH 395/523] Updated util ref for implementaion of shared memory library --- util | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/util b/util index 19a42f2de..e27ea8cec 160000 --- a/util +++ b/util @@ -1 +1 @@ -Subproject commit 19a42f2dec489753d31322e0527de876c1b18660 +Subproject commit e27ea8cec8ef1107adcaccb711d32862b2df057d From 631050f309dce4e314fb5a35b9ea219e4b53c9e9 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 26 Mar 2020 17:29:20 +0100 Subject: [PATCH 396/523] Linked shared memory library into cmake --- core/CMakeLists.txt | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index 2e7a9c501..5e2b724d5 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -18,10 +18,12 @@ set(DATA_DICTIONARY_TARGET MaestroDataDictionary) set(TIME_LIBRARY MaestroTime) set(LOGGING_LIBRARY MaestroLogging) set(MESSAGE_BUS_LIBRARY MaestroMQ) +set(SHARED_MEMORY_LIBRARY MaestroSHM) set(MATH_LIBRARY m) get_target_property(TIME_HEADERS ${TIME_LIBRARY} INCLUDE_DIRECTORIES) get_target_property(LOGGING_HEADERS ${LOGGING_LIBRARY} INCLUDE_DIRECTORIES) get_target_property(MESSAGE_BUS_HEADERS ${MESSAGE_BUS_LIBRARY} INCLUDE_DIRECTORIES) +get_target_property(SHARED_MEMORY_HEADERS ${SHARED_MEMORY_LIBRARY} INCLUDE_DIRECTORIES) include(GNUInstallDirs) @@ -80,6 +82,7 @@ target_include_directories(${DATA_DICTIONARY_TARGET} PUBLIC $ ${LOGGING_HEADERS} ${MESSAGE_BUS_HEADERS} + ${SHARED_MEMORY_HEADERS} ) set_target_properties(${DATA_DICTIONARY_TARGET} PROPERTIES PUBLIC_HEADER ${CMAKE_CURRENT_SOURCE_DIR}/inc/datadictionary.h @@ -102,6 +105,10 @@ target_link_libraries(${COREUTILS_TARGET} LINK_PUBLIC ${MATH_LIBRARY} ) +target_link_libraries(${DATA_DICTIONARY_TARGET} LINK_PUBLIC + ${SHARED_MEMORY_LIBRARY} +) + # Installation rules install(CODE "MESSAGE(STATUS \"Installing target ${CORE_TARGET}\")") install(TARGETS ${CORE_TARGET} ${COREUTILS_TARGET} From 98aa1c11c65bc6bd6aab4a2fc3c1c7532b92044d Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 26 Mar 2020 17:30:04 +0100 Subject: [PATCH 397/523] Added data dict destruction when main exits --- core/src/main.c | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/core/src/main.c b/core/src/main.c index 20b2d5d02..3a78d874d 100644 --- a/core/src/main.c +++ b/core/src/main.c @@ -99,7 +99,7 @@ int main(int argc, char *argv[]) { Options options; pid_t pID[numberOfModules]; char moduleExitStatus[numberOfModules]; - ReadWriteAccess_t dataDictInitResult = UNDEFINED; + ReadWriteAccess_t dataDictOperationResult = UNDEFINED; if (readArgumentList(argc, argv, &options)) exit(EXIT_FAILURE); @@ -124,8 +124,8 @@ int main(int argc, char *argv[]) { // Initialise data dictionary LogMessage(LOG_LEVEL_INFO, "Initializing data dictionary"); - dataDictInitResult = DataDictionaryConstructor(GSD); - if (dataDictInitResult != READ_OK && dataDictInitResult != READ_WRITE_OK) { + dataDictOperationResult = DataDictionaryConstructor(GSD); + if (dataDictOperationResult != READ_OK && dataDictOperationResult != READ_WRITE_OK) { util_error("Unable to initialize shared memory space"); } else { @@ -160,8 +160,13 @@ int main(int argc, char *argv[]) { LogMessage(LOG_LEVEL_DEBUG, "Cleaning up message bus resources"); if (shutdownMessageQueueBus()) util_error("Unable to successfully clean up message bus resources"); - else - exit(EXIT_SUCCESS); + + dataDictOperationResult = DataDictionaryDestructor(GSD); + if (dataDictOperationResult != WRITE_OK && dataDictOperationResult != READ_WRITE_OK) { + util_error("Unable to clear shared memory space"); + } + + exit(EXIT_SUCCESS); } /*! From d70f71c1acbe28b264531b9c82f62b39ab974d21 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 26 Mar 2020 17:30:59 +0100 Subject: [PATCH 398/523] Some refactoring --- core/inc/datadictionary.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/core/inc/datadictionary.h b/core/inc/datadictionary.h index 3f3c0559c..3b865bdd9 100644 --- a/core/inc/datadictionary.h +++ b/core/inc/datadictionary.h @@ -126,10 +126,11 @@ ReadWriteAccess_t DataDictionaryInitMONR(); ReadWriteAccess_t DataDictionarySetMONR(const MonitorDataType * MONR); ReadWriteAccess_t DataDictionaryGetMONR(MonitorDataType * MONR, const uint32_t TransmitterId); -ReadWriteAccess_t DataDictionarySetNumberOfObjectsU8(const uint32_t numberOfObjects); +ReadWriteAccess_t DataDictionarySetNumberOfObjectsU8(const uint32_t newNumberOfObjects); ReadWriteAccess_t DataDictionaryGetNumberOfObjectsU8(uint32_t *numberOfObjects); ReadWriteAccess_t DataDictionaryConstructor(GSDType *GSD); +ReadWriteAccess_t DataDictionaryDestructor(GSDType *GSD); #endif From ddbf271e7d9a52029202261d4c35e6013ed489c2 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 26 Mar 2020 17:32:35 +0100 Subject: [PATCH 399/523] Implemented use of the shared memory library in datadictionary --- core/src/datadictionary.c | 173 +++++++++++++------------------------- 1 file changed, 60 insertions(+), 113 deletions(-) diff --git a/core/src/datadictionary.c b/core/src/datadictionary.c index df630f0f6..aae0e0334 100644 --- a/core/src/datadictionary.c +++ b/core/src/datadictionary.c @@ -11,14 +11,17 @@ /*------------------------------------------------------------ -- Include files. ------------------------------------------------------------*/ + + #include #include #include -#include #include #include "datadictionary.h" #include "iso22133.h" #include "logging.h" +#include "shmem.h" + // Parameters and variables static pthread_mutex_t OriginLatitudeMutex = PTHREAD_MUTEX_INITIALIZER; @@ -46,15 +49,10 @@ static pthread_mutex_t DataDictionaryRVSSRateMutex = PTHREAD_MUTEX_INITIALIZER; static pthread_mutex_t ASPDataMutex = PTHREAD_MUTEX_INITIALIZER; static pthread_mutex_t MiscDataMutex = PTHREAD_MUTEX_INITIALIZER; static pthread_mutex_t OBCStateMutex = PTHREAD_MUTEX_INITIALIZER; -static pthread_mutex_t MONRMutex = PTHREAD_MUTEX_INITIALIZER; -static pthread_mutex_t numberOfObjectsMutex = PTHREAD_MUTEX_INITIALIZER; -#define NUMBER_OF_OBJECTS_FILENAME "/NUMBER_OF_OBJECTS.mem" -#define MONR_DATA_FILENAME "/MONR.mem" +#define MONR_DATA_FILENAME "MonitorData" -static int fdObjectCount, fdObjectMONRData; -static uint32_t* numberOfObjectsMemory = NULL; -static MonitorDataType* MONRMemory = NULL; +static volatile MonitorDataType* MONRMemory = NULL; /*------------------------------------------------------------ @@ -107,6 +105,20 @@ ReadWriteAccess_t DataDictionaryConstructor(GSDType * GSD) { } +/*! + * \brief DataDictionaryDestructor Deallocate data held by DataDictionary. + * \param GSD Pointer to allocated shared memory + * \return Error code defined by ::ReadWriteAccess_t + */ +ReadWriteAccess_t DataDictionaryDestructor(GSDType * GSD) { + ReadWriteAccess_t result = WRITE_OK; + + result = result == WRITE_OK ? DataDictionaryFreeMONR() : result; + + return result; +} + + /*! * \brief DataDictionaryInitOriginLatitudeDbl Initializes variable according to the configuration file * \param GSD Pointer to shared allocated memory @@ -1681,51 +1693,12 @@ OBCState_t DataDictionaryGetOBCStateU8(GSDType * GSD) { */ ReadWriteAccess_t DataDictionaryInitMONR() { - fdObjectCount = shm_open(NUMBER_OF_OBJECTS_FILENAME, O_RDWR | O_CREAT, S_IRUSR | S_IWUSR); - if (fdObjectMONRData == -1) { - LogMessage(LOG_LEVEL_ERROR, "Failed to create shared memory"); - return UNDEFINED; - } - - fdObjectMONRData = shm_open(MONR_DATA_FILENAME, O_RDWR | O_CREAT, S_IRUSR | S_IWUSR); - if (fdObjectMONRData == -1) { - LogMessage(LOG_LEVEL_ERROR, "Failed to create shared memory"); - shm_unlink(NUMBER_OF_OBJECTS_FILENAME); - return UNDEFINED; - } - - if (ftruncate(fdObjectCount, sizeof (unsigned int)) == -1) { - LogMessage(LOG_LEVEL_ERROR, "File truncation error"); - shm_unlink(NUMBER_OF_OBJECTS_FILENAME); - shm_unlink(MONR_DATA_FILENAME); - return UNDEFINED; - } - - if (ftruncate(fdObjectMONRData, sizeof (unsigned int)) == -1) { - LogMessage(LOG_LEVEL_ERROR, "File truncation error"); - shm_unlink(NUMBER_OF_OBJECTS_FILENAME); - shm_unlink(MONR_DATA_FILENAME); + int createdMemory; + MONRMemory = createSharedMemory(MONR_DATA_FILENAME, 0, sizeof (MonitorDataType), &createdMemory); + if (MONRMemory == NULL) { return UNDEFINED; } - - numberOfObjectsMemory = mmap(NULL, sizeof (unsigned int), PROT_WRITE, MAP_SHARED, fdObjectCount, 0); - if (numberOfObjectsMemory == MAP_FAILED) { - LogMessage(LOG_LEVEL_ERROR, "Memory mapping error"); - shm_unlink(NUMBER_OF_OBJECTS_FILENAME); - shm_unlink(MONR_DATA_FILENAME); - return UNDEFINED; - } - - MONRMemory = mmap(NULL, sizeof (MonitorDataType) * (*numberOfObjectsMemory + 1), - PROT_WRITE, MAP_SHARED, fdObjectMONRData, 0); - if (MONRMemory == MAP_FAILED) { - LogMessage(LOG_LEVEL_ERROR, "Memory mapping error"); - munmap(numberOfObjectsMemory, sizeof (unsigned int)); - shm_unlink(NUMBER_OF_OBJECTS_FILENAME); - shm_unlink(MONR_DATA_FILENAME); - return UNDEFINED; - } - return READ_WRITE_OK; + return createdMemory ? WRITE_OK : READ_OK; } /*! @@ -1738,7 +1711,7 @@ ReadWriteAccess_t DataDictionarySetMONR(const MonitorDataType * MONR) { ReadWriteAccess_t result; - if (MONRMemory == NULL || numberOfObjectsMemory == NULL) { + if (MONRMemory == NULL) { errno = EINVAL; LogMessage(LOG_LEVEL_ERROR, "Shared memory not initialized"); return UNDEFINED; @@ -1754,10 +1727,11 @@ ReadWriteAccess_t DataDictionarySetMONR(const MonitorDataType * MONR) { return UNDEFINED; } - pthread_mutex_lock(&numberOfObjectsMutex); - pthread_mutex_lock(&MONRMutex); + MONRMemory = claimSharedMemory(MONRMemory); result = PARAMETER_NOTFOUND; - for (uint32_t i = 0; i < *numberOfObjectsMemory; ++i) { + unsigned int numberOfObjects = getNumberOfMemoryElements(MONRMemory); + LogPrint("Number of objects currently in memory: %u", numberOfObjects); + for (uint32_t i = 0; i < numberOfObjects; ++i) { if (MONRMemory[i].ClientID == MONR->ClientID) { memcpy(&MONRMemory[i], MONR, sizeof (MonitorDataType)); result = WRITE_OK; @@ -1765,9 +1739,9 @@ ReadWriteAccess_t DataDictionarySetMONR(const MonitorDataType * MONR) { } if (result == PARAMETER_NOTFOUND) { - // Searched for unused memory space and place monitor data there + // Search for unused memory space and place monitor data there LogMessage(LOG_LEVEL_INFO, "Received first monitor data from transmitter ID %u", MONR->ClientID); - for (uint32_t i = 0; i < *numberOfObjectsMemory; ++i) { + for (uint32_t i = 0; i < numberOfObjects; ++i) { if (MONRMemory[i].ClientID == 0) { memcpy(&MONRMemory[i], MONR, sizeof (MonitorDataType)); result = WRITE_OK; @@ -1776,18 +1750,20 @@ ReadWriteAccess_t DataDictionarySetMONR(const MonitorDataType * MONR) { // No uninitialized memory space found - create new if (result == PARAMETER_NOTFOUND) { - if (ftruncate(fdObjectMONRData, (*numberOfObjectsMemory + 1) * sizeof (MonitorDataType)) == -1) { - LogMessage(LOG_LEVEL_ERROR, "File truncation error"); - result = UNDEFINED; + MONRMemory = resizeSharedMemory(MONRMemory, numberOfObjects + 1); + if (MONRMemory != NULL) { + numberOfObjects = getNumberOfMemoryElements(MONRMemory); + LogMessage(LOG_LEVEL_INFO, "Modified shared memory to hold MONR data for %u objects, mp now %p", numberOfObjects, MONRMemory); + memcpy(&MONRMemory[numberOfObjects-1], MONR, sizeof (MonitorDataType)); + LogPrint("Printed MONR"); } else { - *numberOfObjectsMemory += 1; - LogMessage(LOG_LEVEL_DEBUG, "Modified shared memory to hold MONR data for %d objects", *numberOfObjectsMemory); + LogMessage(LOG_LEVEL_ERROR, "Error resizing shared memory"); + result = UNDEFINED; } } } - pthread_mutex_unlock(&MONRMutex); - pthread_mutex_unlock(&numberOfObjectsMutex); + MONRMemory = releaseSharedMemory(MONRMemory); return result; } @@ -1812,16 +1788,16 @@ ReadWriteAccess_t DataDictionaryGetMONR(MonitorDataType * MONR, const uint32_t t return UNDEFINED; } - pthread_mutex_lock(&numberOfObjectsMutex); - pthread_mutex_lock(&MONRMutex); - for (uint32_t i = 0; i < *numberOfObjectsMemory; ++i) { + MONRMemory = claimSharedMemory(MONRMemory); + unsigned int numberOfObjects = getNumberOfMemoryElements(MONRMemory); + for (unsigned int i = 0; i < numberOfObjects; ++i) { if (MONRMemory[i].ClientID == transmitterId) { memcpy(MONR, &MONRMemory[i], sizeof (MonitorDataType)); result = READ_OK; } } - pthread_mutex_unlock(&MONRMutex); - pthread_mutex_unlock(&numberOfObjectsMutex); + + MONRMemory = releaseSharedMemory(MONRMemory); if (result == PARAMETER_NOTFOUND) { LogMessage(LOG_LEVEL_WARNING, "Unable to find monitor data for transmitter ID %u", transmitterId); @@ -1837,32 +1813,13 @@ ReadWriteAccess_t DataDictionaryGetMONR(MonitorDataType * MONR, const uint32_t t ReadWriteAccess_t DataDictionaryFreeMONR() { ReadWriteAccess_t result = WRITE_OK; - if (numberOfObjectsMemory == NULL || MONRMemory == NULL) { + if (MONRMemory == NULL) { errno = EINVAL; LogMessage(LOG_LEVEL_ERROR, "Attempt to free uninitialized memory"); return UNDEFINED; } - pthread_mutex_lock(&MONRMutex); - pthread_mutex_lock(&numberOfObjectsMutex); - if (munmap(MONRMemory, *numberOfObjectsMemory * sizeof (MonitorDataType)) == -1) { - LogMessage(LOG_LEVEL_ERROR, "Memory unmapping error"); - result = UNDEFINED; - } - if (shm_unlink(MONR_DATA_FILENAME) == -1) { - LogMessage(LOG_LEVEL_ERROR, "Memory unlinking error"); - result = UNDEFINED; - } - if (munmap(numberOfObjectsMemory, sizeof (*numberOfObjectsMemory)) == -1) { - LogMessage(LOG_LEVEL_ERROR, "Memory unmapping error"); - result = UNDEFINED; - } - if (shm_unlink(NUMBER_OF_OBJECTS_FILENAME) == -1) { - LogMessage(LOG_LEVEL_ERROR, "Memory unlinking error"); - result = UNDEFINED; - } - pthread_mutex_unlock(&numberOfObjectsMutex); - pthread_mutex_unlock(&MONRMutex); + destroySharedMemory(MONRMemory); return result; } @@ -1877,26 +1834,21 @@ ReadWriteAccess_t DataDictionaryFreeMONR() { * \param numberOfobjects number of objects * \return Result according to ::ReadWriteAccess_t */ -ReadWriteAccess_t DataDictionarySetNumberOfObjectsU8(const uint32_t numberOfObjects) { +ReadWriteAccess_t DataDictionarySetNumberOfObjectsU8(const uint32_t newNumberOfObjects) { + + unsigned int numberOfObjects; ReadWriteAccess_t result = WRITE_OK; - if (numberOfObjectsMemory == NULL) { + MONRMemory = claimSharedMemory(MONRMemory); + MONRMemory = resizeSharedMemory(MONRMemory, newNumberOfObjects); + numberOfObjects = getNumberOfMemoryElements(MONRMemory); + MONRMemory = releaseSharedMemory(MONRMemory); + + if (MONRMemory == NULL) { errno = EINVAL; - LogMessage(LOG_LEVEL_ERROR, "Shared memory not initialized"); + LogMessage(LOG_LEVEL_ERROR, "Error resizing shared memory"); return UNDEFINED; } - pthread_mutex_lock(&numberOfObjectsMutex); - if (ftruncate(fdObjectMONRData, numberOfObjects * sizeof (MonitorDataType)) == -1) { - LogMessage(LOG_LEVEL_ERROR, "File truncation error"); - result = UNDEFINED; - } - else { - *numberOfObjectsMemory = numberOfObjects; - memset(MONRMemory, 0, *numberOfObjectsMemory * sizeof (MonitorDataType)); - LogMessage(LOG_LEVEL_DEBUG, "Modified shared memory to hold MONR data for %d objects", *numberOfObjectsMemory); - } - pthread_mutex_unlock(&numberOfObjectsMutex); - return result; } @@ -1906,15 +1858,10 @@ ReadWriteAccess_t DataDictionarySetNumberOfObjectsU8(const uint32_t numberOfObje * \return Current object control state according to ::OBCState_t */ ReadWriteAccess_t DataDictionaryGetNumberOfObjectsU8(uint32_t * numberOfObjects) { - if (numberOfObjectsMemory == NULL) { - errno = EINVAL; - LogMessage(LOG_LEVEL_ERROR, "Shared memory not initialized"); - return UNDEFINED; - } - pthread_mutex_lock(&numberOfObjectsMutex); - *numberOfObjects = *numberOfObjectsMemory; - pthread_mutex_unlock(&numberOfObjectsMutex); + MONRMemory = claimSharedMemory(MONRMemory); + *numberOfObjects = getNumberOfMemoryElements(MONRMemory); + MONRMemory = releaseSharedMemory(MONRMemory); return READ_OK; } From c2e4314ed42eec5cbae83f9aa377a08bbcebef8f Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 26 Mar 2020 18:15:49 +0100 Subject: [PATCH 400/523] Updated util ref --- util | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/util b/util index e27ea8cec..74fa247ed 160000 --- a/util +++ b/util @@ -1 +1 @@ -Subproject commit e27ea8cec8ef1107adcaccb711d32862b2df057d +Subproject commit 74fa247ed95525a4f14e11462d4f63f49cbc3e5c From d8f7be7b42b18ca5dcf7f32787dbc7d113c5b049 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 27 Mar 2020 11:12:48 +0100 Subject: [PATCH 401/523] Ran code formatter --- core/src/datadictionary.c | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/core/src/datadictionary.c b/core/src/datadictionary.c index aae0e0334..03d78f367 100644 --- a/core/src/datadictionary.c +++ b/core/src/datadictionary.c @@ -52,7 +52,7 @@ static pthread_mutex_t OBCStateMutex = PTHREAD_MUTEX_INITIALIZER; #define MONR_DATA_FILENAME "MonitorData" -static volatile MonitorDataType* MONRMemory = NULL; +static volatile MonitorDataType *MONRMemory = NULL; /*------------------------------------------------------------ @@ -1694,6 +1694,7 @@ OBCState_t DataDictionaryGetOBCStateU8(GSDType * GSD) { ReadWriteAccess_t DataDictionaryInitMONR() { int createdMemory; + MONRMemory = createSharedMemory(MONR_DATA_FILENAME, 0, sizeof (MonitorDataType), &createdMemory); if (MONRMemory == NULL) { return UNDEFINED; @@ -1730,6 +1731,7 @@ ReadWriteAccess_t DataDictionarySetMONR(const MonitorDataType * MONR) { MONRMemory = claimSharedMemory(MONRMemory); result = PARAMETER_NOTFOUND; unsigned int numberOfObjects = getNumberOfMemoryElements(MONRMemory); + LogPrint("Number of objects currently in memory: %u", numberOfObjects); for (uint32_t i = 0; i < numberOfObjects; ++i) { if (MONRMemory[i].ClientID == MONR->ClientID) { @@ -1753,8 +1755,10 @@ ReadWriteAccess_t DataDictionarySetMONR(const MonitorDataType * MONR) { MONRMemory = resizeSharedMemory(MONRMemory, numberOfObjects + 1); if (MONRMemory != NULL) { numberOfObjects = getNumberOfMemoryElements(MONRMemory); - LogMessage(LOG_LEVEL_INFO, "Modified shared memory to hold MONR data for %u objects, mp now %p", numberOfObjects, MONRMemory); - memcpy(&MONRMemory[numberOfObjects-1], MONR, sizeof (MonitorDataType)); + LogMessage(LOG_LEVEL_INFO, + "Modified shared memory to hold MONR data for %u objects, mp now %p", + numberOfObjects, MONRMemory); + memcpy(&MONRMemory[numberOfObjects - 1], MONR, sizeof (MonitorDataType)); LogPrint("Printed MONR"); } else { @@ -1790,7 +1794,8 @@ ReadWriteAccess_t DataDictionaryGetMONR(MonitorDataType * MONR, const uint32_t t MONRMemory = claimSharedMemory(MONRMemory); unsigned int numberOfObjects = getNumberOfMemoryElements(MONRMemory); - for (unsigned int i = 0; i < numberOfObjects; ++i) { + + for (unsigned int i = 0; i < numberOfObjects; ++i) { if (MONRMemory[i].ClientID == transmitterId) { memcpy(MONR, &MONRMemory[i], sizeof (MonitorDataType)); result = READ_OK; @@ -1838,6 +1843,7 @@ ReadWriteAccess_t DataDictionarySetNumberOfObjectsU8(const uint32_t newNumberOfO unsigned int numberOfObjects; ReadWriteAccess_t result = WRITE_OK; + MONRMemory = claimSharedMemory(MONRMemory); MONRMemory = resizeSharedMemory(MONRMemory, newNumberOfObjects); numberOfObjects = getNumberOfMemoryElements(MONRMemory); From f9ffc4bd55cfa671bc8028968e24c5bb4f41d651 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 27 Mar 2020 14:26:48 +0100 Subject: [PATCH 402/523] Reimplemented generic file delete function --- core/inc/util.h | 1 + core/src/util.c | 60 ++++++++++++++++++++++++++++++++++++++++--------- 2 files changed, 50 insertions(+), 11 deletions(-) diff --git a/core/inc/util.h b/core/inc/util.h index ef01969dd..5bebdbfd0 100644 --- a/core/inc/util.h +++ b/core/inc/util.h @@ -590,6 +590,7 @@ int UtilDeleteGeofenceFiles(void); int UtilDeleteTrajectoryFile(const char * geofencePath, const size_t nameLen); int UtilDeleteGeofenceFile(const char * geofencePath, const size_t nameLen); +int UtilDeleteGenericFile(const char * genericFilePath, const size_t nameLen); // File parsing functions int UtilCheckTrajectoryFileFormat(const char *path, size_t pathLen); diff --git a/core/src/util.c b/core/src/util.c index bb6c7155b..7c9a24bac 100644 --- a/core/src/util.c +++ b/core/src/util.c @@ -2418,24 +2418,26 @@ void UtilGetGeofenceDirectoryPath(char *path, size_t pathLen) { } /*! - * \brief UtilDeleteTrajectoryFile deletes the specified trajectory and deletes it + * \brief UtilDeleteTrajectoryFile deletes the specified trajectory + * \param name + * \param nameLen * \return returns 0 if the trajectory is now deleted. Non-zero values otherwise. */ int UtilDeleteTrajectoryFile(const char *name, const size_t nameLen) { char filePath[MAX_FILE_PATH] = { '\0' }; UtilGetTrajDirectoryPath(filePath, sizeof (filePath)); + if (name == NULL) { + errno = EINVAL; + LogMessage(LOG_LEVEL_ERROR, "Attempt to call delete on null trajectory file"); + return -1; + } if (strstr(name, "..") != NULL || strstr(name, "/") != NULL) { errno = EPERM; LogMessage(LOG_LEVEL_ERROR, "Attempt to call delete on trajectory file and navigate out of directory"); return -1; } - if (name == NULL) { - errno = EINVAL; - LogMessage(LOG_LEVEL_ERROR, "Attempt to call delete on null trajectory file"); - return -1; - } if (strlen(filePath) + nameLen > MAX_FILE_PATH) { errno = ENOBUFS; LogMessage(LOG_LEVEL_ERROR, "Trajectory file name too long"); @@ -2450,33 +2452,69 @@ int UtilDeleteTrajectoryFile(const char *name, const size_t nameLen) { } /*! - * \brief UtilDeleteGeofenceFile deletes the specified geofence and deletes it + * \brief UtilDeleteGeofenceFile deletes the specified geofence + * \param name + * \param nameLen * \return returns 0 if the geofence is now deleted. Non-zero values otherwise. */ int UtilDeleteGeofenceFile(const char *name, const size_t nameLen) { char filePath[MAX_FILE_PATH] = { '\0' }; UtilGetGeofenceDirectoryPath(filePath, sizeof (filePath)); + if (name == NULL) { + errno = EINVAL; + LogMessage(LOG_LEVEL_ERROR, "Attempt to call delete on null geofence file"); + return -1; + } if (strstr(name, "..") != NULL || strstr(name, "/") != NULL) { errno = EPERM; LogMessage(LOG_LEVEL_ERROR, "Attempt to call delete on geofence file and navigate out of directory"); return -1; } - if (name == NULL) { + if (strlen(filePath) + nameLen > MAX_FILE_PATH) { + errno = ENOBUFS; + LogMessage(LOG_LEVEL_ERROR, "Geofence file name too long"); + return -1; + } + + if (filePath[0] == '\0') + return -1; + + strcat(filePath, name); + return deleteFile(filePath, sizeof (filePath)); +} + +/*! + * \brief UtilDeleteGeofenceFile deletes the specified file and deletes it + * \param pathRelativeToWorkspace + * \return returns 0 if the geofence is now deleted. Non-zero values otherwise. + */ +int UtilDeleteGenericFile(const char *pathRelativeToWorkspace, const size_t nameLen) { + char filePath[MAX_FILE_PATH] = { '\0' }; + UtilGetTestDirectoryPath(filePath, sizeof (filePath)); + + if (pathRelativeToWorkspace == NULL) { errno = EINVAL; - LogMessage(LOG_LEVEL_ERROR, "Attempt to call delete on null geofence file"); + LogMessage(LOG_LEVEL_ERROR, "Attempt to call delete on null generic file"); + return -1; + } + + if (strstr(pathRelativeToWorkspace, "..") != NULL) { + errno = EPERM; + LogMessage(LOG_LEVEL_ERROR, "Attempt to call delete on generic file and navigate out of directory"); return -1; } + if (strlen(filePath) + nameLen > MAX_FILE_PATH) { errno = ENOBUFS; - LogMessage(LOG_LEVEL_ERROR, "Geofence file name too long"); + LogMessage(LOG_LEVEL_ERROR, "Generic path name too long"); return -1; } if (filePath[0] == '\0') return -1; - strcat(filePath, name); + strcat(filePath, pathRelativeToWorkspace); return deleteFile(filePath, sizeof (filePath)); } From 9cf761386679dc2f07a1c5c768c56e37c772f122 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 27 Mar 2020 14:34:23 +0100 Subject: [PATCH 403/523] Reimplemented use of deletefiledirectory command --- core/src/systemcontrol.c | 31 +++++++++++++++++++++++++++++-- 1 file changed, 29 insertions(+), 2 deletions(-) diff --git a/core/src/systemcontrol.c b/core/src/systemcontrol.c index aa8c88baa..4e9588350 100644 --- a/core/src/systemcontrol.c +++ b/core/src/systemcontrol.c @@ -132,7 +132,7 @@ typedef enum { Idle_0, GetServerStatus_0, ArmScenario_0, DisarmScenario_0, StartScenario_1, stop_0, AbortScenario_0, InitializeScenario_0, ConnectObject_0, DisconnectObject_0, GetServerParameterList_0, SetServerParameter_2, GetServerParameter_1, DownloadFile_1, UploadFile_4, CheckFileDirectoryExist_1, - GetRootDirectoryContent_0, GetDirectoryContent_1, DeleteTrajectory_1, DeleteGeofence_1, + GetRootDirectoryContent_0, GetDirectoryContent_1, DeleteTrajectory_1, DeleteGeofence_1, DeleteFileDirectory_1, ClearTrajectories_0, ClearGeofences_0, CreateDirectory_1, GetTestOrigin_0, replay_1, control_0, Exit_0, start_ext_trigg_1, nocommand @@ -143,7 +143,7 @@ static const char *SystemControlCommandsArr[] = { "AbortScenario_0", "InitializeScenario_0", "ConnectObject_0", "DisconnectObject_0", "GetServerParameterList_0", "SetServerParameter_2", "GetServerParameter_1", "DownloadFile_1", "UploadFile_4", "CheckFileDirectoryExist_1", - "GetRootDirectoryContent_0", "GetDirectoryContent_1", "DeleteTrajectory_1", "DeleteGeofence_1", + "GetRootDirectoryContent_0", "GetDirectoryContent_1", "DeleteTrajectory_1", "DeleteGeofence_1", "DeleteFileDirectory_1", "ClearTrajectories_0", "ClearGeofences_0", "CreateDirectory_1", "GetTestOrigin_0", "replay_1", "control_0", @@ -196,6 +196,7 @@ I32 SystemControlReceiveRxData(I32 * sockfd, C8 * Path, C8 * FileSize, C8 * Pack U8 Debug); static C8 SystemControlDeleteTrajectory(const C8 * trajectoryName, const size_t nameLen); static C8 SystemControlDeleteGeofence(const C8 * geofenceName, const size_t nameLen); +static C8 SystemControlDeleteGenericFile(const C8 * filePath, const size_t nameLen); static C8 SystemControlClearTrajectories(void); static C8 SystemControlClearGeofences(void); I32 SystemControlDeleteFileDirectory(C8 * Path, C8 * ReturnValue, U8 Debug); @@ -781,6 +782,22 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { SystemControlCommand = Idle_0; } break; + case DeleteFileDirectory_1: + if (CurrentInputArgCount == CommandArgCount) { + SystemControlCommand = Idle_0; + memset(ControlResponseBuffer, 0, sizeof (ControlResponseBuffer)); + *ControlResponseBuffer = + SystemControlDeleteGenericFile(SystemControlArgument[0], sizeof (SystemControlArgument[0])); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "DeleteFileDirectory:", + ControlResponseBuffer, 1, &ClientSocket, 0); + } + else { + LogMessage(LOG_LEVEL_ERROR, + "Wrong parameter count in DeleteFileDirectory(path)! got:%d, expected:%d", + CurrentInputArgCount, CommandArgCount); + SystemControlCommand = Idle_0; + } + break; case ClearTrajectories_0: if (CurrentInputArgCount == CommandArgCount) { SystemControlCommand = Idle_0; @@ -2134,6 +2151,16 @@ C8 SystemControlDeleteGeofence(const C8 * geofenceName, const size_t nameLen) { return UtilDeleteGeofenceFile(geofenceName, nameLen) ? FAILED_DELETE : SUCCEEDED_DELETE; } +/*! + * \brief SystemControlDeleteGenericFile Deletes the chosen geofence + * \param trajectoryName Name of the geofence file + * \param nameLen Length of the name string + * \return Returns ::SUCCEDED_DELETE upon successfully deleting a file, otherwise ::FAILED_DELETE + */ +C8 SystemControlDeleteGenericFile(const C8 * filePath, const size_t nameLen) { + return UtilDeleteGenericFile(filePath, nameLen) ? FAILED_DELETE : SUCCEEDED_DELETE; +} + /*! * \brief SystemControlClearTrajectories Clears the trajectory directory on the machine * \return Returns ::SUCCEDED_DELETE upon successfully deleting a file, otherwise ::FAILED_DELETE. From 989397024b0febbde25837e68b1b4290c3efddbf Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 27 Mar 2020 14:34:53 +0100 Subject: [PATCH 404/523] Ran code formatter --- core/src/systemcontrol.c | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/core/src/systemcontrol.c b/core/src/systemcontrol.c index 4e9588350..2325a3dcd 100644 --- a/core/src/systemcontrol.c +++ b/core/src/systemcontrol.c @@ -132,7 +132,8 @@ typedef enum { Idle_0, GetServerStatus_0, ArmScenario_0, DisarmScenario_0, StartScenario_1, stop_0, AbortScenario_0, InitializeScenario_0, ConnectObject_0, DisconnectObject_0, GetServerParameterList_0, SetServerParameter_2, GetServerParameter_1, DownloadFile_1, UploadFile_4, CheckFileDirectoryExist_1, - GetRootDirectoryContent_0, GetDirectoryContent_1, DeleteTrajectory_1, DeleteGeofence_1, DeleteFileDirectory_1, + GetRootDirectoryContent_0, GetDirectoryContent_1, DeleteTrajectory_1, DeleteGeofence_1, + DeleteFileDirectory_1, ClearTrajectories_0, ClearGeofences_0, CreateDirectory_1, GetTestOrigin_0, replay_1, control_0, Exit_0, start_ext_trigg_1, nocommand @@ -143,7 +144,8 @@ static const char *SystemControlCommandsArr[] = { "AbortScenario_0", "InitializeScenario_0", "ConnectObject_0", "DisconnectObject_0", "GetServerParameterList_0", "SetServerParameter_2", "GetServerParameter_1", "DownloadFile_1", "UploadFile_4", "CheckFileDirectoryExist_1", - "GetRootDirectoryContent_0", "GetDirectoryContent_1", "DeleteTrajectory_1", "DeleteGeofence_1", "DeleteFileDirectory_1", + "GetRootDirectoryContent_0", "GetDirectoryContent_1", "DeleteTrajectory_1", "DeleteGeofence_1", + "DeleteFileDirectory_1", "ClearTrajectories_0", "ClearGeofences_0", "CreateDirectory_1", "GetTestOrigin_0", "replay_1", "control_0", @@ -787,7 +789,8 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { SystemControlCommand = Idle_0; memset(ControlResponseBuffer, 0, sizeof (ControlResponseBuffer)); *ControlResponseBuffer = - SystemControlDeleteGenericFile(SystemControlArgument[0], sizeof (SystemControlArgument[0])); + SystemControlDeleteGenericFile(SystemControlArgument[0], + sizeof (SystemControlArgument[0])); SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "DeleteFileDirectory:", ControlResponseBuffer, 1, &ClientSocket, 0); } From a1365928844b0d77cec82e61cb33fb72e533119c Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 27 Mar 2020 14:54:53 +0100 Subject: [PATCH 405/523] Added another OK condition for data dict initialization --- core/src/datadictionary.c | 3 +++ core/src/main.c | 4 +++- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/core/src/datadictionary.c b/core/src/datadictionary.c index 03d78f367..c4665eac1 100644 --- a/core/src/datadictionary.c +++ b/core/src/datadictionary.c @@ -98,6 +98,9 @@ ReadWriteAccess_t DataDictionaryConstructor(GSDType * GSD) { Res = Res == READ_OK ? DataDictionaryInitSupervisorTCPPortU16(GSD) : Res; Res = Res == READ_OK ? DataDictionaryInitMiscDataC8(GSD) : Res; Res = Res == READ_OK ? DataDictionaryInitMONR() : Res; + if (Res != WRITE_OK) { + LogMessage(LOG_LEVEL_WARNING, "Preexisting monitor data memory found"); + } DataDictionarySetOBCStateU8(GSD, OBC_STATE_UNDEFINED); diff --git a/core/src/main.c b/core/src/main.c index 3a78d874d..c64564a24 100644 --- a/core/src/main.c +++ b/core/src/main.c @@ -125,7 +125,9 @@ int main(int argc, char *argv[]) { // Initialise data dictionary LogMessage(LOG_LEVEL_INFO, "Initializing data dictionary"); dataDictOperationResult = DataDictionaryConstructor(GSD); - if (dataDictOperationResult != READ_OK && dataDictOperationResult != READ_WRITE_OK) { + if (dataDictOperationResult != READ_OK + && dataDictOperationResult != READ_WRITE_OK + && dataDictOperationResult != WRITE_OK) { util_error("Unable to initialize shared memory space"); } else { From 70111d25c212ae947f53e2bcba86f9c436480550 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 27 Mar 2020 14:55:23 +0100 Subject: [PATCH 406/523] Ran code formatter --- core/src/main.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/core/src/main.c b/core/src/main.c index c64564a24..bf5b403ba 100644 --- a/core/src/main.c +++ b/core/src/main.c @@ -126,8 +126,7 @@ int main(int argc, char *argv[]) { LogMessage(LOG_LEVEL_INFO, "Initializing data dictionary"); dataDictOperationResult = DataDictionaryConstructor(GSD); if (dataDictOperationResult != READ_OK - && dataDictOperationResult != READ_WRITE_OK - && dataDictOperationResult != WRITE_OK) { + && dataDictOperationResult != READ_WRITE_OK && dataDictOperationResult != WRITE_OK) { util_error("Unable to initialize shared memory space"); } else { From 665df149f129b8b73c775fa78a2a226b65caf19e Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 27 Mar 2020 15:04:16 +0100 Subject: [PATCH 407/523] Refactoring --- core/inc/datadictionary.h | 12 ++--- core/src/datadictionary.c | 102 +++++++++++++++++++------------------- 2 files changed, 57 insertions(+), 57 deletions(-) diff --git a/core/inc/datadictionary.h b/core/inc/datadictionary.h index 3b865bdd9..e7a5e3d5e 100644 --- a/core/inc/datadictionary.h +++ b/core/inc/datadictionary.h @@ -121,13 +121,13 @@ ReadWriteAccess_t DataDictionaryGetMiscDataC8(GSDType *GSD, C8 *MiscData, U32 Bu ReadWriteAccess_t DataDictionarySetOBCStateU8(GSDType *GSD, OBCState_t OBCState); OBCState_t DataDictionaryGetOBCStateU8(GSDType *GSD); -ReadWriteAccess_t DataDictionaryFreeMONR(); -ReadWriteAccess_t DataDictionaryInitMONR(); -ReadWriteAccess_t DataDictionarySetMONR(const MonitorDataType * MONR); -ReadWriteAccess_t DataDictionaryGetMONR(MonitorDataType * MONR, const uint32_t TransmitterId); +ReadWriteAccess_t DataDictionaryFreeMonitorData(); +ReadWriteAccess_t DataDictionaryInitMonitorData(); +ReadWriteAccess_t DataDictionarySetMonitorData(const MonitorDataType * monitorData); +ReadWriteAccess_t DataDictionaryGetMonitorData(MonitorDataType * monitorData, const uint32_t TransmitterId); -ReadWriteAccess_t DataDictionarySetNumberOfObjectsU8(const uint32_t newNumberOfObjects); -ReadWriteAccess_t DataDictionaryGetNumberOfObjectsU8(uint32_t *numberOfObjects); +ReadWriteAccess_t DataDictionarySetNumberOfObjects(const uint32_t newNumberOfObjects); +ReadWriteAccess_t DataDictionaryGetNumberOfObjects(uint32_t *numberOfObjects); ReadWriteAccess_t DataDictionaryConstructor(GSDType *GSD); ReadWriteAccess_t DataDictionaryDestructor(GSDType *GSD); diff --git a/core/src/datadictionary.c b/core/src/datadictionary.c index c4665eac1..0510bb671 100644 --- a/core/src/datadictionary.c +++ b/core/src/datadictionary.c @@ -52,7 +52,7 @@ static pthread_mutex_t OBCStateMutex = PTHREAD_MUTEX_INITIALIZER; #define MONR_DATA_FILENAME "MonitorData" -static volatile MonitorDataType *MONRMemory = NULL; +static volatile MonitorDataType *monitorDataMemory = NULL; /*------------------------------------------------------------ @@ -97,7 +97,7 @@ ReadWriteAccess_t DataDictionaryConstructor(GSDType * GSD) { Res = Res == READ_OK ? DataDictionaryInitRVSSRateU8(GSD) : Res; Res = Res == READ_OK ? DataDictionaryInitSupervisorTCPPortU16(GSD) : Res; Res = Res == READ_OK ? DataDictionaryInitMiscDataC8(GSD) : Res; - Res = Res == READ_OK ? DataDictionaryInitMONR() : Res; + Res = Res == READ_OK ? DataDictionaryInitMonitorData() : Res; if (Res != WRITE_OK) { LogMessage(LOG_LEVEL_WARNING, "Preexisting monitor data memory found"); } @@ -116,7 +116,7 @@ ReadWriteAccess_t DataDictionaryConstructor(GSDType * GSD) { ReadWriteAccess_t DataDictionaryDestructor(GSDType * GSD) { ReadWriteAccess_t result = WRITE_OK; - result = result == WRITE_OK ? DataDictionaryFreeMONR() : result; + result = result == WRITE_OK ? DataDictionaryFreeMonitorData() : result; return result; } @@ -1691,77 +1691,77 @@ OBCState_t DataDictionaryGetOBCStateU8(GSDType * GSD) { /*END OBCState*/ /*! - * \brief DataDictionaryInitMONR inits a data structure for saving object monr + * \brief DataDictionaryInitMonitorData inits a data structure for saving object monr * \return Result according to ::ReadWriteAccess_t */ -ReadWriteAccess_t DataDictionaryInitMONR() { +ReadWriteAccess_t DataDictionaryInitMonitorData() { int createdMemory; - MONRMemory = createSharedMemory(MONR_DATA_FILENAME, 0, sizeof (MonitorDataType), &createdMemory); - if (MONRMemory == NULL) { + monitorDataMemory = createSharedMemory(MONR_DATA_FILENAME, 0, sizeof (MonitorDataType), &createdMemory); + if (monitorDataMemory == NULL) { return UNDEFINED; } return createdMemory ? WRITE_OK : READ_OK; } /*! - * \brief DataDictionarySetMONR Parses input variable and sets variable to corresponding value - * \param MONRdata Monitor data + * \brief DataDictionarySetMonitorData Parses input variable and sets variable to corresponding value + * \param monitorData Monitor data * \param transmitterId requested object transmitterId * \return Result according to ::ReadWriteAccess_t */ -ReadWriteAccess_t DataDictionarySetMONR(const MonitorDataType * MONR) { +ReadWriteAccess_t DataDictionarySetMonitorData(const MonitorDataType * monitorData) { ReadWriteAccess_t result; - if (MONRMemory == NULL) { + if (monitorDataMemory == NULL) { errno = EINVAL; LogMessage(LOG_LEVEL_ERROR, "Shared memory not initialized"); return UNDEFINED; } - if (MONR == NULL) { + if (monitorData == NULL) { errno = EINVAL; LogMessage(LOG_LEVEL_ERROR, "Shared memory input pointer error"); return UNDEFINED; } - if (MONR->ClientID == 0) { + if (monitorData->ClientID == 0) { errno = EINVAL; LogMessage(LOG_LEVEL_ERROR, "Transmitter ID 0 is reserved"); return UNDEFINED; } - MONRMemory = claimSharedMemory(MONRMemory); + monitorDataMemory = claimSharedMemory(monitorDataMemory); result = PARAMETER_NOTFOUND; - unsigned int numberOfObjects = getNumberOfMemoryElements(MONRMemory); + unsigned int numberOfObjects = getNumberOfMemoryElements(monitorDataMemory); LogPrint("Number of objects currently in memory: %u", numberOfObjects); for (uint32_t i = 0; i < numberOfObjects; ++i) { - if (MONRMemory[i].ClientID == MONR->ClientID) { - memcpy(&MONRMemory[i], MONR, sizeof (MonitorDataType)); + if (monitorDataMemory[i].ClientID == monitorData->ClientID) { + memcpy(&monitorDataMemory[i], monitorData, sizeof (MonitorDataType)); result = WRITE_OK; } } if (result == PARAMETER_NOTFOUND) { // Search for unused memory space and place monitor data there - LogMessage(LOG_LEVEL_INFO, "Received first monitor data from transmitter ID %u", MONR->ClientID); + LogMessage(LOG_LEVEL_INFO, "Received first monitor data from transmitter ID %u", monitorData->ClientID); for (uint32_t i = 0; i < numberOfObjects; ++i) { - if (MONRMemory[i].ClientID == 0) { - memcpy(&MONRMemory[i], MONR, sizeof (MonitorDataType)); + if (monitorDataMemory[i].ClientID == 0) { + memcpy(&monitorDataMemory[i], monitorData, sizeof (MonitorDataType)); result = WRITE_OK; } } // No uninitialized memory space found - create new if (result == PARAMETER_NOTFOUND) { - MONRMemory = resizeSharedMemory(MONRMemory, numberOfObjects + 1); - if (MONRMemory != NULL) { - numberOfObjects = getNumberOfMemoryElements(MONRMemory); + monitorDataMemory = resizeSharedMemory(monitorDataMemory, numberOfObjects + 1); + if (monitorDataMemory != NULL) { + numberOfObjects = getNumberOfMemoryElements(monitorDataMemory); LogMessage(LOG_LEVEL_INFO, "Modified shared memory to hold MONR data for %u objects, mp now %p", - numberOfObjects, MONRMemory); - memcpy(&MONRMemory[numberOfObjects - 1], MONR, sizeof (MonitorDataType)); + numberOfObjects, monitorDataMemory); + memcpy(&monitorDataMemory[numberOfObjects - 1], monitorData, sizeof (MonitorDataType)); LogPrint("Printed MONR"); } else { @@ -1770,21 +1770,21 @@ ReadWriteAccess_t DataDictionarySetMONR(const MonitorDataType * MONR) { } } } - MONRMemory = releaseSharedMemory(MONRMemory); + monitorDataMemory = releaseSharedMemory(monitorDataMemory); return result; } /*! - * \brief DataDictionaryGetMONR Reads variable from shared memory - * \param MONRdata Return variable pointer + * \brief DataDictionaryGetMonitorData Reads variable from shared memory + * \param monitorData Return variable pointer * \param transmitterId requested object transmitterId * \return Result according to ::ReadWriteAccess_t */ -ReadWriteAccess_t DataDictionaryGetMONR(MonitorDataType * MONR, const uint32_t transmitterId) { +ReadWriteAccess_t DataDictionaryGetMonitorData(MonitorDataType * monitorData, const uint32_t transmitterId) { ReadWriteAccess_t result = PARAMETER_NOTFOUND; - if (MONR == NULL) { + if (monitorData == NULL) { errno = EINVAL; LogMessage(LOG_LEVEL_ERROR, "Shared memory input pointer error"); return UNDEFINED; @@ -1795,17 +1795,17 @@ ReadWriteAccess_t DataDictionaryGetMONR(MonitorDataType * MONR, const uint32_t t return UNDEFINED; } - MONRMemory = claimSharedMemory(MONRMemory); - unsigned int numberOfObjects = getNumberOfMemoryElements(MONRMemory); + monitorDataMemory = claimSharedMemory(monitorDataMemory); + unsigned int numberOfObjects = getNumberOfMemoryElements(monitorDataMemory); for (unsigned int i = 0; i < numberOfObjects; ++i) { - if (MONRMemory[i].ClientID == transmitterId) { - memcpy(MONR, &MONRMemory[i], sizeof (MonitorDataType)); + if (monitorDataMemory[i].ClientID == transmitterId) { + memcpy(monitorData, &monitorDataMemory[i], sizeof (MonitorDataType)); result = READ_OK; } } - MONRMemory = releaseSharedMemory(MONRMemory); + monitorDataMemory = releaseSharedMemory(monitorDataMemory); if (result == PARAMETER_NOTFOUND) { LogMessage(LOG_LEVEL_WARNING, "Unable to find monitor data for transmitter ID %u", transmitterId); @@ -1815,19 +1815,19 @@ ReadWriteAccess_t DataDictionaryGetMONR(MonitorDataType * MONR, const uint32_t t } /*! - * \brief DataDictionaryInitMONR initializes a data structure for saving object monitor data + * \brief DataDictionaryFreeMonitorData Releases data structure for saving object monitor data * \return Result according to ::ReadWriteAccess_t */ -ReadWriteAccess_t DataDictionaryFreeMONR() { +ReadWriteAccess_t DataDictionaryFreeMonitorData() { ReadWriteAccess_t result = WRITE_OK; - if (MONRMemory == NULL) { + if (monitorDataMemory == NULL) { errno = EINVAL; LogMessage(LOG_LEVEL_ERROR, "Attempt to free uninitialized memory"); return UNDEFINED; } - destroySharedMemory(MONRMemory); + destroySharedMemory(monitorDataMemory); return result; } @@ -1837,22 +1837,22 @@ ReadWriteAccess_t DataDictionaryFreeMONR() { /*NbrOfObjects*/ /*! - * \brief DataDictionarySetOBCStateU8 Sets the number of objects to the specified value and clears all + * \brief DataDictionarySetNumberOfObjects Sets the number of objects to the specified value and clears all * monitor data currently present in the system * \param numberOfobjects number of objects * \return Result according to ::ReadWriteAccess_t */ -ReadWriteAccess_t DataDictionarySetNumberOfObjectsU8(const uint32_t newNumberOfObjects) { +ReadWriteAccess_t DataDictionarySetNumberOfObjects(const uint32_t newNumberOfObjects) { unsigned int numberOfObjects; ReadWriteAccess_t result = WRITE_OK; - MONRMemory = claimSharedMemory(MONRMemory); - MONRMemory = resizeSharedMemory(MONRMemory, newNumberOfObjects); - numberOfObjects = getNumberOfMemoryElements(MONRMemory); - MONRMemory = releaseSharedMemory(MONRMemory); + monitorDataMemory = claimSharedMemory(monitorDataMemory); + monitorDataMemory = resizeSharedMemory(monitorDataMemory, newNumberOfObjects); + numberOfObjects = getNumberOfMemoryElements(monitorDataMemory); + monitorDataMemory = releaseSharedMemory(monitorDataMemory); - if (MONRMemory == NULL) { + if (monitorDataMemory == NULL) { errno = EINVAL; LogMessage(LOG_LEVEL_ERROR, "Error resizing shared memory"); return UNDEFINED; @@ -1862,15 +1862,15 @@ ReadWriteAccess_t DataDictionarySetNumberOfObjectsU8(const uint32_t newNumberOfO } /*! - * \brief DataDictionaryGetOBCStateU8 Reads variable from shared memory + * \brief DataDictionaryGetNumberOfObjects Reads variable from shared memory * \param numberOfobjects number of objects in a test * \return Current object control state according to ::OBCState_t */ -ReadWriteAccess_t DataDictionaryGetNumberOfObjectsU8(uint32_t * numberOfObjects) { +ReadWriteAccess_t DataDictionaryGetNumberOfObjects(uint32_t * numberOfObjects) { - MONRMemory = claimSharedMemory(MONRMemory); - *numberOfObjects = getNumberOfMemoryElements(MONRMemory); - MONRMemory = releaseSharedMemory(MONRMemory); + monitorDataMemory = claimSharedMemory(monitorDataMemory); + *numberOfObjects = getNumberOfMemoryElements(monitorDataMemory); + monitorDataMemory = releaseSharedMemory(monitorDataMemory); return READ_OK; } From 4caa848f5f8d5ba7ae8f69a67f8be35df7093f74 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 27 Mar 2020 15:04:37 +0100 Subject: [PATCH 408/523] Ran code formatter --- core/src/datadictionary.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/core/src/datadictionary.c b/core/src/datadictionary.c index 0510bb671..c866b6c59 100644 --- a/core/src/datadictionary.c +++ b/core/src/datadictionary.c @@ -1745,7 +1745,8 @@ ReadWriteAccess_t DataDictionarySetMonitorData(const MonitorDataType * monitorDa if (result == PARAMETER_NOTFOUND) { // Search for unused memory space and place monitor data there - LogMessage(LOG_LEVEL_INFO, "Received first monitor data from transmitter ID %u", monitorData->ClientID); + LogMessage(LOG_LEVEL_INFO, "Received first monitor data from transmitter ID %u", + monitorData->ClientID); for (uint32_t i = 0; i < numberOfObjects; ++i) { if (monitorDataMemory[i].ClientID == 0) { memcpy(&monitorDataMemory[i], monitorData, sizeof (MonitorDataType)); From c7d6e60600a15ee986bb0cb0cbdc7f60364b24be Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 31 Mar 2020 09:35:51 +0200 Subject: [PATCH 409/523] Ran code formatter --- core/src/systemcontrol.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/core/src/systemcontrol.c b/core/src/systemcontrol.c index 2325a3dcd..9d8d13541 100644 --- a/core/src/systemcontrol.c +++ b/core/src/systemcontrol.c @@ -133,7 +133,7 @@ typedef enum { InitializeScenario_0, ConnectObject_0, DisconnectObject_0, GetServerParameterList_0, SetServerParameter_2, GetServerParameter_1, DownloadFile_1, UploadFile_4, CheckFileDirectoryExist_1, GetRootDirectoryContent_0, GetDirectoryContent_1, DeleteTrajectory_1, DeleteGeofence_1, - DeleteFileDirectory_1, + DeleteFileDirectory_1, ClearTrajectories_0, ClearGeofences_0, CreateDirectory_1, GetTestOrigin_0, replay_1, control_0, Exit_0, start_ext_trigg_1, nocommand @@ -145,7 +145,7 @@ static const char *SystemControlCommandsArr[] = { "ConnectObject_0", "DisconnectObject_0", "GetServerParameterList_0", "SetServerParameter_2", "GetServerParameter_1", "DownloadFile_1", "UploadFile_4", "CheckFileDirectoryExist_1", "GetRootDirectoryContent_0", "GetDirectoryContent_1", "DeleteTrajectory_1", "DeleteGeofence_1", - "DeleteFileDirectory_1", + "DeleteFileDirectory_1", "ClearTrajectories_0", "ClearGeofences_0", "CreateDirectory_1", "GetTestOrigin_0", "replay_1", "control_0", From 99feb94ecc6255ec492d184d81c338550c0f9ba2 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 31 Mar 2020 10:41:29 +0200 Subject: [PATCH 410/523] Added missing linkage in core CMakeLists --- core/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index 2e7a9c501..3ad112d9d 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -100,6 +100,7 @@ target_link_libraries(${COREUTILS_TARGET} LINK_PUBLIC ${LOGGING_LIBRARY} ${MESSAGE_BUS_LIBRARY} ${MATH_LIBRARY} + ${POSITIONING_TARGET} ) # Installation rules From 0e6ad722e77adbf79dc7d29715c1b8d17df2f4e7 Mon Sep 17 00:00:00 2001 From: Sebastian Loh Lindholm Date: Tue, 31 Mar 2020 14:05:06 +0200 Subject: [PATCH 411/523] Fixed format errors. --- core/src/systemcontrol.c | 55 ++++++++++++++++++++-------------------- 1 file changed, 28 insertions(+), 27 deletions(-) diff --git a/core/src/systemcontrol.c b/core/src/systemcontrol.c index 48e8f8e30..cd71b7ce5 100644 --- a/core/src/systemcontrol.c +++ b/core/src/systemcontrol.c @@ -745,8 +745,8 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { FileLengthI32 = SystemControlBuildFileContentInfo("dir.info", 0); SystemControlFileDownloadResponse(FileLengthI32, &ClientSocket, 0); SystemControlSendFileContent(&ClientSocket, "dir.info", - STR_SYSTEM_CONTROL_TX_PACKET_SIZE, SystemControlDirectoryInfo.info_buffer, - KEEP_FILE, 0); + STR_SYSTEM_CONTROL_TX_PACKET_SIZE, + SystemControlDirectoryInfo.info_buffer, KEEP_FILE, 0); SystemControlDestroyFileContentInfo("dir.info", 1); } @@ -788,8 +788,8 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { FileLengthI32 = SystemControlBuildFileContentInfo(SystemControlArgument[0], 0); SystemControlFileDownloadResponse(FileLengthI32, &ClientSocket, 0); SystemControlSendFileContent(&ClientSocket, SystemControlArgument[0], - STR_SYSTEM_CONTROL_TX_PACKET_SIZE, SystemControlDirectoryInfo.info_buffer, - KEEP_FILE, 0); + STR_SYSTEM_CONTROL_TX_PACKET_SIZE, + SystemControlDirectoryInfo.info_buffer, KEEP_FILE, 0); SystemControlDestroyFileContentInfo(SystemControlArgument[0], 0); } @@ -1380,15 +1380,16 @@ void SystemControlSendControlResponse(U16 ResponseStatus, C8 * ResponseString, C void SystemControlFileDownloadResponse(I32 FileLength, I32 * Sockfd, U8 Debug) { I32 n, i; C8 Length[4]; + n = FileLength; Length[0] = (C8) (n >> 24); Length[1] = (C8) (n >> 16); Length[2] = (C8) (n >> 8); Length[3] = (C8) n; - + if (Debug) { for (i = 0; i < 4; i++) - printf("%x-", Length[i]); + printf("%x-", Length[i]); printf("\n"); } @@ -2006,7 +2007,7 @@ I32 SystemControlBuildFileContentInfo(C8 * Path, U8 Debug) { if (Debug) LogMessage(LOG_LEVEL_DEBUG, "Filesize %d of %s", (I32) st.st_size, CompletePath); - + return st.st_size; */ struct stat st; @@ -2018,31 +2019,31 @@ I32 SystemControlBuildFileContentInfo(C8 * Path, U8 Debug) { if (SystemControlDirectoryInfo.exist) return -1; - + UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); strcat(CompletePath, Path); stat(CompletePath, &st); /* - // Create new temporary file, containing the length of the current file in hex + the rest of the document - strcat(temporaryCompletePath, ".temp"); - FILE *comp_fd = fopen(CompletePath, "r"); - FILE *temp_fd = fopen(temporaryCompletePath, "w"); + // Create new temporary file, containing the length of the current file in hex + the rest of the document + strcat(temporaryCompletePath, ".temp"); + FILE *comp_fd = fopen(CompletePath, "r"); + FILE *temp_fd = fopen(temporaryCompletePath, "w"); - fprintf(temp_fd, "%c%c%c%c", - (U8) (st.st_size >> 24), (U8) (st.st_size >> 16), (U8) (st.st_size >> 8), (U8) (st.st_size) - ); + fprintf(temp_fd, "%c%c%c%c", + (U8) (st.st_size >> 24), (U8) (st.st_size >> 16), (U8) (st.st_size >> 8), (U8) (st.st_size) + ); - while (!feof(comp_fd)) { - fputc(fgetc(comp_fd), temp_fd); - } + while (!feof(comp_fd)) { + fputc(fgetc(comp_fd), temp_fd); + } - fclose(comp_fd); - fclose(temp_fd); + fclose(comp_fd); + fclose(temp_fd); - // Rename the temporary file to the name of the previous one - rename(temporaryCompletePath, CompletePath); - stat(CompletePath, &st); - */ + // Rename the temporary file to the name of the previous one + rename(temporaryCompletePath, CompletePath); + stat(CompletePath, &st); + */ // Create mmap of the file and return the length SystemControlDirectoryInfo.fd = open(CompletePath, O_RDWR); SystemControlDirectoryInfo.info_buffer = @@ -2064,9 +2065,9 @@ I32 SystemControlDestroyFileContentInfo(C8 * Path, U8 RemoveFile) { munmap(SystemControlDirectoryInfo.info_buffer, SystemControlDirectoryInfo.size); close(SystemControlDirectoryInfo.fd); SystemControlDirectoryInfo.exist = 0; - if(RemoveFile == 1){ - remove(CompletePath); - } + if (RemoveFile == 1) { + remove(CompletePath); + } return 0; } From 767398da38108c4e502f5538fbc707d0d9843324 Mon Sep 17 00:00:00 2001 From: Sebastian Loh Lindholm Date: Tue, 31 Mar 2020 16:14:34 +0200 Subject: [PATCH 412/523] Change UploadFile log message. --- core/src/systemcontrol.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/core/src/systemcontrol.c b/core/src/systemcontrol.c index cd71b7ce5..05fb10512 100644 --- a/core/src/systemcontrol.c +++ b/core/src/systemcontrol.c @@ -837,7 +837,7 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { } else { LogMessage(LOG_LEVEL_ERROR, - "Wrong parameter count in PrepFileRx(path, filesize, packetsize)!"); + "Wrong parameter count in UploadFile(path, filesize, packetsize, filetype)!"); SystemControlCommand = Idle_0; } break; From dbf229cad74905309db48b36b853442b4c66e6bd Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Wed, 1 Apr 2020 14:45:21 +0200 Subject: [PATCH 413/523] Added more conf --- core/src/objectcontrol.c | 6 +++--- core/src/systemcontrol.c | 6 +++--- util | 2 +- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/core/src/objectcontrol.c b/core/src/objectcontrol.c index 194c6232b..4478a1589 100644 --- a/core/src/objectcontrol.c +++ b/core/src/objectcontrol.c @@ -166,7 +166,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { }; const struct timeval adaptiveSyncMessagePeriod = heartbeatPeriod; - U8 iForceObjectToLocalhostU8 = 0; + U8 iForceObjectToLocalhostU8 = DEFAULT_FORCE_OBJECT_TO_LOCALHOST; FILE *fd; C8 Timestamp[SMALL_BUFFER_SIZE_0]; @@ -213,7 +213,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { dbl ASPFilterLevelDbl = DEFAULT_ASP_FILTER_LEVEL; dbl ASPMaxDeltaTimeDbl = DEFAULT_ASP_MAX_DELTA_TIME; I32 ASPDebugRate = 1; - I32 ASPStepBackCount = 0; + I32 ASPStepBackCount = DEFAULT_ASP_STEP_BACK_COUNT; char confDirectoryPath[MAX_FILE_PATH]; ControlCenterStatusType objectControlServerStatus = CONTROL_CENTER_STATUS_INIT; @@ -1315,7 +1315,7 @@ int iFindObjectsInfo(C8 object_traj_file[MAX_OBJECTS][MAX_FILE_PATH], I32 * nbr_objects) { DIR *traj_directory; struct dirent *directory_entry; - int iForceObjectToLocalhost; + int iForceObjectToLocalhost = DEFAULT_FORCE_OBJECT_TO_LOCALHOST; struct sockaddr_in sockaddr; int result; char trajPathDir[MAX_FILE_PATH]; diff --git a/core/src/systemcontrol.c b/core/src/systemcontrol.c index 7e571c5a3..a04961f21 100644 --- a/core/src/systemcontrol.c +++ b/core/src/systemcontrol.c @@ -297,7 +297,7 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { C8 RVSSData[SYSTEM_CONTROL_RVSS_DATA_BUFFER]; U16 RVSSSendCounterU16 = 0; - U32 RVSSConfigU32; + U32 RVSSConfigU32 = DEFAULT_RVSS_CONF; U32 RVSSMessageLengthU32; U16 PCDMessageCodeU16; C8 RxFilePath[MAX_FILE_PATH]; @@ -316,8 +316,8 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { DataDictionaryGetRVSSConfigU32(GSD, &RVSSConfigU32); LogMessage(LOG_LEVEL_INFO, "RVSSConfigU32 = %d", RVSSConfigU32); - U8 RVSSRateU8; - dbl RVSSRateDbl; + U8 RVSSRateU8 = DEFAULT_RVSS_RATE; + dbl RVSSRateDbl = DEFAULT_RVSS_RATE; DataDictionaryGetRVSSRateU8(GSD, &RVSSRateU8); RVSSRateDbl = RVSSRateU8; diff --git a/util b/util index 80f2fd821..19a42f2de 160000 --- a/util +++ b/util @@ -1 +1 @@ -Subproject commit 80f2fd821c909596e3caa6c23e8d4a1e3aedae82 +Subproject commit 19a42f2dec489753d31322e0527de876c1b18660 From f918f964eb62f5cf723a183b5ec54d8cd47595b8 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 1 Apr 2020 15:16:24 +0200 Subject: [PATCH 414/523] Implemented CCITT checksum generator based on ISO specification --- core/src/iso22133.c | 124 +++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 117 insertions(+), 7 deletions(-) diff --git a/core/src/iso22133.c b/core/src/iso22133.c index a46501dc7..e21cac12a 100644 --- a/core/src/iso22133.c +++ b/core/src/iso22133.c @@ -47,6 +47,7 @@ static const uint8_t SupportedProtocolVersions[] = { 2 }; #define ACCELERATION_UNAVAILABLE_VALUE 32001 #define ACCELERATION_ONE_METER_PER_SECOND_SQUARED_VALUE 1000 #define RELATIVE_TIME_ONE_SECOND_VALUE 1000 +#define CRC_INIT_VALUE 0x0000 typedef enum { ISO_DRIVE_DIRECTION_FORWARD = 0, @@ -449,6 +450,9 @@ static char isValidMessageID(const uint16_t id); static double_t mapISOHeadingToHostHeading(const double_t isoHeading_rad); static double_t mapHostHeadingToISOHeading(const double_t hostHeading_rad); static void convertMONRToHostRepresentation(const MONRType * MONRData, ObjectMonitorType * monitorData); +static ISOMessageReturnValue verifyChecksum(const void* data, const size_t dataLen, const uint16_t crc, const char debug); +static uint16_t crcByte(const uint16_t crc, const uint8_t byte); +static uint16_t crc16(const uint8_t* data, size_t dataLen); // ************************** static variables ******************************************************************** static uint16_t trajectoryMessageCrc = 0; @@ -561,7 +565,6 @@ ISOMessageReturnValue decodeISOFooter(const char *MessageBuffer, const size_t le LogPrint("Decoded ISO footer:\n\tCRC: 0x%x", FooterData->Crc); } - // TODO: check on CRC return MESSAGE_OK; } @@ -614,14 +617,15 @@ HeaderType buildISOHeader(ISOMessageID id, uint32_t messageLength, const char de FooterType buildISOFooter(const void *message, const size_t messageSize, const char debug) { FooterType footer; - // TODO: Calculate CRC - remembering that message begins with header and messageSize will include header and footer - footer.Crc = 0; - footer.Crc = htole16(footer.Crc); + // Calculate CRC - remembering that message begins with header and messageSize will include header and footer + footer.Crc = crc16(message, messageSize - sizeof (FooterType)); if (debug) { LogPrint("Encoded ISO footer:\n\tCRC: 0x%x", footer.Crc); } + footer.Crc = htole16(footer.Crc); + return footer; } @@ -668,6 +672,90 @@ ISOMessageID getISOMessageType(const char *messageData, const size_t length, con } +/*! + * \brief crcByte Updates the given CRC based on an input byte from data + * \param crc CRC from previous byte + * \param byte New data byte + * \return New CRC value + */ +uint16_t crcByte(const uint16_t crc, const uint8_t byte) { + static const uint16_t crcTable[256] = { + 0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7, + 0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF, + 0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6, + 0x9339, 0x8318, 0xB37B, 0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE, + 0x2462, 0x3443, 0x0420, 0x1401, 0x64E6, 0x74C7, 0x44A4, 0x5485, + 0xA56A, 0xB54B, 0x8528, 0x9509, 0xE5EE, 0xF5CF, 0xC5AC, 0xD58D, + 0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, 0x46B4, + 0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC, + 0x48C4, 0x58E5, 0x6886, 0x78A7, 0x0840, 0x1861, 0x2802, 0x3823, + 0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, 0x9969, 0xA90A, 0xB92B, + 0x5AF5, 0x4AD4, 0x7AB7, 0x6A96, 0x1A71, 0x0A50, 0x3A33, 0x2A12, + 0xDBFD, 0xCBDC, 0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A, + 0x6CA6, 0x7C87, 0x4CE4, 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41, + 0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, 0xAD2A, 0xBD0B, 0x8D68, 0x9D49, + 0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70, + 0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A, 0x9F59, 0x8F78, + 0x9188, 0x81A9, 0xB1CA, 0xA1EB, 0xD10C, 0xC12D, 0xF14E, 0xE16F, + 0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, 0x6067, + 0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E, + 0x02B1, 0x1290, 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256, + 0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D, + 0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405, + 0xA7DB, 0xB7FA, 0x8799, 0x97B8, 0xE75F, 0xF77E, 0xC71D, 0xD73C, + 0x26D3, 0x36F2, 0x0691, 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634, + 0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9, 0xB98A, 0xA9AB, + 0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882, 0x28A3, + 0xCB7D, 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A, + 0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92, + 0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9, + 0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1, + 0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8, + 0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0 + }; + + return (uint16_t)((crc << 8) ^ crcTable[(crc >> 8) ^ byte]); +} + + +/*! + * \brief crc16 Calculates the 16 bit CCITT checksum value for the polynomial + * x^16 + x^12 + x^5 + 1 + * \param data Block of data for which CRC is to be calculated + * \param dataLen Length of the block of data + * \return CRC checksum + */ +uint16_t crc16(const uint8_t* data, size_t dataLen) { + uint16_t crc = CRC_INIT_VALUE; + while (dataLen-- > 0) { + crc = crcByte(crc, *data++); + } + return crc; +} + + +/*! + * \brief verifyChecksum Generates a checksum for specified data and checks if it matches against + * the specified CRC. If the specified CRC is 0, the message does not contain a CRC value + * and the message is assumed uncorrupted. + * \param data Data for which checksum is to be verified + * \param dataLen Length of the data + * \param CRC Received CRC value for the data + * \return Value according to ::ISOMessageReturnValue + */ +ISOMessageReturnValue verifyChecksum(const void* data, const size_t dataLen, const uint16_t CRC, const char debug) { + if (CRC == 0) { + return MESSAGE_OK; + } + + const uint16_t dataCRC = crc16(data, dataLen); + if (debug) { + LogPrint("CRC given: %u, CRC calculated: %u", CRC, dataCRC); + } + return dataCRC == CRC ? MESSAGE_OK : MESSAGE_CRC_ERROR; +} + + /*! * \brief encodeTRAJMessageHeader Creates a TRAJ message header based on supplied values and resets * an internal CRC to be used in the corresponding footer. The header is printed to a buffer. @@ -690,6 +778,7 @@ ssize_t encodeTRAJMessageHeader(const uint16_t trajectoryID, const uint16_t traj const size_t bufferLength, const char debug) { TRAJHeaderType TRAJData; + size_t dataLen; memset(trajDataBuffer, 0, bufferLength); @@ -763,11 +852,16 @@ ssize_t encodeTRAJMessageHeader(const uint16_t trajectoryID, const uint16_t traj TRAJData.trajectoryNameValueID = htole16(TRAJData.trajectoryNameValueID); TRAJData.trajectoryNameContentLength = htole16(TRAJData.trajectoryNameContentLength); - trajectoryMessageCrc = 0; + // Reset CRC + trajectoryMessageCrc = CRC_INIT_VALUE; memcpy(trajDataBuffer, &TRAJData, sizeof (TRAJData)); - // TODO update CRC based on header data + // Update CRC + dataLen = sizeof (TRAJData); + while (dataLen-- > 0) { + trajectoryMessageCrc = crcByte(trajectoryMessageCrc, (uint8_t)(*trajDataBuffer++)); + } return sizeof (TRAJHeaderType); } @@ -793,6 +887,7 @@ ssize_t encodeTRAJMessagePoint(const struct timeval *pointTimeFromStart, const C const size_t remainingBufferLength, const char debug) { TRAJPointType TRAJData; + size_t dataLen; if (remainingBufferLength < sizeof (TRAJPointType)) { errno = ENOBUFS; @@ -950,7 +1045,12 @@ ssize_t encodeTRAJMessagePoint(const struct timeval *pointTimeFromStart, const C memcpy(trajDataBufferPointer, &TRAJData, sizeof (TRAJData)); - // TODO update CRC based on printed data + // Update CRC + dataLen = sizeof (TRAJData); + while (dataLen-- > 0) { + trajectoryMessageCrc = crcByte(trajectoryMessageCrc, (uint8_t)(*trajDataBufferPointer++)); + } + return sizeof (TRAJPointType); } @@ -984,6 +1084,10 @@ ssize_t encodeTRAJMessageFooter(char *trajDataBuffer, const size_t remainingBuff memcpy(trajDataBuffer, &TRAJData, sizeof (TRAJData)); + if (debug) { + LogPrint("Encoded ISO footer:\n\tCRC: 0x%x", TRAJData.footer.Crc); + } + return sizeof (TRAJFooterType); } @@ -1452,6 +1556,12 @@ ISOMessageReturnValue decodeMONRMessage(const char *monrDataBuffer, const size_t return retval; } + if ((retval = verifyChecksum(&MONRData, sizeof (MONRData) - sizeof (MONRData.footer), + MONRData.footer.Crc, debug)) == MESSAGE_CRC_ERROR) { + LogMessage(LOG_LEVEL_WARNING, "MONR checksum error"); + return retval; + } + if (debug) { LogPrint("MONR:"); LogPrint("SyncWord = %x", MONRData.header.SyncWordU16); From 9261c9a7bd230601e645d1883058068d3eca317f Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 1 Apr 2020 15:28:45 +0200 Subject: [PATCH 415/523] Ran code formatter --- core/src/iso22133.c | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/core/src/iso22133.c b/core/src/iso22133.c index e21cac12a..b09bb49ba 100644 --- a/core/src/iso22133.c +++ b/core/src/iso22133.c @@ -450,9 +450,10 @@ static char isValidMessageID(const uint16_t id); static double_t mapISOHeadingToHostHeading(const double_t isoHeading_rad); static double_t mapHostHeadingToISOHeading(const double_t hostHeading_rad); static void convertMONRToHostRepresentation(const MONRType * MONRData, ObjectMonitorType * monitorData); -static ISOMessageReturnValue verifyChecksum(const void* data, const size_t dataLen, const uint16_t crc, const char debug); +static ISOMessageReturnValue verifyChecksum(const void *data, const size_t dataLen, const uint16_t crc, + const char debug); static uint16_t crcByte(const uint16_t crc, const uint8_t byte); -static uint16_t crc16(const uint8_t* data, size_t dataLen); +static uint16_t crc16(const uint8_t * data, size_t dataLen); // ************************** static variables ******************************************************************** static uint16_t trajectoryMessageCrc = 0; @@ -714,7 +715,7 @@ uint16_t crcByte(const uint16_t crc, const uint8_t byte) { 0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0 }; - return (uint16_t)((crc << 8) ^ crcTable[(crc >> 8) ^ byte]); + return (uint16_t) ((crc << 8) ^ crcTable[(crc >> 8) ^ byte]); } @@ -725,8 +726,9 @@ uint16_t crcByte(const uint16_t crc, const uint8_t byte) { * \param dataLen Length of the block of data * \return CRC checksum */ -uint16_t crc16(const uint8_t* data, size_t dataLen) { +uint16_t crc16(const uint8_t * data, size_t dataLen) { uint16_t crc = CRC_INIT_VALUE; + while (dataLen-- > 0) { crc = crcByte(crc, *data++); } @@ -743,12 +745,14 @@ uint16_t crc16(const uint8_t* data, size_t dataLen) { * \param CRC Received CRC value for the data * \return Value according to ::ISOMessageReturnValue */ -ISOMessageReturnValue verifyChecksum(const void* data, const size_t dataLen, const uint16_t CRC, const char debug) { +ISOMessageReturnValue verifyChecksum(const void *data, const size_t dataLen, const uint16_t CRC, + const char debug) { if (CRC == 0) { return MESSAGE_OK; } const uint16_t dataCRC = crc16(data, dataLen); + if (debug) { LogPrint("CRC given: %u, CRC calculated: %u", CRC, dataCRC); } @@ -860,7 +864,7 @@ ssize_t encodeTRAJMessageHeader(const uint16_t trajectoryID, const uint16_t traj // Update CRC dataLen = sizeof (TRAJData); while (dataLen-- > 0) { - trajectoryMessageCrc = crcByte(trajectoryMessageCrc, (uint8_t)(*trajDataBuffer++)); + trajectoryMessageCrc = crcByte(trajectoryMessageCrc, (uint8_t) (*trajDataBuffer++)); } return sizeof (TRAJHeaderType); } @@ -1048,7 +1052,7 @@ ssize_t encodeTRAJMessagePoint(const struct timeval *pointTimeFromStart, const C // Update CRC dataLen = sizeof (TRAJData); while (dataLen-- > 0) { - trajectoryMessageCrc = crcByte(trajectoryMessageCrc, (uint8_t)(*trajDataBufferPointer++)); + trajectoryMessageCrc = crcByte(trajectoryMessageCrc, (uint8_t) (*trajDataBufferPointer++)); } return sizeof (TRAJPointType); From 8ab62c5c0d849a626d6d0c1091e71984d1780d58 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 2 Apr 2020 13:09:56 +0200 Subject: [PATCH 416/523] WIP --- core/src/iso22133.c | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/core/src/iso22133.c b/core/src/iso22133.c index b09bb49ba..7a0e50cb1 100644 --- a/core/src/iso22133.c +++ b/core/src/iso22133.c @@ -47,7 +47,9 @@ static const uint8_t SupportedProtocolVersions[] = { 2 }; #define ACCELERATION_UNAVAILABLE_VALUE 32001 #define ACCELERATION_ONE_METER_PER_SECOND_SQUARED_VALUE 1000 #define RELATIVE_TIME_ONE_SECOND_VALUE 1000 -#define CRC_INIT_VALUE 0x0000 + +#define DEFAULT_CRC_INIT_VALUE 0x0000 +#define DEFAULT_CRC_CHECK_ENABLED 1 typedef enum { ISO_DRIVE_DIRECTION_FORWARD = 0, @@ -457,6 +459,7 @@ static uint16_t crc16(const uint8_t * data, size_t dataLen); // ************************** static variables ******************************************************************** static uint16_t trajectoryMessageCrc = 0; +static int8_t isCRCCheckEnabled = DEFAULT_CRC_CHECK_ENABLED; // ************************** function definitions **************************************************************** @@ -727,7 +730,7 @@ uint16_t crcByte(const uint16_t crc, const uint8_t byte) { * \return CRC checksum */ uint16_t crc16(const uint8_t * data, size_t dataLen) { - uint16_t crc = CRC_INIT_VALUE; + uint16_t crc = DEFAULT_CRC_INIT_VALUE; while (dataLen-- > 0) { crc = crcByte(crc, *data++); @@ -857,7 +860,7 @@ ssize_t encodeTRAJMessageHeader(const uint16_t trajectoryID, const uint16_t traj TRAJData.trajectoryNameContentLength = htole16(TRAJData.trajectoryNameContentLength); // Reset CRC - trajectoryMessageCrc = CRC_INIT_VALUE; + trajectoryMessageCrc = DEFAULT_CRC_INIT_VALUE; memcpy(trajDataBuffer, &TRAJData, sizeof (TRAJData)); From 4413704c84c2032a571a6d7e9a9c19323789cbc7 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 2 Apr 2020 16:06:23 +0200 Subject: [PATCH 417/523] Added ability to disable CRC verification --- core/inc/iso22133.h | 2 +- core/src/iso22133.c | 14 ++++++++++++-- 2 files changed, 13 insertions(+), 3 deletions(-) diff --git a/core/inc/iso22133.h b/core/inc/iso22133.h index faae10143..26683e2d0 100644 --- a/core/inc/iso22133.h +++ b/core/inc/iso22133.h @@ -208,7 +208,7 @@ ssize_t encodeACCMMessage(const uint16_t* actionID, const ActionType_t* actionTy ssize_t encodeEXACMessage(const uint16_t* actionID, const struct timeval * executionTime, char * exacDataBuffer, const size_t bufferLength, const char debug); ssize_t encodeINSUPMessage(const SupervisorCommandType, char * insupDataBuffer, const size_t bufferLength, const char debug); ISOMessageID getISOMessageType(const char * messageData, const size_t length, const char debug); - +void setISOCRCCheck(const int8_t enabled); #ifdef __cplusplus } diff --git a/core/src/iso22133.c b/core/src/iso22133.c index 7a0e50cb1..4cc48bf26 100644 --- a/core/src/iso22133.c +++ b/core/src/iso22133.c @@ -459,7 +459,7 @@ static uint16_t crc16(const uint8_t * data, size_t dataLen); // ************************** static variables ******************************************************************** static uint16_t trajectoryMessageCrc = 0; -static int8_t isCRCCheckEnabled = DEFAULT_CRC_CHECK_ENABLED; +static int8_t isCRCVerificationEnabled = DEFAULT_CRC_CHECK_ENABLED; // ************************** function definitions **************************************************************** @@ -750,7 +750,7 @@ uint16_t crc16(const uint8_t * data, size_t dataLen) { */ ISOMessageReturnValue verifyChecksum(const void *data, const size_t dataLen, const uint16_t CRC, const char debug) { - if (CRC == 0) { + if (!isCRCVerificationEnabled || CRC == 0) { return MESSAGE_OK; } @@ -762,6 +762,16 @@ ISOMessageReturnValue verifyChecksum(const void *data, const size_t dataLen, con return dataCRC == CRC ? MESSAGE_OK : MESSAGE_CRC_ERROR; } +/*! + * \brief setISOCRCVerification Enables or disables checksum verification on received messages (default + * is to enable checksum verification) + * \param enabled Boolean for enabling or disabling the checksum verification + */ +void setISOCRCVerification(const int8_t enabled) { + isCRCVerificationEnabled = enabled ? 1 : 0; + return; +} + /*! * \brief encodeTRAJMessageHeader Creates a TRAJ message header based on supplied values and resets From 1fa96c4021c460f9fe6462ab874d4380132d5823 Mon Sep 17 00:00:00 2001 From: Sebastian Loh Lindholm Date: Thu, 2 Apr 2020 16:23:08 +0200 Subject: [PATCH 418/523] Added Sub... string to the second response from GetDirectoryContent and DownloadFile command. --- core/src/systemcontrol.c | 49 ++++++++++++++++++++++++++++------------ 1 file changed, 35 insertions(+), 14 deletions(-) diff --git a/core/src/systemcontrol.c b/core/src/systemcontrol.c index 635cfc687..814781112 100644 --- a/core/src/systemcontrol.c +++ b/core/src/systemcontrol.c @@ -180,8 +180,9 @@ void SystemControlSendControlResponse(U16 ResponseStatus, C8 * ResponseString, C I32 ResponseDataLength, I32 * Sockfd, U8 Debug); I32 SystemControlBuildControlResponse(U16 ResponseStatus, C8 * ResponseString, C8 * ResponseData, I32 ResponseDataLength, U8 Debug); -void SystemControlFileDownloadResponse(I32 FileLength, I32 * Sockfd, U8 Debug); -void SystemControlSendLog(C8 * LogString, I32 * Sockfd, U8 Debug); +void SystemControlFileDownloadResponse(U16 ResponseStatus, C8 * ResponseString, + I32 ResponseDataLength, I32 * Sockfd, U8 Debug); + void SystemControlSendLog(C8 * LogString, I32 * Sockfd, U8 Debug); void SystemControlSendMONR(C8 * LogString, I32 * Sockfd, U8 Debug); static void SystemControlCreateProcessChannel(const C8 * name, const U32 port, I32 * sockfd, struct sockaddr_in *addr); @@ -735,7 +736,8 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { UtilCreateDirContent(SystemControlArgument[0], "dir.info"); bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); FileLengthI32 = SystemControlBuildFileContentInfo("dir.info", 0); - SystemControlFileDownloadResponse(FileLengthI32, &ClientSocket, 0); + SystemControlFileDownloadResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "SubGetDirectoryContent:", + FileLengthI32, &ClientSocket, 0); SystemControlSendFileContent(&ClientSocket, "dir.info", STR_SYSTEM_CONTROL_TX_PACKET_SIZE, SystemControlDirectoryInfo.info_buffer, KEEP_FILE, 0); @@ -840,7 +842,8 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { if (ControlResponseBuffer[0] == FILE_EXIST) { bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); FileLengthI32 = SystemControlBuildFileContentInfo(SystemControlArgument[0], 0); - SystemControlFileDownloadResponse(FileLengthI32, &ClientSocket, 0); + SystemControlFileDownloadResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "SubDownloadFile:", + FileLengthI32, &ClientSocket, 0); SystemControlSendFileContent(&ClientSocket, SystemControlArgument[0], STR_SYSTEM_CONTROL_TX_PACKET_SIZE, SystemControlDirectoryInfo.info_buffer, KEEP_FILE, 0); @@ -1431,25 +1434,43 @@ void SystemControlSendControlResponse(U16 ResponseStatus, C8 * ResponseString, C } -void SystemControlFileDownloadResponse(I32 FileLength, I32 * Sockfd, U8 Debug) { - I32 n, i; +void SystemControlFileDownloadResponse(U16 ResponseStatus, C8 * ResponseString, + I32 ResponseDataLength, I32 * Sockfd, U8 Debug) { + int i, n, j, t; C8 Length[4]; + C8 Status[2]; + C8 Data[SYSTEM_CONTROL_SEND_BUFFER_SIZE]; - n = FileLength; + bzero(Data, SYSTEM_CONTROL_SEND_BUFFER_SIZE); + n = 2 + strlen(ResponseString) + ResponseDataLength; Length[0] = (C8) (n >> 24); Length[1] = (C8) (n >> 16); Length[2] = (C8) (n >> 8); Length[3] = (C8) n; + Status[0] = (C8) (ResponseStatus >> 8); + Status[1] = (C8) ResponseStatus; - if (Debug) { - for (i = 0; i < 4; i++) - printf("%x-", Length[i]); - printf("\n"); - } + if (n + 4 < SYSTEM_CONTROL_SEND_BUFFER_SIZE) { + for (i = 0, j = 0; i < 4; i++, j++) + Data[j] = Length[i]; + for (i = 0; i < 2; i++, j++) + Data[j] = Status[i]; + t = strlen(ResponseString); + for (i = 0; i < t; i++, j++) + Data[j] = *(ResponseString + i); - UtilSendTCPData("System Control", Length, 4, Sockfd, 0); -} + if (Debug) { + for (i = 0; i < n + 4; i++) + printf("%x-", Data[i]); + printf("\n"); + } + //SystemControlSendBytes(Data, n + 4, Sockfd, 0); + UtilSendTCPData("System Control", Data, 6+strlen(ResponseString), Sockfd, 0); + } + else + LogMessage(LOG_LEVEL_ERROR, "Response data more than %d bytes!", SYSTEM_CONTROL_SEND_BUFFER_SIZE); +} I32 SystemControlBuildControlResponse(U16 ResponseStatus, C8 * ResponseString, C8 * ResponseData, From 0d3b91db23d0e4951682c60b41b07016de48ca84 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 2 Apr 2020 16:43:29 +0200 Subject: [PATCH 419/523] Missed saving the header file changes --- core/inc/iso22133.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/core/inc/iso22133.h b/core/inc/iso22133.h index 26683e2d0..93b409e22 100644 --- a/core/inc/iso22133.h +++ b/core/inc/iso22133.h @@ -208,7 +208,7 @@ ssize_t encodeACCMMessage(const uint16_t* actionID, const ActionType_t* actionTy ssize_t encodeEXACMessage(const uint16_t* actionID, const struct timeval * executionTime, char * exacDataBuffer, const size_t bufferLength, const char debug); ssize_t encodeINSUPMessage(const SupervisorCommandType, char * insupDataBuffer, const size_t bufferLength, const char debug); ISOMessageID getISOMessageType(const char * messageData, const size_t length, const char debug); -void setISOCRCCheck(const int8_t enabled); +void setISOCRCVerification(const int8_t enabled); #ifdef __cplusplus } From 5479c71bdc8afeed34cbe9c79e4a51201895c4e7 Mon Sep 17 00:00:00 2001 From: Sebastian Loh Lindholm Date: Thu, 2 Apr 2020 17:19:29 +0200 Subject: [PATCH 420/523] Format checked. --- core/src/systemcontrol.c | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/core/src/systemcontrol.c b/core/src/systemcontrol.c index 814781112..2c67b8c1e 100644 --- a/core/src/systemcontrol.c +++ b/core/src/systemcontrol.c @@ -181,8 +181,8 @@ void SystemControlSendControlResponse(U16 ResponseStatus, C8 * ResponseString, C I32 SystemControlBuildControlResponse(U16 ResponseStatus, C8 * ResponseString, C8 * ResponseData, I32 ResponseDataLength, U8 Debug); void SystemControlFileDownloadResponse(U16 ResponseStatus, C8 * ResponseString, - I32 ResponseDataLength, I32 * Sockfd, U8 Debug); - void SystemControlSendLog(C8 * LogString, I32 * Sockfd, U8 Debug); + I32 ResponseDataLength, I32 * Sockfd, U8 Debug); +void SystemControlSendLog(C8 * LogString, I32 * Sockfd, U8 Debug); void SystemControlSendMONR(C8 * LogString, I32 * Sockfd, U8 Debug); static void SystemControlCreateProcessChannel(const C8 * name, const U32 port, I32 * sockfd, struct sockaddr_in *addr); @@ -736,10 +736,10 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { UtilCreateDirContent(SystemControlArgument[0], "dir.info"); bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); FileLengthI32 = SystemControlBuildFileContentInfo("dir.info", 0); - SystemControlFileDownloadResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "SubGetDirectoryContent:", - FileLengthI32, &ClientSocket, 0); - SystemControlSendFileContent(&ClientSocket, "dir.info", - STR_SYSTEM_CONTROL_TX_PACKET_SIZE, + SystemControlFileDownloadResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, + "SubGetDirectoryContent:", FileLengthI32, &ClientSocket, + 0); + SystemControlSendFileContent(&ClientSocket, "dir.info", STR_SYSTEM_CONTROL_TX_PACKET_SIZE, SystemControlDirectoryInfo.info_buffer, KEEP_FILE, 0); SystemControlDestroyFileContentInfo("dir.info", 1); } @@ -843,7 +843,7 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { bzero(ControlResponseBuffer, SYSTEM_CONTROL_CONTROL_RESPONSE_SIZE); FileLengthI32 = SystemControlBuildFileContentInfo(SystemControlArgument[0], 0); SystemControlFileDownloadResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "SubDownloadFile:", - FileLengthI32, &ClientSocket, 0); + FileLengthI32, &ClientSocket, 0); SystemControlSendFileContent(&ClientSocket, SystemControlArgument[0], STR_SYSTEM_CONTROL_TX_PACKET_SIZE, SystemControlDirectoryInfo.info_buffer, KEEP_FILE, 0); @@ -1435,7 +1435,7 @@ void SystemControlSendControlResponse(U16 ResponseStatus, C8 * ResponseString, C void SystemControlFileDownloadResponse(U16 ResponseStatus, C8 * ResponseString, - I32 ResponseDataLength, I32 * Sockfd, U8 Debug) { + I32 ResponseDataLength, I32 * Sockfd, U8 Debug) { int i, n, j, t; C8 Length[4]; C8 Status[2]; @@ -1466,7 +1466,7 @@ void SystemControlFileDownloadResponse(U16 ResponseStatus, C8 * ResponseString, } //SystemControlSendBytes(Data, n + 4, Sockfd, 0); - UtilSendTCPData("System Control", Data, 6+strlen(ResponseString), Sockfd, 0); + UtilSendTCPData("System Control", Data, 6 + strlen(ResponseString), Sockfd, 0); } else LogMessage(LOG_LEVEL_ERROR, "Response data more than %d bytes!", SYSTEM_CONTROL_SEND_BUFFER_SIZE); From 749806ab8269c322927c4ee46b119bd0d69ea756 Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Fri, 3 Apr 2020 08:19:59 +0200 Subject: [PATCH 421/523] Update core/inc/util.h Co-Authored-By: LukasWikander --- core/inc/util.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/core/inc/util.h b/core/inc/util.h index ebdd63ebd..baf00c185 100644 --- a/core/inc/util.h +++ b/core/inc/util.h @@ -40,10 +40,10 @@ extern "C"{ ------------------------------------------------------------*/ #define MaestroVersion "0.4.1" -#define DEFAULT_ORIGO_LAT 57.777073115 -#define DEFAULT_ORIGO_LOG 12.781295498333 -#define DEFAULT_ORIGO_ALT 193.114 -#define DEFAULT_ORIGO_HEADING 0 +#define DEFAULT_ORIGIN_LAT 57.777073115 +#define DEFAULT_ORIGIN_LOG 12.781295498333 +#define DEFAULT_ORIGIN_ALT 193.114 +#define DEFAULT_ORIGIN_HEADING 0 #define DEFAULT_VISUALISATION_SERVER_NAME 0 #define DEFAULT_FORCE_OBJECT_TO_LOCALHOST 0 #define DEFAULT_ASP_MAX_TIME_DIFF 2.5 From 708cf2a474dd67fc7ee714e7ac13aa4ba994c545 Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Fri, 3 Apr 2020 08:20:12 +0200 Subject: [PATCH 422/523] Update core/inc/util.h Co-Authored-By: LukasWikander --- core/inc/util.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/core/inc/util.h b/core/inc/util.h index baf00c185..177189cb0 100644 --- a/core/inc/util.h +++ b/core/inc/util.h @@ -51,7 +51,7 @@ extern "C"{ #define DEFAULT_ASP_STEP_BACK_COUNT 0 #define DEFAULT_ASP_FILTER_LEVEL 5 #define DEFAULT_ASP_MAX_DELTA_TIME 0.05 -#define DEFAULT_TIMESERVER_IP 10.130.23.110 +#define DEFAULT_TIMESERVER_IP "10.130.23.110" #define DEFAULT_TIME_SERVER_PORT 53000 #define DEFAULT_SIMULATOR_SIMULATOR_IP 0 #define DEFAULT_SIMULATOR_TCP_PORT 53001 From 48ec56655d080a71e2a0072901f24564dfbd2489 Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Fri, 3 Apr 2020 14:41:47 +0200 Subject: [PATCH 423/523] Update core/inc/util.h Co-Authored-By: LukasWikander --- core/inc/util.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/core/inc/util.h b/core/inc/util.h index 177189cb0..92f486866 100644 --- a/core/inc/util.h +++ b/core/inc/util.h @@ -49,7 +49,7 @@ extern "C"{ #define DEFAULT_ASP_MAX_TIME_DIFF 2.5 #define DEFAULT_ASP_MAX_TRAJ_DIFF 1.52 #define DEFAULT_ASP_STEP_BACK_COUNT 0 -#define DEFAULT_ASP_FILTER_LEVEL 5 +#define DEFAULT_ASP_FILTER_LEVEL 0 #define DEFAULT_ASP_MAX_DELTA_TIME 0.05 #define DEFAULT_TIMESERVER_IP "10.130.23.110" #define DEFAULT_TIME_SERVER_PORT 53000 From 2f5d41f474763dba46f0c09022a782479339b242 Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Fri, 3 Apr 2020 14:41:57 +0200 Subject: [PATCH 424/523] Update core/inc/util.h Co-Authored-By: LukasWikander --- core/inc/util.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/core/inc/util.h b/core/inc/util.h index 92f486866..d6fee10f5 100644 --- a/core/inc/util.h +++ b/core/inc/util.h @@ -50,7 +50,7 @@ extern "C"{ #define DEFAULT_ASP_MAX_TRAJ_DIFF 1.52 #define DEFAULT_ASP_STEP_BACK_COUNT 0 #define DEFAULT_ASP_FILTER_LEVEL 0 -#define DEFAULT_ASP_MAX_DELTA_TIME 0.05 +#define DEFAULT_ASP_MAX_DELTA_TIME INFINITY #define DEFAULT_TIMESERVER_IP "10.130.23.110" #define DEFAULT_TIME_SERVER_PORT 53000 #define DEFAULT_SIMULATOR_SIMULATOR_IP 0 From 5e93f08b47460aa438623dab0c1d6353b79f3679 Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Fri, 3 Apr 2020 14:51:43 +0200 Subject: [PATCH 425/523] Ran formatting script --- core/src/citscontrol.c | 222 ++++++++++++++++++--------------------- core/src/objectcontrol.c | 24 ++--- core/src/systemcontrol.c | 6 +- core/src/timecontrol.c | 2 +- 4 files changed, 119 insertions(+), 135 deletions(-) diff --git a/core/src/citscontrol.c b/core/src/citscontrol.c index 05f45faeb..f206e4f4c 100644 --- a/core/src/citscontrol.c +++ b/core/src/citscontrol.c @@ -645,18 +645,16 @@ CAM_t *allocateCAMStruct(void) { cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.accelerationControl = NULL; // TODO: Allocate memory for this cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.lanePosition = calloc(1, sizeof (LanePosition_t)); - cam->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.steeringWheelAngle = calloc(1, sizeof (SteeringWheelAngle_t)); - cam->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.lateralAcceleration = - calloc(1, sizeof (LateralAcceleration_t)); - cam->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.verticalAcceleration = - calloc(1, sizeof (VerticalAcceleration_t)); - cam->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.performanceClass = calloc(1, sizeof (PerformanceClass_t)); - cam->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.cenDsrcTollingZone = calloc(1, sizeof (CenDsrcTollingZone_t)); + cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + steeringWheelAngle = calloc(1, sizeof (SteeringWheelAngle_t)); + cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + lateralAcceleration = calloc(1, sizeof (LateralAcceleration_t)); + cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + verticalAcceleration = calloc(1, sizeof (VerticalAcceleration_t)); + cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + performanceClass = calloc(1, sizeof (PerformanceClass_t)); + cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + cenDsrcTollingZone = calloc(1, sizeof (CenDsrcTollingZone_t)); cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.cenDsrcTollingZone->cenDsrcTollingZoneID = NULL; // TODO: Allocate memory for this BasicVehicleContainerHighFrequency_t *bvc = @@ -700,71 +698,67 @@ void initializeCAMStruct(CAM_t * cam) { cam->cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_basicVehicleContainerHighFrequency; - cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - heading.headingValue = HeadingValue_unavailable; - cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - heading.headingConfidence = HeadingConfidence_unavailable; + cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading. + headingValue = HeadingValue_unavailable; + cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading. + headingConfidence = HeadingConfidence_unavailable; cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.speed.speedValue = SpeedValue_unavailable; - cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - speed.speedConfidence = SpeedConfidence_unavailable; + cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.speed. + speedConfidence = SpeedConfidence_unavailable; cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.driveDirection = DriveDirection_unavailable; - cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - vehicleLength.vehicleLengthValue = VehicleLengthValue_unavailable; - cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - vehicleLength.vehicleLengthConfidenceIndication = VehicleLengthConfidenceIndication_unavailable; + cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.vehicleLength. + vehicleLengthValue = VehicleLengthValue_unavailable; + cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.vehicleLength. + vehicleLengthConfidenceIndication = VehicleLengthConfidenceIndication_unavailable; cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.vehicleWidth = VehicleWidth_unavailable; - cam->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.longitudinalAcceleration.longitudinalAccelerationValue = - LongitudinalAccelerationValue_unavailable; - cam->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.longitudinalAcceleration.longitudinalAccelerationConfidence = - AccelerationConfidence_unavailable; cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - curvature.curvatureValue = CurvatureValue_unavailable; + longitudinalAcceleration.longitudinalAccelerationValue = LongitudinalAccelerationValue_unavailable; cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - curvature.curvatureConfidence = CurvatureConfidence_unavailable; - cam->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.curvatureCalculationMode = CurvatureCalculationMode_unavailable; + longitudinalAcceleration.longitudinalAccelerationConfidence = AccelerationConfidence_unavailable; + cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.curvature. + curvatureValue = CurvatureValue_unavailable; + cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.curvature. + curvatureConfidence = CurvatureConfidence_unavailable; cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - yawRate.yawRateValue = YawRateValue_unavailable; + curvatureCalculationMode = CurvatureCalculationMode_unavailable; + cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.yawRate. + yawRateValue = YawRateValue_unavailable; + cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.yawRate. + yawRateConfidence = YawRateConfidence_unavailable; cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - yawRate.yawRateConfidence = YawRateConfidence_unavailable; - cam->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.lateralAcceleration->lateralAccelerationValue = - LateralAccelerationValue_unavailable; - cam->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.lateralAcceleration->lateralAccelerationConfidence = - AccelerationConfidence_unavailable; + lateralAcceleration->lateralAccelerationValue = LateralAccelerationValue_unavailable; + cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + lateralAcceleration->lateralAccelerationConfidence = AccelerationConfidence_unavailable; // Unused highFrequencyContainer optional fields (null their pointers to show unused) // TODO: Modify here to once relevant information can be used - free(cam->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.accelerationControl); - free(cam->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.lanePosition); - free(cam->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.steeringWheelAngle); - free(cam->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.verticalAcceleration); - free(cam->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.performanceClass); - free(cam->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.cenDsrcTollingZone); - cam->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.accelerationControl = NULL; + free(cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + accelerationControl); + free(cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + lanePosition); + free(cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + steeringWheelAngle); + free(cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + verticalAcceleration); + free(cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + performanceClass); + free(cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + cenDsrcTollingZone); + cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + accelerationControl = NULL; cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.lanePosition = NULL; - cam->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.steeringWheelAngle = NULL; - cam->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.verticalAcceleration = NULL; + cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + steeringWheelAngle = NULL; + cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + verticalAcceleration = NULL; cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.performanceClass = NULL; - cam->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.cenDsrcTollingZone = NULL; + cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + cenDsrcTollingZone = NULL; // Low frequency container and special vehicle containers unused for now (TODO) free(cam->cam.camParameters.lowFrequencyContainer); @@ -927,12 +921,12 @@ I32 generateCAMMessage(MONRType * MONRData, CAM_t * cam) { tempCAM->cam.camParameters.basicContainer.referencePosition.longitude = (long)(longitude * 10000000.0); - tempCAM->cam.camParameters.basicContainer.referencePosition. - positionConfidenceEllipse.semiMajorConfidence = SemiAxisLength_unavailable; - tempCAM->cam.camParameters.basicContainer.referencePosition. - positionConfidenceEllipse.semiMinorConfidence = SemiAxisLength_unavailable; - tempCAM->cam.camParameters.basicContainer.referencePosition. - positionConfidenceEllipse.semiMajorOrientation = 0; + tempCAM->cam.camParameters.basicContainer.referencePosition.positionConfidenceEllipse. + semiMajorConfidence = SemiAxisLength_unavailable; + tempCAM->cam.camParameters.basicContainer.referencePosition.positionConfidenceEllipse. + semiMinorConfidence = SemiAxisLength_unavailable; + tempCAM->cam.camParameters.basicContainer.referencePosition.positionConfidenceEllipse. + semiMajorOrientation = 0; tempCAM->cam.camParameters.basicContainer.referencePosition.altitude.altitudeValue = AltitudeValue_unavailable; @@ -945,80 +939,70 @@ I32 generateCAMMessage(MONRType * MONRData, CAM_t * cam) { tempCAM->cam.camParameters.basicContainer.stationType = StationType_roadSideUnit; if (tempCAM->cam.camParameters.highFrequencyContainer.present == HighFrequencyContainer_PR_basicVehicleContainerHighFrequency) { - tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - speed.speedValue = + tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.speed. + speedValue = (long)(sqrt (pow((double)(MONRData->LongitudinalSpeedI16), 2) + pow((double)(MONRData->LateralSpeedI16), 2))); - tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - speed.speedConfidence = SpeedConfidence_unavailable; + tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.speed. + speedConfidence = SpeedConfidence_unavailable; - tempCAM->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.driveDirection = MONRData->DriveDirectionU8; tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - heading.headingValue = MONRData->HeadingU16 / 10; - tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - heading.headingConfidence = HeadingConfidence_unavailable; + driveDirection = MONRData->DriveDirectionU8; + tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading. + headingValue = MONRData->HeadingU16 / 10; + tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading. + headingConfidence = HeadingConfidence_unavailable; if (MONRData->LongitudinalAccI16 == 32001) - tempCAM->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.longitudinalAcceleration.longitudinalAccelerationValue = + tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + longitudinalAcceleration.longitudinalAccelerationValue = LongitudinalAccelerationValue_unavailable; else if (MONRData->LongitudinalAccI16 > 16000) - tempCAM->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.longitudinalAcceleration.longitudinalAccelerationValue = - 160; + tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + longitudinalAcceleration.longitudinalAccelerationValue = 160; else if (MONRData->LongitudinalAccI16 < -16000) - tempCAM->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.longitudinalAcceleration.longitudinalAccelerationValue = - -160; + tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + longitudinalAcceleration.longitudinalAccelerationValue = -160; else - tempCAM->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.longitudinalAcceleration.longitudinalAccelerationValue = - MONRData->LongitudinalAccI16 / 100; - tempCAM->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.longitudinalAcceleration.longitudinalAccelerationConfidence = - AccelerationConfidence_unavailable; + tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + longitudinalAcceleration.longitudinalAccelerationValue = MONRData->LongitudinalAccI16 / 100; + tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + longitudinalAcceleration.longitudinalAccelerationConfidence = AccelerationConfidence_unavailable; if (MONRData->LateralAccI16 == 32001) - tempCAM->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.lateralAcceleration->lateralAccelerationValue = - LateralAccelerationValue_unavailable; + tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + lateralAcceleration->lateralAccelerationValue = LateralAccelerationValue_unavailable; else if (MONRData->LateralAccI16 > 16000) - tempCAM->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.lateralAcceleration->lateralAccelerationValue = 160; + tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + lateralAcceleration->lateralAccelerationValue = 160; else if (MONRData->LateralAccI16 < -16000) - tempCAM->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.lateralAcceleration->lateralAccelerationValue = -160; + tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + lateralAcceleration->lateralAccelerationValue = -160; else - tempCAM->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.lateralAcceleration->lateralAccelerationValue = - MONRData->LateralAccI16 / 100; - tempCAM->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.lateralAcceleration->lateralAccelerationConfidence = - AccelerationConfidence_unavailable; - + tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + lateralAcceleration->lateralAccelerationValue = MONRData->LateralAccI16 / 100; tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - yawRate.yawRateValue = YawRateValue_unavailable; + lateralAcceleration->lateralAccelerationConfidence = AccelerationConfidence_unavailable; + + tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.yawRate. + yawRateValue = YawRateValue_unavailable; + tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.yawRate. + yawRateConfidence = YawRateConfidence_unavailable; + + tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.curvature. + curvatureValue = CurvatureValue_unavailable; + tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.curvature. + curvatureConfidence = CurvatureConfidence_unavailable; tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - yawRate.yawRateConfidence = YawRateConfidence_unavailable; + curvatureCalculationMode = CurvatureCalculationMode_unavailable; tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - curvature.curvatureValue = CurvatureValue_unavailable; + vehicleWidth = VehicleWidth_unavailable; + tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. + vehicleLength.vehicleLengthValue = VehicleLengthValue_unavailable; tempCAM->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency. - curvature.curvatureConfidence = CurvatureConfidence_unavailable; - tempCAM->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.curvatureCalculationMode = - CurvatureCalculationMode_unavailable; - - tempCAM->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.vehicleWidth = VehicleWidth_unavailable; - tempCAM->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.vehicleLength.vehicleLengthValue = - VehicleLengthValue_unavailable; - tempCAM->cam.camParameters.highFrequencyContainer.choice. - basicVehicleContainerHighFrequency.vehicleLength.vehicleLengthConfidenceIndication = - VehicleLengthConfidenceIndication_unavailable; + vehicleLength.vehicleLengthConfidenceIndication = VehicleLengthConfidenceIndication_unavailable; } diff --git a/core/src/objectcontrol.c b/core/src/objectcontrol.c index 4478a1589..82623b4b3 100644 --- a/core/src/objectcontrol.c +++ b/core/src/objectcontrol.c @@ -166,7 +166,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { }; const struct timeval adaptiveSyncMessagePeriod = heartbeatPeriod; - U8 iForceObjectToLocalhostU8 = DEFAULT_FORCE_OBJECT_TO_LOCALHOST; + U8 iForceObjectToLocalhostU8 = DEFAULT_FORCE_OBJECT_TO_LOCALHOST; FILE *fd; C8 Timestamp[SMALL_BUFFER_SIZE_0]; @@ -185,13 +185,13 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { flt SpaceArr[MAX_OBJECTS][TRAJECTORY_FILE_MAX_ROWS]; flt TimeArr[MAX_OBJECTS][TRAJECTORY_FILE_MAX_ROWS]; SpaceTime SpaceTimeArr[MAX_OBJECTS][TRAJECTORY_FILE_MAX_ROWS]; - C8 OriginLatitude[SMALL_BUFFER_SIZE_0], OriginLongitude[SMALL_BUFFER_SIZE_0], + C8 OriginLatitude[SMALL_BUFFER_SIZE_0], OriginLongitude[SMALL_BUFFER_SIZE_0], OriginAltitude[SMALL_BUFFER_SIZE_0], OriginHeading[SMALL_BUFFER_SIZE_0]; C8 TextBuffer[SMALL_BUFFER_SIZE_0]; - dbl OriginLatitudeDbl = DEFAULT_ORIGO_LAT; - dbl OriginLongitudeDbl = DEFAULT_ORIGO_LOG; - dbl OriginAltitudeDbl = DEFAULT_ORIGO_ALT; - dbl OriginHeadingDbl = DEFAULT_ORIGO_HEADING; + dbl OriginLatitudeDbl = DEFAULT_ORIGO_LAT; + dbl OriginLongitudeDbl = DEFAULT_ORIGO_LOG; + dbl OriginAltitudeDbl = DEFAULT_ORIGO_ALT; + dbl OriginHeadingDbl = DEFAULT_ORIGO_HEADING; C8 pcSendBuffer[MBUS_MAX_DATALEN]; C8 ObjectPort[SMALL_BUFFER_SIZE_0]; MonitorDataType monitorData; @@ -208,12 +208,12 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { AdaptiveSyncPoint ASP[MAX_ADAPTIVE_SYNC_POINTS]; I32 SyncPointCount = 0; I32 SearchStartIndex = 0; - dbl ASPMaxTimeDiffDbl = DEFAULT_ASP_MAX_TIME_DIFF; - dbl ASPMaxTrajDiffDbl = DEFAULT_ASP_MAX_TRAJ_DIFF; - dbl ASPFilterLevelDbl = DEFAULT_ASP_FILTER_LEVEL; - dbl ASPMaxDeltaTimeDbl = DEFAULT_ASP_MAX_DELTA_TIME; + dbl ASPMaxTimeDiffDbl = DEFAULT_ASP_MAX_TIME_DIFF; + dbl ASPMaxTrajDiffDbl = DEFAULT_ASP_MAX_TRAJ_DIFF; + dbl ASPFilterLevelDbl = DEFAULT_ASP_FILTER_LEVEL; + dbl ASPMaxDeltaTimeDbl = DEFAULT_ASP_MAX_DELTA_TIME; I32 ASPDebugRate = 1; - I32 ASPStepBackCount = DEFAULT_ASP_STEP_BACK_COUNT; + I32 ASPStepBackCount = DEFAULT_ASP_STEP_BACK_COUNT; char confDirectoryPath[MAX_FILE_PATH]; ControlCenterStatusType objectControlServerStatus = CONTROL_CENTER_STATUS_INIT; @@ -1315,7 +1315,7 @@ int iFindObjectsInfo(C8 object_traj_file[MAX_OBJECTS][MAX_FILE_PATH], I32 * nbr_objects) { DIR *traj_directory; struct dirent *directory_entry; - int iForceObjectToLocalhost = DEFAULT_FORCE_OBJECT_TO_LOCALHOST; + int iForceObjectToLocalhost = DEFAULT_FORCE_OBJECT_TO_LOCALHOST; struct sockaddr_in sockaddr; int result; char trajPathDir[MAX_FILE_PATH]; diff --git a/core/src/systemcontrol.c b/core/src/systemcontrol.c index 6a0a7c0c2..2dd45ec77 100644 --- a/core/src/systemcontrol.c +++ b/core/src/systemcontrol.c @@ -303,7 +303,7 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { C8 RVSSData[SYSTEM_CONTROL_RVSS_DATA_BUFFER]; U16 RVSSSendCounterU16 = 0; - U32 RVSSConfigU32 = DEFAULT_RVSS_CONF; + U32 RVSSConfigU32 = DEFAULT_RVSS_CONF; U32 RVSSMessageLengthU32; U16 PCDMessageCodeU16; C8 RxFilePath[MAX_FILE_PATH]; @@ -322,8 +322,8 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { DataDictionaryGetRVSSConfigU32(GSD, &RVSSConfigU32); LogMessage(LOG_LEVEL_INFO, "RVSSConfigU32 = %d", RVSSConfigU32); - U8 RVSSRateU8 = DEFAULT_RVSS_RATE; - dbl RVSSRateDbl = DEFAULT_RVSS_RATE; + U8 RVSSRateU8 = DEFAULT_RVSS_RATE; + dbl RVSSRateDbl = DEFAULT_RVSS_RATE; DataDictionaryGetRVSSRateU8(GSD, &RVSSRateU8); RVSSRateDbl = RVSSRateU8; diff --git a/core/src/timecontrol.c b/core/src/timecontrol.c index f69e76c01..72e81bea1 100644 --- a/core/src/timecontrol.c +++ b/core/src/timecontrol.c @@ -86,7 +86,7 @@ void timecontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { C8 TextBufferC8[TIME_CONTROL_HOSTNAME_BUFFER_SIZE]; C8 ServerIPC8[TIME_CONTROL_HOSTNAME_BUFFER_SIZE]; - U16 ServerPortU16 = DEFAULT_TIME_SERVER_PORT; + U16 ServerPortU16 = DEFAULT_TIME_SERVER_PORT; I32 SocketfdI32 = -1; struct sockaddr_in time_addr; const struct timespec mqEmptyPollPeriod = { TC_SLEEP_TIME_EMPTY_MQ_S, TC_SLEEP_TIME_EMPTY_MQ_NS }; From 573560aab5b7cf5053163e2c3f725e5faa923129 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 6 Apr 2020 10:07:40 +0200 Subject: [PATCH 426/523] Updated util ref --- util | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/util b/util index 19a42f2de..74fa247ed 160000 --- a/util +++ b/util @@ -1 +1 @@ -Subproject commit 19a42f2dec489753d31322e0527de876c1b18660 +Subproject commit 74fa247ed95525a4f14e11462d4f63f49cbc3e5c From 7e73e48eafbfdca0a2bc2db669b0254167ce9961 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 6 Apr 2020 11:02:33 +0200 Subject: [PATCH 427/523] Linked MQTT paho library into ObjectMonitoring --- modules/ObjectMonitoring/CMakeLists.txt | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/modules/ObjectMonitoring/CMakeLists.txt b/modules/ObjectMonitoring/CMakeLists.txt index 3183793a6..128533075 100644 --- a/modules/ObjectMonitoring/CMakeLists.txt +++ b/modules/ObjectMonitoring/CMakeLists.txt @@ -19,6 +19,12 @@ set(MESSAGE_BUS_LIBRARY MaestroMQ) include(GNUInstallDirs) find_package(Threads REQUIRED) +find_library(paho-mqtt3a NAMES libpaho-mqtt3a.so REQUIRED) +find_library(paho-mqtt3a NAMES libpaho-mqtt3c.so REQUIRED) +add_library(pahomqtt3a SHARED IMPORTED) +add_library(pahomqtt3c SHARED IMPORTED) +set_property(TARGET pahomqtt3a PROPERTY IMPORTED_LOCATION ${paho-mqtt3a}) +set_property(TARGET pahomqtt3c PROPERTY IMPORTED_LOCATION ${paho-mqtt3c}) # Create project main executable target add_executable(${OBJECT_MONITORING_TARGET} @@ -34,6 +40,8 @@ target_link_libraries(${OBJECT_MONITORING_TARGET} LINK_PUBLIC ${LOGGING_LIBRARY} ${COREUTILS_LIBRARY} ${CMAKE_THREAD_LIBS_INIT} + pahomqtt3a + pahomqtt3c ) target_include_directories(${OBJECT_MONITORING_TARGET} PUBLIC From bb194d766cd94dc2a377c28412ef9f73592439b8 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 6 Apr 2020 11:05:30 +0200 Subject: [PATCH 428/523] Refactoring --- .../ObjectMonitoring/inc/connectionhandler.h | 10 ++++----- .../src/connectionhandler.cpp | 14 ++++++------- modules/ObjectMonitoring/src/main.cpp | 21 ++++++++++++------- 3 files changed, 26 insertions(+), 19 deletions(-) diff --git a/modules/ObjectMonitoring/inc/connectionhandler.h b/modules/ObjectMonitoring/inc/connectionhandler.h index 150428c52..856dca1e0 100644 --- a/modules/ObjectMonitoring/inc/connectionhandler.h +++ b/modules/ObjectMonitoring/inc/connectionhandler.h @@ -4,12 +4,12 @@ #include #include "protocoldata.h" -class ConnectionHandler +class RawConnectionHandler { public: - ConnectionHandler(int openSocketDescriptor, ProtocolData& data); - ConnectionHandler(int openSocketDescriptor, ProtocolData& data, unsigned long readBufferSize); - ~ConnectionHandler(); + RawConnectionHandler(int openSocketDescriptor, ProtocolData& data); + RawConnectionHandler(int openSocketDescriptor, ProtocolData& data, unsigned long readBufferSize); + ~RawConnectionHandler(); bool isTerminated() const { return terminated; } private: @@ -22,7 +22,7 @@ class ConnectionHandler void* threadRoutine(void*); static void* routineWrapper(void* context) { - return static_cast(context)->threadRoutine(nullptr); + return static_cast(context)->threadRoutine(nullptr); } [[ noreturn ]] void terminate(void* retval); }; diff --git a/modules/ObjectMonitoring/src/connectionhandler.cpp b/modules/ObjectMonitoring/src/connectionhandler.cpp index 7a6dc0e0e..04f2e9f43 100644 --- a/modules/ObjectMonitoring/src/connectionhandler.cpp +++ b/modules/ObjectMonitoring/src/connectionhandler.cpp @@ -6,10 +6,10 @@ #include #include -ConnectionHandler::ConnectionHandler(int openSocketDescriptor, ProtocolData& data) - : ConnectionHandler::ConnectionHandler(openSocketDescriptor, data, DefaultReadBufferSize) {} +RawConnectionHandler::RawConnectionHandler(int openSocketDescriptor, ProtocolData& data) + : RawConnectionHandler::RawConnectionHandler(openSocketDescriptor, data, DefaultReadBufferSize) {} -ConnectionHandler::ConnectionHandler(int openSocketDescriptor, ProtocolData& data, unsigned long readBufferSize) : data(data) { +RawConnectionHandler::RawConnectionHandler(int openSocketDescriptor, ProtocolData& data, unsigned long readBufferSize) : data(data) { int returnCode; if (openSocketDescriptor < 0) { @@ -25,7 +25,7 @@ ConnectionHandler::ConnectionHandler(int openSocketDescriptor, ProtocolData& dat this->readBufferSize = readBufferSize; LogMessage(LOG_LEVEL_INFO, "Creating thread to handle connection"); - returnCode = pthread_create(&readThread, nullptr, &ConnectionHandler::routineWrapper, this); + returnCode = pthread_create(&readThread, nullptr, &RawConnectionHandler::routineWrapper, this); if (returnCode) { LogMessage(LOG_LEVEL_ERROR, "Error creating thread"); close(socketDescriptor); @@ -34,12 +34,12 @@ ConnectionHandler::ConnectionHandler(int openSocketDescriptor, ProtocolData& dat } } -ConnectionHandler::~ConnectionHandler() { +RawConnectionHandler::~RawConnectionHandler() { close(socketDescriptor); } -void* ConnectionHandler::threadRoutine(void*) { +void* RawConnectionHandler::threadRoutine(void*) { std::vector readBuffer(readBufferSize); std::vector messageBuffer; ssize_t readBytes; @@ -68,7 +68,7 @@ void* ConnectionHandler::threadRoutine(void*) { } } -void ConnectionHandler::terminate(void *retval) { +void RawConnectionHandler::terminate(void *retval) { terminated = true; pthread_exit(retval); } diff --git a/modules/ObjectMonitoring/src/main.cpp b/modules/ObjectMonitoring/src/main.cpp index c42d7d86e..c67bfc0a0 100644 --- a/modules/ObjectMonitoring/src/main.cpp +++ b/modules/ObjectMonitoring/src/main.cpp @@ -4,6 +4,8 @@ #include #include +#include + #include "logging.h" #include "connectionhandler.h" #include "protocoldata.h" @@ -15,8 +17,8 @@ using namespace std; -static void handleNewConnection(int socketDescriptor, vector &handlers); -static void pruneTerminatedConnectionHandlers(vector &handlers); +static void handleNewConnection(int socketDescriptor, vector &handlers); +static void pruneTerminatedConnectionHandlers(vector &handlers); static void listenForNewConnection(void); int main() @@ -26,7 +28,7 @@ int main() const struct timespec sleepTimePeriod = {0,10000000}; const struct timespec abortWaitTime = {1,0}; struct timespec remTime; - vector handlers; + vector handlers; LogInit(MODULE_NAME, LOG_LEVEL_DEBUG); LogMessage(LOG_LEVEL_INFO, "Task running with PID: %u", getpid()); @@ -36,7 +38,12 @@ int main() nanosleep(&sleepTimePeriod,&remTime); } + // TODO: start MQ communication handler + + // Initialize MQTT handler + while(true) { + listenForNewConnection(); handleNewConnection(0, handlers); @@ -47,7 +54,7 @@ int main() return 0; } -void handleNewConnection(int socketDescriptor, vector &handlers) { +void handleNewConnection(int socketDescriptor, vector &handlers) { struct sockaddr_in socketAddress; socklen_t addressLength = sizeof (socketAddress); getsockname(socketDescriptor, (struct sockaddr *)&socketAddress, &addressLength); @@ -56,7 +63,7 @@ void handleNewConnection(int socketDescriptor, vector &handl case ISO22133ProtocolData::TCP_PORT: { ISO22133ProtocolData protoData; - handlers.push_back(new ConnectionHandler(socketDescriptor, protoData)); + handlers.push_back(new RawConnectionHandler(socketDescriptor, protoData)); break; } default: @@ -66,12 +73,12 @@ void handleNewConnection(int socketDescriptor, vector &handl } } -void pruneTerminatedConnectionHandlers(vector &handlers) { +void pruneTerminatedConnectionHandlers(vector &handlers) { // Remove any connection handlers which are null pointers handlers.erase( std::remove(handlers.begin(), handlers.end(), nullptr), handlers.end() ); // Remove any connection handlers which have finished their tasks handlers.erase( std::remove_if(handlers.begin(), handlers.end(), - [](const ConnectionHandler* handler) { return handler->isTerminated(); } + [](const RawConnectionHandler* handler) { return handler->isTerminated(); } ), handlers.end() ); } From f3d586666eb1817292e0a67b54065a2ea712d1b2 Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Mon, 6 Apr 2020 15:12:27 +0200 Subject: [PATCH 429/523] Changed back some values after discussion with Sepast --- core/inc/util.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/core/inc/util.h b/core/inc/util.h index d6fee10f5..177189cb0 100644 --- a/core/inc/util.h +++ b/core/inc/util.h @@ -49,8 +49,8 @@ extern "C"{ #define DEFAULT_ASP_MAX_TIME_DIFF 2.5 #define DEFAULT_ASP_MAX_TRAJ_DIFF 1.52 #define DEFAULT_ASP_STEP_BACK_COUNT 0 -#define DEFAULT_ASP_FILTER_LEVEL 0 -#define DEFAULT_ASP_MAX_DELTA_TIME INFINITY +#define DEFAULT_ASP_FILTER_LEVEL 5 +#define DEFAULT_ASP_MAX_DELTA_TIME 0.05 #define DEFAULT_TIMESERVER_IP "10.130.23.110" #define DEFAULT_TIME_SERVER_PORT 53000 #define DEFAULT_SIMULATOR_SIMULATOR_IP 0 From 6e50acb986f0e84b45eaf24609906acfd3157824 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 6 Apr 2020 15:31:27 +0200 Subject: [PATCH 430/523] Implemented handler for MQTT connections --- .../inc/mqttconnectionhandler.hpp | 67 +++++++++++++++++++ .../src/mqttconnectionhandler.cpp | 23 +++++++ 2 files changed, 90 insertions(+) create mode 100644 modules/ObjectMonitoring/inc/mqttconnectionhandler.hpp create mode 100644 modules/ObjectMonitoring/src/mqttconnectionhandler.cpp diff --git a/modules/ObjectMonitoring/inc/mqttconnectionhandler.hpp b/modules/ObjectMonitoring/inc/mqttconnectionhandler.hpp new file mode 100644 index 000000000..b69fd155a --- /dev/null +++ b/modules/ObjectMonitoring/inc/mqttconnectionhandler.hpp @@ -0,0 +1,67 @@ +#ifndef MQTTCONNECTIONHANDLER_H +#define MQTTCONNECTIONHANDLER_H + +#include +#include + +#include +#include "logging.h" +#include "iso22133.h" + +using namespace std; + +class MQTTError : virtual public std::exception { +protected: + int errorCode; + string errorMessage; +public: + explicit MQTTError(string msg, int errorCode) : errorCode(errorCode), msg(errorMessage) {} + + virtual const char* what() const throw () { + return errorMessage.c_str(); + } + + virtual int getErrorNumber() const throw () { + return errorCode; + } +} ; + + +class MQTTConnectionHandler { +public: + MQTTConnectionHandler(const string clientID); + void establishConnection(void); + +private: + + MQTTClient client; + string clientID = ""; + string serverURI = ""; + MQTTClient_connectOptions connectionOptions; + + template class MQTTConnection { + public: + MQTTConnection(MQTTClient &client, string &topicExpression, int messageCallback(MQTTDataType* message, + string &topicExpression)) : topicExpression(topicExpression), messageCallback(messageCallback) { + MQTTClient_setCallbacks(client, this, nullptr, &MQTTConnection::arrivalCallback, nullptr); + } + private: + string topicExpression = ""; + int (*messageCallback)(void* message, string &topicName); + + static int arrivalCallback(void* context, char* topicName, int, + MQTTClient_message* message) { + MQTTConnection* thisObject = static_cast(context); + string topic(topicName); + if (message->payloadlen == sizeof (MQTTDataType)) { + return thisObject->messageCallback(static_cast(message->payload), topic); + } + LogMessage(LOG_LEVEL_ERROR, "Received MQTT payload of size %d while expecting size %u on topic %s", + message->payloadlen, sizeof (MQTTDataType), topic.c_str()); + return -1; + } + }; + +}; + +#endif // MQTTCONNECTIONHANDLER_H diff --git a/modules/ObjectMonitoring/src/mqttconnectionhandler.cpp b/modules/ObjectMonitoring/src/mqttconnectionhandler.cpp new file mode 100644 index 000000000..0e739f531 --- /dev/null +++ b/modules/ObjectMonitoring/src/mqttconnectionhandler.cpp @@ -0,0 +1,23 @@ +#include "mqttconnection.h" + + +#define DEFAULT_SERVER_URI "tcp://localhost:1883" + +MQTTConnectionHandler::MQTTConnectionHandler(const string clientID) + : clientID(clientID), connectionOptions(MQTTClient_connectOptions_initializer5) { + // TODO: read a config file to get non-default values + this->serverURI = DEFAULT_SERVER_URI; + MQTTClient_create(&this->client, this->serverURI.c_str(), this->clientID.c_str(), MQTTCLIENT_PERSISTENCE_NONE, nullptr); +} + +void MQTTConnectionHandler::establishConnection() { + int returnCode; + LogMessage(LOG_LEVEL_INFO, "Connecting to MQTT broker at %s", this->serverURI.c_str()); + if ((returnCode = MQTTClient_connect(this->client, &this->connectionOptions)) + != MQTTCLIENT_SUCCESS) { + string errMsg = "Failed to connect to MQTT broker: error code " + to_string(returnCode); + LogMessage(LOG_LEVEL_WARNING, errMsg.c_str()); + throw new MQTTError(errMsg, returnCode); + } + LogMessage(LOG_LEVEL_INFO, "Successfully connected to MQTT broker"); +} From 3c94be82b7a0c6049bb83e0546216865014867bf Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 6 Apr 2020 15:32:05 +0200 Subject: [PATCH 431/523] Updated CMakeLists --- modules/ObjectMonitoring/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/modules/ObjectMonitoring/CMakeLists.txt b/modules/ObjectMonitoring/CMakeLists.txt index 128533075..37f701aab 100644 --- a/modules/ObjectMonitoring/CMakeLists.txt +++ b/modules/ObjectMonitoring/CMakeLists.txt @@ -32,6 +32,7 @@ add_executable(${OBJECT_MONITORING_TARGET} ${CMAKE_CURRENT_SOURCE_DIR}/src/connectionhandler.cpp ${CMAKE_CURRENT_SOURCE_DIR}/src/protocoldata.cpp ${CMAKE_CURRENT_SOURCE_DIR}/src/ISO22133ProtocolData.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/mqttconnectionhandler.cpp ) # Link project executable to util libraries From 437d90ee4efb44492dd6544a249b8134910555ff Mon Sep 17 00:00:00 2001 From: sepast Date: Tue, 7 Apr 2020 09:57:14 +0200 Subject: [PATCH 432/523] Update core/src/systemcontrol.c Co-Authored-By: viktorjo <31403385+viktorjo@users.noreply.github.com> --- core/src/systemcontrol.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/core/src/systemcontrol.c b/core/src/systemcontrol.c index 2c67b8c1e..3b45f0ceb 100644 --- a/core/src/systemcontrol.c +++ b/core/src/systemcontrol.c @@ -1437,8 +1437,8 @@ void SystemControlSendControlResponse(U16 ResponseStatus, C8 * ResponseString, C void SystemControlFileDownloadResponse(U16 ResponseStatus, C8 * ResponseString, I32 ResponseDataLength, I32 * Sockfd, U8 Debug) { int i, n, j, t; - C8 Length[4]; - C8 Status[2]; + C8 Length[MSCP_RESPONSE_DATALENGTH_BYTES]; + C8 Status[MSCP_RSPONSE_STATUS_CODE_BYTES]; C8 Data[SYSTEM_CONTROL_SEND_BUFFER_SIZE]; bzero(Data, SYSTEM_CONTROL_SEND_BUFFER_SIZE); From 0ac594c73bc7dd9718ea2000dcbda80ffbbd2650 Mon Sep 17 00:00:00 2001 From: sepast Date: Tue, 7 Apr 2020 09:57:26 +0200 Subject: [PATCH 433/523] Update core/src/systemcontrol.c Co-Authored-By: viktorjo <31403385+viktorjo@users.noreply.github.com> --- core/src/systemcontrol.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/core/src/systemcontrol.c b/core/src/systemcontrol.c index 3b45f0ceb..c4e0e037b 100644 --- a/core/src/systemcontrol.c +++ b/core/src/systemcontrol.c @@ -1442,7 +1442,7 @@ void SystemControlFileDownloadResponse(U16 ResponseStatus, C8 * ResponseString, C8 Data[SYSTEM_CONTROL_SEND_BUFFER_SIZE]; bzero(Data, SYSTEM_CONTROL_SEND_BUFFER_SIZE); - n = 2 + strlen(ResponseString) + ResponseDataLength; + n = MSCP_RESPONSE_STATUS_CODE_BYTES + strlen(ResponseString) + ResponseDataLength; Length[0] = (C8) (n >> 24); Length[1] = (C8) (n >> 16); Length[2] = (C8) (n >> 8); From b89b80769d1890ff062a2104c1bbc8495efe5bd9 Mon Sep 17 00:00:00 2001 From: sepast Date: Tue, 7 Apr 2020 09:57:49 +0200 Subject: [PATCH 434/523] Update core/src/systemcontrol.c Co-Authored-By: viktorjo <31403385+viktorjo@users.noreply.github.com> --- core/src/systemcontrol.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/core/src/systemcontrol.c b/core/src/systemcontrol.c index c4e0e037b..80054398c 100644 --- a/core/src/systemcontrol.c +++ b/core/src/systemcontrol.c @@ -1450,7 +1450,7 @@ void SystemControlFileDownloadResponse(U16 ResponseStatus, C8 * ResponseString, Status[0] = (C8) (ResponseStatus >> 8); Status[1] = (C8) ResponseStatus; - if (n + 4 < SYSTEM_CONTROL_SEND_BUFFER_SIZE) { + if (n + MSCP_RESPONSE_DATALENGTH_BYTES < SYSTEM_CONTROL_SEND_BUFFER_SIZE) { for (i = 0, j = 0; i < 4; i++, j++) Data[j] = Length[i]; for (i = 0; i < 2; i++, j++) From 5501b465d712b9e575f53cc04bfcf173c41a5aa4 Mon Sep 17 00:00:00 2001 From: Sebastian Loh Lindholm Date: Tue, 7 Apr 2020 10:47:45 +0200 Subject: [PATCH 435/523] Added some defines, deleted commented code. --- core/src/systemcontrol.c | 69 +++++++++------------------------------- 1 file changed, 15 insertions(+), 54 deletions(-) diff --git a/core/src/systemcontrol.c b/core/src/systemcontrol.c index 80054398c..fe399b77a 100644 --- a/core/src/systemcontrol.c +++ b/core/src/systemcontrol.c @@ -126,7 +126,8 @@ typedef struct { #define MAESTRO_TRAJ_FILE_TYPE 2 #define MAESTRO_CONF_FILE_TYPE 3 #define MAESTRO_GEOFENCE_FILE_TYPE 4 - +#define MSCP_RESPONSE_DATALENGTH_BYTES 4 +#define MSCP_RESPONSE_STATUS_CODE_BYTES 2 typedef enum { Idle_0, GetServerStatus_0, ArmScenario_0, DisarmScenario_0, StartScenario_1, stop_0, AbortScenario_0, @@ -1438,7 +1439,7 @@ void SystemControlFileDownloadResponse(U16 ResponseStatus, C8 * ResponseString, I32 ResponseDataLength, I32 * Sockfd, U8 Debug) { int i, n, j, t; C8 Length[MSCP_RESPONSE_DATALENGTH_BYTES]; - C8 Status[MSCP_RSPONSE_STATUS_CODE_BYTES]; + C8 Status[MSCP_RESPONSE_STATUS_CODE_BYTES]; C8 Data[SYSTEM_CONTROL_SEND_BUFFER_SIZE]; bzero(Data, SYSTEM_CONTROL_SEND_BUFFER_SIZE); @@ -1451,22 +1452,22 @@ void SystemControlFileDownloadResponse(U16 ResponseStatus, C8 * ResponseString, Status[1] = (C8) ResponseStatus; if (n + MSCP_RESPONSE_DATALENGTH_BYTES < SYSTEM_CONTROL_SEND_BUFFER_SIZE) { - for (i = 0, j = 0; i < 4; i++, j++) + for (i = 0, j = 0; i < MSCP_RESPONSE_DATALENGTH_BYTES; i++, j++) Data[j] = Length[i]; - for (i = 0; i < 2; i++, j++) + for (i = 0; i < MSCP_RESPONSE_STATUS_CODE_BYTES; i++, j++) Data[j] = Status[i]; t = strlen(ResponseString); for (i = 0; i < t; i++, j++) Data[j] = *(ResponseString + i); if (Debug) { - for (i = 0; i < n + 4; i++) + for (i = 0; i < n + MSCP_RESPONSE_DATALENGTH_BYTES; i++) printf("%x-", Data[i]); printf("\n"); } //SystemControlSendBytes(Data, n + 4, Sockfd, 0); - UtilSendTCPData("System Control", Data, 6 + strlen(ResponseString), Sockfd, 0); + UtilSendTCPData("System Control", Data, MSCP_RESPONSE_DATALENGTH_BYTES + MSCP_RESPONSE_STATUS_CODE_BYTES + strlen(ResponseString), Sockfd, 0); } else LogMessage(LOG_LEVEL_ERROR, "Response data more than %d bytes!", SYSTEM_CONTROL_SEND_BUFFER_SIZE); @@ -1476,12 +1477,12 @@ void SystemControlFileDownloadResponse(U16 ResponseStatus, C8 * ResponseString, I32 SystemControlBuildControlResponse(U16 ResponseStatus, C8 * ResponseString, C8 * ResponseData, I32 ResponseDataLength, U8 Debug) { int i = 0, n = 0, j = 0, t = 0; - C8 Length[4]; - C8 Status[2]; + C8 Length[MSCP_RESPONSE_DATALENGTH_BYTES]; + C8 Status[MSCP_RESPONSE_STATUS_CODE_BYTES]; C8 Data[SYSTEM_CONTROL_SEND_BUFFER_SIZE]; bzero(Data, SYSTEM_CONTROL_SEND_BUFFER_SIZE); - n = 2 + strlen(ResponseString) + ResponseDataLength; + n = MSCP_RESPONSE_STATUS_CODE_BYTES + strlen(ResponseString) + ResponseDataLength; Length[0] = (C8) (n >> 24); Length[1] = (C8) (n >> 16); Length[2] = (C8) (n >> 8); @@ -1489,10 +1490,10 @@ I32 SystemControlBuildControlResponse(U16 ResponseStatus, C8 * ResponseString, C Status[0] = (C8) (ResponseStatus >> 8); Status[1] = (C8) ResponseStatus; - if (n + 4 < SYSTEM_CONTROL_SEND_BUFFER_SIZE) { - for (i = 0, j = 0; i < 4; i++, j++) + if (n + MSCP_RESPONSE_DATALENGTH_BYTES < SYSTEM_CONTROL_SEND_BUFFER_SIZE) { + for (i = 0, j = 0; i < MSCP_RESPONSE_DATALENGTH_BYTES; i++, j++) Data[j] = Length[i]; - for (i = 0; i < 2; i++, j++) + for (i = 0; i < MSCP_RESPONSE_STATUS_CODE_BYTES; i++, j++) Data[j] = Status[i]; t = strlen(ResponseString); for (i = 0; i < t; i++, j++) @@ -1504,7 +1505,7 @@ I32 SystemControlBuildControlResponse(U16 ResponseStatus, C8 * ResponseString, C *(ResponseData + i) = Data[i]; //Copy back if (Debug) { - for (i = 0; i < n + 4; i++) + for (i = 0; i < n + MSCP_RESPONSE_DATALENGTH_BYTES; i++) printf("%x-", Data[i]); printf("\n"); } @@ -2065,26 +2066,7 @@ I32 SystemControlReadServerParameterList(C8 * ParameterList, U8 Debug) { I32 SystemControlBuildFileContentInfo(C8 * Path, U8 Debug) { - /* - struct stat st; - C8 CompletePath[MAX_FILE_PATH]; - - bzero(CompletePath, MAX_FILE_PATH); - UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); - strcat(CompletePath, Path); - stat(CompletePath, &st); - *(ReturnValue + 0) = (U8) (st.st_size >> 24); - *(ReturnValue + 1) = (U8) (st.st_size >> 16); - *(ReturnValue + 2) = (U8) (st.st_size >> 8); - *(ReturnValue + 3) = (U8) st.st_size; - - - if (Debug) - LogMessage(LOG_LEVEL_DEBUG, "Filesize %d of %s", (I32) st.st_size, CompletePath); - - return st.st_size; - */ struct stat st; C8 CompletePath[MAX_FILE_PATH]; C8 temporaryCompletePath[MAX_FILE_PATH]; @@ -2094,31 +2076,10 @@ I32 SystemControlBuildFileContentInfo(C8 * Path, U8 Debug) { if (SystemControlDirectoryInfo.exist) return -1; - UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); strcat(CompletePath, Path); stat(CompletePath, &st); - /* - // Create new temporary file, containing the length of the current file in hex + the rest of the document - strcat(temporaryCompletePath, ".temp"); - FILE *comp_fd = fopen(CompletePath, "r"); - FILE *temp_fd = fopen(temporaryCompletePath, "w"); - - fprintf(temp_fd, "%c%c%c%c", - (U8) (st.st_size >> 24), (U8) (st.st_size >> 16), (U8) (st.st_size >> 8), (U8) (st.st_size) - ); - - while (!feof(comp_fd)) { - fputc(fgetc(comp_fd), temp_fd); - } - - fclose(comp_fd); - fclose(temp_fd); - - // Rename the temporary file to the name of the previous one - rename(temporaryCompletePath, CompletePath); - stat(CompletePath, &st); - */ + // Create mmap of the file and return the length SystemControlDirectoryInfo.fd = open(CompletePath, O_RDWR); SystemControlDirectoryInfo.info_buffer = From 0f2636dce0ad6bffd7060336b855b28fa6169ef7 Mon Sep 17 00:00:00 2001 From: Sebastian Loh Lindholm Date: Tue, 7 Apr 2020 10:59:21 +0200 Subject: [PATCH 436/523] Format source files... --- core/src/systemcontrol.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/core/src/systemcontrol.c b/core/src/systemcontrol.c index fe399b77a..22c1ef10b 100644 --- a/core/src/systemcontrol.c +++ b/core/src/systemcontrol.c @@ -1467,7 +1467,9 @@ void SystemControlFileDownloadResponse(U16 ResponseStatus, C8 * ResponseString, } //SystemControlSendBytes(Data, n + 4, Sockfd, 0); - UtilSendTCPData("System Control", Data, MSCP_RESPONSE_DATALENGTH_BYTES + MSCP_RESPONSE_STATUS_CODE_BYTES + strlen(ResponseString), Sockfd, 0); + UtilSendTCPData("System Control", Data, + MSCP_RESPONSE_DATALENGTH_BYTES + MSCP_RESPONSE_STATUS_CODE_BYTES + + strlen(ResponseString), Sockfd, 0); } else LogMessage(LOG_LEVEL_ERROR, "Response data more than %d bytes!", SYSTEM_CONTROL_SEND_BUFFER_SIZE); From c17f835ac178a4ae4c4154f4f50ae3219e0104fa Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 7 Apr 2020 15:55:07 +0200 Subject: [PATCH 437/523] Fixed typo in CMakeLists --- modules/ObjectMonitoring/CMakeLists.txt | 3 ++- modules/ObjectMonitoring/src/topichandlers.cpp | 6 ++++++ modules/ObjectMonitoring/src/topichandlers.h | 11 +++++++++++ 3 files changed, 19 insertions(+), 1 deletion(-) create mode 100644 modules/ObjectMonitoring/src/topichandlers.cpp create mode 100644 modules/ObjectMonitoring/src/topichandlers.h diff --git a/modules/ObjectMonitoring/CMakeLists.txt b/modules/ObjectMonitoring/CMakeLists.txt index 37f701aab..9393b8bf2 100644 --- a/modules/ObjectMonitoring/CMakeLists.txt +++ b/modules/ObjectMonitoring/CMakeLists.txt @@ -20,7 +20,7 @@ include(GNUInstallDirs) find_package(Threads REQUIRED) find_library(paho-mqtt3a NAMES libpaho-mqtt3a.so REQUIRED) -find_library(paho-mqtt3a NAMES libpaho-mqtt3c.so REQUIRED) +find_library(paho-mqtt3c NAMES libpaho-mqtt3c.so REQUIRED) add_library(pahomqtt3a SHARED IMPORTED) add_library(pahomqtt3c SHARED IMPORTED) set_property(TARGET pahomqtt3a PROPERTY IMPORTED_LOCATION ${paho-mqtt3a}) @@ -33,6 +33,7 @@ add_executable(${OBJECT_MONITORING_TARGET} ${CMAKE_CURRENT_SOURCE_DIR}/src/protocoldata.cpp ${CMAKE_CURRENT_SOURCE_DIR}/src/ISO22133ProtocolData.cpp ${CMAKE_CURRENT_SOURCE_DIR}/src/mqttconnectionhandler.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/mqtttopichandlers.cpp ) # Link project executable to util libraries diff --git a/modules/ObjectMonitoring/src/topichandlers.cpp b/modules/ObjectMonitoring/src/topichandlers.cpp new file mode 100644 index 000000000..de46c26be --- /dev/null +++ b/modules/ObjectMonitoring/src/topichandlers.cpp @@ -0,0 +1,6 @@ +#include "topichandlers.h" + +topichandlers::topichandlers() +{ + +} diff --git a/modules/ObjectMonitoring/src/topichandlers.h b/modules/ObjectMonitoring/src/topichandlers.h new file mode 100644 index 000000000..a2784ca03 --- /dev/null +++ b/modules/ObjectMonitoring/src/topichandlers.h @@ -0,0 +1,11 @@ +#ifndef TOPICHANDLERS_H +#define TOPICHANDLERS_H + + +class topichandlers +{ +public: + topichandlers(); +}; + +#endif // TOPICHANDLERS_H \ No newline at end of file From 2b50fc7e6046ab7398a8821438637b3e0464bc07 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 7 Apr 2020 15:56:05 +0200 Subject: [PATCH 438/523] Implemented a mapping functionality between topics and handlers --- .../inc/mqtttopichandlers.hpp | 15 +++ .../src/mqtttopichandlers.cpp | 116 ++++++++++++++++++ .../ObjectMonitoring/src/topichandlers.cpp | 6 - modules/ObjectMonitoring/src/topichandlers.h | 11 -- 4 files changed, 131 insertions(+), 17 deletions(-) create mode 100644 modules/ObjectMonitoring/inc/mqtttopichandlers.hpp create mode 100644 modules/ObjectMonitoring/src/mqtttopichandlers.cpp delete mode 100644 modules/ObjectMonitoring/src/topichandlers.cpp delete mode 100644 modules/ObjectMonitoring/src/topichandlers.h diff --git a/modules/ObjectMonitoring/inc/mqtttopichandlers.hpp b/modules/ObjectMonitoring/inc/mqtttopichandlers.hpp new file mode 100644 index 000000000..69f25f07a --- /dev/null +++ b/modules/ObjectMonitoring/inc/mqtttopichandlers.hpp @@ -0,0 +1,15 @@ +#ifndef MQTTTOPICHANDLERS_H +#define MQTTTOPICHANDLERS_H +#include + +using namespace std; + +namespace MQTTTopicHandlers { + typedef int (*MQTTTopicHandler)(void*, string&); + + int handleMessage(void* message, string &topic); + int monrHandler(void* message, string &topic); + int motorRPMHandler(void* message, string &topic); +} + +#endif // TOPICHANDLERS_H diff --git a/modules/ObjectMonitoring/src/mqtttopichandlers.cpp b/modules/ObjectMonitoring/src/mqtttopichandlers.cpp new file mode 100644 index 000000000..cf836ae2d --- /dev/null +++ b/modules/ObjectMonitoring/src/mqtttopichandlers.cpp @@ -0,0 +1,116 @@ +#include +#include +#include +#include +#include + +#include "mqtttopichandlers.hpp" +#include "logging.h" + + +using namespace std; +using namespace MQTTTopicHandlers; + +static bool containsWildcards(const string &topic); + +/*! + * split String splitter template function: splits input string at delimiter + * and outputs substrings to result iterator + */ +template +void split(const string &s, char delim, Out result) { + istringstream iss(s); + string item; + while(getline(iss, item, delim)) { + *result++ = item; + } +} + +struct comp { + bool operator()(const string &lhs, const string &rhs) const { + if (!lhs.compare(rhs)){ + // Topic expressions identical: equivalent + return true; + } + + bool lhsIsExpression = containsWildcards(lhs); + bool rhsIsExpression = containsWildcards(rhs); + if (lhsIsExpression && rhsIsExpression) { + // Expressions not equal and both contain wildcards: not equivalent + return false; + } + else if (!lhsIsExpression && !rhsIsExpression) { + // Both of the expressions are topic names and not equal: not equivalent + return false; + } + + // One of the strings is an expression and the other a topic name + const string &expr = lhsIsExpression ? lhs : rhs; + const string &topic = lhsIsExpression ? rhs : lhs; + + vector exprTokens, topicTokens; + split(expr, '/', back_inserter(exprTokens)); + split(topic, '/', back_inserter(topicTokens)); + + if (topicTokens.size() < exprTokens.size()) { + // Topic contains fewer levels than the expression thus they cannot match + return false; + } + + for (unsigned int i = 0; i < exprTokens.size(); ++i) { + if (!exprTokens[i].compare("#") && topicTokens[i].size() != 0) { + return true; + } + else if (!exprTokens[i].compare("+") && topicTokens[i].size() != 0) { + continue; + } + else if (!exprTokens[i].compare(topicTokens[i])) { + continue; + } + else { + return false; + } + } + + return true; + } +}; + +bool containsWildcards(const string &topic) { + bool retval = false; + const string singleLevelWildCard = "/+"; + const string multiLevelWildCard = "/#"; + // Check if topic contains any single level wildcards + retval = retval || topic.find(singleLevelWildCard + "/") != string::npos; + // Check if topic ends with a wildcard + retval = retval || topic.compare(topic.length() - singleLevelWildCard.length(), + singleLevelWildCard.length(), singleLevelWildCard); + retval = retval || topic.compare(topic.length() - multiLevelWildCard.length(), + multiLevelWildCard.length(), multiLevelWildCard); + return retval; +} + +typedef unordered_map, comp> HandlerMap; + +static HandlerMap handlerMap { + {"astazero/+/+/motor-rpm/#", MQTTTopicHandlers::motorRPMHandler} +}; + +int MQTTTopicHandlers::handleMessage(void* message, string& topic) { + try { + return handlerMap.at(topic)(message, topic); + } + catch (out_of_range) { + LogMessage(LOG_LEVEL_ERROR, "No handler specified for topic %s", topic.c_str()); + return -1; + } +} + +int MQTTTopicHandlers::monrHandler(void* message, string& topic) { + return -1; +} + +int MQTTTopicHandlers::motorRPMHandler(void* message, string &topic) { + LogPrint("Received %s on topic %s", message, topic.c_str()); + return 0; +} diff --git a/modules/ObjectMonitoring/src/topichandlers.cpp b/modules/ObjectMonitoring/src/topichandlers.cpp deleted file mode 100644 index de46c26be..000000000 --- a/modules/ObjectMonitoring/src/topichandlers.cpp +++ /dev/null @@ -1,6 +0,0 @@ -#include "topichandlers.h" - -topichandlers::topichandlers() -{ - -} diff --git a/modules/ObjectMonitoring/src/topichandlers.h b/modules/ObjectMonitoring/src/topichandlers.h deleted file mode 100644 index a2784ca03..000000000 --- a/modules/ObjectMonitoring/src/topichandlers.h +++ /dev/null @@ -1,11 +0,0 @@ -#ifndef TOPICHANDLERS_H -#define TOPICHANDLERS_H - - -class topichandlers -{ -public: - topichandlers(); -}; - -#endif // TOPICHANDLERS_H \ No newline at end of file From 6bc6d93a227c474e39c3c5a176a1ca1539891f8a Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 7 Apr 2020 15:56:29 +0200 Subject: [PATCH 439/523] Simplified the MQTT connection handler a bit --- .../inc/mqttconnectionhandler.hpp | 50 ++++++++----------- .../src/mqttconnectionhandler.cpp | 3 +- 2 files changed, 24 insertions(+), 29 deletions(-) diff --git a/modules/ObjectMonitoring/inc/mqttconnectionhandler.hpp b/modules/ObjectMonitoring/inc/mqttconnectionhandler.hpp index b69fd155a..81b2b55aa 100644 --- a/modules/ObjectMonitoring/inc/mqttconnectionhandler.hpp +++ b/modules/ObjectMonitoring/inc/mqttconnectionhandler.hpp @@ -3,10 +3,12 @@ #include #include +#include #include #include "logging.h" #include "iso22133.h" +#include "mqtttopichandlers.hpp" using namespace std; @@ -15,16 +17,18 @@ class MQTTError : virtual public std::exception { int errorCode; string errorMessage; public: - explicit MQTTError(string msg, int errorCode) : errorCode(errorCode), msg(errorMessage) {} + explicit MQTTError(const string &msg, int errorCode) : errorCode(errorCode), errorMessage(msg) {} - virtual const char* what() const throw () { + virtual ~MQTTError() noexcept {} + + virtual const char* what() const noexcept { return errorMessage.c_str(); } - virtual int getErrorNumber() const throw () { + virtual int getErrorNumber() const noexcept { return errorCode; } -} ; +}; class MQTTConnectionHandler { @@ -34,34 +38,24 @@ class MQTTConnectionHandler { private: + void setMessageArrivedCallback(MQTTTopicHandlers::MQTTTopicHandler messageCallback) { + this->messageCallback = messageCallback; + MQTTClient_setCallbacks(this->client, this, nullptr, &MQTTConnectionHandler::arrivalCallback, nullptr); + return; + } + MQTTTopicHandlers::MQTTTopicHandler messageCallback; + + static int arrivalCallback(void* context, char* topicName, int, + MQTTClient_message* message) { + MQTTConnectionHandler* thisObject = static_cast(context); + string topic(topicName); + return thisObject->messageCallback(message->payload, topic); + } + MQTTClient client; string clientID = ""; string serverURI = ""; MQTTClient_connectOptions connectionOptions; - - template class MQTTConnection { - public: - MQTTConnection(MQTTClient &client, string &topicExpression, int messageCallback(MQTTDataType* message, - string &topicExpression)) : topicExpression(topicExpression), messageCallback(messageCallback) { - MQTTClient_setCallbacks(client, this, nullptr, &MQTTConnection::arrivalCallback, nullptr); - } - private: - string topicExpression = ""; - int (*messageCallback)(void* message, string &topicName); - - static int arrivalCallback(void* context, char* topicName, int, - MQTTClient_message* message) { - MQTTConnection* thisObject = static_cast(context); - string topic(topicName); - if (message->payloadlen == sizeof (MQTTDataType)) { - return thisObject->messageCallback(static_cast(message->payload), topic); - } - LogMessage(LOG_LEVEL_ERROR, "Received MQTT payload of size %d while expecting size %u on topic %s", - message->payloadlen, sizeof (MQTTDataType), topic.c_str()); - return -1; - } - }; - }; #endif // MQTTCONNECTIONHANDLER_H diff --git a/modules/ObjectMonitoring/src/mqttconnectionhandler.cpp b/modules/ObjectMonitoring/src/mqttconnectionhandler.cpp index 0e739f531..1106c3a6a 100644 --- a/modules/ObjectMonitoring/src/mqttconnectionhandler.cpp +++ b/modules/ObjectMonitoring/src/mqttconnectionhandler.cpp @@ -1,4 +1,4 @@ -#include "mqttconnection.h" +#include "mqttconnectionhandler.hpp" #define DEFAULT_SERVER_URI "tcp://localhost:1883" @@ -20,4 +20,5 @@ void MQTTConnectionHandler::establishConnection() { throw new MQTTError(errMsg, returnCode); } LogMessage(LOG_LEVEL_INFO, "Successfully connected to MQTT broker"); + this->setMessageArrivedCallback(MQTTTopicHandlers::handleMessage); } From 692e615cb4d9d5ce05961bd226504c4caa4b9747 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 7 Apr 2020 15:56:51 +0200 Subject: [PATCH 440/523] Rewrote main to use the connection handler --- modules/ObjectMonitoring/src/main.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/modules/ObjectMonitoring/src/main.cpp b/modules/ObjectMonitoring/src/main.cpp index c67bfc0a0..dd95e9177 100644 --- a/modules/ObjectMonitoring/src/main.cpp +++ b/modules/ObjectMonitoring/src/main.cpp @@ -10,6 +10,7 @@ #include "connectionhandler.h" #include "protocoldata.h" #include "ISO22133ProtocolData.h" +#include "mqttconnectionhandler.hpp" #include "util.h" #define MODULE_NAME "ObjectMonitoring" @@ -41,6 +42,8 @@ int main() // TODO: start MQ communication handler // Initialize MQTT handler + MQTTConnectionHandler mqttConnectionHandler("Maestro"); + mqttConnectionHandler.establishConnection(); while(true) { From c48f36045520f4894f4b7a40a3721879b67b21f4 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 7 Apr 2020 16:19:20 +0200 Subject: [PATCH 441/523] Added function descriptions --- .../inc/mqtttopichandlers.hpp | 7 ++- .../src/mqtttopichandlers.cpp | 52 +++++++++++++++---- 2 files changed, 47 insertions(+), 12 deletions(-) diff --git a/modules/ObjectMonitoring/inc/mqtttopichandlers.hpp b/modules/ObjectMonitoring/inc/mqtttopichandlers.hpp index 69f25f07a..7b744efad 100644 --- a/modules/ObjectMonitoring/inc/mqtttopichandlers.hpp +++ b/modules/ObjectMonitoring/inc/mqtttopichandlers.hpp @@ -1,6 +1,7 @@ #ifndef MQTTTOPICHANDLERS_H #define MQTTTOPICHANDLERS_H #include +#include using namespace std; @@ -8,8 +9,10 @@ namespace MQTTTopicHandlers { typedef int (*MQTTTopicHandler)(void*, string&); int handleMessage(void* message, string &topic); - int monrHandler(void* message, string &topic); + int monrHandler(void*, string &topic); int motorRPMHandler(void* message, string &topic); + + vector getSubscriptions(void); } -#endif // TOPICHANDLERS_H +#endif // MQTTTOPICHANDLERS_H diff --git a/modules/ObjectMonitoring/src/mqtttopichandlers.cpp b/modules/ObjectMonitoring/src/mqtttopichandlers.cpp index cf836ae2d..b2ac439d2 100644 --- a/modules/ObjectMonitoring/src/mqtttopichandlers.cpp +++ b/modules/ObjectMonitoring/src/mqtttopichandlers.cpp @@ -1,7 +1,6 @@ #include #include #include -#include #include #include "mqtttopichandlers.hpp" @@ -14,7 +13,7 @@ using namespace MQTTTopicHandlers; static bool containsWildcards(const string &topic); /*! - * split String splitter template function: splits input string at delimiter + * \brief split String splitter template function: splits input string at delimiter * and outputs substrings to result iterator */ template @@ -26,7 +25,15 @@ void split(const string &s, char delim, Out result) { } } -struct comp { +/*! + * \brief The Comparator struct is used as a replacement predicate by unordered_map to determine + * if two elements are equal. + * \details The default behaviour for strings is the regular string + * compare to determine if they represent the same data. Here, this behaviour is + * replaced such that e.g. the map key "astazero/projectA/vehicle1/velocity" matches + * the key "astazero/+/vehicle1/#". + */ +struct Comparator { bool operator()(const string &lhs, const string &rhs) const { if (!lhs.compare(rhs)){ // Topic expressions identical: equivalent @@ -76,6 +83,22 @@ struct comp { } }; + + +typedef unordered_map, Comparator> HandlerMap; +/*! + * \brief handlerMap maps MQTT topics to their respective handlers + */ +static HandlerMap handlerMap { + {"astazero/+/+/motor-rpm/#", MQTTTopicHandlers::motorRPMHandler}, + {"astazero/+/+/MONR/#", MQTTTopicHandlers::monrHandler} +}; + +/*! + * \brief containsWildcards Checks if a string contains any MQTT wildcard symbols + * \param topic Topic string + * \return True if the string contained any wildcards, false otherwise + */ bool containsWildcards(const string &topic) { bool retval = false; const string singleLevelWildCard = "/+"; @@ -83,19 +106,22 @@ bool containsWildcards(const string &topic) { // Check if topic contains any single level wildcards retval = retval || topic.find(singleLevelWildCard + "/") != string::npos; // Check if topic ends with a wildcard - retval = retval || topic.compare(topic.length() - singleLevelWildCard.length(), + retval = retval || !topic.compare(topic.length() - singleLevelWildCard.length(), singleLevelWildCard.length(), singleLevelWildCard); - retval = retval || topic.compare(topic.length() - multiLevelWildCard.length(), + retval = retval || !topic.compare(topic.length() - multiLevelWildCard.length(), multiLevelWildCard.length(), multiLevelWildCard); return retval; } -typedef unordered_map, comp> HandlerMap; -static HandlerMap handlerMap { - {"astazero/+/+/motor-rpm/#", MQTTTopicHandlers::motorRPMHandler} -}; +/*! + * \brief MQTTTopicHandlers::handleMessage Delegates handling of a message to one of the + * handler functions specified in ::handlerMap depending on the input topic. + * \param message Message to be handled + * \param topic Topic on which the message was received + * \return 0 on success, -1 otherwise + */ int MQTTTopicHandlers::handleMessage(void* message, string& topic) { try { return handlerMap.at(topic)(message, topic); @@ -106,7 +132,13 @@ int MQTTTopicHandlers::handleMessage(void* message, string& topic) { } } -int MQTTTopicHandlers::monrHandler(void* message, string& topic) { +/*! ******************************************************************** + * Topic handlers + * ******************************************************************** + */ + +int MQTTTopicHandlers::monrHandler(void*, string& topic) { + LogPrint("MONR handler not implemented. Topic %s", topic.c_str()); return -1; } From 1e042bf0fecd8b34b72b96c41ad03125d9808fab Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 7 Apr 2020 16:37:39 +0200 Subject: [PATCH 442/523] Added subscription to configured topics with handlers --- .../ObjectMonitoring/inc/mqttconnectionhandler.hpp | 5 +++++ .../ObjectMonitoring/src/mqttconnectionhandler.cpp | 3 +++ modules/ObjectMonitoring/src/mqtttopichandlers.cpp | 13 ++++++++++++- 3 files changed, 20 insertions(+), 1 deletion(-) diff --git a/modules/ObjectMonitoring/inc/mqttconnectionhandler.hpp b/modules/ObjectMonitoring/inc/mqttconnectionhandler.hpp index 81b2b55aa..c9bcebed7 100644 --- a/modules/ObjectMonitoring/inc/mqttconnectionhandler.hpp +++ b/modules/ObjectMonitoring/inc/mqttconnectionhandler.hpp @@ -56,6 +56,11 @@ class MQTTConnectionHandler { string clientID = ""; string serverURI = ""; MQTTClient_connectOptions connectionOptions; + enum : int { + FIRE_AND_FORGET = 0, + AT_LEAST_ONCE = 1, + ONLY_ONCE = 2 + } qualityOfService = FIRE_AND_FORGET; }; #endif // MQTTCONNECTIONHANDLER_H diff --git a/modules/ObjectMonitoring/src/mqttconnectionhandler.cpp b/modules/ObjectMonitoring/src/mqttconnectionhandler.cpp index 1106c3a6a..cb2c4c0c9 100644 --- a/modules/ObjectMonitoring/src/mqttconnectionhandler.cpp +++ b/modules/ObjectMonitoring/src/mqttconnectionhandler.cpp @@ -21,4 +21,7 @@ void MQTTConnectionHandler::establishConnection() { } LogMessage(LOG_LEVEL_INFO, "Successfully connected to MQTT broker"); this->setMessageArrivedCallback(MQTTTopicHandlers::handleMessage); + for (const string &subscription : MQTTTopicHandlers::getSubscriptions()) { + MQTTClient_subscribe(this->client, subscription.c_str(), this->qualityOfService); + } } diff --git a/modules/ObjectMonitoring/src/mqtttopichandlers.cpp b/modules/ObjectMonitoring/src/mqtttopichandlers.cpp index b2ac439d2..206fce0f7 100644 --- a/modules/ObjectMonitoring/src/mqtttopichandlers.cpp +++ b/modules/ObjectMonitoring/src/mqtttopichandlers.cpp @@ -113,7 +113,18 @@ bool containsWildcards(const string &topic) { return retval; } - +/*! + * \brief MQTTTopicHandlers::getSubscriptions Returns a vector of MQTT subscriptions + * which have been configured with handlers. + * \return A vector of MQTT subscriptions + */ +vector MQTTTopicHandlers::getSubscriptions() { + vector subscriptions; + for (HandlerMap::iterator it = handlerMap.begin(); it != handlerMap.end(); ++it) { + subscriptions.push_back(it->first); + } + return subscriptions; +} /*! * \brief MQTTTopicHandlers::handleMessage Delegates handling of a message to one of the From a9fc2f60c73273f20154a67903040b1538a6129d Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Wed, 8 Apr 2020 09:54:18 +0200 Subject: [PATCH 443/523] origin fix --- core/src/objectcontrol.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/core/src/objectcontrol.c b/core/src/objectcontrol.c index 82623b4b3..fb25d1833 100644 --- a/core/src/objectcontrol.c +++ b/core/src/objectcontrol.c @@ -188,10 +188,10 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { C8 OriginLatitude[SMALL_BUFFER_SIZE_0], OriginLongitude[SMALL_BUFFER_SIZE_0], OriginAltitude[SMALL_BUFFER_SIZE_0], OriginHeading[SMALL_BUFFER_SIZE_0]; C8 TextBuffer[SMALL_BUFFER_SIZE_0]; - dbl OriginLatitudeDbl = DEFAULT_ORIGO_LAT; - dbl OriginLongitudeDbl = DEFAULT_ORIGO_LOG; - dbl OriginAltitudeDbl = DEFAULT_ORIGO_ALT; - dbl OriginHeadingDbl = DEFAULT_ORIGO_HEADING; + dbl OriginLatitudeDbl = DEFAULT_ORIGIN_LAT; + dbl OriginLongitudeDbl = DEFAULT_ORIGIN_LOG; + dbl OriginAltitudeDbl = DEFAULT_ORIGIN_ALT; + dbl OriginHeadingDbl = DEFAULT_ORIGIN_HEADING; C8 pcSendBuffer[MBUS_MAX_DATALEN]; C8 ObjectPort[SMALL_BUFFER_SIZE_0]; MonitorDataType monitorData; From 089c0abd764539f142e7f6216d34cf2804b522c8 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 8 Apr 2020 15:16:01 +0200 Subject: [PATCH 444/523] Simplified the connection handler --- .../inc/mqttconnectionhandler.hpp | 26 +++++++-------- .../src/mqttconnectionhandler.cpp | 33 ++++++++++++++++--- 2 files changed, 40 insertions(+), 19 deletions(-) diff --git a/modules/ObjectMonitoring/inc/mqttconnectionhandler.hpp b/modules/ObjectMonitoring/inc/mqttconnectionhandler.hpp index c9bcebed7..69d828be5 100644 --- a/modules/ObjectMonitoring/inc/mqttconnectionhandler.hpp +++ b/modules/ObjectMonitoring/inc/mqttconnectionhandler.hpp @@ -30,7 +30,6 @@ class MQTTError : virtual public std::exception { } }; - class MQTTConnectionHandler { public: MQTTConnectionHandler(const string clientID); @@ -38,12 +37,20 @@ class MQTTConnectionHandler { private: - void setMessageArrivedCallback(MQTTTopicHandlers::MQTTTopicHandler messageCallback) { + MQTTClient client; + string clientID = ""; + string serverURI = ""; + MQTTClient_connectOptions connectionOptions; + enum : int { + FIRE_AND_FORGET = 0, + AT_LEAST_ONCE = 1, + ONLY_ONCE = 2 + } qualityOfService = AT_LEAST_ONCE; + + MQTTTopicHandlers::MQTTTopicHandler messageCallback; + void setMessageCallback(MQTTTopicHandlers::MQTTTopicHandler messageCallback) { this->messageCallback = messageCallback; - MQTTClient_setCallbacks(this->client, this, nullptr, &MQTTConnectionHandler::arrivalCallback, nullptr); - return; } - MQTTTopicHandlers::MQTTTopicHandler messageCallback; static int arrivalCallback(void* context, char* topicName, int, MQTTClient_message* message) { @@ -52,15 +59,6 @@ class MQTTConnectionHandler { return thisObject->messageCallback(message->payload, topic); } - MQTTClient client; - string clientID = ""; - string serverURI = ""; - MQTTClient_connectOptions connectionOptions; - enum : int { - FIRE_AND_FORGET = 0, - AT_LEAST_ONCE = 1, - ONLY_ONCE = 2 - } qualityOfService = FIRE_AND_FORGET; }; #endif // MQTTCONNECTIONHANDLER_H diff --git a/modules/ObjectMonitoring/src/mqttconnectionhandler.cpp b/modules/ObjectMonitoring/src/mqttconnectionhandler.cpp index cb2c4c0c9..c570707df 100644 --- a/modules/ObjectMonitoring/src/mqttconnectionhandler.cpp +++ b/modules/ObjectMonitoring/src/mqttconnectionhandler.cpp @@ -1,17 +1,35 @@ #include "mqttconnectionhandler.hpp" -#define DEFAULT_SERVER_URI "tcp://localhost:1883" +#define DEFAULT_SERVER_URI "tcp://127.0.0.1:1883" MQTTConnectionHandler::MQTTConnectionHandler(const string clientID) - : clientID(clientID), connectionOptions(MQTTClient_connectOptions_initializer5) { + : clientID(clientID), connectionOptions(MQTTClient_connectOptions_initializer) { + int returnCode; // TODO: read a config file to get non-default values this->serverURI = DEFAULT_SERVER_URI; - MQTTClient_create(&this->client, this->serverURI.c_str(), this->clientID.c_str(), MQTTCLIENT_PERSISTENCE_NONE, nullptr); + if ((returnCode = MQTTClient_create(&this->client, this->serverURI.c_str(), this->clientID.c_str(), MQTTCLIENT_PERSISTENCE_NONE, nullptr)) + != MQTTCLIENT_SUCCESS) { + string errMsg = "Failed to create MQTT client: error code " + to_string(returnCode); + LogMessage(LOG_LEVEL_WARNING, errMsg.c_str()); + throw new MQTTError(errMsg, returnCode); + } } void MQTTConnectionHandler::establishConnection() { int returnCode; + if (MQTTClient_isConnected(this->client)) { + return; + } + this->setMessageCallback(&MQTTTopicHandlers::handleMessage); + + if ((returnCode = MQTTClient_setCallbacks(this->client, this, nullptr, &MQTTConnectionHandler::arrivalCallback, nullptr)) + != MQTTCLIENT_SUCCESS) { + string errMsg = "Failed to set callbacks for MQTT client: error code " + to_string(returnCode); + LogMessage(LOG_LEVEL_ERROR, errMsg.c_str()); + throw new MQTTError(errMsg, returnCode); + } + LogMessage(LOG_LEVEL_INFO, "Connecting to MQTT broker at %s", this->serverURI.c_str()); if ((returnCode = MQTTClient_connect(this->client, &this->connectionOptions)) != MQTTCLIENT_SUCCESS) { @@ -20,8 +38,13 @@ void MQTTConnectionHandler::establishConnection() { throw new MQTTError(errMsg, returnCode); } LogMessage(LOG_LEVEL_INFO, "Successfully connected to MQTT broker"); - this->setMessageArrivedCallback(MQTTTopicHandlers::handleMessage); + for (const string &subscription : MQTTTopicHandlers::getSubscriptions()) { - MQTTClient_subscribe(this->client, subscription.c_str(), this->qualityOfService); + LogMessage(LOG_LEVEL_INFO, "Subscribing to topic %s", subscription.c_str()); + if ((returnCode = MQTTClient_subscribe(this->client, subscription.c_str(), this->qualityOfService)) + != MQTTCLIENT_SUCCESS) { + string errMsg = "Failed to subscribe to topic " + subscription + ": error code " + to_string(returnCode); + LogMessage(LOG_LEVEL_WARNING, errMsg.c_str()); + } } } From b8025097a5284215f16ffd6fb5b14bdcee366c65 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 8 Apr 2020 15:16:23 +0200 Subject: [PATCH 445/523] Fixed some issues with the handler map --- .../src/mqtttopichandlers.cpp | 216 ++++++++++-------- 1 file changed, 115 insertions(+), 101 deletions(-) diff --git a/modules/ObjectMonitoring/src/mqtttopichandlers.cpp b/modules/ObjectMonitoring/src/mqtttopichandlers.cpp index 206fce0f7..d9eb8ae3a 100644 --- a/modules/ObjectMonitoring/src/mqtttopichandlers.cpp +++ b/modules/ObjectMonitoring/src/mqtttopichandlers.cpp @@ -9,83 +9,16 @@ using namespace std; using namespace MQTTTopicHandlers; +typedef unordered_map HandlerMap; -static bool containsWildcards(const string &topic); - -/*! - * \brief split String splitter template function: splits input string at delimiter - * and outputs substrings to result iterator - */ -template -void split(const string &s, char delim, Out result) { - istringstream iss(s); - string item; - while(getline(iss, item, delim)) { - *result++ = item; - } -} - -/*! - * \brief The Comparator struct is used as a replacement predicate by unordered_map to determine - * if two elements are equal. - * \details The default behaviour for strings is the regular string - * compare to determine if they represent the same data. Here, this behaviour is - * replaced such that e.g. the map key "astazero/projectA/vehicle1/velocity" matches - * the key "astazero/+/vehicle1/#". - */ -struct Comparator { - bool operator()(const string &lhs, const string &rhs) const { - if (!lhs.compare(rhs)){ - // Topic expressions identical: equivalent - return true; - } - - bool lhsIsExpression = containsWildcards(lhs); - bool rhsIsExpression = containsWildcards(rhs); - if (lhsIsExpression && rhsIsExpression) { - // Expressions not equal and both contain wildcards: not equivalent - return false; - } - else if (!lhsIsExpression && !rhsIsExpression) { - // Both of the expressions are topic names and not equal: not equivalent - return false; - } - - // One of the strings is an expression and the other a topic name - const string &expr = lhsIsExpression ? lhs : rhs; - const string &topic = lhsIsExpression ? rhs : lhs; - - vector exprTokens, topicTokens; - split(expr, '/', back_inserter(exprTokens)); - split(topic, '/', back_inserter(topicTokens)); - - if (topicTokens.size() < exprTokens.size()) { - // Topic contains fewer levels than the expression thus they cannot match - return false; - } - - for (unsigned int i = 0; i < exprTokens.size(); ++i) { - if (!exprTokens[i].compare("#") && topicTokens[i].size() != 0) { - return true; - } - else if (!exprTokens[i].compare("+") && topicTokens[i].size() != 0) { - continue; - } - else if (!exprTokens[i].compare(topicTokens[i])) { - continue; - } - else { - return false; - } - } - - return true; - } -}; +//! ************ Static function declarations ****************************** +static bool containsWildcards(const string &topic); +template void split(const string &s, char delim, Out result); +static bool topicsAreEquivalent(const string &lhs, const string &rhs); -typedef unordered_map, Comparator> HandlerMap; +//! ************ Static variables ****************************************** /*! * \brief handlerMap maps MQTT topics to their respective handlers */ @@ -94,25 +27,26 @@ static HandlerMap handlerMap { {"astazero/+/+/MONR/#", MQTTTopicHandlers::monrHandler} }; -/*! - * \brief containsWildcards Checks if a string contains any MQTT wildcard symbols - * \param topic Topic string - * \return True if the string contained any wildcards, false otherwise + +//! ************ Function definitions ************************************** +/*! ************************************************************************ + * Topic handlers + * ************************************************************************ */ -bool containsWildcards(const string &topic) { - bool retval = false; - const string singleLevelWildCard = "/+"; - const string multiLevelWildCard = "/#"; - // Check if topic contains any single level wildcards - retval = retval || topic.find(singleLevelWildCard + "/") != string::npos; - // Check if topic ends with a wildcard - retval = retval || !topic.compare(topic.length() - singleLevelWildCard.length(), - singleLevelWildCard.length(), singleLevelWildCard); - retval = retval || !topic.compare(topic.length() - multiLevelWildCard.length(), - multiLevelWildCard.length(), multiLevelWildCard); - return retval; +int MQTTTopicHandlers::monrHandler(void*, string& topic) { + LogPrint("MONR handler not implemented. Topic %s", topic.c_str()); + return -1; +} + +int MQTTTopicHandlers::motorRPMHandler(void* message, string &topic) { + LogPrint("Received %s on topic %s", message, topic.c_str()); + return 0; } +/*! ************************************************************************ + * Other functions + * ************************************************************************ + */ /*! * \brief MQTTTopicHandlers::getSubscriptions Returns a vector of MQTT subscriptions * which have been configured with handlers. @@ -126,6 +60,7 @@ vector MQTTTopicHandlers::getSubscriptions() { return subscriptions; } + /*! * \brief MQTTTopicHandlers::handleMessage Delegates handling of a message to one of the * handler functions specified in ::handlerMap depending on the input topic. @@ -134,26 +69,105 @@ vector MQTTTopicHandlers::getSubscriptions() { * \return 0 on success, -1 otherwise */ int MQTTTopicHandlers::handleMessage(void* message, string& topic) { - try { - return handlerMap.at(topic)(message, topic); + bool handlerFound = false; + int returnValue = 0; + for (HandlerMap::iterator it = handlerMap.begin(); it != handlerMap.end(); ++it) { + if (topicsAreEquivalent(it->first, topic)) { + handlerFound = true; + returnValue = returnValue || it->second(message, topic); + } } - catch (out_of_range) { + if (!handlerFound) { LogMessage(LOG_LEVEL_ERROR, "No handler specified for topic %s", topic.c_str()); return -1; } + return returnValue; } -/*! ******************************************************************** - * Topic handlers - * ******************************************************************** + +/*! + * \brief split String splitter template function: splits input string at delimiter + * and outputs substrings to result iterator */ +template +void split(const string &s, char delim, Out result) { + istringstream iss(s); + string item; + while(getline(iss, item, delim)) { + *result++ = item; + } +} -int MQTTTopicHandlers::monrHandler(void*, string& topic) { - LogPrint("MONR handler not implemented. Topic %s", topic.c_str()); - return -1; + +/*! + * \brief topicsAreEquivalent compares two MQTT topics and returns whether or not they + * can be considered equal. The behaviour is such that e.g. the topic + * "astazero/projectA/vehicle1/velocity" matches the topic "astazero/+/vehicle1/#". + */ +bool topicsAreEquivalent(const string &lhs, const string &rhs) { + if (!lhs.compare(rhs)){ + // Topic expressions identical: equivalent + return true; + } + + bool lhsIsExpression = containsWildcards(lhs); + bool rhsIsExpression = containsWildcards(rhs); + if (lhsIsExpression && rhsIsExpression) { + // Expressions not equal and both contain wildcards: not equivalent + return false; + } + else if (!lhsIsExpression && !rhsIsExpression) { + // Both of the expressions are topic names and not equal: not equivalent + return false; + } + + // One of the strings is an expression and the other a topic name + const string &expr = lhsIsExpression ? lhs : rhs; + const string &topic = lhsIsExpression ? rhs : lhs; + + vector exprTokens, topicTokens; + split(expr, '/', back_inserter(exprTokens)); + split(topic, '/', back_inserter(topicTokens)); + + if (topicTokens.size() < exprTokens.size()) { + // Topic contains fewer levels than the expression thus they cannot match + return false; + } + + for (unsigned int i = 0; i < exprTokens.size(); ++i) { + if (!exprTokens[i].compare("#") && topicTokens[i].size() != 0) { + return true; + } + else if (!exprTokens[i].compare("+") && topicTokens[i].size() != 0) { + continue; + } + else if (!exprTokens[i].compare(topicTokens[i])) { + continue; + } + else { + return false; + } + } + + return true; } -int MQTTTopicHandlers::motorRPMHandler(void* message, string &topic) { - LogPrint("Received %s on topic %s", message, topic.c_str()); - return 0; + +/*! + * \brief containsWildcards Checks if a string contains any MQTT wildcard symbols + * \param topic Topic string + * \return True if the string contained any wildcards, false otherwise + */ +bool containsWildcards(const string &topic) { + bool retval = false; + const string singleLevelWildCard = "/+"; + const string multiLevelWildCard = "/#"; + // Check if topic contains any single level wildcards + retval = retval || topic.find(singleLevelWildCard + "/") != string::npos; + // Check if topic ends with a wildcard + retval = retval || !topic.compare(topic.length() - singleLevelWildCard.length(), + singleLevelWildCard.length(), singleLevelWildCard); + retval = retval || !topic.compare(topic.length() - multiLevelWildCard.length(), + multiLevelWildCard.length(), multiLevelWildCard); + return retval; } From 73a09f994716ad7a110d9b2c14f75bf12f8477e5 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 8 Apr 2020 15:32:34 +0200 Subject: [PATCH 446/523] Ran code formatter --- core/src/objectcontrol.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/core/src/objectcontrol.c b/core/src/objectcontrol.c index fb25d1833..29a399759 100644 --- a/core/src/objectcontrol.c +++ b/core/src/objectcontrol.c @@ -188,10 +188,10 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { C8 OriginLatitude[SMALL_BUFFER_SIZE_0], OriginLongitude[SMALL_BUFFER_SIZE_0], OriginAltitude[SMALL_BUFFER_SIZE_0], OriginHeading[SMALL_BUFFER_SIZE_0]; C8 TextBuffer[SMALL_BUFFER_SIZE_0]; - dbl OriginLatitudeDbl = DEFAULT_ORIGIN_LAT; - dbl OriginLongitudeDbl = DEFAULT_ORIGIN_LOG; - dbl OriginAltitudeDbl = DEFAULT_ORIGIN_ALT; - dbl OriginHeadingDbl = DEFAULT_ORIGIN_HEADING; + dbl OriginLatitudeDbl = DEFAULT_ORIGIN_LAT; + dbl OriginLongitudeDbl = DEFAULT_ORIGIN_LOG; + dbl OriginAltitudeDbl = DEFAULT_ORIGIN_ALT; + dbl OriginHeadingDbl = DEFAULT_ORIGIN_HEADING; C8 pcSendBuffer[MBUS_MAX_DATALEN]; C8 ObjectPort[SMALL_BUFFER_SIZE_0]; MonitorDataType monitorData; From a4d7e9dc728ed25966b22485bee3240729948c9a Mon Sep 17 00:00:00 2001 From: LukasWikander Date: Thu, 9 Apr 2020 13:48:13 +0200 Subject: [PATCH 447/523] Commented out ObjectMonitoring in CMakeLists.txt --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 74df14435..aa5f44006 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -23,7 +23,7 @@ add_subdirectory(core) add_subdirectory(modules/ScenarioControl) add_subdirectory(modules/Supervision) add_subdirectory(modules/Visualization) -add_subdirectory(modules/ObjectMonitoring) +#add_subdirectory(modules/ObjectMonitoring) # Ensure .maestro directory is created at build time in build directory add_custom_target(configurationDirectory ALL From 2fbc9914e1249f2dcbceb3c6725f77850f8d4730 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 9 Apr 2020 13:52:15 +0200 Subject: [PATCH 448/523] Enabled ObjectMonitoring in CMakeLists.txt --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index aa5f44006..74df14435 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -23,7 +23,7 @@ add_subdirectory(core) add_subdirectory(modules/ScenarioControl) add_subdirectory(modules/Supervision) add_subdirectory(modules/Visualization) -#add_subdirectory(modules/ObjectMonitoring) +add_subdirectory(modules/ObjectMonitoring) # Ensure .maestro directory is created at build time in build directory add_custom_target(configurationDirectory ALL From 8e21ef8704d47672115dabdcc7407a4ee981337a Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 14 Apr 2020 15:08:02 +0200 Subject: [PATCH 449/523] Fixes after testing at office --- modules/ObjectMonitoring/inc/mqttconnectionhandler.hpp | 6 +++++- modules/ObjectMonitoring/src/mqtttopichandlers.cpp | 4 ++-- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/modules/ObjectMonitoring/inc/mqttconnectionhandler.hpp b/modules/ObjectMonitoring/inc/mqttconnectionhandler.hpp index 69d828be5..bf76a0168 100644 --- a/modules/ObjectMonitoring/inc/mqttconnectionhandler.hpp +++ b/modules/ObjectMonitoring/inc/mqttconnectionhandler.hpp @@ -55,8 +55,12 @@ class MQTTConnectionHandler { static int arrivalCallback(void* context, char* topicName, int, MQTTClient_message* message) { MQTTConnectionHandler* thisObject = static_cast(context); + string topic(topicName); - return thisObject->messageCallback(message->payload, topic); + int retval = thisObject->messageCallback(message->payload, topic); + MQTTClient_freeMessage(&message); + MQTTClient_free(topicName); + return retval; } }; diff --git a/modules/ObjectMonitoring/src/mqtttopichandlers.cpp b/modules/ObjectMonitoring/src/mqtttopichandlers.cpp index d9eb8ae3a..7c5852f93 100644 --- a/modules/ObjectMonitoring/src/mqtttopichandlers.cpp +++ b/modules/ObjectMonitoring/src/mqtttopichandlers.cpp @@ -23,7 +23,7 @@ static bool topicsAreEquivalent(const string &lhs, const string &rhs); * \brief handlerMap maps MQTT topics to their respective handlers */ static HandlerMap handlerMap { - {"astazero/+/+/motor-rpm/#", MQTTTopicHandlers::motorRPMHandler}, + {"astazero/+/+/RPM/#", MQTTTopicHandlers::motorRPMHandler}, {"astazero/+/+/MONR/#", MQTTTopicHandlers::monrHandler} }; @@ -40,7 +40,7 @@ int MQTTTopicHandlers::monrHandler(void*, string& topic) { int MQTTTopicHandlers::motorRPMHandler(void* message, string &topic) { LogPrint("Received %s on topic %s", message, topic.c_str()); - return 0; + return 1; } /*! ************************************************************************ From 519b1776dc80177948bf0a20c8809b4bdd388a14 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 24 Apr 2020 15:56:26 +0200 Subject: [PATCH 450/523] Re-referenced util to master --- util | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/util b/util index 74fa247ed..e73444774 160000 --- a/util +++ b/util @@ -1 +1 @@ -Subproject commit 74fa247ed95525a4f14e11462d4f63f49cbc3e5c +Subproject commit e7344477470c3ac90449e90c0bde86e1261372ad From cba775b306ca8033a23ede7328110357f21942d6 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 28 Apr 2020 10:07:25 +0200 Subject: [PATCH 451/523] Added an abstract position trigger class --- modules/ScenarioControl/inc/positiontrigger.h | 47 +++++++++++++++++++ .../ScenarioControl/src/positiontrigger.cpp | 18 +++++++ 2 files changed, 65 insertions(+) create mode 100644 modules/ScenarioControl/inc/positiontrigger.h create mode 100644 modules/ScenarioControl/src/positiontrigger.cpp diff --git a/modules/ScenarioControl/inc/positiontrigger.h b/modules/ScenarioControl/inc/positiontrigger.h new file mode 100644 index 000000000..6d3cdb4bf --- /dev/null +++ b/modules/ScenarioControl/inc/positiontrigger.h @@ -0,0 +1,47 @@ +#ifndef POSITIONTRIGGER_H +#define POSITIONTRIGGER_H + +#include "trigger.h" + +#include +#include + +class PositionTrigger : public Trigger +{ +public: + using Trigger::Trigger; + + virtual TriggerReturnCode_t parseParameters() override = 0; + + TriggerReturnCode_t update(bool, struct timeval measurementTime) override; + +protected: + TriggerReturnCode_t checkIfTriggered(void) const override; + + bool isStateTrue = false; + +private: + virtual const std::set getAcceptedParameters() const override + { + std::set accParams; + accParams.insert(TRIGGER_PARAMETER_X); + accParams.insert(TRIGGER_PARAMETER_Y); + accParams.insert(TRIGGER_PARAMETER_Z); + accParams.insert(TRIGGER_PARAMETER_EQUAL_TO); + accParams.insert(TRIGGER_PARAMETER_GREATER_THAN); + accParams.insert(TRIGGER_PARAMETER_GREATER_THAN_OR_EQUAL_TO); + accParams.insert(TRIGGER_PARAMETER_LESS_THAN); + accParams.insert(TRIGGER_PARAMETER_LESS_THAN_OR_EQUAL_TO); + accParams.insert(TRIGGER_PARAMETER_NOT_EQUAL_TO); + accParams.insert(TRIGGER_PARAMETER_RELATIVE); + accParams.insert(TRIGGER_PARAMETER_ABSOLUTE); + accParams.insert(TRIGGER_PARAMETER_MIN); + accParams.insert(TRIGGER_PARAMETER_MAX); + accParams.insert(TRIGGER_PARAMETER_MEAN); + return accParams; + } + + +}; + +#endif // POSITIONTRIGGER_H diff --git a/modules/ScenarioControl/src/positiontrigger.cpp b/modules/ScenarioControl/src/positiontrigger.cpp new file mode 100644 index 000000000..9084fdf28 --- /dev/null +++ b/modules/ScenarioControl/src/positiontrigger.cpp @@ -0,0 +1,18 @@ +#include "positiontrigger.h" +/*! + * \brief PositionTrigger::update Updates the tracked signal (i.e. which causes the trigger) to the value specified + * \param + * \param Unused time of measurement + * \return Value according to ::TriggerReturnCode_t + */ +Trigger::TriggerReturnCode_t PositionTrigger::update(bool CHANGE_THIS, struct timeval) { + // TODO +} + +/*! + * \brief PositionTrigger::checkIfTriggered TODO + * \return Value according to ::TriggerReturnCode_t + */ +Trigger::TriggerReturnCode_t PositionTrigger::checkIfTriggered() const { + // TODO +} From 4dbcfff8d64fc269cd0b5e6080abdce8a74e238c Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 28 Apr 2020 10:07:42 +0200 Subject: [PATCH 452/523] Added position trigger to CMakeLists --- modules/ScenarioControl/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/modules/ScenarioControl/CMakeLists.txt b/modules/ScenarioControl/CMakeLists.txt index 51b9cf51a..a4cb4c784 100644 --- a/modules/ScenarioControl/CMakeLists.txt +++ b/modules/ScenarioControl/CMakeLists.txt @@ -23,6 +23,7 @@ add_executable(${SCENARIO_CONTROL_TARGET} ${CMAKE_CURRENT_SOURCE_DIR}/src/action.cpp ${CMAKE_CURRENT_SOURCE_DIR}/src/booleantrigger.cpp ${CMAKE_CURRENT_SOURCE_DIR}/src/braketrigger.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/positiontrigger.cpp ${CMAKE_CURRENT_SOURCE_DIR}/src/causality.cpp ${CMAKE_CURRENT_SOURCE_DIR}/src/externalaction.cpp ${CMAKE_CURRENT_SOURCE_DIR}/src/isotrigger.cpp From faba3689207533cc7176e1c718d3349ffed2482f Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 29 Apr 2020 10:40:35 +0200 Subject: [PATCH 453/523] Removed position trigger --- modules/ScenarioControl/CMakeLists.txt | 2 +- modules/ScenarioControl/inc/positiontrigger.h | 47 ------------------- .../ScenarioControl/src/positiontrigger.cpp | 18 ------- 3 files changed, 1 insertion(+), 66 deletions(-) delete mode 100644 modules/ScenarioControl/inc/positiontrigger.h delete mode 100644 modules/ScenarioControl/src/positiontrigger.cpp diff --git a/modules/ScenarioControl/CMakeLists.txt b/modules/ScenarioControl/CMakeLists.txt index a4cb4c784..8a9290390 100644 --- a/modules/ScenarioControl/CMakeLists.txt +++ b/modules/ScenarioControl/CMakeLists.txt @@ -23,7 +23,7 @@ add_executable(${SCENARIO_CONTROL_TARGET} ${CMAKE_CURRENT_SOURCE_DIR}/src/action.cpp ${CMAKE_CURRENT_SOURCE_DIR}/src/booleantrigger.cpp ${CMAKE_CURRENT_SOURCE_DIR}/src/braketrigger.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/src/positiontrigger.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/distancetrigger.cpp ${CMAKE_CURRENT_SOURCE_DIR}/src/causality.cpp ${CMAKE_CURRENT_SOURCE_DIR}/src/externalaction.cpp ${CMAKE_CURRENT_SOURCE_DIR}/src/isotrigger.cpp diff --git a/modules/ScenarioControl/inc/positiontrigger.h b/modules/ScenarioControl/inc/positiontrigger.h deleted file mode 100644 index 6d3cdb4bf..000000000 --- a/modules/ScenarioControl/inc/positiontrigger.h +++ /dev/null @@ -1,47 +0,0 @@ -#ifndef POSITIONTRIGGER_H -#define POSITIONTRIGGER_H - -#include "trigger.h" - -#include -#include - -class PositionTrigger : public Trigger -{ -public: - using Trigger::Trigger; - - virtual TriggerReturnCode_t parseParameters() override = 0; - - TriggerReturnCode_t update(bool, struct timeval measurementTime) override; - -protected: - TriggerReturnCode_t checkIfTriggered(void) const override; - - bool isStateTrue = false; - -private: - virtual const std::set getAcceptedParameters() const override - { - std::set accParams; - accParams.insert(TRIGGER_PARAMETER_X); - accParams.insert(TRIGGER_PARAMETER_Y); - accParams.insert(TRIGGER_PARAMETER_Z); - accParams.insert(TRIGGER_PARAMETER_EQUAL_TO); - accParams.insert(TRIGGER_PARAMETER_GREATER_THAN); - accParams.insert(TRIGGER_PARAMETER_GREATER_THAN_OR_EQUAL_TO); - accParams.insert(TRIGGER_PARAMETER_LESS_THAN); - accParams.insert(TRIGGER_PARAMETER_LESS_THAN_OR_EQUAL_TO); - accParams.insert(TRIGGER_PARAMETER_NOT_EQUAL_TO); - accParams.insert(TRIGGER_PARAMETER_RELATIVE); - accParams.insert(TRIGGER_PARAMETER_ABSOLUTE); - accParams.insert(TRIGGER_PARAMETER_MIN); - accParams.insert(TRIGGER_PARAMETER_MAX); - accParams.insert(TRIGGER_PARAMETER_MEAN); - return accParams; - } - - -}; - -#endif // POSITIONTRIGGER_H diff --git a/modules/ScenarioControl/src/positiontrigger.cpp b/modules/ScenarioControl/src/positiontrigger.cpp deleted file mode 100644 index 9084fdf28..000000000 --- a/modules/ScenarioControl/src/positiontrigger.cpp +++ /dev/null @@ -1,18 +0,0 @@ -#include "positiontrigger.h" -/*! - * \brief PositionTrigger::update Updates the tracked signal (i.e. which causes the trigger) to the value specified - * \param - * \param Unused time of measurement - * \return Value according to ::TriggerReturnCode_t - */ -Trigger::TriggerReturnCode_t PositionTrigger::update(bool CHANGE_THIS, struct timeval) { - // TODO -} - -/*! - * \brief PositionTrigger::checkIfTriggered TODO - * \return Value according to ::TriggerReturnCode_t - */ -Trigger::TriggerReturnCode_t PositionTrigger::checkIfTriggered() const { - // TODO -} From 8aa14a7bd09e619ad7921f0e2e5faf82177d7839 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 29 Apr 2020 10:41:22 +0200 Subject: [PATCH 454/523] Added support for distance trigger in scenario parsing code --- modules/ScenarioControl/inc/scenario.h | 1 - modules/ScenarioControl/src/scenario.cpp | 16 +++++++++++++++- 2 files changed, 15 insertions(+), 2 deletions(-) diff --git a/modules/ScenarioControl/inc/scenario.h b/modules/ScenarioControl/inc/scenario.h index 21b33cce3..6460de258 100644 --- a/modules/ScenarioControl/inc/scenario.h +++ b/modules/ScenarioControl/inc/scenario.h @@ -5,7 +5,6 @@ #include #include "trigger.h" -#include "braketrigger.h" #include "action.h" #include "causality.h" #include "logging.h" diff --git a/modules/ScenarioControl/src/scenario.cpp b/modules/ScenarioControl/src/scenario.cpp index cad299a6d..fcf4f8fa7 100644 --- a/modules/ScenarioControl/src/scenario.cpp +++ b/modules/ScenarioControl/src/scenario.cpp @@ -10,6 +10,8 @@ #include "isotrigger.h" #include "externalaction.h" +#include "braketrigger.h" +#include "distancetrigger.h" Scenario::Scenario(const std::string scenarioFilePath) { @@ -125,7 +127,7 @@ void Scenario::parseScenarioFileLine(const std::string &inputLine) // Match relevant field according to below patterns regex ipAddrPattern("([0-2]?[0-9]?[0-9]\\.){3}([0-2]?[0-9]?[0-9])"); // Match 3 "<000-299>." followed by "<000-299>" - regex triggerActionPattern("(([a-zA-Z_])+\\[([a-zA-Z0-9\\.,<=>_])+\\])+"); + regex triggerActionPattern("(([a-zA-Z_])+\\[([a-zA-Z0-9\\.,<=>_:()])+\\])+"); in_addr triggerIP, actionIP; string errMsg; set actions; @@ -251,6 +253,9 @@ std::set Scenario::parseTriggerConfiguration(const std::string &inputC trigger = new BrakeTrigger(baseTriggerID + static_cast(returnTriggers.size())); // TODO: possibly the OR between the Maestro trigger and possible TREO messages break; + case TRIGGER_DISTANCE: + trigger = new DistanceTrigger(baseTriggerID + static_cast(returnTriggers.size())); + break; default: // Trigger with unimplemented Maestro monitoring: let object handle trigger reporting trigger = new ISOTrigger(baseTriggerID + static_cast(returnTriggers.size()), triggerType); @@ -443,6 +448,15 @@ Scenario::ScenarioReturnCode_t Scenario::updateTrigger(const MonitorDataType &mo tp->getTypeAsString(tp->getTypeCode()).c_str()); } break; + case Trigger::TriggerTypeCode_t::TRIGGER_DISTANCE: + if (monr.data.position.isPositionValid) { + tp->update(monr.data.position, monr.data.timestamp); + } + else { + LogMessage(LOG_LEVEL_WARNING, "Could not update trigger type %s due to invalid monitor data values", + tp->getTypeAsString(tp->getTypeCode()).c_str()); + } + break; default: LogMessage(LOG_LEVEL_WARNING, "Unhandled trigger type in update: %s", tp->getTypeAsString(tp->getTypeCode()).c_str()); From 50819f2badf64526ebf7255e5eea693d9ab1e715 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 29 Apr 2020 10:42:17 +0200 Subject: [PATCH 455/523] Added another type for trigger update abstract method --- modules/ScenarioControl/inc/trigger.h | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/modules/ScenarioControl/inc/trigger.h b/modules/ScenarioControl/inc/trigger.h index ba33a9e39..0402d92e7 100644 --- a/modules/ScenarioControl/inc/trigger.h +++ b/modules/ScenarioControl/inc/trigger.h @@ -92,7 +92,10 @@ class Trigger virtual TriggerReturnCode_t update(int, struct timeval) { throw std::invalid_argument("Invalid signal type"); } virtual TriggerReturnCode_t update(float, struct timeval) { throw std::invalid_argument("Invalid signal type"); } virtual TriggerReturnCode_t update(double, struct timeval) { throw std::invalid_argument("Invalid signal type"); } - virtual TriggerReturnCode_t update(TREOData){ throw std::invalid_argument("Invalid signal type"); } + virtual TriggerReturnCode_t update(CartesianPosition, struct timeval) { throw std::invalid_argument("Invalid signal type"); } + virtual TriggerReturnCode_t update(TREOData) { throw std::invalid_argument("Invalid signal type"); } + virtual TriggerReturnCode_t update(MonitorDataType newValue, struct timeval measurementTime) { throw std::invalid_argument("Invalid signal type"); }; + static TriggerTypeCode_t asTypeCode(std::string typeCodeString); protected: From abd9eaab21fde96bbed6e7419a0ec45428902bdf Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 29 Apr 2020 10:42:47 +0200 Subject: [PATCH 456/523] Implemented distance trigger class --- modules/ScenarioControl/inc/distancetrigger.h | 55 ++++++++ .../ScenarioControl/src/distancetrigger.cpp | 129 ++++++++++++++++++ 2 files changed, 184 insertions(+) create mode 100644 modules/ScenarioControl/inc/distancetrigger.h create mode 100644 modules/ScenarioControl/src/distancetrigger.cpp diff --git a/modules/ScenarioControl/inc/distancetrigger.h b/modules/ScenarioControl/inc/distancetrigger.h new file mode 100644 index 000000000..e017d9a34 --- /dev/null +++ b/modules/ScenarioControl/inc/distancetrigger.h @@ -0,0 +1,55 @@ +#ifndef DISTANCETRIGGER_H +#define DISTANCETRIGGER_H + +#include "booleantrigger.h" + +#include +#include +#include + +class DistanceTrigger : public BooleanTrigger +{ +public: + DistanceTrigger(TriggerID_t triggerID); + + TriggerReturnCode_t appendParameter(std::string inputStr) override; + TriggerReturnCode_t parseParameters() override; + + using BooleanTrigger::update; + TriggerReturnCode_t update(MonitorDataType newValue, struct timeval measurementTime) override; + + void setTriggerDistance(double distance_m) { this->triggerDistance_m = distance_m; } + void setReferencePoint(CartesianPosition point) { this->referencePoint = point; } + + double getTriggerDistance(void) const { return this->triggerDistance_m; } + CartesianPosition getReferencePoint(void) const { return this->referencePoint; } +private: + static constexpr CartesianPosition defaultReferencePoint = {0.0, 0.0, 0.0, 0.0, true, false}; + + double triggerDistance_m; + CartesianPosition referencePoint; + enum {LESS_THAN, GREATER_THAN} oper = LESS_THAN; + + const std::set getAcceptedParameters() const override + { + std::set accParams; + accParams.insert(TRIGGER_PARAMETER_X); + accParams.insert(TRIGGER_PARAMETER_Y); + accParams.insert(TRIGGER_PARAMETER_Z); + accParams.insert(TRIGGER_PARAMETER_EQUAL_TO); + accParams.insert(TRIGGER_PARAMETER_GREATER_THAN); + accParams.insert(TRIGGER_PARAMETER_GREATER_THAN_OR_EQUAL_TO); + accParams.insert(TRIGGER_PARAMETER_LESS_THAN); + accParams.insert(TRIGGER_PARAMETER_LESS_THAN_OR_EQUAL_TO); + accParams.insert(TRIGGER_PARAMETER_NOT_EQUAL_TO); + accParams.insert(TRIGGER_PARAMETER_RELATIVE); + accParams.insert(TRIGGER_PARAMETER_ABSOLUTE); + accParams.insert(TRIGGER_PARAMETER_MIN); + accParams.insert(TRIGGER_PARAMETER_MAX); + accParams.insert(TRIGGER_PARAMETER_MEAN); + return accParams; + } + + TriggerReturnCode_t parseNumericParameter(std::string inputStr); +}; +#endif // DISTANCETRIGGER_H diff --git a/modules/ScenarioControl/src/distancetrigger.cpp b/modules/ScenarioControl/src/distancetrigger.cpp new file mode 100644 index 000000000..989775920 --- /dev/null +++ b/modules/ScenarioControl/src/distancetrigger.cpp @@ -0,0 +1,129 @@ +#include +#include +#include +#include "maestroTime.h" +#include "distancetrigger.h" +#include "util.h" + +constexpr CartesianPosition DistanceTrigger::defaultReferencePoint; + +DistanceTrigger::DistanceTrigger(Trigger::TriggerID_t triggerID) : BooleanTrigger(triggerID, Trigger::TriggerTypeCode_t::TRIGGER_DISTANCE) { + this->setReferencePoint(defaultReferencePoint); + this->setTriggerDistance(0.0); +} + +Trigger::TriggerReturnCode_t DistanceTrigger::update(MonitorDataType newValue, struct timeval measurementTime) { + double networkDelayCorrection_m = 0.0; + struct timeval currentTime, networkDelay; + + if (!newValue.data.position.isPositionValid || !referencePoint.isPositionValid) { + throw std::logic_error("Unable to update distance trigger on invalid data"); + } + + // Correct for two-way network delay effects on trigger distance + if (newValue.data.speed.isLongitudinalValid && newValue.data.isTimestampValid) { + TimeSetToCurrentSystemTime(¤tTime); + timersub(¤tTime, &measurementTime, &networkDelay); + networkDelayCorrection_m = 2.0 * fabs(static_cast(networkDelay.tv_sec) + static_cast(networkDelay.tv_usec) / 1000000.0) + * newValue.data.speed.longitudinal_m_s; + } + else { + LogMessage(LOG_LEVEL_WARNING, "Invalid monitor data speed or timestamp: cannot correct for network delay"); + } + + switch (this->oper) { + case LESS_THAN: + return update(static_cast(UtilIsPositionNearTarget(newValue.data.position, this->referencePoint, this->triggerDistance_m + networkDelayCorrection_m)), + measurementTime); + case GREATER_THAN: + return update(static_cast(!UtilIsPositionNearTarget(newValue.data.position, this->referencePoint, this->triggerDistance_m - networkDelayCorrection_m)), + measurementTime); + } + throw std::logic_error("Distance trigger unimplemented operator"); +} + +Trigger::TriggerReturnCode_t DistanceTrigger::parseParameters() { + Trigger::TriggerReturnCode_t retval = NOT_OK; + if (parameters.size() == 1) { + switch (parameters.front()) { + case TRIGGER_PARAMETER_LESS_THAN: + case TRIGGER_PARAMETER_LESS_THAN_OR_EQUAL_TO: + // TODO check value to compare against + this->oper = LESS_THAN; + this->isStateTrue = false; + this->wasStateTrue = false; + retval = OK; + break; + case TRIGGER_PARAMETER_GREATER_THAN: + case TRIGGER_PARAMETER_GREATER_THAN_OR_EQUAL_TO: + // TODO check value to compare against + this->oper = GREATER_THAN; + this->isStateTrue = false; + this->wasStateTrue = false; + retval = OK; + break; + default: + return INVALID_ARGUMENT; + } + } + else return INVALID_ARGUMENT; + + LogMessage(LOG_LEVEL_INFO, "Distance trigger configured with reference point (%.3f, %.3f, %.3f) and %.3f m trigger distance", + this->referencePoint.xCoord_m, this->referencePoint.yCoord_m, this->referencePoint.zCoord_m, this->triggerDistance_m); + return retval; +} + + +Trigger::TriggerReturnCode_t DistanceTrigger::appendParameter(std::string inputStr) { + try { + // String represented a trigger parameter defined by ISO + TriggerParameter_t param = asParameterCode(inputStr); + return Trigger::appendParameter(param); + } catch (std::invalid_argument e) { + // String may have represented a number + return parseNumericParameter(inputStr); + } +} + +Trigger::TriggerReturnCode_t DistanceTrigger::parseNumericParameter(std::string inputStr) { + std::istringstream ss(inputStr); + const std::string xString = "x:"; + const std::string yString = "y:"; + const std::string zString = "z:"; + double param = 0.0; + size_t stringPos; + if (inputStr.find("TO:(") != std::string::npos && inputStr.find(")") != std::string::npos) { + if ((stringPos = inputStr.find(xString)) != std::string::npos) { + ss.str(inputStr.substr(stringPos+xString.length())); + if (!(ss >> this->referencePoint.xCoord_m)) { + throw std::invalid_argument("Distance trigger unable to parse " + inputStr + " as reference point"); + } + } + else + throw std::invalid_argument("Distance trigger unable to parse " + inputStr + " as reference point"); + if ((stringPos = inputStr.find(yString)) != std::string::npos) { + ss.str(inputStr.substr(stringPos+yString.length())); + if (!(ss >> this->referencePoint.yCoord_m)) { + throw std::invalid_argument("Distance trigger unable to parse " + inputStr + " as reference point"); + } + } + else + throw std::invalid_argument("Distance trigger unable to parse " + inputStr + " as reference point"); + if ((stringPos = inputStr.find(zString)) != std::string::npos) { + ss.str(inputStr.substr(stringPos+zString.length())); + if (!(ss >> this->referencePoint.zCoord_m)) { + throw std::invalid_argument("Distance trigger unable to parse " + inputStr + " as reference point"); + } + } + else + throw std::invalid_argument("Distance trigger unable to parse " + inputStr + " as reference point"); + this->referencePoint.isPositionValid = true; + } + else if (ss >> param) { + this->triggerDistance_m = param; + } + else { + throw std::invalid_argument("Distance trigger unable to parse " + inputStr + " as numeric parameter"); + } + return OK; +} From a0fa735de3fc3be665866bb647929fe88804027d Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 29 Apr 2020 10:45:19 +0200 Subject: [PATCH 457/523] Updated default triggeraction.conf to be valid --- conf/triggeraction.conf | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/conf/triggeraction.conf b/conf/triggeraction.conf index cdad519e2..1f402b496 100644 --- a/conf/triggeraction.conf +++ b/conf/triggeraction.conf @@ -1,2 +1,2 @@ -#trigger_ip;trigger_index;trigger_type[parameter];action_type[parameter];action[parameter]; -10.168.15.58;3;DI[RISING_EDGE];SERVER[127.0.0.1];SEND_START[2000]; +#trigger_ip;trigger_type[parameter];action_ip;action_type[parameter]; +127.0.0.1;DISTANCE[TO:(x:5.3y:2.5z:2.1),LESS_THAN,5.0];0.0.0.0;TEST_SCENARIO_COMMAND[START,0]; From dfc988cc55e15479ee49598ca51426dd31d79616 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 29 Apr 2020 12:37:47 +0200 Subject: [PATCH 458/523] Moved and renamed defines in util such as 'a', 'f' and so on --- core/inc/util.h | 5 ----- core/src/util.c | 29 ++++++++++++++++------------- 2 files changed, 16 insertions(+), 18 deletions(-) diff --git a/core/inc/util.h b/core/inc/util.h index 177189cb0..572c1c4aa 100644 --- a/core/inc/util.h +++ b/core/inc/util.h @@ -74,11 +74,6 @@ extern "C"{ #define MAX_UTIL_VARIBLE_SIZE 512 -// TODO: Make these constants have more descriptive names -#define a 6378137.0 //meters in WGS84 -#define k 298.257223563 //in WGS84, f = 1/298.257223563 -#define b 6356752.3142451794975639665996337 //b = (1-f)*a -#define l 1e-12 #define PI 3.141592653589793 #define ORIGO_DISTANCE_CALC_ITERATIONS 14 #define TRAJECTORY_LINE_LENGTH 100 diff --git a/core/src/util.c b/core/src/util.c index 7c9a24bac..612bcdc3b 100644 --- a/core/src/util.c +++ b/core/src/util.c @@ -36,8 +36,11 @@ -- Defines ------------------------------------------------------------*/ -#define FE_WGS84 (1.0/298.257223563) // earth flattening (WGS84) -#define RE_WGS84 6378137.0 // earth semimajor axis (WGS84) (m) +#define EARTH_EQUATOR_RADIUS_M 6378137.0 // earth semimajor axis (WGS84) (m) +#define INVERSE_FLATTENING 298.257223563 //in WGS84, f = 1/298.257223563 +#define EARTH_FLATTENING ( 1.0 / INVERSE_FLATTENING ) +#define EARTH_POLE_RADIUS_M 6356752.3142451794975639665996337 //b = (1-f)*a +#define VINCENTY_MIN_STEP_TOLERANCE 1e-12 #define SMALL_BUFFER_SIZE_128 128 #define SMALL_BUFFER_SIZE_64 64 @@ -409,17 +412,17 @@ void util_error(const char *message) { } void xyzToLlh(double x, double y, double z, double *lat, double *lon, double *height) { - double e2 = FE_WGS84 * (2.0 - FE_WGS84); + double e2 = EARTH_FLATTENING * (2.0 - EARTH_FLATTENING); double r2 = x * x + y * y; double za = z; double zk = 0.0; double sinp = 0.0; - double v = RE_WGS84; + double v = EARTH_EQUATOR_RADIUS_M; while (fabs(za - zk) >= 1E-4) { zk = za; sinp = za / sqrt(r2 + za * za); - v = RE_WGS84 / sqrt(1.0 - e2 * sinp * sinp); + v = EARTH_EQUATOR_RADIUS_M / sqrt(1.0 - e2 * sinp * sinp); za = z + v * e2 * sinp; } @@ -433,8 +436,8 @@ void llhToXyz(double lat, double lon, double height, double *x, double *y, doubl double cosp = cos(lat * M_PI / 180.0); double sinl = sin(lon * M_PI / 180.0); double cosl = cos(lon * M_PI / 180.0); - double e2 = FE_WGS84 * (2.0 - FE_WGS84); - double v = RE_WGS84 / sqrt(1.0 - e2 * sinp * sinp); + double e2 = EARTH_FLATTENING * (2.0 - EARTH_FLATTENING); + double v = EARTH_EQUATOR_RADIUS_M / sqrt(1.0 - e2 * sinp * sinp); *x = (v + height) * cosp * cosl; *y = (v + height) * cosp * sinl; @@ -846,7 +849,7 @@ double UtilCalcPositionDelta(double P1Lat, double P1Long, double P2Lat, double P P2LatRad = UtilDegToRad(P2Lat); P2LongRad = UtilDegToRad(P2Long); - f = 1 / k; + f = 1 / INVERSE_FLATTENING; U1 = atan((1 - f) * tan(P1LatRad)); U2 = atan((1 - f) * tan(P2LatRad)); L = P2LongRad - P1LongRad; @@ -921,7 +924,7 @@ int UtilVincentyDirect(double refLat, double refLon, double a1, double distance, // Variables only calculated once - double U1, f = 1 / k, sigma1, sina, pow2cosa, pow2u, A, B, C, L, lambda; + double U1, f = 1 / INVERSE_FLATTENING, sigma1, sina, pow2cosa, pow2u, A, B, C, L, lambda; // Iterative variables double sigma, deltaSigma, sigma2m; @@ -935,14 +938,14 @@ int UtilVincentyDirect(double refLat, double refLon, double a1, double distance, sina = cos(U1) * sin(a1); pow2cosa = 1 - pow(sina, 2); - pow2u = pow2cosa * (pow(a, 2) - pow(b, 2)) / pow(b, 2); + pow2u = pow2cosa * (pow(EARTH_EQUATOR_RADIUS_M, 2) - pow(EARTH_POLE_RADIUS_M, 2)) / pow(EARTH_POLE_RADIUS_M, 2); A = 1 + pow2u / 16384.0 * (4096.0 + pow2u * (-768.0 + pow2u * (320.0 - 175.0 * pow2u))); B = pow2u / 1024.0 * (256.0 + pow2u * (-128.0 + pow2u * (74.0 - 47.0 * pow2u))); int iterations = 0; - double init_sigma = distance / (b * A); + double init_sigma = distance / (EARTH_POLE_RADIUS_M * A); sigma = init_sigma; do { @@ -960,7 +963,7 @@ int UtilVincentyDirect(double refLat, double refLon, double a1, double distance, ) ); sigma = init_sigma + deltaSigma; - } while (fabs(sigma - prev_sigma) > l); + } while (fabs(sigma - prev_sigma) > VINCENTY_MIN_STEP_TOLERANCE); *resLat = UtilRadToDeg(atan2(sin(U1) * cos(sigma) + cos(U1) * sin(sigma) * cos(a1), @@ -2840,7 +2843,7 @@ void traj2ldm(float time, double x, double y, double z, float hdg, float vel, mo char pcTempBuffer[512]; - double earth_radius = a; + double earth_radius = EARTH_EQUATOR_RADIUS_M; double lat_origin = 0.0; double lon_origin = 0.0; From c9d5f4bdf2f73914ff5b345678adc3676de97567 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 29 Apr 2020 12:39:11 +0200 Subject: [PATCH 459/523] Added vendor specific action parameter SEND_START --- core/inc/iso22133.h | 1 + 1 file changed, 1 insertion(+) diff --git a/core/inc/iso22133.h b/core/inc/iso22133.h index 93b409e22..9df2d33bd 100644 --- a/core/inc/iso22133.h +++ b/core/inc/iso22133.h @@ -138,6 +138,7 @@ typedef enum { ACTION_PARAMETER_Y = 0x00000071, ACTION_PARAMETER_Z = 0x00000072, ACTION_PARAMETER_VS_BRAKE_WARNING = 0xA0000000, + ACTION_PARAMETER_VS_SEND_START = 0xA0000100, ACTION_PARAMETER_UNAVAILABLE = 0xFFFFFFFF } ActionTypeParameter_t; From 4575469f562c13ee5860dae3058b6a10362cc7f6 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 29 Apr 2020 12:40:01 +0200 Subject: [PATCH 460/523] tabs --- modules/ScenarioControl/inc/trigger.h | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/modules/ScenarioControl/inc/trigger.h b/modules/ScenarioControl/inc/trigger.h index 0402d92e7..5392b1c0b 100644 --- a/modules/ScenarioControl/inc/trigger.h +++ b/modules/ScenarioControl/inc/trigger.h @@ -87,11 +87,11 @@ class Trigger */ virtual TriggerReturnCode_t update(void) { throw std::invalid_argument("Invalid signal type"); } virtual TriggerReturnCode_t update(struct timeval) { throw std::invalid_argument("Invalid signal type"); } - virtual TriggerReturnCode_t update(bool, struct timeval) { throw std::invalid_argument("Invalid signal type"); } - virtual TriggerReturnCode_t update(char, struct timeval) { throw std::invalid_argument("Invalid signal type"); } - virtual TriggerReturnCode_t update(int, struct timeval) { throw std::invalid_argument("Invalid signal type"); } - virtual TriggerReturnCode_t update(float, struct timeval) { throw std::invalid_argument("Invalid signal type"); } - virtual TriggerReturnCode_t update(double, struct timeval) { throw std::invalid_argument("Invalid signal type"); } + virtual TriggerReturnCode_t update(bool, struct timeval) { throw std::invalid_argument("Invalid signal type"); } + virtual TriggerReturnCode_t update(char, struct timeval) { throw std::invalid_argument("Invalid signal type"); } + virtual TriggerReturnCode_t update(int, struct timeval) { throw std::invalid_argument("Invalid signal type"); } + virtual TriggerReturnCode_t update(float, struct timeval) { throw std::invalid_argument("Invalid signal type"); } + virtual TriggerReturnCode_t update(double, struct timeval) { throw std::invalid_argument("Invalid signal type"); } virtual TriggerReturnCode_t update(CartesianPosition, struct timeval) { throw std::invalid_argument("Invalid signal type"); } virtual TriggerReturnCode_t update(TREOData) { throw std::invalid_argument("Invalid signal type"); } virtual TriggerReturnCode_t update(MonitorDataType newValue, struct timeval measurementTime) { throw std::invalid_argument("Invalid signal type"); }; From efe589366af2a74f9b4bfb43ef70b01493c01c6c Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 29 Apr 2020 12:40:42 +0200 Subject: [PATCH 461/523] Refactored printout --- modules/ScenarioControl/src/trigger.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/ScenarioControl/src/trigger.cpp b/modules/ScenarioControl/src/trigger.cpp index ad81fa852..01edfaa2f 100644 --- a/modules/ScenarioControl/src/trigger.cpp +++ b/modules/ScenarioControl/src/trigger.cpp @@ -316,7 +316,7 @@ Trigger::TriggerParameter_t Trigger::asParameterCode(const std::string &inputStr if(!str.compare("UNAVAILABLE")) return TRIGGER_PARAMETER_UNAVAILABLE; - throw std::invalid_argument("Action parameter " + inputStr + " is not a valid parameter"); + throw std::invalid_argument("Trigger parameter " + inputStr + " is not a valid parameter"); } Trigger::TriggerReturnCode_t Trigger::appendParameter(std::string inputStr) From 59a9b8a54be4778b48c08e623bba0d10fb7a598c Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 29 Apr 2020 12:41:21 +0200 Subject: [PATCH 462/523] Added support for test scenario command in scenario parsing --- modules/ScenarioControl/src/scenario.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/modules/ScenarioControl/src/scenario.cpp b/modules/ScenarioControl/src/scenario.cpp index fcf4f8fa7..a586dabae 100644 --- a/modules/ScenarioControl/src/scenario.cpp +++ b/modules/ScenarioControl/src/scenario.cpp @@ -310,6 +310,9 @@ std::set Scenario::parseActionConfiguration(const std::string &inputCon case ACTION_INFRASTRUCTURE: action = new InfrastructureAction(baseActionID + static_cast(returnActions.size())); break; + case ACTION_TEST_SCENARIO_COMMAND: + action = new TestScenarioCommandAction(baseActionID + static_cast(returnActions.size())); + break; default: // Regular action (only ACCM and EXAC) action = new ExternalAction(baseActionID + static_cast(returnActions.size()), actionType); From 01a8277b1dbfa7f83f28cf832db99a99b82c3e2f Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 29 Apr 2020 16:26:31 +0200 Subject: [PATCH 463/523] Implemented a parse parameters function for actions --- modules/ScenarioControl/inc/action.h | 2 ++ modules/ScenarioControl/inc/externalaction.h | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/modules/ScenarioControl/inc/action.h b/modules/ScenarioControl/inc/action.h index 36c3b0417..79856efc9 100644 --- a/modules/ScenarioControl/inc/action.h +++ b/modules/ScenarioControl/inc/action.h @@ -63,6 +63,8 @@ class Action ActionReturnCode_t appendParameter(ActionParameter_t actionParameter); virtual ActionReturnCode_t appendParameter(std::string parameterString); + virtual ActionReturnCode_t parseParameters() = 0; + static ActionTypeCode_t asTypeCode(const std::string &typeCodeString); protected: diff --git a/modules/ScenarioControl/inc/externalaction.h b/modules/ScenarioControl/inc/externalaction.h index c813bd256..9c762b3b2 100644 --- a/modules/ScenarioControl/inc/externalaction.h +++ b/modules/ScenarioControl/inc/externalaction.h @@ -16,7 +16,7 @@ class InfrastructureAction : public ExternalAction { public: InfrastructureAction(ActionID_t actionID = 0, uint32_t allowedNumberOfRuns = 1); - + ActionReturnCode_t parseParameters(void) override { return parameters.size() == 1 ? OK : NOT_OK; } protected: ActionParameter_t asParameterCode(const std::string ¶meterCodeString) const; From 06ad3258d120528af769d37e760bf807c7596e1c Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 29 Apr 2020 16:27:55 +0200 Subject: [PATCH 464/523] Parse parameters only after all have been added to trigger / action --- modules/ScenarioControl/src/scenario.cpp | 31 ++++++++++++++++++++---- modules/ScenarioControl/src/trigger.cpp | 2 +- 2 files changed, 27 insertions(+), 6 deletions(-) diff --git a/modules/ScenarioControl/src/scenario.cpp b/modules/ScenarioControl/src/scenario.cpp index a586dabae..4611c7cf1 100644 --- a/modules/ScenarioControl/src/scenario.cpp +++ b/modules/ScenarioControl/src/scenario.cpp @@ -263,9 +263,20 @@ std::set Scenario::parseTriggerConfiguration(const std::string &inputC } // Transfer specified parameters to relevant containers - for (unsigned int i = 2; i < match.size(); ++i) - if (!match[i].str().empty()) trigger->appendParameter(match[i].str()); - returnTriggers.insert(trigger); + for (unsigned int i = 2; i < match.size(); ++i) { + if (!match[i].str().empty()){ + if (trigger->appendParameter(match[i].str()) != Trigger::OK) { + throw std::invalid_argument("Unable to interpret trigger parameter " + match[i].str()); + } + } + } + + // Parse all parameters into settings + if (trigger->parseParameters() != Trigger::OK) { + throw std::invalid_argument("Unable to interpret trigger configuration " + inputConfig); + } + + returnTriggers.insert(trigger); } return returnTriggers; @@ -320,8 +331,18 @@ std::set Scenario::parseActionConfiguration(const std::string &inputCon } // Transfer specified parameters to relevant containers - for (unsigned int i = 2; i < match.size(); ++i) - if(!match[i].str().empty()) action->appendParameter(match[i].str()); + for (unsigned int i = 2; i < match.size(); ++i) { + if(!match[i].str().empty()) { + if (action->appendParameter(match[i].str()) != Action::OK) { + throw std::invalid_argument("Unable to interpret action parameter " + match[i].str()); + } + } + } + // Parse all parameters into settings + if (action->parseParameters() != Action::OK) { + throw std::invalid_argument("Unable to interpret trigger configuration " + inputConfig); + } + returnActions.insert(action); } diff --git a/modules/ScenarioControl/src/trigger.cpp b/modules/ScenarioControl/src/trigger.cpp index 01edfaa2f..47ca03cfb 100644 --- a/modules/ScenarioControl/src/trigger.cpp +++ b/modules/ScenarioControl/src/trigger.cpp @@ -42,7 +42,7 @@ Trigger::TriggerReturnCode_t Trigger::appendParameter(Trigger::TriggerParameter_ return retval; parameters.push_back(triggerParameter); - return parseParameters(); + return OK; } /*! From 98682205c9f017dd650fde3676ace408d3537adb Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 29 Apr 2020 16:28:30 +0200 Subject: [PATCH 465/523] Implemeted test scenario command action with send_start as command parameter --- modules/ScenarioControl/inc/externalaction.h | 19 ++++++ .../ScenarioControl/src/externalaction.cpp | 62 ++++++++++++++++++- 2 files changed, 80 insertions(+), 1 deletion(-) diff --git a/modules/ScenarioControl/inc/externalaction.h b/modules/ScenarioControl/inc/externalaction.h index 9c762b3b2..09a2998d4 100644 --- a/modules/ScenarioControl/inc/externalaction.h +++ b/modules/ScenarioControl/inc/externalaction.h @@ -10,6 +10,25 @@ class ExternalAction : public Action using Action::Action; ActionReturnCode_t execute(void) override; + ActionReturnCode_t parseParameters(void) override { return OK; } +}; + +class TestScenarioCommandAction : public ExternalAction { +public: + TestScenarioCommandAction(ActionID_t actionID = 0, uint32_t allowedNumberOfRuns = 1); + + ActionReturnCode_t appendParameter(std::string) override; + ActionReturnCode_t parseParameters(void) override { return parameters.size() == 1 ? OK : NOT_OK; } +protected: + ActionParameter_t asParameterCode(const std::string ¶meterCodeString) const override; +private: + const std::set getAcceptedParameters(void) const override + { + std::set accParams; + accParams.insert(ACTION_PARAMETER_VS_SEND_START); + return accParams; + } + ActionReturnCode_t parseNumericParameter(std::string); }; class InfrastructureAction : public ExternalAction diff --git a/modules/ScenarioControl/src/externalaction.cpp b/modules/ScenarioControl/src/externalaction.cpp index ddd9c960f..9d224e08d 100644 --- a/modules/ScenarioControl/src/externalaction.cpp +++ b/modules/ScenarioControl/src/externalaction.cpp @@ -1,6 +1,9 @@ #include "externalaction.h" #include +#include +#include +#include #include "logging.h" #include "util.h" @@ -14,7 +17,7 @@ Action::ActionReturnCode_t ExternalAction::execute(void) if (remainingAllowedRuns == 0) return NO_REMAINING_RUNS; else { - TimeSetToCurrentSystemTime(&systemTime); // TODO: Set system time according to timecontrol + TimeSetToCurrentSystemTime(&systemTime); data.actionID = actionID; data.executionTime_qmsoW = actionDelayTime_qms == 0 ? 0 : TimeGetAsGPSqmsOfWeek(&systemTime) + actionDelayTime_qms; @@ -30,6 +33,63 @@ Action::ActionReturnCode_t ExternalAction::execute(void) } } + +// ******* Test scenario command action +TestScenarioCommandAction::TestScenarioCommandAction(ActionID_t actionID, uint32_t allowedNumberOfRuns) + : ExternalAction(actionID, Action::ActionTypeCode_t::ACTION_TEST_SCENARIO_COMMAND, allowedNumberOfRuns) +{ +} + +Action::ActionParameter_t TestScenarioCommandAction::asParameterCode(const std::string &inputStr) const +{ + try { + return Action::asParameterCode(inputStr); + } catch (std::invalid_argument e) { + std::string str = inputStr; + for (char &ch : str) + ch = toUpper(ch); + if (!str.compare("SEND_START")) + return ACTION_PARAMETER_VS_SEND_START; + throw e; + } +} + +Action::ActionReturnCode_t TestScenarioCommandAction::appendParameter(std::string inputStr) { + try { + // String represented an action parameter defined by ISO + ActionParameter_t param = asParameterCode(inputStr); + return Action::appendParameter(param); + } catch (std::invalid_argument e) { + // String may have represented a number + return parseNumericParameter(inputStr); + } +} + +Action::ActionReturnCode_t TestScenarioCommandAction::parseNumericParameter(std::string inputStr) { + std::istringstream ss(inputStr); + double param; + + if (ss >> param) { + if (std::find(this->parameters.begin(), this->parameters.end(), + ACTION_PARAMETER_VS_SEND_START) != this->parameters.end()) { + // Scenario command was start, interpret numeric parameter as delay + struct timeval delay; + delay.tv_sec = static_cast(param); + delay.tv_usec = static_cast((param - delay.tv_sec)*1000000); + this->setExecuteDelayTime(delay); + return OK; + } + else { + throw std::invalid_argument("Numeric parameter cannot be linked to test scenario command"); + } + } + else { + throw std::invalid_argument("Test scenario command action unable to parse " + inputStr + " as numeric parameter"); + } +} + + +// ******* Infrastructure action InfrastructureAction::InfrastructureAction(ActionID_t actionID, uint32_t allowedNumberOfRuns) : ExternalAction(actionID, Action::ActionTypeCode_t::ACTION_INFRASTRUCTURE, allowedNumberOfRuns) { From d548161f1be5262ac41606be40a8054ad0424633 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 29 Apr 2020 16:31:29 +0200 Subject: [PATCH 466/523] Set distance trigger mode to be high once configured --- modules/ScenarioControl/src/distancetrigger.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/modules/ScenarioControl/src/distancetrigger.cpp b/modules/ScenarioControl/src/distancetrigger.cpp index 989775920..468dd559b 100644 --- a/modules/ScenarioControl/src/distancetrigger.cpp +++ b/modules/ScenarioControl/src/distancetrigger.cpp @@ -33,9 +33,11 @@ Trigger::TriggerReturnCode_t DistanceTrigger::update(MonitorDataType newValue, s switch (this->oper) { case LESS_THAN: + this->mode = HIGH; return update(static_cast(UtilIsPositionNearTarget(newValue.data.position, this->referencePoint, this->triggerDistance_m + networkDelayCorrection_m)), measurementTime); case GREATER_THAN: + this->mode = HIGH; return update(static_cast(!UtilIsPositionNearTarget(newValue.data.position, this->referencePoint, this->triggerDistance_m - networkDelayCorrection_m)), measurementTime); } From 3fac51f9248da77ad4b95ed243c220ae13237578 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 29 Apr 2020 16:31:57 +0200 Subject: [PATCH 467/523] Whoops --- modules/ScenarioControl/src/distancetrigger.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/modules/ScenarioControl/src/distancetrigger.cpp b/modules/ScenarioControl/src/distancetrigger.cpp index 468dd559b..d782881d9 100644 --- a/modules/ScenarioControl/src/distancetrigger.cpp +++ b/modules/ScenarioControl/src/distancetrigger.cpp @@ -33,11 +33,9 @@ Trigger::TriggerReturnCode_t DistanceTrigger::update(MonitorDataType newValue, s switch (this->oper) { case LESS_THAN: - this->mode = HIGH; return update(static_cast(UtilIsPositionNearTarget(newValue.data.position, this->referencePoint, this->triggerDistance_m + networkDelayCorrection_m)), measurementTime); case GREATER_THAN: - this->mode = HIGH; return update(static_cast(!UtilIsPositionNearTarget(newValue.data.position, this->referencePoint, this->triggerDistance_m - networkDelayCorrection_m)), measurementTime); } @@ -52,6 +50,7 @@ Trigger::TriggerReturnCode_t DistanceTrigger::parseParameters() { case TRIGGER_PARAMETER_LESS_THAN_OR_EQUAL_TO: // TODO check value to compare against this->oper = LESS_THAN; + this->mode = HIGH; this->isStateTrue = false; this->wasStateTrue = false; retval = OK; @@ -60,6 +59,7 @@ Trigger::TriggerReturnCode_t DistanceTrigger::parseParameters() { case TRIGGER_PARAMETER_GREATER_THAN_OR_EQUAL_TO: // TODO check value to compare against this->oper = GREATER_THAN; + this->mode = HIGH; this->isStateTrue = false; this->wasStateTrue = false; retval = OK; From 4359af4e6632262576ef171871b82f3bb41ebc1c Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 29 Apr 2020 17:20:52 +0200 Subject: [PATCH 468/523] Added data length to mq recv function --- modules/ScenarioControl/src/main.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/modules/ScenarioControl/src/main.cpp b/modules/ScenarioControl/src/main.cpp index 840be4d45..f5788e7f5 100644 --- a/modules/ScenarioControl/src/main.cpp +++ b/modules/ScenarioControl/src/main.cpp @@ -15,7 +15,8 @@ int main() { COMMAND command = COMM_INV; - char mqRecvData[MQ_MSG_SIZE]; + char mqRecvData[MBUS_MAX_DATALEN]; + ssize_t recvDataLength = 0; const struct timespec sleepTimePeriod = {0,10000000}; struct timespec remTime; Scenario scenario; @@ -47,7 +48,7 @@ int main() scenario.resetISOTriggers(); } - if (iCommRecv(&command,mqRecvData,MQ_MSG_SIZE,nullptr) < 0) + if ((recvDataLength = iCommRecv(&command,mqRecvData,MQ_MSG_SIZE,nullptr)) < 0) { util_error("Message bus receive error"); } @@ -127,7 +128,7 @@ int main() break; case COMM_MONR: // Update triggers - UtilPopulateMonitorDataStruct(mqRecvData, sizeof(mqRecvData), &monr); + UtilPopulateMonitorDataStruct(mqRecvData, static_cast(recvDataLength), &monr); scenario.updateTrigger(monr); break; case COMM_DISCONNECT: From 35a55907c193e0a0d5002b6341416cef8314fd26 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 30 Apr 2020 09:36:01 +0200 Subject: [PATCH 469/523] Fixed a check in util --- core/src/util.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/core/src/util.c b/core/src/util.c index 612bcdc3b..3393091c6 100644 --- a/core/src/util.c +++ b/core/src/util.c @@ -799,7 +799,7 @@ int UtilStringToMonitorData(const char *monitorString, size_t stringLength, Moni uint8_t UtilIsPositionNearTarget(CartesianPosition position, CartesianPosition target, double tolerance_m) { double distance = 0.0; - if (!position.isPositionValid || target.isPositionValid) + if (!position.isPositionValid || !target.isPositionValid) return 0; distance = sqrt(pow(position.xCoord_m - target.xCoord_m, 2) + pow(position.yCoord_m - target.yCoord_m, 2) From b0444c69e56c22ed39df1cc43d2e8aa82fe49405 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 30 Apr 2020 09:38:46 +0200 Subject: [PATCH 470/523] Removed timestamp from methods using monitor data --- modules/ScenarioControl/inc/distancetrigger.h | 2 +- modules/ScenarioControl/inc/trigger.h | 16 ++++++++-------- modules/ScenarioControl/src/distancetrigger.cpp | 8 ++++---- modules/ScenarioControl/src/scenario.cpp | 2 +- 4 files changed, 14 insertions(+), 14 deletions(-) diff --git a/modules/ScenarioControl/inc/distancetrigger.h b/modules/ScenarioControl/inc/distancetrigger.h index e017d9a34..b18eddfeb 100644 --- a/modules/ScenarioControl/inc/distancetrigger.h +++ b/modules/ScenarioControl/inc/distancetrigger.h @@ -16,7 +16,7 @@ class DistanceTrigger : public BooleanTrigger TriggerReturnCode_t parseParameters() override; using BooleanTrigger::update; - TriggerReturnCode_t update(MonitorDataType newValue, struct timeval measurementTime) override; + TriggerReturnCode_t update(MonitorDataType newValue) override; void setTriggerDistance(double distance_m) { this->triggerDistance_m = distance_m; } void setReferencePoint(CartesianPosition point) { this->referencePoint = point; } diff --git a/modules/ScenarioControl/inc/trigger.h b/modules/ScenarioControl/inc/trigger.h index 5392b1c0b..86bf8c6ba 100644 --- a/modules/ScenarioControl/inc/trigger.h +++ b/modules/ScenarioControl/inc/trigger.h @@ -87,14 +87,14 @@ class Trigger */ virtual TriggerReturnCode_t update(void) { throw std::invalid_argument("Invalid signal type"); } virtual TriggerReturnCode_t update(struct timeval) { throw std::invalid_argument("Invalid signal type"); } - virtual TriggerReturnCode_t update(bool, struct timeval) { throw std::invalid_argument("Invalid signal type"); } - virtual TriggerReturnCode_t update(char, struct timeval) { throw std::invalid_argument("Invalid signal type"); } - virtual TriggerReturnCode_t update(int, struct timeval) { throw std::invalid_argument("Invalid signal type"); } - virtual TriggerReturnCode_t update(float, struct timeval) { throw std::invalid_argument("Invalid signal type"); } - virtual TriggerReturnCode_t update(double, struct timeval) { throw std::invalid_argument("Invalid signal type"); } - virtual TriggerReturnCode_t update(CartesianPosition, struct timeval) { throw std::invalid_argument("Invalid signal type"); } - virtual TriggerReturnCode_t update(TREOData) { throw std::invalid_argument("Invalid signal type"); } - virtual TriggerReturnCode_t update(MonitorDataType newValue, struct timeval measurementTime) { throw std::invalid_argument("Invalid signal type"); }; + virtual TriggerReturnCode_t update(bool, struct timeval) { throw std::invalid_argument("Invalid signal type bool"); } + virtual TriggerReturnCode_t update(char, struct timeval) { throw std::invalid_argument("Invalid signal type char"); } + virtual TriggerReturnCode_t update(int, struct timeval) { throw std::invalid_argument("Invalid signal type int"); } + virtual TriggerReturnCode_t update(float, struct timeval) { throw std::invalid_argument("Invalid signal type float"); } + virtual TriggerReturnCode_t update(double, struct timeval) { throw std::invalid_argument("Invalid signal type double"); } + virtual TriggerReturnCode_t update(CartesianPosition, struct timeval) { throw std::invalid_argument("Invalid signal type cartesian position"); } + virtual TriggerReturnCode_t update(TREOData) { throw std::invalid_argument("Invalid signal type TREO data"); } + virtual TriggerReturnCode_t update(MonitorDataType) { throw std::invalid_argument("Invalid signal type monitor data"); } static TriggerTypeCode_t asTypeCode(std::string typeCodeString); diff --git a/modules/ScenarioControl/src/distancetrigger.cpp b/modules/ScenarioControl/src/distancetrigger.cpp index d782881d9..fc92bc9cc 100644 --- a/modules/ScenarioControl/src/distancetrigger.cpp +++ b/modules/ScenarioControl/src/distancetrigger.cpp @@ -12,7 +12,7 @@ DistanceTrigger::DistanceTrigger(Trigger::TriggerID_t triggerID) : BooleanTrigge this->setTriggerDistance(0.0); } -Trigger::TriggerReturnCode_t DistanceTrigger::update(MonitorDataType newValue, struct timeval measurementTime) { +Trigger::TriggerReturnCode_t DistanceTrigger::update(MonitorDataType newValue) { double networkDelayCorrection_m = 0.0; struct timeval currentTime, networkDelay; @@ -23,7 +23,7 @@ Trigger::TriggerReturnCode_t DistanceTrigger::update(MonitorDataType newValue, s // Correct for two-way network delay effects on trigger distance if (newValue.data.speed.isLongitudinalValid && newValue.data.isTimestampValid) { TimeSetToCurrentSystemTime(¤tTime); - timersub(¤tTime, &measurementTime, &networkDelay); + timersub(¤tTime, &newValue.data.timestamp, &networkDelay); networkDelayCorrection_m = 2.0 * fabs(static_cast(networkDelay.tv_sec) + static_cast(networkDelay.tv_usec) / 1000000.0) * newValue.data.speed.longitudinal_m_s; } @@ -34,10 +34,10 @@ Trigger::TriggerReturnCode_t DistanceTrigger::update(MonitorDataType newValue, s switch (this->oper) { case LESS_THAN: return update(static_cast(UtilIsPositionNearTarget(newValue.data.position, this->referencePoint, this->triggerDistance_m + networkDelayCorrection_m)), - measurementTime); + newValue.data.timestamp); case GREATER_THAN: return update(static_cast(!UtilIsPositionNearTarget(newValue.data.position, this->referencePoint, this->triggerDistance_m - networkDelayCorrection_m)), - measurementTime); + newValue.data.timestamp); } throw std::logic_error("Distance trigger unimplemented operator"); } diff --git a/modules/ScenarioControl/src/scenario.cpp b/modules/ScenarioControl/src/scenario.cpp index 4611c7cf1..9b9436f56 100644 --- a/modules/ScenarioControl/src/scenario.cpp +++ b/modules/ScenarioControl/src/scenario.cpp @@ -474,7 +474,7 @@ Scenario::ScenarioReturnCode_t Scenario::updateTrigger(const MonitorDataType &mo break; case Trigger::TriggerTypeCode_t::TRIGGER_DISTANCE: if (monr.data.position.isPositionValid) { - tp->update(monr.data.position, monr.data.timestamp); + tp->update(monr); } else { LogMessage(LOG_LEVEL_WARNING, "Could not update trigger type %s due to invalid monitor data values", From c50acf3e064489c581cf37d5680b1724bd8badbe Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 4 May 2020 11:20:11 +0200 Subject: [PATCH 471/523] Implemented a list which holds test scenario commands which are to be sent based on ACCM and EXAC --- core/src/objectcontrol.c | 93 ++++++++++++++++++++++++++++++++++------ 1 file changed, 80 insertions(+), 13 deletions(-) diff --git a/core/src/objectcontrol.c b/core/src/objectcontrol.c index 29a399759..9479822b9 100644 --- a/core/src/objectcontrol.c +++ b/core/src/objectcontrol.c @@ -82,6 +82,12 @@ typedef enum { TRANSITION_MEMORY_ERROR } StateTransitionResult; +typedef struct { + uint16_t actionID; + ActionTypeParameter_t command; + in_addr_t ip; +} TestScenarioCommandAction; + /* Small note: syntax for declaring a function pointer is (example for a function taking an int and a float, returning nothing) where the function foo(int a, float b) is declared elsewhere: void (*fooptr)(int,float) = foo; @@ -106,6 +112,8 @@ static void vCloseSafetyChannel(int *sockfd); static size_t uiRecvMonitor(int *sockfd, char *buffer, size_t length); static int iGetObjectIndexFromObjectIP(in_addr_t ipAddr, in_addr_t objectIPs[], unsigned int numberOfObjects); static void signalHandler(int signo); +static void resetCommandActionList(TestScenarioCommandAction commandActions[], const int numberOfElementsInList); +static int addCommandToActionList(const TestScenarioCommandAction command, TestScenarioCommandAction commandActions[], const int numberOfElementsInList); static ssize_t ObjectControlSendTRAJMessage(const char *Filename, int *Socket, const char debug); @@ -146,6 +154,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { in_addr_t objectIPs[MAX_OBJECTS]; U32 object_udp_port[MAX_OBJECTS]; U32 object_tcp_port[MAX_OBJECTS]; + TestScenarioCommandAction commandActions[MAX_OBJECTS]; I32 nbr_objects = 0; enum COMMAND iCommand; U8 pcRecvBuffer[RECV_MESSAGE_BUFFER]; @@ -586,6 +595,8 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { // Get objects; name and drive file DataDictionaryGetForceToLocalhostU8(GSD, &iForceObjectToLocalhostU8); + resetCommandActionList(commandActions, sizeof (commandActions) / sizeof (commandActions[0])); + for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { if (0 == iForceObjectToLocalhostU8) { object_udp_port[iIndex] = SAFETY_CHANNEL_PORT; @@ -638,19 +649,40 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { } else if (iCommand == COMM_ACCM && vGetState(GSD) == OBC_STATE_CONNECTED) { UtilPopulateACCMDataStructFromMQ(pcRecvBuffer, sizeof (pcRecvBuffer), &mqACCMData); - iIndex = - iGetObjectIndexFromObjectIP(mqACCMData.ip, objectIPs, - sizeof (objectIPs) / sizeof (objectIPs[0])); - if (iIndex != -1) { - MessageLength = - encodeACCMMessage(&mqACCMData.actionID, &mqACCMData.actionType, - &mqACCMData.actionTypeParameter1, &mqACCMData.actionTypeParameter2, - &mqACCMData.actionTypeParameter3, MessageBuffer, - sizeof (MessageBuffer), 0); - UtilSendTCPData(MODULE_NAME, MessageBuffer, MessageLength, &(socket_fds[iIndex]), 0); + if (mqACCMData.actionType == ACTION_TEST_SCENARIO_COMMAND) { + // Special handling is required from Maestro for test scenario command + TestScenarioCommandAction newCommandAction; + if (mqACCMData.actionTypeParameter1 == ACTION_PARAMETER_VS_SEND_START) { + newCommandAction.ip = mqACCMData.ip; + newCommandAction.command = mqACCMData.actionTypeParameter1; + newCommandAction.actionID = mqACCMData.actionID; + if (addCommandToActionList(newCommandAction, commandActions, sizeof (commandActions) / sizeof (commandActions[0])) == -1) { + LogMessage(LOG_LEVEL_ERROR, "Unable to handle command action configuration"); + } + } + else { + LogMessage(LOG_LEVEL_WARNING, "Unimplemented test scenario command action"); + } + } + else { + // Send ACCM to objects + iIndex = iGetObjectIndexFromObjectIP(mqACCMData.ip, objectIPs, + sizeof (objectIPs) / sizeof (objectIPs[0])); + if (iIndex != -1) { + MessageLength = + encodeACCMMessage(&mqACCMData.actionID, &mqACCMData.actionType, + &mqACCMData.actionTypeParameter1, &mqACCMData.actionTypeParameter2, + &mqACCMData.actionTypeParameter3, MessageBuffer, + sizeof (MessageBuffer), 0); + UtilSendTCPData(MODULE_NAME, MessageBuffer, MessageLength, &(socket_fds[iIndex]), 0); + } + else if (mqACCMData.ip == 0) { + LogMessage(LOG_LEVEL_DEBUG, "ACCM with no configured target IP: no message sent"); + } + else { + LogMessage(LOG_LEVEL_WARNING, "Unable to send ACCM: no valid socket found"); + } } - else - LogMessage(LOG_LEVEL_WARNING, "Unable to send ACCM: no valid socket found"); } else if (iCommand == COMM_TRCM && vGetState(GSD) == OBC_STATE_CONNECTED) { UtilPopulateTRCMDataStructFromMQ(pcRecvBuffer, sizeof (pcRecvBuffer), &mqTRCMData); @@ -1157,7 +1189,42 @@ ssize_t ObjectControlSendTRAJMessage(const char *Filename, int *Socket, const ch } -static int iGetObjectIndexFromObjectIP(in_addr_t ipAddr, in_addr_t objectIPs[], unsigned int numberOfObjects) { +/*! + * \brief resetCommandActionList Resets the list back to unset values: 0 for the IP and ID, and + * ::ACTION_PARAMETER_UNAVAILABLE for the command. + * \param commandActions List of command actions + * \param numberOfElementsInList Number of elements in the entire list + */ +void resetCommandActionList(TestScenarioCommandAction commandActions[], const int numberOfElementsInList) { + for (int i = 0; i < numberOfElementsInList; ++i) { + commandActions[i].ip = 0; + commandActions[i].command = ACTION_PARAMETER_UNAVAILABLE; + commandActions[i].actionID = 0; + } +} + + +/*! + * \brief addCommandToActionList Adds a command to the command action list by searching for the + * first occurrence of ::ACTION_PARAMETER_UNAVAILABLE and replacing it with the chosen + * command. An error is returned if no such occurrence is found. + * \param command Command to be added to list + * \param commandActions List of all command actions + * \param numberOfElementsInList Number of elements in the entire list + * \return 0 on success, -1 otherwise + */ +int addCommandToActionList(const TestScenarioCommandAction command, TestScenarioCommandAction commandActions[], const int numberOfElementsInList) { + for (int i = 0; i < numberOfElementsInList; ++i) { + if (commandActions[i].command == ACTION_PARAMETER_UNAVAILABLE) { + commandActions[i] = command; + return 0; + } + } + errno = ENOBUFS; + return -1; +} + +int iGetObjectIndexFromObjectIP(in_addr_t ipAddr, in_addr_t objectIPs[], unsigned int numberOfObjects) { for (unsigned int i = 0; i < numberOfObjects; ++i) { if (objectIPs[i] == ipAddr) return (int)i; From 77ad7d27cb570c8fcc065a2dd075b63b8c53f7d7 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 4 May 2020 11:33:40 +0200 Subject: [PATCH 472/523] Added a check on the command action list so that STRT is not sent to objects with delayed start configured --- core/src/objectcontrol.c | 22 +++++++++++++++++++++- 1 file changed, 21 insertions(+), 1 deletion(-) diff --git a/core/src/objectcontrol.c b/core/src/objectcontrol.c index 9479822b9..a2f3bd9d4 100644 --- a/core/src/objectcontrol.c +++ b/core/src/objectcontrol.c @@ -114,6 +114,7 @@ static int iGetObjectIndexFromObjectIP(in_addr_t ipAddr, in_addr_t objectIPs[], static void signalHandler(int signo); static void resetCommandActionList(TestScenarioCommandAction commandActions[], const int numberOfElementsInList); static int addCommandToActionList(const TestScenarioCommandAction command, TestScenarioCommandAction commandActions[], const int numberOfElementsInList); +static int hasDelayedStart(const in_addr_t objectIP, const TestScenarioCommandAction commandActions[], const int numberOfElementsInList); static ssize_t ObjectControlSendTRAJMessage(const char *Filename, int *Socket, const char debug); @@ -555,7 +556,8 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { objectControlServerStatus = CONTROL_CENTER_STATUS_READY; for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - UtilSendTCPData("Object Control", MessageBuffer, MessageLength, &socket_fds[iIndex], 0); + if (!hasDelayedStart(objectIPs[iIndex], commandActions, sizeof (commandActions) / sizeof (commandActions[0]))) + UtilSendTCPData("Object Control", MessageBuffer, MessageLength, &socket_fds[iIndex], 0); } vSetState(OBC_STATE_RUNNING, GSD); @@ -1224,6 +1226,24 @@ int addCommandToActionList(const TestScenarioCommandAction command, TestScenario return -1; } + +/*! + * \brief hasDelayedStart Checks the command action list for any delayed start configurations + * for the chosen object IP. + * \param objectIP IP of the object to be checked for delayed start + * \param commandActions List of all configured command actions + * \param numberOfElementsInList Number of elements in the entire list + * \return Boolean value indicating if the object has a delayed start configuration + */ +int hasDelayedStart(const in_addr_t objectIP, const TestScenarioCommandAction commandActions[], const int numberOfElementsInList) { + for (int i = 0; i < numberOfElementsInList; ++i) { + if (commandActions[i].ip == objectIP && commandActions[i].command == ACTION_PARAMETER_VS_SEND_START) { + return 1; + } + } + return 0; +} + int iGetObjectIndexFromObjectIP(in_addr_t ipAddr, in_addr_t objectIPs[], unsigned int numberOfObjects) { for (unsigned int i = 0; i < numberOfObjects; ++i) { if (objectIPs[i] == ipAddr) From 285ae25c046558d82f97d797ae5acaeca20d9879 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 4 May 2020 15:07:44 +0200 Subject: [PATCH 473/523] Added STRT sent out on EXAC matching configuration --- core/src/objectcontrol.c | 78 ++++++++++++++++++++++++++++++++-------- 1 file changed, 64 insertions(+), 14 deletions(-) diff --git a/core/src/objectcontrol.c b/core/src/objectcontrol.c index a2f3bd9d4..840a11d68 100644 --- a/core/src/objectcontrol.c +++ b/core/src/objectcontrol.c @@ -86,7 +86,7 @@ typedef struct { uint16_t actionID; ActionTypeParameter_t command; in_addr_t ip; -} TestScenarioCommandAction; +} TestScenarioCommandAction; //!< Struct describing a command to be sent as action, e.g. delayed start /* Small note: syntax for declaring a function pointer is (example for a function taking an int and a float, returning nothing) where the function foo(int a, float b) is declared elsewhere: @@ -115,6 +115,7 @@ static void signalHandler(int signo); static void resetCommandActionList(TestScenarioCommandAction commandActions[], const int numberOfElementsInList); static int addCommandToActionList(const TestScenarioCommandAction command, TestScenarioCommandAction commandActions[], const int numberOfElementsInList); static int hasDelayedStart(const in_addr_t objectIP, const TestScenarioCommandAction commandActions[], const int numberOfElementsInList); +static int findCommandAction(const uint16_t actionID, const TestScenarioCommandAction commandActions[], const int numberOfElementsInList); static ssize_t ObjectControlSendTRAJMessage(const char *Filename, int *Socket, const char debug); @@ -704,21 +705,53 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { } else if (iCommand == COMM_EXAC && vGetState(GSD) == OBC_STATE_RUNNING) { UtilPopulateEXACDataStructFromMQ(pcRecvBuffer, sizeof (pcRecvBuffer), &mqEXACData); - iIndex = - iGetObjectIndexFromObjectIP(mqEXACData.ip, objectIPs, - sizeof (objectIPs) / sizeof (objectIPs[0])); - if (iIndex != -1) { - struct timeval executionTime; + int commandIndex; + if ((commandIndex = findCommandAction(mqEXACData.actionID, commandActions, sizeof (commandActions) / sizeof (commandActions[0]))) != -1) { + // TODO: send STRT to correct object with correct delay + switch (commandActions[commandIndex].command) { + case ACTION_PARAMETER_VS_SEND_START: + iIndex = + iGetObjectIndexFromObjectIP(mqEXACData.ip, objectIPs, + sizeof (objectIPs) / sizeof (objectIPs[0])); + if (iIndex != -1) { + struct timeval startTime; + TimeSetToCurrentSystemTime(¤tTime); + TimeSetToGPStime(&startTime, TimeGetAsGPSweek(¤tTime), mqEXACData.executionTime_qmsoW); + MessageLength = encodeSTRTMessage(&startTime, MessageBuffer, sizeof (MessageBuffer), 0); + UtilSendTCPData(MODULE_NAME, MessageBuffer, MessageLength, &socket_fds[iIndex], 0); + } + else if (mqEXACData.ip == 0) { + LogMessage(LOG_LEVEL_DEBUG, "Delayed STRT with no configured target IP: no message sent"); + } + else { + LogMessage(LOG_LEVEL_WARNING, "Unable to send delayed STRT: no valid socket found"); + } + break; + default: + LogMessage(LOG_LEVEL_WARNING, "Unimplemented EXAC test scenario command"); + } + } + else { + iIndex = + iGetObjectIndexFromObjectIP(mqEXACData.ip, objectIPs, + sizeof (objectIPs) / sizeof (objectIPs[0])); + if (iIndex != -1) { + struct timeval executionTime; - TimeSetToGPStime(&executionTime, TimeGetAsGPSweek(¤tTime), - mqEXACData.executionTime_qmsoW); - MessageLength = - encodeEXACMessage(&mqEXACData.actionID, &executionTime, MessageBuffer, - sizeof (MessageBuffer), 0); - UtilSendTCPData(MODULE_NAME, MessageBuffer, MessageLength, &(socket_fds[iIndex]), 0); + TimeSetToGPStime(&executionTime, TimeGetAsGPSweek(¤tTime), + mqEXACData.executionTime_qmsoW); + MessageLength = + encodeEXACMessage(&mqEXACData.actionID, &executionTime, MessageBuffer, + sizeof (MessageBuffer), 0); + UtilSendTCPData(MODULE_NAME, MessageBuffer, MessageLength, &(socket_fds[iIndex]), 0); + } + else if (mqEXACData.ip == 0) { + LogMessage(LOG_LEVEL_DEBUG, "EXAC with no configured target IP: no message sent"); + } + else { + LogMessage(LOG_LEVEL_WARNING, "Unable to send EXAC: no valid socket found"); + } } - else - LogMessage(LOG_LEVEL_WARNING, "Unable to send EXAC: no valid socket found"); } else if (iCommand == COMM_CONNECT && vGetState(GSD) == OBC_STATE_INITIALIZED) { LogMessage(LOG_LEVEL_INFO, "CONNECT received"); @@ -1244,6 +1277,23 @@ int hasDelayedStart(const in_addr_t objectIP, const TestScenarioCommandAction co return 0; } + +/*! + * \brief findCommandAction Finds the command action with specified action ID. + * \param actionID ID of the action to be found + * \param commandActions List of all configured command actions + * \param numberOfElementsInList Number of elements in the entire list + * \return Index of the command action, or -1 if not found + */ +int findCommandAction(const uint16_t actionID, const TestScenarioCommandAction commandActions[], const int numberOfElementsInList) { + for (int i = 0; i < numberOfElementsInList; ++i) { + if (commandActions[i].actionID == actionID && commandActions[i].command != ACTION_PARAMETER_UNAVAILABLE) { + return i; + } + } + return -1; +} + int iGetObjectIndexFromObjectIP(in_addr_t ipAddr, in_addr_t objectIPs[], unsigned int numberOfObjects) { for (unsigned int i = 0; i < numberOfObjects; ++i) { if (objectIPs[i] == ipAddr) From 93b31e2feba7eb104585d48abdf1e5a86a4106c4 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 4 May 2020 16:35:53 +0200 Subject: [PATCH 474/523] Updated start time in scenario control actions --- core/src/objectcontrol.c | 3 ++- modules/ScenarioControl/src/externalaction.cpp | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/core/src/objectcontrol.c b/core/src/objectcontrol.c index 840a11d68..68ffd64b8 100644 --- a/core/src/objectcontrol.c +++ b/core/src/objectcontrol.c @@ -707,7 +707,6 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { UtilPopulateEXACDataStructFromMQ(pcRecvBuffer, sizeof (pcRecvBuffer), &mqEXACData); int commandIndex; if ((commandIndex = findCommandAction(mqEXACData.actionID, commandActions, sizeof (commandActions) / sizeof (commandActions[0]))) != -1) { - // TODO: send STRT to correct object with correct delay switch (commandActions[commandIndex].command) { case ACTION_PARAMETER_VS_SEND_START: iIndex = @@ -717,6 +716,8 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { struct timeval startTime; TimeSetToCurrentSystemTime(¤tTime); TimeSetToGPStime(&startTime, TimeGetAsGPSweek(¤tTime), mqEXACData.executionTime_qmsoW); + LogPrint("Current time: %ld, Start time: %ld, delay: %u", + TimeGetAsUTCms(¤tTime), TimeGetAsUTCms(&startTime), mqEXACData.executionTime_qmsoW); MessageLength = encodeSTRTMessage(&startTime, MessageBuffer, sizeof (MessageBuffer), 0); UtilSendTCPData(MODULE_NAME, MessageBuffer, MessageLength, &socket_fds[iIndex], 0); } diff --git a/modules/ScenarioControl/src/externalaction.cpp b/modules/ScenarioControl/src/externalaction.cpp index 9d224e08d..01c586513 100644 --- a/modules/ScenarioControl/src/externalaction.cpp +++ b/modules/ScenarioControl/src/externalaction.cpp @@ -20,7 +20,7 @@ Action::ActionReturnCode_t ExternalAction::execute(void) TimeSetToCurrentSystemTime(&systemTime); data.actionID = actionID; - data.executionTime_qmsoW = actionDelayTime_qms == 0 ? 0 : TimeGetAsGPSqmsOfWeek(&systemTime) + actionDelayTime_qms; + data.executionTime_qmsoW = actionDelayTime_qms == 0 ? TimeGetAsGPSqmsOfWeek(&systemTime) : TimeGetAsGPSqmsOfWeek(&systemTime) + actionDelayTime_qms; data.ip = actionObjectIP; From 14e680d3b22106b4de21012a7ccfba61c6ba47c6 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 4 May 2020 16:47:41 +0200 Subject: [PATCH 475/523] Ran code formatter --- core/src/util.c | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/core/src/util.c b/core/src/util.c index 3393091c6..29bab3c1e 100644 --- a/core/src/util.c +++ b/core/src/util.c @@ -36,8 +36,8 @@ -- Defines ------------------------------------------------------------*/ -#define EARTH_EQUATOR_RADIUS_M 6378137.0 // earth semimajor axis (WGS84) (m) -#define INVERSE_FLATTENING 298.257223563 //in WGS84, f = 1/298.257223563 +#define EARTH_EQUATOR_RADIUS_M 6378137.0 // earth semimajor axis (WGS84) (m) +#define INVERSE_FLATTENING 298.257223563 //in WGS84, f = 1/298.257223563 #define EARTH_FLATTENING ( 1.0 / INVERSE_FLATTENING ) #define EARTH_POLE_RADIUS_M 6356752.3142451794975639665996337 //b = (1-f)*a #define VINCENTY_MIN_STEP_TOLERANCE 1e-12 @@ -938,7 +938,9 @@ int UtilVincentyDirect(double refLat, double refLon, double a1, double distance, sina = cos(U1) * sin(a1); pow2cosa = 1 - pow(sina, 2); - pow2u = pow2cosa * (pow(EARTH_EQUATOR_RADIUS_M, 2) - pow(EARTH_POLE_RADIUS_M, 2)) / pow(EARTH_POLE_RADIUS_M, 2); + pow2u = + pow2cosa * (pow(EARTH_EQUATOR_RADIUS_M, 2) - pow(EARTH_POLE_RADIUS_M, 2)) / pow(EARTH_POLE_RADIUS_M, + 2); A = 1 + pow2u / 16384.0 * (4096.0 + pow2u * (-768.0 + pow2u * (320.0 - 175.0 * pow2u))); B = pow2u / 1024.0 * (256.0 + pow2u * (-128.0 + pow2u * (74.0 - 47.0 * pow2u))); From c95591abaddbc9a91768426f584e07f706f0c7be Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 4 May 2020 16:48:26 +0200 Subject: [PATCH 476/523] Ran code formatter --- core/src/objectcontrol.c | 23 ++++++++++++++++------- 1 file changed, 16 insertions(+), 7 deletions(-) diff --git a/core/src/objectcontrol.c b/core/src/objectcontrol.c index 9479822b9..f57248685 100644 --- a/core/src/objectcontrol.c +++ b/core/src/objectcontrol.c @@ -112,8 +112,11 @@ static void vCloseSafetyChannel(int *sockfd); static size_t uiRecvMonitor(int *sockfd, char *buffer, size_t length); static int iGetObjectIndexFromObjectIP(in_addr_t ipAddr, in_addr_t objectIPs[], unsigned int numberOfObjects); static void signalHandler(int signo); -static void resetCommandActionList(TestScenarioCommandAction commandActions[], const int numberOfElementsInList); -static int addCommandToActionList(const TestScenarioCommandAction command, TestScenarioCommandAction commandActions[], const int numberOfElementsInList); +static void resetCommandActionList(TestScenarioCommandAction commandActions[], + const int numberOfElementsInList); +static int addCommandToActionList(const TestScenarioCommandAction command, + TestScenarioCommandAction commandActions[], + const int numberOfElementsInList); static ssize_t ObjectControlSendTRAJMessage(const char *Filename, int *Socket, const char debug); @@ -595,7 +598,8 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { // Get objects; name and drive file DataDictionaryGetForceToLocalhostU8(GSD, &iForceObjectToLocalhostU8); - resetCommandActionList(commandActions, sizeof (commandActions) / sizeof (commandActions[0])); + resetCommandActionList(commandActions, + sizeof (commandActions) / sizeof (commandActions[0])); for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { if (0 == iForceObjectToLocalhostU8) { @@ -652,11 +656,14 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { if (mqACCMData.actionType == ACTION_TEST_SCENARIO_COMMAND) { // Special handling is required from Maestro for test scenario command TestScenarioCommandAction newCommandAction; + if (mqACCMData.actionTypeParameter1 == ACTION_PARAMETER_VS_SEND_START) { newCommandAction.ip = mqACCMData.ip; newCommandAction.command = mqACCMData.actionTypeParameter1; newCommandAction.actionID = mqACCMData.actionID; - if (addCommandToActionList(newCommandAction, commandActions, sizeof (commandActions) / sizeof (commandActions[0])) == -1) { + if (addCommandToActionList + (newCommandAction, commandActions, + sizeof (commandActions) / sizeof (commandActions[0])) == -1) { LogMessage(LOG_LEVEL_ERROR, "Unable to handle command action configuration"); } } @@ -667,11 +674,12 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { else { // Send ACCM to objects iIndex = iGetObjectIndexFromObjectIP(mqACCMData.ip, objectIPs, - sizeof (objectIPs) / sizeof (objectIPs[0])); + sizeof (objectIPs) / sizeof (objectIPs[0])); if (iIndex != -1) { MessageLength = encodeACCMMessage(&mqACCMData.actionID, &mqACCMData.actionType, - &mqACCMData.actionTypeParameter1, &mqACCMData.actionTypeParameter2, + &mqACCMData.actionTypeParameter1, + &mqACCMData.actionTypeParameter2, &mqACCMData.actionTypeParameter3, MessageBuffer, sizeof (MessageBuffer), 0); UtilSendTCPData(MODULE_NAME, MessageBuffer, MessageLength, &(socket_fds[iIndex]), 0); @@ -1213,7 +1221,8 @@ void resetCommandActionList(TestScenarioCommandAction commandActions[], const in * \param numberOfElementsInList Number of elements in the entire list * \return 0 on success, -1 otherwise */ -int addCommandToActionList(const TestScenarioCommandAction command, TestScenarioCommandAction commandActions[], const int numberOfElementsInList) { +int addCommandToActionList(const TestScenarioCommandAction command, + TestScenarioCommandAction commandActions[], const int numberOfElementsInList) { for (int i = 0; i < numberOfElementsInList; ++i) { if (commandActions[i].command == ACTION_PARAMETER_UNAVAILABLE) { commandActions[i] = command; From 311c189b1abce302f90006c3689fb6e4bdf750fd Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 4 May 2020 16:52:29 +0200 Subject: [PATCH 477/523] Ran code formatter --- core/src/objectcontrol.c | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/core/src/objectcontrol.c b/core/src/objectcontrol.c index f2a173392..5da6357e7 100644 --- a/core/src/objectcontrol.c +++ b/core/src/objectcontrol.c @@ -117,7 +117,8 @@ static void resetCommandActionList(TestScenarioCommandAction commandActions[], static int addCommandToActionList(const TestScenarioCommandAction command, TestScenarioCommandAction commandActions[], const int numberOfElementsInList); -static int hasDelayedStart(const in_addr_t objectIP, const TestScenarioCommandAction commandActions[], const int numberOfElementsInList); +static int hasDelayedStart(const in_addr_t objectIP, const TestScenarioCommandAction commandActions[], + const int numberOfElementsInList); static ssize_t ObjectControlSendTRAJMessage(const char *Filename, int *Socket, const char debug); static int iFindObjectsInfo(C8 object_traj_file[MAX_OBJECTS][MAX_FILE_PATH], @@ -558,8 +559,11 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { objectControlServerStatus = CONTROL_CENTER_STATUS_READY; for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - if (!hasDelayedStart(objectIPs[iIndex], commandActions, sizeof (commandActions) / sizeof (commandActions[0]))) - UtilSendTCPData("Object Control", MessageBuffer, MessageLength, &socket_fds[iIndex], 0); + if (!hasDelayedStart + (objectIPs[iIndex], commandActions, + sizeof (commandActions) / sizeof (commandActions[0]))) + UtilSendTCPData("Object Control", MessageBuffer, MessageLength, &socket_fds[iIndex], + 0); } vSetState(OBC_STATE_RUNNING, GSD); @@ -1243,7 +1247,8 @@ int addCommandToActionList(const TestScenarioCommandAction command, * \param numberOfElementsInList Number of elements in the entire list * \return Boolean value indicating if the object has a delayed start configuration */ -int hasDelayedStart(const in_addr_t objectIP, const TestScenarioCommandAction commandActions[], const int numberOfElementsInList) { +int hasDelayedStart(const in_addr_t objectIP, const TestScenarioCommandAction commandActions[], + const int numberOfElementsInList) { for (int i = 0; i < numberOfElementsInList; ++i) { if (commandActions[i].ip == objectIP && commandActions[i].command == ACTION_PARAMETER_VS_SEND_START) { return 1; From fc9349def2c6e268564694087dbc29038d025664 Mon Sep 17 00:00:00 2001 From: Sebastian Loh Lindholm Date: Tue, 5 May 2020 15:24:27 +0200 Subject: [PATCH 478/523] Start of RCCM BackToStart for Midspeed. --- core/inc/iso22133.h | 1 + core/inc/util.h | 15 ++++++++-- core/src/iso22133.c | 65 ++++++++++++++++++++++++++++++++++++++++ core/src/objectcontrol.c | 35 ++++++++++++++++++++++ core/src/systemcontrol.c | 61 ++++++++++++++++++++----------------- 5 files changed, 147 insertions(+), 30 deletions(-) diff --git a/core/inc/iso22133.h b/core/inc/iso22133.h index 93b409e22..104c0118d 100644 --- a/core/inc/iso22133.h +++ b/core/inc/iso22133.h @@ -207,6 +207,7 @@ ssize_t decodeTREOMessage(); ssize_t encodeACCMMessage(const uint16_t* actionID, const ActionType_t* actionType, const ActionTypeParameter_t* param1, const ActionTypeParameter_t* param2, const 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 encodeINSUPMessage(const SupervisorCommandType, char * insupDataBuffer, const size_t bufferLength, const char debug); +ssize_t encodeRCCMMessage(const uint8_t rccmControlCommand, char *rccmDataBuffer, const size_t bufferLength, const char debug); ISOMessageID getISOMessageType(const char * messageData, const size_t length, const char debug); void setISOCRCVerification(const int8_t enabled); diff --git a/core/inc/util.h b/core/inc/util.h index 177189cb0..8eacc1f7e 100644 --- a/core/inc/util.h +++ b/core/inc/util.h @@ -98,8 +98,8 @@ extern "C"{ #define TCP_RX_BUFFER 1024 #define MAX_ADAPTIVE_SYNC_POINTS 512 -#define USE_LOCAL_USER_CONTROL 0 -#define LOCAL_USER_CONTROL_IP "192.168.0.7" +#define USE_LOCAL_USER_CONTROL 1 +#define LOCAL_USER_CONTROL_IP "195.0.0.10" #define USE_TEST_HOST 0 #define TESTHOST_IP LOCAL_USER_CONTROL_IP #define TESTSERVER_IP LOCAL_USER_CONTROL_IP @@ -196,6 +196,10 @@ extern "C"{ #define DD_CONTROL_BUFFER_SIZE_52 52 #define DD_CONTROL_TASK_PERIOD_MS 1 +#define REQ_OBC_STATE_CHANGE_REMOTE_CONTROL 1 +#define REQ_OBC_STATE_CHANGE_CONNECTED 2 +#define REQ_RCCM_BACK_TO_START 3 + //! Internal message queue communication identifiers enum COMMAND { @@ -223,6 +227,7 @@ COMM_TREO = 23, COMM_ACCM = 24, COMM_TRCM = 25, COMM_DISARM = 26, +COMM_RCCM = 27, COMM_MONR = 239, COMM_OBJECTS_CONNECTED = 111, COMM_FAILURE = 254, @@ -318,7 +323,8 @@ typedef enum { OBC_STATE_CONNECTED, OBC_STATE_ARMED, OBC_STATE_RUNNING, - OBC_STATE_ERROR + OBC_STATE_ERROR, + OBC_STATE_REMOTE_CONTROL } OBCState_t; typedef struct @@ -522,6 +528,8 @@ typedef enum { } ReadWriteAccess_t; +#pragma pack(push,1) + typedef struct { U32 MessageLengthU32; @@ -552,6 +560,7 @@ typedef struct U8 SysCtrlStateU8; } RVSSMaestroType; +#pragma pack(pop) typedef enum { NORTHERN, diff --git a/core/src/iso22133.c b/core/src/iso22133.c index 4cc48bf26..0cc166706 100644 --- a/core/src/iso22133.c +++ b/core/src/iso22133.c @@ -423,6 +423,20 @@ typedef struct { #define VALUE_ID_INSUP_MODE 0x0200 +/*! RCCM message */ +typedef struct { + HeaderType header; + uint16_t RCCMControlValueID; + uint16_t RCCMControlContentLength; + uint8_t rccmControlU8; + FooterType footer; +} RCCMType; //18 bytes + +//! HEAB value IDs +#define VALUE_ID_RCCM_CONTROL 0x0201 + + + #pragma pack(pop) @@ -1459,6 +1473,57 @@ ssize_t encodeHEABMessage(const ControlCenterStatusType status, char *heabDataBu } + +/*! + * \brief encodeRCCMMessage + * \param + * \param + * \param + * \param + * \return Number of bytes written or -1 in case of an error + */ +ssize_t encodeRCCMMessage(const uint8_t rccmControlCommand, char *rccmDataBuffer, + const size_t bufferLength, const char debug) { + + RCCMType RCCMData; + + memset(rccmDataBuffer, 0, bufferLength); + + // If buffer too small to hold HEAB data, generate an error + if (bufferLength < sizeof (RCCMType)) { + LogMessage(LOG_LEVEL_ERROR, "Buffer too small to hold necessary RCCM data"); + return -1; + } + + // Construct header + RCCMData.header = buildISOHeader(MESSAGE_ID_RCCM, sizeof (RCCMData), debug); + + // Fill contents + RCCMData.RCCMControlValueID = VALUE_ID_RCCM_CONTROL; + RCCMData.RCCMControlContentLength = 1; + RCCMData.rccmControlU8 = rccmControlCommand; + + + if (debug) { + LogPrint("RCCM message:\n\tRCCM Control value ID: 0x%x\n\t" + "RCCM Control content length: %u\n\t" + "Control center status: %d", RCCMData.RCCMControlValueID, RCCMData.RCCMControlContentLength, + RCCMData.rccmControlU8); + } + + // Switch from host endianness to little endian + RCCMData.RCCMControlValueID = htole16(RCCMData.RCCMControlValueID); + RCCMData.RCCMControlContentLength = htole16(RCCMData.RCCMControlContentLength); + + RCCMData.footer = buildISOFooter(&RCCMData, sizeof (RCCMData), debug); + + memcpy(rccmDataBuffer, &RCCMData, sizeof (RCCMData)); + + return sizeof (RCCMType); + +} + + /*! * \brief buildMONRMessage Fills a MONRType struct from a buffer of raw data * \param MonrData Raw data to be decoded diff --git a/core/src/objectcontrol.c b/core/src/objectcontrol.c index 29a399759..836ec7f55 100644 --- a/core/src/objectcontrol.c +++ b/core/src/objectcontrol.c @@ -578,6 +578,41 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { ObjectcontrolExecutionMode = OBJECT_CONTROL_CONTROL_MODE; printf("[ObjectControl] Object control in CONTROL mode\n"); } + else if(iCommand == COMM_RCCM) + { + if(vGetState(GSD) == OBC_STATE_CONNECTED) + { + if(pcRecvBuffer[0] == REQ_OBC_STATE_CHANGE_REMOTE_CONTROL) { + vSetState(OBC_STATE_REMOTE_CONTROL, GSD); + LogMessage(LOG_LEVEL_INFO, "RCCM message changed state to REMOTE_CONTROL."); + LOG_SEND(LogBuffer, "[ObjectControl] RCCM message changed state to REMOTE_CONTROL."); + } + } + else if(vGetState(GSD) == OBC_STATE_REMOTE_CONTROL) + { + if(pcRecvBuffer[0] == REQ_OBC_STATE_CHANGE_CONNECTED) { + vSetState(OBC_STATE_REMOTE_CONTROL, GSD); + LogMessage(LOG_LEVEL_INFO, "RCCM message changed state to CONNECTED."); + LOG_SEND(LogBuffer, "[ObjectControl] RCCM message changed state to CONNECTED."); + } + } + else if(vGetState(GSD) == OBC_STATE_REMOTE_CONTROL) + { + if(pcRecvBuffer[0] == REQ_RCCM_BACK_TO_START) + { + for(iIndex=0;iIndex Date: Fri, 8 May 2020 13:16:41 +0200 Subject: [PATCH 479/523] Updated util ref --- util | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/util b/util index e73444774..68f0c384d 160000 --- a/util +++ b/util @@ -1 +1 @@ -Subproject commit e7344477470c3ac90449e90c0bde86e1261372ad +Subproject commit 68f0c384de24cdff9a0b73d26425577c8a244aa0 From b6367a0f75b6f68e05ad3e40fef496e2dce0dc57 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 11 May 2020 09:56:02 +0200 Subject: [PATCH 480/523] Added minus sign to regex for TandA conf file --- modules/ScenarioControl/src/scenario.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/ScenarioControl/src/scenario.cpp b/modules/ScenarioControl/src/scenario.cpp index 9b9436f56..ec85bbc8c 100644 --- a/modules/ScenarioControl/src/scenario.cpp +++ b/modules/ScenarioControl/src/scenario.cpp @@ -127,7 +127,7 @@ void Scenario::parseScenarioFileLine(const std::string &inputLine) // Match relevant field according to below patterns regex ipAddrPattern("([0-2]?[0-9]?[0-9]\\.){3}([0-2]?[0-9]?[0-9])"); // Match 3 "<000-299>." followed by "<000-299>" - regex triggerActionPattern("(([a-zA-Z_])+\\[([a-zA-Z0-9\\.,<=>_:()])+\\])+"); + regex triggerActionPattern("(([a-zA-Z_])+\\[([a-zA-Z0-9\\.,\\-<=>_:()])+\\])+"); in_addr triggerIP, actionIP; string errMsg; set actions; From 5b7268a7fe05c4ca4c0203d69031d690ff2de354 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 11 May 2020 09:58:25 +0200 Subject: [PATCH 481/523] Split network delay correction into time and distance --- modules/ScenarioControl/inc/distancetrigger.h | 1 + modules/ScenarioControl/src/distancetrigger.cpp | 6 +++--- modules/ScenarioControl/src/scenario.cpp | 2 +- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/modules/ScenarioControl/inc/distancetrigger.h b/modules/ScenarioControl/inc/distancetrigger.h index b18eddfeb..335255772 100644 --- a/modules/ScenarioControl/inc/distancetrigger.h +++ b/modules/ScenarioControl/inc/distancetrigger.h @@ -30,6 +30,7 @@ class DistanceTrigger : public BooleanTrigger CartesianPosition referencePoint; enum {LESS_THAN, GREATER_THAN} oper = LESS_THAN; + const std::set getAcceptedParameters() const override { std::set accParams; diff --git a/modules/ScenarioControl/src/distancetrigger.cpp b/modules/ScenarioControl/src/distancetrigger.cpp index fc92bc9cc..5126e588a 100644 --- a/modules/ScenarioControl/src/distancetrigger.cpp +++ b/modules/ScenarioControl/src/distancetrigger.cpp @@ -13,7 +13,7 @@ DistanceTrigger::DistanceTrigger(Trigger::TriggerID_t triggerID) : BooleanTrigge } Trigger::TriggerReturnCode_t DistanceTrigger::update(MonitorDataType newValue) { - double networkDelayCorrection_m = 0.0; + double networkDelayCorrection_m = 0.0, networkDelay_s = 0.0; struct timeval currentTime, networkDelay; if (!newValue.data.position.isPositionValid || !referencePoint.isPositionValid) { @@ -24,8 +24,8 @@ Trigger::TriggerReturnCode_t DistanceTrigger::update(MonitorDataType newValue) { if (newValue.data.speed.isLongitudinalValid && newValue.data.isTimestampValid) { TimeSetToCurrentSystemTime(¤tTime); timersub(¤tTime, &newValue.data.timestamp, &networkDelay); - networkDelayCorrection_m = 2.0 * fabs(static_cast(networkDelay.tv_sec) + static_cast(networkDelay.tv_usec) / 1000000.0) - * newValue.data.speed.longitudinal_m_s; + networkDelay_s = fabs(static_cast(networkDelay.tv_sec) + static_cast(networkDelay.tv_usec) / 1000000.0); + networkDelayCorrection_m = 2.0 * networkDelay_s * newValue.data.speed.longitudinal_m_s; } else { LogMessage(LOG_LEVEL_WARNING, "Invalid monitor data speed or timestamp: cannot correct for network delay"); diff --git a/modules/ScenarioControl/src/scenario.cpp b/modules/ScenarioControl/src/scenario.cpp index ec85bbc8c..adb283795 100644 --- a/modules/ScenarioControl/src/scenario.cpp +++ b/modules/ScenarioControl/src/scenario.cpp @@ -456,7 +456,7 @@ void Scenario::resetISOTriggers(void) Scenario::ScenarioReturnCode_t Scenario::updateTrigger(const MonitorDataType &monr) { for (Trigger* tp : allTriggers) - { + { if(tp->getObjectIP() == monr.ClientIP && dynamic_cast(tp) == nullptr) { switch (tp->getTypeCode()) From 62902ac2eba74caac8ed83469b7674c0168e0997 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 11 May 2020 15:58:48 +0200 Subject: [PATCH 482/523] Added leap sec diff to pitipo decoding --- core/inc/util.h | 3 --- core/src/timecontrol.c | 1 + 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/core/inc/util.h b/core/inc/util.h index 177189cb0..d43328875 100644 --- a/core/inc/util.h +++ b/core/inc/util.h @@ -119,9 +119,6 @@ extern "C"{ /* Difference of leap seconds between UTC and ETSI */ #define DIFF_LEAP_SECONDS_UTC_ETSI 5 -// Difference is 18 leap seconds between utc and gps -#define MS_LEAP_SEC_DIFF_UTC_GPS 18000 - // 7 * 24 * 3600 * 1000 #define WEEK_TIME_MS 604800000 // 24 * 3600 * 1000 diff --git a/core/src/timecontrol.c b/core/src/timecontrol.c index 72e81bea1..8135399f0 100644 --- a/core/src/timecontrol.c +++ b/core/src/timecontrol.c @@ -515,6 +515,7 @@ static void TimeControlDecodeTimeBuffer(TimeType * GPSTime, C8 * TimeBuffer, C8 [17]) << 32 | ((U64) TimeBuffer[18]) << 24 | ((U64) TimeBuffer[19]) << 16 | ((U64) TimeBuffer[20]) << 8 | TimeBuffer[21]; + GPSTime->GPSMillisecondsU64 += MS_LEAP_SEC_DIFF_UTC_GPS; GPSTime->GPSMinutesU32 = ((U32) TimeBuffer[22]) << 24 | ((U32) TimeBuffer[23]) << 16 | ((U32) TimeBuffer[24]) << 8 | TimeBuffer[25]; From 0c6c1a727572a5064075992b9cbb9a5910a6079d Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 13 May 2020 10:13:37 +0200 Subject: [PATCH 483/523] Added IP to printout of scenario configuration --- modules/ScenarioControl/inc/action.h | 6 ++++++ modules/ScenarioControl/inc/trigger.h | 6 ++++++ modules/ScenarioControl/src/booleantrigger.cpp | 6 ++++++ modules/ScenarioControl/src/scenario.cpp | 4 ++-- 4 files changed, 20 insertions(+), 2 deletions(-) diff --git a/modules/ScenarioControl/inc/action.h b/modules/ScenarioControl/inc/action.h index 79856efc9..306d305d3 100644 --- a/modules/ScenarioControl/inc/action.h +++ b/modules/ScenarioControl/inc/action.h @@ -6,6 +6,7 @@ #include #include #include +#include #include "util.h" @@ -55,6 +56,11 @@ class Action std::string getParametersString(void) const; ACCMData getConfigurationMessageData(void) const; in_addr_t getObjectIP(void) const { return actionObjectIP; } + std::string getObjectIPAsString(void) { + std::vector str(INET_ADDRSTRLEN); + inet_ntop(AF_INET, &actionObjectIP, str.data(), static_cast(str.size())); + return std::string(str.data(), str.size()); + } void setObjectIP(in_addr_t ipAddr) { actionObjectIP = ipAddr; } void setExecuteDelayTime(struct timeval tm); diff --git a/modules/ScenarioControl/inc/trigger.h b/modules/ScenarioControl/inc/trigger.h index 86bf8c6ba..3956e6641 100644 --- a/modules/ScenarioControl/inc/trigger.h +++ b/modules/ScenarioControl/inc/trigger.h @@ -7,6 +7,7 @@ #include #include #include +#include #include "util.h" #include "iso22133.h" @@ -44,6 +45,11 @@ class Trigger std::vector getParameters() const { return parameters; } bool isActive() const; in_addr_t getObjectIP(void) const { return triggerObjectIP; } + std::string getObjectIPAsString(void) { + std::vector str(INET_ADDRSTRLEN); + inet_ntop(AF_INET, &triggerObjectIP, str.data(), static_cast(str.size())); + return std::string(str.data(), str.size()); + } bool operator==(const Trigger &other) const { return (other.triggerID == triggerID) && isSimilar(other); } bool isSimilar(const Trigger &other) const; diff --git a/modules/ScenarioControl/src/booleantrigger.cpp b/modules/ScenarioControl/src/booleantrigger.cpp index 048b2e2d9..8320a7791 100644 --- a/modules/ScenarioControl/src/booleantrigger.cpp +++ b/modules/ScenarioControl/src/booleantrigger.cpp @@ -11,6 +11,12 @@ Trigger::TriggerReturnCode_t BooleanTrigger::update(bool currentStateValue, stru wasStateTrue = isStateTrue; isStateTrue = currentStateValue; wasTriggeredByLastUpdate = checkIfTriggered(); + if (wasTriggeredByLastUpdate == TRIGGER_OCCURRED) { + LogPrint("was triggered"); + } + else if (wasTriggeredByLastUpdate == NO_TRIGGER_OCCURRED) { + LogPrint("no trigger"); + } return wasTriggeredByLastUpdate; } diff --git a/modules/ScenarioControl/src/scenario.cpp b/modules/ScenarioControl/src/scenario.cpp index adb283795..e1665bfc8 100644 --- a/modules/ScenarioControl/src/scenario.cpp +++ b/modules/ScenarioControl/src/scenario.cpp @@ -63,11 +63,11 @@ void Scenario::initialize(const std::string scenarioFilePath) debugStr = "Triggers:\n"; for (Trigger* tp : allTriggers) - debugStr += "\t" + tp->getTypeAsString(tp->getTypeCode()) + "\n"; + debugStr += "\t" + tp->getObjectIPAsString() + " " + tp->getTypeAsString(tp->getTypeCode()) + "\n"; debugStr += "Actions:\n"; for (Action* ap : allActions) - debugStr += "\t" + ap->getTypeAsString(ap->getTypeCode()) + "\n"; + debugStr += "\t" + ap->getObjectIPAsString() + " " + ap->getTypeAsString(ap->getTypeCode()) + "\n"; debugStr.pop_back(); LogMessage(LOG_LEVEL_DEBUG, debugStr.c_str()); From 0ad0f2047f5cdeb87fc94d4c1fd9dc077da083ee Mon Sep 17 00:00:00 2001 From: Sebastian Loh Lindholm Date: Wed, 13 May 2020 16:45:06 +0200 Subject: [PATCH 484/523] Not chrasing when sending RCMM to ObjectControl. --- core/inc/iso22133.h | 2 +- core/inc/util.h | 17 +++++----- core/src/iso22133.c | 54 +++++++++++++++--------------- core/src/objectcontrol.c | 71 +++++++++++++++++++++++++--------------- core/src/systemcontrol.c | 28 ++++++++-------- core/src/util.c | 4 +++ 6 files changed, 101 insertions(+), 75 deletions(-) diff --git a/core/inc/iso22133.h b/core/inc/iso22133.h index 104c0118d..013b0625c 100644 --- a/core/inc/iso22133.h +++ b/core/inc/iso22133.h @@ -207,7 +207,7 @@ ssize_t decodeTREOMessage(); ssize_t encodeACCMMessage(const uint16_t* actionID, const ActionType_t* actionType, const ActionTypeParameter_t* param1, const ActionTypeParameter_t* param2, const 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 encodeINSUPMessage(const SupervisorCommandType, char * insupDataBuffer, const size_t bufferLength, const char debug); -ssize_t encodeRCCMMessage(const uint8_t rccmControlCommand, char *rccmDataBuffer, const size_t bufferLength, const char debug); +ssize_t encodeRCMMMessage(const uint8_t rcmmControlCommand, char *rcmmDataBuffer, const size_t bufferLength, const char debug); ISOMessageID getISOMessageType(const char * messageData, const size_t length, const char debug); void setISOCRCVerification(const int8_t enabled); diff --git a/core/inc/util.h b/core/inc/util.h index 8eacc1f7e..2b27bdc60 100644 --- a/core/inc/util.h +++ b/core/inc/util.h @@ -98,8 +98,8 @@ extern "C"{ #define TCP_RX_BUFFER 1024 #define MAX_ADAPTIVE_SYNC_POINTS 512 -#define USE_LOCAL_USER_CONTROL 1 -#define LOCAL_USER_CONTROL_IP "195.0.0.10" +#define USE_LOCAL_USER_CONTROL 0 +#define LOCAL_USER_CONTROL_IP "10.114.98.26" #define USE_TEST_HOST 0 #define TESTHOST_IP LOCAL_USER_CONTROL_IP #define TESTSERVER_IP LOCAL_USER_CONTROL_IP @@ -196,9 +196,11 @@ extern "C"{ #define DD_CONTROL_BUFFER_SIZE_52 52 #define DD_CONTROL_TASK_PERIOD_MS 1 -#define REQ_OBC_STATE_CHANGE_REMOTE_CONTROL 1 -#define REQ_OBC_STATE_CHANGE_CONNECTED 2 -#define REQ_RCCM_BACK_TO_START 3 +#define REQ_STATE_CHANGE_REMOTE_CONTROL 1 +#define REQ_STATE_CHANGE_DISARMED 2 +#define REQ_RCMM_BACK_TO_START 3 +#define REQ_RCMM_ENABLE_AUTO_START 4 +#define REQ_RCMM_DISABLE_AUTO_START 5 //! Internal message queue communication identifiers enum COMMAND @@ -227,7 +229,7 @@ COMM_TREO = 23, COMM_ACCM = 24, COMM_TRCM = 25, COMM_DISARM = 26, -COMM_RCCM = 27, +COMM_RCMM = 27, COMM_MONR = 239, COMM_OBJECTS_CONNECTED = 111, COMM_FAILURE = 254, @@ -323,8 +325,7 @@ typedef enum { OBC_STATE_CONNECTED, OBC_STATE_ARMED, OBC_STATE_RUNNING, - OBC_STATE_ERROR, - OBC_STATE_REMOTE_CONTROL + OBC_STATE_ERROR } OBCState_t; typedef struct diff --git a/core/src/iso22133.c b/core/src/iso22133.c index 0cc166706..0e3392c45 100644 --- a/core/src/iso22133.c +++ b/core/src/iso22133.c @@ -423,17 +423,17 @@ typedef struct { #define VALUE_ID_INSUP_MODE 0x0200 -/*! RCCM message */ +/*! RCMM message */ typedef struct { HeaderType header; - uint16_t RCCMControlValueID; - uint16_t RCCMControlContentLength; - uint8_t rccmControlU8; + uint16_t RCMMControlValueID; + uint16_t RCMMControlContentLength; + uint8_t rcmmControlU8; FooterType footer; -} RCCMType; //18 bytes +} RCMMType; //18 bytes //! HEAB value IDs -#define VALUE_ID_RCCM_CONTROL 0x0201 +#define VALUE_ID_RCMM_CONTROL 0x0201 @@ -658,7 +658,7 @@ char isValidMessageID(const uint16_t id) { || id == MESSAGE_ID_OSTM || id == MESSAGE_ID_STRT || id == MESSAGE_ID_MONR2 || id == MESSAGE_ID_SOWM || 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_CATA || id == MESSAGE_ID_RCMM || id == MESSAGE_ID_RCRT || id == MESSAGE_ID_PIME || id == MESSAGE_ID_COSE || id == MESSAGE_ID_MOMA || (id >= MESSAGE_ID_VENDOR_SPECIFIC_LOWER_LIMIT && id <= MESSAGE_ID_VENDOR_SPECIFIC_UPPER_LIMIT); } @@ -1475,51 +1475,51 @@ ssize_t encodeHEABMessage(const ControlCenterStatusType status, char *heabDataBu /*! - * \brief encodeRCCMMessage + * \brief encodeRCMMMessage * \param * \param * \param * \param * \return Number of bytes written or -1 in case of an error */ -ssize_t encodeRCCMMessage(const uint8_t rccmControlCommand, char *rccmDataBuffer, +ssize_t encodeRCMMMessage(const uint8_t rcmmControlCommand, char *rcmmDataBuffer, const size_t bufferLength, const char debug) { - RCCMType RCCMData; + RCMMType RCMMData; - memset(rccmDataBuffer, 0, bufferLength); + memset(rcmmDataBuffer, 0, bufferLength); - // If buffer too small to hold HEAB data, generate an error - if (bufferLength < sizeof (RCCMType)) { - LogMessage(LOG_LEVEL_ERROR, "Buffer too small to hold necessary RCCM data"); + // If buffer too small to hold RCMM data, generate an error + if (bufferLength < sizeof (RCMMType)) { + LogMessage(LOG_LEVEL_ERROR, "Buffer too small to hold necessary RCMM data"); return -1; } // Construct header - RCCMData.header = buildISOHeader(MESSAGE_ID_RCCM, sizeof (RCCMData), debug); + RCMMData.header = buildISOHeader(MESSAGE_ID_RCMM, sizeof (RCMMData), debug); // Fill contents - RCCMData.RCCMControlValueID = VALUE_ID_RCCM_CONTROL; - RCCMData.RCCMControlContentLength = 1; - RCCMData.rccmControlU8 = rccmControlCommand; + RCMMData.RCMMControlValueID = VALUE_ID_RCMM_CONTROL; + RCMMData.RCMMControlContentLength = 1; + RCMMData.rcmmControlU8 = rcmmControlCommand; if (debug) { - LogPrint("RCCM message:\n\tRCCM Control value ID: 0x%x\n\t" - "RCCM Control content length: %u\n\t" - "Control center status: %d", RCCMData.RCCMControlValueID, RCCMData.RCCMControlContentLength, - RCCMData.rccmControlU8); + LogPrint("RCMM message:\n\tRCMM Control value ID: 0x%x\n\t" + "RCMM Control content length: %u\n\t" + "Control center status: %d", RCMMData.RCMMControlValueID, RCMMData.RCMMControlContentLength, + RCMMData.rcmmControlU8); } // Switch from host endianness to little endian - RCCMData.RCCMControlValueID = htole16(RCCMData.RCCMControlValueID); - RCCMData.RCCMControlContentLength = htole16(RCCMData.RCCMControlContentLength); + RCMMData.RCMMControlValueID = htole16(RCMMData.RCMMControlValueID); + RCMMData.RCMMControlContentLength = htole16(RCMMData.RCMMControlContentLength); - RCCMData.footer = buildISOFooter(&RCCMData, sizeof (RCCMData), debug); + RCMMData.footer = buildISOFooter(&RCMMData, sizeof (RCMMData), debug); - memcpy(rccmDataBuffer, &RCCMData, sizeof (RCCMData)); + memcpy(rcmmDataBuffer, &RCMMData, sizeof (RCMMData)); - return sizeof (RCCMType); + return sizeof (RCMMType); } diff --git a/core/src/objectcontrol.c b/core/src/objectcontrol.c index 836ec7f55..0436e9e43 100644 --- a/core/src/objectcontrol.c +++ b/core/src/objectcontrol.c @@ -578,39 +578,58 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { ObjectcontrolExecutionMode = OBJECT_CONTROL_CONTROL_MODE; printf("[ObjectControl] Object control in CONTROL mode\n"); } - else if(iCommand == COMM_RCCM) + else if(iCommand == COMM_RCMM) { - if(vGetState(GSD) == OBC_STATE_CONNECTED) - { - if(pcRecvBuffer[0] == REQ_OBC_STATE_CHANGE_REMOTE_CONTROL) { - vSetState(OBC_STATE_REMOTE_CONTROL, GSD); - LogMessage(LOG_LEVEL_INFO, "RCCM message changed state to REMOTE_CONTROL."); - LOG_SEND(LogBuffer, "[ObjectControl] RCCM message changed state to REMOTE_CONTROL."); + printf("command RCMM!\n"); + if(vGetState(GSD) == OBC_STATE_CONNECTED){ + if(pcRecvBuffer[0] == REQ_STATE_CHANGE_REMOTE_CONTROL) { + for(iIndex=0;iIndex Date: Thu, 14 May 2020 09:53:27 +0200 Subject: [PATCH 485/523] Removed annoying printout --- modules/ScenarioControl/src/booleantrigger.cpp | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/modules/ScenarioControl/src/booleantrigger.cpp b/modules/ScenarioControl/src/booleantrigger.cpp index 8320a7791..dc60c51ae 100644 --- a/modules/ScenarioControl/src/booleantrigger.cpp +++ b/modules/ScenarioControl/src/booleantrigger.cpp @@ -10,13 +10,7 @@ Trigger::TriggerReturnCode_t BooleanTrigger::update(bool currentStateValue, stru { wasStateTrue = isStateTrue; isStateTrue = currentStateValue; - wasTriggeredByLastUpdate = checkIfTriggered(); - if (wasTriggeredByLastUpdate == TRIGGER_OCCURRED) { - LogPrint("was triggered"); - } - else if (wasTriggeredByLastUpdate == NO_TRIGGER_OCCURRED) { - LogPrint("no trigger"); - } + wasTriggeredByLastUpdate = checkIfTriggered(); return wasTriggeredByLastUpdate; } From 82ede2057975bd1173c7770539c3b4c03c790ff1 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 14 May 2020 10:13:03 +0200 Subject: [PATCH 486/523] Updated util ref --- util | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/util b/util index 68f0c384d..c5dfca213 160000 --- a/util +++ b/util @@ -1 +1 @@ -Subproject commit 68f0c384de24cdff9a0b73d26425577c8a244aa0 +Subproject commit c5dfca2134ae7fdb9e080b53ab046b72c4091df4 From ded82cc75c9d6d4ea2207aea248eabea6bf663d2 Mon Sep 17 00:00:00 2001 From: Sebastian Loh Lindholm Date: Thu, 14 May 2020 11:23:44 +0200 Subject: [PATCH 487/523] Fixing the commands on MSCP and MessageQueue. --- core/inc/util.h | 2 +- core/src/objectcontrol.c | 43 ++++++++++++++++++++++++---------------- core/src/systemcontrol.c | 29 +++++++++++++++++---------- 3 files changed, 46 insertions(+), 28 deletions(-) diff --git a/core/inc/util.h b/core/inc/util.h index 2b27bdc60..ea14d2a46 100644 --- a/core/inc/util.h +++ b/core/inc/util.h @@ -99,7 +99,7 @@ extern "C"{ #define MAX_ADAPTIVE_SYNC_POINTS 512 #define USE_LOCAL_USER_CONTROL 0 -#define LOCAL_USER_CONTROL_IP "10.114.98.26" +#define LOCAL_USER_CONTROL_IP "195.0.0.10" #define USE_TEST_HOST 0 #define TESTHOST_IP LOCAL_USER_CONTROL_IP #define TESTSERVER_IP LOCAL_USER_CONTROL_IP diff --git a/core/src/objectcontrol.c b/core/src/objectcontrol.c index 0436e9e43..1a69e5e56 100644 --- a/core/src/objectcontrol.c +++ b/core/src/objectcontrol.c @@ -580,25 +580,34 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { } else if(iCommand == COMM_RCMM) { - printf("command RCMM!\n"); + + U32 IpU32 = pcRecvBuffer[0]; + IpU32 = (IpU32 << 8) | pcRecvBuffer[1]; + IpU32 = (IpU32 << 8) | pcRecvBuffer[2]; + IpU32 = (IpU32 << 8) | pcRecvBuffer[3]; + printf("RCMM to IP = "); + for(i = 0; i < 4; i ++) printf("%d.", pcRecvBuffer[i]); + printf(" Command %d\n", pcRecvBuffer[4]); + + if(vGetState(GSD) == OBC_STATE_CONNECTED){ - if(pcRecvBuffer[0] == REQ_STATE_CHANGE_REMOTE_CONTROL) { + if(pcRecvBuffer[4] == REQ_STATE_CHANGE_REMOTE_CONTROL) { for(iIndex=0;iIndex> 24); + pcBuffer[1] = (U8)(IpU32 >> 16); + pcBuffer[2] = (U8)(IpU32 >> 8); + pcBuffer[3] = (U8)(IpU32) ; + if(atoi(SystemControlArgument[1]) == 1) pcBuffer[4] = REQ_STATE_CHANGE_REMOTE_CONTROL; + else if(atoi(SystemControlArgument[1]) == 2) pcBuffer[4] = REQ_STATE_CHANGE_DISARMED; + iCommSend(COMM_RCMM, pcBuffer, 5); } else SystemControlSendLog("[SystemControl] Set state to REMOTE_CONTROL failed, state errors!\n", &ClientSocket, 0); @@ -1023,11 +1027,16 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { if(server_state == SERVER_STATE_IDLE && objectControlState == OBC_STATE_CONNECTED) { bzero(pcBuffer, IPC_BUFFER_SIZE); - if(atoi(SystemControlArgument[0]) == 3) pcBuffer[0] = REQ_RCMM_BACK_TO_START; - else if (atoi(SystemControlArgument[0]) == 4) pcBuffer[0] = REQ_RCMM_ENABLE_AUTO_START; - else if (atoi(SystemControlArgument[0]) == 5) pcBuffer[0] = REQ_RCMM_DISABLE_AUTO_START; - strcpy(pcBuffer+1, SystemControlArgument[1]); - iCommSend(COMM_RCMM, pcBuffer, 1 + strlen(SystemControlArgument[1])); + IpU32 = UtilIPStringToInt(SystemControlArgument[0]); + pcBuffer[0] = (U8)(IpU32 >> 24); + pcBuffer[1] = (U8)(IpU32 >> 16); + pcBuffer[2] = (U8)(IpU32 >> 8); + pcBuffer[3] = (U8)(IpU32) ; + + if(atoi(SystemControlArgument[1]) == 3) pcBuffer[4] = REQ_RCMM_BACK_TO_START; + else if (atoi(SystemControlArgument[1]) == 4) pcBuffer[4] = REQ_RCMM_ENABLE_AUTO_START; + else if (atoi(SystemControlArgument[1]) == 5) pcBuffer[4] = REQ_RCMM_DISABLE_AUTO_START; + iCommSend(COMM_RCMM, pcBuffer, 5); } else SystemControlSendLog("[SystemControl] Remote control ACTION failed, state errors!\n", &ClientSocket, 0); } From 636de22ae0daafe395ee2984109f7e7414bb0d8c Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 14 May 2020 11:41:55 +0200 Subject: [PATCH 488/523] Added remote control mq struct and OBC state remote control --- core/inc/util.h | 20 +++++--- core/src/systemcontrol.c | 105 +++++++++++++++++++++++++++------------ core/src/util.c | 14 ++++-- 3 files changed, 98 insertions(+), 41 deletions(-) diff --git a/core/inc/util.h b/core/inc/util.h index 2b27bdc60..c58ef9618 100644 --- a/core/inc/util.h +++ b/core/inc/util.h @@ -196,12 +196,6 @@ extern "C"{ #define DD_CONTROL_BUFFER_SIZE_52 52 #define DD_CONTROL_TASK_PERIOD_MS 1 -#define REQ_STATE_CHANGE_REMOTE_CONTROL 1 -#define REQ_STATE_CHANGE_DISARMED 2 -#define REQ_RCMM_BACK_TO_START 3 -#define REQ_RCMM_ENABLE_AUTO_START 4 -#define REQ_RCMM_DISABLE_AUTO_START 5 - //! Internal message queue communication identifiers enum COMMAND { @@ -229,13 +223,24 @@ COMM_TREO = 23, COMM_ACCM = 24, COMM_TRCM = 25, COMM_DISARM = 26, -COMM_RCMM = 27, +COMM_REMOTECTRL_ENABLE = 27, +COMM_REMOTECTRL_DISABLE = 28, +COMM_REMOTECTRL_MANOEUVRE = 29, COMM_MONR = 239, COMM_OBJECTS_CONNECTED = 111, COMM_FAILURE = 254, COMM_INV = 255 }; +typedef enum { + MANOEUVRE_BACK_TO_START +} RemoteControlManoeuvreType; + +typedef struct { + RemoteControlManoeuvreType manoeuvre; + in_addr_t objectIP; +} RemoteControlCommandType; + typedef struct { double Latitude; @@ -325,6 +330,7 @@ typedef enum { OBC_STATE_CONNECTED, OBC_STATE_ARMED, OBC_STATE_RUNNING, + OBC_STATE_REMOTECTRL, OBC_STATE_ERROR } OBCState_t; diff --git a/core/src/systemcontrol.c b/core/src/systemcontrol.c index c018a1bf5..c2be536ee 100644 --- a/core/src/systemcontrol.c +++ b/core/src/systemcontrol.c @@ -116,6 +116,9 @@ typedef struct { #define RVSS_MAESTRO_CHANNEL 4 #define RVSS_ASP_CHANNEL 8 +#define ENABLE_COMMAND_STRING "ENABLE" +#define DISABLE_COMMAND_STRING "DISABLE" + // Time intervals for sleeping when no message bus message was received and for when one was received #define SC_SLEEP_TIME_EMPTY_MQ_S 0 #define SC_SLEEP_TIME_EMPTY_MQ_NS 10000000 @@ -135,7 +138,7 @@ typedef enum { SetServerParameter_2, GetServerParameter_1, DownloadFile_1, UploadFile_4, CheckFileDirectoryExist_1, GetRootDirectoryContent_0, GetDirectoryContent_1, DeleteTrajectory_1, DeleteGeofence_1, DeleteFileDirectory_1, - ClearTrajectories_0, ClearGeofences_0, SetObjectState_2, SetObjectAction_2, + ClearTrajectories_0, ClearGeofences_0, RemoteControl_1, RemoteControlManoeuvre_2, CreateDirectory_1, GetTestOrigin_0, replay_1, control_0, Exit_0, start_ext_trigg_1, nocommand } SystemControlCommand_t; @@ -147,7 +150,7 @@ static const char *SystemControlCommandsArr[] = { "GetServerParameter_1", "DownloadFile_1", "UploadFile_4", "CheckFileDirectoryExist_1", "GetRootDirectoryContent_0", "GetDirectoryContent_1", "DeleteTrajectory_1", "DeleteGeofence_1", "DeleteFileDirectory_1", - "ClearTrajectories_0", "ClearGeofences_0", "SetObjectState_2", "SetObjectAction_2", + "ClearTrajectories_0", "ClearGeofences_0", "RemoteControl_1", "RemoteControlManoeuvre_2", "CreateDirectory_1", "GetTestOrigin_0", "replay_1", "control_0", "Exit_0", "start_ext_trigg_1" @@ -160,6 +163,10 @@ const char *SystemControlOBCStatesArr[] = const char *POSTRequestMandatoryContent[] = { "POST", "HTTP/1.1", "\r\n\r\n" }; +typedef enum { + MSCP_BACK_TO_START = 3 +} MSCPRemoteControlCommand; + char SystemControlCommandArgCnt[SYSTEM_CONTROL_ARG_CHAR_COUNT]; char SystemControlStrippedCommand[SYSTEM_CONTROL_COMMAND_MAX_LENGTH]; char SystemControlArgument[SYSTEM_CONTROL_ARG_MAX_COUNT][SYSTEM_CONTROL_ARGUMENT_MAX_LENGTH]; @@ -246,7 +253,7 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { OBCState_t objectControlState = OBC_STATE_UNDEFINED; SystemControlCommand_t SystemControlCommand = Idle_0; SystemControlCommand_t PreviousSystemControlCommand = Idle_0; - + uint16_t responseCode = SYSTEM_CONTROL_RESPONSE_CODE_ERROR; int CommandArgCount = 0, /*CurrentCommandArgCounter=0, */ CurrentInputArgCount = 0; C8 pcBuffer[IPC_BUFFER_SIZE]; char inchr; @@ -1000,41 +1007,77 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { SystemControlCommand = PreviousSystemControlCommand; } break; - case SetObjectState_2: + case RemoteControl_1: + responseCode = SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE; if (CurrentInputArgCount == CommandArgCount) { - if(server_state == SERVER_STATE_IDLE && objectControlState == OBC_STATE_CONNECTED) - { - bzero(pcBuffer, IPC_BUFFER_SIZE); - if(atoi(SystemControlArgument[0]) == 1) pcBuffer[0] = REQ_STATE_CHANGE_REMOTE_CONTROL; - else if(atoi(SystemControlArgument[0]) == 2) pcBuffer[0] = REQ_STATE_CHANGE_DISARMED; - strcpy(pcBuffer+1, SystemControlArgument[1]); - printf("Set object state, %s, %s.\n", SystemControlArgument[0], SystemControlArgument[1]); - iCommSend(COMM_RCMM, pcBuffer, 1 + strlen(SystemControlArgument[1])); - } else - SystemControlSendLog("[SystemControl] Set state to REMOTE_CONTROL failed, state errors!\n", &ClientSocket, 0); - + if(server_state == SERVER_STATE_IDLE + && (objectControlState == OBC_STATE_CONNECTED || objectControlState == OBC_STATE_REMOTECTRL)) { + if (!strcasecmp(SystemControlArgument[0], ENABLE_COMMAND_STRING) + && objectControlState == OBC_STATE_CONNECTED) { + LogMessage(LOG_LEVEL_INFO, "Requesting enabling of remote control"); + iCommSend(COMM_REMOTECTRL_ENABLE, NULL, 0); // TODO check return value + responseCode = SYSTEM_CONTROL_RESPONSE_CODE_OK; + } + else if (!strcasecmp(SystemControlArgument[0], DISABLE_COMMAND_STRING) + && objectControlState == OBC_STATE_REMOTECTRL) { + LogMessage(LOG_LEVEL_INFO, "Requesting disabling of remote control"); + iCommSend(COMM_REMOTECTRL_DISABLE, NULL, 0); // TODO check return value + responseCode = SYSTEM_CONTROL_RESPONSE_CODE_OK; + } + else { + LogMessage(LOG_LEVEL_WARNING, "Incorrect remote control command"); + responseCode = SYSTEM_CONTROL_RESPONSE_CODE_ERROR; + } + } + else { + responseCode = SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE; + } + + } + else { + LogMessage(LOG_LEVEL_WARNING, "Remote control command parameter count error"); + responseCode = SYSTEM_CONTROL_RESPONSE_CODE_ERROR; } - else - LogMessage(LOG_LEVEL_WARNING, "SetRemoteControlState command parameter count error"); SystemControlCommand = Idle_0; + SystemControlSendControlResponse(responseCode, "RemoteControl:", + ControlResponseBuffer, 0, &ClientSocket, 0); break; - case SetObjectAction_2: + case RemoteControlManoeuvre_2: if (CurrentInputArgCount == CommandArgCount) { - if(server_state == SERVER_STATE_IDLE && objectControlState == OBC_STATE_CONNECTED) - { - bzero(pcBuffer, IPC_BUFFER_SIZE); - if(atoi(SystemControlArgument[0]) == 3) pcBuffer[0] = REQ_RCMM_BACK_TO_START; - else if (atoi(SystemControlArgument[0]) == 4) pcBuffer[0] = REQ_RCMM_ENABLE_AUTO_START; - else if (atoi(SystemControlArgument[0]) == 5) pcBuffer[0] = REQ_RCMM_DISABLE_AUTO_START; - strcpy(pcBuffer+1, SystemControlArgument[1]); - iCommSend(COMM_RCMM, pcBuffer, 1 + strlen(SystemControlArgument[1])); - } else - SystemControlSendLog("[SystemControl] Remote control ACTION failed, state errors!\n", &ClientSocket, 0); + if(server_state == SERVER_STATE_IDLE && objectControlState == OBC_STATE_REMOTECTRL) { + memset(pcBuffer, 0, sizeof (pcBuffer)); + RemoteControlCommandType rcCommand; + if (inet_pton(AF_INET, SystemControlArgument[0], &rcCommand.objectIP) != -1) { + responseCode = SYSTEM_CONTROL_RESPONSE_CODE_OK; + switch (atoi(SystemControlArgument[1])) { + case MSCP_BACK_TO_START: + rcCommand.manoeuvre = MANOEUVRE_BACK_TO_START; + break; + default: + responseCode = SYSTEM_CONTROL_RESPONSE_CODE_FUNCTION_NOT_AVAILABLE; + } + if (responseCode != SYSTEM_CONTROL_RESPONSE_CODE_FUNCTION_NOT_AVAILABLE) { + memcpy(pcBuffer, &rcCommand, sizeof (rcCommand)); + iCommSend(COMM_REMOTECTRL_MANOEUVRE, pcBuffer, sizeof (rcCommand)); // TODO check return value + responseCode = SYSTEM_CONTROL_RESPONSE_CODE_OK; + } + } + else { + responseCode = SYSTEM_CONTROL_RESPONSE_CODE_ERROR; + } + } + else { + responseCode = SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE; + } + } + else { + LogMessage(LOG_LEVEL_WARNING, "Remote control manoeuvre command parameter count error"); + responseCode = SYSTEM_CONTROL_RESPONSE_CODE_ERROR; } - else - LogMessage(LOG_LEVEL_WARNING, "SetRemoteControlAction command parameter count error"); SystemControlCommand = Idle_0; - break; + SystemControlSendControlResponse(responseCode, "RemoteControlManoeuvre:", + ControlResponseBuffer, 0, &ClientSocket, 0); + break; case StartScenario_1: if (CurrentInputArgCount == CommandArgCount) { if (server_state == SERVER_STATE_IDLE && objectControlState == OBC_STATE_ARMED) //Temporary! diff --git a/core/src/util.c b/core/src/util.c index 775e9772d..c667b2676 100644 --- a/core/src/util.c +++ b/core/src/util.c @@ -75,13 +75,15 @@ #define PRIO_COMM_INIT 16 #define PRIO_COMM_CONNECT 16 #define PRIO_COMM_DISCONNECT 16 +#define PRIO_COMM_REMOTECTRL_ENABLE 16 +#define PRIO_COMM_REMOTECTRL_DISABLE 16 // Single-shot messages relevant during test run #define PRIO_COMM_EXAC 14 #define PRIO_COMM_TREO 14 // Frequent messages relevant during test run #define PRIO_COMM_TRAJ_TOSUP 12 #define PRIO_COMM_TRAJ_FROMSUP 12 -#define PRIO_COMM_RCMM 12 +#define PRIO_COMM_REMOTECTRL_MANOEUVRE 12 // Logging #define PRIO_COMM_LOG 10 // Unused messages TODO: double check the priority of unused messages @@ -2059,8 +2061,14 @@ int iCommSend(const enum COMMAND iCommand, const char *cpData, size_t dataLength case COMM_OBJECTS_CONNECTED: uiMessagePrio = PRIO_OBJECTS_CONNECTED; break; - case COMM_RCMM: - uiMessagePrio = PRIO_COMM_RCMM; + case COMM_REMOTECTRL_ENABLE: + uiMessagePrio = PRIO_COMM_REMOTECTRL_ENABLE; + break; + case COMM_REMOTECTRL_DISABLE: + uiMessagePrio = PRIO_COMM_REMOTECTRL_DISABLE; + break; + case COMM_REMOTECTRL_MANOEUVRE: + uiMessagePrio = PRIO_COMM_REMOTECTRL_MANOEUVRE; break; case COMM_FAILURE: uiMessagePrio = PRIO_COMM_FAILURE; From 0cbc2f15d01d55c9f7dd91aea1b4647dc052f3f0 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 14 May 2020 13:35:58 +0200 Subject: [PATCH 489/523] Updated object control with new state handling and some error catching --- core/inc/iso22133.h | 8 ++- core/inc/util.h | 4 -- core/src/iso22133.c | 42 +++++++------ core/src/objectcontrol.c | 123 +++++++++++++++++++++------------------ 4 files changed, 93 insertions(+), 84 deletions(-) diff --git a/core/inc/iso22133.h b/core/inc/iso22133.h index 013b0625c..853592b44 100644 --- a/core/inc/iso22133.h +++ b/core/inc/iso22133.h @@ -186,6 +186,12 @@ typedef enum { MESSAGE_ID_VENDOR_SPECIFIC_RISE_INSUP = 0xA102 } ISOMessageID; +/*! Remote control command */ + +typedef enum { + MANOEUVRE_BACK_TO_START = 3 +} RemoteControlManoeuvreType; + /*! Supervisor command */ typedef enum { SUPERVISOR_COMMAND_NORMAL = 1, //!< Place supervisor in normal mode @@ -207,7 +213,7 @@ ssize_t decodeTREOMessage(); ssize_t encodeACCMMessage(const uint16_t* actionID, const ActionType_t* actionType, const ActionTypeParameter_t* param1, const ActionTypeParameter_t* param2, const 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 encodeINSUPMessage(const SupervisorCommandType, char * insupDataBuffer, const size_t bufferLength, const char debug); -ssize_t encodeRCMMMessage(const uint8_t rcmmControlCommand, char *rcmmDataBuffer, const size_t bufferLength, const char debug); +ssize_t encodeRCMMMessage(const RemoteControlManoeuvreType command, char *rcmmDataBuffer, const size_t bufferLength, const char debug); ISOMessageID getISOMessageType(const char * messageData, const size_t length, const char debug); void setISOCRCVerification(const int8_t enabled); diff --git a/core/inc/util.h b/core/inc/util.h index 5927941ce..0f0c0400d 100644 --- a/core/inc/util.h +++ b/core/inc/util.h @@ -232,10 +232,6 @@ COMM_FAILURE = 254, COMM_INV = 255 }; -typedef enum { - MANOEUVRE_BACK_TO_START -} RemoteControlManoeuvreType; - typedef struct { RemoteControlManoeuvreType manoeuvre; in_addr_t objectIP; diff --git a/core/src/iso22133.c b/core/src/iso22133.c index 0e3392c45..781a43804 100644 --- a/core/src/iso22133.c +++ b/core/src/iso22133.c @@ -426,17 +426,16 @@ typedef struct { /*! RCMM message */ typedef struct { HeaderType header; - uint16_t RCMMControlValueID; - uint16_t RCMMControlContentLength; - uint8_t rcmmControlU8; + uint16_t commandValueID; + uint16_t commandContentLength; + uint8_t command; FooterType footer; -} RCMMType; //18 bytes +} RCMMType; -//! HEAB value IDs +//! RCMM value IDs #define VALUE_ID_RCMM_CONTROL 0x0201 - #pragma pack(pop) @@ -1475,14 +1474,14 @@ ssize_t encodeHEABMessage(const ControlCenterStatusType status, char *heabDataBu /*! - * \brief encodeRCMMMessage - * \param - * \param - * \param - * \param + * \brief encodeRCMMMessage Constructs an ISO RCMM message based on an input ::RemoteControlManoeuvreType + * \param command The command to be sent to an object + * \param rcmmDataBuffer Buffer to which RCMM message is to be written + * \param bufferLength Size of buffer to which RCMM message is to be written + * \param debug Flag for enabling debugging * \return Number of bytes written or -1 in case of an error */ -ssize_t encodeRCMMMessage(const uint8_t rcmmControlCommand, char *rcmmDataBuffer, +ssize_t encodeRCMMMessage(const RemoteControlManoeuvreType command, char *rcmmDataBuffer, const size_t bufferLength, const char debug) { RCMMType RCMMData; @@ -1499,21 +1498,20 @@ ssize_t encodeRCMMMessage(const uint8_t rcmmControlCommand, char *rcmmDataBuffer RCMMData.header = buildISOHeader(MESSAGE_ID_RCMM, sizeof (RCMMData), debug); // Fill contents - RCMMData.RCMMControlValueID = VALUE_ID_RCMM_CONTROL; - RCMMData.RCMMControlContentLength = 1; - RCMMData.rcmmControlU8 = rcmmControlCommand; - + RCMMData.commandValueID = VALUE_ID_RCMM_CONTROL; + RCMMData.commandContentLength = sizeof (RCMMData.command); + RCMMData.command = (uint8_t) command; if (debug) { - LogPrint("RCMM message:\n\tRCMM Control value ID: 0x%x\n\t" - "RCMM Control content length: %u\n\t" - "Control center status: %d", RCMMData.RCMMControlValueID, RCMMData.RCMMControlContentLength, - RCMMData.rcmmControlU8); + LogPrint("RCMM message:\n\tCommand value ID: 0x%x\n\t" + "Command content length: %u\n\t" + "Command: %u", RCMMData.commandValueID, RCMMData.commandContentLength, + RCMMData.command); } // Switch from host endianness to little endian - RCMMData.RCMMControlValueID = htole16(RCMMData.RCMMControlValueID); - RCMMData.RCMMControlContentLength = htole16(RCMMData.RCMMControlContentLength); + RCMMData.commandValueID = htole16(RCMMData.commandValueID); + RCMMData.commandContentLength = htole16(RCMMData.commandContentLength); RCMMData.footer = buildISOFooter(&RCMMData, sizeof (RCMMData), debug); diff --git a/core/src/objectcontrol.c b/core/src/objectcontrol.c index 1a69e5e56..b71fc3119 100644 --- a/core/src/objectcontrol.c +++ b/core/src/objectcontrol.c @@ -122,6 +122,7 @@ StateTransitionResult tFromInitialized(OBCState_t * currentState, OBCState_t req StateTransitionResult tFromConnected(OBCState_t * currentState, OBCState_t requestedState); StateTransitionResult tFromArmed(OBCState_t * currentState, OBCState_t requestedState); StateTransitionResult tFromRunning(OBCState_t * currentState, OBCState_t requestedState); +StateTransitionResult tFromRemoteControl(OBCState_t * currentState, OBCState_t requestedState); StateTransitionResult tFromError(OBCState_t * currentState, OBCState_t requestedState); StateTransitionResult tFromUndefined(OBCState_t * currentState, OBCState_t requestedState); @@ -578,67 +579,64 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { ObjectcontrolExecutionMode = OBJECT_CONTROL_CONTROL_MODE; printf("[ObjectControl] Object control in CONTROL mode\n"); } - else if(iCommand == COMM_RCMM) - { - - U32 IpU32 = pcRecvBuffer[0]; - IpU32 = (IpU32 << 8) | pcRecvBuffer[1]; - IpU32 = (IpU32 << 8) | pcRecvBuffer[2]; - IpU32 = (IpU32 << 8) | pcRecvBuffer[3]; - printf("RCMM to IP = "); - for(i = 0; i < 4; i ++) printf("%d.", pcRecvBuffer[i]); - printf(" Command %d\n", pcRecvBuffer[4]); - - - if(vGetState(GSD) == OBC_STATE_CONNECTED){ - if(pcRecvBuffer[4] == REQ_STATE_CHANGE_REMOTE_CONTROL) { - for(iIndex=0;iIndex 0) { + UtilSendTCPData(MODULE_NAME, MessageBuffer, MessageLength, &socket_fds[iIndex], 0); } - } - } - } - if(vGetState(GSD) == OBC_STATE_CONNECTED){ - if(pcRecvBuffer[4] == REQ_STATE_CHANGE_DISARMED) { - for(iIndex=0;iIndex Date: Thu, 14 May 2020 15:36:50 +0200 Subject: [PATCH 490/523] Added IP printouts --- modules/ScenarioControl/inc/action.h | 1 + modules/ScenarioControl/inc/trigger.h | 1 + modules/ScenarioControl/src/action.cpp | 14 ++++++++++++++ modules/ScenarioControl/src/scenario.cpp | 4 ++-- modules/ScenarioControl/src/trigger.cpp | 12 ++++++++++++ 5 files changed, 30 insertions(+), 2 deletions(-) diff --git a/modules/ScenarioControl/inc/action.h b/modules/ScenarioControl/inc/action.h index 79856efc9..2fafb620f 100644 --- a/modules/ScenarioControl/inc/action.h +++ b/modules/ScenarioControl/inc/action.h @@ -55,6 +55,7 @@ class Action std::string getParametersString(void) const; ACCMData getConfigurationMessageData(void) const; in_addr_t getObjectIP(void) const { return actionObjectIP; } + std::string getObjectIPAsString(void) const; void setObjectIP(in_addr_t ipAddr) { actionObjectIP = ipAddr; } void setExecuteDelayTime(struct timeval tm); diff --git a/modules/ScenarioControl/inc/trigger.h b/modules/ScenarioControl/inc/trigger.h index 86bf8c6ba..718e76ae6 100644 --- a/modules/ScenarioControl/inc/trigger.h +++ b/modules/ScenarioControl/inc/trigger.h @@ -44,6 +44,7 @@ class Trigger std::vector getParameters() const { return parameters; } bool isActive() const; in_addr_t getObjectIP(void) const { return triggerObjectIP; } + std::string getObjectIPAsString(void) const; bool operator==(const Trigger &other) const { return (other.triggerID == triggerID) && isSimilar(other); } bool isSimilar(const Trigger &other) const; diff --git a/modules/ScenarioControl/src/action.cpp b/modules/ScenarioControl/src/action.cpp index fabba618c..5f25d278d 100644 --- a/modules/ScenarioControl/src/action.cpp +++ b/modules/ScenarioControl/src/action.cpp @@ -21,6 +21,20 @@ Action::Action(ActionID_t actionID, ActionTypeCode_t actionType, uint32_t allowe this->remainingAllowedRuns = allowedNumberOfRuns; } + +/*! + * \brief Action::getObjectIPAsString Creates a string from the object IP + * \return A string representation of the IP + */ +std::string Action::getObjectIPAsString(void) const { + char ipAddress[INET_ADDRSTRLEN]; + if (inet_ntop(AF_INET, &this->actionObjectIP, ipAddress, sizeof (ipAddress)) != nullptr) { + return std::string(ipAddress); + } + return ""; +} + + /*! * \brief Action::execute Runs the action if allowed and decrements the number of remaining allowed action executions. * \return Value according to ::ActionReturnCode_t diff --git a/modules/ScenarioControl/src/scenario.cpp b/modules/ScenarioControl/src/scenario.cpp index ec85bbc8c..ae3e162b5 100644 --- a/modules/ScenarioControl/src/scenario.cpp +++ b/modules/ScenarioControl/src/scenario.cpp @@ -63,11 +63,11 @@ void Scenario::initialize(const std::string scenarioFilePath) debugStr = "Triggers:\n"; for (Trigger* tp : allTriggers) - debugStr += "\t" + tp->getTypeAsString(tp->getTypeCode()) + "\n"; + debugStr += "\t" + tp->getObjectIPAsString() + " " + tp->getTypeAsString(tp->getTypeCode()) + "\n"; debugStr += "Actions:\n"; for (Action* ap : allActions) - debugStr += "\t" + ap->getTypeAsString(ap->getTypeCode()) + "\n"; + debugStr += "\t" + ap->getObjectIPAsString() + " " + ap->getTypeAsString(ap->getTypeCode()) + "\n"; debugStr.pop_back(); LogMessage(LOG_LEVEL_DEBUG, debugStr.c_str()); diff --git a/modules/ScenarioControl/src/trigger.cpp b/modules/ScenarioControl/src/trigger.cpp index 47ca03cfb..bb0b80c10 100644 --- a/modules/ScenarioControl/src/trigger.cpp +++ b/modules/ScenarioControl/src/trigger.cpp @@ -1,3 +1,4 @@ +#include #include "trigger.h" #include "logging.h" @@ -13,6 +14,17 @@ Trigger::~Trigger() parameters.clear(); } +/*! + * \brief Trigger::getObjectIPAsString Creates a string from the object IP + * \return A string representation of the IP + */ +std::string Trigger::getObjectIPAsString(void) const { + char ipAddress[INET_ADDRSTRLEN]; + if (inet_ntop(AF_INET, &this->triggerObjectIP, ipAddress, sizeof (ipAddress)) != nullptr) { + return std::string(ipAddress); + } + return ""; +} /*! * \brief Trigger::checkTriggerParameter Checks if the queried parameter is within the list of accepted parameters From ba446ef6841e7089aed4ee3c9c1ae1bae7f7fd92 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 14 May 2020 15:38:39 +0200 Subject: [PATCH 491/523] Niced up the printout --- modules/ScenarioControl/src/scenario.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/modules/ScenarioControl/src/scenario.cpp b/modules/ScenarioControl/src/scenario.cpp index ae3e162b5..a63ed5966 100644 --- a/modules/ScenarioControl/src/scenario.cpp +++ b/modules/ScenarioControl/src/scenario.cpp @@ -61,13 +61,13 @@ void Scenario::initialize(const std::string scenarioFilePath) } LogMessage(LOG_LEVEL_INFO, "Successfully initialized scenario with %d unique triggers and %d unique actions", allTriggers.size(), allActions.size()); - debugStr = "Triggers:\n"; + debugStr = "\nTriggers:\n"; for (Trigger* tp : allTriggers) - debugStr += "\t" + tp->getObjectIPAsString() + " " + tp->getTypeAsString(tp->getTypeCode()) + "\n"; + debugStr += "\t" + tp->getObjectIPAsString() + "\t" + tp->getTypeAsString(tp->getTypeCode()) + "\n"; debugStr += "Actions:\n"; for (Action* ap : allActions) - debugStr += "\t" + ap->getObjectIPAsString() + " " + ap->getTypeAsString(ap->getTypeCode()) + "\n"; + debugStr += "\t" + ap->getObjectIPAsString() + "\t" + ap->getTypeAsString(ap->getTypeCode()) + "\n"; debugStr.pop_back(); LogMessage(LOG_LEVEL_DEBUG, debugStr.c_str()); From 8a8f70c1378ec51a5ba5eb663443eff50dc0a82b Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 15 May 2020 09:44:04 +0200 Subject: [PATCH 492/523] Updated util ref --- util | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/util b/util index e73444774..89a4cd648 160000 --- a/util +++ b/util @@ -1 +1 @@ -Subproject commit e7344477470c3ac90449e90c0bde86e1261372ad +Subproject commit 89a4cd6485f99d23e2a73194a03a047e0da8709a From f5b4ff02ef1bd81d571e1bdabc6c62389961fbf3 Mon Sep 17 00:00:00 2001 From: Viktor Johansson Date: Mon, 18 May 2020 11:11:01 +0200 Subject: [PATCH 493/523] added trigger file to logging --- core/src/logger.c | 23 ++++++++++++++++++----- 1 file changed, 18 insertions(+), 5 deletions(-) diff --git a/core/src/logger.c b/core/src/logger.c index 6afd8a348..fa551b8e9 100644 --- a/core/src/logger.c +++ b/core/src/logger.c @@ -342,6 +342,8 @@ void vInitializeLog(char *logFilePath, unsigned int filePathLength, char *csvLog char logFileDirectoryPath[MAX_FILE_PATH]; char trajPathDir[MAX_FILE_PATH]; char confPathDir[MAX_FILE_PATH]; + char confFilePath[MAX_FILE_PATH]; + char trigFilePath[MAX_FILE_PATH]; char journalPathDir[MAX_FILE_PATH]; char DateBuffer[FILENAME_MAX]; FILE *filefd, *fileread; @@ -353,7 +355,10 @@ void vInitializeLog(char *logFilePath, unsigned int filePathLength, char *csvLog int read; UtilGetConfDirectoryPath(confPathDir, sizeof (confPathDir)); - strcat(confPathDir, CONF_FILE_NAME); + strcat(confFilePath, confPathDir); + strcat(confFilePath, CONF_FILE_NAME); + strcat(trigFilePath, confPathDir); + strcat(trigFilePath, TRIGGER_ACTION_FILE_NAME); UtilGetTrajDirectoryPath(trajPathDir, sizeof (trajPathDir)); UtilGetJournalDirectoryPath(journalPathDir, sizeof (journalPathDir)); @@ -380,11 +385,19 @@ void vInitializeLog(char *logFilePath, unsigned int filePathLength, char *csvLog // Copy configuration file to log directory LogMessage(LOG_LEVEL_INFO, "Copying configuration file to log directory"); (void)strcpy(sysCommand, "cp "); - (void)strcat(sysCommand, confPathDir); + (void)strcat(sysCommand, confFilePath); (void)strcat(sysCommand, " "); (void)strcat(sysCommand, logFileDirectoryPath); (void)system(sysCommand); + // Copy configuration file to log directory + LogMessage(LOG_LEVEL_INFO, "Copying TriggerAndAction file to log directory"); + (void)strcpy(sysCommand, "cp "); + (void)strcat(sysCommand, trigFilePath); + (void)strcat(sysCommand, " "); + (void)strcat(sysCommand, logFileDirectoryPath); + (void)system(sysCommand); + // Check if ./traj directory exists if ((dir = opendir(trajPathDir)) == NULL) { sprintf(sysCommand, "Unable to open traj directory <%s>", trajPathDir); @@ -461,9 +474,9 @@ void vInitializeLog(char *logFilePath, unsigned int filePathLength, char *csvLog /* If file conf file exists and we have read permission do */ - if (access(confPathDir, 0) == 0) { + if (access(confFilePath, 0) == 0) { /*read the .conf file and print it in to the .log file */ - fileread = fopen(confPathDir, ACCESS_MODE_READ); + fileread = fopen(confFilePath, ACCESS_MODE_READ); read = fgetc(fileread); while (read != EOF) { fputc(read, filefd); @@ -472,7 +485,7 @@ void vInitializeLog(char *logFilePath, unsigned int filePathLength, char *csvLog fclose(fileread); } else { - sprintf(sysCommand, "Unable to open <%s>", confPathDir); + sprintf(sysCommand, "Unable to open <%s>", confFilePath); util_error(sysCommand); } From 0287a09f367b7fa87e60b5ef8d4fdff2a30d1053 Mon Sep 17 00:00:00 2001 From: Jesper Blidkvist Date: Mon, 18 May 2020 13:01:37 +0200 Subject: [PATCH 494/523] Fixed state printout --- core/src/systemcontrol.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/core/src/systemcontrol.c b/core/src/systemcontrol.c index 7b54146ac..c922bc43e 100644 --- a/core/src/systemcontrol.c +++ b/core/src/systemcontrol.c @@ -159,7 +159,7 @@ static const char *SystemControlCommandsArr[] = { const char *SystemControlStatesArr[] = { "UNDEFINED", "INITIALIZED", "IDLE", "READY", "RUNNING", "INWORK", "ERROR" }; const char *SystemControlOBCStatesArr[] = - { "UNDEFINED", "IDLE", "INITIALIZED", "CONNECTED", "ARMED", "RUNNING", "ERROR" }; + { "UNDEFINED", "IDLE", "INITIALIZED", "CONNECTED", "ARMED", "RUNNING", "REMOTECONTROL", "ERROR" }; const char *POSTRequestMandatoryContent[] = { "POST", "HTTP/1.1", "\r\n\r\n" }; From 42ca98ecd0607cd7215275534c534ea7428bf72f Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 18 May 2020 14:48:56 +0200 Subject: [PATCH 495/523] Made object control send HEAB and receive MONR in REMOTECTRL --- core/src/objectcontrol.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/core/src/objectcontrol.c b/core/src/objectcontrol.c index b71fc3119..076b50619 100644 --- a/core/src/objectcontrol.c +++ b/core/src/objectcontrol.c @@ -273,7 +273,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { // Heartbeat if ((vGetState(GSD) == OBC_STATE_RUNNING || vGetState(GSD) == OBC_STATE_ARMED - || vGetState(GSD) == OBC_STATE_CONNECTED) && timercmp(¤tTime, &nextHeartbeatTime, >)) { + || vGetState(GSD) == OBC_STATE_CONNECTED || vGetState(GSD) == OBC_STATE_REMOTECTRL) && timercmp(¤tTime, &nextHeartbeatTime, >)) { timeradd(&nextHeartbeatTime, &heartbeatPeriod, &nextHeartbeatTime); MessageLength = @@ -308,7 +308,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { } if (vGetState(GSD) == OBC_STATE_RUNNING || vGetState(GSD) == OBC_STATE_CONNECTED - || vGetState(GSD) == OBC_STATE_ARMED) { + || vGetState(GSD) == OBC_STATE_ARMED || vGetState(GSD) == OBC_STATE_REMOTECTRL) { char buffer[RECV_MESSAGE_BUFFER]; size_t receivedMONRData = 0; From 44717c1040693199cb4118b3f57e9c8eb152600e Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Mon, 18 May 2020 14:49:21 +0200 Subject: [PATCH 496/523] Updated scenariocontrol to reset scenario on ARM --- modules/ScenarioControl/inc/action.h | 4 ++++ modules/ScenarioControl/inc/scenario.h | 1 + modules/ScenarioControl/src/action.cpp | 8 +++++++- modules/ScenarioControl/src/main.cpp | 8 +++++++- modules/ScenarioControl/src/scenario.cpp | 6 ++++++ 5 files changed, 25 insertions(+), 2 deletions(-) diff --git a/modules/ScenarioControl/inc/action.h b/modules/ScenarioControl/inc/action.h index 36c3b0417..49bbc50db 100644 --- a/modules/ScenarioControl/inc/action.h +++ b/modules/ScenarioControl/inc/action.h @@ -43,6 +43,9 @@ class Action /*! Run the action once, if allowed */ virtual ActionReturnCode_t execute(void); + /*! Reset to start state */ + ActionReturnCode_t reset(void); + /*! To string */ friend std::ostream& operator<<(std::ostream &strm, const Action &act) { return strm << "ACTION ID " << act.actionID << @@ -69,6 +72,7 @@ class Action ActionTypeCode_t actionTypeCode = ACTION_NONE; ActionID_t actionID = 0; uint32_t remainingAllowedRuns = 0; + uint32_t maxAllowedRuns = 0; std::vector parameters; uint32_t actionDelayTime_qms = 0; in_addr_t actionObjectIP = 0; diff --git a/modules/ScenarioControl/inc/scenario.h b/modules/ScenarioControl/inc/scenario.h index 21b33cce3..ba4a57d7e 100644 --- a/modules/ScenarioControl/inc/scenario.h +++ b/modules/ScenarioControl/inc/scenario.h @@ -50,6 +50,7 @@ class Scenario void resetISOTriggers(void); void refresh(void) const; + void reset(void); void clear(void); ScenarioReturnCode_t updateTrigger(const MonitorDataType&); diff --git a/modules/ScenarioControl/src/action.cpp b/modules/ScenarioControl/src/action.cpp index fabba618c..731306cab 100644 --- a/modules/ScenarioControl/src/action.cpp +++ b/modules/ScenarioControl/src/action.cpp @@ -18,7 +18,8 @@ Action::Action(ActionID_t actionID, ActionTypeCode_t actionType, uint32_t allowe { this->actionID = actionID; this->actionTypeCode = actionType; - this->remainingAllowedRuns = allowedNumberOfRuns; + this->maxAllowedRuns = allowedNumberOfRuns; + this->remainingAllowedRuns = this->maxAllowedRuns; } /*! @@ -37,6 +38,11 @@ Action::ActionReturnCode_t Action::execute(void) } } +Action::ActionReturnCode_t Action::reset(void) { + remainingAllowedRuns = maxAllowedRuns; + return OK; +} + std::string Action::getParametersString() const { std::string retval; diff --git a/modules/ScenarioControl/src/main.cpp b/modules/ScenarioControl/src/main.cpp index 840be4d45..788dba5c6 100644 --- a/modules/ScenarioControl/src/main.cpp +++ b/modules/ScenarioControl/src/main.cpp @@ -116,8 +116,14 @@ int main() break; case COMM_ABORT: LogMessage(LOG_LEVEL_INFO, "Received abort command"); - if (state == RUNNING) state = CONNECTED; + if (state == RUNNING) { + state = CONNECTED; + } break; + case COMM_ARM: + LogMessage(LOG_LEVEL_INFO, "Resetting scenario"); + scenario.reset(); + break; case COMM_STRT: if (state == CONNECTED) { diff --git a/modules/ScenarioControl/src/scenario.cpp b/modules/ScenarioControl/src/scenario.cpp index cad299a6d..9ce210d18 100644 --- a/modules/ScenarioControl/src/scenario.cpp +++ b/modules/ScenarioControl/src/scenario.cpp @@ -71,6 +71,12 @@ void Scenario::initialize(const std::string scenarioFilePath) LogMessage(LOG_LEVEL_DEBUG, debugStr.c_str()); } +void Scenario::reset() { + for (Action* ap : allActions) { + ap->reset(); + } +} + /*! * \brief Scenario::sendConfiguration Sends TRCM and ACCM according to previously initialized scenario */ From b3581cd0a30e0da12b7a3341778f506c23f5faf5 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 26 May 2020 16:58:15 +0200 Subject: [PATCH 497/523] Updated util ref --- util | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/util b/util index 89a4cd648..2cdf6df52 160000 --- a/util +++ b/util @@ -1 +1 @@ -Subproject commit 89a4cd6485f99d23e2a73194a03a047e0da8709a +Subproject commit 2cdf6df528e2959e3aacfa3317cf1939a9c7e56d From 4726ee6289366317ac874737344ad96567bbfed5 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 26 May 2020 16:59:55 +0200 Subject: [PATCH 498/523] Ran code formatter --- core/src/iso22133.c | 3 +-- core/src/objectcontrol.c | 40 ++++++++++++++++++++++++++-------------- core/src/systemcontrol.c | 20 +++++++++++--------- 3 files changed, 38 insertions(+), 25 deletions(-) diff --git a/core/src/iso22133.c b/core/src/iso22133.c index 781a43804..6f985cb0e 100644 --- a/core/src/iso22133.c +++ b/core/src/iso22133.c @@ -1505,8 +1505,7 @@ ssize_t encodeRCMMMessage(const RemoteControlManoeuvreType command, char *rcmmDa if (debug) { LogPrint("RCMM message:\n\tCommand value ID: 0x%x\n\t" "Command content length: %u\n\t" - "Command: %u", RCMMData.commandValueID, RCMMData.commandContentLength, - RCMMData.command); + "Command: %u", RCMMData.commandValueID, RCMMData.commandContentLength, RCMMData.command); } // Switch from host endianness to little endian diff --git a/core/src/objectcontrol.c b/core/src/objectcontrol.c index 076b50619..50dc0135a 100644 --- a/core/src/objectcontrol.c +++ b/core/src/objectcontrol.c @@ -273,7 +273,8 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { // Heartbeat if ((vGetState(GSD) == OBC_STATE_RUNNING || vGetState(GSD) == OBC_STATE_ARMED - || vGetState(GSD) == OBC_STATE_CONNECTED || vGetState(GSD) == OBC_STATE_REMOTECTRL) && timercmp(¤tTime, &nextHeartbeatTime, >)) { + || vGetState(GSD) == OBC_STATE_CONNECTED || vGetState(GSD) == OBC_STATE_REMOTECTRL) + && timercmp(¤tTime, &nextHeartbeatTime, >)) { timeradd(&nextHeartbeatTime, &heartbeatPeriod, &nextHeartbeatTime); MessageLength = @@ -583,9 +584,11 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { vSetState(OBC_STATE_REMOTECTRL, GSD); // TODO: objectControlServerStatus = something MessageLength = - encodeOSTMMessage(OBJECT_COMMAND_REMOTE_CONTROL, MessageBuffer, sizeof (MessageBuffer), 0); + encodeOSTMMessage(OBJECT_COMMAND_REMOTE_CONTROL, MessageBuffer, sizeof (MessageBuffer), + 0); for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - LogMessage(LOG_LEVEL_INFO, "Setting object with IP %s to remote control mode", object_address_name[iIndex]); + LogMessage(LOG_LEVEL_INFO, "Setting object with IP %s to remote control mode", + object_address_name[iIndex]); UtilSendTCPData(MODULE_NAME, MessageBuffer, MessageLength, &socket_fds[iIndex], 0); } // TODO: check objects' states @@ -596,7 +599,8 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { MessageLength = encodeOSTMMessage(OBJECT_COMMAND_DISARM, MessageBuffer, sizeof (MessageBuffer), 0); for (iIndex = 0; iIndex < nbr_objects; ++iIndex) { - LogMessage(LOG_LEVEL_INFO, "Setting object with IP %s to disarmed mode", object_address_name[iIndex]); + LogMessage(LOG_LEVEL_INFO, "Setting object with IP %s to disarmed mode", + object_address_name[iIndex]); UtilSendTCPData(MODULE_NAME, MessageBuffer, MessageLength, &socket_fds[iIndex], 0); } // TODO: check objects' states @@ -604,22 +608,28 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { vSetState(OBC_STATE_CONNECTED, GSD); LogMessage(LOG_LEVEL_INFO, "Disabled remote control mode"); } - else if(iCommand == COMM_REMOTECTRL_MANOEUVRE) { + else if (iCommand == COMM_REMOTECTRL_MANOEUVRE) { RemoteControlCommandType rcCommand; char ipString[INET_ADDRSTRLEN]; + // TODO check size of received data memcpy(&rcCommand, pcRecvBuffer, sizeof (rcCommand)); - LogMessage(LOG_LEVEL_INFO, "Received remote control manoeuvre for object with IP %s", - inet_ntop(AF_INET, &rcCommand.objectIP, ipString, sizeof (ipString))); // TODO print command type + LogMessage(LOG_LEVEL_INFO, "Received remote control manoeuvre for object with IP %s", inet_ntop(AF_INET, &rcCommand.objectIP, ipString, sizeof (ipString))); // TODO print command type if (vGetState(GSD) == OBC_STATE_REMOTECTRL) { switch (rcCommand.manoeuvre) { - case MANOEUVRE_BACK_TO_START: - iIndex = iGetObjectIndexFromObjectIP(rcCommand.objectIP, objectIPs, sizeof (objectIPs) / sizeof (objectIPs[0])); + case MANOEUVRE_BACK_TO_START: + iIndex = + iGetObjectIndexFromObjectIP(rcCommand.objectIP, objectIPs, + sizeof (objectIPs) / sizeof (objectIPs[0])); if (iIndex != -1) { - LogMessage(LOG_LEVEL_INFO,"Sending back to start command to object with IP %s", object_address_name[iIndex]); - MessageLength = encodeRCMMMessage(rcCommand.manoeuvre, MessageBuffer, sizeof (MessageBuffer), 0); + LogMessage(LOG_LEVEL_INFO, "Sending back to start command to object with IP %s", + object_address_name[iIndex]); + MessageLength = + encodeRCMMMessage(rcCommand.manoeuvre, MessageBuffer, sizeof (MessageBuffer), + 0); if (MessageLength > 0) { - UtilSendTCPData(MODULE_NAME, MessageBuffer, MessageLength, &socket_fds[iIndex], 0); + UtilSendTCPData(MODULE_NAME, MessageBuffer, MessageLength, + &socket_fds[iIndex], 0); } else { LogMessage(LOG_LEVEL_ERROR, "Error encoding RCMM message"); @@ -627,6 +637,7 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { } else { char ipString[INET_ADDRSTRLEN]; + LogMessage(LOG_LEVEL_ERROR, "Back to start command for invalid IP %s received", inet_ntop(AF_INET, &rcCommand.objectIP, ipString, sizeof (ipString))); } @@ -636,7 +647,8 @@ void objectcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { } } else { - LogMessage(LOG_LEVEL_WARNING, "Remote control manoeuvring is not allowed outside of remote control mode"); + LogMessage(LOG_LEVEL_WARNING, + "Remote control manoeuvring is not allowed outside of remote control mode"); } } else if (iCommand == COMM_INIT) { @@ -1529,7 +1541,7 @@ StateTransitionResult tFromInitialized(OBCState_t * currentState, OBCState_t req StateTransitionResult tFromConnected(OBCState_t * currentState, OBCState_t requestedState) { if (requestedState == OBC_STATE_ARMED || requestedState == OBC_STATE_IDLE - || requestedState == OBC_STATE_REMOTECTRL) { + || requestedState == OBC_STATE_REMOTECTRL) { *currentState = requestedState; return TRANSITION_OK; } diff --git a/core/src/systemcontrol.c b/core/src/systemcontrol.c index c922bc43e..b35234558 100644 --- a/core/src/systemcontrol.c +++ b/core/src/systemcontrol.c @@ -159,7 +159,7 @@ static const char *SystemControlCommandsArr[] = { const char *SystemControlStatesArr[] = { "UNDEFINED", "INITIALIZED", "IDLE", "READY", "RUNNING", "INWORK", "ERROR" }; const char *SystemControlOBCStatesArr[] = - { "UNDEFINED", "IDLE", "INITIALIZED", "CONNECTED", "ARMED", "RUNNING", "REMOTECONTROL", "ERROR" }; + { "UNDEFINED", "IDLE", "INITIALIZED", "CONNECTED", "ARMED", "RUNNING", "REMOTECONTROL", "ERROR" }; const char *POSTRequestMandatoryContent[] = { "POST", "HTTP/1.1", "\r\n\r\n" }; @@ -1011,18 +1011,19 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { case RemoteControl_1: responseCode = SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE; if (CurrentInputArgCount == CommandArgCount) { - if(server_state == SERVER_STATE_IDLE - && (objectControlState == OBC_STATE_CONNECTED || objectControlState == OBC_STATE_REMOTECTRL)) { + if (server_state == SERVER_STATE_IDLE + && (objectControlState == OBC_STATE_CONNECTED + || objectControlState == OBC_STATE_REMOTECTRL)) { if (!strcasecmp(SystemControlArgument[0], ENABLE_COMMAND_STRING) - && objectControlState == OBC_STATE_CONNECTED) { + && objectControlState == OBC_STATE_CONNECTED) { LogMessage(LOG_LEVEL_INFO, "Requesting enabling of remote control"); - iCommSend(COMM_REMOTECTRL_ENABLE, NULL, 0); // TODO check return value + iCommSend(COMM_REMOTECTRL_ENABLE, NULL, 0); // TODO check return value responseCode = SYSTEM_CONTROL_RESPONSE_CODE_OK; } else if (!strcasecmp(SystemControlArgument[0], DISABLE_COMMAND_STRING) && objectControlState == OBC_STATE_REMOTECTRL) { LogMessage(LOG_LEVEL_INFO, "Requesting disabling of remote control"); - iCommSend(COMM_REMOTECTRL_DISABLE, NULL, 0); // TODO check return value + iCommSend(COMM_REMOTECTRL_DISABLE, NULL, 0); // TODO check return value responseCode = SYSTEM_CONTROL_RESPONSE_CODE_OK; } else { @@ -1042,12 +1043,13 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { SystemControlCommand = Idle_0; SystemControlSendControlResponse(responseCode, "RemoteControl:", ControlResponseBuffer, 0, &ClientSocket, 0); - break; + break; case RemoteControlManoeuvre_2: if (CurrentInputArgCount == CommandArgCount) { - if(server_state == SERVER_STATE_IDLE && objectControlState == OBC_STATE_REMOTECTRL) { + if (server_state == SERVER_STATE_IDLE && objectControlState == OBC_STATE_REMOTECTRL) { memset(pcBuffer, 0, sizeof (pcBuffer)); RemoteControlCommandType rcCommand; + if (inet_pton(AF_INET, SystemControlArgument[0], &rcCommand.objectIP) != -1) { responseCode = SYSTEM_CONTROL_RESPONSE_CODE_OK; switch (atoi(SystemControlArgument[1])) { @@ -1059,7 +1061,7 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { } if (responseCode != SYSTEM_CONTROL_RESPONSE_CODE_FUNCTION_NOT_AVAILABLE) { memcpy(pcBuffer, &rcCommand, sizeof (rcCommand)); - iCommSend(COMM_REMOTECTRL_MANOEUVRE, pcBuffer, sizeof (rcCommand)); // TODO check return value + iCommSend(COMM_REMOTECTRL_MANOEUVRE, pcBuffer, sizeof (rcCommand)); // TODO check return value responseCode = SYSTEM_CONTROL_RESPONSE_CODE_OK; } } From 1e45a86e4c651421a15b96123a6bf0c5ba6a30e3 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 27 May 2020 12:45:37 +0200 Subject: [PATCH 499/523] Fixed merge conflict fix --- core/src/objectcontrol.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/core/src/objectcontrol.c b/core/src/objectcontrol.c index 67d0604e6..758b66d80 100644 --- a/core/src/objectcontrol.c +++ b/core/src/objectcontrol.c @@ -119,7 +119,7 @@ static int addCommandToActionList(const TestScenarioCommandAction command, TestScenarioCommandAction commandActions[], const int numberOfElementsInList); -static int (const in_addr_t objectIP, const TestScenarioCommandAction commandActions[], +static int hasDelayedStart(const in_addr_t objectIP, const TestScenarioCommandAction commandActions[], const int numberOfElementsInList); static int findCommandAction(const uint16_t actionID, const TestScenarioCommandAction commandActions[], const int numberOfElementsInList); @@ -1367,7 +1367,7 @@ int addCommandToActionList(const TestScenarioCommandAction command, * \param numberOfElementsInList Number of elements in the entire list * \return Boolean value indicating if the object has a delayed start configuration */ -int (const in_addr_t objectIP, const TestScenarioCommandAction commandActions[], +int hasDelayedStart(const in_addr_t objectIP, const TestScenarioCommandAction commandActions[], const int numberOfElementsInList) { for (int i = 0; i < numberOfElementsInList; ++i) { if (commandActions[i].ip == objectIP && commandActions[i].command == ACTION_PARAMETER_VS_SEND_START) { From a78f31a2548007f860f4777f1a8a28e8c0130719 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 28 May 2020 15:54:29 +0200 Subject: [PATCH 500/523] Added linguist ignore on traj files, geofences and configuration files --- .gitattributes | 3 +++ 1 file changed, 3 insertions(+) create mode 100644 .gitattributes diff --git a/.gitattributes b/.gitattributes new file mode 100644 index 000000000..26254f6ed --- /dev/null +++ b/.gitattributes @@ -0,0 +1,3 @@ +core/traj/** linguist-detectable=false +core/geofence/** linguist-detectable=false +conf/** linguist-detectable=false From 1df1f5b42892935daed587db0a8dac06e887bf4c Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 29 May 2020 14:46:42 +0200 Subject: [PATCH 501/523] Ran code formatter --- core/src/logger.c | 34 +++++++++++++++++----------------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/core/src/logger.c b/core/src/logger.c index fa551b8e9..a5c9f8a86 100644 --- a/core/src/logger.c +++ b/core/src/logger.c @@ -342,8 +342,8 @@ void vInitializeLog(char *logFilePath, unsigned int filePathLength, char *csvLog char logFileDirectoryPath[MAX_FILE_PATH]; char trajPathDir[MAX_FILE_PATH]; char confPathDir[MAX_FILE_PATH]; - char confFilePath[MAX_FILE_PATH]; - char trigFilePath[MAX_FILE_PATH]; + char confFilePath[MAX_FILE_PATH]; + char trigFilePath[MAX_FILE_PATH]; char journalPathDir[MAX_FILE_PATH]; char DateBuffer[FILENAME_MAX]; FILE *filefd, *fileread; @@ -355,10 +355,10 @@ void vInitializeLog(char *logFilePath, unsigned int filePathLength, char *csvLog int read; UtilGetConfDirectoryPath(confPathDir, sizeof (confPathDir)); - strcat(confFilePath, confPathDir); - strcat(confFilePath, CONF_FILE_NAME); - strcat(trigFilePath, confPathDir); - strcat(trigFilePath, TRIGGER_ACTION_FILE_NAME); + strcat(confFilePath, confPathDir); + strcat(confFilePath, CONF_FILE_NAME); + strcat(trigFilePath, confPathDir); + strcat(trigFilePath, TRIGGER_ACTION_FILE_NAME); UtilGetTrajDirectoryPath(trajPathDir, sizeof (trajPathDir)); UtilGetJournalDirectoryPath(journalPathDir, sizeof (journalPathDir)); @@ -385,18 +385,18 @@ void vInitializeLog(char *logFilePath, unsigned int filePathLength, char *csvLog // Copy configuration file to log directory LogMessage(LOG_LEVEL_INFO, "Copying configuration file to log directory"); (void)strcpy(sysCommand, "cp "); - (void)strcat(sysCommand, confFilePath); + (void)strcat(sysCommand, confFilePath); (void)strcat(sysCommand, " "); (void)strcat(sysCommand, logFileDirectoryPath); (void)system(sysCommand); - // Copy configuration file to log directory - LogMessage(LOG_LEVEL_INFO, "Copying TriggerAndAction file to log directory"); - (void)strcpy(sysCommand, "cp "); - (void)strcat(sysCommand, trigFilePath); - (void)strcat(sysCommand, " "); - (void)strcat(sysCommand, logFileDirectoryPath); - (void)system(sysCommand); + // Copy configuration file to log directory + LogMessage(LOG_LEVEL_INFO, "Copying TriggerAndAction file to log directory"); + (void)strcpy(sysCommand, "cp "); + (void)strcat(sysCommand, trigFilePath); + (void)strcat(sysCommand, " "); + (void)strcat(sysCommand, logFileDirectoryPath); + (void)system(sysCommand); // Check if ./traj directory exists if ((dir = opendir(trajPathDir)) == NULL) { @@ -474,9 +474,9 @@ void vInitializeLog(char *logFilePath, unsigned int filePathLength, char *csvLog /* If file conf file exists and we have read permission do */ - if (access(confFilePath, 0) == 0) { + if (access(confFilePath, 0) == 0) { /*read the .conf file and print it in to the .log file */ - fileread = fopen(confFilePath, ACCESS_MODE_READ); + fileread = fopen(confFilePath, ACCESS_MODE_READ); read = fgetc(fileread); while (read != EOF) { fputc(read, filefd); @@ -485,7 +485,7 @@ void vInitializeLog(char *logFilePath, unsigned int filePathLength, char *csvLog fclose(fileread); } else { - sprintf(sysCommand, "Unable to open <%s>", confFilePath); + sprintf(sysCommand, "Unable to open <%s>", confFilePath); util_error(sysCommand); } From 072fd5f3bef280531bc334f5c62534a2b4993b10 Mon Sep 17 00:00:00 2001 From: Lukas Date: Tue, 2 Jun 2020 09:22:39 +0200 Subject: [PATCH 502/523] Update modules/ScenarioControl/inc/trigger.h --- modules/ScenarioControl/inc/trigger.h | 1 - 1 file changed, 1 deletion(-) diff --git a/modules/ScenarioControl/inc/trigger.h b/modules/ScenarioControl/inc/trigger.h index 6f3cc1156..718e76ae6 100644 --- a/modules/ScenarioControl/inc/trigger.h +++ b/modules/ScenarioControl/inc/trigger.h @@ -7,7 +7,6 @@ #include #include #include -#include #include "util.h" #include "iso22133.h" From 9a953292771d274d4ec21b1219bfde6dfe7e04fa Mon Sep 17 00:00:00 2001 From: Lukas Date: Tue, 2 Jun 2020 09:23:12 +0200 Subject: [PATCH 503/523] Update modules/ScenarioControl/src/distancetrigger.cpp --- modules/ScenarioControl/src/distancetrigger.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/ScenarioControl/src/distancetrigger.cpp b/modules/ScenarioControl/src/distancetrigger.cpp index 5126e588a..981e2df33 100644 --- a/modules/ScenarioControl/src/distancetrigger.cpp +++ b/modules/ScenarioControl/src/distancetrigger.cpp @@ -13,7 +13,7 @@ DistanceTrigger::DistanceTrigger(Trigger::TriggerID_t triggerID) : BooleanTrigge } Trigger::TriggerReturnCode_t DistanceTrigger::update(MonitorDataType newValue) { - double networkDelayCorrection_m = 0.0, networkDelay_s = 0.0; + double networkDelayCorrection_m = 0.0; struct timeval currentTime, networkDelay; if (!newValue.data.position.isPositionValid || !referencePoint.isPositionValid) { From 82c8bc94cc449c6dab6dac5aeccb8d904b13798a Mon Sep 17 00:00:00 2001 From: Lukas Date: Tue, 2 Jun 2020 09:31:11 +0200 Subject: [PATCH 504/523] Update action.h --- modules/ScenarioControl/inc/action.h | 1 - 1 file changed, 1 deletion(-) diff --git a/modules/ScenarioControl/inc/action.h b/modules/ScenarioControl/inc/action.h index 40fffce19..798bdcce0 100644 --- a/modules/ScenarioControl/inc/action.h +++ b/modules/ScenarioControl/inc/action.h @@ -6,7 +6,6 @@ #include #include #include -#include #include "util.h" From 6d996b0afbfe0d8c267ed0d9a893d376a7b321fc Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 2 Jun 2020 10:54:26 +0200 Subject: [PATCH 505/523] Temporary fix to build which is fixed more extensively in another branch --- modules/ScenarioControl/src/distancetrigger.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/modules/ScenarioControl/src/distancetrigger.cpp b/modules/ScenarioControl/src/distancetrigger.cpp index 981e2df33..3807e7575 100644 --- a/modules/ScenarioControl/src/distancetrigger.cpp +++ b/modules/ScenarioControl/src/distancetrigger.cpp @@ -24,8 +24,9 @@ Trigger::TriggerReturnCode_t DistanceTrigger::update(MonitorDataType newValue) { if (newValue.data.speed.isLongitudinalValid && newValue.data.isTimestampValid) { TimeSetToCurrentSystemTime(¤tTime); timersub(¤tTime, &newValue.data.timestamp, &networkDelay); - networkDelay_s = fabs(static_cast(networkDelay.tv_sec) + static_cast(networkDelay.tv_usec) / 1000000.0); - networkDelayCorrection_m = 2.0 * networkDelay_s * newValue.data.speed.longitudinal_m_s; + networkDelayCorrection_m = 2.0 * fabs(static_cast(networkDelay.tv_sec) + + static_cast(networkDelay.tv_usec) / 1000000.0) + * newValue.data.speed.longitudinal_m_s; } else { LogMessage(LOG_LEVEL_WARNING, "Invalid monitor data speed or timestamp: cannot correct for network delay"); From 0c560b4900df8c6f7f28124dac6a90b507863b1d Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Tue, 2 Jun 2020 15:24:01 +0200 Subject: [PATCH 506/523] Claarified assumption for distance trigger --- .../ScenarioControl/src/distancetrigger.cpp | 21 +++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) diff --git a/modules/ScenarioControl/src/distancetrigger.cpp b/modules/ScenarioControl/src/distancetrigger.cpp index 3807e7575..7845d6b37 100644 --- a/modules/ScenarioControl/src/distancetrigger.cpp +++ b/modules/ScenarioControl/src/distancetrigger.cpp @@ -13,8 +13,8 @@ DistanceTrigger::DistanceTrigger(Trigger::TriggerID_t triggerID) : BooleanTrigge } Trigger::TriggerReturnCode_t DistanceTrigger::update(MonitorDataType newValue) { - double networkDelayCorrection_m = 0.0; - struct timeval currentTime, networkDelay; + double networkDelayCorrection_m = 0.0, networkDelay_s = 0.0; + struct timeval currentTime, triggerObjectNetworkDelay, actionObjectNetworkDelay; if (!newValue.data.position.isPositionValid || !referencePoint.isPositionValid) { throw std::logic_error("Unable to update distance trigger on invalid data"); @@ -23,10 +23,19 @@ Trigger::TriggerReturnCode_t DistanceTrigger::update(MonitorDataType newValue) { // Correct for two-way network delay effects on trigger distance if (newValue.data.speed.isLongitudinalValid && newValue.data.isTimestampValid) { TimeSetToCurrentSystemTime(¤tTime); - timersub(¤tTime, &newValue.data.timestamp, &networkDelay); - networkDelayCorrection_m = 2.0 * fabs(static_cast(networkDelay.tv_sec) - + static_cast(networkDelay.tv_usec) / 1000000.0) - * newValue.data.speed.longitudinal_m_s; + timersub(¤tTime, &newValue.data.timestamp, &triggerObjectNetworkDelay); + + // TODO Get rid of this false assumption + actionObjectNetworkDelay = triggerObjectNetworkDelay; + + // Network delay consists of both positional data reporting delay and action execution message delay + networkDelay_s = fabs(static_cast(triggerObjectNetworkDelay.tv_sec) + + static_cast(triggerObjectNetworkDelay.tv_usec) / 1000000.0) + + fabs(static_cast(actionObjectNetworkDelay.tv_sec) + + static_cast(actionObjectNetworkDelay.tv_usec) / 1000000.0); + + // Predict position offset + networkDelayCorrection_m = networkDelay_s * newValue.data.speed.longitudinal_m_s; } else { LogMessage(LOG_LEVEL_WARNING, "Invalid monitor data speed or timestamp: cannot correct for network delay"); From 1e6dd42f81cf870523b12d2e8baf44bf6d2c21b6 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 3 Jun 2020 09:18:52 +0200 Subject: [PATCH 507/523] Updated util reference --- util | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/util b/util index 2cdf6df52..fcead3142 160000 --- a/util +++ b/util @@ -1 +1 @@ -Subproject commit 2cdf6df528e2959e3aacfa3317cf1939a9c7e56d +Subproject commit fcead3142b2b124134180d4c735513a43dcbb0eb From 4d5eaa7857633ce190d0413d047c9664210b50e4 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 3 Jun 2020 11:03:36 +0200 Subject: [PATCH 508/523] Updated util ref with fixed iso22133 linking --- util | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/util b/util index fcead3142..74565e185 160000 --- a/util +++ b/util @@ -1 +1 @@ -Subproject commit fcead3142b2b124134180d4c735513a43dcbb0eb +Subproject commit 74565e1851d3f6682e13a229404466de9bcebbe7 From b0950979fe127bb2a3e51f304ed6aa3867a01935 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 3 Jun 2020 11:07:04 +0200 Subject: [PATCH 509/523] Removed iso22133 and positioning files from repo and cmake --- core/CMakeLists.txt | 47 +- core/inc/iso22133.h | 224 ---- core/inc/positioning.h | 99 -- core/src/iso22133.c | 2257 ---------------------------------------- core/src/positioning.c | 353 ------- 5 files changed, 11 insertions(+), 2969 deletions(-) delete mode 100644 core/inc/iso22133.h delete mode 100644 core/inc/positioning.h delete mode 100644 core/src/iso22133.c delete mode 100644 core/src/positioning.c diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index fbc969b11..e06cfc8f5 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -11,19 +11,19 @@ set(SYSTEM_SHARED_MEMORY_PATH "/dev/shm/maestro") set(CORE_TARGET ${PROJECT_NAME}) set(COREUTILS_TARGET MaestroCoreUtil) -set(ISO_22133_TARGET MaestroISO22133) -set(POSITIONING_TARGET MaestroPositioning) set(DATA_DICTIONARY_TARGET MaestroDataDictionary) set(TIME_LIBRARY MaestroTime) set(LOGGING_LIBRARY MaestroLogging) set(MESSAGE_BUS_LIBRARY MaestroMQ) set(SHARED_MEMORY_LIBRARY MaestroSHM) +set(ISO_22133_LIBRARY ISO22133) set(MATH_LIBRARY m) -get_target_property(TIME_HEADERS ${TIME_LIBRARY} INCLUDE_DIRECTORIES) -get_target_property(LOGGING_HEADERS ${LOGGING_LIBRARY} INCLUDE_DIRECTORIES) -get_target_property(MESSAGE_BUS_HEADERS ${MESSAGE_BUS_LIBRARY} INCLUDE_DIRECTORIES) -get_target_property(SHARED_MEMORY_HEADERS ${SHARED_MEMORY_LIBRARY} INCLUDE_DIRECTORIES) +get_target_property(TIME_HEADERS ${TIME_LIBRARY} PUBLIC_HEADER) +get_target_property(LOGGING_HEADERS ${LOGGING_LIBRARY} PUBLIC_HEADER) +get_target_property(MESSAGE_BUS_HEADERS ${MESSAGE_BUS_LIBRARY} PUBLIC_HEADER) +get_target_property(SHARED_MEMORY_HEADERS ${SHARED_MEMORY_LIBRARY} PUBLIC_HEADER) +get_target_property(ISO_22133_HEADERS ${ISO_22133_LIBRARY} PUBLIC_HEADER) include(GNUInstallDirs) @@ -44,36 +44,12 @@ target_include_directories(${COREUTILS_TARGET} PUBLIC ${TIME_HEADERS} ${LOGGING_HEADERS} ${MESSAGE_BUS_HEADERS} + ${ISO_22133_HEADERS} ) set_target_properties(${COREUTILS_TARGET} PROPERTIES PUBLIC_HEADER ${CMAKE_CURRENT_SOURCE_DIR}/inc/util.h ) -add_library(${ISO_22133_TARGET} SHARED - ${CMAKE_CURRENT_SOURCE_DIR}/src/iso22133.c -) -target_include_directories(${ISO_22133_TARGET} PUBLIC - $ - $ - ${TIME_HEADERS} - ${LOGGING_HEADERS} -) -set_target_properties(${ISO_22133_TARGET} PROPERTIES - PUBLIC_HEADER ${CMAKE_CURRENT_SOURCE_DIR}/inc/iso22133.h -) - -add_library(${POSITIONING_TARGET} SHARED - ${CMAKE_CURRENT_SOURCE_DIR}/src/positioning.c -) -target_include_directories(${POSITIONING_TARGET} PUBLIC - $ - $ - ${TIME_HEADERS} -) -set_target_properties(${POSITIONING_TARGET} PROPERTIES - PUBLIC_HEADER ${CMAKE_CURRENT_SOURCE_DIR}/inc/positioning.h -) - add_library(${DATA_DICTIONARY_TARGET} SHARED ${CMAKE_CURRENT_SOURCE_DIR}/src/datadictionary.c ) @@ -94,8 +70,7 @@ target_link_libraries(${CORE_TARGET} LINK_PUBLIC ${LOGGING_LIBRARY} ${COREUTILS_TARGET} ${DATA_DICTIONARY_TARGET} - ${ISO_22133_TARGET} - ${POSITIONING_TARGET} + ${ISO_22133_LIBRARY} ) target_link_libraries(${COREUTILS_TARGET} LINK_PUBLIC @@ -103,18 +78,18 @@ target_link_libraries(${COREUTILS_TARGET} LINK_PUBLIC ${LOGGING_LIBRARY} ${MESSAGE_BUS_LIBRARY} ${MATH_LIBRARY} - ${POSITIONING_TARGET} + ${ISO_22133_LIBRARY} ) target_link_libraries(${DATA_DICTIONARY_TARGET} LINK_PUBLIC ${SHARED_MEMORY_LIBRARY} + ${COREUTILS_TARGET} ) # Installation rules install(CODE "MESSAGE(STATUS \"Installing target ${CORE_TARGET}\")") install(TARGETS ${CORE_TARGET} ${COREUTILS_TARGET} - ${ISO_22133_TARGET} ${DATA_DICTIONARY_TARGET} - ${POSITIONING_TARGET} + ${DATA_DICTIONARY_TARGET} RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} diff --git a/core/inc/iso22133.h b/core/inc/iso22133.h deleted file mode 100644 index 6c9f11811..000000000 --- a/core/inc/iso22133.h +++ /dev/null @@ -1,224 +0,0 @@ -#ifndef ISO22133_H -#define ISO22133_H -#ifdef __cplusplus -extern "C" { -#endif -/*! ------------------------------------------------------------------------------ - * -- Copyright : (C) AstaZero AB - * ------------------------------------------------------------------------------ - * -- File : iso22133.h - * -- Author : Lukas Wikander - * -- Description : This file specifies an interface for converting from network - * messages in the ISO 22133 format to native data types. - * -- Purpose : Reduce the amount of boilerplate needed to read an ISO 22133 - * message. - * -- Reference : ISO/TC 22/SC 33/WG 16 - ISO/WD 22133-1 - * ------------------------------------------------------------------------------ - */ - -#include -#include -#include -#include -#include - -#include "positioning.h" - -/*! OSTM commands */ -typedef enum { - OBJECT_COMMAND_ARM = 0x02, //!< Request to arm the target object - OBJECT_COMMAND_DISARM = 0x03, //!< Request to disarm the target object - OBJECT_COMMAND_REMOTE_CONTROL = 0x06 //!< Request for remote control of the target object -} ObjectCommandType; - - -#define HEAB_FREQUENCY_HZ 100 -/*! HEAB control center statuses */ -typedef enum { - CONTROL_CENTER_STATUS_INIT = 0x00, //!< - CONTROL_CENTER_STATUS_READY = 0x01, //!< - CONTROL_CENTER_STATUS_ABORT = 0x02, //!< - CONTROL_CENTER_STATUS_RUNNING = 0x03, //!< - CONTROL_CENTER_STATUS_TEST_DONE = 0x04, //!< - CONTROL_CENTER_STATUS_NORMAL_STOP = 0x05 //!< -} ControlCenterStatusType; - -#define MONR_EXPECTED_FREQUENCY_HZ 100 - -typedef enum { - TRIGGER_UNDEFINED = 0x0000, - TRIGGER_TYPE_1 = 0x0001, - TRIGGER_SPEED = 0x0010, - TRIGGER_DISTANCE = 0x0020, - TRIGGER_ACCELERATION = 0x0030, - TRIGGER_LANE_CHANGED = 0x0040, - TRIGGER_LANE_OFFSET = 0x0050, - TRIGGER_POSITION_REACHED = 0x0060, - TRIGGER_POSITION_LEFT = 0x0061, - TRIGGER_POSITION_OFFSET = 0x0062, - TRIGGER_STEERING_ANGLE = 0x0070, - TRIGGER_THROTTLE_VALUE = 0x0080, - TRIGGER_BRAKE = 0x0090, - TRIGGER_ACTIVE_TRAJECTORY = 0x00A0, - TRIGGER_OTHER_OBJECT_FEATURE = 0x00B0, - TRIGGER_INFRASTRUCTURE = 0x00C0, - TRIGGER_TEST_SCENARIO_EVENT = 0x00D0, - TRIGGER_MISC_DIGITAL_INPUT = 0x00E0, - TRIGGER_MISC_ANALOG_INPUT = 0x00F0, - TRIGGER_TIMER_EVENT_OCCURRED = 0x0100, - TRIGGER_MODE_CHANGED = 0x0110, - TRIGGER_UNAVAILABLE = 0xFFFF -} TriggerType_t; - -typedef enum { - TRIGGER_PARAMETER_FALSE = 0x00000000, - TRIGGER_PARAMETER_TRUE = 0x00000001, - TRIGGER_PARAMETER_RELEASED = 0x00000010, - TRIGGER_PARAMETER_PRESSED = 0x00000011, - TRIGGER_PARAMETER_LOW = 0x00000020, - TRIGGER_PARAMETER_HIGH = 0x00000021, - TRIGGER_PARAMETER_RISING_EDGE = 0x00000022, - TRIGGER_PARAMETER_FALLING_EDGE = 0x00000023, - TRIGGER_PARAMETER_ANY_EDGE = 0x00000024, - TRIGGER_PARAMETER_RELATIVE = 0x00000030, - TRIGGER_PARAMETER_ABSOLUTE = 0x00000031, - TRIGGER_PARAMETER_VALUE = 0x00000040, - TRIGGER_PARAMETER_MIN = 0x00000050, - TRIGGER_PARAMETER_MAX = 0x00000051, - TRIGGER_PARAMETER_MEAN = 0x00000052, - TRIGGER_PARAMETER_EQUAL_TO = 0x00000060, - TRIGGER_PARAMETER_GREATER_THAN = 0x00000061, - TRIGGER_PARAMETER_GREATER_THAN_OR_EQUAL_TO = 0x00000062, - TRIGGER_PARAMETER_LESS_THAN = 0x00000063, - TRIGGER_PARAMETER_LESS_THAN_OR_EQUAL_TO = 0x00000064, - TRIGGER_PARAMETER_NOT_EQUAL_TO = 0x00000065, - TRIGGER_PARAMETER_X = 0x00000070, - TRIGGER_PARAMETER_Y = 0x00000071, - TRIGGER_PARAMETER_Z = 0x00000072, - TRIGGER_PARAMETER_TIME = 0x00000080, - TRIGGER_PARAMETER_DATE = 0x00000081, - TRIGGER_PARAMETER_RULE = 0x000000A0, - TRIGGER_PARAMETER_UNAVAILABLE = 0xFFFFFFFF -} TriggerTypeParameter_t; - - -typedef enum { - ACTION_NONE = 0x0000, - ACTION_TYPE_1 = 0x0001, - ACTION_TYPE_2 = 0x0002, - ACTION_SET_SPEED = 0x0010, - ACTION_SET_DISTANCE = 0x0020, - ACTION_SET_ACCELERATION = 0x0030, - ACTION_LANE_CHANGE = 0x0040, - ACTION_LANE_OFFSET = 0x0050, - ACTION_SET_POSITION = 0x0060, - ACTION_SET_STEERING_ANGLE = 0x0070, - ACTION_SET_TRHOTTLE_VALUE = 0x0080, - ACTION_BRAKE = 0x0090, - ACTION_FOLLOW_TRAJECTORY = 0x00A0, - ACTION_OTHER_OBJECT_FEATURE = 0x00B0, - ACTION_INFRASTRUCTURE = 0x00C0, - ACTION_TEST_SCENARIO_COMMAND = 0x00D0, - ACTION_MISC_DIGITAL_OUTPUT = 0x00E0, - ACTION_MISC_ANALOG_OUTPUT = 0x00F0, - ACTION_START_TIMER = 0x0100, - ACTION_MODE_CHANGE = 0x0110, - ACTION_UNAVAILABLE = 0xFFFF -} ActionType_t; - -typedef enum { - ACTION_PARAMETER_SET_FALSE = 0x00000000, - ACTION_PARAMETER_SET_TRUE = 0x00000001, - ACTION_PARAMETER_RELEASE = 0x00000010, - ACTION_PARAMETER_PRESS = 0x00000011, - ACTION_PARAMETER_SET_VALUE = 0x00000020, - ACTION_PARAMETER_MIN = 0x00000040, - ACTION_PARAMETER_MAX = 0x00000041, - ACTION_PARAMETER_X = 0x00000070, - ACTION_PARAMETER_Y = 0x00000071, - ACTION_PARAMETER_Z = 0x00000072, - ACTION_PARAMETER_VS_BRAKE_WARNING = 0xA0000000, - ACTION_PARAMETER_VS_SEND_START = 0xA0000100, - ACTION_PARAMETER_UNAVAILABLE = 0xFFFFFFFF -} ActionTypeParameter_t; - - -typedef enum { - MESSAGE_OK, - MESSAGE_LENGTH_ERROR, - MESSAGE_TYPE_ERROR, - MESSAGE_CRC_ERROR, - MESSAGE_VERSION_ERROR, - MESSAGE_VALUE_ID_ERROR, - MESSAGE_SYNC_WORD_ERROR -} ISOMessageReturnValue; - -/*! Valid ISO message identifiers */ -typedef enum { - MESSAGE_ID_INVALID = 0x0000, - MESSAGE_ID_TRAJ = 0x0001, - MESSAGE_ID_OSEM = 0x0002, - MESSAGE_ID_OSTM = 0x0003, - MESSAGE_ID_STRT = 0x0004, - MESSAGE_ID_HEAB = 0x0005, - MESSAGE_ID_MONR = 0x0006, - MESSAGE_ID_MONR2 = 0x0007, - MESSAGE_ID_SOWM = 0x0008, - MESSAGE_ID_INFO = 0x0009, - MESSAGE_ID_RCMM = 0x000A, - MESSAGE_ID_SYPM = 0x000B, - MESSAGE_ID_MTSP = 0x000C, - MESSAGE_ID_TRCM = 0x0011, - MESSAGE_ID_ACCM = 0x0012, - MESSAGE_ID_TREO = 0x0013, - MESSAGE_ID_EXAC = 0x0014, - MESSAGE_ID_CATA = 0x0015, - MESSAGE_ID_RCCM = 0x0020, - MESSAGE_ID_RCRT = 0x0021, - MESSAGE_ID_PIME = 0x0030, - MESSAGE_ID_COSE = 0x0031, - MESSAGE_ID_MOMA = 0x0032, - MESSAGE_ID_RESERVE_RANGE_1_LOWER_LIMIT = 0x0100, - MESSAGE_ID_RESERVE_RANGE_1_UPPER_LIMIT = 0x0FFF, - MESSAGE_ID_RESERVE_RANGE_2_LOWER_LIMIT = 0xF000, - MESSAGE_ID_RESERVE_RANGE_2_UPPER_LIMIT = 0xFFFF, - MESSAGE_ID_VENDOR_SPECIFIC_LOWER_LIMIT = 0xA100, - MESSAGE_ID_VENDOR_SPECIFIC_UPPER_LIMIT = 0xBFFF, - MESSAGE_ID_VENDOR_SPECIFIC_RISE_INSUP = 0xA102 -} ISOMessageID; - -/*! Remote control command */ - -typedef enum { - MANOEUVRE_BACK_TO_START = 3 -} RemoteControlManoeuvreType; - -/*! Supervisor command */ -typedef enum { - SUPERVISOR_COMMAND_NORMAL = 1, //!< Place supervisor in normal mode - SUPERVISOR_COMMAND_DEBUG = 2 //!< Place supervisor in debug mode -} SupervisorCommandType; - -ISOMessageReturnValue decodeMONRMessage(const char * monrDataBuffer, const size_t bufferLength, uint32_t * objectID, ObjectMonitorType * MonitorData, const char debug); -ssize_t encodeTRAJMessageHeader(const uint16_t trajectoryID, const uint16_t trajectoryVersion, const char * trajectoryName, const size_t nameLength, const uint32_t numberOfPointsInTraj, char * trajDataBuffer, const size_t bufferLength, const char debug); -ssize_t encodeTRAJMessagePoint(const struct timeval * pointTimeFromStart, const CartesianPosition position, const SpeedType speed, const AccelerationType acceleration, const float curvature, char * trajDataBufferPointer, const size_t remainingBufferLength, const char debug); -ssize_t encodeTRAJMessageFooter(char * trajDataBuffer, const size_t bufferLength, const char debug); -ssize_t encodeSTRTMessage(const struct timeval* timeOfStart, char * strtDataBuffer, const size_t bufferLength, const char debug); -ssize_t encodeOSEMMessage(const double_t * latitude_deg, const double_t * longitude_deg, const float * altitude_m, const float * maxPositionDeviation_m, const float * maxLateralDeviation_m, const float * minimumPositioningAccuracy_m, char * osemDataBuffer, const size_t bufferLength, const char debug); -ssize_t encodeOSTMMessage(const ObjectCommandType command, char * ostmDataBuffer, const size_t bufferLength, const char debug); -ssize_t encodeHEABMessage(const ControlCenterStatusType status, char * heabDataBuffer, const size_t bufferLength, const char debug); -ssize_t encodeSYPMMessage(const struct timeval synchronizationTime, const struct timeval freezeTime, char * sypmDataBuffer, const size_t bufferLength, const char debug); -ssize_t encodeMTSPMessage(const struct timeval * estSyncPointTime, char * mtspDataBuffer, const size_t bufferLength, const char debug); -ssize_t encodeTRCMMessage(const uint16_t* triggerID, const TriggerType_t* triggerType, const TriggerTypeParameter_t* param1, const TriggerTypeParameter_t* param2, const TriggerTypeParameter_t* param3, char * trcmDataBuffer, const size_t bufferLength, const char debug); -ssize_t decodeTREOMessage(); -ssize_t encodeACCMMessage(const uint16_t* actionID, const ActionType_t* actionType, const ActionTypeParameter_t* param1, const ActionTypeParameter_t* param2, const 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 encodeINSUPMessage(const SupervisorCommandType, char * insupDataBuffer, const size_t bufferLength, const char debug); -ssize_t encodeRCMMMessage(const RemoteControlManoeuvreType command, char *rcmmDataBuffer, const size_t bufferLength, const char debug); -ISOMessageID getISOMessageType(const char * messageData, const size_t length, const char debug); -void setISOCRCVerification(const int8_t enabled); - -#ifdef __cplusplus -} -#endif -#endif diff --git a/core/inc/positioning.h b/core/inc/positioning.h deleted file mode 100644 index 8874cd995..000000000 --- a/core/inc/positioning.h +++ /dev/null @@ -1,99 +0,0 @@ -#ifndef POSITIONING_H -#define POSITIONING_H -#ifdef __cplusplus -extern "C" { -#endif - -#include -#include -#include -#include - -/*! Struct describing a position and orientation in a local cartesian coordinate system */ -typedef struct { - double xCoord_m; - double yCoord_m; - double zCoord_m; - double heading_rad; - bool isPositionValid; - bool isHeadingValid; -} CartesianPosition; // TODO: rename - -/*! Struct describing longitudinal and lateral speed of an object */ -typedef struct { - double longitudinal_m_s; - double lateral_m_s; - bool isLongitudinalValid; - bool isLateralValid; -} SpeedType; - -/*! Struct describing longitudinal and lateral acceleration of an object */ -typedef struct { - double longitudinal_m_s2; - double lateral_m_s2; - bool isLongitudinalValid; - bool isLateralValid; -} AccelerationType; - -/*! Enumeration of object driving direction descriptions */ -typedef enum { - OBJECT_DRIVE_DIRECTION_FORWARD, - OBJECT_DRIVE_DIRECTION_BACKWARD, - OBJECT_DRIVE_DIRECTION_UNAVAILABLE -} DriveDirectionType; - -/*! Enumeration of observable object states */ -typedef enum { - OBJECT_STATE_UNKNOWN, - OBJECT_STATE_DISARMED, - OBJECT_STATE_ARMED, - OBJECT_STATE_RUNNING, - OBJECT_STATE_POSTRUN, - OBJECT_STATE_ABORTING, - OBJECT_STATE_REMOTE_CONTROL -} ObjectStateType; - -/*! Enumeration of ready to arm statuses of an object */ -typedef enum { - OBJECT_NOT_READY_TO_ARM, - OBJECT_READY_TO_ARM, - OBJECT_READY_TO_ARM_UNAVAILABLE -} ObjectArmReadinessType; - -/*! Struct describing a range of possible errors reported by an object */ -typedef struct { - bool abortRequest; - bool outsideGeofence; - bool badPositioningAccuracy; - bool engineFault; - bool batteryFault; - bool syncPointEnded; - bool unknownError; -} ObjectErrorType; - -/*! Struct containing measured object data from a single point in time */ -typedef struct { - bool isTimestampValid; - struct timeval timestamp; - CartesianPosition position; - SpeedType speed; - AccelerationType acceleration; - DriveDirectionType drivingDirection; - ObjectStateType state; - ObjectArmReadinessType armReadiness; - ObjectErrorType error; -} ObjectMonitorType; - - -int objectMonitorDataToASCII(const ObjectMonitorType * monitorData, char *asciiBuffer, const size_t bufferLength); -int ASCIIToObjectMonitorData(const char *asciiBuffer, ObjectMonitorType * monitorData); -bool hasError(const ObjectErrorType error); -const char * objectStateToASCII(const ObjectStateType state); -ObjectStateType ASCIIToObjectState(const char * asciiString); -void errorStatusToASCII(const ObjectErrorType error, char * asciiBuffer, const size_t bufferLength); -ObjectErrorType ASCIIToErrorStatus(const char * asciiString); - -#ifdef __cplusplus -} -#endif -#endif diff --git a/core/src/iso22133.c b/core/src/iso22133.c deleted file mode 100644 index 6f985cb0e..000000000 --- a/core/src/iso22133.c +++ /dev/null @@ -1,2257 +0,0 @@ -#include "iso22133.h" -#include "logging.h" -#include "maestroTime.h" -#include -#include -#include -#include -#include -#include - -// ************************* Global ISO protocol settings ******************************************************** -static const uint8_t SupportedProtocolVersions[] = { 2 }; - -#define ISO_PROTOCOL_VERSION 2 //!< ISO protocol version of messages sent -#define ACK_REQ 0 - - -// ************************* Type definitions according ISO protocol specification ******************************* -//! Predefined integer values with special meaning -#define ISO_SYNC_WORD 0x7E7E -#define LATITUDE_UNAVAILABLE_VALUE 900000000001 -#define LATITUDE_ONE_DEGREE_VALUE 10000000000 -#define LONGITUDE_UNAVAILABLE_VALUE 1800000000001 -#define LONGITUDE_ONE_DEGREE_VALUE 10000000000 -#define ALTITUDE_UNAVAILABLE_VALUE 800001 -#define ALTITUDE_ONE_METER_VALUE 100 -#define DATE_UNAVAILABLE_VALUE 0 -#define GPS_WEEK_UNAVAILABLE_VALUE 10001 -#define GPS_SECOND_OF_WEEK_UNAVAILABLE_VALUE 2419200000 -#define MAX_WAY_DEVIATION_UNAVAILABLE_VALUE 65535 -#define MAX_WAY_DEVIATION_ONE_METER_VALUE 1000 -#define MAX_LATERAL_DEVIATION_UNAVAILABLE_VALUE 65535 -#define MAX_LATERAL_DEVIATION_ONE_METER_VALUE 1000 -#define MIN_POSITIONING_ACCURACY_NOT_REQUIRED_VALUE 0 -#define MIN_POSITIONING_ACCURACY_ONE_METER_VALUE 1000 // ISO specification unclear on this value -#define TRIGGER_ID_UNAVAILABLE 65535 -#define TRIGGER_TYPE_UNAVAILABLE 65535 -#define TRIGGER_TYPE_PARAMETER_UNAVAILABLE 4294967295 -#define ACTION_ID_UNAVAILABLE 65535 -#define ACTION_TYPE_UNAVAILABLE 65535 -#define ACTION_TYPE_PARAMETER_UNAVAILABLE 4294967295 -#define POSITION_ONE_METER_VALUE 1000 -#define HEADING_UNAVAILABLE_VALUE 36001 -#define HEADING_ONE_DEGREE_VALUE 100 -#define SPEED_UNAVAILABLE_VALUE (-32768) -#define SPEED_ONE_METER_PER_SECOND_VALUE 100 -#define ACCELERATION_UNAVAILABLE_VALUE 32001 -#define ACCELERATION_ONE_METER_PER_SECOND_SQUARED_VALUE 1000 -#define RELATIVE_TIME_ONE_SECOND_VALUE 1000 - -#define DEFAULT_CRC_INIT_VALUE 0x0000 -#define DEFAULT_CRC_CHECK_ENABLED 1 - -typedef enum { - ISO_DRIVE_DIRECTION_FORWARD = 0, - ISO_DRIVE_DIRECTION_BACKWARD = 1, - ISO_DRIVE_DIRECTION_UNAVAILABLE = 2 -} DriveDirectionValues; -typedef enum { - ISO_OBJECT_STATE_OFF = 0, - ISO_OBJECT_STATE_INIT = 1, - ISO_OBJECT_STATE_ARMED = 2, - ISO_OBJECT_STATE_DISARMED = 3, - ISO_OBJECT_STATE_RUNNING = 4, - ISO_OBJECT_STATE_POSTRUN = 5, - ISO_OBJECT_STATE_REMOTE_CONTROLLED = 6, - ISO_OBJECT_STATE_ABORTING = 7 -} ObjectStateValues; -typedef enum { - ISO_NOT_READY_TO_ARM = 0, - ISO_READY_TO_ARM = 1, - ISO_READY_TO_ARM_UNAVAILABLE = 2 -} ArmReadinessValues; - -#define BITMASK_ERROR_ABORT_REQUEST 0x80 -#define BITMASK_ERROR_OUTSIDE_GEOFENCE 0x40 -#define BITMASK_ERROR_BAD_POSITIONING_ACCURACY 0x20 -#define BITMASK_ERROR_ENGINE_FAULT 0x10 -#define BITMASK_ERROR_BATTERY_FAULT 0x08 -#define BITMASK_ERROR_OTHER 0x04 -#define BITMASK_ERROR_SYNC_POINT_ENDED 0x02 -#define BITMASK_ERROR_VENDOR_SPECIFIC 0x01 - - - -#pragma pack(push,1) // Ensure sizeof() is useable for (most) network byte lengths -/*! ISO message header */ -typedef struct { - uint16_t SyncWordU16; - uint8_t TransmitterIdU8; - uint8_t MessageCounterU8; - uint8_t AckReqProtVerU8; - uint16_t MessageIdU16; - uint32_t MessageLengthU32; -} HeaderType; - - -/*! ISO message footer */ -typedef struct { - uint16_t Crc; -} FooterType; - - -/*! TRAJ message */ -#define TRAJ_NAME_STRING_MAX_LENGTH 64 -typedef struct { - HeaderType header; - uint16_t trajectoryIDValueID; - uint16_t trajectoryIDContentLength; - uint16_t trajectoryID; - uint16_t trajectoryNameValueID; - uint16_t trajectoryNameContentLength; - char trajectoryName[TRAJ_NAME_STRING_MAX_LENGTH]; - uint16_t trajectoryVersionValueID; - uint16_t trajectoryVersionContentLength; - uint16_t trajectoryVersion; -} TRAJHeaderType; - -typedef struct { - uint16_t relativeTimeValueID; - uint16_t relativeTimeContentLength; - uint32_t relativeTime; - uint16_t xPositionValueID; - uint16_t xPositionContentLength; - int32_t xPosition; - uint16_t yPositionValueID; - uint16_t yPositionContentLength; - int32_t yPosition; - uint16_t zPositionValueID; - uint16_t zPositionContentLength; - int32_t zPosition; - uint16_t headingValueID; - uint16_t headingContentLength; - uint16_t heading; - uint16_t longitudinalSpeedValueID; - uint16_t longitudinalSpeedContentLength; - int16_t longitudinalSpeed; - uint16_t lateralSpeedValueID; - uint16_t lateralSpeedContentLength; - int16_t lateralSpeed; - uint16_t longitudinalAccelerationValueID; - uint16_t longitudinalAccelerationContentLength; - int16_t longitudinalAcceleration; - uint16_t lateralAccelerationValueID; - uint16_t lateralAccelerationContentLength; - int16_t lateralAcceleration; - uint16_t curvatureValueID; - uint16_t curvatureContentLength; - float_t curvature; -} TRAJPointType; - -typedef struct { - FooterType footer; -} TRAJFooterType; - -//! TRAJ value IDs -#define VALUE_ID_TRAJ_TRAJECTORY_IDENTIFIER 0x0101 -#define VALUE_ID_TRAJ_TRAJECTORY_NAME 0x0102 -#define VALUE_ID_TRAJ_TRAJECTORY_VERSION 0x0103 -#define VALUE_ID_TRAJ_RELATIVE_TIME 0x0001 -#define VALUE_ID_TRAJ_X_POSITION 0x0010 -#define VALUE_ID_TRAJ_Y_POSITION 0x0011 -#define VALUE_ID_TRAJ_Z_POSITION 0x0012 -#define VALUE_ID_TRAJ_HEADING 0x0030 -#define VALUE_ID_TRAJ_LONGITUDINAL_SPEED 0x0040 -#define VALUE_ID_TRAJ_LATERAL_SPEED 0x0041 -#define VALUE_ID_TRAJ_LONGITUDINAL_ACCELERATION 0x0050 -#define VALUE_ID_TRAJ_LATERAL_ACCELERATION 0x0051 -#define VALUE_ID_TRAJ_CURVATURE 0x0052 - - -/*! OSEM message */ -typedef struct { - HeaderType header; - uint16_t latitudeValueID; - uint16_t latitudeContentLength; - int64_t latitude; - uint16_t longitudeValueID; - uint16_t longitudeContentLength; - int64_t longitude; - uint16_t altitudeValueID; - uint16_t altitudeContentLength; - int32_t altitude; - uint16_t dateValueID; - uint16_t dateContentLength; - uint32_t date; - uint16_t GPSWeekValueID; - uint16_t GPSWeekContentLength; - uint16_t GPSWeek; - uint16_t GPSQmsOfWeekValueID; - uint16_t GPSQmsOfWeekContentLength; - uint32_t GPSQmsOfWeek; - uint16_t maxWayDeviationValueID; - uint16_t maxWayDeviationContentLength; - uint16_t maxWayDeviation; - uint16_t maxLateralDeviationValueID; - uint16_t maxLateralDeviationContentLength; - uint16_t maxLateralDeviation; - uint16_t minPosAccuracyValueID; - uint16_t minPosAccuracyContentLength; - uint16_t minPosAccuracy; - FooterType footer; -} OSEMType; //85 bytes - -//! OSEM value IDs -#define VALUE_ID_OSEM_LATITUDE 0x0020 -#define VALUE_ID_OSEM_LONGITUDE 0x0021 -#define VALUE_ID_OSEM_ALTITUDE 0x0022 -#define VALUE_ID_OSEM_DATE 0x0004 -#define VALUE_ID_OSEM_GPS_WEEK 0x0003 -#define VALUE_ID_OSEM_GPS_QUARTER_MILLISECOND_OF_WEEK 0x0002 -#define VALUE_ID_OSEM_MAX_WAY_DEVIATION 0x0070 -#define VALUE_ID_OSEM_MAX_LATERAL_DEVIATION 0x0072 -#define VALUE_ID_OSEM_MIN_POSITIONING_ACCURACY 0x0074 - - -/*! OSTM message */ -typedef struct { - HeaderType header; - uint16_t stateValueID; - uint16_t stateContentLength; - uint8_t state; - FooterType footer; -} OSTMType; //16 bytes - -//! OSTM value IDs -#define VALUE_ID_OSTM_STATE_CHANGE_REQUEST 0x0064 - - -/*! STRT message */ -typedef struct { - HeaderType header; - uint16_t StartTimeValueIdU16; - uint16_t StartTimeContentLengthU16; - uint32_t StartTimeU32; - uint16_t GPSWeekValueID; - uint16_t GPSWeekContentLength; - uint16_t GPSWeek; - FooterType footer; -} STRTType; //27 bytes - -//! STRT value IDs -#define VALUE_ID_STRT_GPS_QMS_OF_WEEK 0x0002 -#define VALUE_ID_STRT_GPS_WEEK 0x0003 - - -//! MONR message */ -typedef struct { - HeaderType header; - uint16_t monrStructValueID; - uint16_t monrStructContentLength; - uint32_t gpsQmsOfWeek; - int32_t xPosition; - int32_t yPosition; - int32_t zPosition; - uint16_t heading; - int16_t longitudinalSpeed; - int16_t lateralSpeed; - int16_t longitudinalAcc; - int16_t lateralAcc; - uint8_t driveDirection; - uint8_t state; - uint8_t readyToArm; - uint8_t errorStatus; - FooterType footer; -} MONRType; - -//! MONR value IDs -#define VALUE_ID_MONR_STRUCT 0x80 - - -/*! HEAB message */ -typedef struct { - HeaderType header; - uint16_t HEABStructValueID; - uint16_t HEABStructContentLength; - uint32_t GPSQmsOfWeek; - uint8_t controlCenterStatus; - FooterType footer; -} HEABType; //16 bytes - -//! HEAB value IDs -#define VALUE_ID_HEAB_STRUCT 0x0090 - - -/*! SYPM message */ -typedef struct { - HeaderType header; - uint16_t syncPointTimeValueID; - uint16_t syncPointTimeContentLength; - uint32_t syncPointTime; - uint16_t freezeTimeValueID; - uint16_t freezeTimeContentLength; - uint32_t freezeTime; - FooterType footer; -} SYPMType; - -//! SYPM value IDs -#define VALUE_ID_SYPM_SYNC_POINT_TIME 0x0001 -#define VALUE_ID_SYPM_FREEZE_TIME 0x0002 - - -/*! MTSP message */ -typedef struct { - HeaderType header; - uint16_t estSyncPointTimeValueID; - uint16_t estSyncPointTimeContentLength; - uint32_t estSyncPointTime; - FooterType footer; -} MTSPType; - -//! MTSP value IDs -#define VALUE_ID_MTSP_EST_SYNC_POINT_TIME 0x0001 - - -/*! TRCM message */ -typedef struct { - HeaderType header; - uint16_t triggerIDValueID; - uint16_t triggerIDContentLength; - uint16_t triggerID; - uint16_t triggerTypeValueID; - uint16_t triggerTypeContentLength; - uint16_t triggerType; - uint16_t triggerTypeParameter1ValueID; - uint16_t triggerTypeParameter1ContentLength; - uint32_t triggerTypeParameter1; - uint16_t triggerTypeParameter2ValueID; - uint16_t triggerTypeParameter2ContentLength; - uint32_t triggerTypeParameter2; - uint16_t triggerTypeParameter3ValueID; - uint16_t triggerTypeParameter3ContentLength; - uint32_t triggerTypeParameter3; - FooterType footer; -} TRCMType; - -//! TRCM value IDs -#define VALUE_ID_TRCM_TRIGGER_ID 0x0001 -#define VALUE_ID_TRCM_TRIGGER_TYPE 0x0002 -#define VALUE_ID_TRCM_TRIGGER_TYPE_PARAM1 0x0011 -#define VALUE_ID_TRCM_TRIGGER_TYPE_PARAM2 0x0012 -#define VALUE_ID_TRCM_TRIGGER_TYPE_PARAM3 0x0013 - - -/*! TREO message */ -typedef struct { - HeaderType header; - uint16_t triggerIDValueID; - uint16_t triggerIDContentLength; - uint16_t triggerID; - uint16_t timestamp_qmsowValueID; - uint16_t timestamp_qmsowContentLength; - uint32_t timestamp_qmsow; - FooterType footer; -} TREOType; - -//! TREO value IDs -#define VALUE_ID_TREO_TRIGGER_ID 0x0001 -#define VALUE_ID_TREO_TRIGGER_TIMESTAMP 0x0002 - - -/*! ACCM message */ -typedef struct { - HeaderType header; - uint16_t actionIDValueID; - uint16_t actionIDContentLength; - uint16_t actionID; - uint16_t actionTypeValueID; - uint16_t actionTypeContentLength; - uint16_t actionType; - uint16_t actionTypeParameter1ValueID; - uint16_t actionTypeParameter1ContentLength; - uint32_t actionTypeParameter1; - uint16_t actionTypeParameter2ValueID; - uint16_t actionTypeParameter2ContentLength; - uint32_t actionTypeParameter2; - uint16_t actionTypeParameter3ValueID; - uint16_t actionTypeParameter3ContentLength; - uint32_t actionTypeParameter3; - FooterType footer; -} ACCMType; - -//! ACCM value IDs -#define VALUE_ID_ACCM_ACTION_ID 0x0002 -#define VALUE_ID_ACCM_ACTION_TYPE 0x0003 -#define VALUE_ID_ACCM_ACTION_TYPE_PARAM1 0x00A1 -#define VALUE_ID_ACCM_ACTION_TYPE_PARAM2 0x00A2 -#define VALUE_ID_ACCM_ACTION_TYPE_PARAM3 0x00A3 - - -/*! EXAC message */ -typedef struct { - HeaderType header; - uint16_t actionIDValueID; - uint16_t actionIDContentLength; - uint16_t actionID; - uint16_t executionTime_qmsoWValueID; - uint16_t executionTime_qmsoWContentLength; - uint32_t executionTime_qmsoW; - FooterType footer; -} EXACType; - -//! EXAC value IDs -#define VALUE_ID_EXAC_ACTION_ID 0x0002 -#define VALUE_ID_EXAC_ACTION_EXECUTE_TIME 0x0003 - -/*! CATA message */ -// TODO -//! CATA value IDs -// TODO - - -/*! INSUP message */ -typedef struct { - HeaderType header; - uint16_t modeValueID; - uint16_t modeContentLength; - uint8_t mode; - FooterType footer; -} INSUPType; - -//! INSUP value IDs -#define VALUE_ID_INSUP_MODE 0x0200 - - -/*! RCMM message */ -typedef struct { - HeaderType header; - uint16_t commandValueID; - uint16_t commandContentLength; - uint8_t command; - FooterType footer; -} RCMMType; - -//! RCMM value IDs -#define VALUE_ID_RCMM_CONTROL 0x0201 - - -#pragma pack(pop) - - -// ************************* Non-ISO type definitions and defines ************************************************ -// Byte swapper definitions for 6 byte values and floats -#if __BYTE_ORDER == __LITTLE_ENDIAN -#define le48toh(x) (x) -#define htole48(x) (x) -#define htolef(x) (x) -#else -#define le48toh(x) (le64toh(x) >> 16) -#define htole48(x) (htole64(x) >> 16) -#define htolef_a(x) \ - htole32((union { uint32_t i; float f; }){ .f = (x) }.i) -#define htolef(x) \ - ((union { uint32_t i; float f; }){ .i = htolef_a(x) }.f) -#endif - -// ************************** static function declarations ******************************************************** -static ISOMessageReturnValue decodeISOHeader(const char *MessageBuffer, const size_t length, - HeaderType * HeaderData, const char debug); -static ISOMessageReturnValue decodeISOFooter(const char *MessageBuffer, const size_t length, - FooterType * HeaderData, const char debug); -static HeaderType buildISOHeader(ISOMessageID id, uint32_t messageLength, const char debug); -static FooterType buildISOFooter(const void *message, const size_t sizeExclFooter, const char debug); -static char isValidMessageID(const uint16_t id); -static double_t mapISOHeadingToHostHeading(const double_t isoHeading_rad); -static double_t mapHostHeadingToISOHeading(const double_t hostHeading_rad); -static void convertMONRToHostRepresentation(const MONRType * MONRData, ObjectMonitorType * monitorData); -static ISOMessageReturnValue verifyChecksum(const void *data, const size_t dataLen, const uint16_t crc, - const char debug); -static uint16_t crcByte(const uint16_t crc, const uint8_t byte); -static uint16_t crc16(const uint8_t * data, size_t dataLen); - -// ************************** static variables ******************************************************************** -static uint16_t trajectoryMessageCrc = 0; -static int8_t isCRCVerificationEnabled = DEFAULT_CRC_CHECK_ENABLED; - -// ************************** function definitions **************************************************************** - -/*! - * \brief decodeISOHeader Convert data in a buffer to an ISO heade - * \param MessageBuffer Buffer containing raw data to be converted - * \param length Length of buffer - * \param HeaderData Struct in which to store resulting data - * \param debug Flag for enabling debugging of this function - * \return value according to ::ISOMessageReturnValue - */ -ISOMessageReturnValue decodeISOHeader(const char *MessageBuffer, const size_t length, HeaderType * HeaderData, - const char debug) { - - const char *p = MessageBuffer; - ISOMessageReturnValue retval = MESSAGE_OK; - const char ProtocolVersionBitmask = 0x7F; - char messageProtocolVersion = 0; - char isProtocolVersionSupported = 0; - - // If not enough data to fill header, generate error - if (length < sizeof (HeaderData)) { - LogMessage(LOG_LEVEL_ERROR, "Too little raw data to fill ISO header"); - memset(HeaderData, 0, sizeof (*HeaderData)); - return MESSAGE_LENGTH_ERROR; - } - - // Decode ISO header - memcpy(&HeaderData->SyncWordU16, p, sizeof (HeaderData->SyncWordU16)); - HeaderData->SyncWordU16 = le16toh(HeaderData->SyncWordU16); - p += sizeof (HeaderData->SyncWordU16); - - // If sync word is not correct, generate error - if (HeaderData->SyncWordU16 != ISO_SYNC_WORD) { - LogMessage(LOG_LEVEL_ERROR, "Sync word error when decoding ISO header"); - memset(HeaderData, 0, sizeof (*HeaderData)); - return MESSAGE_SYNC_WORD_ERROR; - } - - memcpy(&HeaderData->TransmitterIdU8, p, sizeof (HeaderData->TransmitterIdU8)); - p += sizeof (HeaderData->TransmitterIdU8); - - memcpy(&HeaderData->MessageCounterU8, p, sizeof (HeaderData->MessageCounterU8)); - p += sizeof (HeaderData->MessageCounterU8); - - memcpy(&HeaderData->AckReqProtVerU8, p, sizeof (HeaderData->AckReqProtVerU8)); - p += sizeof (HeaderData->AckReqProtVerU8); - - // Loop over permitted protocol versions to see if current version is among them - messageProtocolVersion = HeaderData->AckReqProtVerU8 & ProtocolVersionBitmask; - for (size_t i = 0; i < sizeof (SupportedProtocolVersions) / sizeof (SupportedProtocolVersions[0]); ++i) { - if (SupportedProtocolVersions[i] == messageProtocolVersion) { - isProtocolVersionSupported = 1; - break; - } - } - - // Generate error if protocol version not supported - if (!isProtocolVersionSupported) { - LogMessage(LOG_LEVEL_WARNING, "Protocol version %u not supported", messageProtocolVersion); - retval = MESSAGE_VERSION_ERROR; - memset(HeaderData, 0, sizeof (*HeaderData)); - return retval; - } - - memcpy(&HeaderData->MessageIdU16, p, sizeof (HeaderData->MessageIdU16)); - p += sizeof (HeaderData->MessageIdU16); - HeaderData->MessageIdU16 = le16toh(HeaderData->MessageIdU16); - - memcpy(&HeaderData->MessageLengthU32, p, sizeof (HeaderData->MessageLengthU32)); - p += sizeof (HeaderData->MessageLengthU32); - HeaderData->MessageLengthU32 = le32toh(HeaderData->MessageLengthU32); - - if (debug) { - LogPrint("SyncWordU16 = 0x%x", HeaderData->SyncWordU16); - LogPrint("TransmitterIdU8 = 0x%x", HeaderData->TransmitterIdU8); - LogPrint("MessageCounterU8 = 0x%x", HeaderData->MessageCounterU8); - LogPrint("AckReqProtVerU8 = 0x%x", HeaderData->AckReqProtVerU8); - LogPrint("MessageIdU16 = 0x%x", HeaderData->MessageIdU16); - LogPrint("MessageLengthU32 = 0x%x", HeaderData->MessageLengthU32); - } - - return retval; -} - -/*! - * \brief decodeISOFooter Convert data in a buffer to an ISO footer - * \param MessageBuffer Buffer containing raw data to be converted - * \param length Length of buffer - * \param HeaderData Struct in which to store resulting data - * \param debug Flag for enabling debugging of this function - * \return value according to ::ISOMessageReturnValue - */ -ISOMessageReturnValue decodeISOFooter(const char *MessageBuffer, const size_t length, FooterType * FooterData, - const char debug) { - - // If too little data, generate error - if (length < sizeof (FooterData->Crc)) { - LogMessage(LOG_LEVEL_ERROR, "Too little raw data to fill ISO footer"); - memset(FooterData, 0, sizeof (*FooterData)); - return MESSAGE_LENGTH_ERROR; - } - memcpy(&FooterData->Crc, MessageBuffer, sizeof (FooterData->Crc)); - FooterData->Crc = le16toh(FooterData->Crc); - - if (debug) { - LogPrint("Decoded ISO footer:\n\tCRC: 0x%x", FooterData->Crc); - } - - return MESSAGE_OK; -} - -/*! - * \brief buildISOHeader Constructs an ISO header based on the supplied message ID and content length - * \param id Message ID of the message for which the header is to be used - * \param messageLength Length of the message including header and footer - * \param debug Flag for enabling debugging - * \return A struct containing ISO header data - */ -HeaderType buildISOHeader(ISOMessageID id, uint32_t messageLength, const char debug) { - HeaderType header; - - header.SyncWordU16 = ISO_SYNC_WORD; - header.TransmitterIdU8 = 0; - header.MessageCounterU8 = 0; - header.AckReqProtVerU8 = ACK_REQ | ISO_PROTOCOL_VERSION; - if (messageLength >= sizeof (HeaderType) + sizeof (FooterType)) { - header.MessageIdU16 = (uint16_t) id; - header.MessageLengthU32 = messageLength - sizeof (HeaderType) - sizeof (FooterType); - } - else { - LogMessage(LOG_LEVEL_ERROR, "Supplied message length too small to hold header and footer"); - header.MessageIdU16 = (uint16_t) MESSAGE_ID_INVALID; - header.MessageLengthU32 = 0; - } - - if (debug) { - LogPrint("Encoded ISO header:\n\tSync word: 0x%x\n\tTransmitter ID: %u\n\tMessage counter: %u\n\t" - "Ack request | Protocol version: 0x%x\n\tMessage ID: 0x%x\n\tMessage length: %u", - header.SyncWordU16, header.TransmitterIdU8, header.MessageCounterU8, header.AckReqProtVerU8, - header.MessageIdU16, header.MessageLengthU32); - } - - // Convert from host endianness to little endian - header.SyncWordU16 = htole16(header.SyncWordU16); - header.MessageIdU16 = htole16(header.MessageIdU16); - header.MessageLengthU32 = htole32(header.MessageLengthU32); - - return header; -} - -/*! - * \brief buildISOFooter Constructs a footer for an ISO message - * \param message Pointer to start of message header - * \param messageSize Size of the entire message including header and footer - * \param debug Flag for enabling debugging - * \return A struct containing ISO footer data - */ -FooterType buildISOFooter(const void *message, const size_t messageSize, const char debug) { - FooterType footer; - - // Calculate CRC - remembering that message begins with header and messageSize will include header and footer - footer.Crc = crc16(message, messageSize - sizeof (FooterType)); - - if (debug) { - LogPrint("Encoded ISO footer:\n\tCRC: 0x%x", footer.Crc); - } - - footer.Crc = htole16(footer.Crc); - - return footer; -} - -/*! - * \brief isValidMessageID Determines if specified id is a valid ISO message ID. The reserved range is deemed - * invalid and vendor specific range is deemed valid. - * \param id An ISO message id to be checked - * \return 1 if valid, 0 if not - */ -char isValidMessageID(const uint16_t id) { - return id == MESSAGE_ID_MONR || id == MESSAGE_ID_HEAB || id == MESSAGE_ID_TRAJ || id == MESSAGE_ID_OSEM - || id == MESSAGE_ID_OSTM || id == MESSAGE_ID_STRT || id == MESSAGE_ID_MONR2 || id == MESSAGE_ID_SOWM - || 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_RCMM || id == MESSAGE_ID_RCRT || id == MESSAGE_ID_PIME - || id == MESSAGE_ID_COSE || id == MESSAGE_ID_MOMA - || (id >= MESSAGE_ID_VENDOR_SPECIFIC_LOWER_LIMIT && id <= MESSAGE_ID_VENDOR_SPECIFIC_UPPER_LIMIT); -} - -/*! - * \brief getISOMessageType Determines the ISO message type of a raw data buffer - * \param messageData Buffer containing raw data to be parsed into an ISO message - * \param length Size of buffer to be parsed - * \param debug Flag for enabling debugging information - * \return Value according to ::ISOMessageID - */ -ISOMessageID getISOMessageType(const char *messageData, const size_t length, const char debug) { - HeaderType header; - - // Decode header - if (decodeISOHeader(messageData, length, &header, debug) != MESSAGE_OK) { - LogMessage(LOG_LEVEL_ERROR, "Unable to parse raw data into ISO message header"); - return MESSAGE_ID_INVALID; - } - - // Check if header contains valid message ID, if so return it - if (isValidMessageID(header.MessageIdU16)) - return (ISOMessageID) header.MessageIdU16; - else { - LogMessage(LOG_LEVEL_WARNING, "Message ID %u does not match any known ISO message", - header.MessageIdU16); - return MESSAGE_ID_INVALID; - } -} - - -/*! - * \brief crcByte Updates the given CRC based on an input byte from data - * \param crc CRC from previous byte - * \param byte New data byte - * \return New CRC value - */ -uint16_t crcByte(const uint16_t crc, const uint8_t byte) { - static const uint16_t crcTable[256] = { - 0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7, - 0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF, - 0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6, - 0x9339, 0x8318, 0xB37B, 0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE, - 0x2462, 0x3443, 0x0420, 0x1401, 0x64E6, 0x74C7, 0x44A4, 0x5485, - 0xA56A, 0xB54B, 0x8528, 0x9509, 0xE5EE, 0xF5CF, 0xC5AC, 0xD58D, - 0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, 0x46B4, - 0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC, - 0x48C4, 0x58E5, 0x6886, 0x78A7, 0x0840, 0x1861, 0x2802, 0x3823, - 0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, 0x9969, 0xA90A, 0xB92B, - 0x5AF5, 0x4AD4, 0x7AB7, 0x6A96, 0x1A71, 0x0A50, 0x3A33, 0x2A12, - 0xDBFD, 0xCBDC, 0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A, - 0x6CA6, 0x7C87, 0x4CE4, 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41, - 0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, 0xAD2A, 0xBD0B, 0x8D68, 0x9D49, - 0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70, - 0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A, 0x9F59, 0x8F78, - 0x9188, 0x81A9, 0xB1CA, 0xA1EB, 0xD10C, 0xC12D, 0xF14E, 0xE16F, - 0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, 0x6067, - 0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E, - 0x02B1, 0x1290, 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256, - 0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D, - 0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405, - 0xA7DB, 0xB7FA, 0x8799, 0x97B8, 0xE75F, 0xF77E, 0xC71D, 0xD73C, - 0x26D3, 0x36F2, 0x0691, 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634, - 0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9, 0xB98A, 0xA9AB, - 0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882, 0x28A3, - 0xCB7D, 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A, - 0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92, - 0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9, - 0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1, - 0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8, - 0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0 - }; - - return (uint16_t) ((crc << 8) ^ crcTable[(crc >> 8) ^ byte]); -} - - -/*! - * \brief crc16 Calculates the 16 bit CCITT checksum value for the polynomial - * x^16 + x^12 + x^5 + 1 - * \param data Block of data for which CRC is to be calculated - * \param dataLen Length of the block of data - * \return CRC checksum - */ -uint16_t crc16(const uint8_t * data, size_t dataLen) { - uint16_t crc = DEFAULT_CRC_INIT_VALUE; - - while (dataLen-- > 0) { - crc = crcByte(crc, *data++); - } - return crc; -} - - -/*! - * \brief verifyChecksum Generates a checksum for specified data and checks if it matches against - * the specified CRC. If the specified CRC is 0, the message does not contain a CRC value - * and the message is assumed uncorrupted. - * \param data Data for which checksum is to be verified - * \param dataLen Length of the data - * \param CRC Received CRC value for the data - * \return Value according to ::ISOMessageReturnValue - */ -ISOMessageReturnValue verifyChecksum(const void *data, const size_t dataLen, const uint16_t CRC, - const char debug) { - if (!isCRCVerificationEnabled || CRC == 0) { - return MESSAGE_OK; - } - - const uint16_t dataCRC = crc16(data, dataLen); - - if (debug) { - LogPrint("CRC given: %u, CRC calculated: %u", CRC, dataCRC); - } - return dataCRC == CRC ? MESSAGE_OK : MESSAGE_CRC_ERROR; -} - -/*! - * \brief setISOCRCVerification Enables or disables checksum verification on received messages (default - * is to enable checksum verification) - * \param enabled Boolean for enabling or disabling the checksum verification - */ -void setISOCRCVerification(const int8_t enabled) { - isCRCVerificationEnabled = enabled ? 1 : 0; - return; -} - - -/*! - * \brief encodeTRAJMessageHeader Creates a TRAJ message header based on supplied values and resets - * an internal CRC to be used in the corresponding footer. The header is printed to a buffer. - * \param trajectoryID ID of the trajectory - * \param trajectoryVersion Version of the trajectory - * \param trajectoryName A string of maximum length 63 excluding the null terminator - * \param nameLength Length of the name string excluding the null terminator - * \param numberOfPointsInTraj Number of points in the subsequent trajectory - * \param trajDataBuffer Buffer to which TRAJ header is to be printed - * \param bufferLength Length of buffer to which TRAJ header is to be printed - * \param debug Flag for enabling debugging - * \return Number of bytes printed, or -1 in case of error with the following errnos: - * EINVAL if one of the input parameters are invalid - * ENOBUFS if supplied buffer is too small to hold header - * EMSGSIZE if trajectory name is too long - */ -ssize_t encodeTRAJMessageHeader(const uint16_t trajectoryID, const uint16_t trajectoryVersion, - const char *trajectoryName, const size_t nameLength, - const uint32_t numberOfPointsInTraj, char *trajDataBuffer, - const size_t bufferLength, const char debug) { - - TRAJHeaderType TRAJData; - size_t dataLen; - - memset(trajDataBuffer, 0, bufferLength); - - // Error guarding - if (trajectoryName == NULL && nameLength > 0) { - errno = EINVAL; - LogMessage(LOG_LEVEL_ERROR, "Trajectory name length and pointer mismatch"); - return -1; - } - else if (trajDataBuffer == NULL) { - errno = EINVAL; - LogMessage(LOG_LEVEL_ERROR, "Trajectory data buffer invalid"); - return -1; - } - else if (bufferLength < sizeof (TRAJHeaderType)) { - errno = ENOBUFS; - LogMessage(LOG_LEVEL_ERROR, "Buffer too small to hold necessary TRAJ header data"); - return -1; - } - else if (nameLength >= sizeof (TRAJData.trajectoryName)) { - errno = EMSGSIZE; - LogMessage(LOG_LEVEL_ERROR, "Trajectory name <%s> too long for TRAJ message", trajectoryName); - return -1; - } - - // Construct ISO header - TRAJData.header = buildISOHeader(MESSAGE_ID_TRAJ, sizeof (TRAJHeaderType) - + numberOfPointsInTraj * sizeof (TRAJPointType) + - sizeof (TRAJFooterType), debug); - - // Fill contents - TRAJData.trajectoryIDValueID = VALUE_ID_TRAJ_TRAJECTORY_IDENTIFIER; - TRAJData.trajectoryIDContentLength = sizeof (TRAJData.trajectoryID); - TRAJData.trajectoryID = trajectoryID; - - TRAJData.trajectoryVersionValueID = VALUE_ID_TRAJ_TRAJECTORY_VERSION; - TRAJData.trajectoryVersionContentLength = sizeof (TRAJData.trajectoryVersion); - TRAJData.trajectoryVersion = trajectoryVersion; - - TRAJData.trajectoryNameValueID = VALUE_ID_TRAJ_TRAJECTORY_NAME; - TRAJData.trajectoryNameContentLength = sizeof (TRAJData.trajectoryName); - memset(TRAJData.trajectoryName, 0, sizeof (TRAJData.trajectoryName)); - if (trajectoryName != NULL) { - memcpy(&TRAJData.trajectoryName, trajectoryName, nameLength); - } - - if (debug) { - LogPrint("TRAJ message header:\n\t" - "Trajectory ID value ID: 0x%x\n\t" - "Trajectory ID content length: %u\n\t" - "Trajectory ID: %u\n\t" - "Trajectory name value ID: 0x%x\n\t" - "Trajectory name content length: %u\n\t" - "Trajectory name: %s\n\t" - "Trajectory version value ID: 0x%x\n\t" - "Trajectory version content length: %u\n\t" - "Trajectory version: %u", TRAJData.trajectoryIDValueID, - TRAJData.trajectoryIDContentLength, TRAJData.trajectoryID, - TRAJData.trajectoryNameValueID, TRAJData.trajectoryNameContentLength, - TRAJData.trajectoryName, TRAJData.trajectoryVersionValueID, - TRAJData.trajectoryVersionContentLength, TRAJData.trajectoryVersion); - } - - // Switch endianness to little endian for all fields - TRAJData.trajectoryIDValueID = htole16(TRAJData.trajectoryIDValueID); - TRAJData.trajectoryIDContentLength = htole16(TRAJData.trajectoryIDContentLength); - TRAJData.trajectoryID = htole16(TRAJData.trajectoryID); - TRAJData.trajectoryVersionValueID = htole16(TRAJData.trajectoryVersionValueID); - TRAJData.trajectoryVersionContentLength = htole16(TRAJData.trajectoryVersionContentLength); - TRAJData.trajectoryVersion = htole16(TRAJData.trajectoryVersion); - TRAJData.trajectoryNameValueID = htole16(TRAJData.trajectoryNameValueID); - TRAJData.trajectoryNameContentLength = htole16(TRAJData.trajectoryNameContentLength); - - // Reset CRC - trajectoryMessageCrc = DEFAULT_CRC_INIT_VALUE; - - memcpy(trajDataBuffer, &TRAJData, sizeof (TRAJData)); - - // Update CRC - dataLen = sizeof (TRAJData); - while (dataLen-- > 0) { - trajectoryMessageCrc = crcByte(trajectoryMessageCrc, (uint8_t) (*trajDataBuffer++)); - } - return sizeof (TRAJHeaderType); -} - - -/*! - * \brief encodeTRAJMessagePoint Creates a TRAJ message point based on supplied values and updates an internal - * CRC to be used in the footer. Also prints the TRAJ point to a buffer. - * \param pointTimeFromStart Time from start of the trajectory point - * \param position Position of the point - * \param speed Speed at the point - * \param acceleration Acceleration at the point - * \param curvature Curvature of the trajectory at the point - * \param trajDataBufferPointer Buffer to which the message is to be printed - * \param remainingBufferLength Remaining bytes in the buffer to which the message is to be printed - * \param debug Flag for enabling debugging - * \return Number of bytes printed, or -1 in case of error with the following errnos: - * EINVAL if one of the input parameters are invalid - * ENOBUFS if supplied buffer is too small to hold point - */ -ssize_t encodeTRAJMessagePoint(const struct timeval *pointTimeFromStart, const CartesianPosition position, - const SpeedType speed, const AccelerationType acceleration, - const float curvature, char *trajDataBufferPointer, - const size_t remainingBufferLength, const char debug) { - - TRAJPointType TRAJData; - size_t dataLen; - - if (remainingBufferLength < sizeof (TRAJPointType)) { - errno = ENOBUFS; - LogMessage(LOG_LEVEL_DEBUG, "Buffer too small to hold necessary TRAJ point data"); - return -1; - } - else if (trajDataBufferPointer == NULL) { - errno = EINVAL; - LogMessage(LOG_LEVEL_ERROR, "Trajectory data buffer invalid"); - return -1; - } - - // Fill contents - TRAJData.relativeTimeValueID = VALUE_ID_TRAJ_RELATIVE_TIME; - TRAJData.relativeTimeContentLength = sizeof (TRAJData.relativeTime); - TRAJData.relativeTime = - (uint32_t) TimeGetAsUTCms(pointTimeFromStart) / (1000 / RELATIVE_TIME_ONE_SECOND_VALUE); - - TRAJData.xPositionValueID = VALUE_ID_TRAJ_X_POSITION; - TRAJData.xPositionContentLength = sizeof (TRAJData.xPosition); - TRAJData.yPositionValueID = VALUE_ID_TRAJ_Y_POSITION; - TRAJData.yPositionContentLength = sizeof (TRAJData.yPosition); - TRAJData.zPositionValueID = VALUE_ID_TRAJ_Z_POSITION; - TRAJData.zPositionContentLength = sizeof (TRAJData.zPosition); - if (position.isPositionValid) { - TRAJData.xPosition = (int32_t) (position.xCoord_m * POSITION_ONE_METER_VALUE); - TRAJData.yPosition = (int32_t) (position.yCoord_m * POSITION_ONE_METER_VALUE); - TRAJData.zPosition = (int32_t) (position.zCoord_m * POSITION_ONE_METER_VALUE); - } - else { - errno = EINVAL; - LogMessage(LOG_LEVEL_ERROR, "Position is a required field in TRAJ messages"); - return -1; - } - - TRAJData.headingValueID = VALUE_ID_TRAJ_HEADING; - TRAJData.headingContentLength = sizeof (TRAJData.heading); - if (position.isHeadingValid) { - TRAJData.heading = (uint16_t) (mapHostHeadingToISOHeading(position.heading_rad) - * 180.0 / M_PI * HEADING_ONE_DEGREE_VALUE); - } - else { - TRAJData.heading = HEADING_UNAVAILABLE_VALUE; - } - - TRAJData.longitudinalSpeedValueID = VALUE_ID_TRAJ_LONGITUDINAL_SPEED; - TRAJData.longitudinalSpeedContentLength = sizeof (TRAJData.longitudinalSpeed); - TRAJData.lateralSpeedValueID = VALUE_ID_TRAJ_LATERAL_SPEED; - TRAJData.lateralSpeedContentLength = sizeof (TRAJData.lateralSpeed); - if (speed.isLongitudinalValid) { - TRAJData.longitudinalSpeed = (int16_t) (speed.longitudinal_m_s * SPEED_ONE_METER_PER_SECOND_VALUE); - } - else { - errno = EINVAL; - LogMessage(LOG_LEVEL_ERROR, "Longitudinal speed is a required field in TRAJ messages"); - return -1; - } - TRAJData.lateralSpeed = - speed.isLateralValid ? (int16_t) (speed.lateral_m_s * - SPEED_ONE_METER_PER_SECOND_VALUE) : SPEED_UNAVAILABLE_VALUE; - - TRAJData.longitudinalAccelerationValueID = VALUE_ID_TRAJ_LONGITUDINAL_ACCELERATION; - TRAJData.longitudinalAccelerationContentLength = sizeof (TRAJData.longitudinalAcceleration); - TRAJData.lateralAccelerationValueID = VALUE_ID_TRAJ_LATERAL_ACCELERATION; - TRAJData.lateralAccelerationContentLength = sizeof (TRAJData.lateralAcceleration); - TRAJData.longitudinalAcceleration = acceleration.isLongitudinalValid ? - (int16_t) (acceleration.longitudinal_m_s2 * - ACCELERATION_ONE_METER_PER_SECOND_SQUARED_VALUE) : ACCELERATION_UNAVAILABLE_VALUE; - TRAJData.lateralAcceleration = - acceleration.isLateralValid ? (int16_t) (acceleration.lateral_m_s2 * - ACCELERATION_ONE_METER_PER_SECOND_SQUARED_VALUE) : - ACCELERATION_UNAVAILABLE_VALUE; - - TRAJData.curvatureValueID = VALUE_ID_TRAJ_CURVATURE; - TRAJData.curvatureContentLength = sizeof (TRAJData.curvature); - TRAJData.curvature = curvature; - - if (debug) { - LogPrint("TRAJ message point:\n\t" - "Relative time value ID: 0x%x\n\t" - "Relative time content length: %u\n\t" - "Relative time: %u\n\t" - "x position value ID: 0x%x\n\t" - "x position content length: %u\n\t" - "x position: %d\n\t" - "y position value ID: 0x%x\n\t" - "y position content length: %u\n\t" - "y position: %d\n\t" - "z position value ID: 0x%x\n\t" - "z position content length: %u\n\t" - "z position: %d\n\t" - "Heading value ID: 0x%x\n\t" - "Heading content length: %u\n\t" - "Heading: %u\n\t" - "Longitudinal speed value ID: 0x%x\n\t" - "Longitudinal speed content length: %u\n\t" - "Longitudinal speed: %d\n\t" - "Lateral speed value ID: 0x%x\n\t" - "Lateral speed content length: %u\n\t" - "Lateral speed: %d\n\t" - "Longitudinal acceleration value ID: 0x%x\n\t" - "Longitudinal acceleration content length: %u\n\t" - "Longitudinal acceleration: %d\n\t" - "Lateral acceleration value ID: 0x%x\n\t" - "Lateral acceleration content length: %u\n\t" - "Lateral acceleration: %d\n\t" - "Curvature value ID: 0x%x\n\t" - "Curvature content length: %u\n\t" - "Curvature: %.6f", - TRAJData.relativeTimeValueID, TRAJData.relativeTimeContentLength, - TRAJData.relativeTime, TRAJData.xPositionValueID, TRAJData.xPositionContentLength, - TRAJData.xPosition, TRAJData.yPositionValueID, TRAJData.yPositionContentLength, - TRAJData.yPosition, TRAJData.zPositionValueID, TRAJData.zPositionContentLength, - TRAJData.zPosition, TRAJData.headingValueID, TRAJData.headingContentLength, - TRAJData.heading, TRAJData.longitudinalSpeedValueID, TRAJData.longitudinalSpeedContentLength, - TRAJData.longitudinalSpeed, TRAJData.lateralSpeedValueID, TRAJData.lateralSpeedContentLength, - TRAJData.lateralSpeed, TRAJData.longitudinalAccelerationValueID, - TRAJData.longitudinalAccelerationContentLength, TRAJData.longitudinalAcceleration, - TRAJData.lateralAccelerationValueID, TRAJData.lateralAccelerationContentLength, - TRAJData.lateralAcceleration, TRAJData.curvatureValueID, TRAJData.curvatureContentLength, - (double_t) TRAJData.curvature); - } - - // Convert from host endianness to little endian - TRAJData.relativeTimeValueID = htole16(TRAJData.relativeTimeValueID); - TRAJData.relativeTimeContentLength = htole16(TRAJData.relativeTimeContentLength); - TRAJData.relativeTime = htole32(TRAJData.relativeTime); - TRAJData.xPositionValueID = htole16(TRAJData.xPositionValueID); - TRAJData.xPositionContentLength = htole16(TRAJData.xPositionContentLength); - TRAJData.xPosition = (int32_t) htole32(TRAJData.xPosition); - TRAJData.yPositionValueID = htole16(TRAJData.yPositionValueID); - TRAJData.yPositionContentLength = htole16(TRAJData.yPositionContentLength); - TRAJData.yPosition = (int32_t) htole32(TRAJData.yPosition); - TRAJData.zPositionValueID = htole16(TRAJData.zPositionValueID); - TRAJData.zPositionContentLength = htole16(TRAJData.zPositionContentLength); - TRAJData.zPosition = (int32_t) htole32(TRAJData.zPosition); - TRAJData.headingValueID = htole16(TRAJData.headingValueID); - TRAJData.headingContentLength = htole16(TRAJData.headingContentLength); - TRAJData.heading = htole16(TRAJData.heading); - TRAJData.longitudinalSpeedValueID = htole16(TRAJData.longitudinalSpeedValueID); - TRAJData.longitudinalSpeedContentLength = htole16(TRAJData.longitudinalSpeedContentLength); - TRAJData.longitudinalSpeed = (int16_t) htole16(TRAJData.longitudinalSpeed); - TRAJData.lateralSpeedValueID = htole16(TRAJData.lateralSpeedValueID); - TRAJData.lateralSpeedContentLength = htole16(TRAJData.lateralSpeedContentLength); - TRAJData.lateralSpeed = (int16_t) htole16(TRAJData.lateralSpeed); - TRAJData.longitudinalAccelerationValueID = htole16(TRAJData.longitudinalAccelerationValueID); - TRAJData.longitudinalAccelerationContentLength = htole16(TRAJData.longitudinalAccelerationContentLength); - TRAJData.longitudinalAcceleration = (int16_t) htole16(TRAJData.longitudinalAcceleration); - TRAJData.lateralAccelerationValueID = htole16(TRAJData.lateralAccelerationValueID); - TRAJData.lateralAccelerationContentLength = htole16(TRAJData.lateralAccelerationContentLength); - TRAJData.lateralAcceleration = (int16_t) htole16(TRAJData.lateralAcceleration); - TRAJData.curvatureValueID = htole16(TRAJData.curvatureValueID); - TRAJData.curvatureContentLength = htole16(TRAJData.curvatureContentLength); - TRAJData.curvature = htolef(TRAJData.curvature); - - memcpy(trajDataBufferPointer, &TRAJData, sizeof (TRAJData)); - - // Update CRC - dataLen = sizeof (TRAJData); - while (dataLen-- > 0) { - trajectoryMessageCrc = crcByte(trajectoryMessageCrc, (uint8_t) (*trajDataBufferPointer++)); - } - - return sizeof (TRAJPointType); -} - - -/*! - * \brief encodeTRAJMessageFooter Creates a TRAJ message footer based on an internal CRC from previous header - * and points, and prints it to a buffer. - * \param trajDataBuffer Buffer to which TRAJ message is to be printed - * \param remainingBufferLength Remaining bytes in the buffer to which the message is to be printed - * \param debug Flag for enabling debugging - * \return Number of bytes printed, or -1 in case of error with the following errnos: - * EINVAL if one of the input parameters are invalid - * ENOBUFS if supplied buffer is too small to hold footer - */ -ssize_t encodeTRAJMessageFooter(char *trajDataBuffer, const size_t remainingBufferLength, const char debug) { - - TRAJFooterType TRAJData; - - if (remainingBufferLength < sizeof (TRAJFooterType)) { - errno = ENOBUFS; - LogMessage(LOG_LEVEL_DEBUG, "Buffer too small to hold TRAJ footer data"); - return -1; - } - else if (trajDataBuffer == NULL) { - errno = EINVAL; - LogMessage(LOG_LEVEL_ERROR, "Invalid trajectory data buffer supplied"); - return -1; - } - - TRAJData.footer.Crc = trajectoryMessageCrc; - - memcpy(trajDataBuffer, &TRAJData, sizeof (TRAJData)); - - if (debug) { - LogPrint("Encoded ISO footer:\n\tCRC: 0x%x", TRAJData.footer.Crc); - } - - return sizeof (TRAJFooterType); -} - - -/*! - * \brief encodeOSEMMessage Creates an OSEM message and writes it into a buffer based on supplied values. All values are passed as pointers and - * passing them as NULL causes the OSEM message to contain a default value for that field (a value representing "unavailable" or similar). - * \param latitude_deg Latitude in degrees of the test origin - * \param longitude_deg Longitude in degrees of the test origin - * \param altitude_m Altitude in meters above sea level of the test origin - * \param maxWayDeviation_m Maximum allowed deviation from target trajectory point, in meters - * \param maxLateralDeviation_m Maximum lateral deviation from trajectory allowed, in meters - * \param minimumPositioningAccuracy_m Minimum positioning accuracy required of the object - * \param osemDataBuffer Buffer to which OSEM message is to be written - * \param bufferLength Size of the buffer to which OSEM message is to be written - * \param debug Flag for enabling debugging - * \return Number of bytes written to the buffer, or -1 in case of an error - */ -ssize_t encodeOSEMMessage(const double_t * latitude_deg, const double_t * longitude_deg, - const float *altitude_m, const float *maxWayDeviation_m, - const float *maxLateralDeviation_m, const float *minimumPositioningAccuracy_m, - char *osemDataBuffer, const size_t bufferLength, const char debug) { - - const char SizeDifference64bitTo48bit = 2; - OSEMType OSEMData; - struct timeval currentTime; - struct tm *printableTime; - char *p = osemDataBuffer; - - TimeSetToCurrentSystemTime(¤tTime); - printableTime = localtime(¤tTime.tv_sec); - - memset(osemDataBuffer, 0, bufferLength); - - // If buffer too small to hold OSEM data, generate an error - if (bufferLength < sizeof (OSEMData) - 2 * SizeDifference64bitTo48bit) { - LogMessage(LOG_LEVEL_ERROR, "Buffer too small to hold necessary OSEM data"); - return -1; - } - - // Build header, and account for the two values which are 48 bit in the message - OSEMData.header = buildISOHeader(MESSAGE_ID_OSEM, sizeof (OSEMData) - - 2 * SizeDifference64bitTo48bit, debug); - - // Fill the OSEM struct with relevant values - OSEMData.latitudeValueID = VALUE_ID_OSEM_LATITUDE; - OSEMData.latitudeContentLength = sizeof (OSEMData.latitude) - SizeDifference64bitTo48bit; - OSEMData.latitude = (latitude_deg == NULL) ? - LATITUDE_UNAVAILABLE_VALUE : (int64_t) (*latitude_deg * LATITUDE_ONE_DEGREE_VALUE); - - OSEMData.longitudeValueID = VALUE_ID_OSEM_LONGITUDE; - OSEMData.longitudeContentLength = sizeof (OSEMData.longitude) - SizeDifference64bitTo48bit; - OSEMData.longitude = (longitude_deg == NULL) ? - LONGITUDE_UNAVAILABLE_VALUE : (int64_t) (*longitude_deg * LONGITUDE_ONE_DEGREE_VALUE); - - OSEMData.altitudeValueID = VALUE_ID_OSEM_ALTITUDE; - OSEMData.altitudeContentLength = sizeof (OSEMData.altitude); - OSEMData.altitude = (altitude_m == NULL) ? - ALTITUDE_UNAVAILABLE_VALUE : (int32_t) (*altitude_m * ALTITUDE_ONE_METER_VALUE); - - OSEMData.dateValueID = VALUE_ID_OSEM_DATE; - OSEMData.dateContentLength = sizeof (OSEMData.date); - OSEMData.date = (uint32_t) ((printableTime->tm_year + 1900) * 10000 + (printableTime->tm_mon + 1) * 100 - + (printableTime->tm_mday)); - - OSEMData.GPSWeekValueID = VALUE_ID_OSEM_GPS_WEEK; - OSEMData.GPSWeekContentLength = sizeof (OSEMData.GPSWeek); - OSEMData.GPSWeek = TimeGetAsGPSweek(¤tTime); - - OSEMData.GPSQmsOfWeekValueID = VALUE_ID_OSEM_GPS_QUARTER_MILLISECOND_OF_WEEK; - OSEMData.GPSQmsOfWeekContentLength = sizeof (OSEMData.GPSQmsOfWeek); - OSEMData.GPSQmsOfWeek = TimeGetAsGPSqmsOfWeek(¤tTime); - - OSEMData.maxWayDeviationValueID = VALUE_ID_OSEM_MAX_WAY_DEVIATION; - OSEMData.maxWayDeviationContentLength = sizeof (OSEMData.maxWayDeviation); - OSEMData.maxWayDeviation = (maxWayDeviation_m == NULL) ? - MAX_WAY_DEVIATION_UNAVAILABLE_VALUE : (uint16_t) (*maxWayDeviation_m * - MAX_WAY_DEVIATION_ONE_METER_VALUE); - - OSEMData.maxLateralDeviationValueID = VALUE_ID_OSEM_MAX_LATERAL_DEVIATION; - OSEMData.maxLateralDeviationContentLength = sizeof (OSEMData.maxLateralDeviation); - OSEMData.maxLateralDeviation = (maxLateralDeviation_m == NULL) ? - MAX_LATERAL_DEVIATION_UNAVAILABLE_VALUE : (uint16_t) (*maxLateralDeviation_m * - MAX_LATERAL_DEVIATION_ONE_METER_VALUE); - - OSEMData.minPosAccuracyValueID = VALUE_ID_OSEM_MIN_POSITIONING_ACCURACY; - OSEMData.minPosAccuracyContentLength = sizeof (OSEMData.minPosAccuracy); - OSEMData.minPosAccuracy = (minimumPositioningAccuracy_m == NULL) ? - MIN_POSITIONING_ACCURACY_NOT_REQUIRED_VALUE : (uint16_t) (*minimumPositioningAccuracy_m * - MIN_POSITIONING_ACCURACY_ONE_METER_VALUE); - - if (debug) { - LogPrint - ("OSEM message:\n\tLatitude value ID: 0x%x\n\tLatitude content length: %u\n\tLatitude: %ld [100 nanodegrees]\n\t" - "Longitude value ID: 0x%x\n\tLongitude content length: %u\n\tLongitude: %ld [100 nanodegrees]\n\t" - "Altitude value ID: 0x%x\n\tAltitude content length: %u\n\tAltitude: %d [cm]\n\t" - "Date value ID: 0x%x\n\tDate content length: %u\n\tDate: %u\n\t" - "GPS week value ID: 0x%x\n\tGPS week content length: %u\n\tGPS week: %u\n\t" - "GPS second of week value ID: 0x%x\n\tGPS second of week content length: %u\n\tGPS second of week: %u [¼ ms]\n\t" - "Max way deviation value ID: 0x%x\n\tMax way deviation content length: %u\n\tMax way deviation: %u\n\t" - "Max lateral deviation value ID: 0x%x\n\tMax lateral deviation content length: %u\n\t" - "Min positioning accuracy value ID: 0x%x\n\tMin positioning accuracy content length: %u\n\tMin positioning accuracy: %u", - OSEMData.latitudeValueID, OSEMData.latitudeContentLength, OSEMData.latitude, - OSEMData.longitudeValueID, OSEMData.longitudeContentLength, OSEMData.longitude, - OSEMData.altitudeValueID, OSEMData.altitudeContentLength, OSEMData.altitude, - OSEMData.dateValueID, OSEMData.dateContentLength, OSEMData.date, OSEMData.GPSWeekValueID, - OSEMData.GPSWeekContentLength, OSEMData.GPSWeek, OSEMData.GPSQmsOfWeekValueID, - OSEMData.GPSQmsOfWeekContentLength, OSEMData.GPSQmsOfWeek, OSEMData.maxWayDeviationValueID, - OSEMData.maxWayDeviationContentLength, OSEMData.maxWayDeviation, - OSEMData.maxLateralDeviationValueID, OSEMData.maxLateralDeviationContentLength, - OSEMData.maxLateralDeviation, OSEMData.minPosAccuracyValueID, - OSEMData.minPosAccuracyContentLength, OSEMData.minPosAccuracy); - } - - // Switch endianness to little endian for all fields - OSEMData.latitudeValueID = htole16(OSEMData.latitudeValueID); - OSEMData.latitudeContentLength = htole16(OSEMData.latitudeContentLength); - OSEMData.latitude = (int64_t) htole48(OSEMData.latitude); - OSEMData.longitudeValueID = htole16(OSEMData.longitudeValueID); - OSEMData.longitudeContentLength = htole16(OSEMData.longitudeContentLength); - OSEMData.longitude = (int64_t) htole48(OSEMData.longitude); - OSEMData.altitudeValueID = htole16(OSEMData.altitudeValueID); - OSEMData.altitudeContentLength = htole16(OSEMData.altitudeContentLength); - OSEMData.altitude = (int32_t) htole32(OSEMData.altitude); - OSEMData.dateValueID = htole16(OSEMData.dateValueID); - OSEMData.dateContentLength = htole16(OSEMData.dateContentLength); - OSEMData.date = htole32(OSEMData.date); - OSEMData.GPSWeekValueID = htole16(OSEMData.GPSWeekValueID); - OSEMData.GPSWeekContentLength = htole16(OSEMData.GPSWeekContentLength); - OSEMData.GPSWeek = htole16(OSEMData.GPSWeek); - OSEMData.GPSQmsOfWeekValueID = htole16(OSEMData.GPSQmsOfWeekValueID); - OSEMData.GPSQmsOfWeekContentLength = htole16(OSEMData.GPSQmsOfWeekContentLength); - OSEMData.GPSQmsOfWeek = htole32(OSEMData.GPSQmsOfWeek); - OSEMData.maxWayDeviationValueID = htole16(OSEMData.maxWayDeviationValueID); - OSEMData.maxWayDeviationContentLength = htole16(OSEMData.maxWayDeviationContentLength); - OSEMData.maxWayDeviation = htole16(OSEMData.maxWayDeviation); - OSEMData.maxLateralDeviationValueID = htole16(OSEMData.maxLateralDeviationValueID); - OSEMData.maxLateralDeviationContentLength = htole16(OSEMData.maxLateralDeviationContentLength); - OSEMData.maxLateralDeviation = htole16(OSEMData.maxLateralDeviation); - OSEMData.minPosAccuracyValueID = htole16(OSEMData.minPosAccuracyValueID); - OSEMData.minPosAccuracyContentLength = htole16(OSEMData.minPosAccuracyContentLength); - OSEMData.minPosAccuracy = htole16(OSEMData.minPosAccuracy); - - - // Copy data from OSEM struct into the buffer - // Must be done before constructing the footer due to the two 48bit size anomalies - memcpy(p, &OSEMData.header, sizeof (OSEMData.header)); - p += sizeof (OSEMData.header); - - // Special handling of 48 bit value - memcpy(p, &OSEMData.latitudeValueID, - sizeof (OSEMData.latitudeValueID) + sizeof (OSEMData.latitudeContentLength)); - p += sizeof (OSEMData.latitudeValueID) + sizeof (OSEMData.latitudeContentLength); - memcpy(p, &OSEMData.latitude, sizeof (OSEMData.latitude) - SizeDifference64bitTo48bit); - p += sizeof (OSEMData.latitude) - SizeDifference64bitTo48bit; - - // Special handling of 48 bit value - memcpy(p, &OSEMData.longitudeValueID, - sizeof (OSEMData.longitudeValueID) + sizeof (OSEMData.longitudeContentLength)); - p += sizeof (OSEMData.longitudeValueID) + sizeof (OSEMData.longitudeContentLength); - memcpy(p, &OSEMData.longitude, sizeof (OSEMData.longitude) - SizeDifference64bitTo48bit); - p += sizeof (OSEMData.longitude) - SizeDifference64bitTo48bit; - - // Copy rest of struct (excluding footer) directly into buffer since no more byte anomalies remain - memcpy(p, &OSEMData.altitudeValueID, sizeof (OSEMData) - sizeof (OSEMData.footer) - - (size_t) (p - osemDataBuffer + 2 * SizeDifference64bitTo48bit)); - p += sizeof (OSEMData) - sizeof (OSEMData.footer) - (size_t) (p - osemDataBuffer + - 2 * SizeDifference64bitTo48bit); - - // Build footer - OSEMData.footer = - buildISOFooter(osemDataBuffer, sizeof (OSEMType) - 2 * SizeDifference64bitTo48bit, debug); - memcpy(p, &OSEMData.footer, sizeof (OSEMData.footer)); - - return sizeof (OSEMType) - 2 * SizeDifference64bitTo48bit; -} - -/*! - * \brief encodeOSTMMessage Constructs an ISO OSTM message based on specified command - * \param command Command to send to object according to ::ObjectCommandType - * \param ostmDataBuffer Data buffer to which OSTM is to be written - * \param bufferLength Length of data buffer to which OSTM is to be written - * \param debug Flag for enabling debugging - * \return Number of bytes written to buffer, or -1 in case of error - */ -ssize_t encodeOSTMMessage(const ObjectCommandType command, char *ostmDataBuffer, const size_t bufferLength, - const char debug) { - - OSTMType OSTMData; - - memset(ostmDataBuffer, 0, bufferLength); - - // Check so buffer can hold message - if (bufferLength < sizeof (OSTMData)) { - LogMessage(LOG_LEVEL_ERROR, "Buffer too small to hold necessary OSTM data"); - return -1; - } - - // Check vs allowed commands - if (! - (command == OBJECT_COMMAND_ARM || command == OBJECT_COMMAND_DISARM - || command == OBJECT_COMMAND_REMOTE_CONTROL)) { - LogMessage(LOG_LEVEL_ERROR, "OSTM does not support command %u", (uint8_t) command); - return -1; - } - - // Construct header - OSTMData.header = buildISOHeader(MESSAGE_ID_OSTM, sizeof (OSTMData), debug); - - // Fill contents - OSTMData.stateValueID = VALUE_ID_OSTM_STATE_CHANGE_REQUEST; - OSTMData.stateContentLength = sizeof (OSTMData.state); - OSTMData.state = (uint8_t) command; - - if (debug) { - LogPrint("OSTM message:\n\tState change request value ID: 0x%x\n\t" - "State change request content length: %u\n\tState change request: %u", - OSTMData.stateValueID, OSTMData.stateContentLength, OSTMData.state); - } - - // Convert from host endianness to little endian - OSTMData.stateValueID = htole16(OSTMData.stateValueID); - OSTMData.stateContentLength = htole16(OSTMData.stateContentLength); - - // Construct footer - OSTMData.footer = buildISOFooter(&OSTMData, sizeof (OSTMData), debug); - - memcpy(ostmDataBuffer, &OSTMData, sizeof (OSTMData)); - - return sizeof (OSTMType); -} - - -/*! - * \brief encodeSTRTMessage Constructs an ISO STRT message based on start time parameters - * \param timeOfStart Time when test shall start, a value of NULL indicates that the time is not known - * \param strtDataBuffer Data buffer in which to place encoded STRT message - * \param bufferLength Size of data buffer in which to place encoded STRT message - * \param debug Flag for enabling debugging - * \return number of bytes written to the data buffer, or -1 if an error occurred - */ -ssize_t encodeSTRTMessage(const struct timeval *timeOfStart, char *strtDataBuffer, - const size_t bufferLength, const char debug) { - - STRTType STRTData; - - memset(strtDataBuffer, 0, bufferLength); - - // If buffer too small to hold STRT data, generate an error - if (bufferLength < sizeof (STRTType)) { - LogMessage(LOG_LEVEL_ERROR, "Buffer too small to hold necessary STRT data"); - return -1; - } - - STRTData.header = buildISOHeader(MESSAGE_ID_STRT, sizeof (STRTType), debug); - - // Fill contents - STRTData.StartTimeValueIdU16 = VALUE_ID_STRT_GPS_QMS_OF_WEEK; - STRTData.StartTimeContentLengthU16 = sizeof (STRTData.StartTimeU32); - STRTData.StartTimeU32 = - timeOfStart == NULL ? GPS_SECOND_OF_WEEK_UNAVAILABLE_VALUE : TimeGetAsGPSqmsOfWeek(timeOfStart); - STRTData.GPSWeekValueID = VALUE_ID_STRT_GPS_WEEK; - STRTData.GPSWeekContentLength = sizeof (STRTData.GPSWeek); - STRTData.GPSWeek = timeOfStart == NULL ? GPS_WEEK_UNAVAILABLE_VALUE : TimeGetAsGPSweek(timeOfStart); - - if (debug) { - LogPrint("STRT message:\n\tGPS second of week value ID: 0x%x\n\t" - "GPS second of week content length: %u\n\tGPS second of week: %u [¼ ms]\n\t" - "GPS week value ID: 0x%x\n\tGPS week content length: %u\n\t" - "GPS week: %u", STRTData.StartTimeValueIdU16, STRTData.StartTimeContentLengthU16, - STRTData.StartTimeU32, STRTData.GPSWeekValueID, STRTData.GPSWeekContentLength, - STRTData.GPSWeek); - } - - // Swap from host endianness to little endian - STRTData.StartTimeValueIdU16 = htole16(STRTData.StartTimeValueIdU16); - STRTData.StartTimeContentLengthU16 = htole16(STRTData.StartTimeContentLengthU16); - STRTData.StartTimeU32 = htole32(STRTData.StartTimeU32); - STRTData.GPSWeekValueID = htole16(STRTData.GPSWeekValueID); - STRTData.GPSWeekContentLength = htole16(STRTData.GPSWeekContentLength); - STRTData.GPSWeek = htole16(STRTData.GPSWeek); - - // Construct footer - STRTData.footer = buildISOFooter(&STRTData, sizeof (STRTType), debug); - - memcpy(strtDataBuffer, &STRTData, sizeof (STRTType)); - - return sizeof (STRTType); -} - -/*! - * \brief encodeHEABMessage Constructs an ISO HEAB message based on current control center status and system time - * \param status Current control center status according to ::ControlCenterStatusType. Entering an unaccepable value - * makes this parameter default to ABORT - * \param heabDataBuffer Buffer to which HEAB message is to be written - * \param bufferLength Size of buffer to which HEAB message is to be written - * \param debug Flag for enabling debugging - * \return Number of bytes written or -1 in case of an error - */ -ssize_t encodeHEABMessage(const ControlCenterStatusType status, char *heabDataBuffer, - const size_t bufferLength, const char debug) { - - HEABType HEABData; - struct timeval currentTime; - - TimeSetToCurrentSystemTime(¤tTime); - - memset(heabDataBuffer, 0, bufferLength); - - // If buffer too small to hold HEAB data, generate an error - if (bufferLength < sizeof (HEABType)) { - LogMessage(LOG_LEVEL_ERROR, "Buffer too small to hold necessary HEAB data"); - return -1; - } - - // Construct header - HEABData.header = buildISOHeader(MESSAGE_ID_HEAB, sizeof (HEABData), debug); - - // Fill contents - HEABData.HEABStructValueID = VALUE_ID_HEAB_STRUCT; - HEABData.HEABStructContentLength = sizeof (HEABType) - sizeof (HeaderType) - sizeof (FooterType) - - sizeof (HEABData.HEABStructValueID) - sizeof (HEABData.HEABStructContentLength); - HEABData.GPSQmsOfWeek = TimeGetAsGPSqmsOfWeek(¤tTime); - if (!(status == CONTROL_CENTER_STATUS_INIT || status == CONTROL_CENTER_STATUS_READY - || status == CONTROL_CENTER_STATUS_ABORT || status == CONTROL_CENTER_STATUS_RUNNING - || status == CONTROL_CENTER_STATUS_TEST_DONE || status == CONTROL_CENTER_STATUS_NORMAL_STOP)) { - LogMessage(LOG_LEVEL_ERROR, "HEAB does not support status ID %u - defaulting to ABORT", - (uint8_t) status); - HEABData.controlCenterStatus = (uint8_t) CONTROL_CENTER_STATUS_ABORT; - } - else { - HEABData.controlCenterStatus = (uint8_t) status; - } - - if (debug) { - LogPrint("HEAB message:\n\tHEAB struct value ID: 0x%x\n\t" - "HEAB struct content length: %u\n\tGPS second of week: %u [¼ ms]\n\t" - "Control center status: 0x%x", HEABData.HEABStructValueID, HEABData.HEABStructContentLength, - HEABData.GPSQmsOfWeek, HEABData.controlCenterStatus); - } - - // Switch from host endianness to little endian - HEABData.HEABStructValueID = htole16(HEABData.HEABStructValueID); - HEABData.HEABStructContentLength = htole16(HEABData.HEABStructContentLength); - HEABData.GPSQmsOfWeek = htole32(HEABData.GPSQmsOfWeek); - - HEABData.footer = buildISOFooter(&HEABData, sizeof (HEABData), debug); - - memcpy(heabDataBuffer, &HEABData, sizeof (HEABData)); - - return sizeof (HEABType); - -} - - -/*! - * \brief encodeRCMMMessage Constructs an ISO RCMM message based on an input ::RemoteControlManoeuvreType - * \param command The command to be sent to an object - * \param rcmmDataBuffer Buffer to which RCMM message is to be written - * \param bufferLength Size of buffer to which RCMM message is to be written - * \param debug Flag for enabling debugging - * \return Number of bytes written or -1 in case of an error - */ -ssize_t encodeRCMMMessage(const RemoteControlManoeuvreType command, char *rcmmDataBuffer, - const size_t bufferLength, const char debug) { - - RCMMType RCMMData; - - memset(rcmmDataBuffer, 0, bufferLength); - - // If buffer too small to hold RCMM data, generate an error - if (bufferLength < sizeof (RCMMType)) { - LogMessage(LOG_LEVEL_ERROR, "Buffer too small to hold necessary RCMM data"); - return -1; - } - - // Construct header - RCMMData.header = buildISOHeader(MESSAGE_ID_RCMM, sizeof (RCMMData), debug); - - // Fill contents - RCMMData.commandValueID = VALUE_ID_RCMM_CONTROL; - RCMMData.commandContentLength = sizeof (RCMMData.command); - RCMMData.command = (uint8_t) command; - - if (debug) { - LogPrint("RCMM message:\n\tCommand value ID: 0x%x\n\t" - "Command content length: %u\n\t" - "Command: %u", RCMMData.commandValueID, RCMMData.commandContentLength, RCMMData.command); - } - - // Switch from host endianness to little endian - RCMMData.commandValueID = htole16(RCMMData.commandValueID); - RCMMData.commandContentLength = htole16(RCMMData.commandContentLength); - - RCMMData.footer = buildISOFooter(&RCMMData, sizeof (RCMMData), debug); - - memcpy(rcmmDataBuffer, &RCMMData, sizeof (RCMMData)); - - return sizeof (RCMMType); - -} - - -/*! - * \brief buildMONRMessage Fills a MONRType struct from a buffer of raw data - * \param MonrData Raw data to be decoded - * \param MONRData Struct to be filled - * \param debug Flag for enabling of debugging - * \return value according to ::ISOMessageReturnValue - */ -ISOMessageReturnValue decodeMONRMessage(const char *monrDataBuffer, const size_t bufferLength, - uint32_t * objectID, ObjectMonitorType * monitorData, - const char debug) { - - MONRType MONRData; - const char *p = monrDataBuffer; - const uint16_t ExpectedMONRStructSize = (uint16_t) (sizeof (MONRData) - sizeof (MONRData.header) - - sizeof (MONRData.footer.Crc) - - sizeof (MONRData.monrStructValueID) - - sizeof (MONRData.monrStructContentLength)); - ISOMessageReturnValue retval = MESSAGE_OK; - - memset(monitorData, 0, sizeof (*monitorData)); - *objectID = 0; - - // Decode ISO header - if ((retval = decodeISOHeader(p, bufferLength, &MONRData.header, debug)) != MESSAGE_OK) { - memset(monitorData, 0, sizeof (*monitorData)); - return retval; - } - p += sizeof (MONRData.header); - *objectID = MONRData.header.TransmitterIdU8; - - // If message is not a MONR message, generate an error - if (MONRData.header.MessageIdU16 != MESSAGE_ID_MONR) { - LogMessage(LOG_LEVEL_ERROR, "Attempted to pass non-MONR message into MONR parsing function"); - return MESSAGE_TYPE_ERROR; - } - - // Decode content header - memcpy(&MONRData.monrStructValueID, p, sizeof (MONRData.monrStructValueID)); - p += sizeof (MONRData.monrStructValueID); - MONRData.monrStructValueID = le16toh(MONRData.monrStructValueID); - - // If content is not a MONR struct or an unexpected size, generate an error - if (MONRData.monrStructValueID != VALUE_ID_MONR_STRUCT) { - LogMessage(LOG_LEVEL_ERROR, "Attempted to pass non-MONR struct into MONR parsing function"); - return MESSAGE_VALUE_ID_ERROR; - } - - memcpy(&MONRData.monrStructContentLength, p, sizeof (MONRData.monrStructContentLength)); - p += sizeof (MONRData.monrStructContentLength); - MONRData.monrStructContentLength = le16toh(MONRData.monrStructContentLength); - - if (MONRData.monrStructContentLength != ExpectedMONRStructSize) { - LogMessage(LOG_LEVEL_ERROR, "MONR content length %u differs from the expected length %u", - MONRData.monrStructContentLength, ExpectedMONRStructSize); - return MESSAGE_LENGTH_ERROR; - } - - // Decode content - memcpy(&MONRData.gpsQmsOfWeek, p, sizeof (MONRData.gpsQmsOfWeek)); - p += sizeof (MONRData.gpsQmsOfWeek); - MONRData.gpsQmsOfWeek = le32toh(MONRData.gpsQmsOfWeek); - - memcpy(&MONRData.xPosition, p, sizeof (MONRData.xPosition)); - p += sizeof (MONRData.xPosition); - MONRData.xPosition = (int32_t) le32toh(MONRData.xPosition); - - memcpy(&MONRData.yPosition, p, sizeof (MONRData.yPosition)); - p += sizeof (MONRData.yPosition); - MONRData.yPosition = (int32_t) le32toh(MONRData.yPosition); - - memcpy(&MONRData.zPosition, p, sizeof (MONRData.zPosition)); - p += sizeof (MONRData.zPosition); - MONRData.zPosition = (int32_t) le32toh(MONRData.zPosition); - - memcpy(&MONRData.heading, p, sizeof (MONRData.heading)); - p += sizeof (MONRData.heading); - MONRData.heading = le16toh(MONRData.heading); - - memcpy(&MONRData.longitudinalSpeed, p, sizeof (MONRData.longitudinalSpeed)); - p += sizeof (MONRData.longitudinalSpeed); - MONRData.longitudinalSpeed = (int16_t) le16toh(MONRData.longitudinalSpeed); - - memcpy(&MONRData.lateralSpeed, p, sizeof (MONRData.lateralSpeed)); - p += sizeof (MONRData.lateralSpeed); - MONRData.lateralSpeed = (int16_t) le16toh(MONRData.lateralSpeed); - - memcpy(&MONRData.longitudinalAcc, p, sizeof (MONRData.longitudinalAcc)); - p += sizeof (MONRData.longitudinalAcc); - MONRData.longitudinalAcc = (int16_t) le16toh(MONRData.longitudinalAcc); - - memcpy(&MONRData.lateralAcc, p, sizeof (MONRData.lateralAcc)); - p += sizeof (MONRData.lateralAcc); - MONRData.lateralAcc = (int16_t) le16toh(MONRData.lateralAcc); - - memcpy(&MONRData.driveDirection, p, sizeof (MONRData.driveDirection)); - p += sizeof (MONRData.driveDirection); - - memcpy(&MONRData.state, p, sizeof (MONRData.state)); - p += sizeof (MONRData.state); - - memcpy(&MONRData.readyToArm, p, sizeof (MONRData.readyToArm)); - p += sizeof (MONRData.readyToArm); - - memcpy(&MONRData.errorStatus, p, sizeof (MONRData.errorStatus)); - p += sizeof (MONRData.errorStatus); - - // Decode footer - if ((retval = - decodeISOFooter(p, bufferLength - (size_t) (p - monrDataBuffer), &MONRData.footer, - debug)) != MESSAGE_OK) { - LogMessage(LOG_LEVEL_ERROR, "Error decoding MONR footer"); - return retval; - } - - if ((retval = verifyChecksum(&MONRData, sizeof (MONRData) - sizeof (MONRData.footer), - MONRData.footer.Crc, debug)) == MESSAGE_CRC_ERROR) { - LogMessage(LOG_LEVEL_WARNING, "MONR checksum error"); - return retval; - } - - if (debug) { - LogPrint("MONR:"); - LogPrint("SyncWord = %x", MONRData.header.SyncWordU16); - LogPrint("TransmitterId = %d", MONRData.header.TransmitterIdU8); - LogPrint("PackageCounter = %d", MONRData.header.MessageCounterU8); - LogPrint("AckReq = %d", MONRData.header.AckReqProtVerU8); - LogPrint("MessageId = %d", MONRData.header.MessageIdU16); - LogPrint("MessageLength = %d", MONRData.header.MessageLengthU32); - LogPrint("ValueId = %d", MONRData.monrStructValueID); - LogPrint("ContentLength = %d", MONRData.monrStructContentLength); - LogPrint("GPSSOW = %d", MONRData.gpsQmsOfWeek); - LogPrint("XPosition = %d", MONRData.xPosition); - LogPrint("YPosition = %d", MONRData.yPosition); - LogPrint("ZPosition = %d", MONRData.zPosition); - LogPrint("Heading = %d", MONRData.heading); - LogPrint("LongitudinalSpeed = %d", MONRData.longitudinalSpeed); - LogPrint("LateralSpeed = %d", MONRData.lateralSpeed); - LogPrint("LongitudinalAcc = %d", MONRData.longitudinalAcc); - LogPrint("LateralAcc = %d", MONRData.lateralAcc); - LogPrint("DriveDirection = %d", MONRData.driveDirection); - LogPrint("State = %d", MONRData.state); - LogPrint("ReadyToArm = %d", MONRData.readyToArm); - LogPrint("ErrorStatus = %d", MONRData.errorStatus); - } - - // Fill output struct with parsed data - convertMONRToHostRepresentation(&MONRData, monitorData); - - return retval; -} - - -/*! - * \brief convertMONRToHostRepresentation Converts a MONR message to the internal representation for - * object monitoring data - * \param MONRData MONR message to be converted - * \param monitorData Monitor data in which result is to be placed - */ -void convertMONRToHostRepresentation(const MONRType * MONRData, ObjectMonitorType * monitorData) { - - // Timestamp - monitorData->isTimestampValid = MONRData->gpsQmsOfWeek != GPS_SECOND_OF_WEEK_UNAVAILABLE_VALUE; - if (monitorData->isTimestampValid) { - struct timeval currentTime; - - TimeSetToCurrentSystemTime(¤tTime); - TimeSetToGPStime(&monitorData->timestamp, TimeGetAsGPSweek(¤tTime), MONRData->gpsQmsOfWeek); - } - - // Position / heading - monitorData->position.xCoord_m = (double)(MONRData->xPosition) / POSITION_ONE_METER_VALUE; - monitorData->position.yCoord_m = (double)(MONRData->yPosition) / POSITION_ONE_METER_VALUE; - monitorData->position.zCoord_m = (double)(MONRData->zPosition) / POSITION_ONE_METER_VALUE; - monitorData->position.isPositionValid = true; - monitorData->position.isHeadingValid = MONRData->heading != HEADING_UNAVAILABLE_VALUE; - if (monitorData->position.isHeadingValid) { - monitorData->position.heading_rad = - mapISOHeadingToHostHeading(MONRData->heading / 100.0 * M_PI / 180.0); - } - - // Velocity - monitorData->speed.isLongitudinalValid = MONRData->longitudinalSpeed != SPEED_UNAVAILABLE_VALUE; - monitorData->speed.longitudinal_m_s = monitorData->speed.isLongitudinalValid ? - (double)(MONRData->longitudinalSpeed) / SPEED_ONE_METER_PER_SECOND_VALUE : 0; - monitorData->speed.isLateralValid = MONRData->lateralSpeed != SPEED_UNAVAILABLE_VALUE; - monitorData->speed.lateral_m_s = monitorData->speed.isLateralValid ? - (double)(MONRData->lateralSpeed) / SPEED_ONE_METER_PER_SECOND_VALUE : 0; - - // Acceleration - monitorData->acceleration.isLongitudinalValid = - MONRData->longitudinalAcc != ACCELERATION_UNAVAILABLE_VALUE; - monitorData->acceleration.longitudinal_m_s2 = - monitorData->acceleration.isLongitudinalValid ? (double)(MONRData->longitudinalAcc) / - ACCELERATION_ONE_METER_PER_SECOND_SQUARED_VALUE : 0; - monitorData->acceleration.isLateralValid = MONRData->lateralAcc != ACCELERATION_UNAVAILABLE_VALUE; - monitorData->acceleration.lateral_m_s2 = monitorData->acceleration.isLateralValid ? - (double)(MONRData->lateralAcc) / ACCELERATION_ONE_METER_PER_SECOND_SQUARED_VALUE : 0; - - // Drive direction - switch (MONRData->driveDirection) { - case ISO_DRIVE_DIRECTION_FORWARD: - monitorData->drivingDirection = OBJECT_DRIVE_DIRECTION_FORWARD; - break; - case ISO_DRIVE_DIRECTION_BACKWARD: - monitorData->drivingDirection = OBJECT_DRIVE_DIRECTION_BACKWARD; - break; - case ISO_DRIVE_DIRECTION_UNAVAILABLE: - default: - monitorData->drivingDirection = OBJECT_DRIVE_DIRECTION_UNAVAILABLE; - } - - // State - switch (MONRData->state) { - case ISO_OBJECT_STATE_DISARMED: - monitorData->state = OBJECT_STATE_DISARMED; - break; - case ISO_OBJECT_STATE_ARMED: - monitorData->state = OBJECT_STATE_ARMED; - break; - case ISO_OBJECT_STATE_RUNNING: - monitorData->state = OBJECT_STATE_RUNNING; - break; - case ISO_OBJECT_STATE_POSTRUN: - monitorData->state = OBJECT_STATE_POSTRUN; - break; - case ISO_OBJECT_STATE_ABORTING: - monitorData->state = OBJECT_STATE_ABORTING; - break; - case ISO_OBJECT_STATE_REMOTE_CONTROLLED: - monitorData->state = OBJECT_STATE_REMOTE_CONTROL; - break; - case ISO_OBJECT_STATE_OFF: - case ISO_OBJECT_STATE_INIT: - default: - monitorData->state = OBJECT_STATE_UNKNOWN; - break; - } - - // Ready to arm - switch (MONRData->readyToArm) { - case ISO_READY_TO_ARM: - monitorData->armReadiness = OBJECT_READY_TO_ARM; - break; - case ISO_NOT_READY_TO_ARM: - monitorData->armReadiness = OBJECT_NOT_READY_TO_ARM; - break; - case ISO_READY_TO_ARM_UNAVAILABLE: - default: - monitorData->armReadiness = OBJECT_READY_TO_ARM_UNAVAILABLE; - } - - // Error status - monitorData->error.engineFault = MONRData->errorStatus & BITMASK_ERROR_ENGINE_FAULT; - monitorData->error.abortRequest = MONRData->errorStatus & BITMASK_ERROR_ABORT_REQUEST; - monitorData->error.batteryFault = MONRData->errorStatus & BITMASK_ERROR_BATTERY_FAULT; - monitorData->error.unknownError = MONRData->errorStatus & BITMASK_ERROR_OTHER - || MONRData->errorStatus & BITMASK_ERROR_VENDOR_SPECIFIC; - monitorData->error.syncPointEnded = MONRData->errorStatus & BITMASK_ERROR_SYNC_POINT_ENDED; - monitorData->error.outsideGeofence = MONRData->errorStatus & BITMASK_ERROR_OUTSIDE_GEOFENCE; - monitorData->error.badPositioningAccuracy = - MONRData->errorStatus & BITMASK_ERROR_BAD_POSITIONING_ACCURACY; - - return; -} - - -/*! - * \brief encodeSYPMMessage Fills an ISO SYPM struct with relevant data fields, and corresponding value IDs and content lengths - * \param synchronizationTime Time along trajectory at which objects are to be synchronized - * \param freezeTime Time along trajectory after which no further adaptation to the master is allowed - * \param mtspDataBuffer Buffer to which SYPM message is to be written - * \param bufferLength Size of buffer to which SYPM message is to be written - * \param debug Flag for enabling debugging - * \return Number of bytes written to buffer, or -1 in case of an error - */ -ssize_t encodeSYPMMessage(const struct timeval synchronizationTime, const struct timeval freezeTime, - char *sypmDataBuffer, const size_t bufferLength, const char debug) { - - SYPMType SYPMData; - - // If buffer too small to hold SYPM data, generate an error - if (bufferLength < sizeof (SYPMType)) { - LogMessage(LOG_LEVEL_ERROR, "Buffer too small to hold necessary SYPM data"); - return -1; - } - - // Construct header - SYPMData.header = buildISOHeader(MESSAGE_ID_SYPM, sizeof (SYPMData), debug); - - // Fill contents - SYPMData.syncPointTimeValueID = VALUE_ID_SYPM_SYNC_POINT_TIME; - SYPMData.syncPointTimeContentLength = sizeof (SYPMData.syncPointTime); - SYPMData.syncPointTime = (uint32_t) TimeGetAsUTCms(&synchronizationTime); - - SYPMData.freezeTimeValueID = VALUE_ID_SYPM_FREEZE_TIME; - SYPMData.freezeTimeContentLength = sizeof (SYPMData.freezeTime); - SYPMData.freezeTime = (uint32_t) TimeGetAsUTCms(&freezeTime); - - if (debug) { - LogPrint("SYPM message:\n\tSynchronization point time value ID: 0x%x\n\t" - "Synchronization point time content length: %u\n\t" - "Synchronization point time: %u [ms]\n\t" - "Freeze time value ID: 0x%x\n\t" - "Freeze time content length: %u\n\t" - "Freeze time: %u [ms]", SYPMData.syncPointTimeValueID, - SYPMData.syncPointTimeContentLength, SYPMData.syncPointTime, - SYPMData.freezeTimeValueID, SYPMData.freezeTimeContentLength, SYPMData.freezeTime); - } - - // Switch from host endianness to little endian - SYPMData.syncPointTimeValueID = htole16(SYPMData.syncPointTimeValueID); - SYPMData.syncPointTimeContentLength = htole16(SYPMData.syncPointTimeContentLength); - SYPMData.syncPointTime = htole16(SYPMData.syncPointTime); - SYPMData.freezeTimeValueID = htole16(SYPMData.freezeTimeValueID); - SYPMData.freezeTimeContentLength = htole16(SYPMData.freezeTimeContentLength); - SYPMData.freezeTime = htobe16(SYPMData.freezeTime); - - // Construct footer - SYPMData.footer = buildISOFooter(&SYPMData, sizeof (SYPMData), debug); - - memcpy(sypmDataBuffer, &SYPMData, sizeof (SYPMData)); - - return sizeof (SYPMType); -} - -/*! - * \brief encodeMTSPMessage Fills an ISO MTSP struct with relevant data fields, and corresponding value IDs and content lengths - * \param estSyncPointTime Estimated time when the master object will reach the synchronization point - * \param mtspDataBuffer Buffer to which MTSP message is to be written - * \param bufferLength Size of buffer to which MTSP message is to be written - * \param debug Flag for enabling debugging - * \return Number of bytes written to buffer, or -1 in case of an error - */ -ssize_t encodeMTSPMessage(const struct timeval *estSyncPointTime, char *mtspDataBuffer, - const size_t bufferLength, const char debug) { - - MTSPType MTSPData; - - memset(mtspDataBuffer, 0, bufferLength); - - // If buffer too small to hold MTSP data, generate an error - if (bufferLength < sizeof (MTSPType)) { - LogMessage(LOG_LEVEL_ERROR, "Buffer too small to hold necessary MTSP data"); - return -1; - } - - // Construct header - MTSPData.header = buildISOHeader(MESSAGE_ID_MTSP, sizeof (MTSPData), debug); - - // Fill contents - MTSPData.estSyncPointTimeValueID = VALUE_ID_MTSP_EST_SYNC_POINT_TIME; - MTSPData.estSyncPointTimeContentLength = sizeof (MTSPData.estSyncPointTime); - MTSPData.estSyncPointTime = - estSyncPointTime == - NULL ? GPS_SECOND_OF_WEEK_UNAVAILABLE_VALUE : TimeGetAsGPSqmsOfWeek(estSyncPointTime); - - if (debug) { - LogPrint("MTSP message:\n\t" - "Estimated sync point time value ID: 0x%x\n\t" - "Estimated sync point time content length: %u\n\t" - "Estimated sync point time: %u [¼ ms]", - MTSPData.estSyncPointTimeValueID, MTSPData.estSyncPointTimeContentLength, - MTSPData.estSyncPointTime); - } - - // Switch from host endianness to little endian - MTSPData.estSyncPointTimeValueID = htole16(MTSPData.estSyncPointTimeValueID); - MTSPData.estSyncPointTimeContentLength = htole16(MTSPData.estSyncPointTimeContentLength); - MTSPData.estSyncPointTime = htole32(MTSPData.estSyncPointTime); - - // Construct footer - MTSPData.footer = buildISOFooter(&MTSPData, sizeof (MTSPData), debug); - - memcpy(mtspDataBuffer, &MTSPData, sizeof (MTSPData)); - - return sizeof (MTSPType); -} - - -/*! - * \brief encodeTRCMMessage Fills an ISO TRCM struct with relevant data fields, and corresponding value IDs and content lengths - * \param triggerID ID of the trigger to be configured - * \param triggerType Type of the trigger to be configured according to ::TriggerType_t - * \param param1 First parameter of the trigger to be configured according to ::TriggerTypeParameter_t - * \param param2 Second parameter of the trigger to be configured ::TriggerTypeParameter_t - * \param param3 Third parameter of the trigger to be configured ::TriggerTypeParameter_t - * \param trcmDataBuffer Buffer to which TRCM message is to be written - * \param bufferLength Size of buffer to which TRCM message is to be written - * \param debug Flag for enabling debugging - * \return Number of bytes written or -1 in case of an error - */ -ssize_t encodeTRCMMessage(const uint16_t * triggerID, const TriggerType_t * triggerType, - const TriggerTypeParameter_t * param1, const TriggerTypeParameter_t * param2, - const TriggerTypeParameter_t * param3, char *trcmDataBuffer, - const size_t bufferLength, const char debug) { - - TRCMType TRCMData; - - memset(trcmDataBuffer, 0, bufferLength); - - // If buffer too small to hold TRCM data, generate an error - if (bufferLength < sizeof (TRCMType)) { - LogMessage(LOG_LEVEL_ERROR, "Buffer too small to hold necessary TRCM data"); - return -1; - } - - // Construct header - TRCMData.header = buildISOHeader(MESSAGE_ID_TRCM, sizeof (TRCMData), debug); - - // Fill contents - TRCMData.triggerIDValueID = VALUE_ID_TRCM_TRIGGER_ID; - TRCMData.triggerIDContentLength = sizeof (TRCMData.triggerID); - TRCMData.triggerID = triggerID == NULL ? TRIGGER_ID_UNAVAILABLE : *triggerID; - - TRCMData.triggerTypeValueID = VALUE_ID_TRCM_TRIGGER_TYPE; - TRCMData.triggerTypeContentLength = sizeof (TRCMData.triggerType); - TRCMData.triggerType = triggerType == NULL ? TRIGGER_TYPE_UNAVAILABLE : (uint16_t) (*triggerType); - - TRCMData.triggerTypeParameter1ValueID = VALUE_ID_TRCM_TRIGGER_TYPE_PARAM1; - TRCMData.triggerTypeParameter2ValueID = VALUE_ID_TRCM_TRIGGER_TYPE_PARAM2; - TRCMData.triggerTypeParameter3ValueID = VALUE_ID_TRCM_TRIGGER_TYPE_PARAM3; - - TRCMData.triggerTypeParameter1ContentLength = sizeof (TRCMData.triggerTypeParameter1); - TRCMData.triggerTypeParameter2ContentLength = sizeof (TRCMData.triggerTypeParameter2); - TRCMData.triggerTypeParameter3ContentLength = sizeof (TRCMData.triggerTypeParameter3); - - TRCMData.triggerTypeParameter1 = - param1 == NULL ? TRIGGER_TYPE_PARAMETER_UNAVAILABLE : (uint32_t) (*param1); - TRCMData.triggerTypeParameter2 = - param2 == NULL ? TRIGGER_TYPE_PARAMETER_UNAVAILABLE : (uint32_t) (*param2); - TRCMData.triggerTypeParameter3 = - param3 == NULL ? TRIGGER_TYPE_PARAMETER_UNAVAILABLE : (uint32_t) (*param3); - - if (debug) { - LogPrint("TRCM message:\n\tTrigger ID value ID: 0x%x\n\tTrigger ID content length: %u\n\t" - "Trigger ID: %u\n\tTrigger type value ID: 0x%x\n\tTrigger type content length: %u\n\t" - "Trigger type: %u\n\tTrigger type parameter 1 value ID: 0x%x\n\tTrigger type parameter 1 content length: %u\n\t" - "Trigger type parameter 1: %u\n\tTrigger type parameter 2 value ID: 0x%x\n\tTrigger type parameter 2 content length: %u\n\t" - "Trigger type parameter 2: %u\n\tTrigger type parameter 3 value ID: 0x%x\n\tTrigger type parameter 3 content length: %u" - "Trigger type parameter 3: %u\n\t", TRCMData.triggerIDValueID, - TRCMData.triggerIDContentLength, TRCMData.triggerID, TRCMData.triggerTypeValueID, - TRCMData.triggerTypeContentLength, TRCMData.triggerType, - TRCMData.triggerTypeParameter1ValueID, TRCMData.triggerTypeParameter1ContentLength, - TRCMData.triggerTypeParameter1, TRCMData.triggerTypeParameter2ValueID, - TRCMData.triggerTypeParameter2ContentLength, TRCMData.triggerTypeParameter2, - TRCMData.triggerTypeParameter3ValueID, TRCMData.triggerTypeParameter3ContentLength, - TRCMData.triggerTypeParameter3); - } - - // Switch from host endianness to little endian - TRCMData.triggerIDValueID = htole16(TRCMData.triggerIDValueID); - TRCMData.triggerIDContentLength = htole16(TRCMData.triggerIDContentLength); - TRCMData.triggerID = htole16(TRCMData.triggerID); - - TRCMData.triggerTypeValueID = htole16(TRCMData.triggerTypeValueID); - TRCMData.triggerTypeContentLength = htole16(TRCMData.triggerTypeContentLength); - TRCMData.triggerType = htole16(TRCMData.triggerType); - - TRCMData.triggerTypeParameter1ValueID = htole16(TRCMData.triggerTypeParameter1ValueID); - TRCMData.triggerTypeParameter2ValueID = htole16(TRCMData.triggerTypeParameter2ValueID); - TRCMData.triggerTypeParameter3ValueID = htole16(TRCMData.triggerTypeParameter3ValueID); - - TRCMData.triggerTypeParameter1ContentLength = htole16(TRCMData.triggerTypeParameter1ContentLength); - TRCMData.triggerTypeParameter2ContentLength = htole16(TRCMData.triggerTypeParameter2ContentLength); - TRCMData.triggerTypeParameter3ContentLength = htole16(TRCMData.triggerTypeParameter3ContentLength); - - TRCMData.triggerTypeParameter1 = htole32(TRCMData.triggerTypeParameter1); - TRCMData.triggerTypeParameter2 = htole32(TRCMData.triggerTypeParameter2); - TRCMData.triggerTypeParameter3 = htole32(TRCMData.triggerTypeParameter3); - - // Construct footer - TRCMData.footer = buildISOFooter(&TRCMData, sizeof (TRCMData), debug); - - memcpy(trcmDataBuffer, &TRCMData, sizeof (TRCMData)); - - return sizeof (TRCMData); -} - - - - - -/*! - * \brief encodeACCMMessage Fills an ISO ACCM struct with relevant data fields, and corresponding value IDs and content lengths - * \param actionID ID of the action to be configured - * \param actionType Type of the action to be configured according to ::ActionType_t - * \param param1 First parameter of the action to be configured according to ::ActionTypeParameter_t - * \param param2 Second parameter of the action to be configured ::ActionTypeParameter_t - * \param param3 Third parameter of the action to be configured ::ActionTypeParameter_t - * \param trcmDataBuffer Buffer to which ACCM message is to be written - * \param bufferLength Size of buffer to which ACCM message is to be written - * \param debug Flag for enabling debugging - * \return Number of bytes written or -1 in case of an error - */ -ssize_t encodeACCMMessage(const uint16_t * actionID, const ActionType_t * actionType, - const ActionTypeParameter_t * param1, const ActionTypeParameter_t * param2, - const ActionTypeParameter_t * param3, char *accmDataBuffer, - const size_t bufferLength, const char debug) { - - ACCMType ACCMData; - - memset(accmDataBuffer, 0, bufferLength); - - // If buffer too small to hold ACCM data, generate an error - if (bufferLength < sizeof (ACCMType)) { - LogMessage(LOG_LEVEL_ERROR, "Buffer too small to hold necessary ACCM data"); - return -1; - } - - // Construct header - ACCMData.header = buildISOHeader(MESSAGE_ID_ACCM, sizeof (ACCMData), debug); - - // Fill contents - ACCMData.actionIDValueID = VALUE_ID_ACCM_ACTION_ID; - ACCMData.actionIDContentLength = sizeof (ACCMData.actionID); - ACCMData.actionID = actionID == NULL ? ACTION_ID_UNAVAILABLE : *actionID; - - ACCMData.actionTypeValueID = VALUE_ID_ACCM_ACTION_TYPE; - ACCMData.actionTypeContentLength = sizeof (ACCMData.actionType); - ACCMData.actionType = actionType == NULL ? ACTION_TYPE_UNAVAILABLE : (uint16_t) (*actionType); - - ACCMData.actionTypeParameter1ValueID = VALUE_ID_ACCM_ACTION_TYPE_PARAM1; - ACCMData.actionTypeParameter2ValueID = VALUE_ID_ACCM_ACTION_TYPE_PARAM2; - ACCMData.actionTypeParameter3ValueID = VALUE_ID_ACCM_ACTION_TYPE_PARAM3; - - ACCMData.actionTypeParameter1ContentLength = sizeof (ACCMData.actionTypeParameter1); - ACCMData.actionTypeParameter2ContentLength = sizeof (ACCMData.actionTypeParameter2); - ACCMData.actionTypeParameter3ContentLength = sizeof (ACCMData.actionTypeParameter3); - - ACCMData.actionTypeParameter1 = param1 == NULL ? ACTION_TYPE_PARAMETER_UNAVAILABLE : (uint32_t) (*param1); - ACCMData.actionTypeParameter2 = param2 == NULL ? ACTION_TYPE_PARAMETER_UNAVAILABLE : (uint32_t) (*param2); - ACCMData.actionTypeParameter3 = param3 == NULL ? ACTION_TYPE_PARAMETER_UNAVAILABLE : (uint32_t) (*param3); - - if (debug) { - LogPrint("ACCM message:\n\tAction ID value ID: 0x%x\n\tAction ID content length: %u\n\t" - "Action ID: %u\n\tAction type value ID: 0x%x\n\tAction type content length: %u\n\t" - "Action type: %u\n\tAction type parameter 1 value ID: 0x%x\n\tAction type parameter 1 content length: %u\n\t" - "Action type parameter 1: %u\n\tAction type parameter 2 value ID: 0x%x\n\tAction type parameter 2 content length: %u\n\t" - "Action type parameter 2: %u\n\tAction type parameter 3 value ID: 0x%x\n\tAction type parameter 3 content length: %u" - "Action type parameter 3: %u\n\t", ACCMData.actionIDValueID, ACCMData.actionIDContentLength, - ACCMData.actionID, ACCMData.actionTypeValueID, ACCMData.actionTypeContentLength, - ACCMData.actionType, ACCMData.actionTypeParameter1ValueID, - ACCMData.actionTypeParameter1ContentLength, ACCMData.actionTypeParameter1, - ACCMData.actionTypeParameter2ValueID, ACCMData.actionTypeParameter2ContentLength, - ACCMData.actionTypeParameter2, ACCMData.actionTypeParameter3ValueID, - ACCMData.actionTypeParameter3ContentLength, ACCMData.actionTypeParameter3); - } - - // Switch from host endianness to little endian - ACCMData.actionIDValueID = htole16(ACCMData.actionIDValueID); - ACCMData.actionIDContentLength = htole16(ACCMData.actionIDContentLength); - ACCMData.actionID = htole16(ACCMData.actionID); - - ACCMData.actionTypeValueID = htole16(ACCMData.actionTypeValueID); - ACCMData.actionTypeContentLength = htole16(ACCMData.actionTypeContentLength); - ACCMData.actionType = htole16(ACCMData.actionType); - - ACCMData.actionTypeParameter1ValueID = htole16(ACCMData.actionTypeParameter1ValueID); - ACCMData.actionTypeParameter2ValueID = htole16(ACCMData.actionTypeParameter2ValueID); - ACCMData.actionTypeParameter3ValueID = htole16(ACCMData.actionTypeParameter3ValueID); - - ACCMData.actionTypeParameter1ContentLength = htole16(ACCMData.actionTypeParameter1ContentLength); - ACCMData.actionTypeParameter2ContentLength = htole16(ACCMData.actionTypeParameter2ContentLength); - ACCMData.actionTypeParameter3ContentLength = htole16(ACCMData.actionTypeParameter3ContentLength); - - ACCMData.actionTypeParameter1 = htole32(ACCMData.actionTypeParameter1); - ACCMData.actionTypeParameter2 = htole32(ACCMData.actionTypeParameter2); - ACCMData.actionTypeParameter3 = htole32(ACCMData.actionTypeParameter3); - - // Construct footer - ACCMData.footer = buildISOFooter(&ACCMData, sizeof (ACCMData), debug); - - memcpy(accmDataBuffer, &ACCMData, sizeof (ACCMData)); - - return sizeof (ACCMData); -} - - - -/*! - * \brief encodeEXACMessage Fills an ISO EXAC struct with relevant data fields, and corresponding value IDs and content lengths - * \param actionID ID of the action to be executed - * \param executionTime Time when the action is to be executed - * \param exacDataBuffer Buffer to which EXAC message is to be written - * \param bufferLength Size of buffer to which EXAC message is to be written - * \param debug Flag for enabling debugging - * \return Number of bytes written or -1 in case of an error - */ -ssize_t encodeEXACMessage(const uint16_t * actionID, const struct timeval *executionTime, - char *exacDataBuffer, const size_t bufferLength, const char debug) { - - EXACType EXACData; - - memset(exacDataBuffer, 0, bufferLength); - - // If buffer too small to hold EXAC data, generate an error - if (bufferLength < sizeof (EXACType)) { - LogMessage(LOG_LEVEL_ERROR, "Buffer too small to hold necessary EXAC data"); - return -1; - } - - // Construct header - EXACData.header = buildISOHeader(MESSAGE_ID_EXAC, sizeof (EXACData), debug); - - // Fill contents - EXACData.actionIDValueID = VALUE_ID_EXAC_ACTION_ID; - EXACData.actionIDContentLength = sizeof (EXACData.actionID); - EXACData.actionID = actionID == NULL ? ACTION_ID_UNAVAILABLE : *actionID; - - EXACData.executionTime_qmsoWValueID = VALUE_ID_EXAC_ACTION_EXECUTE_TIME; - EXACData.executionTime_qmsoWContentLength = sizeof (EXACData.executionTime_qmsoW); - EXACData.executionTime_qmsoW = - executionTime == NULL ? GPS_SECOND_OF_WEEK_UNAVAILABLE_VALUE : TimeGetAsGPSqmsOfWeek(executionTime); - - if (debug) { - LogPrint - ("EXAC message:\n\tAction ID value ID: 0x%x\n\tAction ID content length: %u\n\tAction ID: %u\n\t" - "Action execute time value ID: 0x%x\n\tAction execute time content length: %u\n\tAction execute time: %u [¼ ms]", - EXACData.actionIDValueID, EXACData.actionIDContentLength, EXACData.actionID, - EXACData.executionTime_qmsoWValueID, EXACData.executionTime_qmsoWContentLength, - EXACData.executionTime_qmsoW); - } - - // Switch from host endianness to little endian - EXACData.actionIDValueID = htole16(EXACData.actionIDValueID); - EXACData.actionIDContentLength = htole16(EXACData.actionIDContentLength); - EXACData.actionID = htole16(EXACData.actionID); - EXACData.executionTime_qmsoWValueID = htole16(EXACData.executionTime_qmsoWValueID); - EXACData.executionTime_qmsoWContentLength = htole16(EXACData.executionTime_qmsoWContentLength); - EXACData.executionTime_qmsoW = htole32(EXACData.executionTime_qmsoW); - - // Construct footer - EXACData.footer = buildISOFooter(&EXACData, sizeof (EXACData), debug); - - memcpy(exacDataBuffer, &EXACData, sizeof (EXACData)); - - return sizeof (EXACType); -} - -/*! - * \brief encodeINSUPMessage Fills an ISO vendor specific (RISE) INSUP struct with relevant data fields, - * and corresponding value IDs and content lengths - * \param command Command to send to supervisor - * \param insupDataBuffer Data buffer to which INSUP is to be written - * \param bufferLength Length of data buffer to which INSUP is to be written - * \param debug Flag for enabling debugging - * \return Number of bytes written to buffer, or -1 in case of error - */ -ssize_t encodeINSUPMessage(const SupervisorCommandType command, char *insupDataBuffer, - const size_t bufferLength, const char debug) { - INSUPType INSUPData; - - memset(insupDataBuffer, 0, bufferLength); - - // If buffer too small to hold EXAC data, generate an error - if (bufferLength < sizeof (INSUPType)) { - LogMessage(LOG_LEVEL_ERROR, "Buffer too small to hold necessary INSUP data"); - return -1; - } - - // Construct header - INSUPData.header = buildISOHeader(MESSAGE_ID_VENDOR_SPECIFIC_RISE_INSUP, sizeof (INSUPData), debug); - - // Fill contents - INSUPData.modeValueID = VALUE_ID_INSUP_MODE; - INSUPData.modeContentLength = sizeof (INSUPData.mode); - INSUPData.mode = (uint8_t) command; - - if (debug) { - LogPrint("INSUP message:\n\tMode value ID: 0x%x\n\t" - "Mode content length: %u\n\tMode: %u", INSUPData.modeValueID, - INSUPData.modeContentLength, INSUPData.mode); - } - - // Switch from host endianness to little endian - INSUPData.modeValueID = htole16(INSUPData.modeValueID); - INSUPData.modeContentLength = htole16(INSUPData.modeContentLength); - - // Construct footer - INSUPData.footer = buildISOFooter(&INSUPData, sizeof (INSUPData), debug); - - memcpy(insupDataBuffer, &INSUPData, sizeof (INSUPData)); - - return sizeof (INSUPData); -} - - -/*! - * \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 - * \return Heading, in radians, measured from the test x axis - */ -double_t mapISOHeadingToHostHeading(const double_t isoHeading_rad) { - // TODO: Reevaluate this when ISO specification is updated with new heading and rotated coordinate system - - double_t retval = isoHeading_rad; - - // Host heading is CCW while ISO is CW - retval = -retval; - // Host heading is measured from the x axis while ISO is measured from the y axis - retval = retval + M_PI / 2.0; - // Ensure angle lies between 0 and 2pi - while (retval < 0.0) { - retval += 2.0 * M_PI; - } - while (retval >= 2.0 * M_PI) { - retval -= 2.0 * M_PI; - } - return retval; -} - -/*! - * \brief mapHostHeadingToISOHeading Converts between internal heading measured from the test x axis to ISO NED heading - * \param isoHeading_rad Heading measured form test x axis, in radians - * \return Heading, in radians, measured as specified by ISO 22133 - */ -double_t mapHostHeadingToISOHeading(const double_t hostHeading_rad) { - // TODO: Reevaluate this when ISO specification is updated with new heading and rotated coordinate system - - double_t retval = hostHeading_rad; - - // Host heading is CCW while ISO is CW - retval = -retval; - // Host heading is measured from the x axis while ISO is measured from the y axis - retval = retval + M_PI / 2.0; - // Ensure angle lies between 0 and 2pi - while (retval < 0.0) { - retval += 2.0 * M_PI; - } - while (retval >= 2.0 * M_PI) { - retval -= 2.0 * M_PI; - } - return retval; -} diff --git a/core/src/positioning.c b/core/src/positioning.c deleted file mode 100644 index 9fb3f1e12..000000000 --- a/core/src/positioning.c +++ /dev/null @@ -1,353 +0,0 @@ -#include -#include -#include -#include - -#include "positioning.h" -#include "maestroTime.h" - -static const char unknownStateString[] = "UNKNOWN"; -static const char disarmedStateString[] = "DISARMED"; -static const char armedStateString[] = "ARMED"; -static const char runningStateString[] = "RUNNING"; -static const char postrunStateString[] = "POSTRUN"; -static const char abortingStateString[] = "ABORTING"; -static const char remoteControlStateString[] = "REMOTECTRL"; - -static const char noErrorString[] = "OK"; -static const char engineFaultString[] = "ENGINE_FAULT"; -static const char batteryFaultString[] = "BATTERY_FAULT"; -static const char abortRequestString[] = "ABORT_REQUEST"; -static const char otherErrorString[] = "OTHER_ERROR"; -static const char syncPointEndedString[] = "SYNC_PT_ENDED"; -static const char outsideGeofenceString[] = "OUTSIDE_GEOFENCE"; -static const char badPositioningAccuracyString[] = "BAD_POS_ACC"; - -static const char driveDirectionForwardString[] = "FORWARD"; -static const char driveDirectionReverseString[] = "REVERSE"; - -static const char readyToArmString[] = "READY"; -static const char notReadyToArmString[] = "NOT_READY"; - -/*! - * \brief objectStateToASCII Converts an enumerated object state into a printable string - * \param state Variable to be converted - * \return Pointer to a string of the state - */ -const char *objectStateToASCII(const ObjectStateType state) { - switch (state) { - case OBJECT_STATE_DISARMED: - return disarmedStateString; - case OBJECT_STATE_ARMED: - return armedStateString; - case OBJECT_STATE_RUNNING: - return runningStateString; - case OBJECT_STATE_POSTRUN: - return postrunStateString; - case OBJECT_STATE_ABORTING: - return abortingStateString; - case OBJECT_STATE_REMOTE_CONTROL: - return remoteControlStateString; - case OBJECT_STATE_UNKNOWN: - default: - return unknownStateString; - } -} - -/*! - * \brief ASCIIToObjectState Converts a string into an enumerated state - * \param asciiString String to convert - * \return Value according to ::ObjectStateType - */ -ObjectStateType ASCIIToObjectState(const char *asciiString) { - if (strstr(asciiString, disarmedStateString) != NULL) - return OBJECT_STATE_DISARMED; - if (strstr(asciiString, armedStateString) != NULL) - return OBJECT_STATE_ARMED; - if (strstr(asciiString, runningStateString) != NULL) - return OBJECT_STATE_RUNNING; - if (strstr(asciiString, postrunStateString) != NULL) - return OBJECT_STATE_POSTRUN; - if (strstr(asciiString, abortingStateString) != NULL) - return OBJECT_STATE_ABORTING; - if (strstr(asciiString, remoteControlStateString) != NULL) - return OBJECT_STATE_REMOTE_CONTROL; - return OBJECT_STATE_UNKNOWN; -} - - -/*! - * \brief hasError Returns whether or not an ObjectErrorType contains an error - * \param error Struct containing possible errors - * \return True if any of the errors are set - */ -bool hasError(const ObjectErrorType error) { - return error.engineFault || error.abortRequest || error.batteryFault || error.unknownError - || error.syncPointEnded || error.outsideGeofence || error.badPositioningAccuracy; -} - -/*! - * \brief errorStatusToASCII Converts an object error struct to ASCII text and prints it to a buffer - * \param error Struct to be converted - * \param asciiBuffer Buffer to which text is to be written - * \param bufferLength Size of buffer to which text is to be written - */ -void errorStatusToASCII(const ObjectErrorType error, char *asciiBuffer, const size_t bufferLength) { - - memset(asciiBuffer, 0, bufferLength); - - if (!hasError(error)) { - snprintf(asciiBuffer, bufferLength, noErrorString); - return; - } - if (error.engineFault) - snprintf(asciiBuffer, bufferLength, "%s,", engineFaultString); - if (error.batteryFault) - snprintf(asciiBuffer, bufferLength - strlen(asciiBuffer), "%s,", batteryFaultString); - if (error.unknownError) - snprintf(asciiBuffer, bufferLength - strlen(asciiBuffer), "%s,", otherErrorString); - if (error.syncPointEnded) - snprintf(asciiBuffer, bufferLength - strlen(asciiBuffer), "%s,", syncPointEndedString); - if (error.outsideGeofence) - snprintf(asciiBuffer, bufferLength - strlen(asciiBuffer), "%s,", outsideGeofenceString); - if (error.badPositioningAccuracy) - snprintf(asciiBuffer, bufferLength - strlen(asciiBuffer), "%s,", badPositioningAccuracyString); - if (error.abortRequest) - snprintf(asciiBuffer, bufferLength - strlen(asciiBuffer), "%s,", abortRequestString); - - if (asciiBuffer[strlen(asciiBuffer)] == ',') - asciiBuffer[strlen(asciiBuffer)] = '\0'; - - return; -} - -/*! - * \brief ASCIIToErrorStatus Converts a string with ASCII representation of object errors - * into a corresponding struct - * \param asciiString String to be parsed - * \return Value according to ::ObjectErrorType - */ -ObjectErrorType ASCIIToErrorStatus(const char *asciiString) { - ObjectErrorType error; - - memset(&error, 0, sizeof (error)); - - // First check if string starts with the "all ok" string - if (strstr(asciiString, noErrorString) == asciiString) - return error; - - // Check against all error string representations - if (strstr(asciiString, batteryFaultString) != NULL) - error.batteryFault = true; - if (strstr(asciiString, engineFaultString) != NULL) - error.engineFault = true; - if (strstr(asciiString, abortRequestString) != NULL) - error.abortRequest = true; - if (strstr(asciiString, otherErrorString) != NULL) - error.unknownError = true; - if (strstr(asciiString, syncPointEndedString) != NULL) - error.syncPointEnded = true; - if (strstr(asciiString, outsideGeofenceString) != NULL) - error.outsideGeofence = true; - if (strstr(asciiString, badPositioningAccuracyString) != NULL) - error.badPositioningAccuracy = true; - - // Error unmatched against all strings casted into unknown type - if (!hasError(error)) - error.unknownError = true; - - return error; -} - -/*! - * \brief objectMonitorDataToASCII Converts a monitor data struct into human readable ASCII text - * \param MONRData Struct containing monitor data - * \param asciiBuffer Buffer in which to print ASCII text representation - * \param bufferLength Length of ASCII buffer - * \return Number of bytes printed - */ -int objectMonitorDataToASCII(const ObjectMonitorType * monitorData, char *asciiBuffer, - const size_t bufferLength) { - - memset(asciiBuffer, 0, bufferLength); - - if (monitorData->isTimestampValid) - snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), - "%u;", TimeGetAsGPSqmsOfWeek(&monitorData->timestamp)); - else - snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), "NaN;"); - - if (monitorData->position.isPositionValid) - snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), - "%.3f;%.3f;%.3f;", monitorData->position.xCoord_m, monitorData->position.yCoord_m, - monitorData->position.zCoord_m); - else - snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), "NaN;NaN;NaN;"); - - if (monitorData->position.isHeadingValid) - snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), - "%.2f;", monitorData->position.heading_rad * 180.0 / M_PI); - else - snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), "NaN;"); - - if (monitorData->speed.isLongitudinalValid) - snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), - "%.2f;", monitorData->speed.longitudinal_m_s); - else - snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), "NaN;"); - - if (monitorData->speed.isLateralValid) - snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), - "%.2f;", monitorData->speed.lateral_m_s); - else - snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), "NaN;"); - - if (monitorData->acceleration.isLongitudinalValid) - snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), - "%.3f;", monitorData->acceleration.longitudinal_m_s2); - else - snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), "NaN;"); - - if (monitorData->acceleration.isLateralValid) - snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), - "%.3f;", monitorData->acceleration.lateral_m_s2); - else - snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), "NaN;"); - - if (monitorData->drivingDirection != OBJECT_DRIVE_DIRECTION_UNAVAILABLE) - snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), - "%s;", monitorData->drivingDirection == OBJECT_DRIVE_DIRECTION_FORWARD ? - driveDirectionForwardString : driveDirectionReverseString); - else - snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), "UNKNOWN;"); - - snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), - "%s;", objectStateToASCII(monitorData->state)); - - if (monitorData->armReadiness != OBJECT_READY_TO_ARM_UNAVAILABLE) - snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), - "%s;", monitorData->armReadiness == OBJECT_READY_TO_ARM ? - readyToArmString : notReadyToArmString); - else - snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), "UNKNOWN;"); - - errorStatusToASCII(monitorData->error, asciiBuffer + strlen(asciiBuffer), - bufferLength - strlen(asciiBuffer)); - - snprintf(asciiBuffer + strlen(asciiBuffer), bufferLength - strlen(asciiBuffer), ";"); - - return (int)strlen(asciiBuffer); -} - - - - -/*! - * \brief ASCIIToObjectMonitorData Converts an ASCII string into a monitor data struct - * \param asciiBuffer Buffer containing ASCII text representation - * \param monitorData Struct containing monitor data - * \return 0 on success, -1 otherwise - */ -int ASCIIToObjectMonitorData(const char *asciiBuffer, ObjectMonitorType * monitorData) { - - const char *token; - char *endPtr; - const char delim[] = ";"; - const int NumberBaseDecimal = 10; - char *copy = strdup(asciiBuffer); - struct timeval currentTime; - - TimeSetToCurrentSystemTime(¤tTime); - - memset(monitorData, 0, sizeof (*monitorData)); - - token = strtok(copy, delim); - - // Timestamp - token = strtok(NULL, delim); - TimeSetToGPStime(&monitorData->timestamp, TimeGetAsGPSweek(¤tTime), - (uint32_t) strtoul(token, &endPtr, NumberBaseDecimal)); - if (endPtr == token) { - monitorData->isTimestampValid = false; - memset(&monitorData->timestamp, 0, sizeof (monitorData->timestamp)); - } - - // Position - monitorData->position.isPositionValid = true; - - token = strtok(NULL, delim); - monitorData->position.xCoord_m = strtod(token, &endPtr); - if (endPtr == token) { - monitorData->position.isPositionValid = false; - } - - token = strtok(NULL, delim); - monitorData->position.yCoord_m = strtod(token, &endPtr); - if (endPtr == token) { - monitorData->position.isPositionValid = false; - } - - token = strtok(NULL, delim); - monitorData->position.zCoord_m = strtod(token, &endPtr); - if (endPtr == token) { - monitorData->position.isPositionValid = false; - } - - token = strtok(NULL, delim); - monitorData->position.heading_rad = strtod(token, &endPtr) * M_PI / 180.0; - if (endPtr == token) { - monitorData->position.isHeadingValid = false; - } - - // Velocity - token = strtok(NULL, delim); - monitorData->speed.longitudinal_m_s = strtod(token, &endPtr); - monitorData->speed.isLongitudinalValid = endPtr != token; - - token = strtok(NULL, delim); - monitorData->speed.lateral_m_s = strtod(token, &endPtr); - monitorData->speed.isLateralValid = endPtr != token; - - // Acceleration - token = strtok(NULL, delim); - monitorData->acceleration.longitudinal_m_s2 = strtod(token, &endPtr); - monitorData->acceleration.isLongitudinalValid = endPtr != token; - - token = strtok(NULL, delim); - monitorData->acceleration.lateral_m_s2 = strtod(token, &endPtr); - monitorData->acceleration.isLateralValid = endPtr != token; - - // Drive direction - token = strtok(NULL, delim); - if (strstr(token, driveDirectionForwardString) != NULL) { - monitorData->drivingDirection = OBJECT_DRIVE_DIRECTION_FORWARD; - } - else if (strstr(token, driveDirectionReverseString) != NULL) { - monitorData->drivingDirection = OBJECT_DRIVE_DIRECTION_BACKWARD; - } - else { - monitorData->drivingDirection = OBJECT_DRIVE_DIRECTION_UNAVAILABLE; - } - - // State - token = strtok(NULL, delim); - monitorData->state = ASCIIToObjectState(token); - - // Ready to arm - token = strtok(NULL, delim); - if (strstr(token, readyToArmString) != NULL) { - monitorData->armReadiness = OBJECT_READY_TO_ARM; - } - else if (strstr(token, notReadyToArmString) != NULL) { - monitorData->armReadiness = OBJECT_NOT_READY_TO_ARM; - } - else { - monitorData->armReadiness = OBJECT_READY_TO_ARM_UNAVAILABLE; - } - - // Error status - token = strtok(NULL, delim); - monitorData->error = ASCIIToErrorStatus(token); - - return 0; -} From e5b5963000eccd2c151fd2a42e0d99778720d902 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Wed, 3 Jun 2020 11:07:30 +0200 Subject: [PATCH 510/523] Removed #include instruction in data dictionary for iso22133.h --- core/src/datadictionary.c | 1 - 1 file changed, 1 deletion(-) diff --git a/core/src/datadictionary.c b/core/src/datadictionary.c index c866b6c59..680cdedb8 100644 --- a/core/src/datadictionary.c +++ b/core/src/datadictionary.c @@ -18,7 +18,6 @@ #include #include #include "datadictionary.h" -#include "iso22133.h" #include "logging.h" #include "shmem.h" From efb5d577de3a5ed58ec06803ab2e6cad48d64d47 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 4 Jun 2020 11:45:21 +0200 Subject: [PATCH 511/523] Updated CMakeLists for all modules with new names --- core/CMakeLists.txt | 8 ++++++++ modules/ObjectMonitoring/CMakeLists.txt | 3 +-- modules/ScenarioControl/CMakeLists.txt | 3 +-- modules/Supervision/CMakeLists.txt | 3 +-- modules/Visualization/CMakeLists.txt | 3 +-- 5 files changed, 12 insertions(+), 8 deletions(-) diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index e06cfc8f5..8412f7202 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -19,11 +19,19 @@ set(MESSAGE_BUS_LIBRARY MaestroMQ) set(SHARED_MEMORY_LIBRARY MaestroSHM) set(ISO_22133_LIBRARY ISO22133) set(MATH_LIBRARY m) + +# Get header directories for build interface get_target_property(TIME_HEADERS ${TIME_LIBRARY} PUBLIC_HEADER) get_target_property(LOGGING_HEADERS ${LOGGING_LIBRARY} PUBLIC_HEADER) get_target_property(MESSAGE_BUS_HEADERS ${MESSAGE_BUS_LIBRARY} PUBLIC_HEADER) get_target_property(SHARED_MEMORY_HEADERS ${SHARED_MEMORY_LIBRARY} PUBLIC_HEADER) get_target_property(ISO_22133_HEADERS ${ISO_22133_LIBRARY} PUBLIC_HEADER) +get_filename_component(TIME_HEADERS ${TIME_HEADERS} DIRECTORY) +get_filename_component(LOGGING_HEADERS ${LOGGING_HEADERS} DIRECTORY) +get_filename_component(MESSAGE_BUS_HEADERS ${MESSAGE_BUS_HEADERS} DIRECTORY) +get_filename_component(SHARED_MEMORY_HEADERS ${SHARED_MEMORY_HEADERS} DIRECTORY) +get_filename_component(ISO_22133_HEADERS ${ISO_22133_HEADERS} DIRECTORY) + include(GNUInstallDirs) diff --git a/modules/ObjectMonitoring/CMakeLists.txt b/modules/ObjectMonitoring/CMakeLists.txt index 9393b8bf2..30f2b3e8c 100644 --- a/modules/ObjectMonitoring/CMakeLists.txt +++ b/modules/ObjectMonitoring/CMakeLists.txt @@ -10,8 +10,7 @@ project(ObjectMonitoring LANGUAGES C CXX) set(OBJECT_MONITORING_TARGET ${PROJECT_NAME}) set(COREUTILS_LIBRARY MaestroCoreUtil) -set(ISO_22133_LIBRARY MaestroISO22133) -set(POSITIONING_LIBRARY MaestroPositioning) +set(ISO_22133_LIBRARY ISO22133) set(TIME_LIBRARY MaestroTime) set(LOGGING_LIBRARY MaestroLogging) set(MESSAGE_BUS_LIBRARY MaestroMQ) diff --git a/modules/ScenarioControl/CMakeLists.txt b/modules/ScenarioControl/CMakeLists.txt index 8a9290390..dd295ec4c 100644 --- a/modules/ScenarioControl/CMakeLists.txt +++ b/modules/ScenarioControl/CMakeLists.txt @@ -10,8 +10,7 @@ project(ScenarioControl LANGUAGES C CXX) set(SCENARIO_CONTROL_TARGET ${PROJECT_NAME}) set(COREUTILS_LIBRARY MaestroCoreUtil) -set(ISO_22133_LIBRARY MaestroISO22133) -set(POSITIONING_LIBRARY MaestroPositioning) +set(ISO_22133_LIBRARY ISO22133) set(TIME_LIBRARY MaestroTime) set(LOGGING_LIBRARY MaestroLogging) set(MESSAGE_BUS_LIBRARY MaestroMQ) diff --git a/modules/Supervision/CMakeLists.txt b/modules/Supervision/CMakeLists.txt index d5589392e..70950badb 100644 --- a/modules/Supervision/CMakeLists.txt +++ b/modules/Supervision/CMakeLists.txt @@ -10,8 +10,7 @@ project(Supervision LANGUAGES C CXX) set(SUPERVISION_TARGET ${PROJECT_NAME}) set(COREUTILS_LIBRARY MaestroCoreUtil) -set(ISO_22133_LIBRARY MaestroISO22133) -set(POSITIONING_LIBRARY MaestroPositioning) +set(ISO_22133_LIBRARY ISO22133) set(TIME_LIBRARY MaestroTime) set(LOGGING_LIBRARY MaestroLogging) set(MESSAGE_BUS_LIBRARY MaestroMQ) diff --git a/modules/Visualization/CMakeLists.txt b/modules/Visualization/CMakeLists.txt index cea98b06b..9e000180b 100644 --- a/modules/Visualization/CMakeLists.txt +++ b/modules/Visualization/CMakeLists.txt @@ -8,8 +8,7 @@ project(Visualization LANGUAGES C) set(VISUALIZATION_TARGET ${PROJECT_NAME}) set(COREUTILS_LIBRARY MaestroCoreUtil) -set(ISO_22133_LIBRARY MaestroISO22133) -set(POSITIONING_LIBRARY MaestroPositioning) +set(ISO_22133_LIBRARY ISO22133) set(TIME_LIBRARY MaestroTime) set(LOGGING_LIBRARY MaestroLogging) set(MESSAGE_BUS_LIBRARY MaestroMQ) From d24ed6191d3bcd44a30d5387e825a16d7a6f3c0d Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 4 Jun 2020 11:48:18 +0200 Subject: [PATCH 512/523] Util reference to include iso22133 header update --- util | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/util b/util index 74565e185..cffd1bd6b 160000 --- a/util +++ b/util @@ -1 +1 @@ -Subproject commit 74565e1851d3f6682e13a229404466de9bcebbe7 +Subproject commit cffd1bd6bd0fcbf50811f17361c714f9738e74fe From bdbfb1c1bf5bdbbefe67a8890231bab0cd6be79f Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 4 Jun 2020 12:13:28 +0200 Subject: [PATCH 513/523] Simplified CMakeLists header handling --- core/CMakeLists.txt | 15 +++++---------- 1 file changed, 5 insertions(+), 10 deletions(-) diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index 8412f7202..ccfcb5219 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -21,16 +21,11 @@ set(ISO_22133_LIBRARY ISO22133) set(MATH_LIBRARY m) # Get header directories for build interface -get_target_property(TIME_HEADERS ${TIME_LIBRARY} PUBLIC_HEADER) -get_target_property(LOGGING_HEADERS ${LOGGING_LIBRARY} PUBLIC_HEADER) -get_target_property(MESSAGE_BUS_HEADERS ${MESSAGE_BUS_LIBRARY} PUBLIC_HEADER) -get_target_property(SHARED_MEMORY_HEADERS ${SHARED_MEMORY_LIBRARY} PUBLIC_HEADER) -get_target_property(ISO_22133_HEADERS ${ISO_22133_LIBRARY} PUBLIC_HEADER) -get_filename_component(TIME_HEADERS ${TIME_HEADERS} DIRECTORY) -get_filename_component(LOGGING_HEADERS ${LOGGING_HEADERS} DIRECTORY) -get_filename_component(MESSAGE_BUS_HEADERS ${MESSAGE_BUS_HEADERS} DIRECTORY) -get_filename_component(SHARED_MEMORY_HEADERS ${SHARED_MEMORY_HEADERS} DIRECTORY) -get_filename_component(ISO_22133_HEADERS ${ISO_22133_HEADERS} DIRECTORY) +get_target_property(TIME_HEADERS ${TIME_LIBRARY} INCLUDE_DIRECTORIES) +get_target_property(LOGGING_HEADERS ${LOGGING_LIBRARY} INCLUDE_DIRECTORIES) +get_target_property(MESSAGE_BUS_HEADERS ${MESSAGE_BUS_LIBRARY} INCLUDE_DIRECTORIES) +get_target_property(SHARED_MEMORY_HEADERS ${SHARED_MEMORY_LIBRARY} INCLUDE_DIRECTORIES) +get_target_property(ISO_22133_HEADERS ${ISO_22133_LIBRARY} INCLUDE_DIRECTORIES) include(GNUInstallDirs) From 171fe5a45902dd99ecd8bbeb50e30e908f48aa4d Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 4 Jun 2020 12:18:30 +0200 Subject: [PATCH 514/523] Updated util reference --- util | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/util b/util index cffd1bd6b..763201c8c 160000 --- a/util +++ b/util @@ -1 +1 @@ -Subproject commit cffd1bd6bd0fcbf50811f17361c714f9738e74fe +Subproject commit 763201c8c2c6a8ee25113c7829233fe7b5239b65 From 95946951bfa3e3179368a4b2e38c39940c388f5f Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 4 Jun 2020 13:25:13 +0200 Subject: [PATCH 515/523] memset 0 for path variables --- core/src/logger.c | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/core/src/logger.c b/core/src/logger.c index a5c9f8a86..df6165a03 100644 --- a/core/src/logger.c +++ b/core/src/logger.c @@ -354,6 +354,17 @@ void vInitializeLog(char *logFilePath, unsigned int filePathLength, char *csvLog struct dirent *ent; int read; + memset(confPathDir, 0, sizeof(confPathDir)); + memset(confFilePath, 0, sizeof(confFilePath)); + memset(trigFilePath, 0, sizeof(trigFilePath)); + memset(trajPathDir, 0, sizeof(trajPathDir)); + memset(journalPathDir, 0, sizeof(journalPathDir)); + memset(logFileDirectoryPath, 0, sizeof(logFileDirectoryPath)); + memset(DateBuffer, 0, sizeof(DateBuffer)); + memset(logFilePath, 0, filePathLength); + memset(csvLogFilePath, 0, csvFilePathLength); + + UtilGetConfDirectoryPath(confPathDir, sizeof (confPathDir)); strcat(confFilePath, confPathDir); strcat(confFilePath, CONF_FILE_NAME); From 192e293d71775c1ffb606b1654186a8dff49354e Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 4 Jun 2020 13:55:55 +0200 Subject: [PATCH 516/523] Updated util reference --- util | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/util b/util index 763201c8c..f2e4b1b81 160000 --- a/util +++ b/util @@ -1 +1 @@ -Subproject commit 763201c8c2c6a8ee25113c7829233fe7b5239b65 +Subproject commit f2e4b1b817688badb938e78587be7bf096152514 From c46d545a2f163c439f317b2be8b2d45df2142883 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 4 Jun 2020 14:20:20 +0200 Subject: [PATCH 517/523] Ran code formatter --- core/src/logger.c | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/core/src/logger.c b/core/src/logger.c index df6165a03..404db239d 100644 --- a/core/src/logger.c +++ b/core/src/logger.c @@ -354,13 +354,13 @@ void vInitializeLog(char *logFilePath, unsigned int filePathLength, char *csvLog struct dirent *ent; int read; - memset(confPathDir, 0, sizeof(confPathDir)); - memset(confFilePath, 0, sizeof(confFilePath)); - memset(trigFilePath, 0, sizeof(trigFilePath)); - memset(trajPathDir, 0, sizeof(trajPathDir)); - memset(journalPathDir, 0, sizeof(journalPathDir)); - memset(logFileDirectoryPath, 0, sizeof(logFileDirectoryPath)); - memset(DateBuffer, 0, sizeof(DateBuffer)); + memset(confPathDir, 0, sizeof (confPathDir)); + memset(confFilePath, 0, sizeof (confFilePath)); + memset(trigFilePath, 0, sizeof (trigFilePath)); + memset(trajPathDir, 0, sizeof (trajPathDir)); + memset(journalPathDir, 0, sizeof (journalPathDir)); + memset(logFileDirectoryPath, 0, sizeof (logFileDirectoryPath)); + memset(DateBuffer, 0, sizeof (DateBuffer)); memset(logFilePath, 0, filePathLength); memset(csvLogFilePath, 0, csvFilePathLength); From 70669aa1c3a65ae1721de28631858993114a0869 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Thu, 4 Jun 2020 15:15:14 +0200 Subject: [PATCH 518/523] Updated util reference to master 0.5 --- util | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/util b/util index 763201c8c..55c7b8726 160000 --- a/util +++ b/util @@ -1 +1 @@ -Subproject commit 763201c8c2c6a8ee25113c7829233fe7b5239b65 +Subproject commit 55c7b8726c87418e7d6b5049c97f8d33af147dc1 From f5eacb74813caeabe509d238a74a675a25e0b42d Mon Sep 17 00:00:00 2001 From: Lukas Date: Thu, 4 Jun 2020 17:09:27 +0200 Subject: [PATCH 519/523] Update conf/triggeraction.conf --- conf/triggeraction.conf | 1 - 1 file changed, 1 deletion(-) diff --git a/conf/triggeraction.conf b/conf/triggeraction.conf index 1f402b496..4b68eb05c 100644 --- a/conf/triggeraction.conf +++ b/conf/triggeraction.conf @@ -1,2 +1 @@ #trigger_ip;trigger_type[parameter];action_ip;action_type[parameter]; -127.0.0.1;DISTANCE[TO:(x:5.3y:2.5z:2.1),LESS_THAN,5.0];0.0.0.0;TEST_SCENARIO_COMMAND[START,0]; From 827b7b628ef220d9b5ab8991d02a6c483344c823 Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 5 Jun 2020 11:21:58 +0200 Subject: [PATCH 520/523] Updated version in CMakeLists --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 74df14435..b4b79b377 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,7 +6,7 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_VERBOSE_MAKEFILE ON) set(CMAKE_COLOR_MAKEFILE ON) -project(Maestro) +project(Maestro VERSION 0.5.0) set(MAESTRO_TEST_DIR ".maestro") set(MAESTRO_JOURNAL_DIR "${MAESTRO_TEST_DIR}/journal") From 8932b3d64719d65d1d3a5709ba890336888840cf Mon Sep 17 00:00:00 2001 From: Lukas Wikander Date: Fri, 5 Jun 2020 11:23:45 +0200 Subject: [PATCH 521/523] Updated version in util.h --- core/inc/util.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/core/inc/util.h b/core/inc/util.h index 5b221b309..23966130c 100644 --- a/core/inc/util.h +++ b/core/inc/util.h @@ -38,7 +38,7 @@ extern "C"{ /*------------------------------------------------------------ -- Defines ------------------------------------------------------------*/ -#define MaestroVersion "0.4.1" +#define MaestroVersion "0.5.0" #define DEFAULT_ORIGIN_LAT 57.777073115 #define DEFAULT_ORIGIN_LOG 12.781295498333 From d594398acef8a713a006d883db218553a50b5c58 Mon Sep 17 00:00:00 2001 From: Lukas Date: Thu, 11 Jun 2020 15:24:22 +0200 Subject: [PATCH 522/523] Update core/src/systemcontrol.c --- core/src/systemcontrol.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/core/src/systemcontrol.c b/core/src/systemcontrol.c index b35234558..4bf8c6bd8 100644 --- a/core/src/systemcontrol.c +++ b/core/src/systemcontrol.c @@ -733,7 +733,7 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { } break; case GetRootDirectoryContent_0: - LogMessage(LOG_LEVEL_ERROR, "GetRootDirectory called"); + LogMessage(LOG_LEVEL_INFO, "GetRootDirectory called: defaulting to GetDirectoryContent"); case GetDirectoryContent_1: if (CurrentInputArgCount == CommandArgCount) { SystemControlCommand = Idle_0; From ca74917731b4ebf8e5650ad9a8580f1d3c37b36e Mon Sep 17 00:00:00 2001 From: hyle Date: Fri, 12 Jun 2020 09:12:47 +0200 Subject: [PATCH 523/523] Removed older version of same script --- .../manual tests/MSCPManualTest.py | 111 ------------------ 1 file changed, 111 deletions(-) delete mode 100644 core/integration-tests/manual tests/MSCPManualTest.py diff --git a/core/integration-tests/manual tests/MSCPManualTest.py b/core/integration-tests/manual tests/MSCPManualTest.py deleted file mode 100644 index 524535102..000000000 --- a/core/integration-tests/manual tests/MSCPManualTest.py +++ /dev/null @@ -1,111 +0,0 @@ -from tools.MSCP import MSCP -from tools.Executable import Executable -import time -import subprocess -import sys - - -tests = ["INIT", - "CONNECTOBJECT", - "ARM", - "START", - "ABORT", - "GETSERVERSTATUS", - "CREATEOBJECTS", - "DISARM", - "GETSERVERTIME", - "UPLOADFILE", - "DOWNLOADFILE", - "DELETEFILE", - "CHECKFILEEXISTS", - "REMOTECONTROL", - "SETSERVERPARAMETER", - "GETSERVERPARAMETERS", - "GETDIRECTORYCONTENT", - "DISCONNECTOBJECT" ] - - -if __name__ == "__main__": - - S = Executable("../build/TEServer",["-m","0"]) - time.sleep(0.05) - M = MSCP("127.0.0.1") - time.sleep(1) - - print("Starting test") - - - for t in tests: - print("\n Press enter to test {}".format(t)) - s = input() - - if(t == "INIT"): - M.Init() - - if(t == "CONNECTOBJECT"): - M.Connect() - - if(t == "ARM"): - M.Arm() - - if(t == "START"): - M.Start(0) - - if(t == "ABORT"): - M.Abort() - - if(t == "GETSERVERSTATUS"): - M.GetStatus() - - if(t == "CREATEOBJECTS"): - M.CreateObjects(1) - - if(t == "DISARM"): - M.Disarm() - - if(t == "GETSERVERTIME"): - M.GetServerTime() - - #if(t == "UPLOADFILE"): - # M.UploadFile("","") - - if(t == "DOWNLOADFILE"): - M.DownloadFile() - - if(t == "DELETEFILE"): - M.DeleteFile() - - if(t == "CHECKFILEEXISTS"): - M.CheckFileExists() - - if(t == "REMOTECONTROL"): - M.RemoteControl() - - if(t == "SETSERVERPARAMETER"): - M.SetServerParameter() - - if(t == "GETSERVERPARAMETERS"): - M.GetServerParameters() - - if(t == "GETDIRECTORYCONTENT"): - M.GetDirectoryContent() - - if(t == "DISCONNECTOBJECT"): - M.Disconnect() - - - print("Checking server...") - time.sleep(1) - if S.poll(): - S.stop() - M.shutdown() - sys.exit(1) - - - S.stop() - M.shutdown() - sys.exit(1) - - - -