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

787 lines
33 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/TransportRequest.h>
#include <it2s-config.h>
#include <math.h>
#include <signal.h>
#include <unistd.h>
#include <itss-facilities/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;
if (s_objectControl.u8_objectData0Format == 005) // without Update Flag
{
as_objects[u16_objectIndex].f_xPoint = (au8_can_data[1] & 0x3F) << 7;
as_objects[u16_objectIndex].f_xPoint += au8_can_data[0] >> 1;
as_objects[u16_objectIndex].f_xPoint -= 4096;
as_objects[u16_objectIndex].f_xPoint *= 0.128;
as_objects[u16_objectIndex].f_yPoint = (au8_can_data[3] & 0x7) << 10;
as_objects[u16_objectIndex].f_yPoint += au8_can_data[2] << 2;
as_objects[u16_objectIndex].f_yPoint += au8_can_data[1] >> 6;
as_objects[u16_objectIndex].f_yPoint -= 4096;
as_objects[u16_objectIndex].f_yPoint *= 0.128;
as_objects[u16_objectIndex].f_xSpeed = ((au8_can_data[4] & 0x3F) << 5);
as_objects[u16_objectIndex].f_xSpeed += (au8_can_data[3] >> 3);
as_objects[u16_objectIndex].f_xSpeed -= 1024;
as_objects[u16_objectIndex].f_xSpeed *= 0.1;
as_objects[u16_objectIndex].f_ySpeed = (au8_can_data[6] & 0x1) << 10;
as_objects[u16_objectIndex].f_ySpeed += (au8_can_data[5] << 2);
as_objects[u16_objectIndex].f_ySpeed += (au8_can_data[4] >> 6);
as_objects[u16_objectIndex].f_ySpeed -= 1024.0f;
as_objects[u16_objectIndex].f_ySpeed *= 0.1;
as_objects[u16_objectIndex].f_objectLength = (au8_can_data[6] >> 1) * 0.2f;
as_objects[u16_objectIndex].u8_objectID = au8_can_data[7] & 0x7F;
} else if (s_objectControl.u8_objectData0Format == 4) {
as_objects[u16_objectIndex].f_xPoint = (au8_can_data[1] & 0x3F) << 7;
as_objects[u16_objectIndex].f_xPoint += au8_can_data[0] >> 1;
as_objects[u16_objectIndex].f_xPoint -= 4096;
as_objects[u16_objectIndex].f_xPoint *= 0.128;
as_objects[u16_objectIndex].f_yPoint = (au8_can_data[3] & 0x7) << 10;
as_objects[u16_objectIndex].f_yPoint += au8_can_data[2] << 2;
as_objects[u16_objectIndex].f_yPoint += au8_can_data[1] >> 6;
as_objects[u16_objectIndex].f_yPoint -= 4096;
as_objects[u16_objectIndex].f_yPoint *= 0.128;
as_objects[u16_objectIndex].f_xSpeed = (au8_can_data[4] << 5);
as_objects[u16_objectIndex].f_xSpeed += (au8_can_data[3] >> 3);
as_objects[u16_objectIndex].f_xSpeed -= 1024;
as_objects[u16_objectIndex].f_xSpeed *= 0.1;
as_objects[u16_objectIndex].f_ySpeed = (au8_can_data[6] & 0x1) << 10;
as_objects[u16_objectIndex].f_ySpeed += (au8_can_data[5] << 2);
as_objects[u16_objectIndex].f_ySpeed += (au8_can_data[4] >> 6);
as_objects[u16_objectIndex].f_ySpeed -= 1024.0f;
as_objects[u16_objectIndex].f_ySpeed *= 0.1;
as_objects[u16_objectIndex].f_objectLength = (au8_can_data[6] >> 1) * 0.2f;
as_objects[u16_objectIndex].u8_objectID = au8_can_data[7] & 0x7F;
as_objects[u16_objectIndex].u8_updateFlag = au8_can_data[7] >> 7;
}
}
}
void parse_input(u_int8_t* u8_input_buffer, int i32_len) {
enum state_t s_state = IDLE;
u_int8_t u8_input = 0;
int i32_i;
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, CPM_t* cpm_tx, long history_list[NOF_OBJECTS][4], int valid_array[], uint64_t history_timestamp[]){
/* Fill CPM */
cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j] = calloc(1, sizeof(PerceivedObject_t));
cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->objectID = (long)as_objects[i].u8_objectID;
cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->timeOfMeasurement = 0; //Sem informaçao do radar
cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->objectConfidence = 95;
cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->xDistance.value = rotate_x(
(long)as_objects[i].f_xPoint * 100, (long)as_objects[i].f_yPoint * 100);
cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->xDistance.confidence = 102;
cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->yDistance.value = rotate_y(
(long)as_objects[i].f_xPoint * 100, (long)as_objects[i].f_yPoint * 100);
cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->yDistance.confidence = 102;
cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->xSpeed.value = rotate_x(
(long)as_objects[i].f_xSpeed * 100, (long)as_objects[i].f_ySpeed * 100);
cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->xSpeed.confidence = 40;
cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->ySpeed.value = rotate_y(
(long)as_objects[i].f_xSpeed * 100, (long)as_objects[i].f_ySpeed * 100);
cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->ySpeed.confidence = 40;
cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->objectRefPoint = ObjectRefPoint_bottomMid;
/* Fill History values */
valid_array[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 */
CPM_t* cpm_tx = calloc(1, sizeof(CPM_t));
long euclidian_dist, abs_speed, abs_speed_hist, angle, angle_hist, angle_diff;
int j = 0, rv = 0;
int temp[NOF_OBJECTS];
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.latitude;
lon = epv.space.longitude;
alt = epv.space.altitude;
alt_conf = epv.space.altitude_conf;
itss_space_unlock();
cpm_tx->cpm.generationDeltaTime = generationDeltaTime;
cpm_tx->cpm.cpmParameters.managementContainer.stationType = StationType_roadSideUnit;
cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.latitude = lat;
cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.longitude = lon;
cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.positionConfidenceEllipse.semiMajorConfidence = 100; // TODO
cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.positionConfidenceEllipse.semiMinorConfidence = 100; // TODO
cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.positionConfidenceEllipse.semiMajorOrientation = HeadingValue_wgs84North; // TODO
cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.altitude.altitudeValue = alt;
cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.altitude.altitudeConfidence = alt_conf;
if(dissemination_check(0) == 1){ /* Sensor Information Container Inclusion Management */
cpm_tx->cpm.cpmParameters.sensorInformationContainer = calloc(1, sizeof(SensorInformationContainer_t));
cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.count = 1;
cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.size = 1;
cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array = calloc(1, sizeof(SensorInformation_t));
cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0] = calloc(1, sizeof(SensorInformation_t));
cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->sensorID = 0;
cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->type = SensorType_radar;
cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.present = DetectionArea_PR_stationarySensorRadial;
cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.range = 3400;
cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.stationaryHorizontalOpeningAngleStart = ROAD_ANGLE * 10 - 500;
cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.stationaryHorizontalOpeningAngleEnd = ROAD_ANGLE * 10 + 500;
cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.verticalOpeningAngleStart = calloc(1, sizeof(CartesianAngleValue_t));
(*cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.verticalOpeningAngleStart) = 1730;
cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.verticalOpeningAngleEnd = calloc(1, sizeof(CartesianAngleValue_t));
(*cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.verticalOpeningAngleEnd) = 1890;
cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.sensorHeight = calloc(1, sizeof(SensorHeight_t));
(*cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.sensorHeight) = 600;
dissemination_reset_timer(0);
}
if (s_objectControl.u8_numberOfObjects > 0) {
cpm_tx->cpm.cpmParameters.perceivedObjectContainer = calloc(1, sizeof(PerceivedObjectContainer_t));
cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array = calloc(s_objectControl.u8_numberOfObjects,sizeof(PerceivedObject_t*));
cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.count = s_objectControl.u8_numberOfObjects;
memcpy(temp, valid_array, NOF_OBJECTS * sizeof(int)); // NOF_OBJECTS * sizeof(int) = size of valid_array
memset(valid_array, 0, NOF_OBJECTS * sizeof(int));
for(int i = 0; i < cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.count;i++){
if(temp[as_objects[i].u8_objectID] == 0 ){ // The object is going to be added without comparison (It is a new object) (valid_array[id] = 0)
set_values(i,j,generationDeltaTime,cpm_tx,history_list,valid_array,history_timestamp);
j++;
}else{ // The object is going to be compared (It was included in previous CPMs) (valid_array[id] = 1)
// Getting the euclidian distance value from the object detected and the same object in the last cpm (xcurrent - xhistory)^2 + (ycurrent - yhistory)^2
euclidian_dist = sqrt( pow(((long)as_objects[i].f_xPoint * 100) - (history_list[(long)as_objects[i].u8_objectID][0]), 2) + pow(((long)as_objects[i].f_yPoint * 100) - (history_list[(long)as_objects[i].u8_objectID][1]) ,2) );
// Getting the absolute speed value from the object detected and the same object in the last cpm (sqrt(x^2 + y^2))
abs_speed = sqrt( pow( ((long)as_objects[i].f_xSpeed * 100),2) + pow( ( (long)as_objects[i].f_ySpeed * 100),2) );
abs_speed_hist = sqrt( pow( history_list[(long)as_objects[i].u8_objectID][2] ,2) + pow( history_list[(long)as_objects[i].u8_objectID][3],2) ); // sqrt(xSpeed^2 + ySpeed^2)
// Getting the angle from the velocity vector detected and the same object in the last cpm
angle = (long)((180 / PI) * atan2( (long)as_objects[i].f_ySpeed * 100 , (long)as_objects[i].f_xSpeed * 100 ));
angle_hist = (long)((180 / PI) * atan2( history_list[(long)as_objects[i].u8_objectID][3] , history_list[(long)as_objects[i].u8_objectID][2]) ); // tan(yspeed / xspeed)
angle_diff = ((angle - angle_hist) + 180) % 360 - 180;
// Requirements to include the object in the CPM (> 4 m or > 0.5 m/s or > 4º or > T_GenCpmMax)
if(abs(euclidian_dist) > 400 || abs(abs_speed - abs_speed_hist) > 50 || abs(angle_diff) > 4 || abs(generationDeltaTime - history_timestamp[i]) >= facilities.dissemination.T_GenCpmMax){
set_values(i,j,generationDeltaTime,cpm_tx, history_list, valid_array,history_timestamp);
j++;
}else{ //The object is not included but is valid for comparison in the upcoming CPMs
valid_array[(long)as_objects[i].u8_objectID] = 1;
}
}
}
cpm_tx->cpm.cpmParameters.numberOfPerceivedObjects = cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.count; // Object perceived by the Radar (Does not have to match the objects included in the CPM)
cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.count = j; // The number of objects that were included in the CPM
cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.size = j;
}
cpm_tx->cpm.cpmParameters.numberOfPerceivedObjects = j;
/******* Encode CPMs to FDI and BDR ********/
//BDR
memset(bdr_oer, 0, 1500);
asn_enc_rval_t retval_enc_bdr = uper_encode_to_buffer(&asn_DEF_CPM, NULL, cpm_tx, bdr_oer, 1500);
if (retval_enc_bdr.encoded == -1) {
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_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_CPM, cpm_tx);
return rv;
}
void *cp_service(){
/* 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[2048];
uint8_t fi_oer[2048];
tr_oer[0] = 4; //Facilities
fi_oer[0] = 4;
TransportRequest_t* tr = calloc(1, sizeof(TransportRequest_t));
tr->present = TransportRequest_PR_packet;
TransportPacketRequest_t* tpr = &tr->choice.packet;
tpr->present = TransportPacketRequest_PR_btp;
BTPPacketRequest_t *bpr = &tpr->choice.btp;
FacilitiesIndication_t* fi = calloc(1, sizeof(FacilitiesIndication_t));
fi->present = FacilitiesIndication_PR_message;
FacilitiesMessageIndication_t* fmi = &fi->choice.message;
roadRotationSin = sin(((facilities.dissemination.radar_rotation + 90.0) * PI) / 180);
roadRotationCos = cos(((facilities.dissemination.radar_rotation + 90.0) * PI) / 180);
/*--- Fill mandatory BTP Data Request parameters ---*/
bpr->gn.destinationAddress.buf = malloc(6);
bpr->gn.destinationAddress.size = 6;
for(int i = 0; i < 6; i++)
bpr->gn.destinationAddress.buf[i] = 0xff; //Broadcast addr
bpr->btpType = BTPType_btpB; //BTP Type B is for non-interactive packet transport | BTP Type A is for interactive packet transport
//The former doesn't have a source port and the latter have
bpr->destinationPort = Port_cpm; //CPM entity port for communication between Facilities and Transport
bpr->gn.packetTransportType = PacketTransportType_shb; //shb = Single Hop Broadcast packet
bpr->gn.trafficClass = 2; //Identifier assigned to a GeoNetworking packet that expresses its requirements on data transport
bpr->data.buf = malloc(1500); //CPM Data to be sent to the Transport layer
bpr->gn.securityProfile.sign = true;
/*--- Fill mandatory Facilities Message Indication parameters ---*/
fmi->itsMessageType = ItsMessageType_cpm;
fmi->data.buf = malloc(1500);
/* Creating sockets and waiting for radar to connect*/
is_radar_connected = radar_connection(RADAR_PORT);
while(!facilities.exit){
usleep(1000*50);
/* If the Radar is not connected to TMC, a TCP socket is needed to fool the Radar */
/* To maintain the connection the content must be read */
if(facilities.dissemination.tmc_connect == false)
i32_recv_bytes = recv(s_socket.i32_client, &au8_readTcp, READ_BUFFER_SIZE, 0);
/* Reads from the radar */
i32_recv_bytes = recv(raw_socket.raw_fd, &au8_readBuffer, READ_BUFFER_SIZE, 0);
if (dissemination_check(1) && facilities.dissemination.active){
if(is_radar_connected){
/* Information parsing from radar */
parse_input(au8_readBuffer,i32_recv_bytes);
/* CPM build and encoding to BDR and FDI */
if(mk_cpm(bpr->data.buf, (uint32_t *) &bpr->data.size, fmi->data.buf, (uint32_t *) &fmi->data.size, history_list, valid_array, history_timestamp) == 1)
continue;
uint32_t id = itss_id(bpr->data.buf, bpr->data.size);
bpr->id = id;
fmi->id = id;
/* Encode TransportRequest */
asn_enc_rval_t enc_tdr = oer_encode_to_buffer(&asn_DEF_TransportRequest, NULL, tr, tr_oer+1, 2047);
if(enc_tdr.encoded == -1){
log_error("encoding TR for cpm failed");
continue;
}
/* Encode FacilitiesIndication */
asn_enc_rval_t enc_fdi = oer_encode_to_buffer(&asn_DEF_FacilitiesIndication, NULL, fi, fi_oer+1, 2047);
if(enc_fdi.encoded == -1){
log_error("encoding FI for cpm failed");
continue;
}
/* Create thread to send packet to the Transport Layer (=3) */
itss_queue_send(facilities.tx_queue, tr_oer, enc_tdr.encoded+1, ITSS_TRANSPORT, id, "TR.packet.btp");
/* Create thread to send packet to the Applications Layer (=5) */
itss_queue_send(facilities.tx_queue, 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);
uint64_t station_id = facilities.id.station_id;
pthread_mutex_unlock(&facilities.id.lock);
itss_db_add(facilities.logging.dbms, station_id, bpr->id, true, 14, NULL, bpr->data.buf, bpr->data.size);
}
if (facilities.logging.recorder) {
uint16_t buffer_len = 2048;
uint8_t buffer[buffer_len];
int e = itss_management_record_packet_sdu(
buffer,
buffer_len,
bpr->data.buf,
bpr->data.size,
bpr->id,
itss_time_get(),
ITSS_FACILITIES,
true);
if (e != -1) {
itss_queue_send(facilities.tx_queue, buffer, e, ITSS_MANAGEMENT, bpr->id, "MReq.packet.set");
}
}
}else{
is_radar_connected = radar_connection(RADAR_PORT);
}
}
}
ASN_STRUCT_FREE(asn_DEF_TransportRequest, tr);
ASN_STRUCT_FREE(asn_DEF_FacilitiesIndication, fi);
/* Close sockets */
if(facilities.dissemination.tmc_connect)
shutdown(s_socket.i32_socket,2);
shutdown(raw_socket.raw_fd,2);
return NULL;
}
Reference in New Issue View Git Blame Copy Permalink