diff --git a/backup/src/CMakeLists.txt b/backup/src/CMakeLists.txt new file mode 100644 index 0000000..3fa9939 --- /dev/null +++ b/backup/src/CMakeLists.txt @@ -0,0 +1,51 @@ +ADD_EXECUTABLE(it2s-itss-facilities + config.c + cam.c + denm.c + infrastructure.c + requests.c + indications.c + facilities.c + cpm.c + saem.c + tpm.c + vcm.c + evm.c + edm.c + mcm.c +) + +TARGET_LINK_LIBRARIES(it2s-itss-facilities + -lit2s-asn-etsi-its-sdu-itss-facilities + -lit2s-asn-etsi-its-sdu-itss-security + -lit2s-asn-etsi-its-sdu-itss-management + -lit2s-asn-etsi-its-sdu-itss-networking + -lit2s-asn-etsi-its-sdu-cdd-1.3.1 + -lzmq + -lit2s_gnss + -lpthread + -lit2s-config-etsi-its + -lit2s-asn-etsi-its-v1-cdd-1.3.1 + -lit2s-asn-etsi-its-v1-cam + -lit2s-asn-etsi-its-v1-ivim + -lit2s-asn-etsi-its-v1-denm + -lit2s-asn-etsi-its-v1-cpm + -lit2s-asn-etsi-its-v1-saem + -lit2s-asn-etsi-its-v1-tpm + -lit2s-asn-etsi-its-v1-vcm + -lit2s-asn-etsi-its-v1-evcsnm + -lit2s-asn-etsi-its-v1-evrsrm + -lit2s-asn-etsi-its-v2-cdd-2.2.1 + -lit2s-asn-etsi-its-v2-cam + -lit2s-asn-etsi-its-v2-denm + -lit2s-asn-etsi-its-v2-mcm + -lit2s-tender + -lm + -lrt +) + +INSTALL( + TARGETS + it2s-itss-facilities + RUNTIME DESTINATION bin +) diff --git a/backup/src/config.c b/backup/src/config.c new file mode 100644 index 0000000..2e8d625 --- /dev/null +++ b/backup/src/config.c @@ -0,0 +1,590 @@ +#include "config.h" +#include "facilities.h" +#include "saem.h" +#include "tpm.h" +#include "vcm.h" +#include "mcm.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +static int fetch_target_address(char** addresses, uint16_t addresses_len) { + int index = -1; + + for (int i = 0; i < addresses_len; ++i) { + char* addr = addresses[i]; + + // If TCP, ignore receiver addresses, e.g. tcp://*:[port] + if (!memcmp(addr, "tcp", 3)) { + bool found_astk = false; + for (int j = 0; j < strlen(addr); ++j) { + if (addr[j] == '*') { + found_astk = true; + break; + } + } + + if (found_astk) continue; + } + + index = i; + break; + } + + return index; +} + +int facilities_config() { + int rv = 0; + + t2c_itss_t* itss_cfg = NULL; + t2c_etsi_its_t* etsi_its_cfg = NULL; + + rv = t2c_itss_read("/etc/it2s/itss.toml", &itss_cfg); + if (rv) { + syslog_error("[config] read ITS-S config failed"); + goto cleanup; + } + rv = t2c_etsi_its_read("/etc/it2s/etsi-its.toml", &etsi_its_cfg); + if (rv) { + syslog_error("[config] read ETSI ITS config failed"); + goto cleanup; + } + + // Logging - status messages + bool use_syslog = false, use_std = false, use_file = false; + for (int i = 0; i < itss_cfg->general.logging.status_len; ++i) { + if (!strcmp(itss_cfg->general.logging.status[i], "syslog")) { + use_syslog = true; + } else if (!strcmp(itss_cfg->general.logging.status[i], "std")) { + use_std = true; + } else if (!strcmp(itss_cfg->general.logging.status[i], "file")) { + use_file = true; + } + } + log_init(use_syslog, use_std, use_file, "facilities", NULL); + + itss_0init(&facilities.exit); + + struct timeval time; + gettimeofday(&time,NULL); + srand((time.tv_sec * 1000) + (time.tv_usec / 1000)); + + facilities.zmq.responders = calloc(etsi_its_cfg->facilities.zmq.addresses_len + 1, sizeof(zmq_pollitem_t)); + facilities.zmq.n_responders = 1; + + // Internal socket + void* socket = itss_0bind(ZMQ_INTERNAL_ADDR, ZMQ_PAIR); + facilities.zmq.responders[0].socket = socket; + facilities.zmq.responders[0].events = ZMQ_POLLIN; + + for (int i = 0; i < etsi_its_cfg->facilities.zmq.addresses_len; ++i) { + char* addr = etsi_its_cfg->facilities.zmq.addresses[i]; + + // IPC + if (!memcmp(addr, "ipc", 3)) { + + // Create dir + int lp = 0; + for (int j = 0; j < strlen(addr); ++j) { + if (addr[j] == '/') lp = j; + } + + char dir[256]; + memcpy(dir, addr+6, lp-6); + dir[lp-6] = 0; + + struct stat st = {0}; + if (stat(dir, &st) == -1) { + mkdir(dir, 0777); + } + + // Bind + void* socket = itss_0bind(addr, ZMQ_REP); + + facilities.zmq.responders[facilities.zmq.n_responders].socket = socket; + facilities.zmq.responders[facilities.zmq.n_responders].events = ZMQ_POLLIN; + ++facilities.zmq.n_responders; + + } else if (!memcmp(addr, "tcp", 3)) { + + bool found_astk = false; + for (int j = 0; j < strlen(addr); ++j) { + if (addr[j] == '*') { + found_astk = true; + break; + } + } + + if (found_astk) { + // Bind + void* socket = itss_0bind(addr, ZMQ_REP); + + facilities.zmq.responders[facilities.zmq.n_responders].socket = socket; + facilities.zmq.responders[facilities.zmq.n_responders].events = ZMQ_POLLIN; + ++facilities.zmq.n_responders; + } + } + } + if (facilities.zmq.n_responders <= 1) { + log_info("[config] a valid address to listen to was not found, exiting now"); + rv = 1; + goto cleanup; + } + + // Fetch [networking] address + int index = fetch_target_address(etsi_its_cfg->networking.zmq.addresses, etsi_its_cfg->networking.zmq.addresses_len); + if (index != -1) { + facilities.zmq.networking_address = malloc(strlen(etsi_its_cfg->networking.zmq.addresses[index])+1); + strcpy(facilities.zmq.networking_address, etsi_its_cfg->networking.zmq.addresses[index]); + } else { + log_error("[config] a valid address for [networking] was not found"); + rv = 1; + goto cleanup; + } + + + // Fetch [applications] address + index = fetch_target_address(itss_cfg->applications.zmq.addresses, itss_cfg->applications.zmq.addresses_len); + if (index != -1) { + facilities.zmq.applications_address = malloc(strlen(itss_cfg->applications.zmq.addresses[index])+1); + strcpy(facilities.zmq.applications_address, itss_cfg->applications.zmq.addresses[index]); + } else { + log_error("[config] a valid address for [applications] was not found"); + rv = 1; + goto cleanup; + } + + // Fetch [security] address + index = fetch_target_address(itss_cfg->security.zmq.addresses, itss_cfg->security.zmq.addresses_len); + if (index != -1) { + facilities.zmq.security_address = malloc(strlen(itss_cfg->security.zmq.addresses[index])+1); + strcpy(facilities.zmq.security_address, itss_cfg->security.zmq.addresses[index]); + } else { + log_error("[config] a valid address for [security] was not found"); + rv = 1; + goto cleanup; + } + + // Fetch [management] address + index = fetch_target_address(etsi_its_cfg->management.zmq.addresses, etsi_its_cfg->management.zmq.addresses_len); + if (index != -1) { + facilities.zmq.management_address = malloc(strlen(etsi_its_cfg->management.zmq.addresses[index])+1); + strcpy(facilities.zmq.management_address, etsi_its_cfg->management.zmq.addresses[index]); + } else { + log_error("[config] a valid address for [management] was not found"); + rv = 1; + goto cleanup; + } + + // Values + // General + if (!strcmp("obu", itss_cfg->general.itss_type)) { + facilities.station_type = 5; + } else if (!strcmp("rsu", itss_cfg->general.itss_type)) { + facilities.station_type = 15; + } else if (!strcmp("unknown", itss_cfg->general.itss_type)) { + facilities.station_type = 0; + } else if (!strcmp("pedestrian", itss_cfg->general.itss_type)) { + facilities.station_type = 1; + } else if (!strcmp("cyclist", itss_cfg->general.itss_type)) { + facilities.station_type = 2; + } else if (!strcmp("moped", itss_cfg->general.itss_type)) { + facilities.station_type = 3; + } else if (!strcmp("motorcycle", itss_cfg->general.itss_type)) { + facilities.station_type = 4; + } else if (!strcmp("passengerCar", itss_cfg->general.itss_type)) { + facilities.station_type = 5; + } else if (!strcmp("bus", itss_cfg->general.itss_type)) { + facilities.station_type = 6; + } else if (!strcmp("lightTruck", itss_cfg->general.itss_type)) { + facilities.station_type = 7; + } else if (!strcmp("heavyTruck", itss_cfg->general.itss_type)) { + facilities.station_type = 8; + } else if (!strcmp("trailer", itss_cfg->general.itss_type)) { + facilities.station_type = 9; + } else if (!strcmp("specialVehicles", itss_cfg->general.itss_type)) { + facilities.station_type = 10; + } else if (!strcmp("tram", itss_cfg->general.itss_type)) { + facilities.station_type = 11; + } else if (!strcmp("roadSideUnit", itss_cfg->general.itss_type)) { + facilities.station_type = 15; + } else { + log_error("[config] unrecognized ITS-S type, running as OBU"); + facilities.station_type = 5; + } + + + facilities.use_security = itss_cfg->security.use_security; + + pthread_mutex_init(&facilities.id.lock, NULL); + pthread_mutex_init(&facilities.id.change.lock, NULL); + + facilities.id.change.random = itss_cfg->security.identity.random; + if (facilities.id.change.random) { + // Ask [security] for station id + + EIS_SecurityRequest_t* sREQ = calloc(1, sizeof(EIS_SecurityRequest_t)); + EIS_SecurityReply_t* sREP = NULL; + + sREQ->present = EIS_SecurityRequest_PR_ids; + sREQ->choice.ids.list.count = 2; + sREQ->choice.ids.list.size = 2*sizeof(void*); + sREQ->choice.ids.list.array = malloc(2*sizeof(void*)); + sREQ->choice.ids.list.array[0] = calloc(1, sizeof(EIS_SecurityIdType_t)); + *sREQ->choice.ids.list.array[0] = EIS_SecurityIdType_stationId; + sREQ->choice.ids.list.array[1] = calloc(1, sizeof(EIS_SecurityIdType_t)); + *sREQ->choice.ids.list.array[1] = EIS_SecurityIdType_ipv6Address; + + uint8_t b_tx[256], b_rx[256]; + asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_EIS_SecurityRequest, NULL, sREQ, b_tx, 256); + + void* ss = itss_0connect(facilities.zmq.security_address, ZMQ_REQ); + itss_0send(ss, b_tx, enc.encoded); + int rl = itss_0recv_rt(&ss, b_rx, 256, b_tx, 256, -1); + itss_0close(ss); + if (rl == -1) { + rv = 1; + goto cleanup; + } + + asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_EIS_SecurityReply, (void**) &sREP, b_rx, 256); + + if (sREP->returnCode == EIS_SecurityReplyReturnCode_rejected) { + // TODO handle it + goto cleanup; + } + + for (int i = 0; i < sREP->data->choice.ids.list.count; ++i) { + switch (sREP->data->choice.ids.list.array[i]->present) { + case EIS_SecurityId_PR_stationId: + facilities.id.station_id = sREP->data->choice.ids.list.array[i]->choice.stationId; + break; + case EIS_SecurityId_PR_ipv6Address: + memcpy(facilities.id.ipv6_addr, sREP->data->choice.ids.list.array[i]->choice.ipv6Address.buf, 16); + break; + default: + break; + } + } + + ASN_STRUCT_FREE(asn_DEF_EIS_SecurityRequest, sREQ); + ASN_STRUCT_FREE(asn_DEF_EIS_SecurityReply, sREP); + + } else { + facilities.id.station_id = itss_cfg->security.identity.station_id; + + uint8_t src_mac[6]; + unsigned int tmp_uint[6]; + + sscanf(itss_cfg->security.identity.mac_address, "%02x:%02x:%02x:%02x:%02x:%02x", + &tmp_uint[0], + &tmp_uint[1], + &tmp_uint[2], + &tmp_uint[3], + &tmp_uint[4], + &tmp_uint[5]); + + src_mac[0] = (uint8_t)tmp_uint[0]; + src_mac[1] = (uint8_t)tmp_uint[1]; + src_mac[2] = (uint8_t)tmp_uint[2]; + src_mac[3] = (uint8_t)tmp_uint[3]; + src_mac[4] = (uint8_t)tmp_uint[4]; + src_mac[5] = (uint8_t)tmp_uint[5]; + + memset(facilities.id.ipv6_addr, 0, 8); + memcpy(facilities.id.ipv6_addr+8, src_mac, 3); + facilities.id.ipv6_addr[11] = 0xff; + facilities.id.ipv6_addr[12] = 0xfe; + memcpy(facilities.id.ipv6_addr+13, src_mac+3, 3); + facilities.id.ipv6_addr[8] ^= 0x02; + } + + // Message version + if (!strcmp("v1", etsi_its_cfg->facilities.mver.default_version)) { + log_error("[config] only default message version 2 is supported"); + return -1; + } else if (!strcmp("v2", etsi_its_cfg->facilities.mver.default_version)) { + facilities.pver.defaultv = 2; + } else { + log_warn("[config] unrecognized default messages version :: using version 2"); + facilities.pver.defaultv = 2; + } + + // DENM + facilities.den.n_max_events = etsi_its_cfg->facilities.denm.nmax_active_events; + + // CAM + facilities.lightship.active = etsi_its_cfg->facilities.cam.activate; + facilities.lightship.vehicle_gen_min = etsi_its_cfg->facilities.cam.obu_period_min; + facilities.lightship.vehicle_gen_max = etsi_its_cfg->facilities.cam.obu_period_max; + facilities.lightship.rsu_gen_min = etsi_its_cfg->facilities.cam.rsu_period_min; + facilities.lightship.rsu_vehicle_permanence = etsi_its_cfg->facilities.cam.rsu_vehicle_permanence; + + // IVIM + facilities.infrastructure.n_max_services = etsi_its_cfg->facilities.ivim.nmax_active_services; + facilities.infrastructure.replay_interval = etsi_its_cfg->facilities.ivim.replay_interval; + facilities.infrastructure.default_service_duration = etsi_its_cfg->facilities.ivim.default_service_duration * 60000; + + // CPM + facilities.dissemination.active = etsi_its_cfg->facilities.cpm.activate; + facilities.dissemination.T_GenCpmMin = etsi_its_cfg->facilities.cpm.rsu_obu_period_min; + facilities.dissemination.T_GenCpmMax = etsi_its_cfg->facilities.cpm.rsu_obu_period_max; + // Rotation not needed anymore? + //facilities.dissemination.radar_rotation = itss_cfg->radar.radar_rotation; + facilities.dissemination.tmc_connect = etsi_its_cfg->facilities.cpm.tmc_connected; + facilities.dissemination.T_AddSensorInformation = 1000; + int oa_start = (360-facilities.dissemination.radar_rotation) * 10 - 500; + int oa_end = (360-facilities.dissemination.radar_rotation) * 10 + 500; + facilities.dissemination.opening_angle_start = (oa_start + 3600) % 3600; + facilities.dissemination.opening_angle_end = (oa_end + 3600) % 3600; + + facilities.dissemination.int_radar = malloc(strlen(etsi_its_cfg->facilities.cpm.radar_interface)+1); + strcpy(facilities.dissemination.int_radar,etsi_its_cfg->facilities.cpm.radar_interface); + facilities.dissemination.ip_radar = malloc(strlen(etsi_its_cfg->facilities.cpm.radar_ip)+1); + strcpy(facilities.dissemination.ip_radar,etsi_its_cfg->facilities.cpm.radar_ip); + + // TPM + facilities.tolling.enabled = etsi_its_cfg->facilities.tpm.activate; + if (!strcmp("gn-spki", etsi_its_cfg->facilities.tpm.protocol)) { + facilities.tolling.protocol.p = TOLLING_PROTOCOL_GN_SPKI; + } else if (!strcmp("gn-dpki", etsi_its_cfg->facilities.tpm.protocol)) { + facilities.tolling.protocol.p = TOLLING_PROTOCOL_GN_DPKI; + } else if (!strcmp("tls", etsi_its_cfg->facilities.tpm.protocol)) { + facilities.tolling.protocol.p = TOLLING_PROTOCOL_TLS; + } else if (!strcmp("tls-gn", etsi_its_cfg->facilities.tpm.protocol)) { + facilities.tolling.protocol.p = TOLLING_PROTOCOL_TLS_GN; + } else if (!strcmp("tls-shs", etsi_its_cfg->facilities.tpm.protocol)) { + facilities.tolling.protocol.p = TOLLING_PROTOCOL_TLS_SHS; + } else { + log_error("[config] unrecognized tolling protocol, defaulting to 'gn-spki'"); + facilities.tolling.protocol.p = TOLLING_PROTOCOL_GN_SPKI; + } + facilities.tolling.station.obu.client_id = etsi_its_cfg->facilities.tpm.client_id; + + // MCM + if (!strcmp("mcm", etsi_its_cfg->facilities.mcm.protocol)){ + facilities.mcm_coord.protocol = MC_PROTOCOL_MCM; + facilities.mcm_coord.active = etsi_its_cfg->facilities.mcm.activate; + facilities.coordination.active = false; + } + else if (!strcmp("vcm-RR", etsi_its_cfg->facilities.mcm.protocol)){ + facilities.coordination.protocol = MC_PROTOCOL_VCM_RR; + facilities.coordination.active = etsi_its_cfg->facilities.mcm.activate; + facilities.mcm_coord.active = false; + } + else if (!strcmp("vcm-RR1C", etsi_its_cfg->facilities.mcm.protocol)){ + facilities.coordination.protocol = MC_PROTOCOL_VCM_RR1C; + facilities.coordination.active = etsi_its_cfg->facilities.mcm.activate; + facilities.mcm_coord.active = false; + } + else if (!strcmp("vcm-RRAC", etsi_its_cfg->facilities.mcm.protocol)){ + facilities.coordination.protocol = MC_PROTOCOL_VCM_RRAC; + facilities.coordination.active = etsi_its_cfg->facilities.mcm.activate; + facilities.mcm_coord.active = false; + } + else { + log_error("[config] unrecognized mcm protocol"); + facilities.coordination.active = false; + facilities.mcm_coord.active = false; + } + + // EVCSNM + facilities.evm_args.activate = etsi_its_cfg->facilities.evcsnm.activate; + + // Replay + facilities.replay = etsi_its_cfg->networking.replay.activate; + + // Forward unknown packets (in-stack) + facilities.upf = etsi_its_cfg->general.unknown_packet_forwarding; + + // PZ + if (facilities.station_type == 15) { + + int i = 0; + + DIR *d = opendir(etsi_its_cfg->facilities.protected_zones.path); + struct dirent *dir; + char file[256]; + char pz_xml[2048]; + if (d) { + while ((dir = readdir(d)) != NULL && i < 16) { + if (dir->d_name[0] == '.') continue; + sprintf(file, "%s/%s", etsi_its_cfg->facilities.protected_zones.path, dir->d_name); + FILE *fp = fopen(file, "r"); + if (!fp) continue; + fseek(fp, 0, SEEK_END); + uint16_t size = ftell(fp); + fseek(fp, 0, SEEK_SET); + if (!size) { + fclose(fp); + continue; + } + if (!fread(pz_xml, 1, size, fp)) { + fclose(fp); + continue; + } + fclose(fp); + + EIS_ProtectedCommunicationZone_t *zone = calloc(1, sizeof(EIS_ProtectedCommunicationZone_t)); + + asn_dec_rval_t dec = xer_decode(NULL, &asn_DEF_EIS_ProtectedCommunicationZone, (void**) &zone, pz_xml, size); + if (!dec.code) { + facilities.lightship.protected_zones.pz[i] = (EI1_ProtectedCommunicationZone_t*)zone; + ++facilities.lightship.protected_zones.pz_len; + ++i; + log_debug("[config] loaded protection zone @ (%lld, %lld)", zone->protectedZoneLatitude, zone->protectedZoneLongitude); + } else { + log_error("[config] failure to decode protection zone '%s'", dir->d_name); + ASN_STRUCT_FREE(asn_DEF_EIS_ProtectedCommunicationZone, zone); + } + + } + closedir(d); + } + } + + // TZ + if (facilities.station_type == 15) { + + int i = 0; + + DIR *d = opendir(etsi_its_cfg->facilities.tpm.tis_path); + struct dirent *dir; + char file[256]; + char ti_xml[2048]; + if (d) { + while ((dir = readdir(d)) != NULL && i < TOLLING_INFOS_MAX_LENGTH) { + if (dir->d_name[0] == '.') continue; + sprintf(file, "%s/%s", etsi_its_cfg->facilities.tpm.tis_path, dir->d_name); + FILE *fp = fopen(file, "r"); + if (!fp) continue; + fseek(fp, 0, SEEK_END); + uint16_t size = ftell(fp); + fseek(fp, 0, SEEK_SET); + if (!size) { + fclose(fp); + continue; + } + if (!fread(ti_xml, 1, size, fp)) { + fclose(fp); + continue; + } + fclose(fp); + + EI1_TollingPaymentInfo_t *ti = calloc(1, sizeof(EI1_TollingPaymentInfo_t)); + + asn_dec_rval_t dec = xer_decode(NULL, &asn_DEF_EI1_TollingPaymentInfo, (void**) &ti, ti_xml, size); + if (!dec.code) { + facilities.tolling.infos.z[i] = calloc(1, sizeof(tolling_info_t)); + facilities.tolling.infos.z[i]->asn = ti; + ++facilities.tolling.infos.length; + ++i; + log_debug("[config] loaded tolling info | id:%lld type:%s", + ti->id, + ti->tollType==EI1_TollType_entry ? "entry": ti->tollType==EI1_TollType_exit ? "exit": "single" + ); + } else { + log_error("[config] failure to decode tolling info '%s'", dir->d_name); + ASN_STRUCT_FREE(asn_DEF_EI1_TollingPaymentInfo, ti); + } + + } + closedir(d); + } + } + + + facilities.edm.enabled = itss_cfg->applications.extensions.enabled; + + EIS_ManagementRequest_t* mreq = calloc(1, sizeof(EIS_ManagementRequest_t)); + mreq->present = EIS_ManagementRequest_PR_attributes; + mreq->choice.attributes.present = EIS_ManagementRequestAttributes_PR_get; + mreq->choice.attributes.choice.get.clockType = 1; + mreq->choice.attributes.choice.get.clockOffset = 1; + void* management_socket = itss_0connect(facilities.zmq.management_address, ZMQ_REQ); + uint8_t b_tx[256], b_rx[256]; + asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_EIS_ManagementRequest, NULL, mreq, b_tx, 256); + + itss_0send(management_socket, b_tx, enc.encoded); + int rl = itss_0recv_rt(&management_socket, b_rx, 256, b_tx, 256, -1); + itss_0close(management_socket); + if (rl == -1) { + rv = 1; + goto cleanup; + } + + EIS_ManagementReply_t* mrep = calloc(1, sizeof(EIS_ManagementReply_t)); + oer_decode(NULL, &asn_DEF_EIS_ManagementReply, (void**) &mrep, b_rx, 256); + long lat, lon, alt, alt_conf; + if (mrep->returnCode == EIS_ManagementReplyReturnCode_accepted && + mrep->data && + mrep->data->choice.attributes.clockType && + mrep->data->choice.attributes.clockOffset) { + + itss_epv_init(*mrep->data->choice.attributes.clockType, TIME_MILLISECONDS, itss_cfg->time.simulated.source); + + asn_INTEGER2uint64(mrep->data->choice.attributes.clockOffset, &epv.time.offset); + } else { + log_error("rejected MR attribute request"); + rv = 1; + goto cleanup; + } + ASN_STRUCT_FREE(asn_DEF_EIS_ManagementRequest, mreq); + ASN_STRUCT_FREE(asn_DEF_EIS_ManagementReply, mrep); + + + if (etsi_its_cfg->facilities.saem.activate) { // TODO handle various services + facilities.bulletin.to_provide_len = 1; + facilities.bulletin.to_provide[0] = calloc(1, sizeof(announcement_t)); + facilities.bulletin.to_provide[0]->endpoint.port = 7010 + etsi_its_cfg->facilities.saem.service_to_advertise; + facilities.bulletin.to_provide[0]->its_aid = etsi_its_cfg->facilities.saem.service_to_advertise; + } + + facilities.vehicle.length = itss_cfg->vehicle.length; + facilities.vehicle.width = itss_cfg->vehicle.width; + facilities.vehicle.role = itss_cfg->vehicle.role; + + // Logging + facilities.logging.recorder = etsi_its_cfg->facilities.logging.management; + if (itss_cfg->general.logging.enabled && etsi_its_cfg->facilities.logging.dbms) { + facilities.logging.dbms = calloc(1, sizeof(itss_db_t)); + if (itss_db_init( + facilities.logging.dbms, + itss_cfg->general.logging.database, + itss_cfg->general.logging.table_style, + ITSS_FACILITIES, + itss_cfg->general.logging.host, + itss_cfg->general.logging.port, + itss_cfg->general.logging.username, + itss_cfg->general.logging.password + )) { + log_error("failed to initialize the database -> turning off logging"); + free(facilities.logging.dbms); + facilities.logging.dbms = NULL; + } + } + +cleanup: + t2c_itss_free(itss_cfg); + t2c_etsi_its_free(etsi_its_cfg); + + return rv; +} + diff --git a/backup/src/cpm.c b/backup/src/cpm.c new file mode 100644 index 0000000..eb1c643 --- /dev/null +++ b/backup/src/cpm.c @@ -0,0 +1,832 @@ +#include "cpm.h" +#include "facilities.h" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#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(){ + // 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) + log_error(" 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){ + if(radar_ready() == 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){ + log_error("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){ + log_error("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){ + log_error("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){ + log_error("Client disconnected..."); + return false; + } + + log_debug("Radar connected"); + + } + + // Create RAW socket + + raw_socket.raw_fd = socket(AF_PACKET, SOCK_RAW, htons(0x0800)); + + if(raw_socket.raw_fd < 0){ + log_error("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){ + log_error("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){ + log_error("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(int f) { + int rv = 0; + + dissemination_t* dissemination = &facilities.dissemination; + + uint64_t now = itss_time_get(); + + 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(int f){ + + dissemination_t* dissemination = &facilities.dissemination; + uint64_t now = itss_time_get(); + + /* 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; + switch (s_objectControl.u8_objectData0Format) { + /* data without updated flag */ + case 3: + case 5: /* with 7 classes */ + 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; + as_objects[u16_objectIndex].f_ySpeed *= 0.1; + + as_objects[u16_objectIndex].f_objectLength = (au8_can_data[6] >> 1) * 0.2; + + as_objects[u16_objectIndex].u8_objectID = au8_can_data[7] & 0x7F; + break; + + /* data with updated flag */ + case 4: + case 6: /* with 7 classes */ + 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; + as_objects[u16_objectIndex].f_ySpeed *= 0.1; + + as_objects[u16_objectIndex].f_objectLength = (au8_can_data[6] >> 1) * 0.2; + + as_objects[u16_objectIndex].u8_objectID = au8_can_data[7] & 0x7F; + as_objects[u16_objectIndex].u8_updateFlag = au8_can_data[7] >> 7; + break; + + default: + log_warn("[cp] unhandled object data0 format"); + } + } +} + + + + +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, EI1_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(EI1_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 = EI1_ObjectRefPoint_bottomMid; + + /* Detected Object Class*/ + cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->classification = calloc(1, sizeof(EI1_ObjectClassDescription_t)); + EI1_ObjectClassDescription_t *class = cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->classification; + + class->list.size = 1 * sizeof(void*); + class->list.array = malloc(1* sizeof(void*)); + class->list.count = 1; + + class->list.array[0] = calloc(1, sizeof(EI1_ObjectClass_t)); + class->list.array[0]->confidence = 0; + int obj_len = (int)(as_objects[i].f_objectLength*10); + + if (obj_len == 10) { + class->list.array[0]->Class.present = EI1_class_PR_person; + class->list.array[0]->Class.choice.person.type = 1; // pedestrian + class->list.array[0]->Class.choice.person.confidence = 0; + } else if (obj_len == 16) { + class->list.array[0]->Class.present = EI1_class_PR_person; + class->list.array[0]->Class.choice.person.type = 3; // cyclist + class->list.array[0]->Class.choice.person.confidence = 0; + } else if (obj_len == 26) { + class->list.array[0]->Class.present = EI1_class_PR_vehicle; + class->list.array[0]->Class.choice.vehicle.type = 2; // motorcycle + class->list.array[0]->Class.choice.vehicle.confidence = 0; + } else if (obj_len >= 46 && obj_len <= 54) { + class->list.array[0]->Class.present = EI1_class_PR_vehicle; + class->list.array[0]->Class.choice.vehicle.type = 3; // passenger car + class->list.array[0]->Class.choice.vehicle.confidence = 0; + } else if (obj_len >= 56 && obj_len <= 88) { + class->list.array[0]->Class.present = EI1_class_PR_vehicle; + class->list.array[0]->Class.choice.vehicle.type = 5; // light truck + class->list.array[0]->Class.choice.vehicle.confidence = 0; + } else if (obj_len >= 90) { + class->list.array[0]->Class.present = EI1_class_PR_vehicle; + class->list.array[0]->Class.choice.vehicle.type = 6; // heavy truck + class->list.array[0]->Class.choice.vehicle.confidence = 0; + } else { + class->list.array[0]->Class.present = EI1_class_PR_other; + class->list.array[0]->Class.choice.other.type = 0; // unknown + class->list.array[0]->Class.choice.other.confidence = 0; + } + + /* 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(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 */ + EI1_CPM_t* cpm_tx = calloc(1, sizeof(EI1_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 = itss_time_get() % 65536; // generationDeltaTime = TimestampIts mod 65 536 + + int32_t lat, lon, alt, alt_conf; + itss_space_lock(); + itss_space_get(); + lat = epv.space.data.latitude.value; + lon = epv.space.data.longitude.value; + alt = epv.space.data.altitude.value; + alt_conf = epv.space.data.altitude.confidence; + itss_space_unlock(); + + cpm_tx->cpm.generationDeltaTime = generationDeltaTime; + cpm_tx->cpm.cpmParameters.managementContainer.stationType = EI1_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 = EI1_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(0) == 1){ /* Sensor Information Container Inclusion Management */ + + cpm_tx->cpm.cpmParameters.sensorInformationContainer = calloc(1, sizeof(EI1_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(EI1_SensorInformation_t)); + cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0] = calloc(1, sizeof(EI1_SensorInformation_t)); + + cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->sensorID = 0; + cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->type = EI1_SensorType_radar; + cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.present = EI1_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 = facilities.dissemination.opening_angle_start; + cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.stationaryHorizontalOpeningAngleEnd = facilities.dissemination.opening_angle_end; + cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.verticalOpeningAngleStart = calloc(1, sizeof(EI1_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(EI1_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(EI1_SensorHeight_t)); + (*cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.sensorHeight) = 600; + dissemination_reset_timer(0); + } + + if (s_objectControl.u8_numberOfObjects > 0) { + cpm_tx->cpm.cpmParameters.perceivedObjectContainer = calloc(1, sizeof(EI1_PerceivedObjectContainer_t)); + cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array = calloc(s_objectControl.u8_numberOfObjects,sizeof(EI1_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_EI1_CPM, NULL, cpm_tx, bdr_oer, 1500); + if (retval_enc_bdr.encoded == -1) { + log_error("[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_EI1_CPM, NULL, cpm_tx, fdi_oer, 1500); + if (retval_enc_fdi.encoded == -1) { + log_error("[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_EI1_CPM, cpm_tx); + + return rv; +} + +void *cp_service(void *arg){ + /* 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]; + + 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[INDICATION_BUFFER_SIZE]; + uint8_t fi_oer[INDICATION_BUFFER_SIZE]; + tr_oer[0] = ITSS_FACILITIES; //Facilities + fi_oer[0] = ITSS_FACILITIES; + + EIS_NetworkingRequest_t* nr = calloc(1, sizeof(EIS_NetworkingRequest_t)); + nr->present = EIS_NetworkingRequest_PR_packet; + EIS_NetworkingPacketRequest_t* npr = &nr->choice.packet; + + npr->network.present = EIS_NetworkingPacketRequestNW_PR_gn; + npr->network.choice.gn.trafficClass = 2; + npr->network.choice.gn.destinationAddress.buf = malloc(6); + for (int i = 0; i < 6; ++i) { + npr->network.choice.gn.destinationAddress.buf[i] = 0xff; + } + npr->network.choice.gn.destinationAddress.size = 6; + npr->network.choice.gn.packetTransportType = EIS_PacketTransportType_shb; + npr->network.choice.gn.securityProfile.sign = true; + + npr->transport.present = EIS_NetworkingPacketRequestTP_PR_btp; + npr->transport.choice.btp.btpType = EIS_BTPType_btpB; + npr->transport.choice.btp.destinationPort = EIS_Port_cpm; + + EIS_FacilitiesIndication_t* fi = calloc(1, sizeof(EIS_FacilitiesIndication_t)); + fi->present = EIS_FacilitiesIndication_PR_message; + EIS_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); + + + npr->data.buf = malloc(1500); //CPM Data to be sent to the Networking layer + + /*--- Fill mandatory Facilities Message Indication parameters ---*/ + fmi->itsMessageType = EIS_ItsMessageType_cpm; + fmi->data.buf = malloc(1500); + + /* Creating sockets and waiting for radar to connect*/ + is_radar_connected = radar_connection(RADAR_PORT); + + while(!facilities.exit){ + itss_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(1)) { + 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(npr->data.buf, (uint32_t *) &npr->data.size, fmi->data.buf, (uint32_t *) &fmi->data.size, history_list, valid_array, history_timestamp) == 1) + continue; + + uint32_t id = itss_id(npr->data.buf, npr->data.size); + npr->id = id; + fmi->id = id; + + /* Encode NetworkingRequest */ + asn_enc_rval_t enc_tdr = oer_encode_to_buffer(&asn_DEF_EIS_NetworkingRequest, NULL, nr, tr_oer+1, INDICATION_BUFFER_SIZE-1); + if(enc_tdr.encoded == -1){ + log_error("encoding TR for cpm failed"); + continue; + } + + /* Encode FacilitiesIndication */ + asn_enc_rval_t enc_fdi = oer_encode_to_buffer(&asn_DEF_EIS_FacilitiesIndication, NULL, fi, fi_oer+1, INDICATION_BUFFER_SIZE-1); + if(enc_fdi.encoded == -1){ + log_error("encoding FI for cpm failed"); + continue; + } + + /* Create thread to send packet to the Networking Layer (=3) */ + itss_queue_send(facilities.tx_queue, itss_queue_packet_new(tr_oer, enc_tdr.encoded+1, ITSS_NETWORKING, id, "NR.packet.btp")); + + /* Create thread to send packet to the Applications Layer (=5) */ + itss_queue_send(facilities.tx_queue, itss_queue_packet_new(fi_oer, enc_fdi.encoded+1, ITSS_APPLICATIONS, id, "FI.message")); + + /*Reset Timer for dissemination control */ + dissemination_reset_timer(1); + + // Logging + if (facilities.logging.dbms) { + pthread_mutex_lock(&facilities.id.lock); + uint32_t station_id = facilities.id.station_id; + pthread_mutex_unlock(&facilities.id.lock); + itss_db_add(facilities.logging.dbms, station_id, npr->id, true, 14, NULL, npr->data.buf, npr->data.size); + } + if (facilities.logging.recorder) { + uint16_t buffer_len = INDICATION_BUFFER_SIZE; + uint8_t buffer[buffer_len]; + int e = itss_management_record_packet_sdu( + buffer, + buffer_len, + npr->data.buf, + npr->data.size, + npr->id, + itss_time_get(), + ITSS_FACILITIES, + true); + if (e != -1) { + itss_queue_send(facilities.tx_queue, itss_queue_packet_new(buffer, e, ITSS_MANAGEMENT, npr->id, "MReq.packet.set")); + } + } + + }else{ + is_radar_connected = radar_connection(RADAR_PORT); + } + } + } + + ASN_STRUCT_FREE(asn_DEF_EIS_NetworkingRequest, nr); + ASN_STRUCT_FREE(asn_DEF_EIS_FacilitiesIndication, fi); + + /* Close sockets */ + if(facilities.dissemination.tmc_connect) + shutdown(s_socket.i32_socket,2); + + shutdown(raw_socket.raw_fd,2); + return NULL; +} diff --git a/backup/src/cpm.h b/backup/src/cpm.h new file mode 100644 index 0000000..160e1a7 --- /dev/null +++ b/backup/src/cpm.h @@ -0,0 +1,187 @@ +#ifndef FACILITIES_CPM_H +#define FACILITIES_CPM_H + + +#include +#include + +#include +#include +#include +#include +#include +#include + +#define READ_BUFFER_SIZE 1024 // Buffer for Serial and Ethernet read function (Used in cp_service) +#define NOF_OBJECTS 127 // Number of Object UMRR-0C +#define MESSAGE_ID 14 +#define PROTOCOL_VERSION 1 +#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 raw_fd; + struct sockaddr_ll sll; + struct ifreq ifr; +} S_INTERFACE_CONNECTION_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_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; + bool tmc_connect; + char* int_radar; + char* ip_radar; + + pthread_mutex_t lock; + + uint32_t type; + + uint64_t next_cpm_max; + uint64_t next_cpm_min; + + + uint64_t T_GenCpmMin; + uint64_t T_GenCpmMax; + + uint64_t T_AddSensorInformation; + uint64_t next_AddSensorInformation; + + +/* Position of the radar (Value from toml) */ + + int64_t radar_rotation; + int16_t opening_angle_start; + int16_t opening_angle_end; + +} dissemination_t; + +/* Prototype Functions */ + +/* +Summary : Waits for client to connect (Radar) +*/ + +bool waitingIncomingConnection(void); + + + +/* +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 : 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 *arg); + +#endif diff --git a/backup/src/facilities.c b/backup/src/facilities.c new file mode 100644 index 0000000..f9e8b54 --- /dev/null +++ b/backup/src/facilities.c @@ -0,0 +1,542 @@ +#include "facilities.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "cam.h" +#include "config.h" +#include "cpm.h" +#include "denm.h" +#include "evm.h" +#include "indications.h" +#include "infrastructure.h" +#include "requests.h" +#include "saem.h" +#include "tpm.h" +#include "vcm.h" + +facilities_t facilities = {0}; + +static int networking_indication(void *responder, void **security_socket, uint8_t *msg, uint32_t msg_len) { + int rv = 0; + uint8_t code = 0; + bool stored = false; + + EIS_NetworkingIndication_t *ni = calloc(1, sizeof(EIS_NetworkingIndication_t)); + + asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_EIS_NetworkingIndication, (void **)&ni, msg, msg_len); + if (dec.code) { + log_error("<- invalid TI received"); + rv = 1; + code = 1; + itss_0send(responder, &code, 1); + goto cleanup; + } + + itss_0send(responder, &code, 1); + + switch (ni->present) { + case EIS_NetworkingIndication_PR_packet: + networking_packet_indication(&ni->choice.packet, security_socket); + break; + case EIS_NetworkingIndication_PR_data: + networking_data_indication(&ni->choice.data, security_socket); + goto cleanup; + default: + log_debug("<- unrecognized NI.choice received"); + rv = 1; + goto cleanup; + } + +cleanup: + ASN_STRUCT_FREE(asn_DEF_EIS_NetworkingIndication, ni); + + return rv; +} + +static int facilities_request(void *responder, uint8_t *msg, uint32_t msg_len) { + int rv = 0; + + EIS_FacilitiesRequest_t *fr = calloc(1, sizeof(EIS_FacilitiesRequest_t)); + + asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_EIS_FacilitiesRequest, (void **)&fr, msg, msg_len); + if (dec.code) { + log_error("<- invalid FR received"); + facilities_request_result_rejected(responder); + rv = 1; + goto cleanup; + } + + switch (fr->present) { + case EIS_FacilitiesRequest_PR_message: + rv = facilities_request_single_message(responder, &fr->choice.message); + break; + + case EIS_FacilitiesRequest_PR_data: + switch (fr->choice.data.present) { + case EIS_FacilitiesDataRequest_PR_activeEpisodes: + rv = facilities_request_active_episodes(responder, fr); + break; + + case EIS_FacilitiesDataRequest_PR_attributeTypes: + rv = facilities_request_attribute_types(responder, fr); + break; + + case EIS_FacilitiesDataRequest_PR_loadedProtectionZones: + rv = facilities_request_loaded_protected_zones(responder, fr); + break; + + case EIS_FacilitiesDataRequest_PR_chainInfoSet: + rv = facilities_request_chaininfo_set(responder, &fr->choice.data.choice.chainInfoSet); + break; + + default: + log_error("<- unrecognized FDR type received (%d)", fr->choice.data.present); + facilities_request_result_rejected(responder); + rv = 1; + goto cleanup; + } + break; + + default: + log_error("<- unrecognized FR type received (%d)", fr->present); + facilities_request_result_rejected(responder); + rv = 1; + goto cleanup; + } + +cleanup: + ASN_STRUCT_FREE(asn_DEF_EIS_FacilitiesRequest, fr); + + return rv; +} + +static int security_indication(void *responder_secured, uint8_t *msg, uint32_t msg_len) { + int rv = 0; + + EIS_SecurityIndication_t *si = calloc(1, sizeof(EIS_SecurityIndication_t)); + EIS_SecurityResponse_t *sr = calloc(1, sizeof(EIS_SecurityResponse_t)); + + uint8_t buffer[64]; + + asn_enc_rval_t enc; + + asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_EIS_SecurityIndication, (void **)&si, msg, msg_len); + if (dec.code) { + log_error("<- invalid SIndication received"); + rv = 1; + goto cleanup; + } + + pthread_mutex_lock(&facilities.id.change.lock); + if (facilities.id.change.stage == ID_CHANGE_BLOCKED) { + pthread_mutex_unlock(&facilities.id.change.lock); + log_debug("identity change is currently blocked"); + rv = 1; + goto cleanup; + } + + if (facilities.id.change.stage == ID_CHANGE_PREPARE && + si->choice.idChangeEvent.command != EIS_SecurityIdChangeEventType_commit && + si->choice.idChangeEvent.command != EIS_SecurityIdChangeEventType_abort) { + pthread_mutex_unlock(&facilities.id.change.lock); + log_debug("current identity change state is prepare, but received identity change command is not commit nor abort"); + rv = 1; + goto cleanup; + } + + bool id_changed = false; + + switch (si->choice.idChangeEvent.command) { + case EIS_SecurityIdChangeEventType_prepare: + + facilities.id.change.stage = ID_CHANGE_PREPARE; + + pthread_mutex_lock(&facilities.id.lock); + pthread_mutex_lock(&facilities.lightship.lock); + break; + + case EIS_SecurityIdChangeEventType_commit:; + facilities.id.change.stage = ID_CHANGE_COMMIT; + + // Reset lightship + for (int i = 0; i < facilities.lightship.path_history_len; ++i) { + free(facilities.lightship.path_history[i]); + } + facilities.lightship.path_history_len = 0; + + facilities.lightship.t_last_cam = 0; + facilities.lightship.t_last_cam_lfc = 0; + facilities.lightship.next_cam_max = 0; + facilities.lightship.next_cam_min = 0; + + pthread_mutex_unlock(&facilities.lightship.lock); + + // Change Station ID + for (int i = 0; i < si->choice.idChangeEvent.ids.list.count; ++i) { + switch (si->choice.idChangeEvent.ids.list.array[i]->present) { + case EIS_SecurityId_PR_stationId: + facilities.id.station_id = si->choice.idChangeEvent.ids.list.array[i]->choice.stationId; + break; + case EIS_SecurityId_PR_ipv6Address: + memcpy(facilities.id.ipv6_addr, si->choice.idChangeEvent.ids.list.array[i]->choice.ipv6Address.buf, 16); + break; + default: + break; + } + } + + facilities.id.change.stage = ID_CHANGE_INACTIVE; + + pthread_mutex_unlock(&facilities.id.lock); + + id_changed = true; + + break; + + case EIS_SecurityIdChangeEventType_abort: + pthread_mutex_unlock(&facilities.id.lock); + pthread_mutex_unlock(&facilities.lightship.lock); + + facilities.id.change.stage = ID_CHANGE_INACTIVE; + break; + + default: + pthread_mutex_unlock(&facilities.id.change.lock); + log_error("[networking]<- unhandled idChangeEvent command type"); + rv = 1; + goto cleanup; + } + + sr->present = EIS_SecurityResponse_PR_idChangeEvent; + sr->choice.idChangeEvent.returnCode = 0; + enc = oer_encode_to_buffer(&asn_DEF_EIS_SecurityResponse, NULL, sr, buffer, 64); + itss_0send(responder_secured, buffer, enc.encoded); + + pthread_mutex_unlock(&facilities.id.change.lock); + +cleanup: + if (rv) { + sr->present = EIS_SecurityResponse_PR_idChangeEvent; + sr->choice.idChangeEvent.returnCode = 1; + enc = oer_encode_to_buffer(&asn_DEF_EIS_SecurityResponse, NULL, sr, buffer, 64); + itss_0send(responder_secured, buffer, enc.encoded); + } + + pthread_mutex_unlock(&facilities.id.change.lock); + ASN_STRUCT_FREE(asn_DEF_EIS_SecurityResponse, sr); + ASN_STRUCT_FREE(asn_DEF_EIS_SecurityIndication, si); + + return rv; +} + + +static int management_indication(void *responder, uint8_t *msg, uint32_t msg_len) { + int rv = 0; + + uint8_t code = 0; + + EIS_ManagementIndication_t *mi = calloc(1, sizeof(EIS_ManagementIndication_t)); + + asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_EIS_ManagementIndication, (void **)&mi, msg, msg_len); + if (dec.code) { + rv = 1; + code = 1; + itss_0send(responder, &code, 1); + goto cleanup; + } + itss_0send(responder, &code, 1); + + if (mi->present == EIS_ManagementIndication_PR_attributes) { + itss_time_lock(); + asn_INTEGER2ulong(&mi->choice.attributes.clock, &epv.time.clock); + itss_time_unlock(); + } + +cleanup: + ASN_STRUCT_FREE(asn_DEF_EIS_ManagementIndication, mi); + + return rv; +} + +void *tx(void *arg) { + int rv = 0; + + itss_queue_t *queue = facilities.tx_queue; + + uint8_t code; + void *applications_socket = itss_0connect(facilities.zmq.applications_address, ZMQ_REQ); + void *networking_socket = itss_0connect(facilities.zmq.networking_address, ZMQ_REQ); + void *management_socket = itss_0connect(facilities.zmq.management_address, ZMQ_REQ); + + itss_queue_t *stream = itss_queue_new(); + + while (!facilities.exit) { + pthread_mutex_lock(&queue->lock); + while (!queue->len && !facilities.exit) { + pthread_cond_wait(&queue->trigger, &queue->lock); + } + + itss_queue_packet_t* qp; + while ((qp = itss_queue_pop(queue))) { + itss_queue_add(stream, qp); + }; + pthread_mutex_unlock(&queue->lock); + + while ((qp = itss_queue_pop(stream))) { + switch (qp->destination) { + case ITSS_NETWORKING: + log_debug("-> %s ->[networking] | id:%08x size:%dB", + qp->info_msg, (uint32_t)qp->id, qp->data_len); + itss_0send(networking_socket, qp->data, qp->data_len); + rv = itss_0recv_rt(&networking_socket, &code, 1, qp->data, qp->data_len, 1000); + if (rv == -1) { + log_error("-> %s ->[networking] | id:%08x size:%dB ", + qp->info_msg, (uint32_t)qp->id, qp->data_len); + } + break; + case ITSS_APPLICATIONS: + log_debug("-> %s ->[applications] | id:%08x size:%dB", + qp->info_msg, (uint32_t)qp->id, qp->data_len); + itss_0send(applications_socket, qp->data, qp->data_len); + rv = itss_0recv_rt(&applications_socket, &code, 1, qp->data, qp->data_len, 1000); + if (rv == -1) { + log_error("-> %s ->[applications] | id:%08x size:%dB ", + qp->info_msg, (uint32_t)qp->id, qp->data_len); + } + break; + case ITSS_MANAGEMENT: + itss_0send(management_socket, qp->data, qp->data_len); + rv = itss_0recv_rt(&management_socket, &code, 1, qp->data, qp->data_len, 1000); + if (rv == -1) { + log_error("-> %s ->[management] | id:%08x size:%dB ", + qp->info_msg, (uint32_t)qp->id, qp->data_len); + } + break; + } + itss_queue_packet_free(qp); + } + } + + itss_0close(networking_socket); + itss_0close(management_socket); + itss_0close(applications_socket); + + return NULL; +} + +static void sigh(int signum) { + facilities.exit = true; + uint8_t code = 0; + void *socket = itss_0connect(ZMQ_INTERNAL_ADDR, ZMQ_PAIR); + itss_0send(socket, &code, sizeof(code)); + itss_0close(socket); +} + +int main() { + signal(SIGTERM, sigh); + signal(SIGINT, sigh); + signal(SIGKILL, sigh); + + facilities.tx_queue = itss_queue_new(); + lightship_init(); + den_init(); + infrastructure_init(); + dissemination_init(); + bulletin_init(); + void *security_socket = NULL; + + if (facilities_config()) { + goto cleanup; + } + + facilities.lightship.type = facilities.station_type; + + // Tx + pthread_create(&facilities.transmitting, NULL, tx, NULL); + + // CA + pthread_create(&facilities.ca_service, NULL, ca_service, NULL); + + // DEN + pthread_create(&facilities.den_service, NULL, den_service, NULL); + + // Infrastructure + pthread_create(&facilities.infrastructure_service, NULL, infrastructure_service, NULL); + + // CP + if (facilities.dissemination.active) + pthread_create(&facilities.cp_service, NULL, cp_service, NULL); + + // SA + pthread_create(&facilities.sa_service, NULL, sa_service, NULL); + + // Tolling + tolling_init(facilities.station_type); + + // VC + if (facilities.coordination.active) { + coordination_init(); + pthread_create(&facilities.vc_service, NULL, vc_service, NULL); + } + + // EVCSN + if (facilities.evm_args.activate) + pthread_create(&facilities.evcsn_service, NULL, evcsn_service, NULL); + + // EDM + if (facilities.edm.enabled) { + edm_init(); + } + + // MCM + if (facilities.mcm_coord.active){ + mcm_coord_init(); + pthread_create(&facilities.mcm_service, NULL, mc_service, NULL); + } + + facilities.apps_socket = itss_0connect(facilities.zmq.applications_address, ZMQ_REQ); + security_socket = itss_0connect(facilities.zmq.security_address, ZMQ_REQ); + + uint8_t buffer[ITSS_SDU_MAX_LEN]; + log_info("listening"); + uint8_t code; + bool in_idchange; + + int32_t rl; + + while (!facilities.exit) { + itss_0poll(facilities.zmq.responders, facilities.zmq.n_responders, -1); + + for (int i = 0; i < facilities.zmq.n_responders; ++i) { + if (facilities.zmq.responders[i].revents) { + rl = itss_0recv(facilities.zmq.responders[i].socket, buffer, ITSS_SDU_MAX_LEN); + switch (buffer[0]) { /* source */ + + case ITSS_INTERNAL: + break; + + case ITSS_NETWORKING: + in_idchange = true; + pthread_mutex_lock(&facilities.id.change.lock); + if (facilities.id.change.stage == ID_CHANGE_INACTIVE) { + in_idchange = false; + facilities.id.change.stage = ID_CHANGE_BLOCKED; + } + pthread_mutex_unlock(&facilities.id.change.lock); + + if (!in_idchange) { + networking_indication(facilities.zmq.responders[i].socket, &security_socket, buffer + 1, rl-1); + + pthread_mutex_lock(&facilities.id.change.lock); + facilities.id.change.stage = ID_CHANGE_INACTIVE; + pthread_mutex_unlock(&facilities.id.change.lock); + } else { + code = 1; + itss_0send(facilities.zmq.responders[i].socket, &code, 1); + } + break; + + case ITSS_APPLICATIONS: + in_idchange = true; + pthread_mutex_lock(&facilities.id.change.lock); + if (facilities.id.change.stage == ID_CHANGE_INACTIVE) { + in_idchange = false; + facilities.id.change.stage = ID_CHANGE_BLOCKED; + } + pthread_mutex_unlock(&facilities.id.change.lock); + + if (!in_idchange) { + facilities_request(facilities.zmq.responders[i].socket, buffer + 1, rl-1); + + pthread_mutex_lock(&facilities.id.change.lock); + facilities.id.change.stage = ID_CHANGE_INACTIVE; + pthread_mutex_unlock(&facilities.id.change.lock); + } else { + code = 1; + itss_0send(facilities.zmq.responders[i].socket, &code, 1); + } + + break; + + case ITSS_MANAGEMENT: + management_indication(facilities.zmq.responders[i].socket, buffer + 1, rl-1); + break; + + case ITSS_SECURITY: + security_indication(facilities.zmq.responders[i].socket, buffer + 1, rl-1); + break; + + case ITSS_STATUS: + code = 0; + itss_0send(facilities.zmq.responders[i].socket, &code, 1); + break; + + default: + log_debug("<- unrecognized service"); + code = 1; + itss_0send(facilities.zmq.responders[i].socket, &code, 1); + continue; + } + } + } + } + + // Exit +cleanup: + if (facilities.evm_args.activate) + pthread_join(facilities.evcsn_service, NULL); + pthread_join(facilities.ca_service, NULL); + pthread_join(facilities.den_service, NULL); + pthread_join(facilities.infrastructure_service, NULL); + if (facilities.dissemination.active) + pthread_join(facilities.cp_service, NULL); + pthread_join(facilities.sa_service, NULL); + itss_queue_trigger(facilities.tx_queue); + pthread_join(facilities.transmitting, NULL); + if (facilities.coordination.active) + pthread_join(facilities.vc_service, NULL); + if (facilities.edm.enabled) { + itss_0close(facilities.edm.app_socket); + } + if (facilities.edm.enabled) { + itss_0close(facilities.edm.app_socket); + } + itss_0close(facilities.apps_socket); + + itss_0close(security_socket); + for (int i = 0; i < facilities.zmq.n_responders; ++i) { + itss_0close(facilities.zmq.responders[i].socket); + } + itss_0destroy(); + + log_info("exiting"); + log_close(); + + return 0; +} diff --git a/backup/src/facilities.h b/backup/src/facilities.h new file mode 100644 index 0000000..b31e205 --- /dev/null +++ b/backup/src/facilities.h @@ -0,0 +1,132 @@ +#ifndef FACILITIES_H +#define FACILITIES_H + +#include +#include +#include + +#include "cam.h" +#include "denm.h" +#include "infrastructure.h" +#include "cpm.h" +#include "saem.h" +#include "tpm.h" +#include "vcm.h" +#include "evm.h" +#include "edm.h" +#include "mcm.h" + +#include +#include +#include +#include +#include + +/* + * IT2S-GN defines a maximum packet size equal to 2360 bytes. + * For indications to the networking layer, a maximum of 2360 bytes for data + 512 bytes for other fields is set. + */ +#define INDICATION_BUFFER_SIZE 2560 + +enum ID_CHANGE_STAGE { + ID_CHANGE_INACTIVE, + ID_CHANGE_BLOCKED, + ID_CHANGE_PREPARE, + ID_CHANGE_COMMIT +}; + +typedef struct facilities { + pthread_t ca_service; + pthread_t den_service; + pthread_t infrastructure_service; + pthread_t transmitting; + pthread_t cp_service; + pthread_t sa_service; + pthread_t vc_service; + pthread_t evcsn_service; + pthread_t mcm_service; + + // ZMQ + struct { + zmq_pollitem_t* responders; + uint16_t n_responders; + + char* networking_address; + char* applications_address; + char* security_address; + char* management_address; + } zmq; + + // Transmitter + itss_queue_t* tx_queue; + void* apps_socket; /* alternative to tx queue, only used in rx/main thread */ + + struct { + uint8_t defaultv; + bool handle_all; + } pver; + + // CA + lightship_t lightship; + + // DEN + den_t den; + + // Infrastructure + infrastructure_t infrastructure; + + // CPM + dissemination_t dissemination; + + // SA + bulletin_t bulletin; + + // TP + tolling_t tolling; + + // VC + coordination_t coordination; + + // EVCSN + evm_args_t evm_args; + + // EDM + edm_t edm; + + // MCM + mcm_coord_t mcm_coord; + + // Logging + struct { + bool recorder; + itss_db_t* dbms; + } logging; + + int station_type; + bool use_security; + bool replay; + bool upf; + + struct { + uint16_t width; + uint16_t length; + uint8_t role; + } vehicle; + + struct { + pthread_mutex_t lock; + uint32_t station_id; + uint8_t ipv6_addr[16]; + struct { + pthread_mutex_t lock; + bool random; + int stage; + } change; + } id; + + bool exit; +} facilities_t; + +extern facilities_t facilities; + +#endif diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 3fa9939..04141d8 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -23,13 +23,13 @@ TARGET_LINK_LIBRARIES(it2s-itss-facilities -lit2s-asn-etsi-its-sdu-cdd-1.3.1 -lzmq -lit2s_gnss + -lit2s-radar -lpthread -lit2s-config-etsi-its -lit2s-asn-etsi-its-v1-cdd-1.3.1 -lit2s-asn-etsi-its-v1-cam -lit2s-asn-etsi-its-v1-ivim -lit2s-asn-etsi-its-v1-denm - -lit2s-asn-etsi-its-v1-cpm -lit2s-asn-etsi-its-v1-saem -lit2s-asn-etsi-its-v1-tpm -lit2s-asn-etsi-its-v1-vcm @@ -39,6 +39,8 @@ TARGET_LINK_LIBRARIES(it2s-itss-facilities -lit2s-asn-etsi-its-v2-cam -lit2s-asn-etsi-its-v2-denm -lit2s-asn-etsi-its-v2-mcm + -lit2s-asn-etsi-its-v2-cpm + -lit2s-asn-etsi-its-v2-ldm -lit2s-tender -lm -lrt diff --git a/src/config.c b/src/config.c index 2e8d625..aacb364 100644 --- a/src/config.c +++ b/src/config.c @@ -19,6 +19,10 @@ #include +#include +#include +#include + #include #include #include @@ -55,6 +59,7 @@ int facilities_config() { t2c_itss_t* itss_cfg = NULL; t2c_etsi_its_t* etsi_its_cfg = NULL; + t2c_radar_t* radar_cfg = NULL; rv = t2c_itss_read("/etc/it2s/itss.toml", &itss_cfg); if (rv) { @@ -66,6 +71,11 @@ int facilities_config() { syslog_error("[config] read ETSI ITS config failed"); goto cleanup; } + rv = t2c_radar_read("/etc/it2s/radar.toml", &radar_cfg); + if (rv) { + syslog_error("[config] read radar config failed"); + radar_cfg = NULL; // goto cleanup; // radar is optional + } // Logging - status messages bool use_syslog = false, use_std = false, use_file = false; @@ -343,22 +353,53 @@ int facilities_config() { facilities.infrastructure.default_service_duration = etsi_its_cfg->facilities.ivim.default_service_duration * 60000; // CPM - facilities.dissemination.active = etsi_its_cfg->facilities.cpm.activate; - facilities.dissemination.T_GenCpmMin = etsi_its_cfg->facilities.cpm.rsu_obu_period_min; - facilities.dissemination.T_GenCpmMax = etsi_its_cfg->facilities.cpm.rsu_obu_period_max; - // Rotation not needed anymore? - //facilities.dissemination.radar_rotation = itss_cfg->radar.radar_rotation; - facilities.dissemination.tmc_connect = etsi_its_cfg->facilities.cpm.tmc_connected; - facilities.dissemination.T_AddSensorInformation = 1000; - int oa_start = (360-facilities.dissemination.radar_rotation) * 10 - 500; - int oa_end = (360-facilities.dissemination.radar_rotation) * 10 + 500; - facilities.dissemination.opening_angle_start = (oa_start + 3600) % 3600; - facilities.dissemination.opening_angle_end = (oa_end + 3600) % 3600; + facilities.cp.active = etsi_its_cfg->facilities.cpm.activate; + facilities.cp.T_GenCpm = etsi_its_cfg->facilities.cpm.t_gen_cpm; - facilities.dissemination.int_radar = malloc(strlen(etsi_its_cfg->facilities.cpm.radar_interface)+1); - strcpy(facilities.dissemination.int_radar,etsi_its_cfg->facilities.cpm.radar_interface); - facilities.dissemination.ip_radar = malloc(strlen(etsi_its_cfg->facilities.cpm.radar_ip)+1); - strcpy(facilities.dissemination.ip_radar,etsi_its_cfg->facilities.cpm.radar_ip); + // CP - LDM connection + facilities.cp.ldm.active = etsi_its_cfg->facilities.ldm.activate; + facilities.cp.ldm.registered = false; + index = fetch_target_address(etsi_its_cfg->facilities.ldm.zmq.addresses, etsi_its_cfg->facilities.ldm.zmq.addresses_len); + if (index != -1) { + facilities.cp.ldm.address = malloc(strlen(etsi_its_cfg->facilities.ldm.zmq.addresses[index]) + 1); + strcpy(facilities.cp.ldm.address, etsi_its_cfg->facilities.ldm.zmq.addresses[index]); + } else { + log_error("[config] a valid address for [ldm] was not found"); + rv = 1; + goto cleanup; + } + + // CPM - Register UMRR0C radar + if (radar_cfg && radar_cfg->umrr0c.activate) { + EI2_SensorInformation_t *si = calloc(1, sizeof(EI2_SensorInformation_t)); + + si->sensorId = UMRR0CId; + si->sensorType = EI2_SensorType_radar; + si->perceptionRegionShape = calloc(1, sizeof(EI2_Shape_t)); + si->perceptionRegionShape->present = EI2_Shape_PR_radial; + si->perceptionRegionShape->choice.radial.range = 3400; + int hoa = (360 - radar_cfg->umrr0c.rotation) * 10 - 500; + si->perceptionRegionShape->choice.radial.horizontalOpeningAngleStart = (hoa + 3600) % 3600; + hoa = (360 - radar_cfg->umrr0c.rotation) * 10 + 500; + si->perceptionRegionShape->choice.radial.horizontalOpeningAngleEnd = (hoa + 3600) % 3600; + si->perceptionRegionShape->choice.radial.verticalOpeningAngleStart = calloc(1, sizeof(EI2_CartesianAngleValue_t)); + *(si->perceptionRegionShape->choice.radial.verticalOpeningAngleStart) = 1730; + si->perceptionRegionShape->choice.radial.verticalOpeningAngleEnd = calloc(1, sizeof(EI2_CartesianAngleValue_t)); + *(si->perceptionRegionShape->choice.radial.verticalOpeningAngleEnd) = 1890; + si->perceptionRegionShape->choice.radial.shapeReferencePoint = calloc(1, sizeof(EI2_CartesianPosition3d_t)); + si->perceptionRegionShape->choice.radial.shapeReferencePoint->xCoordinate = 0; + si->perceptionRegionShape->choice.radial.shapeReferencePoint->yCoordinate = 0; + si->perceptionRegionShape->choice.radial.shapeReferencePoint->zCoordinate = calloc(1, sizeof(EI2_CartesianCoordinate_t)); + *(si->perceptionRegionShape->choice.radial.shapeReferencePoint->zCoordinate) = radar_cfg->umrr0c.height; + + if (facilities.cp.si_count >= MAX_SENSORS) { + log_error("failed to register UMRR0-C"); + ASN_STRUCT_FREE(asn_DEF_EI2_SensorInformation, si); + } else { + facilities.cp.si_arr[facilities.cp.si_count] = si; + facilities.cp.si_count++; + } + } // TPM facilities.tolling.enabled = etsi_its_cfg->facilities.tpm.activate; @@ -584,6 +625,7 @@ int facilities_config() { cleanup: t2c_itss_free(itss_cfg); t2c_etsi_its_free(etsi_its_cfg); + t2c_radar_free(radar_cfg); return rv; } diff --git a/src/cpm.c b/src/cpm.c index eb1c643..76125d8 100644 --- a/src/cpm.c +++ b/src/cpm.c @@ -1,712 +1,1412 @@ +/** + * @file cpm.c + * @author Diogo Jesus (diogopjesus@ua.pt) + * @brief Collective Perception Service implementation. + * + * @note Parts of the codebase take for granted that the timestamp represeting *now* + * is not a small number. If given a small number (e.g < 1000), it is undefined + * behavior as there are subtractions with uint64_t. + */ + #include "cpm.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + #include "facilities.h" -#include -#include -#include -#include -#include -#include +static void* ldm_socket = NULL; -#include -#include -#include -#include -#include -#include +// Prototypes +static void po_replace_id(EI2_PerceivedObject_t* po, cp_obj_id_t* man, cp_config_t* config, uint64_t now); +static int si_get_id(int sensor_id, cp_sensor_id_t* man); -#include -#include -#include -#include +/** + * @brief Retrieve the perceived objects from the LDM. + * + * The function retrieves the perceived objects from the LDM and stores them in the po_arr array. + * Is responsible to correct data received from the LDM, such as the object position, the measurement + * delta time, the sensor ID list, etc. + * The returned perceived objects should be the finalized version ready to be included in a CPM. + * + * @param po_arr Pointer to the array where the perceived objects will be stored. + * @param po_count Pointer to the variable where the number of perceived objects will be stored (counts as total number + * of objects present in the LDM). + * @param po_len Length of the po_arr array. + * @param now Current time in milliseconds. + * @param man CP object identifier manager. + * @return int 0 if the perceived objects were successfully retrieved, different than 0 otherwise (always 0 for now). + */ +static int po_get(EI2_PerceivedObject_t** po_arr, size_t* po_count, size_t po_len, uint64_t now, cp_obj_id_t* po_man, + cp_sensor_id_t* sen_man) { + int ret = 0; -#define PI 3.141592654 + if (facilities.cp.ldm.registered) { + EI2_RequestDataobjectsReq_t* req = calloc(1, sizeof(EI2_RequestDataobjectsReq_t)); + EI2_RequestDataobjectsResp_t* resp = NULL; -/* Variables */ + req->applicationId.present = EI2_ITSaid0v0VarLengthNumber_PR_extension; + req->applicationId.choice.extension.present = EI2_ITSaid0v0Ext1_PR_content; + req->applicationId.choice.extension.choice.content = 639; -float roadRotationSin; -float roadRotationCos; + req->dataObjectType = EI2_DataObjectType_ism; -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(){ - // 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) - log_error(" 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){ - if(radar_ready() == 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){ - log_error("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){ - log_error("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){ - log_error("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){ - log_error("Client disconnected..."); - return false; - } - - log_debug("Radar connected"); + uint8_t req_oer[256]; + req_oer[0] = 9; + asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_EI2_RequestDataobjectsReq, NULL, req, req_oer + 1, 256 - 1); + if (enc.encoded == -1) { + log_error("[po_get] failed to encode request to LDM"); + ret = 1; + goto cleanup; + } + itss_0send(ldm_socket, req_oer, enc.encoded + 1); + // wait a maximum of 100ms for a response + uint8_t resp_oer[4096]; + int rl = itss_0recv_rt(&ldm_socket, resp_oer, 4096, req_oer, 256, -1); + if (rl == -1) { + log_error("[cp][po_get] failed to receive response from LDM"); + ret = 1; + goto cleanup; + } + resp = calloc(1, sizeof(*resp)); + asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_EI2_RequestDataobjectsResp, (void**)&resp, resp_oer, rl); + if (dec.code) { + log_error("[cp][po_get] failed to decode response from LDM"); + ret = 1; + goto cleanup; } - // Create RAW socket - - raw_socket.raw_fd = socket(AF_PACKET, SOCK_RAW, htons(0x0800)); - - if(raw_socket.raw_fd < 0){ - log_error("Failed to initializing RAW socket ..."); - return false; + if (resp->result != EI2_RequestedDataObjectsResult_successful) { + log_error("[cp][po_get] LDM failed to retrieve objects"); + ret = 1; + goto cleanup; } - // Get interface index + *po_count = 0; + if (resp->requestedData) { + log_debug("[cp][po_get] received %d ism from LDM", resp->requestedData->list.count); - bzero(&raw_socket.sll, sizeof(raw_socket.sll)); - bzero(&raw_socket.ifr, sizeof(raw_socket.ifr)); + EI2_InternalSensorMessage_t* ism = NULL; + for (int i = 0; i < resp->requestedData->list.count; i++) { + EI2_InternalSensorMessage_t* tmp = &resp->requestedData->list.array[i]->choice.ism; + // TODO: maintain the most recent iteration. for now, only one should be received + ism = tmp; + } + // no ISM received + if (!ism) { + goto cleanup; + } - strncpy((char *)raw_socket.ifr.ifr_name, facilities.dissemination.int_radar, IFNAMSIZ); - if((ioctl(raw_socket.raw_fd, SIOCGIFINDEX, &raw_socket.ifr)) == -1){ - log_error("Error getting interface index"); - return false; + // copy objects + uint64_t ism_ref_time = 0; + asn_INTEGER2uint64(&ism->referenceTime, &ism_ref_time); + int64_t delta_time = now - ism_ref_time; + if (delta_time < -2048) { + delta_time = -2048; + } else if (delta_time > 2047) { + delta_time = 2047; + } + + for (int i = 0; i < ism->numberOfPerceivedObjects; ++i) { + EI2_PerceivedObject_t* po = ism->perceivedObjects.list.array[i]; + + //po_arr[*po_count] = calloc(1, sizeof(EI2_PerceivedObject_t)); + asn_copy(&asn_DEF_EI2_PerceivedObject, (void**)&po_arr[*po_count], po); + + // TODO: measurementDeltaTime, position, age need to be corrected + po_arr[*po_count]->measurementDeltaTime = delta_time; + if (po_arr[*po_count]->objectAge) { + *po_arr[*po_count]->objectAge += delta_time; + if (*po_arr[*po_count]->objectAge < 0) { + *po_arr[*po_count]->objectAge = 0; + } else if (*po_arr[*po_count]->objectAge > 2047) { + *po_arr[*po_count]->objectAge = 2047; + } + } + + (*po_count)++; + if ((*po_count) >= po_len) { + break; + } + } } - raw_socket.sll.sll_family = AF_PACKET; - raw_socket.sll.sll_ifindex = raw_socket.ifr.ifr_ifindex; - raw_socket.sll.sll_protocol = htons(0x0800); + log_debug("[cp][po_get] received %d perceived objects", *po_count); - // Bind it to the interface - - if(bind(raw_socket.raw_fd, (struct sockaddr *)&raw_socket.sll, sizeof(raw_socket.sll))<0){ - log_error("Error binding RAW socket ..."); - return false; - } - - - return true; - }else{ - return false; + cleanup: + ASN_STRUCT_FREE(asn_DEF_EI2_RequestDataobjectsReq, req); + ASN_STRUCT_FREE(asn_DEF_EI2_RequestDataobjectsResp, resp); } + // Replace object Ids and sensor Ids in each of the received objects + // in order to comply with the standard. + for (size_t i = 0; i < *po_count; i++) { + po_replace_id(po_arr[i], po_man, &facilities.cp.config, now); + if (po_arr[i]->sensorIdList) { + for (size_t j = 0; j < po_arr[i]->sensorIdList->list.count; j++) { + int sen_id = *(po_arr[i]->sensorIdList->list.array[j]); + int new_id = si_get_id(sen_id, sen_man); // sensor id must already be registered + if (new_id >= 0) { + *(po_arr[i]->sensorIdList->list.array[j]) = new_id; + } else { + log_warn("[cp][po_get] could not retrieve valid sensor Id"); + //asn_sequence_del(&po_arr[i]->sensorIdList->list, j, 1); + //j--; + *(po_arr[i]->sensorIdList->list.array[j]) = UNUSED_SI_ID; + } + } + } + } + + return ret; } -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); +/** + * @brief Calculate the message rate, represented by a mantissa and an exponent. + * + * The function is successful if it can represent the message rate with: + * 1 <= mantissa <= 100. + * -5 <= exponent <= 2. + * This respect the limits of the DataFrame MessageRateHz present in the CDDv2. + * + * Return Codes: + * -1 if could not calculate the message rate. + * 0 if successful. + * 1 if mantissa does not respect the MessageRateHz boundaries. + * 2 if exponent does not respect the MessageRateHz boundaries. + * 3 if both mantissa and exponent do not respect the MessageRateHz boundaries. + * + * @param ms Period (T) in milliseconds. + * @param man Pointer where the mantissa will be stored. + * @param exp Pointer where the exponent will be stored. + * @return int 0 if the message rate is well represented, !=0 otherwise. + */ +static int calc_message_rate(int ms, int* man, int* exp) { + if (ms <= 0) { + log_warn("[cp][calc_message_rate] invalid period (T) value"); + return -1; + } + if (man == NULL || exp == NULL) { + log_warn("[cp][calc_message_rate] invalid pointers"); + return -1; + } - return dissemination; -} - - -int dissemination_check(int f) { int rv = 0; + double mantissa = 1000.0 / (double)ms; + int exponent = 0; - dissemination_t* dissemination = &facilities.dissemination; - - uint64_t now = itss_time_get(); - - 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; - } + while (mantissa < 1) { + mantissa *= 10; + exponent -= 1; } - pthread_mutex_unlock(&dissemination->lock); + while (mantissa > 100) { + mantissa /= 10; + exponent += 1; + } + + // Conditions defined in the MessageRateHz + if (mantissa < 1 || mantissa > 100) { + rv |= 1; + } + if (exponent < -5 || exponent > 2) { + rv |= 2; + } + + // increase precision if possible + if (mantissa <= 10 && exponent > -5) { + mantissa *= 10; + exponent -= 1; + } + + *man = (int)(mantissa); + *exp = exponent; return rv; } - -void dissemination_reset_timer(int f){ - - dissemination_t* dissemination = &facilities.dissemination; - uint64_t now = itss_time_get(); - - /* 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; +/** + * @brief Compute perceived object type, as defined on clause 6.1.2.3 in ETSI TS 103 324 V2.1.1 (2023-06). + * @param po Perceived object pointer. + * @param type_A Pointer to a boolean where the result will be stored. + * @return int 0 if the type was successfully computed, different than 0 otherwise. + */ +static int po_type(EI2_PerceivedObject_t* po, bool* type_A) { + if (!po || !type_A) { + log_warn("[cp][po_type] invalid pointer"); + return -1; + } + if (!po->classification || po->classification->list.count <= 0) { + log_warn("[cp][po_type] perceived object classification is not available"); + return -1; } - pthread_mutex_unlock(&dissemination->lock); -} + EI2_ObjectClass_t* class = NULL; + int confidence = 0; + class = &po->classification->list.array[0]->objectClass; + if (po->classification->list.array[0]->confidence != EI2_ConfidenceLevel_unavailable) { + confidence = po->classification->list.array[0]->confidence; + } -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; + for (size_t j = 1; j < po->classification->list.count; j++) { + if (po->classification->list.array[j]->confidence == EI2_ConfidenceLevel_unavailable) { + continue; + } + if (po->classification->list.array[j]->confidence > confidence) { + confidence = po->classification->list.array[j]->confidence; + class = &po->classification->list.array[j]->objectClass; + } + } - 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; + switch (class->present) { + case EI2_ObjectClass_PR_vehicleSubClass: + *type_A = false; + break; + case EI2_ObjectClass_PR_vruSubClass: + switch (class->choice.vruSubClass.present) { + case EI2_VruProfileAndSubprofile_PR_pedestrian: + case EI2_VruProfileAndSubprofile_PR_bicyclistAndLightVruVehicle: + case EI2_VruProfileAndSubprofile_PR_animal: + *type_A = true; + break; - 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]; + case EI2_VruProfileAndSubprofile_PR_motorcyclist: + *type_A = false; + break; - 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; + default: + log_warn("[cp][po_type] could not identify object class"); + *type_A = false; } 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]; + case EI2_ObjectClass_PR_groupSubClass: + *type_A = true; 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]; + case EI2_ObjectClass_PR_otherSubClass: + *type_A = true; + break; + default: + log_warn("[cp][po_type] could not identify object class. considering perceived object as type B"); + *type_A = false; + } + + return 0; +} + +/** + * @brief Compute perceived object geographic position (latitude and longitude) based on the reference position of the + * vehicle. + * + * @note Method used: https://stackoverflow.com/a/40471701 + * + * @param po Perceived object pointer. + * @param epv_lat Ego vehicle latitude. + * @param epv_lon Ego vehicle longitude. + * @param po_lat Pointer to the latitude where the result will be stored. + * @param po_lon Pointer to the longitude where the result will be stored. + */ +static void po_position(EI2_PerceivedObject_t* po, double epv_lat, double epv_lon, double* po_lat, double* po_lon) { + static const double M_PER_DEG = 111320.0; + static const double RAD_PER_DEG = M_PI / 180.0; + + EI2_CartesianPosition3dWithConfidence_t* p = &po->position; + + double coef = (p->yCoordinate.value / 100.0) / M_PER_DEG; + *po_lat = epv_lat + coef; + + coef = (p->xCoordinate.value / 100.0) / (M_PER_DEG * cos(*po_lat * RAD_PER_DEG)); + *po_lon = epv_lon + coef; +} + +/** + * @brief Compute perceived object speed. + * + * The speed when cartesian velocity is present only considers the X and Y components. The Z component is ignored. + * + * @param po Perceived object pointer. + * @return double Speed in m/s. + */ +static double po_speed(EI2_PerceivedObject_t* po) { + EI2_Velocity3dWithConfidence_t* vel = po->velocity; + switch (vel->present) { + case EI2_Velocity3dWithConfidence_PR_polarVelocity: + if (vel->choice.polarVelocity.velocityMagnitude.speedValue == EI2_SpeedValue_unavailable || + vel->choice.polarVelocity.velocityMagnitude.speedValue == EI2_SpeedValue_outOfRange) { + break; + } + return vel->choice.polarVelocity.velocityMagnitude.speedValue / 100.0; + case EI2_Velocity3dWithConfidence_PR_cartesianVelocity: + if (vel->choice.cartesianVelocity.xVelocity.value == EI2_VelocityComponentValue_unavailable || + vel->choice.cartesianVelocity.xVelocity.value == EI2_VelocityComponentValue_negativeOutOfRange || + vel->choice.cartesianVelocity.xVelocity.value == EI2_VelocityComponentValue_positiveOutOfRange) { + break; + } + if (vel->choice.cartesianVelocity.yVelocity.value == EI2_VelocityComponentValue_unavailable || + vel->choice.cartesianVelocity.yVelocity.value == EI2_VelocityComponentValue_negativeOutOfRange || + vel->choice.cartesianVelocity.yVelocity.value == EI2_VelocityComponentValue_positiveOutOfRange) { + break; + } + return sqrt((vel->choice.cartesianVelocity.xVelocity.value * vel->choice.cartesianVelocity.xVelocity.value) + + (vel->choice.cartesianVelocity.yVelocity.value * vel->choice.cartesianVelocity.yVelocity.value)) / + 100.0; + case EI2_Velocity3dWithConfidence_PR_NOTHING: + log_warn("[cp][po_speed] velocity component is of type NOTHING"); 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; - switch (s_objectControl.u8_objectData0Format) { - /* data without updated flag */ - case 3: - case 5: /* with 7 classes */ - 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; + log_warn("[cp][po_speed] could not retrieve valid velocity component. returning velocity as 0"); + return 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; - - 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; - as_objects[u16_objectIndex].f_ySpeed *= 0.1; - - as_objects[u16_objectIndex].f_objectLength = (au8_can_data[6] >> 1) * 0.2; - - as_objects[u16_objectIndex].u8_objectID = au8_can_data[7] & 0x7F; +/** + * @brief Compute perceived object velocity orientation. + * + * The orientation when cartesian velocity is present only considers the X and Y components. The Z component is ignored. + * + * @param po Perceived object pointer. + * @return double Velocity orientation in degrees. + */ +static double po_vel_ori(EI2_PerceivedObject_t* po) { + EI2_Velocity3dWithConfidence_t* vel = po->velocity; + switch (vel->present) { + case EI2_Velocity3dWithConfidence_PR_polarVelocity: + if (vel->choice.polarVelocity.velocityDirection.value == EI2_CartesianAngleValue_unavailable || + vel->choice.polarVelocity.velocityDirection.value == EI2_CartesianAngleValue_valueNotUsed) { break; + } + return vel->choice.polarVelocity.velocityDirection.value / 10.0; - /* data with updated flag */ - case 4: - case 6: /* with 7 classes */ - 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; - as_objects[u16_objectIndex].f_ySpeed *= 0.1; - - as_objects[u16_objectIndex].f_objectLength = (au8_can_data[6] >> 1) * 0.2; - - as_objects[u16_objectIndex].u8_objectID = au8_can_data[7] & 0x7F; - as_objects[u16_objectIndex].u8_updateFlag = au8_can_data[7] >> 7; + case EI2_Velocity3dWithConfidence_PR_cartesianVelocity: + if (vel->choice.cartesianVelocity.xVelocity.value == EI2_VelocityComponentValue_unavailable || + vel->choice.cartesianVelocity.xVelocity.value == EI2_VelocityComponentValue_negativeOutOfRange || + vel->choice.cartesianVelocity.xVelocity.value == EI2_VelocityComponentValue_positiveOutOfRange) { break; + } + if (vel->choice.cartesianVelocity.yVelocity.value == EI2_VelocityComponentValue_unavailable || + vel->choice.cartesianVelocity.yVelocity.value == EI2_VelocityComponentValue_negativeOutOfRange || + vel->choice.cartesianVelocity.yVelocity.value == EI2_VelocityComponentValue_positiveOutOfRange) { + break; + } + if (vel->choice.cartesianVelocity.xVelocity.value == 0 && vel->choice.cartesianVelocity.yVelocity.value == 0) { + return 0; + } + return atan2(vel->choice.cartesianVelocity.yVelocity.value, vel->choice.cartesianVelocity.xVelocity.value) * 180.0 / M_PI; + case EI2_Velocity3dWithConfidence_PR_NOTHING:; + } - default: - log_warn("[cp] unhandled object data0 format"); + log_warn("[cp][po_vel_ori] could not retrieve valid velocity component. returning velocity orientation as 0"); + return 0; +} + +/** + * @brief Compute the object utility function, as defined in clause 6.1.3.2 of the ETSI TS 103 324 V2.1.1 (2023-06). + * + * As defined by the standard, the object utility function is calculated based on the object perception quality, + * position change, speed change, orientation change and last inclusion time. + * Its value is capped between 0 and 5. + * + * @note The given perceived object must be valid. If not, the function has undefined behavior. + * @note The given perceived object must contain the velocity component. If not, the function has undefined behavior. + * + * @param po Perceived object pointer. + * @param history CP Service perceived object history struct. + * @param config CP Service configuration struct. + * @param now Current time in milliseconds. + * @param epv_lat Ego vehicle latitude. + * @param epv_lon Ego vehicle longitude. + * @return double Object utility function. + */ +static double po_ouf(EI2_PerceivedObject_t* po, cp_history_t* history, cp_config_t* config, uint64_t now, double epv_lat, double epv_lon) { + double ouf = 0.0, lat, lon; + + if (po->objectPerceptionQuality) { + ouf += *po->objectPerceptionQuality / 15.0; + } + po_position(po, epv_lat, epv_lon, &lat, &lon); + double diff = it2s_geodesy_haversine(history->position[*po->objectId].latitude, history->position[*po->objectId].longitude, lat, lon); + if (diff > config->minPositionChangePriorityThreshold) { + ouf += fmin(1.0, (diff - config->minPositionChangePriorityThreshold) / + (config->maxPositionChangePriorityThreshold - config->minPositionChangePriorityThreshold)); + } + diff = po_speed(po) - history->speed[*po->objectId]; + if (diff > config->minGroundSpeedChangePriorityThreshold) { + ouf += fmin(1.0, (diff - config->minGroundSpeedChangePriorityThreshold) / + (config->maxGroundSpeedChangePriorityThreshold - config->minGroundSpeedChangePriorityThreshold)); + } + diff = po_vel_ori(po) - history->vel_ori[*po->objectId]; + diff += (diff > 1800) ? -3600 : (diff < -1800) ? 3600 : 0; + diff = fabs(diff); + if (diff > config->minGroundVelocityOrientationChangePriorityThreshold) { + ouf += fmin(1.0, (diff - config->minGroundVelocityOrientationChangePriorityThreshold) / + (config->maxGroundVelocityOrientationChangePriorityThreshold - + config->minGroundVelocityOrientationChangePriorityThreshold)); + } + diff = now - history->inclusion[*po->objectId]; + if (diff > config->minLastInclusionTimePriorityThreshold) { + ouf += fmin(1.0, (diff - config->minLastInclusionTimePriorityThreshold) / + (config->maxLastInclusionTimePriorityThreshold - config->minLastInclusionTimePriorityThreshold)); + } + + return ouf; +} + +/** + * @brief Sort the perceived object array based on the object utility function. + * + * The array is sorted in descending order. + * The algorithm used is quicksort. + * + * @param po_arr Array of perceived object pointers. + * @param ouf_arr Array of object utility function values. + * @param po_count Number of perceived objects in the array. + */ +static void po_sort(EI2_PerceivedObject_t** po_arr, double* ouf_arr, size_t po_count) { + if (po_count <= 1) { + return; + } + + double pivot = ouf_arr[po_count / 2]; + int i = 0, j = po_count - 1; + while (i <= j) { + while (ouf_arr[i] > pivot) { + i++; } + while (ouf_arr[j] < pivot) { + j--; + } + if (i <= j) { + EI2_PerceivedObject_t* tmp_po = po_arr[i]; + po_arr[i] = po_arr[j]; + po_arr[j] = tmp_po; + + double tmp_ouf = ouf_arr[i]; + ouf_arr[i] = ouf_arr[j]; + ouf_arr[j] = tmp_ouf; + + i++; + j--; + } + } + + if (j > 0) { + po_sort(po_arr, ouf_arr, j + 1); + } + if (i < po_count) { + po_sort(&po_arr[i], &ouf_arr[i], po_count - i); } } +/** + * @brief Assign an objectId to a perceived object according to its LDM identifier. + * + * The object is assumed to have an assigned objectId that corresponds to the LDM + * identifier, and an object age. + * This identifier is then replaced by the objectId. + * + * @note There is the assumption that a new perceived object never appear for the + * first time with an object age equal to the maximum (0x7FF). + * @note No verifications regarding the arguments are made. + * + * @param po Perceived object pointer. + * @param man CP object identifier manager. + * @param config CP configuration. + * @param now Current time in milliseconds (timestamp related to the perceived object). + */ +static void po_replace_id(EI2_PerceivedObject_t* po, cp_obj_id_t* man, cp_config_t* config, uint64_t now) { + uint16_t ldm_id, cps_id; + ldm_id = *po->objectId; + cps_id = man->ldm2cps[ldm_id]; + // The if-else block is used separated to simplify readability -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:; - } + // Object ID assigned + if (ldm_id != man->cps2ldm[cps_id] || cps_id == UNUSED_PO_ID) { + goto assign; } + // Retention period exceeded + else if ((now - man->timestamp[cps_id]) >= config->UnusedObjectIdRetentionPeriod) { + goto assign; + } + // New object with same LDM identifier + // TODO: uncomment this and assert correct object age + // else if ((*po->objectAge < 0x7FF) && (now - *po->objectAge) > (man->timestamp[cps_id] - man->age[cps_id])) { + // goto assign; + // } + + goto update; // object id already assigned and within retention period + +assign: + do { + cps_id = random() % UNUSED_PO_ID; // UNUSED_PO_ID is the greatest id, so it is used as the limit + } while (((now - man->timestamp[cps_id]) < config->UnusedObjectIdRetentionPeriod)); + +update: + man->ldm2cps[ldm_id] = cps_id; + man->cps2ldm[cps_id] = ldm_id; + man->age[cps_id] = *po->objectAge; + man->timestamp[cps_id] = now; + + *po->objectId = cps_id; } +/** + * @brief Assign a CPS sensor ID to a sensor information according to its sensor identifier (outside the CPS). + * + * The sensor information argument must have the sensorId field set to the sensor identifier (outside the CPS). + * + * @param si Sensor information pointer. + * @param man CP sensor identifier manager. + * @param config CP configuration. + * @param now Current time in milliseconds. + */ +static void si_replace_id(EI2_SensorInformation_t* si, cp_sensor_id_t* man, cp_config_t* config, uint64_t now) { + uint8_t sen_id, cps_id; -long rotate_x(long x, long y) { - return (long)(x * roadRotationCos - y * roadRotationSin); -} + sen_id = si->sensorId; + cps_id = man->sen2cps[sen_id]; -long rotate_y(long x, long y) { - return (long)(x * roadRotationSin + y * roadRotationCos); -} - -static void set_values(int i, int j, uint64_t timestamp, EI1_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(EI1_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 = EI1_ObjectRefPoint_bottomMid; - - /* Detected Object Class*/ - cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->classification = calloc(1, sizeof(EI1_ObjectClassDescription_t)); - EI1_ObjectClassDescription_t *class = cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->classification; - - class->list.size = 1 * sizeof(void*); - class->list.array = malloc(1* sizeof(void*)); - class->list.count = 1; - - class->list.array[0] = calloc(1, sizeof(EI1_ObjectClass_t)); - class->list.array[0]->confidence = 0; - int obj_len = (int)(as_objects[i].f_objectLength*10); - - if (obj_len == 10) { - class->list.array[0]->Class.present = EI1_class_PR_person; - class->list.array[0]->Class.choice.person.type = 1; // pedestrian - class->list.array[0]->Class.choice.person.confidence = 0; - } else if (obj_len == 16) { - class->list.array[0]->Class.present = EI1_class_PR_person; - class->list.array[0]->Class.choice.person.type = 3; // cyclist - class->list.array[0]->Class.choice.person.confidence = 0; - } else if (obj_len == 26) { - class->list.array[0]->Class.present = EI1_class_PR_vehicle; - class->list.array[0]->Class.choice.vehicle.type = 2; // motorcycle - class->list.array[0]->Class.choice.vehicle.confidence = 0; - } else if (obj_len >= 46 && obj_len <= 54) { - class->list.array[0]->Class.present = EI1_class_PR_vehicle; - class->list.array[0]->Class.choice.vehicle.type = 3; // passenger car - class->list.array[0]->Class.choice.vehicle.confidence = 0; - } else if (obj_len >= 56 && obj_len <= 88) { - class->list.array[0]->Class.present = EI1_class_PR_vehicle; - class->list.array[0]->Class.choice.vehicle.type = 5; // light truck - class->list.array[0]->Class.choice.vehicle.confidence = 0; - } else if (obj_len >= 90) { - class->list.array[0]->Class.present = EI1_class_PR_vehicle; - class->list.array[0]->Class.choice.vehicle.type = 6; // heavy truck - class->list.array[0]->Class.choice.vehicle.confidence = 0; - } else { - class->list.array[0]->Class.present = EI1_class_PR_other; - class->list.array[0]->Class.choice.other.type = 0; // unknown - class->list.array[0]->Class.choice.other.confidence = 0; + // CPS Sensor ID assigned + if (sen_id != man->cps2sen[cps_id] || cps_id == UNUSED_SI_ID) { + do { + cps_id = random() % UNUSED_SI_ID; // UNUSED_SI_ID is the highest possible id, so it is also used as the limit + } while ((now - man->timestamp[cps_id]) < config->UnusedSensorIdRetentionPeriod); } - /* 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 + man->sen2cps[sen_id] = cps_id; + man->cps2sen[cps_id] = sen_id; + man->timestamp[cps_id] = now; + + si->sensorId = cps_id; } +/** + * @brief Get the matching CPS sensor Id for a given sensor identifier. + * + * @param sensor_id Sensor identifier (outside the CPS). + * @param man CP sensor id manager. + * @return int CPS sensor Id if it exists, -1 otherwise. + */ +static int si_get_id(int sensor_id, cp_sensor_id_t* man) { + uint8_t cps_id; -static int mk_cpm(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[]) { + cps_id = man->sen2cps[sensor_id]; - /* Variables */ - EI1_CPM_t* cpm_tx = calloc(1, sizeof(EI1_CPM_t)); + if (sensor_id != man->cps2sen[cps_id] || cps_id == UNUSED_SI_ID) { + return -1; + } - long euclidian_dist, abs_speed, abs_speed_hist, angle, angle_hist, angle_diff; - int j = 0, rv = 0; - int temp[NOF_OBJECTS]; + return cps_id; +} +/** + * @brief Check if SensorInformationContainer should be included in the CPM and retrieve that information if so. + * Data to be sent in the CPM is written in the cp struct. + * The caller is responsible to free the data. + * + * @note Not thread safe. + * @return int 1 if SIC should be added, 0 otherwise. + */ +static int sic_check(uint64_t now) { + int rv = 0; + cp_t* cp = &facilities.cp; + EI2_SensorInformation_t* si = NULL; - 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); + pthread_mutex_lock(&cp->lock); + uint64_t T_elapsed = now - cp->lastSicTimestamp; - uint64_t generationDeltaTime = itss_time_get() % 65536; // generationDeltaTime = TimestampIts mod 65 536 + if (T_elapsed >= cp->config.T_AddSensorInformation) { + cp->lastSicTimestamp = now; + rv = 1; + } + pthread_mutex_unlock(&cp->lock); + + return rv; +} + +/** + * @brief Check if there are perception regions that should be included in the CPM. + * + * Currently not implemented because perception regions are not considered. + * + * @return int 1 if there are perception regions to be added, 0 otherwise. + */ +static int pr_check() { + int rv = 0; + return rv; +} + +/** + * @brief Check if there are perceived objects that should be included in the CPM. + * + * The objects received from the LDM have to use always with the same referencial, that is, the + * reference position of the vehicle(RSU) must be the same. + * In practice, this is only true in the RSU, where the reference position does not change over time. + * TODO: store the position of vehicle at the time of detection in the LDM. + */ +static void po_check(uint64_t now) { + cp_t* cp = &facilities.cp; + + EI2_PerceivedObject_t* po = NULL; // perceived object pointer + + bool type_A = false, // flag to check if the perceived object is type A + include_all_type_A = false; // flag to check if all type A objects should be included + int* type_A_arr = NULL; // array of indexes of type A objects + int type_A_count = 0; // number of type A objects + + double* ouf_arr = NULL; // array of object utility function values + + double epv_lat, // vehicle latitude + epv_lon, // vehicle longitude + quality, // perceived object quality + lat, // perceived object latitude + lon; // perceived object longitude + int speed, // perceived object speed + vel_ori; // perceived object velocity orientation - int32_t lat, lon, alt, alt_conf; itss_space_lock(); itss_space_get(); - lat = epv.space.data.latitude.value; - lon = epv.space.data.longitude.value; - alt = epv.space.data.altitude.value; - alt_conf = epv.space.data.altitude.confidence; + epv_lat = epv.space.data.latitude.value / 1.0E7; + epv_lon = epv.space.data.longitude.value / 1.0E7; itss_space_unlock(); - cpm_tx->cpm.generationDeltaTime = generationDeltaTime; - cpm_tx->cpm.cpmParameters.managementContainer.stationType = EI1_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 = EI1_HeadingValue_wgs84North; // TODO - cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.altitude.altitudeValue = alt; - cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.altitude.altitudeConfidence = alt_conf; + pthread_mutex_lock(&cp->lock); - if(dissemination_check(0) == 1){ /* Sensor Information Container Inclusion Management */ - - cpm_tx->cpm.cpmParameters.sensorInformationContainer = calloc(1, sizeof(EI1_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(EI1_SensorInformation_t)); - cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0] = calloc(1, sizeof(EI1_SensorInformation_t)); - - cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->sensorID = 0; - cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->type = EI1_SensorType_radar; - cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.present = EI1_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 = facilities.dissemination.opening_angle_start; - cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.stationaryHorizontalOpeningAngleEnd = facilities.dissemination.opening_angle_end; - cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.verticalOpeningAngleStart = calloc(1, sizeof(EI1_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(EI1_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(EI1_SensorHeight_t)); - (*cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.sensorHeight) = 600; - dissemination_reset_timer(0); + if (cp->config.ObjectInclusionConfig == 0) { + log_warn( + "[cp][po_check] ObjectInclusionConfig is set to 0 although there is no pre-defined rules for object " + "inclusion. Using default rules."); } - if (s_objectControl.u8_numberOfObjects > 0) { - cpm_tx->cpm.cpmParameters.perceivedObjectContainer = calloc(1, sizeof(EI1_PerceivedObjectContainer_t)); - cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array = calloc(s_objectControl.u8_numberOfObjects,sizeof(EI1_PerceivedObject_t*)); + po_get(cp->po_arr, &cp->po_count, MAX_PERCEIVED_OBJECTS, now, &cp->obj_id, &cp->sen_id); - cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.count = s_objectControl.u8_numberOfObjects; + cp->po_total = cp->po_count; - memcpy(temp, valid_array, NOF_OBJECTS * sizeof(int)); // NOF_OBJECTS * sizeof(int) = size of valid_array - memset(valid_array, 0, NOF_OBJECTS * sizeof(int)); + type_A_arr = malloc(cp->po_count * sizeof(int)); + ouf_arr = malloc(cp->po_count * sizeof(double)); - 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++; + for (int i = 0; i < cp->po_count; i++) { + po = cp->po_arr[i]; - }else{ // The object is going to be compared (It was included in previous CPMs) (valid_array[id] = 1) + if (po == NULL) log_error("[cp][po_check] po is NULL. Why?"); - // 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) ); + // Object perception quality exceeds threshold + if (!po->objectPerceptionQuality) { + log_warn("[cp][po_check] perceived object perception quality is not available"); + goto cleanobj; + } + if (*po->objectPerceptionQuality <= cp->config.ObjectPerceptionQualityThreshold) { + goto cleanobj; + } - // 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) + // The object has first been detected after the last CPM generation event + if (!po->objectAge) { + log_warn("[cp][po_check] perceived object age is not available"); + goto cleanobj; + } + if (*po->objectAge < (now - cp->lastCpmTimestamp)) { + continue; + } - // 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; + // Get perceived object type based on its classification + if (po_type(po, &type_A)) { + log_warn("[cp][po_check] could not get perceived object type"); + goto cleanobj; + } - // Requirements to include the object in the CPM (> 4 m or > 0.5 m/s or > 4º or > T_GenCpmMax) + // perceived object inclusion rules - type A + if (type_A) { + type_A_arr[type_A_count] = i; + type_A_count++; + if (!include_all_type_A) { + include_all_type_A = (now - cp->history.inclusion[*po->objectId]) >= (cp->config.T_GenCpmMax / 2); + } + continue; + } - 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++; + // perceived object inclusion rules - type B + if ((now - cp->history.inclusion[*po->objectId]) >= cp->config.T_GenCpmMax) { + // printf("time exceeded for object %d\n", *po->objectId); + continue; + } + po_position(po, epv_lat, epv_lon, &lat, &lon); + double diff = + it2s_geodesy_haversine(cp->history.position[*po->objectId].latitude, cp->history.position[*po->objectId].longitude, lat, lon); + if (diff > cp->config.minPositionChangeThreshold) { + // printf("position change exceeded for object %d\n", *po->objectId); + continue; + } + if (fabs(po_speed(po) - cp->history.speed[*po->objectId]) > cp->config.minGroundSpeedChangeThreshold) { + // printf("speed change exceeded for object %d\n", *po->objectId); + continue; + } + diff = po_vel_ori(po) - cp->history.vel_ori[*po->objectId]; + diff += (diff > 1800) ? -3600 : (diff < -1800) ? 3600 : 0; + if (fabs(diff) > cp->config.minGroundVelocityOrientationChangeThreshold) { + // printf("velocity orientation change exceeded for object %d\n", *po->objectId); + continue; + } - }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_EI1_CPM, NULL, cpm_tx, bdr_oer, 1500); - if (retval_enc_bdr.encoded == -1) { - log_error("[cp] failed encoding CPM (%s)", retval_enc_bdr.failed_type->name); - rv = 1; - goto cleanup; + cleanobj: + ASN_STRUCT_FREE(asn_DEF_EI2_PerceivedObject, po); + cp->po_arr[i] = NULL; } - *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_EI1_CPM, NULL, cpm_tx, fdi_oer, 1500); - if (retval_enc_fdi.encoded == -1) { - log_error("[cp] failed encoding CPM (%s)", retval_enc_fdi.failed_type->name); - rv = 1; - goto cleanup; + if (!include_all_type_A) { + for (int i = 0; i < type_A_count; i++) { + po = cp->po_arr[type_A_arr[i]]; + ASN_STRUCT_FREE(asn_DEF_EI2_PerceivedObject, po); + cp->po_arr[type_A_arr[i]] = NULL; + } } - *fdi_len = ((retval_enc_fdi.encoded + 7) / 8); + // re-arrange the perceived object array and compute the object utility function + int j = 0; + for (int i = 0; i < cp->po_count; i++) { + po = cp->po_arr[i]; + if (po != NULL) { + cp->po_arr[i] = cp->po_arr[j]; + cp->po_arr[j] = po; + ouf_arr[j] = po_ouf(po, &cp->history, &cp->config, now, epv_lat, epv_lon); + j++; + + // update history + cp->history.inclusion[*po->objectId] = now; + po_position(po, epv_lat, epv_lon, &lat, &lon); + cp->history.position[*po->objectId].latitude = lat; + cp->history.position[*po->objectId].longitude = lon; + cp->history.speed[*po->objectId] = po_speed(po); + cp->history.vel_ori[*po->objectId] = po_vel_ori(po); + } + } + cp->po_count = j; + + po_sort(cp->po_arr, ouf_arr, cp->po_count); + + pthread_mutex_unlock(&cp->lock); cleanup: - ASN_STRUCT_FREE(asn_DEF_EI1_CPM, cpm_tx); + free(type_A_arr); + free(ouf_arr); +} + +/** + * @brief Add an Originating RSU Container to a Collective Perception Message. + * + * Missing fields: `mapReference`. + * + * @note Does not check the station type. + * @note Thread safe. + * @param cpm CPM pointer. + * @return int 0 if orc is added succesfully, different than 0 otherwise. + */ +static int mk_cpm_add_orc(EI2_CollectivePerceptionMessage_t* cpm) { + int rv = 0; + + EI2_WrappedCpmContainer_t* wcc = calloc(1, sizeof(EI2_WrappedCpmContainer_t)); + wcc->containerId = EI2_CpmContainerId_originatingRsuContainer; + wcc->containerData.present = EI2_containerData_PR_OriginatingRsuContainer; + + EI2_OriginatingRsuContainer_t* orc = &wcc->containerData.choice.OriginatingRsuContainer; + + rv = ASN_SEQUENCE_ADD(&cpm->payload.cpmContainers.list, wcc); + if (rv != 0) { + log_warn("[cp][mk_cpm_add_orc] could not add OriginatingRSUContainer to CPM"); + ASN_STRUCT_FREE(asn_DEF_EI2_WrappedCpmContainer, wcc); + } return rv; } -void *cp_service(void *arg){ - /* 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]; +/** + * @brief Add Originating Vehicle Container to a Collective Perception Message. + * + * Missing fields: `pitchAngle`, `rollAngle`, `trailerDataSet`. + * + * @note The orientationAngle is (wrongly) filled as the heading (see clause 7.1.4). + * @note Does not check the station type. + * @note Thread unsafe. + * @param cpm CPM pointer. + * @return int 0 if ovc is added successfully, different than 0 otherwise. + */ +static int mk_cpm_add_ovc(EI2_CollectivePerceptionMessage_t* cpm) { + int rv = 0; - memset(history_list, 0, sizeof(history_list)); - memset(valid_array, 0, sizeof(valid_array)); - memset(history_timestamp, 0, sizeof(history_timestamp)); + EI2_WrappedCpmContainer_t* wcc = calloc(1, sizeof(EI2_WrappedCpmContainer_t)); + wcc->containerId = EI2_CpmContainerId_originatingVehicleContainer; + wcc->containerData.present = EI2_containerData_PR_OriginatingVehicleContainer; - uint8_t tr_oer[INDICATION_BUFFER_SIZE]; - uint8_t fi_oer[INDICATION_BUFFER_SIZE]; - tr_oer[0] = ITSS_FACILITIES; //Facilities - fi_oer[0] = ITSS_FACILITIES; + EI2_OriginatingVehicleContainer_t* ovc = &wcc->containerData.choice.OriginatingVehicleContainer; + + itss_space_lock(); + itss_space_get(); + ovc->orientationAngle.value = epv.space.data.heading.value; + ovc->orientationAngle.confidence = epv.space.data.heading.confidence; + itss_space_unlock(); + + rv = ASN_SEQUENCE_ADD(&cpm->payload.cpmContainers.list, wcc); + if (rv != 0) { + log_warn("[cp][mk_cpm_add_ovc] could not add OriginatingVehicleContainer to CPM"); + ASN_STRUCT_FREE(asn_DEF_EI2_WrappedCpmContainer, wcc); + } + + return rv; +} + +/** + * @brief Add Sensor Information Container to a Collective Perception Message. + * + * The contents of the given array are copied to the message. + * + * @note Thread safe. + * @param cpm CPM pointer. + * @return int 0 if sic is added successfully, different than 0 otherwise. + */ +static int mk_cpm_add_sic(EI2_CollectivePerceptionMessage_t* cpm, EI2_SensorInformation_t** si_arr, size_t si_count, cp_sensor_id_t* man, + cp_config_t* config, uint64_t now) { + if (si_arr == NULL && si_count != 0) { + log_warn("[cp][mk_cpm_add_sic] sensor information array is NULL and its size is different than 0"); + return -1; + } + + int rv = 0; + + cp_t* cp = &facilities.cp; + + EI2_WrappedCpmContainer_t* wcc = calloc(1, sizeof(*wcc)); + wcc->containerId = EI2_CpmContainerId_sensorInformationContainer; + wcc->containerData.present = EI2_containerData_PR_SensorInformationContainer; + + EI2_SensorInformationContainer_t* sic = &wcc->containerData.choice.SensorInformationContainer; + + EI2_SensorInformation_t* si; + + for (size_t i = 0; i < si_count; i++) { + si = NULL; + rv = asn_copy(&asn_DEF_EI2_SensorInformation, (void**)&si, si_arr[i]); + if (rv != 0) { + log_warn("[cp][mk_cpm_add_sic] could not copy SensorInformation"); + continue; + } + + si_replace_id(si, man, config, now); + + rv = ASN_SEQUENCE_ADD(&sic->list, si); + if (rv != 0) { + log_warn("[cp][mk_cpm_add_sic] could not add SensorInformation to SensorInformationContainer"); + ASN_STRUCT_FREE(asn_DEF_EI2_SensorInformation, si); + } + } + + rv = ASN_SEQUENCE_ADD(&cpm->payload.cpmContainers.list, wcc); + if (rv != 0) { + log_warn("[cp][mk_cpm_add_sic] could not add SensorInformationContainer to CPM"); + ASN_STRUCT_FREE(asn_DEF_EI2_WrappedCpmContainer, wcc); + } + + return rv; +} + +/** + * @brief Add Perception Region Container to a Collective Perception Message. + * + * @note An empty PRC can be added to the message if `pr_count` is set to 0. + * @note Not used, since perception regions are not considered. + * + * @param cpm CPM pointer. + * @return int 0 if prc is added successfully, !=0 otherwise. + */ +static int mk_cpm_add_prc(EI2_CollectivePerceptionMessage_t* cpm, EI2_PerceptionRegion_t** pr_arr, size_t pr_count) { + if (pr_arr == NULL && pr_count != 0) { + log_warn("[cp][mk_cpm_add_prc] perception region array is NULL and its size is different than 0"); + return -1; + } + + int rv = 0; + + EI2_WrappedCpmContainer_t* wcc = calloc(1, sizeof(*wcc)); + wcc->containerId = EI2_CpmContainerId_perceptionRegionContainer; + wcc->containerData.present = EI2_containerData_PR_PerceptionRegionContainer; + + EI2_PerceptionRegionContainer_t* prc = &wcc->containerData.choice.PerceptionRegionContainer; + + EI2_PerceptionRegion_t* pr; + + for (size_t i = 0; i < pr_count; i++) { + pr = NULL; // asn_copy allocates the memory + rv = asn_copy(&asn_DEF_EI2_PerceptionRegion, (void**)&pr, pr_arr[i]); + if (rv != 0) { + log_warn("[cp][mk_cpm_add_prc] could not copy PerceptionRegion"); + continue; + } + + rv = ASN_SEQUENCE_ADD(&prc->list, pr); + if (rv != 0) { + log_warn("[cp][mk_cpm_add_prc] could not add PerceptionRegion to PerceptionRegionContainer"); + ASN_STRUCT_FREE(asn_DEF_EI2_PerceptionRegion, pr); + } + } + + rv = ASN_SEQUENCE_ADD(&cpm->payload.cpmContainers.list, wcc); + if (rv != 0) { + log_warn("[cp][mk_cpm_add_prc] could not add PerceptionRegionContainer to CPM"); + ASN_STRUCT_FREE(asn_DEF_EI2_WrappedCpmContainer, wcc); + } + + return rv; +} + +/** + * @brief Add Perceived Object Container to a Collective Perception Message. + * + * Each perceived object in the array received must already have the parameters defined. + * + * Because the objects need to be compared and verified before the message assembly to check + * + * @note An empty POC can be added to the message if `po_count` is set to 0. + * @note It is assumed that the perceived objects fit into a single cpm. + * + * @param cpm CPM pointer. + * @param po_arr Array of PerceivedObject_t to be added to the CPM. + * @param po_count Length of the PerceivedObject_t array. + * @param po_total Total number of perceived objects in the LDM. + * @return int 0 if poc is added successfully, !=0 otherwise. + */ +static int mk_cpm_add_poc(EI2_CollectivePerceptionMessage_t* cpm, EI2_PerceivedObject_t** po_arr, size_t po_count, size_t po_total) { + if (po_arr == NULL && po_count != 0) { + log_warn("[cp][mk_cpm_add_poc] perceived object array is NULL and its size is different than 0"); + return 1; + } + + int rv = 0; + + EI2_WrappedCpmContainer_t* wcc = calloc(1, sizeof(*wcc)); + wcc->containerId = EI2_CpmContainerId_perceivedObjectContainer; + wcc->containerData.present = EI2_containerData_PR_PerceivedObjectContainer; + + EI2_PerceivedObjectContainer_t* poc = &wcc->containerData.choice.PerceivedObjectContainer; + poc->numberOfPerceivedObjects = po_total > 255 ? 255 : po_total; + + EI2_PerceivedObject_t* po; + + for (size_t i = 0; i < po_count; i++) { + po = NULL; // asn_copy allocates the memory + rv = asn_copy(&asn_DEF_EI2_PerceivedObject, (void**)&po, po_arr[i]); + if (rv != 0) { + log_warn("[cp][mk_cpm_add_poc] could not copy PerceivedObject"); + continue; + } + + rv = ASN_SEQUENCE_ADD(&poc->perceivedObjects.list, po); + if (rv != 0) { + log_warn("[cp][mk_cpm_add_poc] could not add PerceivedObject to PerceivedObjectContainer"); + ASN_STRUCT_FREE(asn_DEF_EI2_PerceivedObject, po); + } + } + + rv = ASN_SEQUENCE_ADD(&cpm->payload.cpmContainers.list, wcc); + if (rv != 0) { + log_warn("[cp][mk_cpm_add_poc] could not add PerceivedObjectContainer to CPM"); + ASN_STRUCT_FREE(asn_DEF_EI2_WrappedCpmContainer, wcc); + } + + return rv; +} + +/** + * @brief Make a CollectivePerceptionMessage. + * + * @note Does not support message segmentation. Total number of messages generated is always 1. + * @note Does not check if the message is too large to be transmitted. + * @note Does not check if the total number of objects fits into one CPM. + * @note Perception region based message assembly (clause 6.1.3.3) is unavailable. + * @note Not thread safe. + * + * @param cpm_oer + * @param cpm_len + * @param now + * @return int + */ +static int mk_cpm(uint8_t* cpm_oer, uint16_t* cpm_len, uint64_t now) { + int rv = 0; + + cp_t* cp = &facilities.cp; + + EI2_CollectivePerceptionMessage_t* cpm = calloc(1, sizeof(EI2_CollectivePerceptionMessage_t)); + + cpm->header.protocolVersion = 2; + cpm->header.messageId = EI2_MessageId_cpm; + pthread_mutex_lock(&facilities.id.lock); + cpm->header.stationId = facilities.id.station_id; + pthread_mutex_unlock(&facilities.id.lock); + + EI2_CpmManagementContainer_t* mc = &cpm->payload.managementContainer; + + rv = asn_uint642INTEGER(&mc->referenceTime, now); + if (rv != 0) { + log_warn("[cp][mk_cpm] could not store the timestamp on the CPM"); + goto cleanup; + } + + itss_space_lock(); + itss_space_get(); + mc->referencePosition.latitude = epv.space.data.latitude.value; + mc->referencePosition.longitude = epv.space.data.longitude.value; + mc->referencePosition.positionConfidenceEllipse.semiMajorConfidence = EI2_SemiAxisLength_unavailable; + mc->referencePosition.positionConfidenceEllipse.semiMinorConfidence = EI2_SemiAxisLength_unavailable; + mc->referencePosition.positionConfidenceEllipse.semiMajorOrientation = EI2_HeadingValue_unavailable; + + mc->referencePosition.altitude.altitudeValue = epv.space.data.altitude.value; + mc->referencePosition.altitude.altitudeConfidence = epv.space.data.altitude.confidence; + itss_space_unlock(); + + mc->segmentationInfo = calloc(1, sizeof(*mc->segmentationInfo)); + mc->segmentationInfo->thisMsgNo = 1; + mc->segmentationInfo->totalMsgNo = 1; + + int man = 0, exp = 0; + pthread_mutex_lock(&cp->lock); + uint64_t T_GenCpmMin = cp->config.T_GenCpmMin; + uint64_t T_GenCpmMax = cp->config.T_GenCpmMax; + pthread_mutex_unlock(&facilities.cp.lock); + + rv = calc_message_rate((int)T_GenCpmMin, &man, &exp); + if (rv == 0) { + mc->messageRateRange = calloc(1, sizeof(*mc->messageRateRange)); + mc->messageRateRange->messageRateMin.mantissa = man; + mc->messageRateRange->messageRateMin.exponent = exp; + + rv = calc_message_rate((int)T_GenCpmMax, &man, &exp); + if (rv == 0) { + mc->messageRateRange->messageRateMax.mantissa = man; + mc->messageRateRange->messageRateMax.exponent = exp; + } else { + log_warn("[cp] failed to compute a valid maximum message rate", NULL); + ASN_STRUCT_FREE(asn_DEF_EI2_MessageRateRange, mc->messageRateRange); + } + } else { + log_warn("[cp] failed to compute a valid minimum message rate", NULL); + } + + if (facilities.station_type == 15) { + rv = mk_cpm_add_orc(cpm); + } else { + rv = mk_cpm_add_ovc(cpm); + } + if (rv != 0) { + log_error("[cp][mk_cpm] failed to create message, originating station container could not be added"); + goto cleanup; + } + + pthread_mutex_lock(&cp->lock); + if (cp->config.MessageAssemblyConfig != 0) { + log_warn( + "[cp][mk_cpm] CPM Assembly is only available according to clause 6.1.3.2 of the standard (Object Utility " + "Function). Please set MessageAssemblyConfig to 0."); + } + pthread_mutex_unlock(&cp->lock); + + if (sic_check(now)) { + pthread_mutex_lock(&cp->lock); + rv = mk_cpm_add_sic(cpm, cp->si_arr, cp->si_count, &cp->sen_id, &cp->config, now); + pthread_mutex_unlock(&cp->lock); + if (rv != 0) { + log_error("[cp][mk_cpm] failed to create message, sensor information container could not be added"); + goto cleanup; + } + } + + if (pr_check()) { + pthread_mutex_lock(&cp->lock); + rv = mk_cpm_add_prc(cpm, cp->pr_arr, cp->pr_count); + pthread_mutex_unlock(&cp->lock); + if (rv != 0) { + log_error("[cp][mk_cpm] failed to create message, perception region container could not be added"); + goto cleanup; + } + } + + // poc is an obligatory container + po_check(now); // check perceived objects for inclusion + pthread_mutex_lock(&cp->lock); + rv = mk_cpm_add_poc(cpm, cp->po_arr, cp->po_count, cp->po_total); + pthread_mutex_unlock(&cp->lock); + if (rv != 0) { + log_error("[cp][mk_cpm] failed to create message, perceived object container could not be added"); + goto cleanup; + } + + cp->lastCpmTimestamp = now; + + asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_EI2_CollectivePerceptionMessage, NULL, cpm, cpm_oer, 2048); + if (enc.encoded == -1) { + log_error("[cp][mk_cpm] failed to encode CPM (%s)", enc.failed_type->name); + // asn_fprint(stdout, &asn_DEF_EI2_CollectivePerceptionMessage, cpm); + rv = 1; + goto cleanup; + } + *cpm_len = (enc.encoded + 7) / 8; + //asn_fprint(stdout, &asn_DEF_EI2_CollectivePerceptionMessage, cpm); + +cleanup: + ASN_STRUCT_FREE(asn_DEF_EI2_CollectivePerceptionMessage, cpm); + + pthread_mutex_lock(&cp->lock); + for (int i = 0; i < cp->pr_count; i++) { + ASN_STRUCT_FREE(asn_DEF_EI2_PerceptionRegion, cp->pr_arr[i]); + cp->pr_arr[i] = NULL; + } + cp->pr_count = 0; + + for (int i = 0; i < cp->po_count; i++) { + ASN_STRUCT_FREE(asn_DEF_EI2_PerceivedObject, cp->po_arr[i]); + cp->po_arr[i] = NULL; + } + cp->po_count = 0; + pthread_mutex_unlock(&cp->lock); + + return rv; +} + +int cp_init() { + cp_t* cp = &facilities.cp; + + cp->lastCpmTimestamp = 0; + cp->lastSicTimestamp = 0; + + // default configuration for CPS (according to standard) + cp->config.T_GenCpmMin = 100; + cp->config.T_GenCpmMax = 1000; + cp->config.T_AddSensorInformation = 1000; + cp->config.MaxPerceptionRegions = 8; + cp->config.ObjectInclusionConfig = 1; + cp->config.ObjectPerceptionQualityThreshold = 0; // default value is 3, but since this value may not be available, it is set as 0 + // for now (it must be set in the po either way) + cp->config.minPositionChangeThreshold = 4; + cp->config.minGroundSpeedChangeThreshold = 0.5; + cp->config.minGroundVelocityOrientationChangeThreshold = 4; + cp->config.MessageAssemblyConfig = 0; + cp->config.minPositionChangePriorityThreshold = 0; + cp->config.maxPositionChangePriorityThreshold = 8; + cp->config.minGroundSpeedChangePriorityThreshold = 0; + cp->config.maxGroundSpeedChangePriorityThreshold = 1; + cp->config.minGroundVelocityOrientationChangePriorityThreshold = 0; + cp->config.maxGroundVelocityOrientationChangePriorityThreshold = 8; + cp->config.minLastInclusionTimePriorityThreshold = 100; + cp->config.maxLastInclusionTimePriorityThreshold = 1000; + cp->config.UnusedSensorIdRetentionPeriod = 60 * 1000; + cp->config.UnusedObjectIdRetentionPeriod = 60 * 1000; + + cp->history.inclusion = calloc(TOTAL_PO_IDS, sizeof(uint64_t)); + cp->history.position = calloc(TOTAL_PO_IDS, sizeof(double) * 2); + cp->history.speed = calloc(TOTAL_PO_IDS, sizeof(double)); + cp->history.vel_ori = calloc(TOTAL_PO_IDS, sizeof(double)); + + // sensor information array shall then be filled by inside config.c, where the sensor information is available + cp->si_arr = calloc(MAX_SENSORS, sizeof(EI2_SensorInformation_t*)); + cp->si_count = 0; + + cp->pr_arr = calloc(MAX_PERCEPTION_REGIONS, sizeof(EI2_PerceptionRegion_t*)); + cp->pr_count = 0; + + cp->po_arr = calloc(MAX_PERCEIVED_OBJECTS, sizeof(EI2_PerceivedObject_t*)); + cp->po_count = 0; + + cp->po_total = 0; + + cp->sen_id.sen2cps = malloc(TOTAL_SI_IDS * sizeof(uint8_t)); + cp->sen_id.cps2sen = malloc(TOTAL_SI_IDS * sizeof(uint8_t)); + cp->sen_id.timestamp = calloc(TOTAL_SI_IDS, sizeof(uint64_t)); + + cp->obj_id.ldm2cps = malloc(TOTAL_PO_IDS * sizeof(uint16_t)); + cp->obj_id.cps2ldm = malloc(TOTAL_PO_IDS * sizeof(uint16_t)); + cp->obj_id.age = calloc(TOTAL_PO_IDS, sizeof(uint64_t)); + cp->obj_id.timestamp = calloc(TOTAL_PO_IDS, sizeof(uint64_t)); + + for (size_t i = 0; i < TOTAL_SI_IDS; i++) { + cp->sen_id.sen2cps[i] = UNUSED_SI_ID; + cp->sen_id.cps2sen[i] = UNUSED_SI_ID; + } + + for (size_t i = 0; i < TOTAL_PO_IDS; i++) { + cp->obj_id.ldm2cps[i] = UNUSED_PO_ID; + cp->obj_id.cps2ldm[i] = UNUSED_PO_ID; + } + + pthread_mutex_init(&cp->lock, NULL); + + return 0; +} + +int cp_check(uint64_t now) { + int rv = 0; + + cp_t* cp = &facilities.cp; + + pthread_mutex_lock(&cp->lock); + + if (cp->T_GenCpm < cp->config.T_GenCpmMin || cp->T_GenCpm > cp->config.T_GenCpmMax) { + log_error("[cp][cp_check] current T_GenCpm is out of boundaries, setting T_GenCpm to 100ms"); + cp->T_GenCpm = 100; + } + + uint64_t T_elapsed = now - cp->lastCpmTimestamp; + if (T_elapsed >= cp->T_GenCpm) { + rv = 1; + } + + pthread_mutex_unlock(&cp->lock); + + return rv; +} + +static int cp_register_to_ldm() { + int rv = 0; + + EI2_RegisterDataConsumerReq_t* req = calloc(1, sizeof(EI2_RegisterDataConsumerReq_t)); + EI2_RegisterDataConsumerResp_t* resp = NULL; + + req->applicationId.present = EI2_ITSaid0v0VarLengthNumber_PR_extension; + req->applicationId.choice.extension.present = EI2_ITSaid0v0Ext1_PR_content; + req->applicationId.choice.extension.choice.content = 639; + + EI2_Permission_t* permission = calloc(1, sizeof(EI2_Permission_t)); + *permission = EI2_DataObjectType_ism; + ASN_SEQUENCE_ADD(&req->accessPermissions.list, permission); + + uint8_t req_oer[256]; + req_oer[0] = 7; + asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_EI2_RegisterDataConsumerReq, NULL, req, req_oer + 1, 256 - 1); + if (enc.encoded == -1) { + log_error("failed to encode registration to LDM"); + rv = 1; + goto cleanup; + } + void* ldm_socket = itss_0connect(facilities.cp.ldm.address, ZMQ_REQ); + itss_0send(ldm_socket, req_oer, enc.encoded + 1); + + uint8_t resp_oer[256]; + int rl = itss_0recv_rt(&ldm_socket, resp_oer, 256, req_oer, enc.encoded + 1, 1000); + itss_0close(ldm_socket); + if (rl < 0) { + log_error("failed to receive registration response from LDM"); + rv = 1; + goto cleanup; + } + + resp = calloc(1, sizeof(EI2_RegisterDataConsumerResp_t)); + asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_EI2_RegisterDataConsumerResp, (void**)&resp, resp_oer, rl); + if (dec.code) { + log_error("<- invalid FR received"); + rv = 1; + goto cleanup; + } + + if (resp->result != EI2_RegisterDataConsumerResult_accepted) { + log_warn("ldm rejected the register request"); + rv = 1; + goto cleanup; + } + + bool perm_accepted = false; + if (resp->accessPermissions) { + for (int i = 0; i < resp->accessPermissions->list.count; ++i) { + EI2_Permission_t* p = resp->accessPermissions->list.array[i]; + if (*p == EI2_DataObjectType_ism) { + perm_accepted = true; + break; + } + } + } else { + perm_accepted = true; + } + + if (!perm_accepted) { + log_error("ldm did not grant permission to add perceived objects"); + rv = 1; + goto cleanup; + } + +cleanup: + ASN_STRUCT_FREE(asn_DEF_EI2_RegisterDataConsumerReq, req); + ASN_STRUCT_FREE(asn_DEF_EI2_RegisterDataConsumerResp, resp); + + return rv; +} + +static int cp_deregister_to_ldm() { + int rv = 0; + + EI2_DeregisterDataConsumerReq_t* req = calloc(1, sizeof(EI2_DeregisterDataConsumerReq_t)); + EI2_DeregisterDataConsumerResp_t* resp = NULL; + + req->applicationId.present = EI2_ITSaid0v0VarLengthNumber_PR_extension; + req->applicationId.choice.extension.present = EI2_ITSaid0v0Ext1_PR_content; + req->applicationId.choice.extension.choice.content = 639; + + uint8_t req_oer[256]; + req_oer[0] = 8; + asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_EI2_DeregisterDataConsumerReq, NULL, req, req_oer + 1, 256 - 1); + if (enc.encoded == -1) { + log_error("failed to encode deregistration to LDM"); + rv = 1; + goto cleanup; + } + void* ldm_socket = itss_0connect(facilities.cp.ldm.address, ZMQ_REQ); + itss_0send(ldm_socket, req_oer, enc.encoded + 1); + + uint8_t resp_oer[256]; + int rl = itss_0recv_rt(&ldm_socket, resp_oer, 256, req_oer, enc.encoded + 1, 1000); + itss_0close(ldm_socket); + if (rl < 0) { + log_error("failed to receive deregistration response from LDM"); + rv = 1; + goto cleanup; + } + + resp = calloc(1, sizeof(EI2_DeregisterDataConsumerResp_t)); + asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_EI2_DeregisterDataConsumerResp, (void**)&resp, resp_oer, rl); + if (dec.code) { + log_error("<- invalid FR received"); + rv = 1; + goto cleanup; + } + + if (resp->ack != EI2_DeregisterDataConsumerAck_succeed) { + log_warn("ldm rejected the deregister request"); + rv = 1; + goto cleanup; + } + +cleanup: + ASN_STRUCT_FREE(asn_DEF_EI2_DeregisterDataConsumerReq, req); + ASN_STRUCT_FREE(asn_DEF_EI2_DeregisterDataConsumerResp, resp); + + return rv; +} + +void* cp_service() { + int rv = 0; + srand(time(NULL)); EIS_NetworkingRequest_t* nr = calloc(1, sizeof(EIS_NetworkingRequest_t)); nr->present = EIS_NetworkingRequest_PR_packet; @@ -726,107 +1426,112 @@ void *cp_service(void *arg){ npr->transport.choice.btp.btpType = EIS_BTPType_btpB; npr->transport.choice.btp.destinationPort = EIS_Port_cpm; + const int buf_len = 2048; + npr->data.buf = malloc(buf_len); + + // Fill header for FacilitiesIndication and FacilitiesMessageIndication structs + EIS_FacilitiesIndication_t* fi = calloc(1, sizeof(EIS_FacilitiesIndication_t)); fi->present = EIS_FacilitiesIndication_PR_message; EIS_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); - - - npr->data.buf = malloc(1500); //CPM Data to be sent to the Networking layer - - /*--- Fill mandatory Facilities Message Indication parameters ---*/ fmi->itsMessageType = EIS_ItsMessageType_cpm; - fmi->data.buf = malloc(1500); + fmi->data.buf = malloc(buf_len); - /* Creating sockets and waiting for radar to connect*/ - is_radar_connected = radar_connection(RADAR_PORT); + uint8_t nr_oer[buf_len]; + uint8_t fi_oer[buf_len]; + nr_oer[0] = ITSS_FACILITIES; + fi_oer[0] = ITSS_FACILITIES; - while(!facilities.exit){ - itss_usleep(1000*50); + uint64_t now; + itss_queue_packet_t* qp; - /* 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(1)) { - 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(npr->data.buf, (uint32_t *) &npr->data.size, fmi->data.buf, (uint32_t *) &fmi->data.size, history_list, valid_array, history_timestamp) == 1) - continue; - - uint32_t id = itss_id(npr->data.buf, npr->data.size); - npr->id = id; - fmi->id = id; - - /* Encode NetworkingRequest */ - asn_enc_rval_t enc_tdr = oer_encode_to_buffer(&asn_DEF_EIS_NetworkingRequest, NULL, nr, tr_oer+1, INDICATION_BUFFER_SIZE-1); - if(enc_tdr.encoded == -1){ - log_error("encoding TR for cpm failed"); - continue; - } - - /* Encode FacilitiesIndication */ - asn_enc_rval_t enc_fdi = oer_encode_to_buffer(&asn_DEF_EIS_FacilitiesIndication, NULL, fi, fi_oer+1, INDICATION_BUFFER_SIZE-1); - if(enc_fdi.encoded == -1){ - log_error("encoding FI for cpm failed"); - continue; - } - - /* Create thread to send packet to the Networking Layer (=3) */ - itss_queue_send(facilities.tx_queue, itss_queue_packet_new(tr_oer, enc_tdr.encoded+1, ITSS_NETWORKING, id, "NR.packet.btp")); - - /* Create thread to send packet to the Applications Layer (=5) */ - itss_queue_send(facilities.tx_queue, itss_queue_packet_new(fi_oer, enc_fdi.encoded+1, ITSS_APPLICATIONS, id, "FI.message")); - - /*Reset Timer for dissemination control */ - dissemination_reset_timer(1); - - // Logging - if (facilities.logging.dbms) { - pthread_mutex_lock(&facilities.id.lock); - uint32_t station_id = facilities.id.station_id; - pthread_mutex_unlock(&facilities.id.lock); - itss_db_add(facilities.logging.dbms, station_id, npr->id, true, 14, NULL, npr->data.buf, npr->data.size); - } - if (facilities.logging.recorder) { - uint16_t buffer_len = INDICATION_BUFFER_SIZE; - uint8_t buffer[buffer_len]; - int e = itss_management_record_packet_sdu( - buffer, - buffer_len, - npr->data.buf, - npr->data.size, - npr->id, - itss_time_get(), - ITSS_FACILITIES, - true); - if (e != -1) { - itss_queue_send(facilities.tx_queue, itss_queue_packet_new(buffer, e, ITSS_MANAGEMENT, npr->id, "MReq.packet.set")); - } - } - - }else{ - is_radar_connected = radar_connection(RADAR_PORT); - } + // Register to LDM as a consumer + if (facilities.cp.ldm.active) { + if (cp_register_to_ldm() == 0) { + log_info("registered to LDM as a consumer"); + facilities.cp.ldm.registered = true; + ldm_socket = itss_0connect(facilities.cp.ldm.address, ZMQ_REQ); + } else { + log_warn("could not register to LDM! Empty CPMs will be generated..."); + facilities.cp.ldm.registered = false; + ldm_socket = NULL; } } + while (!facilities.exit) { + itss_usleep(1000 * 50); + + now = itss_time_get(); + + if (cp_check(now) && facilities.cp.active) { + rv = mk_cpm(npr->data.buf, (uint16_t*)&npr->data.size, now); + if (rv) { + continue; + } + + memcpy(fmi->data.buf, npr->data.buf, npr->data.size); + fmi->data.size = npr->data.size; + + npr->network.choice.gn.communicationProfile = 0; + + uint32_t id = itss_id(npr->data.buf, npr->data.size); + npr->id = id; + fmi->id = id; + + asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_EIS_NetworkingRequest, NULL, nr, nr_oer + 1, buf_len - 1); + if (enc.encoded == -1) { + log_error("[cp] encoding TR for cpm failed"); + continue; + } + + asn_enc_rval_t enc_fdi = oer_encode_to_buffer(&asn_DEF_EIS_FacilitiesIndication, NULL, fi, fi_oer + 1, buf_len - 1); + if (enc_fdi.encoded == -1) { + log_error("[cp] encoding FI for cpm failed"); + continue; + } + + qp = itss_queue_packet_new(nr_oer, enc.encoded + 1, ITSS_NETWORKING, id, "NR.packet.btp"); + if (itss_queue_send(facilities.tx_queue, qp) == 1) { + log_warn("[cp] failed to send NR packet! Queue full..."); + itss_queue_packet_free(qp); + } + + qp = itss_queue_packet_new(fi_oer, enc_fdi.encoded + 1, ITSS_APPLICATIONS, id, "FI.message"); + if (itss_queue_send(facilities.tx_queue, qp) == 1) { + log_warn("[cp] failed to send FI message! Queue full..."); + itss_queue_packet_free(qp); + } + + // Logging + if (facilities.logging.dbms) { + pthread_mutex_lock(&facilities.id.lock); + uint32_t station_id = facilities.id.station_id; + pthread_mutex_unlock(&facilities.id.lock); + itss_db_add(facilities.logging.dbms, station_id, npr->id, true, EI2_MessageId_cpm, NULL, npr->data.buf, npr->data.size); + } + + if (facilities.logging.recorder) { + uint16_t buffer_len = 2048; + uint8_t buffer[buffer_len]; + int e = itss_management_record_packet_sdu(buffer, buffer_len, npr->data.buf, npr->data.size, id, itss_time_get(), + ITSS_FACILITIES, true); + if (e != -1) { + itss_queue_send(facilities.tx_queue, itss_queue_packet_new(buffer, e, ITSS_MANAGEMENT, id, "MReq.packet.set")); + } + } + } + } + + if (facilities.cp.ldm.registered) { + cp_deregister_to_ldm(); + } + + if (ldm_socket) { + itss_0close(ldm_socket); + } + ASN_STRUCT_FREE(asn_DEF_EIS_NetworkingRequest, nr); ASN_STRUCT_FREE(asn_DEF_EIS_FacilitiesIndication, fi); - /* Close sockets */ - if(facilities.dissemination.tmc_connect) - shutdown(s_socket.i32_socket,2); - - shutdown(raw_socket.raw_fd,2); - return NULL; + return NULL; } diff --git a/src/cpm.h b/src/cpm.h index 160e1a7..fe0d330 100644 --- a/src/cpm.h +++ b/src/cpm.h @@ -1,187 +1,210 @@ +/** + * @file cpmv2.h + * @author Diogo Jesus (diogopjesus@ua.pt) + * @brief Collective Perception Service implementation. + */ + #ifndef FACILITIES_CPM_H #define FACILITIES_CPM_H - -#include +#include +#include +#include #include +#include -#include -#include -#include -#include -#include -#include +#define MAX_SENSORS \ + 128 ///< Maximum number of sensors allowed to exist inside the sensing system. (128 is the standard defined value for the maximum + ///< number of sensors in a CPM) +#define MAX_PERCEPTION_REGIONS \ + 256 ///< Maximum number of perception regions allowed to be retrived from the LDM. (8 is standard defined value for the maximum number + ///< of perception regions in a CPM) +#define MAX_PERCEIVED_OBJECTS \ + 1024 ///< Maximum number of perceived objects allowed to be retrieved from the LDM. (arbitrary value to help with memory allocation). -#define READ_BUFFER_SIZE 1024 // Buffer for Serial and Ethernet read function (Used in cp_service) -#define NOF_OBJECTS 127 // Number of Object UMRR-0C -#define MESSAGE_ID 14 -#define PROTOCOL_VERSION 1 -#define RADAR_PORT "55555" // Destination port from the radar +#define TOTAL_PO_IDS (1 << 16) ///< Total number of possible object identifiers. +#define UNUSED_PO_ID (TOTAL_PO_IDS - 1) ///< Value used to indicate an unused object identifier (last identifier). +#define TOTAL_SI_IDS (1 << 8) ///< Total number of possible sensor identifiers. +#define UNUSED_SI_ID (TOTAL_SI_IDS - 1) ///< Value used to indicate an unused sensor identifier (last identifier). +/** + * @brief CPS configuration struct. + * + * Variable names match the standard ts_103324v020101. + * For more information regarding each field, please refer to Annex F of the standard. + * + * Parameter names follow the standard (Annex F). + * Type matching: + * `Time(ms)` to `uint64_t`, + * `Number` to `uint8_t`, + * `Identifier` to `uint8_t`, + * `Ratio` to `double`. + * `Distance(m)` to `double`. + * `Speed(m/s)` to `double`. + * `Orientation(degrees)` to `double`. + * `Time(s)` to `uint64_t`. + * + * @note In the standard the `UnusedSensorIdRetentionPeriod` and `UnusedObjectIdRetentionPeriod` are defined + * in seconds (`Time(s)`). However, to maintain consistency, this values will be treated as `Time(ms)` instead. + */ +typedef struct cp_config { + uint64_t T_GenCpmMin; ///< Lower limit for the time T_GenCpm elapsed between two consecutive CPM generation events. + uint64_t T_GenCpmMax; ///< Upper limit for the time T_GenCpm elapsed between two consecutive CPM generation events. + uint64_t T_AddSensorInformation; ///< Time after which the SensorInformationContainer is usually included again into a CPM -/* Structures */ + uint8_t MaxPerceptionRegions; ///< Maximum number of perception regions included into a CPM to constrain the message size + uint8_t ObjectInclusionConfig; ///< CPS configuration parameter that indicates the object inclusion management to be employed + ///< by the disemminating ITS-S. -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; + double ObjectPerceptionQualityThreshold; ///< Threshold of the objectPerceptionQuality defining the lower limit for objects + ///< to be included into a CPM. + double minPositionChangeThreshold; ///< Minimum Euclidean distance between the current estimated position of the reference + ///< point of a perceived object and the estimated position of the reference point lastly + ///< included in a CPM + double minGroundSpeedChangeThreshold; ///< Minimum difference between the current estimated ground speed of the reference + ///< point of a perceived object and the estimated ground speed of the reference point + ///< lastly included in a CPM + double minGroundVelocityOrientationChangeThreshold; ///< Minimum difference between the current estimated ground velocity + ///< orientation of the reference point of a perceived object and the + ///< estimated ground velocity orientation of the reference point lastly + ///< included in a CPM -typedef struct -{ - int raw_fd; - struct sockaddr_ll sll; - struct ifreq ifr; -} S_INTERFACE_CONNECTION_T; + uint8_t MessageAssemblyConfig; ///< CPS configuration parameter that indicates the CPM assembly mechanism to be employed by + ///< the disemminating ITS-S -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; + double minPositionChangePriorityThreshold; ///< Lower limit for which the Euclidean distance between the current estimated + ///< position of the reference point of the object and the estimated position of + ///< the reference point of this object lastly included in a CPM starts to + ///< contribute to the object utility function + double maxPositionChangePriorityThreshold; ///< Upper limit for which the Euclidean distance between the current estimated + ///< position of the reference point of the object and the estimated position of + ///< the reference point of this object lastly included in a CPM starts to + ///< contribute to the object utility function + double minGroundSpeedChangePriorityThreshold; ///< Lower limit for which the difference between the current estimated ground + ///< speed of the reference point of the object and the estimated ground speed + ///< of the reference point of this object lastly included in a CPM starts to + ///< contribute to the object utility function + double maxGroundSpeedChangePriorityThreshold; ///< Upper limit for which the difference between the current estimated ground + ///< speed of the reference point of the object and the estimated ground speed + ///< of the reference point of this object lastly included in a CPM starts to + ///< contribute to the object utility function + double minGroundVelocityOrientationChangePriorityThreshold; ///< Lower limit for which the difference between the current + ///< estimated ground velocity orientation of the reference point + ///< of the object and the estimated ground velocity orientation + ///< of the reference point of this object lastly included in a + ///< CPM starts to contribute to the object utility function + double maxGroundVelocityOrientationChangePriorityThreshold; ///< Upper limit for which the difference between the current + ///< estimated ground velocity orientation of the reference point + ///< of the object and the estimated ground velocity orientation + ///< of the reference point of this object lastly included in a + ///< CPM starts to contribute to the object utility function + uint64_t minLastInclusionTimePriorityThreshold; ///< Lower limit for which the time elapsed since the last inclusion of the + ///< object into a CPM starts to contribute to the object utility function + uint64_t maxLastInclusionTimePriorityThreshold; ///< Upper limit for which the time elapsed since the last inclusion of the + ///< object into a CPM starts to contribute to the object utility function -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; + uint64_t UnusedSensorIdRetentionPeriod; ///< Minimum time to ellapse before an ID used to identify a sensor can be reused + ///< after it has last been used + uint64_t UnusedObjectIdRetentionPeriod; ///< Minimum time to ellapse before an ID used to identify an object can be reused + ///< after it has last been used +} cp_config_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; +/** + * @brief CP Perceived Object History struct. + * + * Contains data related to the inclusion management of objects into a CPM. + */ +typedef struct cp_history { + uint64_t* inclusion; ///< Timestamp of the last inclusion of an object into a CPM. (milliseconds) + struct { + double latitude; // degrees + double longitude; // degrees + }* position; ///< Position of an object at the time of the last inclusion into a CPM. (degrees, degrees) + double* speed; ///< Speed of an object at the time of the last inclusion into a CPM. (metes per second) + double* vel_ori; ///< Velocity orientation of an object at the time of the last inclusion into a CPM. (degrees) +} cp_history_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; +/** + * @brief Bi-directional mapping of LDM object IDs to CPS object IDs. + * @note There is the assumption that the sensor identifier (outside the CPS) does not change. + */ +typedef struct cp_obj_id { + uint16_t* ldm2cps; ///< Maps LDM object IDs to CPS object IDs. + uint16_t* cps2ldm; ///< Maps CPS object IDs to LDM object IDs. + uint16_t* age; ///< Age associated to an object with a CPS object ID. + uint64_t* timestamp; ///< Timestamp associated to an object with a CPS object ID. +} cp_obj_id_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 -}; +/** + * @brief Bi-directional mapping of sensor IDs (ouside the CPS) to CPS sensor IDs. + */ +typedef struct cp_sensor_id { + uint8_t* sen2cps; ///< Maps sensor IDs (outside CPS) to CPS sensor IDs. + uint8_t* cps2sen; ///< Maps CPS sensor IDs to sensor IDs (outside CPS). -typedef struct -{ + uint64_t* timestamp; ///< Timestamp associated to a sensor with a CPS sensor ID. +} cp_sensor_id_t; - bool active; - bool tmc_connect; - char* int_radar; - char* ip_radar; +/** + * @brief Main CPS struct. + */ +typedef struct cp { + bool active; ///< Flag to signal the CPS is active. - pthread_mutex_t lock; + pthread_mutex_t lock; ///< Mutex to protect the CP struct. - uint32_t type; + uint64_t T_GenCpm; ///< CPM generation period. - uint64_t next_cpm_max; - uint64_t next_cpm_min; - + uint64_t lastCpmTimestamp; ///< Timestamp of the last CPM sent. + uint64_t lastSicTimestamp; ///< Timestamp of the last SIC sent. - uint64_t T_GenCpmMin; - uint64_t T_GenCpmMax; + cp_config_t config; ///< CPS configuration. + cp_history_t history; ///< CPS history. - uint64_t T_AddSensorInformation; - uint64_t next_AddSensorInformation; + EI2_SensorInformation_t** si_arr; ///< Array of sensor information. + size_t si_count; ///< Number of sensor information in the sensor information array. + EI2_PerceptionRegion_t** pr_arr; ///< Array of perception regions. + size_t pr_count; ///< Number of perception regions in the perception region array. -/* Position of the radar (Value from toml) */ - - int64_t radar_rotation; - int16_t opening_angle_start; - int16_t opening_angle_end; - -} dissemination_t; + EI2_PerceivedObject_t** po_arr; ///< Array of perceived objects. + size_t po_count; ///< Number of perceived objects in the perceived object array. -/* Prototype Functions */ + size_t po_total; ///< Total number of perceived objects in the LDM (as per standard) -/* -Summary : Waits for client to connect (Radar) -*/ + cp_sensor_id_t sen_id; ///< Sensor ID mapping. + cp_obj_id_t obj_id; ///< Object ID mapping. -bool waitingIncomingConnection(void); + struct { + bool active; + bool registered; + char* address; + } ldm; +} cp_t; +/** + * @brief Initialize the main CP struct. + * + * @return Always zero. + */ +int cp_init(); +/** + * @brief Check if a CPM should be sent. + * + * Also checks if the CPS should reset the ID mappings after a pseudonym change (reset_id flag). + * + * @arg now Current time in milliseconds. + * @return true if a CPM should be sent, false otherwise. + */ +int cp_check(uint64_t now); -/* -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 -*/ +/** + * @brief Main CP service. + * + * @return NULL + */ +void* cp_service(); -bool initEthernetConnection(const char* radar_ip,const char* radar_port); - - - -/* -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 *arg); - -#endif +#endif // FACILITIES_CPM_H diff --git a/src/facilities.c b/src/facilities.c index f9e8b54..4e7bfc5 100644 --- a/src/facilities.c +++ b/src/facilities.c @@ -1,7 +1,6 @@ #include "facilities.h" #include -#include #include #include #include @@ -17,6 +16,7 @@ #include #include #include +#include #include #include #include @@ -367,7 +367,7 @@ int main() { lightship_init(); den_init(); infrastructure_init(); - dissemination_init(); + cp_init(); bulletin_init(); void *security_socket = NULL; @@ -390,8 +390,7 @@ int main() { pthread_create(&facilities.infrastructure_service, NULL, infrastructure_service, NULL); // CP - if (facilities.dissemination.active) - pthread_create(&facilities.cp_service, NULL, cp_service, NULL); + pthread_create(&facilities.cp_service, NULL, cp_service, NULL); // SA pthread_create(&facilities.sa_service, NULL, sa_service, NULL); @@ -514,8 +513,7 @@ cleanup: pthread_join(facilities.ca_service, NULL); pthread_join(facilities.den_service, NULL); pthread_join(facilities.infrastructure_service, NULL); - if (facilities.dissemination.active) - pthread_join(facilities.cp_service, NULL); + pthread_join(facilities.cp_service, NULL); pthread_join(facilities.sa_service, NULL); itss_queue_trigger(facilities.tx_queue); pthread_join(facilities.transmitting, NULL); diff --git a/src/facilities.h b/src/facilities.h index b31e205..017b370 100644 --- a/src/facilities.h +++ b/src/facilities.h @@ -76,7 +76,7 @@ typedef struct facilities { infrastructure_t infrastructure; // CPM - dissemination_t dissemination; + cp_t cp; // SA bulletin_t bulletin; diff --git a/src/mcm.c b/src/mcm.c index d478dac..3ea61fd 100644 --- a/src/mcm.c +++ b/src/mcm.c @@ -175,7 +175,7 @@ void *mc_service(void *arg){ mcm_coord_t *mcm_coord = (mcm_coord_t *)&facilities.mcm_coord; while (!facilities.exit){ - sleep(1); + itss_sleep(1); pthread_mutex_lock(&mcm_coord->lock); rv = mk_mcm(); diff --git a/src/requests.c b/src/requests.c index fb899a0..335e09e 100644 --- a/src/requests.c +++ b/src/requests.c @@ -1,6 +1,6 @@ #include "requests.h" -#include +#include #include #include #include @@ -125,8 +125,8 @@ int facilities_request_single_message(void *responder, EIS_FacilitiesMessageRequ break; case EIS_ItsMessageType_cpm: - its_msg_def = &asn_DEF_EI1_CPM; - its_msg = calloc(1, sizeof(EI1_CPM_t)); + its_msg_def = &asn_DEF_EI2_CollectivePerceptionMessage; + its_msg = calloc(1, sizeof(EI2_CollectivePerceptionMessage_t)); npr->transport.choice.btp.destinationPort = EIS_Port_cpm; npr->network.choice.gn.packetTransportType = EIS_PacketTransportType_shb; npr->network.choice.gn.trafficClass = 2; @@ -710,8 +710,8 @@ static int networking_packet_indication_btp(EIS_NetworkingPacketIndication_t* np break; case EIS_Port_cpm: - its_msg_descriptor = &asn_DEF_EI1_CPM; - its_msg = calloc(1, sizeof(EI1_CPM_t)); + its_msg_descriptor = &asn_DEF_EI2_CollectivePerceptionMessage; + its_msg = calloc(1, sizeof(EI2_CollectivePerceptionMessage_t)); its_msg_type = 14; break;