#include "cpm.h" #include "facilities.h" #include #include #include #include #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_INTERFACE_CONNECTION_T raw_socket; S_OBJECT_CONTROL_T s_objectControl; S_OBJECTS_T as_objects[NOF_OBJECTS]; int radar_ready(facilities_t* facilities){ // Create temporary ifr struct and socket to // check if the radar interface is running i.e if the // radar has booted up struct ifreq ifr; memset(&ifr, 0, sizeof(ifr)); int sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_IP); strncpy(ifr.ifr_name, facilities->dissemination->int_radar, sizeof(ifr.ifr_name)); if(ioctl(sock, SIOCGIFFLAGS, &ifr) <0) syslog_err(" IOCTL failed, could not retrieve radar interface flags"); close(sock); return (ifr.ifr_flags & IFF_UP) && (ifr.ifr_flags & IFF_RUNNING); } bool radar_connection(char* radar_port, facilities_t* facilities){ if(radar_ready(facilities) == 1){ if(facilities->dissemination->tmc_connect == false){ // Create TCP socket s_socket.i32_socket = socket(AF_INET, SOCK_STREAM, 0); if(s_socket.i32_socket < 0){ syslog_err("Initializing socket failed ..."); return false; } // 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(facilities->dissemination->ip_radar); 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; } // 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"); } // Create RAW socket raw_socket.raw_fd = socket(AF_PACKET, SOCK_RAW, htons(0x0800)); if(raw_socket.raw_fd < 0){ syslog_err("Failed to initializing RAW socket ..."); return false; } // Get interface index bzero(&raw_socket.sll, sizeof(raw_socket.sll)); bzero(&raw_socket.ifr, sizeof(raw_socket.ifr)); strncpy((char *)raw_socket.ifr.ifr_name, facilities->dissemination->int_radar, IFNAMSIZ); if((ioctl(raw_socket.raw_fd, SIOCGIFINDEX, &raw_socket.ifr)) == -1){ syslog_err("Error getting interface index"); return false; } raw_socket.sll.sll_family = AF_PACKET; raw_socket.sll.sll_ifindex = raw_socket.ifr.ifr_ifindex; raw_socket.sll.sll_protocol = htons(0x0800); // Bind it to the interface if(bind(raw_socket.raw_fd, (struct sockaddr *)&raw_socket.sll, sizeof(raw_socket.sll))<0){ syslog_err("Error binding RAW socket ..."); return false; } return true; }else{ return false; } } 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, it2s_tender_epv_t* epv, int f) { int rv; uint64_t now = it2s_tender_get_clock(epv); pthread_mutex_lock(&dissemination->lock); // mutex is used to lock shared resources /* If f = 0 indicates that it is to check the Sensor Information Container timer * If f = 1 inidcates that it is to check the CPM generation */ if(f == 0){ if(now >= dissemination->next_AddSensorInformation){ rv = 1; }else{ rv = 0; } }else{ if (now >= dissemination->next_cpm_min){ rv = 1; }else if(now >= dissemination->next_cpm_max){ rv = 0; } } pthread_mutex_unlock(&dissemination->lock); return rv; } void dissemination_reset_timer(dissemination_t* dissemination, it2s_tender_epv_t* epv, int f){ uint64_t now = it2s_tender_get_clock(epv); /* Both cases for RSU and OBU */ /* If f = 0 indicates that the reset corresponds to the timer of the Sensor Information Container Inclusion If f = 1 indicates that the reset corresponds to the timer of the CPM generation */ pthread_mutex_lock(&dissemination->lock); if(f == 0){ dissemination->next_AddSensorInformation = now + dissemination->T_AddSensorInformation; }else{ dissemination->next_cpm_min = now + dissemination->T_GenCpmMin; dissemination->next_cpm_max = now + dissemination->T_GenCpmMax; } pthread_mutex_unlock(&dissemination->lock); } 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) { enum state_t s_state = IDLE; u_int8_t u8_input = 0; int i32_i; int i32_can_len_counter = 0, i32_can_len = 0, i32_can_id = 0, i32_stop_search = 0; unsigned char au8_can_data[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; 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); } static void set_values(int i, int j, uint64_t timestamp, CPM_t* cpm_tx, long history_list[NOF_OBJECTS][4], int valid_array[], uint64_t history_timestamp[]){ /* Fill CPM */ cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j] = calloc(1, sizeof(PerceivedObject_t)); cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->objectID = (long)as_objects[i].u8_objectID; cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->timeOfMeasurement = 0; //Sem informaçao do radar cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->objectConfidence = 95; cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->xDistance.value = rotate_x( (long)as_objects[i].f_xPoint * 100, (long)as_objects[i].f_yPoint * 100); cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->xDistance.confidence = 102; cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->yDistance.value = rotate_y( (long)as_objects[i].f_xPoint * 100, (long)as_objects[i].f_yPoint * 100); cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->yDistance.confidence = 102; cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->xSpeed.value = rotate_x( (long)as_objects[i].f_xSpeed * 100, (long)as_objects[i].f_ySpeed * 100); cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->xSpeed.confidence = 40; cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->ySpeed.value = rotate_y( (long)as_objects[i].f_xSpeed * 100, (long)as_objects[i].f_ySpeed * 100); cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->ySpeed.confidence = 40; cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->objectRefPoint = ObjectRefPoint_bottomMid; /* Fill History values */ valid_array[(long)as_objects[i].u8_objectID] = 1; // Comparation Array history_list[(long)as_objects[i].u8_objectID][0] = (long)as_objects[i].f_xPoint * 100; // xPoint (Distance) history_list[(long)as_objects[i].u8_objectID][1] = (long)as_objects[i].f_yPoint * 100; // yPoint (Distance) history_list[(long)as_objects[i].u8_objectID][2] = (long)as_objects[i].f_xSpeed * 100; // xSpeed (Speed) history_list[(long)as_objects[i].u8_objectID][3] = (long)as_objects[i].f_ySpeed * 100; // ySpeed (Speed) history_timestamp[(long)as_objects[i].u8_objectID] = timestamp; // Time stamp of detected object } static int mk_cpm(facilities_t* facilities, uint8_t *bdr_oer, uint32_t *bdr_len, uint8_t *fdi_oer, uint32_t *fdi_len, long history_list[NOF_OBJECTS][4], int valid_array[], uint64_t history_timestamp[]) { /* Variables */ CPM_t* cpm_tx = calloc(1, sizeof(CPM_t)); long euclidian_dist, abs_speed, abs_speed_hist, angle, angle_hist, angle_diff; int j = 0, rv = 0; int temp[NOF_OBJECTS]; cpm_tx->header.protocolVersion = PROTOCOL_VERSION; cpm_tx->header.messageID = MESSAGE_ID; pthread_mutex_lock(&facilities->id.lock); cpm_tx->header.stationID = facilities->id.station_id; pthread_mutex_unlock(&facilities->id.lock); uint64_t generationDeltaTime = it2s_tender_get_clock(&facilities->epv) % 65536; // generationDeltaTime = TimestampIts mod 65 536 int32_t lat, lon, alt, alt_conf; pthread_mutex_lock(&facilities->epv.space.lock); it2s_tender_get_space(&facilities->epv); lat = facilities->epv.space.latitude; lon = facilities->epv.space.longitude; alt = facilities->epv.space.altitude; alt_conf = facilities->epv.space.altitude_conf; pthread_mutex_unlock(&facilities->epv.space.lock); cpm_tx->cpm.generationDeltaTime = generationDeltaTime; cpm_tx->cpm.cpmParameters.managementContainer.stationType = StationType_roadSideUnit; cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.latitude = lat; cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.longitude = lon; cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.positionConfidenceEllipse.semiMajorConfidence = 100; // TODO cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.positionConfidenceEllipse.semiMinorConfidence = 100; // TODO cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.positionConfidenceEllipse.semiMajorOrientation = HeadingValue_wgs84North; // TODO cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.altitude.altitudeValue = alt; cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.altitude.altitudeConfidence = alt_conf; if(dissemination_check(facilities->dissemination,&facilities->epv,0) == 1){ /* Sensor Information Container Inclusion Management */ 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; dissemination_reset_timer(facilities->dissemination, &facilities->epv, 0); } cpm_tx->cpm.cpmParameters.perceivedObjectContainer = calloc(1, sizeof(PerceivedObjectContainer_t)); cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array = calloc(s_objectControl.u8_numberOfObjects,sizeof(PerceivedObject_t*)); cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.count = s_objectControl.u8_numberOfObjects; memcpy(temp, valid_array, NOF_OBJECTS * sizeof(int)); // NOF_OBJECTS * sizeof(int) = size of valid_array memset(valid_array, 0, NOF_OBJECTS * sizeof(int)); for(int i = 0; i < cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.count;i++){ if(temp[(long)as_objects[i].u8_objectID] == 0 ){ // The object is going to be added without comparison (It is a new object) (valid_array[id] = 0) set_values(i,j,generationDeltaTime,cpm_tx,history_list,valid_array,history_timestamp); j++; }else{ // The object is going to be compared (It was included in previous CPMs) (valid_array[id] = 1) // Getting the euclidian distance value from the object detected and the same object in the last cpm (xcurrent - xhistory)^2 + (ycurrent - yhistory)^2 euclidian_dist = sqrt( pow(((long)as_objects[i].f_xPoint * 100) - (history_list[(long)as_objects[i].u8_objectID][0]), 2) + pow(((long)as_objects[i].f_yPoint * 100) - (history_list[(long)as_objects[i].u8_objectID][1]) ,2) ); // Getting the absolute speed value from the object detected and the same object in the last cpm (sqrt(x^2 + y^2)) abs_speed = sqrt( pow( ((long)as_objects[i].f_xSpeed * 100),2) + pow( ( (long)as_objects[i].f_ySpeed * 100),2) ); abs_speed_hist = sqrt( pow( history_list[(long)as_objects[i].u8_objectID][2] ,2) + pow( history_list[(long)as_objects[i].u8_objectID][3],2) ); // sqrt(xSpeed^2 + ySpeed^2) // Getting the angle from the velocity vector detected and the same object in the last cpm angle = (long)((180 / PI) * atan2( (long)as_objects[i].f_ySpeed * 100 , (long)as_objects[i].f_xSpeed * 100 )); angle_hist = (long)((180 / PI) * atan2( history_list[(long)as_objects[i].u8_objectID][3] , history_list[(long)as_objects[i].u8_objectID][2]) ); // tan(yspeed / xspeed) angle_diff = ((angle - angle_hist) + 180) % 360 - 180; // Requirements to include the object in the CPM (> 4 m or > 0.5 m/s or > 4º or > T_GenCpmMax) if(abs(euclidian_dist) > 400 || abs(abs_speed - abs_speed_hist) > 50 || abs(angle_diff) > 4 || abs(generationDeltaTime - history_timestamp[i]) >= facilities->dissemination->T_GenCpmMax){ set_values(i,j,generationDeltaTime,cpm_tx, history_list, valid_array,history_timestamp); j++; }else{ //The object is not included but is valid for comparison in the upcoming CPMs valid_array[(long)as_objects[i].u8_objectID] = 1; } } } cpm_tx->cpm.cpmParameters.numberOfPerceivedObjects = cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.count; // Object perceived by the Radar (Does not have to match the objects included in the CPM) cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.count = j; // The number of objects that were included in the CPM cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.size = j; cpm_tx->cpm.cpmParameters.numberOfPerceivedObjects = j; /******* Encode CPMs to FDI and BDR ********/ //BDR memset(bdr_oer, 0, 1500); asn_enc_rval_t retval_enc_bdr = uper_encode_to_buffer(&asn_DEF_CPM, NULL, cpm_tx, bdr_oer, 1500); if (retval_enc_bdr.encoded == -1) { syslog_err("[facilities] [cp] failed encoding CPM (%s)", retval_enc_bdr.failed_type->name); rv = 1; goto cleanup; } *bdr_len = ((retval_enc_bdr.encoded + 7) / 8); //FDI memset(fdi_oer, 0, 1500); asn_enc_rval_t retval_enc_fdi = uper_encode_to_buffer(&asn_DEF_CPM, NULL, cpm_tx, fdi_oer, 1500); if (retval_enc_fdi.encoded == -1) { syslog_err("[facilities] [cp] failed encoding CPM (%s)", retval_enc_fdi.failed_type->name); rv = 1; goto cleanup; } *fdi_len = ((retval_enc_fdi.encoded + 7) / 8); cleanup: ASN_STRUCT_FREE(asn_DEF_CPM, cpm_tx); return rv; } void *cp_service(void *fc){ /* Variables */ int i32_recv_bytes; u_int8_t au8_readBuffer[READ_BUFFER_SIZE]; u_int8_t au8_readTcp[READ_BUFFER_SIZE]; bool is_radar_connected; static long history_list[NOF_OBJECTS][4]; static int valid_array[NOF_OBJECTS]; static uint64_t history_timestamp[NOF_OBJECTS]; facilities_t *facilities = (facilities_t *) fc; memset(history_list, 0, sizeof(history_list)); memset(valid_array, 0, sizeof(valid_array)); memset(history_timestamp, 0, sizeof(history_timestamp)); uint8_t tr_oer[2048]; uint8_t fi_oer[2048]; tr_oer[0] = 4; //Facilities fi_oer[0] = 4; TransportRequest_t* tr = calloc(1, sizeof(TransportRequest_t)); tr->present = TransportRequest_PR_packet; TransportPacketRequest_t* tpr = &tr->choice.packet; tpr->present = TransportPacketRequest_PR_btp; BTPPacketRequest_t *bpr = &tpr->choice.btp; FacilitiesIndication_t* fi = calloc(1, sizeof(FacilitiesIndication_t)); fi->present = FacilitiesIndication_PR_message; FacilitiesMessageIndication_t* fmi = &fi->choice.message; roadRotationSin = sin(((facilities->dissemination->radar_rotation + 90.0) * PI) / 180); roadRotationCos = cos(((facilities->dissemination->radar_rotation + 90.0) * PI) / 180); /*--- Fill mandatory BTP Data Request parameters ---*/ bpr->gn.destinationAddress.buf = malloc(6); bpr->gn.destinationAddress.size = 6; for(int i = 0; i < 6; i++) bpr->gn.destinationAddress.buf[i] = 0xff; //Broadcast addr bpr->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 bpr->destinationPort = Port_cpm; //CPM entity port for communication between Facilities and Transport bpr->gn.packetTransportType = PacketTransportType_shb; //shb = Single Hop Broadcast packet bpr->gn.trafficClass = 2; //Identifier assigned to a GeoNetworking packet that expresses its requirements on data transport bpr->data.buf = malloc(1500); //CPM Data to be sent to the Transport layer bpr->gn.securityProfile.sign = true; /*--- Fill mandatory Facilities Message Indication parameters ---*/ fmi->itsMessageType = ItsMessageType_cpm; fmi->data.buf = malloc(1500); /* Creating sockets and waiting for radar to connect*/ is_radar_connected = radar_connection(RADAR_PORT,facilities); while(!facilities->exit){ usleep(1000*50); /* If the Radar is not connected to TMC, a TCP socket is needed to fool the Radar */ /* To maintain the connection the content must be read */ if(facilities->dissemination->tmc_connect == false) i32_recv_bytes = recv(s_socket.i32_client, &au8_readTcp, READ_BUFFER_SIZE, 0); /* Reads from the radar */ i32_recv_bytes = recv(raw_socket.raw_fd, &au8_readBuffer, READ_BUFFER_SIZE, 0); if (dissemination_check(facilities->dissemination, &facilities->epv,1) && facilities->dissemination->active){ if(is_radar_connected){ /* Information parsing from radar */ parse_input(au8_readBuffer,i32_recv_bytes); /* CPM build and encoding to BDR and FDI */ if(mk_cpm(facilities, bpr->data.buf, (uint32_t *) &bpr->data.size, fmi->data.buf, (uint32_t *) &fmi->data.size, history_list, valid_array, history_timestamp) == 1) continue; /* Encode TransportRequest */ asn_enc_rval_t enc_tdr = oer_encode_to_buffer(&asn_DEF_TransportRequest, NULL, tr, tr_oer+1, 2047); if(enc_tdr.encoded == -1){ syslog_err("[facilities] encoding TR for cpm failed"); continue; } /* Encode FacilitiesIndication */ asn_enc_rval_t enc_fdi = oer_encode_to_buffer(&asn_DEF_FacilitiesIndication, NULL, fi, fi_oer+1, 2047); if(enc_fdi.encoded == -1){ syslog_err("[facilities] encoding FI for cpm failed"); continue; } /* Create thread to send packet to the Transport Layer (=3) */ queue_add(facilities->tx_queue, tr_oer, enc_tdr.encoded+1, 3); pthread_cond_signal(&facilities->tx_queue->trigger); /* Create thread to send packet to the Applications Layer (=5) */ queue_add(facilities->tx_queue, fi_oer, enc_fdi.encoded+1, 5); pthread_cond_signal(&facilities->tx_queue->trigger); /*Reset Timer for dissemination control */ dissemination_reset_timer(facilities->dissemination, &facilities->epv,1); }else{ is_radar_connected = radar_connection(RADAR_PORT,facilities); } } } ASN_STRUCT_FREE(asn_DEF_TransportRequest, tr); ASN_STRUCT_FREE(asn_DEF_FacilitiesIndication, fi); /* Close sockets */ if(facilities->dissemination->tmc_connect) shutdown(s_socket.i32_socket,2); shutdown(raw_socket.raw_fd,2); return NULL; }