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

CPM generation can work without TMC software

This commit is contained in:
Marco 2021-05-26 10:59:42 +01:00
parent 7d16f0525e
commit 11f519fe67
3 changed files with 95 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/config.c
View File

@ -249,6 +249,11 @@ int facilities_config(void* facilities_s) {
facilities->dissemination->T_GenCpmMin = config->facilities.cpm.rsu_obu_period_min;
facilities->dissemination->T_GenCpmMax = config->facilities.cpm.rsu_obu_period_max;
facilities->dissemination->radar_rotation = config->applications.its_center.rotation;
facilities->dissemination->tmc_connect = config->facilities.cpm.tmc_connected;
facilities->dissemination->latitude = config->management.gps.latitude;
facilities->dissemination->longitude = config->management.gps.longitude;
// Replay
facilities->replay = config->networking.replay.activate;

93
src/cpm.c
View File

@ -17,6 +17,8 @@
#include <stdbool.h>
#include <stdio.h>
#include <signal.h>
#include <sys/ioctl.h>
#include <net/if.h>
#include <it2s-tender/time.h>
@ -38,6 +40,7 @@
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];
@ -63,29 +66,64 @@ bool waitingIncomingConnection(void){
bool interface_connection(char* radar_ip,char* radar_port){
bool interface_connection(char* radar_ip,char* radar_port, facilities_t* facilities){
// 1 - Create socket
s_socket.i32_socket = socket(AF_INET, SOCK_STREAM, 0);
if(facilities->dissemination->tmc_connect == false){
if(s_socket.i32_socket < 0){
syslog_err("Initializing socket failed ...");
// 1 - Create socket
s_socket.i32_socket = socket(AF_INET, SOCK_STREAM, 0);
if(s_socket.i32_socket < 0){
syslog_err("Initializing socket failed ...");
return false;
}
// 2 - Bind it to server address and port
bzero(&s_socket.s_server, sizeof(s_socket.s_server));
s_socket.s_server.sin_family = AF_INET;
s_socket.s_server.sin_addr.s_addr = inet_addr(radar_ip);
s_socket.s_server.sin_port = htons(atoi(radar_port));
if(bind(s_socket.i32_socket, (struct sockaddr*)&s_socket.s_server,sizeof(s_socket.s_server)) < 0){
syslog_err("Binding socket to address error ...");
return false;
}
if(!waitingIncomingConnection())
return false;
}
// Create RAW socket
raw_socket.raw_fd = socket(AF_PACKET, SOCK_RAW, htons(0x0800));
if(raw_socket.raw_fd < 0){
syslog_err("Failed to initializing RAW socket ...");
return false;
}
// 2 - Bind it to server address and port
bzero(&s_socket.s_server, sizeof(s_socket.s_server));
s_socket.s_server.sin_family = AF_INET;
s_socket.s_server.sin_addr.s_addr = inet_addr(radar_ip);
s_socket.s_server.sin_port = htons(atoi(radar_port));
// Get interface index
if(bind(s_socket.i32_socket, (struct sockaddr*)&s_socket.s_server,sizeof(s_socket.s_server)) < 0){
syslog_err("Binding socket to address error ...");
return false;
bzero(&raw_socket.sll, sizeof(raw_socket.sll));
bzero(&raw_socket.ifr, sizeof(raw_socket.ifr));
strncpy((char *)raw_socket.ifr.ifr_name, INTERFACE_RADAR, IFNAMSIZ);
if((ioctl(raw_socket.raw_fd, SIOCGIFINDEX, &raw_socket.ifr)) == -1){
syslog_err("Error getting interface index");
return false;
}
if(!waitingIncomingConnection())
raw_socket.sll.sll_family = AF_PACKET;
raw_socket.sll.sll_ifindex = raw_socket.ifr.ifr_ifindex;
raw_socket.sll.sll_protocol = htons(0x0800);
// Bind it to the interface
if(bind(raw_socket.raw_fd, (struct sockaddr *)&raw_socket.sll, sizeof(raw_socket.sll))<0){
syslog_err("Error binding RAW socket ...");
return false;
}
return true;
@ -377,8 +415,8 @@ void mk_cpm(facilities_t* facilities,CPM_t* cpm_tx, struct timespec* systemtime)
cpm_tx->cpm.generationDeltaTime = generationDeltaTime;
cpm_tx->cpm.cpmParameters.managementContainer.stationType = StationType_roadSideUnit;
cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.latitude = 420309860; //A3 // 412400078; (A24)
cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.longitude = -86515778; //A3 // -86950224; (A24)
cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.latitude = facilities->dissemination->latitude;
cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.longitude = facilities->dissemination->longitude;
cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.positionConfidenceEllipse.semiMajorConfidence = 100;
cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.positionConfidenceEllipse.semiMinorConfidence = 100;
cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.positionConfidenceEllipse.semiMajorOrientation = HeadingValue_wgs84North;
@ -504,6 +542,10 @@ void *cp_service(void *fc){
int i32_recv_bytes;
u_int8_t au8_readBuffer[READ_BUFFER_SIZE];
bool is_radar_connected;
u_int8_t au8_readTcp[READ_BUFFER_SIZE];
facilities_t *facilities = (facilities_t *) fc;
uint8_t bdr_oer[2048];
uint8_t fdi_oer[2048];
@ -511,7 +553,6 @@ void *cp_service(void *fc){
fdi_oer[0] = 4;
facilities_t *facilities = (facilities_t *) fc;
BTPDataRequest_t *bdr = calloc(1, sizeof(BTPDataRequest_t));
FacilitiesDataIndication_t *fdi = calloc(1, sizeof(FacilitiesDataIndication_t));
@ -540,7 +581,7 @@ void *cp_service(void *fc){
*bdr->gnSecurityProfile = 1;
}
is_radar_connected = interface_connection(RADAR_IP,RADAR_PORT); // Create Radar listening socket
is_radar_connected = interface_connection(RADAR_IP,RADAR_PORT,facilities); // Create Radar listening socket
/*--- Fill mandatory Facilities Data Indication parameters ---*/
@ -560,9 +601,15 @@ void *cp_service(void *fc){
while(!facilities->exit){
usleep(1000*50);
/* Receive Data from radar interface */
CPM_t* cpm_tx = calloc(1, sizeof(CPM_t));
i32_recv_bytes = recv(s_socket.i32_client, &au8_readBuffer, READ_BUFFER_SIZE, 0); //recv(socket,buffer,size,flags)
/* If the Radar is not connected to TMC, a TCP socket is needed to fool the Radar */
if(facilities->dissemination->tmc_connect == false)
i32_recv_bytes = recv(s_socket.i32_client, &au8_readTcp, READ_BUFFER_SIZE, 0);
/* Reads from the radar */
CPM_t* cpm_tx = calloc(1, sizeof(CPM_t));
i32_recv_bytes = recv(raw_socket.raw_fd, &au8_readBuffer, READ_BUFFER_SIZE, 0); //recv(socket,buffer,size,flags)
if (dissemination_check(facilities->dissemination, &facilities->epv) && facilities->dissemination->active){
if(is_radar_connected){
@ -573,12 +620,14 @@ void *cp_service(void *fc){
continue;
}
/* Information parsing */
parse_input(au8_readBuffer,i32_recv_bytes);
/* CPM build and encoding to bdr and fdi (Could be merged if needed) */
mk_cpm(facilities,cpm_tx,systemtime);
if(encode_cpm(cpm_tx, bdr->data.buf, (uint32_t *) &bdr->data.size) == 1){
ASN_STRUCT_FREE(asn_DEF_CPM, cpm_tx);
continue;
@ -621,7 +670,7 @@ void *cp_service(void *fc){
} else { /* Waiting for Radar to reconnect */
is_radar_connected = waitingIncomingConnection();
is_radar_connected = waitingIncomingConnection();
}
}
}

14
src/cpm.h
View File

@ -17,8 +17,9 @@
#define ROAD_ANGLE 120
#define MESSAGE_ID 14
#define PROTOCOL_VERSION 1
#define INTERFACE_RADAR "enp3s0"
#define RADAR_IP "11.11.11.13" // ITS-S interface connected to the radar
#define RADAR_IP "192.168.11.1" // ITS-S interface connected to the radar
#define RADAR_PORT "55555" // Destination port from the radar
@ -118,10 +119,11 @@ typedef struct
{
bool active;
bool tmc_connect;
pthread_mutex_t lock;
uint8_t type;
uint32_t type;
uint64_t next_cpm_max;
uint64_t next_cpm_min;
@ -130,9 +132,15 @@ typedef struct
uint64_t T_GenCpmMin;
uint64_t T_GenCpmMax;
/* Position of the radar (Value from toml) */
int8_t radar_rotation;
int64_t radar_rotation;
/* Fixed GPS coordenates */
double latitude;
double longitude;
} dissemination_t;