TiagoRG
/
it2s-itss-facilities
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Compare commits

merge into: TiagoRG:master
Branches Tags
TiagoRG:master
TiagoRG:cpmv2
...
pull from: TiagoRG:cpmv2
Branches Tags
TiagoRG:cpmv2
TiagoRG:master

1 Commits
master ... cpmv2

Author SHA1 Message Date
Diogo 71a1f10fe2 cpmv2 2025-06-26 17:24:15 +01:00
14 changed files with 3985 additions and 881 deletions
Show Stats Expand all files Collapse all files

51
backup/src/CMakeLists.txt Normal file
View File

@ -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
)

590
backup/src/config.c Normal file
View File

@ -0,0 +1,590 @@
#include "config.h"
#include "facilities.h"
#include "saem.h"
#include "tpm.h"
#include "vcm.h"
#include "mcm.h"
#include <string.h>
#include <stdlib.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <it2s-config/t2c-itss.h>
#include <it2s-config/t2c-etsi-its.h>
#include <dirent.h>
#include <sys/time.h>
#include <it2s-asn/etsi-its-sdu/cdd-1.3.1/EIS_ProtectedCommunicationZone.h>
#include <it2s-asn/etsi-its-v1/tpm/EI1_TollingPaymentInfo.h>
#include <zmq.h>
#include <it2s-tender/packet.h>
#include <it2s-asn/etsi-its-sdu/itss-management/EIS_ManagementRequest.h>
#include <it2s-asn/etsi-its-sdu/itss-management/EIS_ManagementReply.h>
#include <it2s-asn/etsi-its-sdu/itss-security/EIS_SecurityRequest.h>
#include <it2s-asn/etsi-its-sdu/itss-security/EIS_SecurityReply.h>
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;
}

832
backup/src/cpm.c Normal file
View File

@ -0,0 +1,832 @@
#include "cpm.h"
#include "facilities.h"
#include <it2s-asn/etsi-its-v1/cpm/EI1_CPM.h>
#include <it2s-asn/etsi-its-sdu/itss-networking/EIS_NetworkingRequest.h>
#include <math.h>
#include <signal.h>
#include <unistd.h>
#include <it2s-asn/etsi-its-sdu/itss-facilities/EIS_FacilitiesIndication.h>
#include <arpa/inet.h>
#include <stdbool.h>
#include <stdio.h>
#include <signal.h>
#include <sys/ioctl.h>
#include <net/if.h>
#include <it2s-tender/time.h>
#include <it2s-tender/space.h>
#include <it2s-tender/recorder.h>
#include <it2s-tender/packet.h>
#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;
}

187
backup/src/cpm.h Normal file
View File

@ -0,0 +1,187 @@
#ifndef FACILITIES_CPM_H
#define FACILITIES_CPM_H
#include <arpa/inet.h>
#include <stdbool.h>
#include <it2s-asn/etsi-its-v1/cpm/EI1_CPM.h>
#include <termios.h>
#include <unistd.h>
#include <time.h>
#include <net/if.h>
#include <linux/if_packet.h>
#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

542
backup/src/facilities.c Normal file
View File

@ -0,0 +1,542 @@
#include "facilities.h"
#include <it2s-asn/etsi-its-v1/cam/EI1_CAM.h>
#include <it2s-asn/etsi-its-v1/cpm/EI1_CPM.h>
#include <it2s-asn/etsi-its-v1/denm/EI1_DENM.h>
#include <it2s-asn/etsi-its-sdu/itss-facilities/EIS_FacilitiesIndication.h>
#include <it2s-asn/etsi-its-sdu/itss-facilities/EIS_FacilitiesReply.h>
#include <it2s-asn/etsi-its-sdu/itss-facilities/EIS_FacilitiesRequest.h>
#include <it2s-asn/etsi-its-sdu/itss-management/EIS_ManagementIndication.h>
#include <it2s-asn/etsi-its-sdu/itss-management/EIS_ManagementRequest.h>
#include <it2s-asn/etsi-its-sdu/itss-networking/EIS_NetworkingIndication.h>
#include <it2s-asn/etsi-its-sdu/itss-security/EIS_SecurityIndication.h>
#include <it2s-asn/etsi-its-sdu/itss-security/EIS_SecurityReply.h>
#include <it2s-asn/etsi-its-sdu/itss-security/EIS_SecurityRequest.h>
#include <it2s-asn/etsi-its-sdu/itss-security/EIS_SecurityResponse.h>
#include <it2s-asn/etsi-its-sdu/itss-networking/EIS_NetworkingIndication.h>
#include <it2s-asn/etsi-its-sdu/itss-networking/EIS_NetworkingRequest.h>
#include <it2s-asn/etsi-its-v1/ivim/EI1_IVIM.h>
#include <it2s-asn/etsi-its-v1/saem/EI1_SAEM.h>
#include <it2s-tender/packet.h>
#include <it2s-tender/recorder.h>
#include <it2s-tender/space.h>
#include <it2s-tender/time.h>
#include <signal.h>
#include <stdbool.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <unistd.h>
#include <zmq.h>
#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 <TIMEOUT>",
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 <TIMEOUT>",
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 <TIMEOUT>",
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;
}

132
backup/src/facilities.h Normal file
View File

@ -0,0 +1,132 @@
#ifndef FACILITIES_H
#define FACILITIES_H
#include <pthread.h>
#include <stdbool.h>
#include <zmq.h>
#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 <it2s-tender/epv.h>
#include <it2s-tender/database.h>
#include <it2s-tender/log.h>
#include <it2s-tender/constants.h>
#include <it2s-tender/queue.h>
/*
* 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

4
src/CMakeLists.txt
View File

@ -23,13 +23,13 @@ TARGET_LINK_LIBRARIES(it2s-itss-facilities
-lit2s-asn-etsi-its-sdu-cdd-1.3.1 -lit2s-asn-etsi-its-sdu-cdd-1.3.1
-lzmq -lzmq
-lit2s_gnss -lit2s_gnss
-lit2s-radar
-lpthread -lpthread
-lit2s-config-etsi-its -lit2s-config-etsi-its
-lit2s-asn-etsi-its-v1-cdd-1.3.1 -lit2s-asn-etsi-its-v1-cdd-1.3.1
-lit2s-asn-etsi-its-v1-cam -lit2s-asn-etsi-its-v1-cam
-lit2s-asn-etsi-its-v1-ivim -lit2s-asn-etsi-its-v1-ivim
-lit2s-asn-etsi-its-v1-denm -lit2s-asn-etsi-its-v1-denm
-lit2s-asn-etsi-its-v1-cpm
-lit2s-asn-etsi-its-v1-saem -lit2s-asn-etsi-its-v1-saem
-lit2s-asn-etsi-its-v1-tpm -lit2s-asn-etsi-its-v1-tpm
-lit2s-asn-etsi-its-v1-vcm -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-cam
-lit2s-asn-etsi-its-v2-denm -lit2s-asn-etsi-its-v2-denm
-lit2s-asn-etsi-its-v2-mcm -lit2s-asn-etsi-its-v2-mcm
-lit2s-asn-etsi-its-v2-cpm
-lit2s-asn-etsi-its-v2-ldm
-lit2s-tender -lit2s-tender
-lm -lm
-lrt -lrt

72
src/config.c
View File

@ -19,6 +19,10 @@
#include <it2s-tender/packet.h> #include <it2s-tender/packet.h>
#include <it2s-radar/constants.h>
#include <it2s-radar/t2c-radar.h>
#include <it2s-radar/umrr0c.h>
#include <it2s-asn/etsi-its-sdu/itss-management/EIS_ManagementRequest.h> #include <it2s-asn/etsi-its-sdu/itss-management/EIS_ManagementRequest.h>
#include <it2s-asn/etsi-its-sdu/itss-management/EIS_ManagementReply.h> #include <it2s-asn/etsi-its-sdu/itss-management/EIS_ManagementReply.h>
#include <it2s-asn/etsi-its-sdu/itss-security/EIS_SecurityRequest.h> #include <it2s-asn/etsi-its-sdu/itss-security/EIS_SecurityRequest.h>
@ -55,6 +59,7 @@ int facilities_config() {
t2c_itss_t* itss_cfg = NULL; t2c_itss_t* itss_cfg = NULL;
t2c_etsi_its_t* etsi_its_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); rv = t2c_itss_read("/etc/it2s/itss.toml", &itss_cfg);
if (rv) { if (rv) {
@ -66,6 +71,11 @@ int facilities_config() {
syslog_error("[config] read ETSI ITS config failed"); syslog_error("[config] read ETSI ITS config failed");
goto cleanup; 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 // Logging - status messages
bool use_syslog = false, use_std = false, use_file = false; 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; facilities.infrastructure.default_service_duration = etsi_its_cfg->facilities.ivim.default_service_duration * 60000;
// CPM // CPM
facilities.dissemination.active = etsi_its_cfg->facilities.cpm.activate; facilities.cp.active = etsi_its_cfg->facilities.cpm.activate;
facilities.dissemination.T_GenCpmMin = etsi_its_cfg->facilities.cpm.rsu_obu_period_min; facilities.cp.T_GenCpm = etsi_its_cfg->facilities.cpm.t_gen_cpm;
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); // CP - LDM connection
strcpy(facilities.dissemination.int_radar,etsi_its_cfg->facilities.cpm.radar_interface); facilities.cp.ldm.active = etsi_its_cfg->facilities.ldm.activate;
facilities.dissemination.ip_radar = malloc(strlen(etsi_its_cfg->facilities.cpm.radar_ip)+1); facilities.cp.ldm.registered = false;
strcpy(facilities.dissemination.ip_radar,etsi_its_cfg->facilities.cpm.radar_ip); 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 // TPM
facilities.tolling.enabled = etsi_its_cfg->facilities.tpm.activate; facilities.tolling.enabled = etsi_its_cfg->facilities.tpm.activate;
@ -584,6 +625,7 @@ int facilities_config() {
cleanup: cleanup:
t2c_itss_free(itss_cfg); t2c_itss_free(itss_cfg);
t2c_etsi_its_free(etsi_its_cfg); t2c_etsi_its_free(etsi_its_cfg);
t2c_radar_free(radar_cfg);
return rv; return rv;
} }

2099
src/cpm.c
View File

File diff suppressed because it is too large Load Diff

333
src/cpm.h
View File

@ -1,187 +1,210 @@
/**
* @file cpmv2.h
* @author Diogo Jesus (diogopjesus@ua.pt)
* @brief Collective Perception Service implementation.
*/
#ifndef FACILITIES_CPM_H #ifndef FACILITIES_CPM_H
#define FACILITIES_CPM_H #define FACILITIES_CPM_H
#include <it2s-asn/etsi-its-v2/cpm/EI2_CollectivePerceptionMessage.h>
#include <arpa/inet.h> #include <it2s-radar/it2s-radar.h>
#include <pthread.h>
#include <stdbool.h> #include <stdbool.h>
#include <stdint.h>
#include <it2s-asn/etsi-its-v1/cpm/EI1_CPM.h> #define MAX_SENSORS \
#include <termios.h> 128 ///< Maximum number of sensors allowed to exist inside the sensing system. (128 is the standard defined value for the maximum
#include <unistd.h> ///< number of sensors in a CPM)
#include <time.h> #define MAX_PERCEPTION_REGIONS \
#include <net/if.h> 256 ///< Maximum number of perception regions allowed to be retrived from the LDM. (8 is standard defined value for the maximum number
#include <linux/if_packet.h> ///< 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 TOTAL_PO_IDS (1 << 16) ///< Total number of possible object identifiers.
#define NOF_OBJECTS 127 // Number of Object UMRR-0C #define UNUSED_PO_ID (TOTAL_PO_IDS - 1) ///< Value used to indicate an unused object identifier (last identifier).
#define MESSAGE_ID 14 #define TOTAL_SI_IDS (1 << 8) ///< Total number of possible sensor identifiers.
#define PROTOCOL_VERSION 1 #define UNUSED_SI_ID (TOTAL_SI_IDS - 1) ///< Value used to indicate an unused sensor identifier (last identifier).
#define RADAR_PORT "55555" // Destination port from the radar
/**
* @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 double ObjectPerceptionQualityThreshold; ///< Threshold of the objectPerceptionQuality defining the lower limit for objects
{ ///< to be included into a CPM.
int i32_socket; // Socket descriptor server double minPositionChangeThreshold; ///< Minimum Euclidean distance between the current estimated position of the reference
int i32_client; // Socket descriptor UMRR-0C ///< point of a perceived object and the estimated position of the reference point lastly
int i32_port; // Listen on Port ///< included in a CPM
bool b_moxa; double minGroundSpeedChangeThreshold; ///< Minimum difference between the current estimated ground speed of the reference
struct sockaddr_in s_server; ///< point of a perceived object and the estimated ground speed of the reference point
struct sockaddr_in s_client; ///< lastly included in a CPM
} S_ETHERNET_CONNECTION_T; 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 uint8_t MessageAssemblyConfig; ///< CPS configuration parameter that indicates the CPM assembly mechanism to be employed by
{ ///< the disemminating ITS-S
int raw_fd;
struct sockaddr_ll sll;
struct ifreq ifr;
} S_INTERFACE_CONNECTION_T;
typedef struct 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
u_int8_t u8_numberOfCountedObjects; ///< the reference point of this object lastly included in a CPM starts to
u_int32_t u32_UnixTime; ///< contribute to the object utility function
u_int16_t u16_Milliseconds; double maxPositionChangePriorityThreshold; ///< Upper limit for which the Euclidean distance between the current estimated
u_int8_t u8_SensorNetworkID; ///< position of the reference point of the object and the estimated position of
u_int8_t u8_ObjectNumber; ///< the reference point of this object lastly included in a CPM starts to
u_int8_t u8_ObjectID; ///< contribute to the object utility function
int16_t i16_speed; double minGroundSpeedChangePriorityThreshold; ///< Lower limit for which the difference between the current estimated ground
u_int8_t u8_class; ///< speed of the reference point of the object and the estimated ground speed
u_int8_t u8_mLineNumber; ///< of the reference point of this object lastly included in a CPM starts to
u_int16_t u8_laneNumber; ///< contribute to the object utility function
} S_PVR_T; 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 uint64_t UnusedSensorIdRetentionPeriod; ///< Minimum time to ellapse before an ID used to identify a sensor can be reused
{ ///< after it has last been used
u_int8_t u8_sensorStatus; uint64_t UnusedObjectIdRetentionPeriod; ///< Minimum time to ellapse before an ID used to identify an object can be reused
u_int8_t u8_InterfaceMode; ///< after it has last been used
u_int8_t u8_networkID; } cp_config_t;
u_int8_t u8_diagnose;
u_int32_t u32_time;
} S_SENSOR_CONTROL_T;
typedef struct /**
{ * @brief CP Perceived Object History struct.
u_int8_t u8_numberOfObjects; *
u_int8_t u8_numberOfMessages; * Contains data related to the inclusion management of objects into a CPM.
u_int8_t u8_cycleDuration; */
u_int8_t u8_objectData0Format; typedef struct cp_history {
u_int8_t u8_objectData1Format; uint64_t* inclusion; ///< Timestamp of the last inclusion of an object into a CPM. (milliseconds)
u_int32_t u32_cycleCount; struct {
} S_OBJECT_CONTROL_T; 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 /**
{ * @brief Bi-directional mapping of LDM object IDs to CPS object IDs.
u_int8_t u8_modeSignal; * @note There is the assumption that the sensor identifier (outside the CPS) does not change.
float f_xPoint; */
float f_yPoint; typedef struct cp_obj_id {
float f_xSpeed; uint16_t* ldm2cps; ///< Maps LDM object IDs to CPS object IDs.
float f_ySpeed; uint16_t* cps2ldm; ///< Maps CPS object IDs to LDM object IDs.
float f_objectLength;
u_int8_t u8_objectID;
u_int8_t u8_updateFlag;
} S_OBJECTS_T;
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, * @brief Bi-directional mapping of sensor IDs (ouside the CPS) to CPS sensor IDs.
START_1, */
START_2, typedef struct cp_sensor_id {
START_3, uint8_t* sen2cps; ///< Maps sensor IDs (outside CPS) to CPS sensor IDs.
START_4, uint8_t* cps2sen; ///< Maps CPS sensor IDs to sensor IDs (outside CPS).
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 uint64_t* timestamp; ///< Timestamp associated to a sensor with a CPS sensor ID.
{ } cp_sensor_id_t;
bool active; /**
bool tmc_connect; * @brief Main CPS struct.
char* int_radar; */
char* ip_radar; 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 lastCpmTimestamp; ///< Timestamp of the last CPM sent.
uint64_t next_cpm_min; uint64_t lastSicTimestamp; ///< Timestamp of the last SIC sent.
uint64_t T_GenCpmMin; cp_config_t config; ///< CPS configuration.
uint64_t T_GenCpmMax; cp_history_t history; ///< CPS history.
uint64_t T_AddSensorInformation; EI2_SensorInformation_t** si_arr; ///< Array of sensor information.
uint64_t next_AddSensorInformation; 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) */ EI2_PerceivedObject_t** po_arr; ///< Array of perceived objects.
size_t po_count; ///< Number of perceived objects in the perceived object array.
int64_t radar_rotation;
int16_t opening_angle_start;
int16_t opening_angle_end;
} dissemination_t;
/* Prototype Functions */ size_t po_total; ///< Total number of perceived objects in the LDM (as per standard)
/* cp_sensor_id_t sen_id; ///< Sensor ID mapping.
Summary : Waits for client to connect (Radar) 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 * @brief Main CP service.
param [in] char* radar_ip: Radar IP *
param [in] char* radar_port: Radar PORT * @return NULL
*/ */
void* cp_service();
bool initEthernetConnection(const char* radar_ip,const char* radar_port); #endif // FACILITIES_CPM_H
/*
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

10
src/facilities.c
View File

@ -1,7 +1,6 @@
#include "facilities.h" #include "facilities.h"
#include <it2s-asn/etsi-its-v1/cam/EI1_CAM.h> #include <it2s-asn/etsi-its-v1/cam/EI1_CAM.h>
#include <it2s-asn/etsi-its-v1/cpm/EI1_CPM.h>
#include <it2s-asn/etsi-its-v1/denm/EI1_DENM.h> #include <it2s-asn/etsi-its-v1/denm/EI1_DENM.h>
#include <it2s-asn/etsi-its-sdu/itss-facilities/EIS_FacilitiesIndication.h> #include <it2s-asn/etsi-its-sdu/itss-facilities/EIS_FacilitiesIndication.h>
#include <it2s-asn/etsi-its-sdu/itss-facilities/EIS_FacilitiesReply.h> #include <it2s-asn/etsi-its-sdu/itss-facilities/EIS_FacilitiesReply.h>
@ -17,6 +16,7 @@
#include <it2s-asn/etsi-its-sdu/itss-networking/EIS_NetworkingRequest.h> #include <it2s-asn/etsi-its-sdu/itss-networking/EIS_NetworkingRequest.h>
#include <it2s-asn/etsi-its-v1/ivim/EI1_IVIM.h> #include <it2s-asn/etsi-its-v1/ivim/EI1_IVIM.h>
#include <it2s-asn/etsi-its-v1/saem/EI1_SAEM.h> #include <it2s-asn/etsi-its-v1/saem/EI1_SAEM.h>
#include <it2s-asn/etsi-its-v2/cpm/EI2_CollectivePerceptionMessage.h>
#include <it2s-tender/packet.h> #include <it2s-tender/packet.h>
#include <it2s-tender/recorder.h> #include <it2s-tender/recorder.h>
#include <it2s-tender/space.h> #include <it2s-tender/space.h>
@ -367,7 +367,7 @@ int main() {
lightship_init(); lightship_init();
den_init(); den_init();
infrastructure_init(); infrastructure_init();
dissemination_init(); cp_init();
bulletin_init(); bulletin_init();
void *security_socket = NULL; void *security_socket = NULL;
@ -390,8 +390,7 @@ int main() {
pthread_create(&facilities.infrastructure_service, NULL, infrastructure_service, NULL); pthread_create(&facilities.infrastructure_service, NULL, infrastructure_service, NULL);
// CP // CP
if (facilities.dissemination.active) pthread_create(&facilities.cp_service, NULL, cp_service, NULL);
pthread_create(&facilities.cp_service, NULL, cp_service, NULL);
// SA // SA
pthread_create(&facilities.sa_service, NULL, sa_service, NULL); pthread_create(&facilities.sa_service, NULL, sa_service, NULL);
@ -514,8 +513,7 @@ cleanup:
pthread_join(facilities.ca_service, NULL); pthread_join(facilities.ca_service, NULL);
pthread_join(facilities.den_service, NULL); pthread_join(facilities.den_service, NULL);
pthread_join(facilities.infrastructure_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); pthread_join(facilities.sa_service, NULL);
itss_queue_trigger(facilities.tx_queue); itss_queue_trigger(facilities.tx_queue);
pthread_join(facilities.transmitting, NULL); pthread_join(facilities.transmitting, NULL);

2
src/facilities.h
View File

@ -76,7 +76,7 @@ typedef struct facilities {
infrastructure_t infrastructure; infrastructure_t infrastructure;
// CPM // CPM
dissemination_t dissemination; cp_t cp;
// SA // SA
bulletin_t bulletin; bulletin_t bulletin;

2
src/mcm.c
View File

@ -175,7 +175,7 @@ void *mc_service(void *arg){
mcm_coord_t *mcm_coord = (mcm_coord_t *)&facilities.mcm_coord; mcm_coord_t *mcm_coord = (mcm_coord_t *)&facilities.mcm_coord;
while (!facilities.exit){ while (!facilities.exit){
sleep(1); itss_sleep(1);
pthread_mutex_lock(&mcm_coord->lock); pthread_mutex_lock(&mcm_coord->lock);
rv = mk_mcm(); rv = mk_mcm();

10
src/requests.c
View File

@ -1,6 +1,6 @@
#include "requests.h" #include "requests.h"
#include <it2s-asn/etsi-its-v1/cpm/EI1_CPM.h> #include <it2s-asn/etsi-its-v2/cpm/EI2_CollectivePerceptionMessage.h>
#include <it2s-asn/etsi-its-sdu/itss-facilities/EIS_FacilitiesReply.h> #include <it2s-asn/etsi-its-sdu/itss-facilities/EIS_FacilitiesReply.h>
#include <it2s-asn/etsi-its-sdu/itss-networking/EIS_NetworkingRequest.h> #include <it2s-asn/etsi-its-sdu/itss-networking/EIS_NetworkingRequest.h>
#include <it2s-asn/etsi-its-sdu/itss-facilities/EIS_FacilitiesIndication.h> #include <it2s-asn/etsi-its-sdu/itss-facilities/EIS_FacilitiesIndication.h>
@ -125,8 +125,8 @@ int facilities_request_single_message(void *responder, EIS_FacilitiesMessageRequ
break; break;
case EIS_ItsMessageType_cpm: case EIS_ItsMessageType_cpm:
its_msg_def = &asn_DEF_EI1_CPM; its_msg_def = &asn_DEF_EI2_CollectivePerceptionMessage;
its_msg = calloc(1, sizeof(EI1_CPM_t)); its_msg = calloc(1, sizeof(EI2_CollectivePerceptionMessage_t));
npr->transport.choice.btp.destinationPort = EIS_Port_cpm; npr->transport.choice.btp.destinationPort = EIS_Port_cpm;
npr->network.choice.gn.packetTransportType = EIS_PacketTransportType_shb; npr->network.choice.gn.packetTransportType = EIS_PacketTransportType_shb;
npr->network.choice.gn.trafficClass = 2; npr->network.choice.gn.trafficClass = 2;
@ -710,8 +710,8 @@ static int networking_packet_indication_btp(EIS_NetworkingPacketIndication_t* np
break; break;
case EIS_Port_cpm: case EIS_Port_cpm:
its_msg_descriptor = &asn_DEF_EI1_CPM; its_msg_descriptor = &asn_DEF_EI2_CollectivePerceptionMessage;
its_msg = calloc(1, sizeof(EI1_CPM_t)); its_msg = calloc(1, sizeof(EI2_CollectivePerceptionMessage_t));
its_msg_type = 14; its_msg_type = 14;
break; break;