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

680 lines
25 KiB
C
Raw Blame History

#include "cpm.h"
#include "facilities.h"
#include <cpm/CPM.h>
#include <cpm/INTEGER.h>
#include <cpm/asn_application.h>
#include <itss-transport/BTPDataRequest.h>
#include <itss-transport/BTPDataIndication.h>
#include <it2s-config.h>
#include <math.h>
#include <signal.h>
#include <unistd.h>
#include <itss-facilities/FacilitiesDataIndication.h>
#include <syslog.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>
#define syslog_info(msg, ...) syslog(LOG_INFO, msg, ##__VA_ARGS__)
#define syslog_emerg(msg, ...) syslog(LOG_EMERG, "%s:%d [" msg "]", __func__, __LINE__, ##__VA_ARGS__)
#define syslog_err(msg, ...) syslog(LOG_ERR, "%s:%d [" msg "]", __func__, __LINE__, ##__VA_ARGS__)
#ifndef NDEBUG
#define syslog_debug(msg, ...) syslog(LOG_DEBUG, "%s:%d " msg "", __func__, __LINE__, ##__VA_ARGS__)
#else
#define syslog_debug(msg, ...)
#endif
#define PI 3.141592654
/* Variables */
float roadRotationSin;
float roadRotationCos;
S_ETHERNET_CONNECTION_T s_socket;
S_INTERFACE_CONNECTION_T raw_socket;
S_OBJECT_CONTROL_T s_objectControl;
S_OBJECTS_T as_objects[NOF_OBJECTS];
bool waitingIncomingConnection(void){
// Listening to the socket (Waiting for incoming connection)
unsigned int len = sizeof(s_socket.s_client);
if(listen(s_socket.i32_socket,1)<0){
syslog_err("Waiting for incoming requests failed...");
return false;
}
if((s_socket.i32_client = accept(s_socket.i32_socket, (struct sockaddr*)&s_socket.s_server, &len)) < 0){
syslog_err("Client disconnected...");
return false;
}
syslog_debug("Radar connected");
return true;
}
bool interface_connection(char* radar_port, facilities_t* facilities){
if(facilities->dissemination->tmc_connect == false){
// 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;
}
// Bind it to server address and port
bzero(&s_socket.s_server, sizeof(s_socket.s_server));
s_socket.s_server.sin_family = AF_INET;
s_socket.s_server.sin_addr.s_addr = inet_addr(facilities->dissemination->ip_radar);
s_socket.s_server.sin_port = htons(atoi(radar_port));
if(bind(s_socket.i32_socket, (struct sockaddr*)&s_socket.s_server,sizeof(s_socket.s_server)) < 0){
syslog_err("Binding socket to address error ...");
return false;
}
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;
}
// Get interface index
bzero(&raw_socket.sll, sizeof(raw_socket.sll));
bzero(&raw_socket.ifr, sizeof(raw_socket.ifr));
strncpy((char *)raw_socket.ifr.ifr_name, facilities->dissemination->int_radar, IFNAMSIZ);
if((ioctl(raw_socket.raw_fd, SIOCGIFINDEX, &raw_socket.ifr)) == -1){
syslog_err("Error getting interface index");
return false;
}
raw_socket.sll.sll_family = AF_PACKET;
raw_socket.sll.sll_ifindex = raw_socket.ifr.ifr_ifindex;
raw_socket.sll.sll_protocol = htons(0x0800);
// Bind it to the interface
if(bind(raw_socket.raw_fd, (struct sockaddr *)&raw_socket.sll, sizeof(raw_socket.sll))<0){
syslog_err("Error binding RAW socket ...");
return false;
}
return true;
}
void parse_can_data_tm(u_int32_t u32_can_id, int i32_can_len, u_int8_t* au8_can_data) {
u_int8_t u8_pvrMessagePart = 0;
u_int8_t tmp = 0;
//static long last = 0;
S_PVR_T s_pvr;
S_SENSOR_CONTROL_T s_sensorControl;
switch (u32_can_id) // Interpret the different types of CAN messages
{
case 0x785:
s_pvr.u8_numberOfCountedObjects = 0;
s_pvr.u32_UnixTime = 0;
s_pvr.u16_Milliseconds = 0;
s_pvr.u8_SensorNetworkID = 0;
u8_pvrMessagePart = (au8_can_data[0] & 0x1);
if (u8_pvrMessagePart == 0) {
s_pvr.u8_numberOfCountedObjects = (au8_can_data[0] & 0xFE) >> 1;
s_pvr.u32_UnixTime |= au8_can_data[4] << 24;
s_pvr.u32_UnixTime |= au8_can_data[3] << 16;
s_pvr.u32_UnixTime |= au8_can_data[2] << 8;
s_pvr.u32_UnixTime |= au8_can_data[1];
s_pvr.u16_Milliseconds |= (au8_can_data[6] & 0x3) << 8;
s_pvr.u16_Milliseconds |= au8_can_data[5];
s_pvr.u8_SensorNetworkID = (au8_can_data[6] & 0xC) >> 2;
// printf("Unix Time: %u, Ms: %u\n", s_pvr.u32_UnixTime, s_pvr.u16_Milliseconds);
long current = s_pvr.u32_UnixTime * 1000 + s_pvr.u16_Milliseconds;
//printf("Unix Time Epoch Fixed: %ld\n", (long)(current - 1072915200000));
//printf("Diff: %ld\n", current - last);
//last = current;
} else if (u8_pvrMessagePart == 1) {
s_pvr.u8_ObjectNumber = 0;
s_pvr.u8_ObjectID = 0;
s_pvr.i16_speed = 0;
s_pvr.u8_class = 0;
s_pvr.u8_mLineNumber = 0;
s_pvr.u8_laneNumber = 0;
s_pvr.u8_ObjectNumber = (au8_can_data[0] & 0xFE) >> 1;
s_pvr.u8_ObjectID = au8_can_data[1];
s_pvr.i16_speed |= (au8_can_data[3] & 0x7) << 8;
s_pvr.i16_speed |= au8_can_data[2];
s_pvr.i16_speed -= 1024; // Speed Offset
s_pvr.u8_class = (au8_can_data[3] & 0x38) >> 3;
s_pvr.u8_mLineNumber = (au8_can_data[3] & 0xC0) >> 6;
s_pvr.u8_laneNumber = (au8_can_data[4] & 0x7) >> 3;
}
break;
case 0x500:
memset(&s_sensorControl, 0, sizeof(s_sensorControl));
s_sensorControl.u8_sensorStatus = au8_can_data[0];
s_sensorControl.u8_InterfaceMode = (au8_can_data[1] & 0xF);
s_sensorControl.u8_networkID = (au8_can_data[1] & 0xF0) >> 4;
s_sensorControl.u8_diagnose = au8_can_data[2];
s_sensorControl.u32_time |= au8_can_data[7] << 24;
s_sensorControl.u32_time |= au8_can_data[6] << 16;
s_sensorControl.u32_time |= au8_can_data[5] << 8;
s_sensorControl.u32_time |= au8_can_data[4];
break;
case 0x501:
memset(&s_objectControl, 0, sizeof(s_objectControl));
s_objectControl.u8_numberOfObjects = au8_can_data[0];
s_objectControl.u8_numberOfMessages = au8_can_data[1];
s_objectControl.u8_cycleDuration = au8_can_data[2];
s_objectControl.u8_objectData0Format = au8_can_data[3] & 0xF;
s_objectControl.u8_objectData1Format = (au8_can_data[3] & 0xF0) >> 4;
s_objectControl.u32_cycleCount |= au8_can_data[7] << 24;
s_objectControl.u32_cycleCount |= au8_can_data[6] << 16;
s_objectControl.u32_cycleCount |= au8_can_data[5] << 8;
s_objectControl.u32_cycleCount |= au8_can_data[4];
break;
}
if ((u32_can_id >= 0x502) && (u32_can_id <= 0x57F)) {
u_int16_t u16_objectIndex = (u_int16_t)u32_can_id - 0x502;
as_objects[u16_objectIndex].u8_modeSignal = au8_can_data[0] & 0x1;
if (s_objectControl.u8_objectData0Format == 005) // without Update Flag
{
as_objects[u16_objectIndex].f_xPoint = (au8_can_data[1] & 0x3F) << 7;
as_objects[u16_objectIndex].f_xPoint += au8_can_data[0] >> 1;
as_objects[u16_objectIndex].f_xPoint -= 4096;
as_objects[u16_objectIndex].f_xPoint *= 0.128;
as_objects[u16_objectIndex].f_yPoint = (au8_can_data[3] & 0x7) << 10;
as_objects[u16_objectIndex].f_yPoint += au8_can_data[2] << 2;
as_objects[u16_objectIndex].f_yPoint += au8_can_data[1] >> 6;
as_objects[u16_objectIndex].f_yPoint -= 4096;
as_objects[u16_objectIndex].f_yPoint *= 0.128;
as_objects[u16_objectIndex].f_xSpeed = ((au8_can_data[4] & 0x3F) << 5);
as_objects[u16_objectIndex].f_xSpeed += (au8_can_data[3] >> 3);
as_objects[u16_objectIndex].f_xSpeed -= 1024;
as_objects[u16_objectIndex].f_xSpeed *= 0.1;
as_objects[u16_objectIndex].f_ySpeed = (au8_can_data[6] & 0x1) << 10;
as_objects[u16_objectIndex].f_ySpeed += (au8_can_data[5] << 2);
as_objects[u16_objectIndex].f_ySpeed += (au8_can_data[4] >> 6);
as_objects[u16_objectIndex].f_ySpeed -= 1024.0f;
as_objects[u16_objectIndex].f_ySpeed *= 0.1;
as_objects[u16_objectIndex].f_objectLength = (au8_can_data[6] >> 1) * 0.2f;
as_objects[u16_objectIndex].u8_objectID = au8_can_data[7];
} else if (s_objectControl.u8_objectData0Format == 4) {
as_objects[u16_objectIndex].f_xPoint = (au8_can_data[1] & 0x3F) << 7;
as_objects[u16_objectIndex].f_xPoint += au8_can_data[0] >> 1;
as_objects[u16_objectIndex].f_xPoint -= 4096;
as_objects[u16_objectIndex].f_xPoint *= 0.128;
as_objects[u16_objectIndex].f_yPoint = (au8_can_data[3] & 0x7) << 10;
as_objects[u16_objectIndex].f_yPoint += au8_can_data[2] << 2;
as_objects[u16_objectIndex].f_yPoint += au8_can_data[1] >> 6;
as_objects[u16_objectIndex].f_yPoint -= 4096;
as_objects[u16_objectIndex].f_yPoint *= 0.128;
as_objects[u16_objectIndex].f_xSpeed = (au8_can_data[4] << 5);
as_objects[u16_objectIndex].f_xSpeed += (au8_can_data[3] >> 3);
as_objects[u16_objectIndex].f_xSpeed -= 1024;
as_objects[u16_objectIndex].f_xSpeed *= 0.1;
as_objects[u16_objectIndex].f_ySpeed = (au8_can_data[6] & 0x1) << 10;
as_objects[u16_objectIndex].f_ySpeed += (au8_can_data[5] << 2);
as_objects[u16_objectIndex].f_ySpeed += (au8_can_data[4] >> 6);
as_objects[u16_objectIndex].f_ySpeed -= 1024.0f;
as_objects[u16_objectIndex].f_ySpeed *= 0.1;
as_objects[u16_objectIndex].f_objectLength = (au8_can_data[6] >> 1) * 0.2f;
as_objects[u16_objectIndex].u8_objectID = au8_can_data[7] & 0x7F;
as_objects[u16_objectIndex].u8_updateFlag = au8_can_data[7] >> 7;
}
}
}
void parse_input(u_int8_t* u8_input_buffer, int i32_len) {
static enum state_t s_state = IDLE;
u_int8_t u8_input = 0;
int i32_i;
static int i32_can_len_counter = 0, i32_can_len = 0, i32_can_id = 0, i32_stop_search = 0;
static unsigned char au8_can_data[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
static 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);
}
void mk_cpm(facilities_t* facilities,CPM_t* cpm_tx, struct timespec* systemtime) {
cpm_tx->header.protocolVersion = PROTOCOL_VERSION;
cpm_tx->header.messageID = MESSAGE_ID;
cpm_tx->header.stationID = facilities->id.value;
uint64_t generationDeltaTime = it2s_tender_get_clock(&facilities->epv) % 65536; // generationDeltaTime = TimestampIts mod 65 536
cpm_tx->cpm.generationDeltaTime = generationDeltaTime;
cpm_tx->cpm.cpmParameters.managementContainer.stationType = StationType_roadSideUnit;
cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.latitude = facilities->epv.space.latitude;
cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.longitude = facilities->epv.space.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;
cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.altitude.altitudeValue = 4000;
cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.altitude.altitudeConfidence = AltitudeConfidence_alt_005_00;
cpm_tx->cpm.cpmParameters.sensorInformationContainer = calloc(1, sizeof(SensorInformationContainer_t));
cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.count = 1;
cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.size = 1;
cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array = calloc(1, sizeof(SensorInformation_t));
cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0] = calloc(1, sizeof(SensorInformation_t));
cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->sensorID = 0;
cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->type = SensorType_radar;
cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.present = DetectionArea_PR_stationarySensorRadial;
cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.range = 3400;
cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.stationaryHorizontalOpeningAngleStart = ROAD_ANGLE * 10 - 500;
cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.stationaryHorizontalOpeningAngleEnd = ROAD_ANGLE * 10 + 500;
cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.verticalOpeningAngleStart = calloc(1, sizeof(CartesianAngleValue_t));
(*cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.verticalOpeningAngleStart) = 1730;
cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.verticalOpeningAngleEnd = calloc(1, sizeof(CartesianAngleValue_t));
(*cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.verticalOpeningAngleEnd) = 1890;
cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.sensorHeight = calloc(1, sizeof(SensorHeight_t));
(*cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.sensorHeight) = 600;
cpm_tx->cpm.cpmParameters.perceivedObjectContainer = calloc(1, sizeof(PerceivedObjectContainer_t));
cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.count = s_objectControl.u8_numberOfObjects;
cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.size = s_objectControl.u8_numberOfObjects;
cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array = calloc(
cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.count,
sizeof(PerceivedObject_t*));
for (int i = 0; i < cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.count; i++) {
cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i] = calloc(1, sizeof(PerceivedObject_t));
cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i]->objectID = (long)as_objects[i].u8_objectID;
cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i]->timeOfMeasurement = 0; //sem informaçao do radar
cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i]->objectConfidence = 95;
cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i]->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[i]->xDistance.confidence = 102;
// printf("->%f<-\n", as_objects[i].f_xPoint);
cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i]->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[i]->yDistance.confidence = 102;
cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i]->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[i]->xSpeed.confidence = 40;
cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i]->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[i]->ySpeed.confidence = 40;
cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[i]->objectRefPoint = ObjectRefPoint_bottomMid;
}
cpm_tx->cpm.cpmParameters.numberOfPerceivedObjects = cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.count;
}
int encode_cpm(CPM_t* cpm_tx, uint8_t *out_message_data, uint32_t *out_message_size) {
memset(out_message_data, 0, 1500);
asn_enc_rval_t retval_enc = uper_encode_to_buffer(&asn_DEF_CPM, NULL, cpm_tx, out_message_data, 1500);
if (retval_enc.encoded == -1) {
syslog_err("[facilities] [cp] failed encoding CPM (%s)", retval_enc.failed_type->name);
return 1;
}
*out_message_size = ((retval_enc.encoded + 7) / 8);
return 0;
}
dissemination_t* dissemination_init(){
/* Mutex init and dissemination memory allocation */
dissemination_t* dissemination = (dissemination_t*) calloc(1, sizeof(dissemination_t));
pthread_mutex_init(&dissemination->lock, NULL);
return dissemination;
}
int dissemination_check(dissemination_t* dissemination, it2s_tender_epv_t* epv) {
int rv = 0;
uint64_t now = it2s_tender_get_clock(epv);
/* mutex is used to lock shared resources */
pthread_mutex_lock(&dissemination->lock);
// Both cases for OBU and RSU (BASIC: generation interval 1s)
if (now >= dissemination->next_cpm_max)
rv = 1;
pthread_mutex_unlock(&dissemination->lock);
return rv;
}
void dissemination_reset_timer(dissemination_t* dissemination, it2s_tender_epv_t* epv){
uint64_t now = it2s_tender_get_clock(epv);
/* Both cases for RSU and OBU */
pthread_mutex_lock(&dissemination->lock);
dissemination->next_cpm_min = now + dissemination->T_GenCpmMin;
dissemination->next_cpm_max = now + dissemination->T_GenCpmMax;
pthread_mutex_unlock(&dissemination->lock);
}
void *cp_service(void *fc){
/* Variables */
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];
bdr_oer[0] = 4; //Facilities
fdi_oer[0] = 4;
BTPDataRequest_t *bdr = calloc(1, sizeof(BTPDataRequest_t));
FacilitiesDataIndication_t *fdi = calloc(1, sizeof(FacilitiesDataIndication_t));
roadRotationSin = sin(((facilities->dissemination->radar_rotation + 90.0) * PI) / 180);
roadRotationCos = cos(((facilities->dissemination->radar_rotation + 90.0) * PI) / 180);
/*--- Fill mandatory BTP Data Request parameters ---*/
bdr->gnDestinationAddress.buf = malloc(6);
bdr->gnDestinationAddress.size = 6;
for(int i = 0; i < 6; i++)
bdr->gnDestinationAddress.buf[i] = 0xff; //Broadcast addr
bdr->btpType = BTPType_btpB; //BTP Type B is for non-interactive packet transport | BTP Type A is for interactive packet transport
//The former doesn't have a source port and the latter have
bdr->destinationPort = Port_cpm; //CPM entity port for communication between Facilities and Transport
bdr->gnPacketTransportType = PacketTransportType_shb; //shb = Single Hop Broadcast packet
bdr->gnTrafficClass = 2; //Identifier assigned to a GeoNetworking packet that expresses its requirements on data transport
bdr->data.buf = malloc(1500); //CPM Data to be sent to the Transport layer
if(facilities->use_security) {
bdr->gnSecurityProfile = malloc(sizeof(long));
*bdr->gnSecurityProfile = 1;
}
is_radar_connected = interface_connection(RADAR_PORT,facilities); // Create Radar listening socket
/*--- Fill mandatory Facilities Data Indication parameters ---*/
fdi->itsMessageType = ItsMessageType_cpm;
fdi->data.buf = malloc(1500);
struct timespec* systemtime;
systemtime = (struct timespec*)malloc(sizeof(struct timespec));
if (systemtime == NULL) {
printf("memory allocation failed: %m");
exit(1);
}
while(!facilities->exit){
usleep(1000*50);
/* 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){
if(i32_recv_bytes <= 0){
syslog_debug("No data received from radar ...");
is_radar_connected = false;
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;
}
if(encode_cpm(cpm_tx, fdi->data.buf, (uint32_t *) &fdi->data.size) == 1){
ASN_STRUCT_FREE(asn_DEF_CPM, cpm_tx);
continue;
}
ASN_STRUCT_FREE(asn_DEF_CPM, cpm_tx);
/* Encode BTPDataRequest */
asn_enc_rval_t enc_bdr = oer_encode_to_buffer(&asn_DEF_BTPDataRequest, NULL, bdr, bdr_oer+1, 2047);
if(enc_bdr.encoded == -1){
syslog_err("[facilities] encoding BTPDataRequest for cpm failed");
continue;
}
/* Encode FacilitiesDataIndication */
asn_enc_rval_t enc_fdi = oer_encode_to_buffer(&asn_DEF_FacilitiesDataIndication, NULL, fdi, fdi_oer+1, 2047);
if(enc_fdi.encoded == -1){
syslog_err("[facilities] encoding FacilitiesDataIndication for cpm failed");
continue;
}
/* Create thread to send packet to the Transport Layer (=3) */
queue_add(facilities->tx_queue, bdr_oer, enc_bdr.encoded+1, 3);
pthread_cond_signal(&facilities->tx_queue->trigger);
/* Create thread to send packet to the Applications Layer (=5) */
queue_add(facilities->tx_queue, fdi_oer, enc_fdi.encoded+1, 5);
pthread_cond_signal(&facilities->tx_queue->trigger);
/*Reset Timer for dissemination control */
dissemination_reset_timer(facilities->dissemination, &facilities->epv);
} else { /* Waiting for Radar to reconnect */
is_radar_connected = waitingIncomingConnection();
}
}
}
ASN_STRUCT_FREE(asn_DEF_BTPDataRequest,bdr);
ASN_STRUCT_FREE(asn_DEF_FacilitiesDataIndication,fdi);
return NULL;
}
Reference in New Issue View Git Blame Copy Permalink