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 diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 000000000..b4b79b377 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,50 @@ +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) +set(CMAKE_VERBOSE_MAKEFILE ON) +set(CMAKE_COLOR_MAKEFILE ON) + +project(Maestro VERSION 0.5.0) + +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) +set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib) +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/ObjectMonitoring) + +# 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_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_TEST_DIR})") + diff --git a/Jenkinsfile b/Jenkinsfile index 71c65f554..6596d68cc 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -1,30 +1,34 @@ 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..."' + cmakeBuild cleanBuild: true, buildDir: 'build', installation: 'InSearchPath', steps: [[envVars: 'DESTDIR=${WORKSPACE}/artifacts', withCmake: true]] + } + } + 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' + } + } + } + } + } } diff --git a/README.md b/README.md index 74609943b..92b0b76b1 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,31 @@ 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 +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 and configure the project: ```sh -./TEServer +cd build +cmake -G "Unix Makefiles" .. +``` +then build and install the server (be aware that this requires superuser privileges) +```sh +make install ``` ## Building the server with CITS module and mqtt @@ -83,18 +92,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 .. ``` diff --git a/allMaestroIntegrationTests.sh b/allMaestroIntegrationTests.sh index 4c880c3f8..51d178dff 100755 --- a/allMaestroIntegrationTests.sh +++ b/allMaestroIntegrationTests.sh @@ -1,27 +1,34 @@ #!/bin/bash MAESTRODIR=$(pwd) -cd ${MAESTRODIR}/server/integration-tests +cd ${MAESTRODIR}/core/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 diff --git a/buildMaestro.sh b/buildMaestro.sh deleted file mode 100755 index bad494e87..000000000 --- a/buildMaestro.sh +++ /dev/null @@ -1,52 +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 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 - diff --git a/server/conf/adaptivesync.conf b/conf/adaptivesync.conf old mode 100755 new mode 100644 similarity index 100% rename from server/conf/adaptivesync.conf rename to conf/adaptivesync.conf diff --git a/server/conf/test.conf b/conf/test.conf similarity index 100% rename from server/conf/test.conf rename to conf/test.conf diff --git a/conf/triggeraction.conf b/conf/triggeraction.conf new file mode 100644 index 000000000..4b68eb05c --- /dev/null +++ b/conf/triggeraction.conf @@ -0,0 +1 @@ +#trigger_ip;trigger_type[parameter];action_ip;action_type[parameter]; diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt new file mode 100644 index 000000000..ccfcb5219 --- /dev/null +++ b/core/CMakeLists.txt @@ -0,0 +1,112 @@ +cmake_minimum_required(VERSION 3.1) +set(CMAKE_C_STANDARD 11) +set(CMAKE_C_STANDARD_REQUIRED ON) + +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 ${PROJECT_NAME}) +set(COREUTILS_TARGET MaestroCoreUtil) +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 header directories for build interface +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) + +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(${COREUTILS_TARGET} SHARED + ${CMAKE_CURRENT_SOURCE_DIR}/src/util.c +) +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(${DATA_DICTIONARY_TARGET} SHARED + ${CMAKE_CURRENT_SOURCE_DIR}/src/datadictionary.c +) +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 +) + +# Linking +target_link_libraries(${CORE_TARGET} LINK_PUBLIC + ${TIME_LIBRARY} + ${LOGGING_LIBRARY} + ${COREUTILS_TARGET} + ${DATA_DICTIONARY_TARGET} + ${ISO_22133_LIBRARY} +) + +target_link_libraries(${COREUTILS_TARGET} LINK_PUBLIC + ${TIME_LIBRARY} + ${LOGGING_LIBRARY} + ${MESSAGE_BUS_LIBRARY} + ${MATH_LIBRARY} + ${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} + ${DATA_DICTIONARY_TARGET} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + 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})") + +# 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})") + +# Ensure linkage is reloaded after install +install(CODE "execute_process(COMMAND ldconfig)") 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 92% rename from server/inc/datadictionary.h rename to core/inc/datadictionary.h index 5c8287c91..e7a5e3d5e 100644 --- a/server/inc/datadictionary.h +++ b/core/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. @@ -119,7 +121,16 @@ ReadWriteAccess_t DataDictionaryGetMiscDataC8(GSDType *GSD, C8 *MiscData, U32 Bu ReadWriteAccess_t DataDictionarySetOBCStateU8(GSDType *GSD, OBCState_t OBCState); OBCState_t DataDictionaryGetOBCStateU8(GSDType *GSD); +ReadWriteAccess_t DataDictionaryFreeMonitorData(); +ReadWriteAccess_t DataDictionaryInitMonitorData(); +ReadWriteAccess_t DataDictionarySetMonitorData(const MonitorDataType * monitorData); +ReadWriteAccess_t DataDictionaryGetMonitorData(MonitorDataType * monitorData, const uint32_t TransmitterId); + +ReadWriteAccess_t DataDictionarySetNumberOfObjects(const uint32_t newNumberOfObjects); +ReadWriteAccess_t DataDictionaryGetNumberOfObjects(uint32_t *numberOfObjects); + ReadWriteAccess_t DataDictionaryConstructor(GSDType *GSD); +ReadWriteAccess_t DataDictionaryDestructor(GSDType *GSD); #endif 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/core/inc/objectcontrol.h b/core/inc/objectcontrol.h new file mode 100644 index 000000000..b794ff533 --- /dev/null +++ b/core/inc/objectcontrol.h @@ -0,0 +1,24 @@ +/*------------------------------------------------------------------------------ + -- Copyright : (C) 2016 CHRONOS project + ------------------------------------------------------------------------------ + -- File : objectcontrol.h + -- Author : Sebastian Loh Lindholm + -- Description : CHRONOS + -- Purpose : + -- Reference : + ------------------------------------------------------------------------------*/ + +#ifndef __OBJECTCONTROL_H_INCLUDED__ +#define __OBJECTCONTROL_H_INCLUDED__ + +#include +#include +#include "util.h" +#include "logging.h" + +/*------------------------------------------------------------ + -- Function declarations. + ------------------------------------------------------------*/ +void objectcontrol_task(TimeType *GPSTime, GSDType *GSD, LOG_LEVEL logLevel); + +#endif //__OBJECTCONTROL_H_INCLUDED__ 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 68% rename from server/inc/util.h rename to core/inc/util.h index f85df3b8a..23966130c 100644 --- a/server/inc/util.h +++ b/core/inc/util.h @@ -33,14 +33,36 @@ extern "C"{ #include "mqbus.h" #include "iso22133.h" #include "logging.h" +#include "positioning.h" /*------------------------------------------------------------ -- Defines ------------------------------------------------------------*/ -#define MaestroVersion "0.4.1" - -#define ISO_PROTOCOL_VERSION 2 -#define ACK_REQ 0 +#define MaestroVersion "0.5.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 +#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 @@ -52,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 @@ -68,7 +85,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 @@ -76,7 +94,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 "195.0.0.10" #define USE_TEST_HOST 0 #define TESTHOST_IP LOCAL_USER_CONTROL_IP #define TESTSERVER_IP LOCAL_USER_CONTROL_IP @@ -96,9 +114,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 @@ -133,34 +148,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_MONR_STRUCT 0x80 -#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_HEAB_STRUCT 0x90 -#define VALUE_ID_INSUP_MODE 0x200 - #define C8 uint8_t #define U8 uint8_t #define I8 int8_t @@ -173,11 +160,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 SYNC_WORD 0x7e7e - #define SERVER_PREPARED 0x01 #define SERVER_PREPARED_BIG_PACKET_SIZE 0x02 #define PATH_INVALID_MISSING 0x03 @@ -189,44 +171,18 @@ 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 // 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 -#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_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) - -#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 @@ -259,12 +215,20 @@ COMM_TREO = 23, COMM_ACCM = 24, COMM_TRCM = 25, COMM_DISARM = 26, +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 struct { + RemoteControlManoeuvreType manoeuvre; + in_addr_t objectIP; +} RemoteControlCommandType; + typedef struct { double Latitude; @@ -276,187 +240,33 @@ typedef struct typedef struct { - double xCoord_m; - double yCoord_m; - double zCoord_m; - double heading_deg; -} 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; - 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; - U16 StateValueIdU16; - U16 StateContentLengthU16; - U8 StateU8; -} OSTMType; //16 bytes - - -typedef struct -{ - HeaderType Header; - U16 ModeValueIdU16; - U16 ModeContentLengthU16; - U8 ModeU8; -} INSUPType; //16 bytes - - -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 -{ - HeaderType Header; - U16 HeabStructValueIdU16; - U16 HeabStructContentLengthU16; - U32 GPSQmsOfWeekU32; - 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 -{ - MONRType MONR; + ObjectMonitorType data; in_addr_t ClientIP; + 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; + 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 { @@ -508,6 +318,7 @@ typedef enum { OBC_STATE_CONNECTED, OBC_STATE_ARMED, OBC_STATE_RUNNING, + OBC_STATE_REMOTECTRL, OBC_STATE_ERROR } OBCState_t; @@ -527,11 +338,14 @@ 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; + char *memory; //U8 OSTMSizeU8; //U8 OSTMData[100]; //U8 STRTSizeU8; @@ -587,18 +401,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; @@ -676,43 +490,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; - - -typedef struct -{ - HeaderType Header; - U32 GPSQmsOfWeekU32; - U8 WorldStateU8; - U8 ObjectCountU8; - Sim1Type SimObjects[16]; - -} 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]; @@ -746,6 +523,8 @@ typedef enum { } ReadWriteAccess_t; +#pragma pack(push,1) + typedef struct { U32 MessageLengthU32; @@ -776,6 +555,7 @@ typedef struct U8 SysCtrlStateU8; } RVSSMaestroType; +#pragma pack(pop) typedef enum { NORTHERN, @@ -833,15 +613,24 @@ void UtilGetConfDirectoryPath(char* path, size_t pathLen); void UtilGetTrajDirectoryPath(char* path, size_t pathLen); 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); +int UtilDeleteGenericFile(const char * genericFilePath, const size_t nameLen); + // 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); + -// -CartesianPosition MONRToCartesianPosition(MonitorDataType MONR); -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); +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); @@ -897,15 +686,9 @@ 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 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); 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); diff --git a/server/integration-tests/000-StartupAndKillMainExecutable.sh b/core/integration-tests/000-StartupAndKillMainExecutable.sh similarity index 94% rename from server/integration-tests/000-StartupAndKillMainExecutable.sh rename to core/integration-tests/000-StartupAndKillMainExecutable.sh index dbf04c2aa..e5ec1a751 100755 --- a/server/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/server/integration-tests/005-UserControlConnect.py b/core/integration-tests/005-UserControlConnect.py similarity index 88% rename from server/integration-tests/005-UserControlConnect.py rename to core/integration-tests/005-UserControlConnect.py index d48382093..266653c1a 100644 --- a/server/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/server/integration-tests/100-SingleTypicalTest.py b/core/integration-tests/100-SingleTypicalTest.py similarity index 91% rename from server/integration-tests/100-SingleTypicalTest.py rename to core/integration-tests/100-SingleTypicalTest.py index 59a22da13..f03cd805a 100644 --- a/server/integration-tests/100-SingleTypicalTest.py +++ b/core/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() @@ -27,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") @@ -45,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 new file mode 100644 index 000000000..2c7435644 --- /dev/null +++ b/core/integration-tests/249-uploadRowMismatchingTrajectories.py @@ -0,0 +1,105 @@ +from tools.MSCP import MSCP +from tools.Executable import Executable +from tools.TrajectoryFile import * +from tools.PortChecker import * +import time +import subprocess +import sys + + +userControl = None +server = None +obj = None + + +def checkProgramStatus(failurePrintout): + if server != None: + if server.poll(): + print(failurePrintout) + if userControl != None: + userControl.shutdown() + server.stop() + if obj != None: + obj.stop() + sys.exit(1) + +if __name__ == "__main__": + + 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"]) + 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("127.0.0.1", fewRowTraj, "trajectory") + + # 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("127.0.0.1", normalTraj, "trajectory") + + userControl.Init() + userControl.waitForObjectControlState("INITIALIZED") + + time.sleep(0.05) + + userControl.Disconnect() + userControl.waitForObjectControlState("IDLE") + + # 7: Upload long trajectory + userControl.UploadFile("127.0.0.1", manyRowTraj, "trajectory") + + # 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) + + # 9: Upload normal trajectory, to verify we can still initialise + userControl.UploadFile("127.0.0.1", normalTraj,"trajectory") + + userControl.Init() + userControl.waitForObjectControlState("INITIALIZED") + + time.sleep(0.05) + + userControl.Disconnect() + userControl.waitForObjectControlState("IDLE") + + finally: + # 10: Done! + userControl.shutdown() + server.stop() + + sys.exit(0) + diff --git a/core/integration-tests/MSCPManualTest.py b/core/integration-tests/MSCPManualTest.py new file mode 100644 index 000000000..f25aaa5f7 --- /dev/null +++ b/core/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) + + + + diff --git a/core/integration-tests/ManualMessageTest.py b/core/integration-tests/ManualMessageTest.py new file mode 100644 index 000000000..f25aaa5f7 --- /dev/null +++ b/core/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) + + + + 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/core/integration-tests/tools/ISO.py b/core/integration-tests/tools/ISO.py new file mode 100644 index 000000000..e98a8819b --- /dev/null +++ b/core/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 diff --git a/core/integration-tests/tools/MSCP.py b/core/integration-tests/tools/MSCP.py new file mode 100644 index 000000000..3e3a0e748 --- /dev/null +++ b/core/integration-tests/tools/MSCP.py @@ -0,0 +1,357 @@ +import socket +import sys, select +import threading +import re +import time + +class MSCP: + def __init__(self,host,port=54241): + self.host = host + self.port = port + self.socket = socket.socket() + print("=== MSCP connecting to " + str(self.host) + ":" + str(self.port)) + self.socket.connect((self.host,self.port)) + self.lastStatusReply = {} + self.lastAbortReply = {} + self.lastStartReply = {} + self.lastUploadReply = {} + self.lastStatusReply["objectControlState"] = "UNKNOWN" + self.lastStatusReply["systemControlState"] = "UNKNOWN" + self.lastStatusReply["systemControlErrorCode"] = 0 + self.lastStatusReply["objectControlErrorCode"] = 0 + self.lastAbortReply["scenarioActive"] = 0 + self.lastStartReply["scenarioActive"] = 0 + self.lastUploadReply["status"] = "UNKNOWN" + self.uploadReplyLock = threading.Lock() + self.startReplyLock = threading.Lock() + self.abortReplyLock = threading.Lock() + self.statusReplyLock = threading.Lock() + self.responseCodeLock = threading.Lock() + self.quit = False + self.lastResponseCode = "No message received yet" + self.listener = threading.Thread(target=self.listen) + self.listener.start() + + def listen(self): + replyPatterns = [ + {"command": "init", "regex": re.compile(b'(..)InitializeScenario:')}, + {"command": "status", "regex": re.compile(b'(..)GetServerStatus:(.)(.)(.)?(.)?')}, + {"command": "abort", "regex": re.compile(b'(..)AbortScenario:(.)')}, + {"command": "arm", "regex": re.compile(b'(..)ArmScenario:')}, + {"command": "start", "regex": re.compile(b'(..)StartScenario:(.)')}, + {"command": "connect", "regex": re.compile(b'(..)ConnectObject:')}, + {"command": "disconnect", "regex": re.compile(b'(..)DisconnectObject:')}, + {"command": "upload", "regex": re.compile(b'([^u][^b])UploadFile:(.)')}, + {"command": "subupload", "regex": re.compile(b'(..)SubUploadFile:(.)')} + ] + + print("=== Starting listener on " + str(self.host) + ":" + str(self.port)) + while not self.quit: + try: + byteData = self.socket.recv(2048) + data = bytearray(byteData) + except ConnectionResetError as e: + if not self.quit: + raise e + + 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() + data = data[match.end():] + 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" + 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" + + 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() + else: + print("=== Unable to match against data") + data = bytearray([]) + + + def GetStatus(self): + 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();\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();\r\n\r\n" + 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) + print("=== Init() sent") + + 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" + 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) + ");\r\n\r\n" + self.Send(message) + print("=== StartScenario() sent") + + def UploadFile(self,fileName,fileContents,fileType): + packetSize = 1200 + 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() + self.Send(message) + print("=== UploadFile() sent") + self.waitForUploadReply("SERVER_PREPARED") + print("=== Sending file contents") + # Send file + 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") + + 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()) + + def shutdown(self): + self.quit = True + self.socket.close() + + def waitForUploadReply(self,status,timeout=3.0): + timeoutTime = time.time() + timeout + self.uploadReplyLock.acquire() + ur = self.lastUploadReply["status"] + self.uploadReplyLock.release() + while ur == "UNKNOWN" and time.time() < timeoutTime: + self.uploadReplyLock.acquire() + ur = self.lastUploadReply["status"] + self.uploadReplyLock.release() + + if ur != status and time.time() >= timeoutTime: + print("=== Timed out while waiting for upload reply") + raise TimeoutError("Timed out while waiting for reply to UploadFile") + elif ur != status: + print("=== File upload error response") + raise ValueError("Expected status " + status + " but received " + ur) + + def waitForObjectControlState(self,state,timeout=3.0): + timeoutTime = time.time() + timeout + self.statusReplyLock.acquire() + sr = self.lastStatusReply["objectControlState"] + self.statusReplyLock.release() + while sr != state and time.time() < timeoutTime: + time.sleep(0.005) + self.statusReplyLock.acquire() + sr = self.lastStatusReply["objectControlState"] + self.statusReplyLock.release() + print("=== Expecting: " + state + ", Current: " + sr) + self.GetStatus() + + if sr != state: + raise TimeoutError("Timed out while waiting for transition to " + state) + + def interpretResponseCode(self,code): + num = int.from_bytes(code,byteorder='big') + if num == 0x0001: + return "OK" + elif num == 0x0002: + return "Logging data" + elif num == 0x0F10: + return "Error" + elif num == 0x0F20: + return "Function not available" + elif num == 0x0F25: + return "Incorrect state" + elif num == 0x0F30: + return "Invalid length" + elif num == 0x0F40: + return "Busy" + elif num == 0x0F42: + return "Timeout" + elif num == 0x0F50: + return "Invalid script" + elif num == 0x0F60: + return "Invalid encryption code" + elif num == 0x0F61: + return "Decryption error" + elif num == 0x0F62: + return "No data" + else: + raise ValueError("Response code " + str(num) + " is not recognized") + 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 86% rename from server/src/citscontrol.c rename to core/src/citscontrol.c index 05f45faeb..f206e4f4c 100644 --- a/server/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/server/src/datadictionary.c b/core/src/datadictionary.c similarity index 89% rename from server/src/datadictionary.c rename to core/src/datadictionary.c index 2b93a108d..680cdedb8 100644 --- a/server/src/datadictionary.c +++ b/core/src/datadictionary.c @@ -11,11 +11,16 @@ /*------------------------------------------------------------ -- Include files. ------------------------------------------------------------*/ + + #include #include - +#include +#include #include "datadictionary.h" #include "logging.h" +#include "shmem.h" + // Parameters and variables static pthread_mutex_t OriginLatitudeMutex = PTHREAD_MUTEX_INITIALIZER; @@ -44,6 +49,9 @@ static pthread_mutex_t ASPDataMutex = PTHREAD_MUTEX_INITIALIZER; static pthread_mutex_t MiscDataMutex = PTHREAD_MUTEX_INITIALIZER; static pthread_mutex_t OBCStateMutex = PTHREAD_MUTEX_INITIALIZER; +#define MONR_DATA_FILENAME "MonitorData" + +static volatile MonitorDataType *monitorDataMemory = NULL; /*------------------------------------------------------------ @@ -88,6 +96,10 @@ 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 ? DataDictionaryInitMonitorData() : Res; + if (Res != WRITE_OK) { + LogMessage(LOG_LEVEL_WARNING, "Preexisting monitor data memory found"); + } DataDictionarySetOBCStateU8(GSD, OBC_STATE_UNDEFINED); @@ -95,6 +107,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 ? DataDictionaryFreeMonitorData() : result; + + return result; +} + + /*! * \brief DataDictionaryInitOriginLatitudeDbl Initializes variable according to the configuration file * \param GSD Pointer to shared allocated memory @@ -1658,12 +1684,199 @@ OBCState_t DataDictionaryGetOBCStateU8(GSDType * GSD) { pthread_mutex_lock(&OBCStateMutex); Ret = GSD->OBCStateU8; pthread_mutex_unlock(&OBCStateMutex); - return Ret; } /*END OBCState*/ +/*! + * \brief DataDictionaryInitMonitorData inits a data structure for saving object monr + * \return Result according to ::ReadWriteAccess_t + */ +ReadWriteAccess_t DataDictionaryInitMonitorData() { + + int createdMemory; + + monitorDataMemory = createSharedMemory(MONR_DATA_FILENAME, 0, sizeof (MonitorDataType), &createdMemory); + if (monitorDataMemory == NULL) { + return UNDEFINED; + } + return createdMemory ? WRITE_OK : READ_OK; +} + +/*! + * \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 DataDictionarySetMonitorData(const MonitorDataType * monitorData) { + + ReadWriteAccess_t result; + + if (monitorDataMemory == NULL) { + errno = EINVAL; + LogMessage(LOG_LEVEL_ERROR, "Shared memory not initialized"); + return UNDEFINED; + } + if (monitorData == NULL) { + errno = EINVAL; + LogMessage(LOG_LEVEL_ERROR, "Shared memory input pointer error"); + return UNDEFINED; + } + if (monitorData->ClientID == 0) { + errno = EINVAL; + LogMessage(LOG_LEVEL_ERROR, "Transmitter ID 0 is reserved"); + return UNDEFINED; + } + + monitorDataMemory = claimSharedMemory(monitorDataMemory); + result = PARAMETER_NOTFOUND; + unsigned int numberOfObjects = getNumberOfMemoryElements(monitorDataMemory); + + LogPrint("Number of objects currently in memory: %u", numberOfObjects); + for (uint32_t i = 0; i < numberOfObjects; ++i) { + 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", + monitorData->ClientID); + for (uint32_t i = 0; i < numberOfObjects; ++i) { + 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) { + 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, monitorDataMemory); + memcpy(&monitorDataMemory[numberOfObjects - 1], monitorData, sizeof (MonitorDataType)); + LogPrint("Printed MONR"); + } + else { + LogMessage(LOG_LEVEL_ERROR, "Error resizing shared memory"); + result = UNDEFINED; + } + } + } + monitorDataMemory = releaseSharedMemory(monitorDataMemory); + + return result; +} + +/*! + * \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 DataDictionaryGetMonitorData(MonitorDataType * monitorData, const uint32_t transmitterId) { + ReadWriteAccess_t result = PARAMETER_NOTFOUND; + + if (monitorData == 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; + } + + monitorDataMemory = claimSharedMemory(monitorDataMemory); + unsigned int numberOfObjects = getNumberOfMemoryElements(monitorDataMemory); + + for (unsigned int i = 0; i < numberOfObjects; ++i) { + if (monitorDataMemory[i].ClientID == transmitterId) { + memcpy(monitorData, &monitorDataMemory[i], sizeof (MonitorDataType)); + result = READ_OK; + } + } + + monitorDataMemory = releaseSharedMemory(monitorDataMemory); + + if (result == PARAMETER_NOTFOUND) { + LogMessage(LOG_LEVEL_WARNING, "Unable to find monitor data for transmitter ID %u", transmitterId); + } + + return result; +} + +/*! + * \brief DataDictionaryFreeMonitorData Releases data structure for saving object monitor data + * \return Result according to ::ReadWriteAccess_t + */ +ReadWriteAccess_t DataDictionaryFreeMonitorData() { + ReadWriteAccess_t result = WRITE_OK; + + if (monitorDataMemory == NULL) { + errno = EINVAL; + LogMessage(LOG_LEVEL_ERROR, "Attempt to free uninitialized memory"); + return UNDEFINED; + } + + destroySharedMemory(monitorDataMemory); + + return result; +} + +/*END of MONR*/ + + +/*NbrOfObjects*/ +/*! + * \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 DataDictionarySetNumberOfObjects(const uint32_t newNumberOfObjects) { + + unsigned int numberOfObjects; + ReadWriteAccess_t result = WRITE_OK; + + monitorDataMemory = claimSharedMemory(monitorDataMemory); + monitorDataMemory = resizeSharedMemory(monitorDataMemory, newNumberOfObjects); + numberOfObjects = getNumberOfMemoryElements(monitorDataMemory); + monitorDataMemory = releaseSharedMemory(monitorDataMemory); + + if (monitorDataMemory == NULL) { + errno = EINVAL; + LogMessage(LOG_LEVEL_ERROR, "Error resizing shared memory"); + return UNDEFINED; + } + + return result; +} + +/*! + * \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 DataDictionaryGetNumberOfObjects(uint32_t * numberOfObjects) { + + monitorDataMemory = claimSharedMemory(monitorDataMemory); + *numberOfObjects = getNumberOfMemoryElements(monitorDataMemory); + monitorDataMemory = releaseSharedMemory(monitorDataMemory); + + return READ_OK; +} + +/*END of NbrOfObjects*/ + /*! * \brief DataDictionarySearchParameter Searches for parameters in the configuration file and returns 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/logger.c b/core/src/logger.c similarity index 87% rename from server/src/logger.c rename to core/src/logger.c index 01c6f4612..404db239d 100644 --- a/server/src/logger.c +++ b/core/src/logger.c @@ -24,6 +24,7 @@ #include #include +#include "positioning.h" #include "maestroTime.h" #include "logger.h" @@ -87,7 +88,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 +116,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 @@ -175,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; @@ -187,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 /* @@ -237,7 +244,7 @@ void logger_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { MonitorDataType monrData; UtilStringToMonitorData(pcReadBuffer, sizeof (pcReadBuffer), &monrData); - NewTimestamp = monrData.MONR.GPSQmsOfWeekU32; + NewTimestamp = TimeGetAsGPSqmsOfWeek(&monrData.data.timestamp); if (!FirstIteration) { /* Wait a little bit */ sleep_time.tv_sec = 0; @@ -335,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; @@ -345,8 +354,22 @@ 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(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)); @@ -373,7 +396,15 @@ 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); @@ -454,9 +485,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); @@ -465,7 +496,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); } @@ -505,7 +536,6 @@ void vInitializeLog(char *logFilePath, unsigned int filePathLength, char *csvLog COMM_VIOP, COMM_INV); (void)fwrite(pcBuffer, 1, strlen(pcBuffer), filefd); - fclose(filefd); } @@ -565,39 +595,58 @@ 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); - UtilPopulateMonitorDataStruct(commandData, (size_t) (commandDatalen), &monitorData, debug); - TimeSetToGPStime(&monrTime, TimeGetAsGPSweek(&systemTime), monitorData.MONR.GPSQmsOfWeekU32); - - 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.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, "%s\n", printBuffer); fclose(filefd); } diff --git a/server/src/main.c b/core/src/main.c similarity index 96% rename from server/src/main.c rename to core/src/main.c index eca12b059..bf5b403ba 100644 --- a/server/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,12 +124,13 @@ 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 && dataDictOperationResult != WRITE_OK) { 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"); @@ -160,9 +161,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); } /*! diff --git a/core/src/objectcontrol.c b/core/src/objectcontrol.c new file mode 100644 index 000000000..87f2f31af --- /dev/null +++ b/core/src/objectcontrol.c @@ -0,0 +1,1767 @@ +/*------------------------------------------------------------------------------ + -- Copyright : (C) 2016 CHRONOS project + ------------------------------------------------------------------------------ + -- File : objectcontrol.c + -- Author : Sebastian Loh Lindholm + -- Description : CHRONOS + -- Purpose : + -- Reference : + ------------------------------------------------------------------------------*/ + +/*------------------------------------------------------------ + -- Include files. + ------------------------------------------------------------*/ +#include "objectcontrol.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "timecontrol.h" +#include "datadictionary.h" +#include "maestroTime.h" +#include "iso22133.h" + + + +/*------------------------------------------------------------ + -- Defines + ------------------------------------------------------------*/ + +// Macro for determining individual struct member sizes +#define member_sizeof(type, member) sizeof(((type *)0)->member) + +#define LOCALHOST "127.0.0.1" + +#define RECV_MESSAGE_BUFFER 6200 +#define BUFFER_SIZE_3100 3100 +#define TRAJ_FILE_HEADER_ROW 256 + +#define OC_SLEEP_TIME_EMPTY_MQ_S 0 +#define OC_SLEEP_TIME_EMPTY_MQ_NS 1000000 +#define OC_SLEEP_TIME_NONEMPTY_MQ_S 0 +#define OC_SLEEP_TIME_NONEMPTY_MQ_NS 0 + +#define OBJECT_CONTROL_CONTROL_MODE 0 +#define OBJECT_CONTROL_REPLAY_MODE 1 +#define OBJECT_CONTROL_ABORT_MODE 1 + +#define OC_STATE_REPORT_PERIOD_S 1 +#define OC_STATE_REPORT_PERIOD_US 0 + +#define TRAJECTORY_TX_BUFFER_SIZE 2048 + +#define SMALL_BUFFER_SIZE_0 20 +#define SMALL_BUFFER_SIZE_1 2 +#define SMALL_BUFFER_SIZE_2 1 +#define SMALL_BUFFER_SIZE_254 254 + +#define TRAJECTORY_FILE_MAX_ROWS 4096 + +#define LOG_BUFFER_LENGTH 128 + +#define USE_TEMP_LOGFILE 0 +#define TEMP_LOG_FILE "log/temp.log" + + +typedef enum { + TRANSITION_RESULT_UNDEFINED, + TRANSITION_OK, + TRANSITION_INVALID, + TRANSITION_MEMORY_ERROR +} StateTransitionResult; + +typedef struct { + uint16_t actionID; + ActionTypeParameter_t command; + in_addr_t ip; +} 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: + void (*fooptr)(int,float) = foo; + fooptr(10,1.5); + + Consequently, the below typedef defines a StateTransition type as a function pointer to a function taking + (OBCState_t, OBCState_t) as input, and returning a StateTransitionResult +*/ +typedef StateTransitionResult(*StateTransition) (OBCState_t * currentState, OBCState_t requestedState); + + +/*------------------------------------------------------------ +-- Function declarations. +------------------------------------------------------------*/ +static I32 vConnectObject(int *sockfd, const char *name, const uint32_t port, U8 * Disconnect); +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); +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 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); + +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); + +OBCState_t vInitializeState(OBCState_t firstState, GSDType * GSD); +inline OBCState_t vGetState(GSDType * GSD); +StateTransitionResult vSetState(OBCState_t requestedState, GSDType * GSD); +StateTransition tGetTransition(OBCState_t fromState); +StateTransitionResult tFromIdle(OBCState_t * currentState, OBCState_t requestedState); +StateTransitionResult tFromInitialized(OBCState_t * currentState, OBCState_t requestedState); +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); + +/*------------------------------------------------------------ +-- Private variables +------------------------------------------------------------*/ + +#define MODULE_NAME "ObjectControl" +static volatile int iExit = 0; + +/*------------------------------------------------------------ + -- Public functions + ------------------------------------------------------------*/ + +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]; + TestScenarioCommandAction commandActions[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, nextHeartbeatTime, nextAdaptiveSyncMessageTime; + + const struct timeval heartbeatPeriod = { 1 / HEAB_FREQUENCY_HZ, + (1000000 / HEAB_FREQUENCY_HZ) % 1000000 + }; + const struct timeval adaptiveSyncMessagePeriod = heartbeatPeriod; + + U8 iForceObjectToLocalhostU8 = DEFAULT_FORCE_OBJECT_TO_LOCALHOST; + + FILE *fd; + C8 Timestamp[SMALL_BUFFER_SIZE_0]; + C8 GPSWeek[SMALL_BUFFER_SIZE_0]; + ssize_t MessageLength; + C8 *MiscPtr; + C8 MiscText[SMALL_BUFFER_SIZE_0]; + U32 StartTimeU32 = 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 = 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; + ACCMData mqACCMData; + EXACData mqEXACData; + TRCMData mqTRCMData; + 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 = 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; + char confDirectoryPath[MAX_FILE_PATH]; + + ControlCenterStatusType objectControlServerStatus = CONTROL_CENTER_STATUS_INIT; + + 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; + + 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; + nextHeartbeatTime = currentTime; + nextAdaptiveSyncMessageTime = currentTime; + + while (!iExit) { + + if (vGetState(GSD) == OBC_STATE_ERROR) { + 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); + } + } + + // 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, >)) { + + 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) { + 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 || vGetState(GSD) == OBC_STATE_REMOTECTRL) { + char buffer[RECV_MESSAGE_BUFFER]; + size_t receivedMONRData = 0; + + 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); + + 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 = + 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 ) + { + 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 && 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); + + 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]); + // TODO smarter way of handling? + continue; + } + + if (ObjectcontrolExecutionMode == OBJECT_CONTROL_CONTROL_MODE) { + // Place struct in buffer + memcpy(&buffer, &monitorData, sizeof (monitorData)); + // Send MONR message as bytes + 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); + objectControlServerStatus = CONTROL_CENTER_STATUS_ABORT; + } + } + + + //Store MONR in GSD + 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 + sprintf(buffer + strlen(buffer), "%" PRIu32 ";", ASPData.MTSPU32); + } + + //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 = 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, + monitorData.data.position.xCoord_m, + monitorData.data.position.yCoord_m, &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 (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); + 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 = + (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"); + } + } + + + 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 = + 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); + } + + objectControlServerStatus = CONTROL_CENTER_STATUS_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 = + 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], + 0); + } + + objectControlServerStatus = CONTROL_CENTER_STATUS_READY; + } + 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)); + timeradd(&startTime, &startDelay, &startTime); + MessageLength = encodeSTRTMessage(&startTime, MessageBuffer, sizeof (MessageBuffer), 0); + + ASPData.MTSPU32 = 0; + ASPData.TimeToSyncPointDbl = 0; + SearchStartIndex = -1; + ASPData.PrevTimeToSyncPointDbl = 0; + OldTimeU32 = CurrentTimeU32; + 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); + } + 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 = 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, + 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 = CONTROL_CENTER_STATUS_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_REMOTECTRL_ENABLE) { + vSetState(OBC_STATE_REMOTECTRL, GSD); + // TODO: objectControlServerStatus = something + MessageLength = + 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]); + UtilSendTCPData(MODULE_NAME, MessageBuffer, MessageLength, &socket_fds[iIndex], 0); + } + // TODO: check objects' states + LogMessage(LOG_LEVEL_INFO, "Enabled remote control mode"); + } + else if (iCommand == COMM_REMOTECTRL_DISABLE) { + // TODO set objects' states to connected + 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]); + UtilSendTCPData(MODULE_NAME, MessageBuffer, MessageLength, &socket_fds[iIndex], 0); + } + // TODO: check objects' states + // TODO: objectControlServerStatus = something + vSetState(OBC_STATE_CONNECTED, GSD); + LogMessage(LOG_LEVEL_INFO, "Disabled remote control mode"); + } + 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 + if (vGetState(GSD) == OBC_STATE_REMOTECTRL) { + switch (rcCommand.manoeuvre) { + 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); + if (MessageLength > 0) { + UtilSendTCPData(MODULE_NAME, MessageBuffer, MessageLength, + &socket_fds[iIndex], 0); + } + else { + LogMessage(LOG_LEVEL_ERROR, "Error encoding RCMM message"); + } + } + 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))); + } + break; + default: + LogMessage(LOG_LEVEL_ERROR, "Unsupported remote control manoeuvre"); + } + } + else { + LogMessage(LOG_LEVEL_WARNING, + "Remote control manoeuvring is not allowed outside of remote control mode"); + } + } + 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); + + resetCommandActionList(commandActions, + sizeof (commandActions) / sizeof (commandActions[0])); + + 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); + 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 if (iCommand == COMM_TRCM && vGetState(GSD) == OBC_STATE_CONNECTED) { + 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); + int commandIndex; + + if ((commandIndex = + findCommandAction(mqEXACData.actionID, commandActions, + sizeof (commandActions) / sizeof (commandActions[0]))) != -1) { + 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); + 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); + } + 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); + } + 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 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]); + float altitude = (float)OriginPosition.Altitude; + + MessageLength = + encodeOSEMMessage(&OriginPosition.Latitude, &OriginPosition.Longitude, + &altitude, NULL, NULL, NULL, MessageBuffer, + sizeof (MessageBuffer), 0); + if (MessageLength < 0) { + util_error("OSEM encoding error"); + } + + 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: + 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 (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."); + + //Restore the buffers + DataDictionaryGetOriginLatitudeC8(GSD, OriginLatitude, SMALL_BUFFER_SIZE_0); + 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"); + vSetState(OBC_STATE_ERROR, GSD); + objectControlServerStatus = CONTROL_CENTER_STATUS_ABORT; + } + LogPrint("OSEM msglen: %ld", MessageLength); + 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) { + 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; + } + } + + /* 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++) { + 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); + 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); + UtilSendTCPData(MODULE_NAME, 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 = CONTROL_CENTER_STATUS_READY; + + if (DisconnectU8 == 0) { + nextHeartbeatTime = currentTime; + nextAdaptiveSyncMessageTime = currentTime; + 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; + + // 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 = CONTROL_CENTER_STATUS_ABORT; + } + } + + (void)nanosleep(&sleep_time, &ref_time); + } + } + + LogMessage(LOG_LEVEL_INFO, "Object control 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"); + } +} + + +/*! + * \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; + 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); + + if (retval < 0) { + LogMessage(LOG_LEVEL_ERROR, "Error getting socket options with fcntl"); + return -1; + } + 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; + } + + // 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; + } + + 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 > 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"); + fclose(fd); + return -1; + } + + 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; + } + + 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.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.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; + + // Print to buffer + if ((printedBytes = encodeTRAJMessagePoint(&relTime, position, speed, acceleration, + (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); + + 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 { + totalPrintedBytes += printedBytes; + messageBufferPosition += printedBytes; + remainingBufferSpace -= (size_t) printedBytes; + } + } + + // 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; + } + + UtilSendTCPData(MODULE_NAME, messageBuffer, messageBufferPosition - messageBuffer, Socket, debug); + + 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 totalPrintedBytes; +} + + +/*! + * \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; +} + + +/*! + * \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; +} + +/*! + * \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) + 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; + + char buffer[256]; + int iResult; + + *sockfd = socket(AF_INET, SOCK_STREAM, 0); + + if (*sockfd < 0) { + util_error("[ObjectControl] ERR: Failed to open control socket"); + } + + 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; + + 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); + + // 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); + + 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; +} + +static void vDisconnectObject(int *sockfd) { + close(*sockfd); +} + + +static void vCreateSafetyChannel(const char *name, const uint32_t port, int *sockfd, struct sockaddr_in *addr) { + int result; + struct hostent *object; + + /* 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"); + } + + /* Set address to object */ + object = gethostbyname(name); + + 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); + + /* 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); +} + +static void vCloseSafetyChannel(int *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; +} + + +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; +} + +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 = DEFAULT_FORCE_OBJECT_TO_LOCALHOST; + 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); +} + +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 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_REMOTECTRL: + return &tFromRemoteControl; + 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; +} + +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; +} + +StateTransitionResult tFromConnected(OBCState_t * currentState, OBCState_t requestedState) { + if (requestedState == OBC_STATE_ARMED || requestedState == OBC_STATE_IDLE + || requestedState == OBC_STATE_REMOTECTRL) { + *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; +} + +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; +} + +StateTransitionResult tFromRemoteControl(OBCState_t * currentState, OBCState_t requestedState) { + 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; +} + +StateTransitionResult tFromUndefined(OBCState_t * currentState, OBCState_t requestedState) { + 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); +} 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 97% rename from server/src/supervisorcontrol.c rename to core/src/supervisorcontrol.c index 69240ee6c..f061d63c2 100644 --- a/server/src/supervisorcontrol.c +++ b/core/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,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) { - UtilISOBuildINSUPMessage(TxBuffer, &INSUPData, SUP_MODE_NORMAL, 0); - UtilSendTCPData("SupervisorControl", TxBuffer, - INSUPData.Header.MessageLengthU32 + ISO_MESSAGE_HEADER_LENGTH + - ISO_MESSAGE_FOOTER_LENGTH, &SupervisorTCPSocketfdI32, 0); + MessageLength = + encodeINSUPMessage(SUPERVISOR_COMMAND_NORMAL, TxBuffer, sizeof (TxBuffer), 0); + UtilSendTCPData("SupervisorControl", TxBuffer, MessageLength, &SupervisorTCPSocketfdI32, + 0); SupervisorInitiatedU8 = 1; } diff --git a/server/src/systemcontrol.c b/core/src/systemcontrol.c similarity index 77% rename from server/src/systemcontrol.c rename to core/src/systemcontrol.c index 68adcba69..4bf8c6bd8 100644 --- a/server/src/systemcontrol.c +++ b/core/src/systemcontrol.c @@ -14,6 +14,7 @@ #include #include #include +#include #include #include #include @@ -33,6 +34,7 @@ #include #include "systemcontrol.h" +#include "maestroTime.h" #include "timecontrol.h" #include "datadictionary.h" @@ -50,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 @@ -108,45 +116,65 @@ typedef enum { #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 #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 +#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, - 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 + 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, + 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; -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", - "GetDirectoryContent_1", - "DeleteFileDirectory_1", "CreateDirectory_1", "GetTestOrigin_0", "replay_1", "control_0", "Exit_0", - "start_ext_trigg_1" + "GetServerParameter_1", "DownloadFile_1", "UploadFile_4", "CheckFileDirectoryExist_1", + "GetRootDirectoryContent_0", "GetDirectoryContent_1", "DeleteTrajectory_1", "DeleteGeofence_1", + "DeleteFileDirectory_1", + "ClearTrajectories_0", "ClearGeofences_0", "RemoteControl_1", "RemoteControlManoeuvre_2", + "CreateDirectory_1", "GetTestOrigin_0", "replay_1", + "control_0", + "Exit_0", "start_ext_trigg_1" }; 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" }; +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]; 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. ------------------------------------------------------------*/ @@ -160,6 +188,8 @@ 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(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, @@ -172,11 +202,18 @@ 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, + 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); +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); -I32 SystemControlBuildFileContentInfo(C8 * Path, C8 * ReturnValue, U8 Debug); +I32 SystemControlBuildFileContentInfo(C8 * Path, U8 Debug); +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); @@ -187,6 +224,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); @@ -215,12 +253,16 @@ 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; struct timeval tvTime; + const struct timeval VirtualMachineLagCompensation = { VIRTUAL_MACHINE_LAG_COMPENSATION_S, + VIRTUAL_MACHINE_LAG_COMPENSATION_US + }; + ObjectPosition OP; int i, i1; char *StartPtr, *StopPtr, *CmdPtr, *OpeningQuotationMarkPtr, *ClosingQuotationMarkPtr, *StringPos; @@ -271,9 +313,12 @@ 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]; + + U32 IpU32; LogInit(MODULE_NAME, logLevel); LogMessage(LOG_LEVEL_INFO, "System control task running with PID: %i", getpid()); @@ -288,8 +333,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; @@ -312,6 +357,7 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { } + I32 FileLengthI32 = 0; while (!iExit) { if (server_state == SERVER_STATE_ERROR) { @@ -354,7 +400,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) { @@ -464,7 +510,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) @@ -564,9 +612,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); @@ -617,7 +664,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:", @@ -671,20 +718,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; @@ -699,6 +732,8 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { SystemControlCommand = Idle_0; } break; + case GetRootDirectoryContent_0: + LogMessage(LOG_LEVEL_INFO, "GetRootDirectory called: defaulting to GetDirectoryContent"); case GetDirectoryContent_1: if (CurrentInputArgCount == CommandArgCount) { SystemControlCommand = Idle_0; @@ -709,16 +744,100 @@ 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); + 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, - ControlResponseBuffer, REMOVE_FILE, 0); + SystemControlDirectoryInfo.info_buffer, KEEP_FILE, 0); + SystemControlDestroyFileContentInfo("dir.info", 1); } } 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; + 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); + } + 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) { + 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); + } + else { + LogMessage(LOG_LEVEL_ERROR, + "Wrong parameter count in DeleteGeofence(name)! got:%d, expected:%d", + CurrentInputArgCount, CommandArgCount); + 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; + memset(ControlResponseBuffer, 0, sizeof (ControlResponseBuffer)); + *ControlResponseBuffer = SystemControlClearTrajectories(); + 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) { + SystemControlCommand = Idle_0; + memset(ControlResponseBuffer, 0, sizeof (ControlResponseBuffer)); + *ControlResponseBuffer = SystemControlClearGeofences(); + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "ClearGeofences:", + ControlResponseBuffer, 1, &ClientSocket, 0); + } + else { + LogMessage(LOG_LEVEL_ERROR, + "Wrong parameter count in ClearGeofences()! got:%d, expected:%d", + CurrentInputArgCount, CommandArgCount); SystemControlCommand = Idle_0; } break; @@ -730,14 +849,14 @@ 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], ControlResponseBuffer, 0); - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "SubDownloadFile:", - ControlResponseBuffer, 4, &ClientSocket, 0); + FileLengthI32 = SystemControlBuildFileContentInfo(SystemControlArgument[0], 0); + SystemControlFileDownloadResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "SubDownloadFile:", + FileLengthI32, &ClientSocket, 0); SystemControlSendFileContent(&ClientSocket, SystemControlArgument[0], - STR_SYSTEM_CONTROL_TX_PACKET_SIZE, ControlResponseBuffer, - KEEP_FILE, 0); + STR_SYSTEM_CONTROL_TX_PACKET_SIZE, + SystemControlDirectoryInfo.info_buffer, KEEP_FILE, 0); + SystemControlDestroyFileContentInfo(SystemControlArgument[0], 0); } } @@ -746,43 +865,45 @@ 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); + bzero(RxFilePath, MAX_FILE_PATH); SystemControlUploadFile(SystemControlArgument[0], SystemControlArgument[1], - SystemControlArgument[2], ControlResponseBuffer, 0); + SystemControlArgument[2], SystemControlArgument[3], + 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); } - SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "UploadFile:", + SystemControlSendControlResponse(SYSTEM_CONTROL_RESPONSE_CODE_OK, "SubUploadFile:", ControlResponseBuffer, 1, &ClientSocket, 0); } 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; @@ -796,6 +917,7 @@ 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); + SystemControlSendLog("[SystemControl] Sending INIT.\n", &ClientSocket, 0); } else if (server_state == SERVER_STATE_INWORK && objectControlState == OBC_STATE_INITIALIZED) { @@ -886,59 +1008,95 @@ void systemcontrol_task(TimeType * GPSTime, GSDType * GSD, LOG_LEVEL logLevel) { 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; + 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 (!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; + } } - 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 { + responseCode = SYSTEM_CONTROL_RESPONSE_CODE_INCORRECT_STATE; + } + } - 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 { + LogMessage(LOG_LEVEL_WARNING, "Remote control command parameter count error"); + responseCode = SYSTEM_CONTROL_RESPONSE_CODE_ERROR; } - 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; + SystemControlCommand = Idle_0; + SystemControlSendControlResponse(responseCode, "RemoteControl:", + ControlResponseBuffer, 0, &ClientSocket, 0); + break; + case RemoteControlManoeuvre_2: + if (CurrentInputArgCount == CommandArgCount) { + 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; } + SystemControlCommand = Idle_0; + 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! { 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; + TimeSetToCurrentSystemTime(&tvTime); - LogMessage(LOG_LEVEL_INFO, "Current timestamp (gtd): %lu", uiTime); + if (TIME_COMPENSATE_LAGING_VM) + timersub(&tvTime, &VirtualMachineLagCompensation, &tvTime); - //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 ); + LogMessage(LOG_LEVEL_INFO, "Current timestamp (epoch): %lu", TimeGetAsUTCms(&tvTime)); - //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]); + 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"); @@ -996,8 +1154,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); } } } @@ -1005,7 +1164,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); } @@ -1109,10 +1268,13 @@ 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); } + + (void)iCommClose(); LogMessage(LOG_LEVEL_INFO, "Exiting"); @@ -1166,6 +1328,61 @@ 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; + const char endOfMessagePattern[] = ";\r\n\r\n"; + 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); + } + else { + messageLength = 0; + readResult = -1; + errno = EAGAIN; + LogMessage(LOG_LEVEL_WARNING, "Part of message received"); + } + + 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); + readResult = (ssize_t) messageLength; + } + bytesInBuffer -= messageLength; + memmove(recvBuffer, recvBuffer + messageLength, bytesInBuffer); + } + } + + return readResult; +} + + void SystemControlSendMONR(C8 * MONRStr, I32 * Sockfd, U8 Debug) { int i, n, j, t; C8 Length[4]; @@ -1273,15 +1490,56 @@ 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[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 = MSCP_RESPONSE_STATUS_CODE_BYTES + 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 + 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 < 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 + MSCP_RESPONSE_DATALENGTH_BYTES; i++) + printf("%x-", Data[i]); + printf("\n"); + } + + //SystemControlSendBytes(Data, n + 4, 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); +} + + 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); @@ -1289,10 +1547,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++) @@ -1304,7 +1562,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"); } @@ -1350,6 +1608,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"); @@ -1397,6 +1660,8 @@ static I32 SystemControlInitServer(int *ClientSocket, int *ServerHandle, struct *ClientSocket = accept(*ServerHandle, (struct sockaddr *)&cli_addr, &cli_length); 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), @@ -1856,25 +2121,46 @@ I32 SystemControlReadServerParameterList(C8 * ParameterList, U8 Debug) { return strlen(ParameterList); } -I32 SystemControlBuildFileContentInfo(C8 * Path, C8 * ReturnValue, U8 Debug) { +I32 SystemControlBuildFileContentInfo(C8 * Path, U8 Debug) { + 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 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; +} - if (Debug) - LogMessage(LOG_LEVEL_DEBUG, "Filesize %d of %s", (I32) st.st_size, CompletePath); +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); + + munmap(SystemControlDirectoryInfo.info_buffer, SystemControlDirectoryInfo.size); + close(SystemControlDirectoryInfo.fd); + SystemControlDirectoryInfo.exist = 0; + if (RemoveFile == 1) { + remove(CompletePath); + } return 0; } @@ -1911,6 +2197,57 @@ 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 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. + */ +C8 SystemControlClearTrajectories(void) { + if (UtilDeleteTrajectoryFiles() != 0) { + return FAILED_DELETE; + } + return SUCCEEDED_DELETE; +} + +/*! + * \brief SystemControlClearGeofences Clears the geofence directory on the machine + * \return Returns ::SUCCEDED_DELETE upon successfully deleting a file, otherwise ::FAILED_DELETE. + */ +C8 SystemControlClearGeofences(void) { + if (UtilDeleteGeofenceFiles() != 0) { + return FAILED_DELETE; + } + return SUCCEEDED_DELETE; +} I32 SystemControlDeleteFileDirectory(C8 * Path, C8 * ReturnValue, U8 Debug) { @@ -1933,7 +2270,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; @@ -1943,14 +2280,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); @@ -2003,27 +2340,61 @@ 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, + C8 * CompleteFilePath, U8 Debug) { FILE *fd; C8 CompletePath[MAX_FILE_PATH]; - bzero(CompletePath, MAX_FILE_PATH); - UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); - strcat(CompletePath, 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; + } + + 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"); + //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); + strcpy(CompleteFilePath, 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); + 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 - { + if (atoi(PacketSize) > SYSTEM_CONTROL_RX_PACKET_SIZE) { //Check packet size *ReturnValue = SERVER_PREPARED_BIG_PACKET_SIZE; return 0; } @@ -2032,6 +2403,7 @@ I32 SystemControlUploadFile(C8 * Path, C8 * FileSize, C8 * PacketSize, C8 * Retu if (fd != NULL) { fclose(fd); remove(CompletePath); //Remove file if exist + LogMessage(LOG_LEVEL_INFO, "Deleted existing file <%s>", CompletePath); } fd = fopen(CompletePath, "w+"); //Create the file @@ -2041,10 +2413,9 @@ I32 SystemControlUploadFile(C8 * Path, C8 * FileSize, C8 * PacketSize, C8 * Retu return 0; } else { - //ok, path invalid create temporary file - bzero(CompletePath, MAX_FILE_PATH); + //Failed to open path create temporary file UtilGetTestDirectoryPath(CompletePath, sizeof (CompletePath)); - strcat(CompletePath, "file.tmp"); + strcat(CompletePath, "/file.tmp"); fd = fopen(CompletePath, "r"); if (fd != NULL) { fclose(fd); @@ -2057,9 +2428,10 @@ I32 SystemControlUploadFile(C8 * Path, C8 * FileSize, C8 * PacketSize, C8 * Retu return 0; } - return 0; + return -1; } + I32 SystemControlReceiveRxData(I32 * sockfd, C8 * Path, C8 * FileSize, C8 * PacketSize, C8 * ReturnValue, U8 Debug) { @@ -2067,7 +2439,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); @@ -2079,11 +2451,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); } @@ -2106,7 +2478,6 @@ I32 SystemControlReceiveRxData(I32 * sockfd, C8 * Path, C8 * FileSize, C8 * Pack bzero(RxBuffer, PacketSizeU16); ClientStatus = recv(*sockfd, RxBuffer, PacketSizeU16, MSG_WAITALL); - if (ClientStatus > 0) { i++; fwrite(RxBuffer, 1, ClientStatus, fd); @@ -2143,7 +2514,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); } @@ -2212,7 +2583,7 @@ I32 SystemControlSendFileContent(I32 * sockfd, C8 * Path, C8 * PacketSize, C8 * /* 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 @@ -2260,7 +2631,7 @@ I32 SystemControlBuildRVSSTimeChannelMessage(C8 * RVSSData, U32 * RVSSDataLength /* 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 @@ -2295,10 +2666,10 @@ 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. +See the architecture document for the protocol of RVSS. - *RVSSData the buffer the message - *RVSSDataLengthU32 the length of the message @@ -2312,7 +2683,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); @@ -2335,7 +2711,7 @@ I32 SystemControlBuildRVSSMONRChannelMessage(C8 * RVSSData, U32 * RVSSDataLength /* 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 diff --git a/server/src/timecontrol.c b/core/src/timecontrol.c similarity index 90% rename from server/src/timecontrol.c rename to core/src/timecontrol.c index ebdb0abe5..8135399f0 100644 --- a/server/src/timecontrol.c +++ b/core/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. ------------------------------------------------------------*/ @@ -80,9 +86,12 @@ 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 }; + 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); @@ -182,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); @@ -236,17 +274,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"); @@ -485,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]; diff --git a/server/src/util.c b/core/src/util.c similarity index 79% rename from server/src/util.c rename to core/src/util.c index ed2634a18..e167d3316 100644 --- a/server/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 @@ -75,12 +78,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_REMOTECTRL_MANOEUVRE 12 // Logging #define PRIO_COMM_LOG 10 // Unused messages TODO: double check the priority of unused messages @@ -95,27 +101,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 @@ -134,12 +119,8 @@ 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 UtilParseTrajectoryFileHeader(char *headerLine, TrajectoryFileHeader * header); -static int UtilParseTrajectoryFileFooter(char *footerLine); -static int UtilParseTrajectoryFileLine(char *fileLine, TrajectoryFileLine * line); - +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) { @@ -203,6 +184,75 @@ 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; + } + fclose(fd); + + if (remove(path) != 0) { + LogMessage(LOG_LEVEL_ERROR, "Path <%s> could not be deleted", 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. + * \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 (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 + DIR *theFolder = opendir(path); + + if (theFolder == NULL) { + LogMessage(LOG_LEVEL_ERROR, "Path: %s could not be opened", path); + errno = ENOENT; + return -1; + } + 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 @@ -338,6 +388,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 +405,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); } @@ -363,17 +415,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; } @@ -387,8 +439,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; @@ -687,27 +739,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.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 - LogMessage(LOG_LEVEL_DEBUG, "MONR heading unavailable, assuming 0"); - retval.heading_deg = 0.0; - } - else { - retval.heading_deg = MONR.MONR.HeadingU16 / 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 @@ -715,19 +746,18 @@ 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(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(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); + strcat(monitorDataString, ";0;"); + + if (objectMonitorDataToASCII + (&monitorData.data, monitorDataString + strlen(monitorDataString), + stringLength - strlen(monitorDataString)) < 0) { + memset(monitorDataString, 0, stringLength); + return -1; + } return 0; } @@ -738,61 +768,27 @@ int UtilMonitorDataToString(MonitorDataType monrData, char *monrString, size_t s * \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[] = ";"; - const int NumberBaseDecimal = 10; struct in_addr addr; + char *copy = strdup(monitorString); - token = strtok(monrString, delim); + 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); - 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 (ASCIIToObjectMonitorData(token, &monitorData->data) == 0) + return 0; + else + return -1; } /*! @@ -806,6 +802,8 @@ int UtilStringToMonitorData(const char *monrString, size_t stringLength, Monitor 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)); @@ -813,16 +811,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_rad, tarHeading = target.heading_rad; - const double oneRotation = 360.0; - double posHeading = position.heading_deg, tarHeading = target.heading_deg; + if (!position.isHeadingValid || !target.isHeadingValid) + return 0; while (posHeading < 0) { posHeading += oneRotation; @@ -831,7 +832,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) { @@ -851,7 +852,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; @@ -926,7 +927,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; @@ -940,14 +941,16 @@ 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 { @@ -965,7 +968,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), @@ -1935,6 +1938,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) { @@ -2062,6 +2066,15 @@ int iCommSend(const enum COMMAND iCommand, const char *cpData, size_t dataLength case COMM_OBJECTS_CONNECTED: uiMessagePrio = PRIO_OBJECTS_CONNECTED; break; + 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; break; @@ -2421,6 +2434,131 @@ void UtilGetGeofenceDirectoryPath(char *path, size_t pathLen) { strcat(path, "/"); } +/*! + * \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 (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 + * \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 (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 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, "Generic path name too long"); + return -1; + } + + if (filePath[0] == '\0') + return -1; + + strcat(filePath, pathRelativeToWorkspace); + 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. + */ +int UtilDeleteTrajectoryFiles(void) { + char filePath[MAX_FILE_PATH] = { '\0' }; + UtilGetTrajDirectoryPath(filePath, sizeof (filePath)); + if (filePath[0] == '\0') + return -1; + 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)); +} + /*! * \brief UtilParseTrajectoryFileHeader Attempts to parse a line into a trajectory header * \param line Line to be parsed @@ -2522,6 +2660,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 @@ -2718,7 +2857,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; @@ -3248,106 +3387,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 = 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 -} - -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 = 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; U16 MilliU16 = 0, NowU16 = 0; @@ -3364,332 +3403,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 = 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 -} - -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; - - U16Data = (U16Data | *(MessageBuffer + 1)) << 8; - U16Data = U16Data | *(MessageBuffer + 0); - - HeaderData->SyncWordU16 = U16Data; - HeaderData->TransmitterIdU8 = *(MessageBuffer + 2); - HeaderData->MessageCounterU8 = *(MessageBuffer + 3); - HeaderData->AckReqProtVerU8 = *(MessageBuffer + 4); - - U16Data = 0; - U16Data = (U16Data | *(MessageBuffer + 6)) << 8; - U16Data = U16Data | *(MessageBuffer + 5); - HeaderData->MessageIdU16 = U16Data; - - U32Data = (U32Data | *(MessageBuffer + 10)) << 8; - U32Data = (U32Data | *(MessageBuffer + 9)) << 8; - U32Data = (U32Data | *(MessageBuffer + 8)) << 8; - U32Data = U32Data | *(MessageBuffer + 7); - HeaderData->MessageLengthU32 = U32Data; - - 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 0; -} - - /* UtilWriteConfigurationParameter updates parameters in the file test.conf. @@ -3783,150 +3496,23 @@ 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.CRC) - sizeof (monitorData->MONR.MonrStructValueIdU16) - - sizeof (monitorData->MONR.MonrStructContentLengthU16); - - if (rawMONRsize < sizeof (MONRType)) { - LogMessage(LOG_LEVEL_ERROR, "Raw MONR array too small to hold all necessary MONR data, %d < %d.", - rawMONRsize, sizeof (MONRType)); +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; } - // 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; - } - - } - else if (debug) { - LogPrint("Received MONR message without content header: corrupt data may result"); - } - - U16Data = 0; - - memcpy(&U32Data, rdPtr, sizeof (U32Data)); - monitorData->MONR.GPSQmsOfWeekU32 = U32Data; - rdPtr += sizeof (U32Data); - U32Data = 0; - - memcpy(&I32Data, rdPtr, sizeof (I32Data)); - monitorData->MONR.XPositionI32 = I32Data; - rdPtr += sizeof (I32Data); - I32Data = 0; - - memcpy(&I32Data, rdPtr, sizeof (I32Data)); - monitorData->MONR.YPositionI32 = I32Data; - rdPtr += sizeof (I32Data); - I32Data = 0; - - memcpy(&I32Data, rdPtr, sizeof (I32Data)); - monitorData->MONR.ZPositionI32 = I32Data; - rdPtr += sizeof (I32Data); - I32Data = 0; - - memcpy(&U16Data, rdPtr, sizeof (U16Data)); - monitorData->MONR.HeadingU16 = U16Data; - rdPtr += sizeof (U16Data); - U16Data = 0; - - memcpy(&I16Data, rdPtr, sizeof (I16Data)); - monitorData->MONR.LongitudinalSpeedI16 = I16Data; - rdPtr += sizeof (I16Data); - I16Data = 0; - - memcpy(&I16Data, rdPtr, sizeof (I16Data)); - monitorData->MONR.LateralSpeedI16 = I16Data; - rdPtr += sizeof (I16Data); - I16Data = 0; - - memcpy(&I16Data, rdPtr, sizeof (I16Data)); - monitorData->MONR.LongitudinalAccI16 = I16Data; - rdPtr += sizeof (I16Data); - I16Data = 0; - - memcpy(&I16Data, rdPtr, sizeof (I16Data)); - monitorData->MONR.LateralAccI16 = I16Data; - rdPtr += sizeof (I16Data); - I16Data = 0; - - monitorData->MONR.DriveDirectionU8 = *(rdPtr++); - monitorData->MONR.StateU8 = *(rdPtr++); - monitorData->MONR.ReadyToArmU8 = *(rdPtr++); - monitorData->MONR.ErrorStatusU8 = *(rdPtr++); - - memcpy(&U16Data, rdPtr, sizeof (U16Data)); - monitorData->MONR.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.GPSQmsOfWeekU32); - } + memcpy(monitorData, rawData, rawDataSize); return 0; } 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/modules/ObjectMonitoring/CMakeLists.txt b/modules/ObjectMonitoring/CMakeLists.txt new file mode 100644 index 000000000..30f2b3e8c --- /dev/null +++ b/modules/ObjectMonitoring/CMakeLists.txt @@ -0,0 +1,62 @@ +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 LANGUAGES C CXX) + +# Define target names +set(OBJECT_MONITORING_TARGET ${PROJECT_NAME}) + +set(COREUTILS_LIBRARY MaestroCoreUtil) +set(ISO_22133_LIBRARY ISO22133) +set(TIME_LIBRARY MaestroTime) +set(LOGGING_LIBRARY MaestroLogging) +set(MESSAGE_BUS_LIBRARY MaestroMQ) + +include(GNUInstallDirs) + +find_package(Threads REQUIRED) +find_library(paho-mqtt3a NAMES libpaho-mqtt3a.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}) +set_property(TARGET pahomqtt3c PROPERTY IMPORTED_LOCATION ${paho-mqtt3c}) + +# 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 + ${CMAKE_CURRENT_SOURCE_DIR}/src/mqttconnectionhandler.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/mqtttopichandlers.cpp +) + +# Link project executable to util libraries +target_link_libraries(${OBJECT_MONITORING_TARGET} LINK_PUBLIC + ${TIME_LIBRARY} + ${LOGGING_LIBRARY} + ${COREUTILS_LIBRARY} + ${CMAKE_THREAD_LIBS_INIT} + pahomqtt3a + pahomqtt3c +) + +target_include_directories(${OBJECT_MONITORING_TARGET} PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/inc +) + +# 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} +) + +# Ensure linkage is reloaded after install +install(CODE "execute_process(COMMAND ldconfig)") diff --git a/modules/ObjectMonitoring/README.md b/modules/ObjectMonitoring/README.md new file mode 100644 index 000000000..e8e7fafc7 --- /dev/null +++ b/modules/ObjectMonitoring/README.md @@ -0,0 +1,18 @@ +## Object monitoring module +TODO + +### 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: ```./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 diff --git a/modules/ObjectMonitoring/inc/ISO22133ProtocolData.h b/modules/ObjectMonitoring/inc/ISO22133ProtocolData.h new file mode 100644 index 000000000..d55ea9a50 --- /dev/null +++ b/modules/ObjectMonitoring/inc/ISO22133ProtocolData.h @@ -0,0 +1,15 @@ +#ifndef ISO22133PROTOCOLDATA_H +#define ISO22133PROTOCOLDATA_H + +#include "protocoldata.h" + +class ISO22133ProtocolData : public ProtocolData { +public: + static constexpr uint16_t TCP_PORT = 12345; + + ISO22133ProtocolData() {} + ~ISO22133ProtocolData() override; + ReturnCode decodeFrom(const std::vector& rawData) override; +}; + +#endif // PROTOCOLDATA_H diff --git a/modules/ObjectMonitoring/inc/connectionhandler.h b/modules/ObjectMonitoring/inc/connectionhandler.h new file mode 100644 index 000000000..856dca1e0 --- /dev/null +++ b/modules/ObjectMonitoring/inc/connectionhandler.h @@ -0,0 +1,30 @@ +#ifndef CONNECTIONHANDLER_H +#define CONNECTIONHANDLER_H + +#include +#include "protocoldata.h" + +class RawConnectionHandler +{ +public: + RawConnectionHandler(int openSocketDescriptor, ProtocolData& data); + RawConnectionHandler(int openSocketDescriptor, ProtocolData& data, unsigned long readBufferSize); + ~RawConnectionHandler(); + + bool isTerminated() const { return terminated; } +private: + int socketDescriptor = 0; + ProtocolData &data; + unsigned long readBufferSize; + pthread_t readThread; + bool terminated = false; + constexpr static unsigned long DefaultReadBufferSize = 4096; + + 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/mqttconnectionhandler.hpp b/modules/ObjectMonitoring/inc/mqttconnectionhandler.hpp new file mode 100644 index 000000000..bf76a0168 --- /dev/null +++ b/modules/ObjectMonitoring/inc/mqttconnectionhandler.hpp @@ -0,0 +1,68 @@ +#ifndef MQTTCONNECTIONHANDLER_H +#define MQTTCONNECTIONHANDLER_H + +#include +#include +#include + +#include +#include "logging.h" +#include "iso22133.h" +#include "mqtttopichandlers.hpp" + +using namespace std; + +class MQTTError : virtual public std::exception { +protected: + int errorCode; + string errorMessage; +public: + explicit MQTTError(const string &msg, int errorCode) : errorCode(errorCode), errorMessage(msg) {} + + virtual ~MQTTError() noexcept {} + + virtual const char* what() const noexcept { + return errorMessage.c_str(); + } + + virtual int getErrorNumber() const noexcept { + return errorCode; + } +}; + +class MQTTConnectionHandler { +public: + MQTTConnectionHandler(const string clientID); + void establishConnection(void); + +private: + + 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; + } + + static int arrivalCallback(void* context, char* topicName, int, + MQTTClient_message* message) { + MQTTConnectionHandler* thisObject = static_cast(context); + + string topic(topicName); + int retval = thisObject->messageCallback(message->payload, topic); + MQTTClient_freeMessage(&message); + MQTTClient_free(topicName); + return retval; + } + +}; + +#endif // MQTTCONNECTIONHANDLER_H diff --git a/modules/ObjectMonitoring/inc/mqtttopichandlers.hpp b/modules/ObjectMonitoring/inc/mqtttopichandlers.hpp new file mode 100644 index 000000000..7b744efad --- /dev/null +++ b/modules/ObjectMonitoring/inc/mqtttopichandlers.hpp @@ -0,0 +1,18 @@ +#ifndef MQTTTOPICHANDLERS_H +#define MQTTTOPICHANDLERS_H +#include +#include + +using namespace std; + +namespace MQTTTopicHandlers { + typedef int (*MQTTTopicHandler)(void*, string&); + + int handleMessage(void* message, string &topic); + int monrHandler(void*, string &topic); + int motorRPMHandler(void* message, string &topic); + + vector getSubscriptions(void); +} + +#endif // MQTTTOPICHANDLERS_H diff --git a/modules/ObjectMonitoring/inc/protocoldata.h b/modules/ObjectMonitoring/inc/protocoldata.h new file mode 100644 index 000000000..4d4e6fe02 --- /dev/null +++ b/modules/ObjectMonitoring/inc/protocoldata.h @@ -0,0 +1,32 @@ +#ifndef PROTOCOLDATA_H +#define PROTOCOLDATA_H + +#include +#include +#include +#include +#include +#include + +class ProtocolData { +public: + ProtocolData() = default; + ProtocolData(const ProtocolData&) = default; + ProtocolData(ProtocolData&&) = default; + ProtocolData& operator=(const ProtocolData&) = default; + ProtocolData& operator=(ProtocolData&&) = default; + virtual ~ProtocolData(); + + typedef enum { + DECODE_PARTIAL, + DECODE_SUCCESSFUL, + DECODE_ERROR + } ReturnCode; + + virtual ReturnCode decodeFrom(const std::vector &rawData) = 0; + +private: + +}; + +#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; +} diff --git a/modules/ObjectMonitoring/src/connectionhandler.cpp b/modules/ObjectMonitoring/src/connectionhandler.cpp new file mode 100644 index 000000000..04f2e9f43 --- /dev/null +++ b/modules/ObjectMonitoring/src/connectionhandler.cpp @@ -0,0 +1,74 @@ +#include "connectionhandler.h" +#include "logging.h" + +#include +#include +#include +#include + +RawConnectionHandler::RawConnectionHandler(int openSocketDescriptor, ProtocolData& data) + : RawConnectionHandler::RawConnectionHandler(openSocketDescriptor, data, DefaultReadBufferSize) {} + +RawConnectionHandler::RawConnectionHandler(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; + } + socketDescriptor = openSocketDescriptor; + + if (readBufferSize == 0) { + LogMessage(LOG_LEVEL_ERROR, "Attempted to create connection handler with zero buffer size"); + return; + } + this->readBufferSize = readBufferSize; + + LogMessage(LOG_LEVEL_INFO, "Creating thread to handle connection"); + returnCode = pthread_create(&readThread, nullptr, &RawConnectionHandler::routineWrapper, this); + if (returnCode) { + LogMessage(LOG_LEVEL_ERROR, "Error creating thread"); + close(socketDescriptor); + terminated = true; + throw std::system_error(); + } +} + +RawConnectionHandler::~RawConnectionHandler() { + close(socketDescriptor); +} + + +void* RawConnectionHandler::threadRoutine(void*) { + std::vector readBuffer(readBufferSize); + std::vector messageBuffer; + ssize_t readBytes; + + while ( (readBytes = read(socketDescriptor, readBuffer.data(), readBuffer.size()) ) > 0) { + messageBuffer.insert(messageBuffer.end(), readBuffer.begin(), readBuffer.end()); + 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"); + terminate(static_cast(&readBytes)); + } + else { + LogMessage(LOG_LEVEL_INFO, "Remote closed connection"); + terminate(nullptr); + } +} + +void RawConnectionHandler::terminate(void *retval) { + terminated = true; + pthread_exit(retval); +} diff --git a/modules/ObjectMonitoring/src/main.cpp b/modules/ObjectMonitoring/src/main.cpp new file mode 100644 index 000000000..dd95e9177 --- /dev/null +++ b/modules/ObjectMonitoring/src/main.cpp @@ -0,0 +1,91 @@ +#include +#include +#include +#include +#include + +#include + +#include "logging.h" +#include "connectionhandler.h" +#include "protocoldata.h" +#include "ISO22133ProtocolData.h" +#include "mqttconnectionhandler.hpp" +#include "util.h" + +#define MODULE_NAME "ObjectMonitoring" +#define ISO22133_PORT 53240 + +using namespace std; + +static void handleNewConnection(int socketDescriptor, vector &handlers); +static void pruneTerminatedConnectionHandlers(vector &handlers); +static void listenForNewConnection(void); + +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; + vector handlers; + + 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); + } + + // TODO: start MQ communication handler + + // Initialize MQTT handler + MQTTConnectionHandler mqttConnectionHandler("Maestro"); + mqttConnectionHandler.establishConnection(); + + while(true) { + + listenForNewConnection(); + + handleNewConnection(0, handlers); + + pruneTerminatedConnectionHandlers(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 ISO22133ProtocolData::TCP_PORT: + { + ISO22133ProtocolData protoData; + handlers.push_back(new RawConnectionHandler(socketDescriptor, protoData)); + break; + } + default: + LogMessage(LOG_LEVEL_WARNING, "New connection made but no protocol specified for connected port: closing connection"); + close(socketDescriptor); + 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 RawConnectionHandler* handler) { return handler->isTerminated(); } + ), handlers.end() ); +} + + +void listenForNewConnection(void) { + // TODO +} diff --git a/modules/ObjectMonitoring/src/mqttconnectionhandler.cpp b/modules/ObjectMonitoring/src/mqttconnectionhandler.cpp new file mode 100644 index 000000000..c570707df --- /dev/null +++ b/modules/ObjectMonitoring/src/mqttconnectionhandler.cpp @@ -0,0 +1,50 @@ +#include "mqttconnectionhandler.hpp" + + +#define DEFAULT_SERVER_URI "tcp://127.0.0.1:1883" + +MQTTConnectionHandler::MQTTConnectionHandler(const string clientID) + : clientID(clientID), connectionOptions(MQTTClient_connectOptions_initializer) { + int returnCode; + // TODO: read a config file to get non-default values + this->serverURI = DEFAULT_SERVER_URI; + 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) { + 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"); + + for (const string &subscription : MQTTTopicHandlers::getSubscriptions()) { + 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()); + } + } +} diff --git a/modules/ObjectMonitoring/src/mqtttopichandlers.cpp b/modules/ObjectMonitoring/src/mqtttopichandlers.cpp new file mode 100644 index 000000000..7c5852f93 --- /dev/null +++ b/modules/ObjectMonitoring/src/mqtttopichandlers.cpp @@ -0,0 +1,173 @@ +#include +#include +#include +#include + +#include "mqtttopichandlers.hpp" +#include "logging.h" + + +using namespace std; +using namespace MQTTTopicHandlers; +typedef unordered_map HandlerMap; + + +//! ************ 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); + + +//! ************ Static variables ****************************************** +/*! + * \brief handlerMap maps MQTT topics to their respective handlers + */ +static HandlerMap handlerMap { + {"astazero/+/+/RPM/#", MQTTTopicHandlers::motorRPMHandler}, + {"astazero/+/+/MONR/#", MQTTTopicHandlers::monrHandler} +}; + + +//! ************ Function definitions ************************************** +/*! ************************************************************************ + * Topic handlers + * ************************************************************************ + */ +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 1; +} + +/*! ************************************************************************ + * Other functions + * ************************************************************************ + */ +/*! + * \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 + * 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) { + 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); + } + } + if (!handlerFound) { + LogMessage(LOG_LEVEL_ERROR, "No handler specified for topic %s", topic.c_str()); + return -1; + } + return returnValue; +} + + +/*! + * \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 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; +} + + +/*! + * \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; +} diff --git a/modules/ObjectMonitoring/src/protocoldata.cpp b/modules/ObjectMonitoring/src/protocoldata.cpp new file mode 100644 index 000000000..4f721bc34 --- /dev/null +++ b/modules/ObjectMonitoring/src/protocoldata.cpp @@ -0,0 +1,4 @@ +#include "protocoldata.h" + +ProtocolData::~ProtocolData() { +} diff --git a/modules/ScenarioControl/CMakeLists.txt b/modules/ScenarioControl/CMakeLists.txt index ab7e2aec0..dd295ec4c 100644 --- a/modules/ScenarioControl/CMakeLists.txt +++ b/modules/ScenarioControl/CMakeLists.txt @@ -1,79 +1,57 @@ 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 +project(ScenarioControl LANGUAGES C CXX) +# Define target names +set(SCENARIO_CONTROL_TARGET ${PROJECT_NAME}) -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 -) +set(COREUTILS_LIBRARY MaestroCoreUtil) +set(ISO_22133_LIBRARY ISO22133) +set(TIME_LIBRARY MaestroTime) +set(LOGGING_LIBRARY MaestroLogging) +set(MESSAGE_BUS_LIBRARY MaestroMQ) -add_library(MaestroTime - ../../util/C/time/maestroTime.h - ../../util/C/time/maestroTime.c -) +include(GNUInstallDirs) -add_library(MQBus - ../../util/C/MQBus/mqbus.h - ../../util/C/MQBus/mqbus.c +# Create project main executable target +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/distancetrigger.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 ) -# Create library -add_library(util - ../../server/src/util.c - ../../server/inc/util.h +# Link project executable to util libraries +target_link_libraries(${SCENARIO_CONTROL_TARGET} LINK_PUBLIC + ${TIME_LIBRARY} + ${COREUTILS_LIBRARY} + ${LOGGING_LIBRARY} + ${MESSAGE_BUS_LIBRARY} + ${POSITIONING_LIBRARY} + ${ISO_22133_LIBRARY} ) - -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 +target_include_directories(${SCENARIO_CONTROL_TARGET} PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/inc ) -add_library(Action - inc/action.h - src/action.cpp - inc/externalaction.h - src/externalaction.cpp +# Installation rules +install(CODE "MESSAGE(STATUS \"Installing target ${SCENARIO_CONTROL_TARGET}\")") +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} ) -add_library(Scenario - inc/scenario.h - src/scenario.cpp - inc/causality.h - src/causality.cpp -) - - -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) - - - +# Ensure linkage is reloaded after install +install(CODE "execute_process(COMMAND ldconfig)") diff --git a/modules/ScenarioControl/inc/action.h b/modules/ScenarioControl/inc/action.h index 36c3b0417..798bdcce0 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 << @@ -55,6 +58,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); @@ -63,12 +67,15 @@ 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: 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/distancetrigger.h b/modules/ScenarioControl/inc/distancetrigger.h new file mode 100644 index 000000000..335255772 --- /dev/null +++ b/modules/ScenarioControl/inc/distancetrigger.h @@ -0,0 +1,56 @@ +#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) 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/inc/externalaction.h b/modules/ScenarioControl/inc/externalaction.h index c813bd256..09a2998d4 100644 --- a/modules/ScenarioControl/inc/externalaction.h +++ b/modules/ScenarioControl/inc/externalaction.h @@ -10,13 +10,32 @@ 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 { 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; diff --git a/modules/ScenarioControl/inc/scenario.h b/modules/ScenarioControl/inc/scenario.h index 21b33cce3..7e77536b6 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" @@ -50,6 +49,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/inc/trigger.h b/modules/ScenarioControl/inc/trigger.h index ba33a9e39..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; @@ -87,12 +88,15 @@ 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(TREOData){ 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); protected: diff --git a/modules/ScenarioControl/src/action.cpp b/modules/ScenarioControl/src/action.cpp index fabba618c..802cece08 100644 --- a/modules/ScenarioControl/src/action.cpp +++ b/modules/ScenarioControl/src/action.cpp @@ -18,9 +18,24 @@ 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; } + +/*! + * \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 @@ -37,6 +52,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/booleantrigger.cpp b/modules/ScenarioControl/src/booleantrigger.cpp index 048b2e2d9..dc60c51ae 100644 --- a/modules/ScenarioControl/src/booleantrigger.cpp +++ b/modules/ScenarioControl/src/booleantrigger.cpp @@ -10,7 +10,7 @@ Trigger::TriggerReturnCode_t BooleanTrigger::update(bool currentStateValue, stru { wasStateTrue = isStateTrue; isStateTrue = currentStateValue; - wasTriggeredByLastUpdate = checkIfTriggered(); + wasTriggeredByLastUpdate = checkIfTriggered(); return wasTriggeredByLastUpdate; } diff --git a/modules/ScenarioControl/src/distancetrigger.cpp b/modules/ScenarioControl/src/distancetrigger.cpp new file mode 100644 index 000000000..7845d6b37 --- /dev/null +++ b/modules/ScenarioControl/src/distancetrigger.cpp @@ -0,0 +1,141 @@ +#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) { + 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"); + } + + // 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, &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"); + } + + switch (this->oper) { + case LESS_THAN: + return update(static_cast(UtilIsPositionNearTarget(newValue.data.position, this->referencePoint, this->triggerDistance_m + networkDelayCorrection_m)), + newValue.data.timestamp); + case GREATER_THAN: + return update(static_cast(!UtilIsPositionNearTarget(newValue.data.position, this->referencePoint, this->triggerDistance_m - networkDelayCorrection_m)), + newValue.data.timestamp); + } + 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->mode = HIGH; + 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->mode = HIGH; + 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; +} diff --git a/modules/ScenarioControl/src/externalaction.cpp b/modules/ScenarioControl/src/externalaction.cpp index ddd9c960f..01c586513 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,10 +17,10 @@ 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; + data.executionTime_qmsoW = actionDelayTime_qms == 0 ? TimeGetAsGPSqmsOfWeek(&systemTime) : TimeGetAsGPSqmsOfWeek(&systemTime) + actionDelayTime_qms; data.ip = actionObjectIP; @@ -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) { diff --git a/modules/ScenarioControl/src/main.cpp b/modules/ScenarioControl/src/main.cpp index 821dc8be1..63ef5c1c1 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"); } @@ -116,8 +117,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) { @@ -127,7 +134,7 @@ int main() break; case COMM_MONR: // Update triggers - UtilPopulateMonitorDataStruct(reinterpret_cast(mqRecvData), sizeof(mqRecvData), &monr, 0); + UtilPopulateMonitorDataStruct(mqRecvData, static_cast(recvDataLength), &monr); scenario.updateTrigger(monr); break; case COMM_DISCONNECT: diff --git a/modules/ScenarioControl/src/scenario.cpp b/modules/ScenarioControl/src/scenario.cpp index 8a7e6691a..9a9701981 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) { @@ -59,18 +61,24 @@ 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->getTypeAsString(tp->getTypeCode()) + "\n"; + debugStr += "\t" + tp->getObjectIPAsString() + "\t" + tp->getTypeAsString(tp->getTypeCode()) + "\n"; debugStr += "Actions:\n"; for (Action* ap : allActions) - debugStr += "\t" + 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()); } +void Scenario::reset() { + for (Action* ap : allActions) { + ap->reset(); + } +} + /*! * \brief Scenario::sendConfiguration Sends TRCM and ACCM according to previously initialized scenario */ @@ -125,7 +133,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 +259,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); @@ -258,9 +269,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; @@ -305,6 +327,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); @@ -312,8 +337,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); } @@ -427,17 +462,31 @@ 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()) { - 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); + case Trigger::TriggerTypeCode_t::TRIGGER_BRAKE: + if (monr.data.speed.isLongitudinalValid && 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; + case Trigger::TriggerTypeCode_t::TRIGGER_DISTANCE: + if (monr.data.position.isPositionValid) { + tp->update(monr); + } + 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()); diff --git a/modules/ScenarioControl/src/trigger.cpp b/modules/ScenarioControl/src/trigger.cpp index ad81fa852..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 @@ -42,7 +54,7 @@ Trigger::TriggerReturnCode_t Trigger::appendParameter(Trigger::TriggerParameter_ return retval; parameters.push_back(triggerParameter); - return parseParameters(); + return OK; } /*! @@ -316,7 +328,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) diff --git a/modules/Supervision/CMakeLists.txt b/modules/Supervision/CMakeLists.txt index 17b6afcb5..70950badb 100644 --- a/modules/Supervision/CMakeLists.txt +++ b/modules/Supervision/CMakeLists.txt @@ -1,59 +1,50 @@ 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 LANGUAGES C CXX) -project(Supervision) +# Define target names +set(SUPERVISION_TARGET ${PROJECT_NAME}) +set(COREUTILS_LIBRARY MaestroCoreUtil) +set(ISO_22133_LIBRARY ISO22133) +set(TIME_LIBRARY MaestroTime) +set(LOGGING_LIBRARY MaestroLogging) +set(MESSAGE_BUS_LIBRARY 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) - -# 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 +# 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 ) -add_library(MQBus - ../../util/C/MQBus/mqbus.h - ../../util/C/MQBus/mqbus.c +# Link project executable to util libraries +target_link_libraries(${SUPERVISION_TARGET} LINK_PUBLIC + ${TIME_LIBRARY} + ${COREUTILS_LIBRARY} + ${ISO_22133_LIBRARY} + ${POSITIONING_LIBRARY} + ${LOGGING_LIBRARY} ) - -# Create library -add_library(util - ../../server/src/util.c - ../../server/inc/util.h +target_include_directories(${SUPERVISION_TARGET} PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/inc ) -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 +# Installation rules +install(CODE "MESSAGE(STATUS \"Installing target ${SUPERVISION_TARGET}\")") +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} ) -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) - - - +# Ensure linkage is reloaded after install +install(CODE "execute_process(COMMAND ldconfig)") diff --git a/modules/Supervision/src/main.cpp b/modules/Supervision/src/main.cpp index 98e0183fd..4ceb45af1 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); @@ -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, @@ -436,23 +438,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.XPositionI32 / 1000.0, MONRdata.MONR.YPositionI32 / 1000.0, - MONRdata.MONR.ZPositionI32 / 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 +464,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 +478,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)) { + && 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, &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,15 +521,15 @@ 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, - 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; diff --git a/modules/Supervision/src/trajectory.cpp b/modules/Supervision/src/trajectory.cpp index f0ea311ea..f2d487e46 100644 --- a/modules/Supervision/src/trajectory.cpp +++ b/modules/Supervision/src/trajectory.cpp @@ -129,6 +129,8 @@ 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; } diff --git a/modules/Visualization/CMakeLists.txt b/modules/Visualization/CMakeLists.txt index 332f31024..9e000180b 100644 --- a/modules/Visualization/CMakeLists.txt +++ b/modules/Visualization/CMakeLists.txt @@ -1,47 +1,42 @@ -cmake_minimum_required(VERSION 2.8) +cmake_minimum_required(VERSION 3.1) +set(CMAKE_C_STANDARD 11) +set(CMAKE_C_STANDARD_REQUIRED ON) -project(Visualization) -# This module is an example of how to set up a new module external to the Maestro executable +project(Visualization LANGUAGES C) +# Define target names +set(VISUALIZATION_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) +set(COREUTILS_LIBRARY MaestroCoreUtil) +set(ISO_22133_LIBRARY ISO22133) +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 -) - -add_library(MaestroTime - ../../util/C/time/maestroTime.h - ../../util/C/time/maestroTime.c +# Create project main executable target +add_executable(${VISUALIZATION_TARGET} + ${CMAKE_CURRENT_SOURCE_DIR}/src/main.c ) -add_library(MQBus - ../../util/C/MQBus/mqbus.h - ../../util/C/MQBus/mqbus.c +# Link project executable to util libraries +target_link_libraries(${VISUALIZATION_TARGET} LINK_PUBLIC + ${COREUTILS_LIBRARY} + ${ISO_22133_LIBRARY} + ${POSITIONING_LIBRARY} + ${TIME_LIBRARY} + ${LOGGING_LIBRARY} + ${MESSAGE_BUS_LIBRARY} ) -# Create library -add_library(util - ../../server/src/util.c - ../../server/inc/util.h +# Installation rules +install(CODE "MESSAGE(STATUS \"Installing target ${VISUALIZATION_TARGET}\")") +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} ) -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) - - +# Ensure linkage is reloaded after install +install(CODE "execute_process(COMMAND ldconfig)") diff --git a/modules/Visualization/src/main.c b/modules/Visualization/src/main.c index 9bb2f52c5..80f900121 100644 --- a/modules/Visualization/src/main.c +++ b/modules/Visualization/src/main.c @@ -8,8 +8,9 @@ #include #include #include - +#include #include "logging.h" +#include "maestroTime.h" #include "util.h" /*------------------------------------------------------------ @@ -21,15 +22,72 @@ #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 + /*------------------------------------------------------------ -- Function declarations. ------------------------------------------------------------*/ 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) { + + char ipStringBuffer[INET_ADDRSTRLEN]; + + sprintf(ipStringBuffer, "%s", + inet_ntop(AF_INET, &_monitorData->ClientIP, ipStringBuffer, sizeof (ipStringBuffer))); + char GPSMsOfWeekString[ENOUGH_BUFFER_SIZE]; + + sprintf(GPSMsOfWeekString, "%u", TimeGetAsGPSqmsOfWeek(&_monitorData->data.timestamp)); + char xPosString[ENOUGH_BUFFER_SIZE]; + + sprintf(xPosString, "%.3f", _monitorData->data.position.xCoord_m); + char yPosString[ENOUGH_BUFFER_SIZE]; + + sprintf(yPosString, "%.3f", _monitorData->data.position.yCoord_m); + char zPosString[ENOUGH_BUFFER_SIZE]; + + sprintf(zPosString, "%.3f", _monitorData->data.position.zCoord_m); + char headingString[ENOUGH_BUFFER_SIZE]; + + 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); + char stateString[ENOUGH_BUFFER_SIZE]; + + sprintf(stateString, "%s", objectStateToASCII(_monitorData->data.state)); + + //Build message from MonitorStruct + snprintf(_visualizationMessage, _sizeOfVisualizationMessage, "%s;%s;%s;%s;%s;%s;%s;%s;", + ipStringBuffer, + GPSMsOfWeekString, + xPosString, yPosString, zPosString, headingString, longSpeedString, stateString); + + + if (_debug) { + //LogMessage(LOG_LEVEL_INFO, "%s", _visualizationMessage); + 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); + } +} int main() { enum COMMAND command = COMM_INV; @@ -38,6 +96,9 @@ int main() { const struct timespec abortWaitTime = { 1, 0 }; struct timespec remTime; + MonitorDataType monitorData; + + LogInit(MODULE_NAME, LOG_LEVEL_DEBUG); LogMessage(LOG_LEVEL_INFO, "Task running with PID: %u", getpid()); @@ -46,8 +107,11 @@ int main() { vConnectVisualizationChannel(&visual_server, &visual_server_addr); - I32 iExit = 0; - char busReceiveBuffer[MBUS_MAX_DATALEN]; //!< Buffer for receiving from message bus + //Setup signal handlers + if (signal(SIGINT, signalHandler) == SIG_ERR) + util_error("Unable to initialize signal handler"); + + // Initialize message bus connection @@ -55,7 +119,9 @@ int main() { nanosleep(&sleepTimePeriod, &remTime); } - while (true) { + while (!iExit) { + + if (iCommRecv(&command, mqRecvData, MQ_MSG_SIZE, NULL) < 0) { util_error("Message bus receive error"); } @@ -75,15 +141,42 @@ int main() { switch (command) { case COMM_INIT: break; - case COMM_MONI: - // 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); + { + + //Populate the monitorType + UtilPopulateMonitorDataStruct(mqRecvData, (size_t) (sizeof (mqRecvData)), &monitorData); + + char dummy[1]; + int sizeOfVisualizationMessage; + + //Calculate size of incoming buffer + 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_rad * 180.0 / M_PI, + monitorData.data.speed.longitudinal_m_s, + objectStateToASCII(monitorData.data.state)); + 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); + + //Send visualization message on the UDP socket + UtilSendUDPData((const uint8_t *)"Visualization", &visual_server, &visual_server_addr, + visualizationMessage, strlen(visualizationMessage), 0); + + //Free memory used by malloc + free(visualizationMessage); + + } break; case COMM_LOG: break; @@ -98,6 +191,11 @@ int main() { } } + //Return MQBus to "stack" + (void)iCommClose(); + + LogMessage(LOG_LEVEL_INFO, "Visualization exiting..."); + return 0; } @@ -105,6 +203,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]; diff --git a/modules/dummy/CMakeLists.txt b/modules/dummy/CMakeLists.txt index 580c7141f..ca5a5125e 100644 --- a/modules/dummy/CMakeLists.txt +++ b/modules/dummy/CMakeLists.txt @@ -1,46 +1,46 @@ -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}) -include_directories(inc) -include_directories(../../util/C/logging) -include_directories(../../util/C/time) -include_directories(../../util/C/MQBus) -include_directories(../../server/inc) -include(GNUInstallDirs) +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 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 +# Link project executable to util libraries +target_link_libraries(${DUMMY_TARGET} LINK_PUBLIC + ${TIME_LIBRARY} + ${COREUTILS_LIBRARY} + ${LOGGING_LIBRARY} ) - -add_library(MQBus - ../../util/C/MQBus/mqbus.h - ../../util/C/MQBus/mqbus.c +target_include_directories(${DUMMY_TARGET} PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/inc ) -# Create library -add_library(util - ../../server/src/util.c - ../../server/inc/util.h +# 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} ) -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) - - +# Ensure linkage is reloaded after install +install(CODE "execute_process(COMMAND ldconfig)") diff --git a/runServer.sh b/runServer.sh index 3b989f36f..fdc3a61ee 100755 --- a/runServer.sh +++ b/runServer.sh @@ -2,7 +2,8 @@ #### User settings # Modify this array by adding more modules to include them in the execution -MODULES=(ScenarioControl) + +MODULES=() #### # Save top directory @@ -14,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 diff --git a/server/CMakeLists.txt b/server/CMakeLists.txt deleted file mode 100644 index 9f2741feb..000000000 --- a/server/CMakeLists.txt +++ /dev/null @@ -1,128 +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_directories(../util/C/logging) -include_directories(../util/C/time) -include_directories(../util/C/MQBus) -include_directories(../util/ASN1/generatedfiles) -include(GNUInstallDirs) - -# 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) - 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() - - -# 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 - src/util.c - inc/util.h - inc/iso22133.h -) - -# Create library -add_library(objctrl - src/objectcontrol.c - inc/objectcontrol.h -) - - -# 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/conf/triggeraction.conf b/server/conf/triggeraction.conf deleted file mode 100755 index cdad519e2..000000000 --- a/server/conf/triggeraction.conf +++ /dev/null @@ -1,2 +0,0 @@ -#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]; diff --git a/server/inc/iso22133.h b/server/inc/iso22133.h deleted file mode 100644 index 478b0bbcd..000000000 --- a/server/inc/iso22133.h +++ /dev/null @@ -1,226 +0,0 @@ -#ifndef ISO22133_H -#define ISO22133_H -#ifdef __cplusplus -extern "C" { -#endif -/*! This file contains all definitions pertaining to the ISO standard 22133 - * - * - * - * - */ - -#include - -#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 - -//! *************************** 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, - 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; - - -//! *************************** 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, - 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 -} 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; - - -//! 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) - -#ifdef __cplusplus -} -#endif -#endif diff --git a/server/inc/objectcontrol.h b/server/inc/objectcontrol.h deleted file mode 100644 index 8ba56708b..000000000 --- a/server/inc/objectcontrol.h +++ /dev/null @@ -1,37 +0,0 @@ -/*------------------------------------------------------------------------------ - -- Copyright : (C) 2016 CHRONOS project - ------------------------------------------------------------------------------ - -- File : objectcontrol.h - -- Author : Sebastian Loh Lindholm - -- Description : CHRONOS - -- Purpose : - -- Reference : - ------------------------------------------------------------------------------*/ - -#ifndef __OBJECTCONTROL_H_INCLUDED__ -#define __OBJECTCONTROL_H_INCLUDED__ - -#include -#include -#include "util.h" -#include "logging.h" - -/*------------------------------------------------------------ - -- Function declarations. - ------------------------------------------------------------*/ -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); -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); -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/integration-tests/249-uploadRowMismatchingTrajectories.py b/server/integration-tests/249-uploadRowMismatchingTrajectories.py deleted file mode 100644 index 6b9d2f1e4..000000000 --- a/server/integration-tests/249-uploadRowMismatchingTrajectories.py +++ /dev/null @@ -1,101 +0,0 @@ -from tools.MSCP import MSCP -from tools.Executable import Executable -from tools.TrajectoryFile import * -from tools.PortChecker import * -import time -import subprocess -import sys - - -userControl = None -server = None -obj = None - - -def checkProgramStatus(failurePrintout): - if server != None: - if server.poll(): - print(failurePrintout) - if userControl != None: - userControl.shutdown() - server.stop() - if obj != None: - obj.stop() - sys.exit(1) - -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 - userControl.UploadFile("traj/127.0.0.1", fewRowTraj) - - # 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) - - userControl.Init() - userControl.waitForObjectControlState("INITIALIZED") - - time.sleep(0.05) - - userControl.Disconnect() - userControl.waitForObjectControlState("IDLE") - - # 7: Upload long trajectory - userControl.UploadFile("traj/127.0.0.1", manyRowTraj) - - # 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) - # 9: Upload normal trajectory, to verify we can still initialise - userControl.UploadFile("traj/127.0.0.1", normalTraj) - - userControl.Init() - userControl.waitForObjectControlState("INITIALIZED") - - time.sleep(0.05) - - userControl.Disconnect() - userControl.waitForObjectControlState("IDLE") - - # 10: Done! - userControl.shutdown() - server.stop() - sys.exit(0) - diff --git a/server/integration-tests/tools/MSCP.py b/server/integration-tests/tools/MSCP.py deleted file mode 100644 index 7a7315311..000000000 --- a/server/integration-tests/tools/MSCP.py +++ /dev/null @@ -1,281 +0,0 @@ -import socket -import sys, select -import threading -import re -import time - -class MSCP: - def __init__(self,host,port=54241): - self.host = host - self.port = port - self.socket = socket.socket() - print("=== MSCP connecting to " + str(self.host) + ":" + str(self.port)) - self.socket.connect((self.host,self.port)) - self.lastStatusReply = {} - self.lastAbortReply = {} - self.lastStartReply = {} - self.lastUploadReply = {} - self.lastStatusReply["objectControlState"] = "UNKNOWN" - self.lastStatusReply["systemControlState"] = "UNKNOWN" - self.lastStatusReply["systemControlErrorCode"] = 0 - self.lastStatusReply["objectControlErrorCode"] = 0 - self.lastAbortReply["scenarioActive"] = 0 - self.lastStartReply["scenarioActive"] = 0 - self.lastUploadReply["status"] = "UNKNOWN" - self.uploadReplyLock = threading.Lock() - self.startReplyLock = threading.Lock() - self.abortReplyLock = threading.Lock() - self.statusReplyLock = threading.Lock() - self.responseCodeLock = threading.Lock() - self.quit = False - self.lastResponseCode = "No message received yet" - self.listener = threading.Thread(target=self.listen) - self.listener.start() - - def listen(self): - replyPatterns = [ - {"command": "init", "regex": re.compile(b'(..)InitializeScenario:')}, - {"command": "status", "regex": re.compile(b'(..)GetServerStatus:(.)(.)(.)?(.)?')}, - {"command": "abort", "regex": re.compile(b'(..)AbortScenario:(.)')}, - {"command": "arm", "regex": re.compile(b'(..)ArmScenario:')}, - {"command": "start", "regex": re.compile(b'(..)StartScenario:(.)')}, - {"command": "connect", "regex": re.compile(b'(..)ConnectObject:')}, - {"command": "disconnect", "regex": re.compile(b'(..)DisconnectObject:')}, - {"command": "upload", "regex": re.compile(b'([^u][^b])UploadFile:(.)')}, - {"command": "subupload", "regex": re.compile(b'(..)SubUploadFile:(.)')} - ] - - print("=== Starting listener on " + str(self.host) + ":" + str(self.port)) - while not self.quit: - try: - data = self.socket.recv(1024) - except ConnectionResetError as e: - if not self.quit: - raise e - - 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" - 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" - - 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() - - def GetStatus(self): - message = "POST /maestro HTTP/1.1\r\nHost: " + self.host + "\r\n\r\nGetServerStatus();" - self.Send(message) - print("=== GetServerStatus() sent") - - def Abort(self): - message = "POST /maestro HTTP/1.1\r\nHost:" + self.host + "\r\n\r\nAbortScenario();" - self.Send(message) - print("=== Abort() sent") - - def Arm(self): - message = "POST /maestro HTTP/1.1\r\nHost:" + self.host + "\r\n\r\nArmScenario();" - self.Send(message) - print("=== ArmScenario() sent") - - def Init(self): - message = "POST /maestro HTTP/1.1\r\nHost:" + self.host + "\r\n\r\nInitializeScenario();" - self.Send(message) - print("=== Init() sent") - - def Connect(self): - message = "POST /maestro HTTP/1.1\r\nHost:" + self.host + "\r\n\r\nConnectObject();" - self.Send(message) - print("=== Connect() sent") - - def Disconnect(self): - message = "POST /maestro HTTP/1.1\r\nHost:" + self.host + "\r\n\r\nDisconnectObject();" - 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) + ");" - 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) + ");" - self.uploadReplyLock.acquire() - self.lastUploadReply["status"] = "UNKNOWN" - self.uploadReplyLock.release() - self.Send(message) - print("=== UploadFile() sent") - self.waitForUploadReply("SERVER_PREPARED") - # Send file - self.Send(fileContents) - self.uploadReplyLock.acquire() - self.lastUploadReply["status"] = "UNKNOWN" - self.uploadReplyLock.release() - self.waitForUploadReply("UPLOAD_SUCCESS") - print("=== File uploaded") - - def Send(self,message): - self.socket.send(message.encode()) - - def shutdown(self): - self.quit = True - self.socket.close() - - def waitForUploadReply(self,status,timeout=3.0): - timeoutTime = time.time() + timeout - self.uploadReplyLock.acquire() - ur = self.lastUploadReply["status"] - self.uploadReplyLock.release() - while ur == "UNKNOWN" and time.time() < timeoutTime: - self.uploadReplyLock.acquire() - ur = self.lastUploadReply["status"] - self.uploadReplyLock.release() - if ur != status and time.time() >= timeoutTime: - raise TimeoutError("Timed out while waiting for reply to UploadFile") - elif ur != status: - raise ValueError("Expected status " + status + " but received " + ur) - - def waitForObjectControlState(self,state,timeout=3.0): - timeoutTime = time.time() + timeout - self.statusReplyLock.acquire() - sr = self.lastStatusReply["objectControlState"] - self.statusReplyLock.release() - while sr != state and time.time() < timeoutTime: - time.sleep(0.005) - self.statusReplyLock.acquire() - sr = self.lastStatusReply["objectControlState"] - 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) - - def interpretResponseCode(self,code): - num = int.from_bytes(code,byteorder='big') - if num == 0x0001: - return "OK" - elif num == 0x0002: - return "Logging data" - elif num == 0x0F10: - return "Error" - elif num == 0x0F20: - return "Function not available" - elif num == 0x0F25: - return "Incorrect state" - elif num == 0x0F30: - return "Invalid length" - elif num == 0x0F40: - return "Busy" - elif num == 0x0F42: - return "Timeout" - elif num == 0x0F50: - return "Invalid script" - elif num == 0x0F60: - return "Invalid encryption code" - elif num == 0x0F61: - return "Decryption error" - elif num == 0x0F62: - return "No data" - else: - raise ValueError("Response code " + str(num) + " is not recognized") - diff --git a/server/src/objectcontrol.c b/server/src/objectcontrol.c deleted file mode 100644 index 87542be21..000000000 --- a/server/src/objectcontrol.c +++ /dev/null @@ -1,3309 +0,0 @@ -/*------------------------------------------------------------------------------ - -- Copyright : (C) 2016 CHRONOS project - ------------------------------------------------------------------------------ - -- File : objectcontrol.c - -- Author : Sebastian Loh Lindholm - -- Description : CHRONOS - -- Purpose : - -- Reference : - ------------------------------------------------------------------------------*/ - -/*------------------------------------------------------------ - -- Include files. - ------------------------------------------------------------*/ -#include "objectcontrol.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "timecontrol.h" -#include "datadictionary.h" -#include "maestroTime.h" -#include "iso22133.h" - - - -/*------------------------------------------------------------ - -- Defines - ------------------------------------------------------------*/ - -// Macro for determining individual struct member sizes -#define member_sizeof(type, member) sizeof(((type *)0)->member) - -#define LOCALHOST "127.0.0.1" - -#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 -#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 - -#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 COMMAND_DOTM_CODE 1 -#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_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) -#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_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) - -#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 -#define SMALL_BUFFER_SIZE_254 254 - -#define TRAJECTORY_FILE_MAX_ROWS 4096 - -#define LOG_BUFFER_LENGTH 128 - -#define USE_TEMP_LOGFILE 0 -#define TEMP_LOG_FILE "log/temp.log" - - -typedef enum { - COMMAND_HEARTBEAT_GO, - COMMAND_HEARTBEAT_ABORT -} hearbeatCommand_t; - - -typedef enum { - 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, - returning nothing) where the function foo(int a, float b) is declared elsewhere: - void (*fooptr)(int,float) = foo; - fooptr(10,1.5); - - Consequently, the below typedef defines a StateTransition type as a function pointer to a function taking - (OBCState_t, OBCState_t) as input, and returning a StateTransitionResult -*/ -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. -------------------------------------------------------------*/ -static I32 vConnectObject(int *sockfd, const char *name, const uint32_t port, U8 * Disconnect); -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); -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); -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); -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); -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 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); -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], - 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); -StateTransitionResult vSetState(OBCState_t requestedState, GSDType * GSD); -StateTransition tGetTransition(OBCState_t fromState); -StateTransitionResult tFromIdle(OBCState_t * currentState, OBCState_t requestedState); -StateTransitionResult tFromInitialized(OBCState_t * currentState, OBCState_t requestedState); -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 tFromError(OBCState_t * currentState, OBCState_t requestedState); -StateTransitionResult tFromUndefined(OBCState_t * currentState, OBCState_t requestedState); - -/*------------------------------------------------------------ --- Private variables -------------------------------------------------------------*/ - -#define MODULE_NAME "ObjectControl" -static volatile int iExit = 0; - -/*------------------------------------------------------------ - -- Public functions - ------------------------------------------------------------*/ - -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 - //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; - } - 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"); -} - - -/*------------------------------------------------------------ - -- 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"); - } -} - -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 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; -} - - -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 -} - -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; -} -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 -} - - -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 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 - -} - - -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 -} - -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 -} - -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 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 -} - - - -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; -} - -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; - - 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 -} - - -/*! - * \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 = 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 = 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 = 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) { - - 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) - 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; - - char buffer[256]; - int iResult; - - *sockfd = socket(AF_INET, SOCK_STREAM, 0); - - if (*sockfd < 0) { - util_error("[ObjectControl] ERR: Failed to open control socket"); - } - - 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; - - 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); - - // 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); - - 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; -} - -static void vDisconnectObject(int *sockfd) { - close(*sockfd); -} - - -static void vCreateSafetyChannel(const char *name, const uint32_t port, int *sockfd, struct sockaddr_in *addr) { - int result; - struct hostent *object; - - /* 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"); - } - - /* Set address to object */ - object = gethostbyname(name); - - 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); - - /* 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); -} - -static void vCloseSafetyChannel(int *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; -} - -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; -} - - -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; -} - -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; -} - - - -OBCState_t vGetState(GSDType * 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 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; - } -} - -StateTransitionResult tFromIdle(OBCState_t * currentState, OBCState_t requestedState) { - 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; -} - -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; -} - -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; -} - -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; -} - -StateTransitionResult tFromError(OBCState_t * currentState, OBCState_t requestedState) { - if (requestedState == OBC_STATE_IDLE) { - *currentState = requestedState; - return TRANSITION_OK; - } - return TRANSITION_INVALID; -} - -StateTransitionResult tFromUndefined(OBCState_t * currentState, OBCState_t requestedState) { - 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); -} 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." - diff --git a/util b/util index 13d1481a9..55c7b8726 160000 --- a/util +++ b/util @@ -1 +1 @@ -Subproject commit 13d1481a9be4ba2e851717c650f7ff884467ce1f +Subproject commit 55c7b8726c87418e7d6b5049c97f8d33af147dc1