From 180fd4766b8ab5e4f5194033e806bdeaea895cf0 Mon Sep 17 00:00:00 2001 From: Marco Date: Tue, 20 Apr 2021 11:59:23 +0100 Subject: [PATCH] Added necessary files for last commit --- src/cpm.c | 620 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ src/cpm.h | 198 +++++++++++++++++ 2 files changed, 818 insertions(+) create mode 100644 src/cpm.c create mode 100644 src/cpm.h diff --git a/src/cpm.c b/src/cpm.c new file mode 100644 index 0000000..eba5e3e --- /dev/null +++ b/src/cpm.c @@ -0,0 +1,620 @@ +#include "cpm.h" +#include "facilities.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#define syslog_info(msg, ...) syslog(LOG_INFO, msg, ##__VA_ARGS__) +#define syslog_emerg(msg, ...) syslog(LOG_EMERG, "%s:%d [" msg "]", __func__, __LINE__, ##__VA_ARGS__) +#define syslog_err(msg, ...) syslog(LOG_ERR, "%s:%d [" msg "]", __func__, __LINE__, ##__VA_ARGS__) + + +#ifndef NDEBUG +#define syslog_debug(msg, ...) syslog(LOG_DEBUG, "%s:%d " msg "", __func__, __LINE__, ##__VA_ARGS__) +#else +#define syslog_debug(msg, ...) +#endif + +#define PI 3.141592654 + +/* Variables */ + +float roadRotationSin; +float roadRotationCos; +S_ETHERNET_CONNECTION_T s_socket; +S_OBJECT_CONTROL_T s_objectControl; +S_OBJECTS_T as_objects[NOF_OBJECTS]; +char* station_id; +it2s_config_t* config; + + + +bool interface_connection(char* radar_ip,char* radar_port){ + + // 1 - Create socket + s_socket.i32_socket = socket(AF_INET, SOCK_STREAM, 0); + + if(s_socket.i32_socket < 0){ + syslog_err("Initializing socket failed ..."); + return false; + } + + // 2 - Bind it to server address and port + bzero(&s_socket.s_server, sizeof(s_socket.s_server)); + s_socket.s_server.sin_family = AF_INET; + s_socket.s_server.sin_addr.s_addr = inet_addr(radar_ip); + s_socket.s_server.sin_port = htons(atoi(radar_port)); + + if(bind(s_socket.i32_socket, (struct sockaddr*)&s_socket.s_server,sizeof(s_socket.s_server)) < 0){ + syslog_err("Binding socket to address error ..."); + return false; + } + + + // 3 - Listening to the socket (Waiting for incoming connection) + unsigned int len = sizeof(s_socket.s_client); + + if(listen(s_socket.i32_socket,1)<0){ + syslog_err("Waiting for incoming requests failed..."); + return false; + } + + if((s_socket.i32_client = accept(s_socket.i32_socket, (struct sockaddr*)&s_socket.s_server, &len)) < 0){ + syslog_err("Client disconnected..."); + return false; + } + + syslog_debug("Radar connected"); + + return true; +} + + +void parse_can_data_tm(u_int32_t u32_can_id, int i32_can_len, u_int8_t* au8_can_data) { + u_int8_t u8_pvrMessagePart = 0; + u_int8_t tmp = 0; + //static long last = 0; + S_PVR_T s_pvr; + S_SENSOR_CONTROL_T s_sensorControl; + + switch (u32_can_id) // Interpret the different types of CAN messages + { + case 0x785: + s_pvr.u8_numberOfCountedObjects = 0; + s_pvr.u32_UnixTime = 0; + s_pvr.u16_Milliseconds = 0; + s_pvr.u8_SensorNetworkID = 0; + + u8_pvrMessagePart = (au8_can_data[0] & 0x1); + if (u8_pvrMessagePart == 0) { + s_pvr.u8_numberOfCountedObjects = (au8_can_data[0] & 0xFE) >> 1; + s_pvr.u32_UnixTime |= au8_can_data[4] << 24; + s_pvr.u32_UnixTime |= au8_can_data[3] << 16; + s_pvr.u32_UnixTime |= au8_can_data[2] << 8; + s_pvr.u32_UnixTime |= au8_can_data[1]; + + s_pvr.u16_Milliseconds |= (au8_can_data[6] & 0x3) << 8; + s_pvr.u16_Milliseconds |= au8_can_data[5]; + + s_pvr.u8_SensorNetworkID = (au8_can_data[6] & 0xC) >> 2; + + // printf("Unix Time: %u, Ms: %u\n", s_pvr.u32_UnixTime, s_pvr.u16_Milliseconds); + + long current = s_pvr.u32_UnixTime * 1000 + s_pvr.u16_Milliseconds; + + //printf("Unix Time Epoch Fixed: %ld\n", (long)(current - 1072915200000)); + //printf("Diff: %ld\n", current - last); + //last = current; + } else if (u8_pvrMessagePart == 1) { + s_pvr.u8_ObjectNumber = 0; + s_pvr.u8_ObjectID = 0; + s_pvr.i16_speed = 0; + s_pvr.u8_class = 0; + s_pvr.u8_mLineNumber = 0; + s_pvr.u8_laneNumber = 0; + + s_pvr.u8_ObjectNumber = (au8_can_data[0] & 0xFE) >> 1; + + s_pvr.u8_ObjectID = au8_can_data[1]; + + s_pvr.i16_speed |= (au8_can_data[3] & 0x7) << 8; + s_pvr.i16_speed |= au8_can_data[2]; + s_pvr.i16_speed -= 1024; // Speed Offset + + s_pvr.u8_class = (au8_can_data[3] & 0x38) >> 3; + + s_pvr.u8_mLineNumber = (au8_can_data[3] & 0xC0) >> 6; + + s_pvr.u8_laneNumber = (au8_can_data[4] & 0x7) >> 3; + } + break; + case 0x500: + memset(&s_sensorControl, 0, sizeof(s_sensorControl)); + s_sensorControl.u8_sensorStatus = au8_can_data[0]; + s_sensorControl.u8_InterfaceMode = (au8_can_data[1] & 0xF); + s_sensorControl.u8_networkID = (au8_can_data[1] & 0xF0) >> 4; + s_sensorControl.u8_diagnose = au8_can_data[2]; + s_sensorControl.u32_time |= au8_can_data[7] << 24; + s_sensorControl.u32_time |= au8_can_data[6] << 16; + s_sensorControl.u32_time |= au8_can_data[5] << 8; + s_sensorControl.u32_time |= au8_can_data[4]; + break; + case 0x501: + memset(&s_objectControl, 0, sizeof(s_objectControl)); + s_objectControl.u8_numberOfObjects = au8_can_data[0]; + s_objectControl.u8_numberOfMessages = au8_can_data[1]; + s_objectControl.u8_cycleDuration = au8_can_data[2]; + s_objectControl.u8_objectData0Format = au8_can_data[3] & 0xF; + s_objectControl.u8_objectData1Format = (au8_can_data[3] & 0xF0) >> 4; + s_objectControl.u32_cycleCount |= au8_can_data[7] << 24; + s_objectControl.u32_cycleCount |= au8_can_data[6] << 16; + s_objectControl.u32_cycleCount |= au8_can_data[5] << 8; + s_objectControl.u32_cycleCount |= au8_can_data[4]; + break; + } + + if ((u32_can_id >= 0x502) && (u32_can_id <= 0x57F)) { + u_int16_t u16_objectIndex = (u_int16_t)u32_can_id - 0x502; + as_objects[u16_objectIndex].u8_modeSignal = au8_can_data[0] & 0x1; + if (s_objectControl.u8_objectData0Format == 005) // without Update Flag + { + as_objects[u16_objectIndex].f_xPoint = (au8_can_data[1] & 0x3F) << 7; + as_objects[u16_objectIndex].f_xPoint += au8_can_data[0] >> 1; + as_objects[u16_objectIndex].f_xPoint -= 4096; + as_objects[u16_objectIndex].f_xPoint *= 0.128; + + as_objects[u16_objectIndex].f_yPoint = (au8_can_data[3] & 0x7) << 10; + as_objects[u16_objectIndex].f_yPoint += au8_can_data[2] << 2; + as_objects[u16_objectIndex].f_yPoint += au8_can_data[1] >> 6; + as_objects[u16_objectIndex].f_yPoint -= 4096; + as_objects[u16_objectIndex].f_yPoint *= 0.128; + + as_objects[u16_objectIndex].f_xSpeed = ((au8_can_data[4] & 0x3F) << 5); + as_objects[u16_objectIndex].f_xSpeed += (au8_can_data[3] >> 3); + as_objects[u16_objectIndex].f_xSpeed -= 1024; + as_objects[u16_objectIndex].f_xSpeed *= 0.1; + + as_objects[u16_objectIndex].f_ySpeed = (au8_can_data[6] & 0x1) << 10; + as_objects[u16_objectIndex].f_ySpeed += (au8_can_data[5] << 2); + as_objects[u16_objectIndex].f_ySpeed += (au8_can_data[4] >> 6); + as_objects[u16_objectIndex].f_ySpeed -= 1024.0f; + as_objects[u16_objectIndex].f_ySpeed *= 0.1; + + as_objects[u16_objectIndex].f_objectLength = (au8_can_data[6] >> 1) * 0.2f; + + as_objects[u16_objectIndex].u8_objectID = au8_can_data[7]; + } else if (s_objectControl.u8_objectData0Format == 4) { + as_objects[u16_objectIndex].f_xPoint = (au8_can_data[1] & 0x3F) << 7; + as_objects[u16_objectIndex].f_xPoint += au8_can_data[0] >> 1; + as_objects[u16_objectIndex].f_xPoint -= 4096; + as_objects[u16_objectIndex].f_xPoint *= 0.128; + + as_objects[u16_objectIndex].f_yPoint = (au8_can_data[3] & 0x7) << 10; + as_objects[u16_objectIndex].f_yPoint += au8_can_data[2] << 2; + as_objects[u16_objectIndex].f_yPoint += au8_can_data[1] >> 6; + as_objects[u16_objectIndex].f_yPoint -= 4096; + as_objects[u16_objectIndex].f_yPoint *= 0.128; + + as_objects[u16_objectIndex].f_xSpeed = (au8_can_data[4] << 5); + as_objects[u16_objectIndex].f_xSpeed += (au8_can_data[3] >> 3); + as_objects[u16_objectIndex].f_xSpeed -= 1024; + as_objects[u16_objectIndex].f_xSpeed *= 0.1; + + as_objects[u16_objectIndex].f_ySpeed = (au8_can_data[6] & 0x1) << 10; + as_objects[u16_objectIndex].f_ySpeed += (au8_can_data[5] << 2); + as_objects[u16_objectIndex].f_ySpeed += (au8_can_data[4] >> 6); + as_objects[u16_objectIndex].f_ySpeed -= 1024.0f; + as_objects[u16_objectIndex].f_ySpeed *= 0.1; + + as_objects[u16_objectIndex].f_objectLength = (au8_can_data[6] >> 1) * 0.2f; + + as_objects[u16_objectIndex].u8_objectID = au8_can_data[7] & 0x7F; + as_objects[u16_objectIndex].u8_updateFlag = au8_can_data[7] >> 7; + } + } +} + + + + +void parse_input(u_int8_t* u8_input_buffer, int i32_len) { + static enum state_t s_state = IDLE; + u_int8_t u8_input = 0; + int i32_i; + + static int i32_can_len_counter = 0, i32_can_len = 0, i32_can_id = 0, i32_stop_search = 0; + static unsigned char au8_can_data[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; + static int i32_xor = 0; + + for (i32_i = 0; i32_i < i32_len; i32_i++) { + u8_input = u8_input_buffer[i32_i]; + switch (i32_stop_search) { + case 0: + if (u8_input == 0xea) { + i32_stop_search = 1; + } + break; + case 1: + if (u8_input == 0xeb) { + i32_stop_search = 2; + } else { + i32_stop_search = 0; + } + break; + case 2: + if (u8_input == 0xec) { + i32_stop_search = 3; + } else { + i32_stop_search = 0; + } + break; + case 3: + if (u8_input == 0xed) { + s_state = IDLE; + } + i32_stop_search = 0; + break; + } + + switch (s_state) { + case IDLE: + if (u8_input == 0xca) { + s_state = START_1; + } + break; + case START_1: + if (u8_input == 0xcb) { + s_state = START_2; + } + break; + case START_2: + if (u8_input == 0xcc) { + s_state = START_3; + } + break; + case START_3: + if (u8_input == 0xcd) { + s_state = START_4; + } + break; + case START_4: + s_state = CAN_ID_H; + break; + case CAN_ID_H: + s_state = CAN_ID_L; + break; + case CAN_ID_L: + s_state = CAN_LEN; + break; + case CAN_LEN: + s_state = CAN_PAYLOAD; + break; + case CAN_PAYLOAD: + if (i32_can_len_counter >= i32_can_len) { + s_state = CAN_ID_H; + } + break; + default: + printf("Something probably went wrong, this code is likely unreachable"); + } + + switch (s_state) { + case START_1: + i32_xor = 0; + break; + case CAN_ID_H: + i32_can_id = 0; + i32_can_id |= u8_input << 8; + i32_xor ^= u8_input; + break; + case CAN_ID_L: + i32_can_id |= u8_input; + i32_xor ^= u8_input; + break; + case CAN_LEN: + i32_can_len = u8_input; + i32_can_len_counter = 0; + i32_xor ^= u8_input; + break; + case CAN_PAYLOAD: + if ((i32_can_len_counter < 16) && (i32_can_len_counter <= i32_can_len)) { + //if (i32_can_len - i32_can_len_counter - 1 > 15) + // printf("%d", i32_can_len - i32_can_len_counter - 1); + if (i32_can_len - i32_can_len_counter - 1 < 16) + au8_can_data[i32_can_len - i32_can_len_counter - 1] = u8_input; + } + i32_can_len_counter++; + if (i32_can_len_counter >= i32_can_len) { + parse_can_data_tm(i32_can_id, i32_can_len, au8_can_data); + } + i32_xor ^= u8_input; + break; + default:; + } + } +} + + +long rotate_x(long x, long y) { + return (long)(x * roadRotationCos - y * roadRotationSin); +} + +long rotate_y(long x, long y) { + return (long)(x * roadRotationSin + y * roadRotationCos); +} + + +void mk_cpm(facilities_t* facilities,CPM_t* cpm_tx, struct timespec* systemtime) { + + + cpm_tx->header.protocolVersion = PROTOCOL_VERSION; + cpm_tx->header.messageID = MESSAGE_ID; + cpm_tx->header.stationID = STATION_ID; + + int retval = clock_gettime(CLOCK_REALTIME, systemtime); + if (retval != 0) { + perror("clock_gettime() failed"); + exit(1); + } + + long timeStampIts = (long)(systemtime->tv_sec * 1000 + systemtime->tv_nsec / 1E6); + timeStampIts = timeStampIts - 1072915200000; // EPOCH -> 2004/01/01 00:00:000 + long generationDeltaTime = timeStampIts % 65536; // generationDeltaTime = TimestampIts mod 65 536 + + cpm_tx->cpm.generationDeltaTime = generationDeltaTime; + cpm_tx->cpm.cpmParameters.managementContainer.stationType = StationType_roadSideUnit; + cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.latitude = 420309860; //A3 // 412400078; (A24) + cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.longitude = -86515778; //A3 // -86950224; (A24) + cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.positionConfidenceEllipse.semiMajorConfidence = 100; + cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.positionConfidenceEllipse.semiMinorConfidence = 100; + cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.positionConfidenceEllipse.semiMajorOrientation = HeadingValue_wgs84North; + cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.altitude.altitudeValue = 4000; + cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.altitude.altitudeConfidence = AltitudeConfidence_alt_005_00; + + cpm_tx->cpm.cpmParameters.sensorInformationContainer = calloc(1, sizeof(SensorInformationContainer_t)); + cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.count = 1; + cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.size = 1; + cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array = calloc(1, sizeof(SensorInformation_t)); + cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0] = calloc(1, sizeof(SensorInformation_t)); + + cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->sensorID = 0; + cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->type = SensorType_radar; + cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.present = DetectionArea_PR_stationarySensorRadial; + cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.range = 3400; + cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.stationaryHorizontalOpeningAngleStart = ROAD_ANGLE * 10 - 500; + cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.stationaryHorizontalOpeningAngleEnd = ROAD_ANGLE * 10 + 500; + cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.verticalOpeningAngleStart = calloc(1, sizeof(CartesianAngleValue_t)); + (*cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.verticalOpeningAngleStart) = 1730; + cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.verticalOpeningAngleEnd = calloc(1, sizeof(CartesianAngleValue_t)); + (*cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.verticalOpeningAngleEnd) = 1890; + cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.sensorHeight = calloc(1, sizeof(SensorHeight_t)); + (*cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.sensorHeight) = 600; + + cpm_tx->cpm.cpmParameters.perceivedObjectContainer = calloc(1, sizeof(PerceivedObjectContainer_t)); + + cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.count = s_objectControl.u8_numberOfObjects; + cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.size = s_objectControl.u8_numberOfObjects; + cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array = calloc( + cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.count, + sizeof(PerceivedObject_t*)); + + + for (int i = 0; i < cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.count; i++) { + cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i] = calloc(1, sizeof(PerceivedObject_t)); + + cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i]->objectID = (long)as_objects[i].u8_objectID; + cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i]->timeOfMeasurement = 0; //sem informaçao do radar + + cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i]->objectConfidence = 95; + cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i]->xDistance.value = rotate_x( + (long)as_objects[i].f_xPoint, (long)as_objects[i].f_yPoint); + cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i]->xDistance.confidence = 102; + // printf("->%f<-\n", as_objects[i].f_xPoint); + cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i]->yDistance.value = rotate_y( + (long)as_objects[i].f_xPoint, (long)as_objects[i].f_yPoint); + cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i]->yDistance.confidence = 102; + cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i]->xSpeed.value = rotate_x( + (long)as_objects[i].f_xSpeed, (long)as_objects[i].f_ySpeed); + cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i]->xSpeed.confidence = 40; + cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i]->ySpeed.value = rotate_y( + (long)as_objects[i].f_xSpeed, (long)as_objects[i].f_ySpeed); + cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i]->ySpeed.confidence = 40; + cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i]->objectRefPoint = ObjectRefPoint_bottomMid; + } + + cpm_tx->cpm.cpmParameters.numberOfPerceivedObjects = cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.count; +} + + +int encode_cpm(CPM_t* cpm_tx, uint8_t *out_message_data, uint32_t *out_message_size) { + memset(out_message_data, 0, 1500); + asn_enc_rval_t retval_enc = uper_encode_to_buffer(&asn_DEF_CPM, NULL, cpm_tx, out_message_data, 1500); + if (retval_enc.encoded == -1) { + syslog_err("[facilities] [cp] failed encoding CPM (%s)", retval_enc.failed_type->name); + return 1; + } + + *out_message_size = ((retval_enc.encoded + 7) / 8); + + return 0; +} + +dissemination_t* dissemination_init(){ + /* Mutex init and dissemination memory allocation */ + dissemination_t* dissemination = (dissemination_t*) calloc(1, sizeof(dissemination_t)); + pthread_mutex_init(&dissemination->lock, NULL); + + return dissemination; +} + + +int dissemination_check(dissemination_t* dissemination){ + int rv = 0; + + /* Get Now time */ + struct timespec systemtime; + clock_gettime(CLOCK_REALTIME, &systemtime); + long now = (long)((systemtime.tv_sec + LEAP_SECONDS) * 1000 + systemtime.tv_nsec / 1E6); + now -= 1072915200000; + + /* mutex is used to lock shared resources */ + + pthread_mutex_lock(&dissemination->lock); + + // Both cases for OBU and RSU (BASIC: generation interval 1s) + if(now >= dissemination->next_cpm_max) + rv = 1; + + + pthread_mutex_unlock(&dissemination->lock); + + + return rv; +} + + +void dissemination_reset_timer(dissemination_t* dissemination){ + + /* Get Now time */ + struct timespec systemtime; + clock_gettime(CLOCK_REALTIME, &systemtime); + long now = (long)((systemtime.tv_sec + LEAP_SECONDS) * 1000 + systemtime.tv_nsec / 1E6); + now -= 1072915200000; + + /* Both cases for RSU and OBU */ + + pthread_mutex_lock(&dissemination->lock); + + dissemination->next_cpm_min = now + dissemination->T_GenCpmMin; + dissemination->next_cpm_max = now + dissemination->T_GenCpmMax; + + + pthread_mutex_unlock(&dissemination->lock); +} + + + + +void *cp_service(void *fc){ + //roadRotationSin = sin(((config->applications.its_center.rotation + 90.0) * PI) / 180); + //roadRotationCos = cos(((config->applications.its_center.rotation + 90.0) * PI) / 180); + + roadRotationSin = 0.8660; // Default values for A3 Configuration + roadRotationCos = -0.5000; // Default values for A3 configuration + + + facilities_t *facilities = (facilities_t *) fc; + BTPDataRequest_t *bdr = calloc(1, sizeof(BTPDataRequest_t)); + + /* Fill mandatory BTP Data Request parameters */ + + bdr->gnDestinationAddress.buf = malloc(6); + bdr->gnDestinationAddress.size = 6; + for(int i = 0; i < 6; i++) + bdr->gnDestinationAddress.buf[i] = 0xff; //Broadcast addr + + + + bdr->btpType = BTPType_btpB; //BTP Type B is for non-interactive packet transport | BTP Type A is for interactive packet transport + //The former doesn't have a source port and the latter have + + bdr->destinationPort = Port_cpm; //CPM entity port for communication between Facilities and Transport + bdr->gnPacketTransportType = PacketTransportType_shb; //shb = Single Hop Broadcast packet + bdr->gnTrafficClass = 2; //Identifier assigned to a GeoNetworking packet that expresses its requirements on data transport (WHY IS IT 2?) + bdr->data.buf = malloc(1500); // CPM Data to be sent to the Transport layer + + if(facilities->use_security) { + bdr->gnSecurityProfile = malloc(sizeof(long)); + *bdr->gnSecurityProfile = 1; + } + + + +/* Not needed + read_config("/etc/it2s/itss.toml"); // To get the parameters of the broker + signal(SIGINT, interruption_handler); // In case Ctrl+C is sent to the process interruption_handler will be executed + setup_mec_server_connection(); + init_tiles(); +*/ + + + interface_connection(RADAR_IP,RADAR_PORT); // Create Radar listening socket + struct timespec* systemtime; + systemtime = (struct timespec*)malloc(sizeof(struct timespec)); + if (systemtime == NULL) { + printf("memory allocation failed: %m"); + exit(1); + } + + + /* Variables */ + int i32_recv_bytes; + u_int8_t au8_readBuffer[READ_BUFFER_SIZE]; + //void* message_data = malloc(1500); + uint8_t bdr_oer[2048]; + bdr_oer[0] = 4; //Facilities + + + while(!facilities->exit){ + usleep(1000*50); + + + if (dissemination_check(facilities->dissemination) && facilities->dissemination->active){ + /* Receive Data from radar interface */ + CPM_t* cpm_tx = calloc(1, sizeof(CPM_t)); + i32_recv_bytes = recv(s_socket.i32_client, &au8_readBuffer, READ_BUFFER_SIZE, 0); //recv(socket,buffer,size,flags) + + if(i32_recv_bytes < 0){ + syslog_debug("No data received from radar ..."); + break; + } + + /* Information parsing */ + parse_input(au8_readBuffer,i32_recv_bytes); + + /* CPM build and encoding (Could be merged if needed) */ + mk_cpm(facilities,cpm_tx,systemtime); + + if(encode_cpm(cpm_tx, bdr->data.buf, (uint32_t *) &bdr->data.size) == 1){ + ASN_STRUCT_FREE(asn_DEF_CPM, cpm_tx); + continue; + } + + /* Encode BTPDataRequest */ + + asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_BTPDataRequest, NULL, bdr, bdr_oer+1, 2047); + if(enc.encoded == -1){ + syslog_err("[facilities] encoding BTPDataRequest for cpm failed"); + continue; + } + + /* Create thread to send packet to the Transport Layer (=3) */ + queue_add(facilities->tx_queue, bdr_oer, enc.encoded+1, 3); + pthread_cond_signal(&facilities->tx_queue->trigger); + + + /* Reset Timer for dissemination control */ + + dissemination_reset_timer(facilities->dissemination); + + /* TODO: Send message to applications to send it to the broker ? *FacilitiesDataIndication*/ + } + } + + ASN_STRUCT_FREE(asn_DEF_BTPDataRequest,bdr); + return NULL; + +} diff --git a/src/cpm.h b/src/cpm.h new file mode 100644 index 0000000..2e514e5 --- /dev/null +++ b/src/cpm.h @@ -0,0 +1,198 @@ +#ifndef FACILITIES_CPM_H +#define FACILITIES_CPM_H + + +#include +#include + +#include +#include +#include +#include + +#define LEAP_SECONDS 5 +#define READ_BUFFER_SIZE 1024 // Buffer for Serial and Ethernet read function (Used in cp_service) +#define STATION_ID 10 +#define MESSAGE_ID 14 +#define PROTOCOL_VERSION 1 +#define CAN_BUFFER_SIZE 24 // Send cmd Buffer +#define CAN_SMS_BUFFER_SIZE 25 // Send cmd Buffer for sms transport protocol +#define SERIAL_PORT_BUFFER 13 // string buffer serial interface +#define BAUDRATE B115200 // default Baudrate +#define CRC16_LENGTH 2 +#define CRC16_START_VALUE 0xFFFF // Start value CRC16 CCITT +#define NOF_OBJECTS 127 // Number of Object UMRR-0C +#define ROAD_ANGLE 120 + + +#define RADAR_IP "11.11.11.13" // ITS-S interface connected to the radar +#define RADAR_PORT "55555" // Destination port from the radar + + +/* Structures */ + +typedef struct +{ + int i32_socket; // Socket descriptor server + int i32_client; // Socket descriptor UMRR-0C + int i32_port; // listen on Port + bool b_moxa; + struct sockaddr_in s_server; + struct sockaddr_in s_client; +} S_ETHERNET_CONNECTION_T; + +typedef struct +{ + int i32_fdSerial; //Serial Port descriptor + char c_portNumber; + struct termios s_config; +} S_SERIAL_T; + +typedef struct +{ + u_int8_t u8_numberOfCountedObjects; + u_int32_t u32_UnixTime; + u_int16_t u16_Milliseconds; + u_int8_t u8_SensorNetworkID; + u_int8_t u8_ObjectNumber; + u_int8_t u8_ObjectID; + int16_t i16_speed; + u_int8_t u8_class; + u_int8_t u8_mLineNumber; + u_int16_t u8_laneNumber; +} S_PVR_T; + +typedef struct +{ + u_int8_t u8_devid; + u_int8_t u8_partyp; + u_int8_t u8_parno; + int32_t i32_value; + float f_value; + u_int8_t u8_action; + u_int8_t u8_xor; + u_int8_t au8_can_cmd[CAN_BUFFER_SIZE]; + u_int8_t au8_can_cmd_SMS_TRANSPORT_PROTOCOL[CAN_SMS_BUFFER_SIZE]; +} S_COMMAND_T; + +typedef struct +{ + u_int16_t u16_startPattern; + u_int8_t u8_protocolVersion; + u_int8_t u8_headerLength; + u_int16_t u16_payloadLength; + u_int8_t u8_AppProtocolType; + u_int32_t u32_flags; + u_int16_t u16_headerCRC16; + u_int8_t au8_Payload[1024]; + u_int8_t au8_Header[1024]; + u_int16_t u16_PayloadCRC16; + u_int16_t u16_indexHeader; + u_int16_t u16_indexPayload; +} S_SMS_DATA_FRAME_T; + +typedef struct +{ + u_int8_t u8_sensorStatus; + u_int8_t u8_InterfaceMode; + u_int8_t u8_networkID; + u_int8_t u8_diagnose; + u_int32_t u32_time; +} S_SENSOR_CONTROL_T; + +typedef struct +{ + u_int8_t u8_numberOfObjects; + u_int8_t u8_numberOfMessages; + u_int8_t u8_cycleDuration; + u_int8_t u8_objectData0Format; + u_int8_t u8_objectData1Format; + u_int32_t u32_cycleCount; +} S_OBJECT_CONTROL_T; + +typedef struct +{ + u_int8_t u8_modeSignal; + float f_xPoint; + float f_yPoint; + float f_xSpeed; + float f_ySpeed; + float f_objectLength; + u_int8_t u8_objectID; + u_int8_t u8_updateFlag; +} S_OBJECTS_T; + + +enum state_t { + IDLE, + START_1, + START_2, + START_3, + START_4, + START_PATTERN, + PROTOCOLVERSION, + HEADERLENGTH, + PAYLOADLENGTH_H, + PAYLOADLENGTH_L, + APPPROTOCOLTYPE, + FLAG_1_H, + FLAG_1_L, + FLAG_2_H, + FLAG_2_L, + CRC16HEADER_H, + CRC16HEADER_L, + PAYLOAD_DATA, + CAN_ID_H, + CAN_ID_L, + CAN_LEN, + CAN_PAYLOAD, + PAYLOAD_CRC16_H, + PAYLOAD_CRC16_L +}; + +typedef struct +{ + + bool active; + + pthread_mutex_t lock; + + uint8_t type; + + uint64_t next_cpm_max; + uint64_t next_cpm_min; + + + + uint64_t T_GenCpmMin; + uint64_t T_GenCpmMax; + +} dissemination_t; + +/* Prototype Functions */ + + +/* +Summary : Initialization for dissemination control. (Memory allocation and mutex init +*/ + +dissemination_t* dissemination_init(); + + +/* +Summary : Creation of socket and binding to the radar add and port + param [in] char* radar_ip: Radar IP + param [in] char* radar_port: Radar PORT +*/ + +bool initEthernetConnection(const char* radar_ip,const char* radar_port); + +/* +Summary: Read from Radar socket (s_socket.i32_socket) call the function to interpret data + param[in] void + +*/ + +void* cp_service(void* fc); + +#endif