From 258190395e06c82233b03aa4d48525b08334bff1 Mon Sep 17 00:00:00 2001 From: emanuel Date: Tue, 8 Feb 2022 18:03:18 +0000 Subject: [PATCH] Fix CPM objID going oob --- src/cpm.c | 1157 ++++++++++++++++++++++++++--------------------------- 1 file changed, 578 insertions(+), 579 deletions(-) diff --git a/src/cpm.c b/src/cpm.c index e67cd9c..8db8b85 100644 --- a/src/cpm.c +++ b/src/cpm.c @@ -46,102 +46,102 @@ 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)); + // Create temporary ifr struct and socket to + // check if the radar interface is running i.e if the + // radar has booted up - int sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_IP); + struct ifreq ifr; + memset(&ifr, 0, sizeof(ifr)); - 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"); + int sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_IP); - close(sock); + 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"); - return (ifr.ifr_flags & IFF_UP) && (ifr.ifr_flags & IFF_RUNNING); + 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){ + if(radar_ready(facilities) == 1){ + if(facilities->dissemination->tmc_connect == false){ - // Create TCP socket - s_socket.i32_socket = socket(AF_INET, SOCK_STREAM, 0); + // 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; - } + 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)); + // 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(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; + } - if(listen(s_socket.i32_socket,1)<0){ - syslog_err("Waiting for incoming requests failed..."); - return false; - } + // Listening to the socket (Waiting for incoming connection) + unsigned int len = sizeof(s_socket.s_client); - if((s_socket.i32_client = accept(s_socket.i32_socket, (struct sockaddr*)&s_socket.s_server, &len)) < 0){ - syslog_err("Client disconnected..."); - return false; - } + if(listen(s_socket.i32_socket,1)<0){ + syslog_err("Waiting for incoming requests failed..."); + return false; + } - syslog_debug("Radar connected"); + if((s_socket.i32_client = accept(s_socket.i32_socket, (struct sockaddr*)&s_socket.s_server, &len)) < 0){ + syslog_err("Client disconnected..."); + return false; + } - } - - // Create RAW socket + syslog_debug("Radar connected"); - 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; - } + // Create RAW socket - // Get interface index + raw_socket.raw_fd = socket(AF_PACKET, SOCK_RAW, htons(0x0800)); - bzero(&raw_socket.sll, sizeof(raw_socket.sll)); - bzero(&raw_socket.ifr, sizeof(raw_socket.ifr)); + if(raw_socket.raw_fd < 0){ + syslog_err("Failed to initializing RAW socket ..."); + return false; + } - 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; - } + // Get interface index - raw_socket.sll.sll_family = AF_PACKET; - raw_socket.sll.sll_ifindex = raw_socket.ifr.ifr_ifindex; - raw_socket.sll.sll_protocol = htons(0x0800); + bzero(&raw_socket.sll, sizeof(raw_socket.sll)); + bzero(&raw_socket.ifr, sizeof(raw_socket.ifr)); - // Bind it to the interface + 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; + } - if(bind(raw_socket.raw_fd, (struct sockaddr *)&raw_socket.sll, sizeof(raw_socket.sll))<0){ - syslog_err("Error binding RAW socket ..."); - 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; - } + return true; + }else{ + return false; + } } @@ -156,26 +156,26 @@ dissemination_t* dissemination_init(){ int dissemination_check(dissemination_t* dissemination, it2s_tender_epv_t* epv, int f) { int rv = 0; - + 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; - } + 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; - } + if (now >= dissemination->next_cpm_min){ + rv = 1; + }else if(now >= dissemination->next_cpm_max){ + rv = 0; + } } pthread_mutex_unlock(&dissemination->lock); @@ -188,579 +188,578 @@ void dissemination_reset_timer(dissemination_t* dissemination, it2s_tender_epv_t uint64_t now = it2s_tender_get_clock(epv); - /* Both cases for RSU and OBU */ + /* 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 */ + /* 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); + 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; + 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 + switch (u32_can_id) // Interpret the different types of CAN messages { - 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; + case 0x785: + s_pvr.u8_numberOfCountedObjects = 0; + s_pvr.u32_UnixTime = 0; + s_pvr.u16_Milliseconds = 0; + s_pvr.u8_SensorNetworkID = 0; - 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; + 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]; - 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; + s_pvr.u16_Milliseconds |= (au8_can_data[6] & 0x3) << 8; + s_pvr.u16_Milliseconds |= au8_can_data[5]; - 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; + s_pvr.u8_SensorNetworkID = (au8_can_data[6] & 0xC) >> 2; - as_objects[u16_objectIndex].f_objectLength = (au8_can_data[6] >> 1) * 0.2f; + // printf("Unix Time: %u, Ms: %u\n", s_pvr.u32_UnixTime, s_pvr.u16_Milliseconds); - 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; + long current = s_pvr.u32_UnixTime * 1000 + s_pvr.u16_Milliseconds; - 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; + //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; - 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; + s_pvr.u8_ObjectNumber = (au8_can_data[0] & 0xFE) >> 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; + s_pvr.u8_ObjectID = au8_can_data[1]; - as_objects[u16_objectIndex].f_objectLength = (au8_can_data[6] >> 1) * 0.2f; + 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 - as_objects[u16_objectIndex].u8_objectID = au8_can_data[7] & 0x7F; - as_objects[u16_objectIndex].u8_updateFlag = au8_can_data[7] >> 7; + 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] & 0x7F; + } 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; + 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; + 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; + 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; } - break; - case 1: - if (u8_input == 0xeb) { - i32_stop_search = 2; - } else { - i32_stop_search = 0; + + 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"); } - break; - case 2: - if (u8_input == 0xec) { - i32_stop_search = 3; - } else { - i32_stop_search = 0; + + 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:; } - 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); + return (long)(x * roadRotationCos - y * roadRotationSin); } long rotate_y(long x, long y) { - return (long)(x * roadRotationSin + y * roadRotationCos); + 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 + /* 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[as_objects[i].u8_objectID] = 1; // Comparation Array + history_list[as_objects[i].u8_objectID][0] = (long)as_objects[i].f_xPoint * 100; // xPoint (Distance) + history_list[as_objects[i].u8_objectID][1] = (long)as_objects[i].f_yPoint * 100; // yPoint (Distance) + history_list[as_objects[i].u8_objectID][2] = (long)as_objects[i].f_xSpeed * 100; // xSpeed (Speed) + history_list[as_objects[i].u8_objectID][3] = (long)as_objects[i].f_ySpeed * 100; // ySpeed (Speed) + history_timestamp[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]; - + /* Variables */ + CPM_t* cpm_tx = calloc(1, sizeof(CPM_t)); - 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++){ -syslog_err("oID = %d", as_objects[i].u8_objectID); - 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; + long euclidian_dist, abs_speed, abs_speed_hist, angle, angle_hist, angle_diff; + int j = 0, rv = 0; + int temp[NOF_OBJECTS]; - /******* 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; - } + 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); - *bdr_len = ((retval_enc_bdr.encoded + 7) / 8); + uint64_t generationDeltaTime = it2s_tender_get_clock(&facilities->epv) % 65536; // generationDeltaTime = TimestampIts mod 65 536 - //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; - } + 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); - *fdi_len = ((retval_enc_fdi.encoded + 7) / 8); + 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[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); + ASN_STRUCT_FREE(asn_DEF_CPM, cpm_tx); - return rv; + 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]; + /* 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; + long history_list[NOF_OBJECTS][4]; + int valid_array[NOF_OBJECTS]; + 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)); + 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; + 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; + 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; + 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); + 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 - + /*--- 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); + 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 - /* Reads from the radar */ - i32_recv_bytes = recv(raw_socket.raw_fd, &au8_readBuffer, READ_BUFFER_SIZE, 0); + 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 - 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; - } + bpr->gn.securityProfile.sign = true; - /* 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); - } - } - } + /*--- 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(s_socket.i32_socket,2); shutdown(raw_socket.raw_fd,2); return NULL;