Added necessary files for last commit
This commit is contained in:
Marco
parent
4d5d859453
commit
180fd4766b
|
|
@ -0,0 +1,620 @@
|
||||||
|
#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 <syslog.h>
|
||||||
|
#include <arpa/inet.h>
|
||||||
|
#include <stdbool.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <signal.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_OBJECT_CONTROL_T s_objectControl;
|
||||||
|
S_OBJECTS_T as_objects[NOF_OBJECTS];
|
||||||
|
char* station_id;
|
||||||
|
it2s_config_t* config;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
bool interface_connection(char* radar_ip,char* radar_port){
|
||||||
|
|
||||||
|
// 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;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// 3 - 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;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
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 = STATION_ID;
|
||||||
|
|
||||||
|
int retval = clock_gettime(CLOCK_REALTIME, systemtime);
|
||||||
|
if (retval != 0) {
|
||||||
|
perror("clock_gettime() failed");
|
||||||
|
exit(1);
|
||||||
|
}
|
||||||
|
|
||||||
|
long timeStampIts = (long)(systemtime->tv_sec * 1000 + systemtime->tv_nsec / 1E6);
|
||||||
|
timeStampIts = timeStampIts - 1072915200000; // EPOCH -> 2004/01/01 00:00:000
|
||||||
|
long generationDeltaTime = timeStampIts % 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 = 420309860; //A3 // 412400078; (A24)
|
||||||
|
cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.longitude = -86515778; //A3 // -86950224; (A24)
|
||||||
|
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, (long)as_objects[i].f_yPoint);
|
||||||
|
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, (long)as_objects[i].f_yPoint);
|
||||||
|
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, (long)as_objects[i].f_ySpeed);
|
||||||
|
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, (long)as_objects[i].f_ySpeed);
|
||||||
|
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){
|
||||||
|
int rv = 0;
|
||||||
|
|
||||||
|
/* Get Now time */
|
||||||
|
struct timespec systemtime;
|
||||||
|
clock_gettime(CLOCK_REALTIME, &systemtime);
|
||||||
|
long now = (long)((systemtime.tv_sec + LEAP_SECONDS) * 1000 + systemtime.tv_nsec / 1E6);
|
||||||
|
now -= 1072915200000;
|
||||||
|
|
||||||
|
/* 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){
|
||||||
|
|
||||||
|
/* Get Now time */
|
||||||
|
struct timespec systemtime;
|
||||||
|
clock_gettime(CLOCK_REALTIME, &systemtime);
|
||||||
|
long now = (long)((systemtime.tv_sec + LEAP_SECONDS) * 1000 + systemtime.tv_nsec / 1E6);
|
||||||
|
now -= 1072915200000;
|
||||||
|
|
||||||
|
/* 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){
|
||||||
|
//roadRotationSin = sin(((config->applications.its_center.rotation + 90.0) * PI) / 180);
|
||||||
|
//roadRotationCos = cos(((config->applications.its_center.rotation + 90.0) * PI) / 180);
|
||||||
|
|
||||||
|
roadRotationSin = 0.8660; // Default values for A3 Configuration
|
||||||
|
roadRotationCos = -0.5000; // Default values for A3 configuration
|
||||||
|
|
||||||
|
|
||||||
|
facilities_t *facilities = (facilities_t *) fc;
|
||||||
|
BTPDataRequest_t *bdr = calloc(1, sizeof(BTPDataRequest_t));
|
||||||
|
|
||||||
|
/* 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 (WHY IS IT 2?)
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
/* Not needed
|
||||||
|
read_config("/etc/it2s/itss.toml"); // To get the parameters of the broker
|
||||||
|
signal(SIGINT, interruption_handler); // In case Ctrl+C is sent to the process interruption_handler will be executed
|
||||||
|
setup_mec_server_connection();
|
||||||
|
init_tiles();
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
interface_connection(RADAR_IP,RADAR_PORT); // Create Radar listening socket
|
||||||
|
struct timespec* systemtime;
|
||||||
|
systemtime = (struct timespec*)malloc(sizeof(struct timespec));
|
||||||
|
if (systemtime == NULL) {
|
||||||
|
printf("memory allocation failed: %m");
|
||||||
|
exit(1);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/* Variables */
|
||||||
|
int i32_recv_bytes;
|
||||||
|
u_int8_t au8_readBuffer[READ_BUFFER_SIZE];
|
||||||
|
//void* message_data = malloc(1500);
|
||||||
|
uint8_t bdr_oer[2048];
|
||||||
|
bdr_oer[0] = 4; //Facilities
|
||||||
|
|
||||||
|
|
||||||
|
while(!facilities->exit){
|
||||||
|
usleep(1000*50);
|
||||||
|
|
||||||
|
|
||||||
|
if (dissemination_check(facilities->dissemination) && facilities->dissemination->active){
|
||||||
|
/* 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(i32_recv_bytes < 0){
|
||||||
|
syslog_debug("No data received from radar ...");
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Information parsing */
|
||||||
|
parse_input(au8_readBuffer,i32_recv_bytes);
|
||||||
|
|
||||||
|
/* CPM build and encoding (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;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Encode BTPDataRequest */
|
||||||
|
|
||||||
|
asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_BTPDataRequest, NULL, bdr, bdr_oer+1, 2047);
|
||||||
|
if(enc.encoded == -1){
|
||||||
|
syslog_err("[facilities] encoding BTPDataRequest for cpm failed");
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Create thread to send packet to the Transport Layer (=3) */
|
||||||
|
queue_add(facilities->tx_queue, bdr_oer, enc.encoded+1, 3);
|
||||||
|
pthread_cond_signal(&facilities->tx_queue->trigger);
|
||||||
|
|
||||||
|
|
||||||
|
/* Reset Timer for dissemination control */
|
||||||
|
|
||||||
|
dissemination_reset_timer(facilities->dissemination);
|
||||||
|
|
||||||
|
/* TODO: Send message to applications to send it to the broker ? *FacilitiesDataIndication*/
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
ASN_STRUCT_FREE(asn_DEF_BTPDataRequest,bdr);
|
||||||
|
return NULL;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,198 @@
|
||||||
|
#ifndef FACILITIES_CPM_H
|
||||||
|
#define FACILITIES_CPM_H
|
||||||
|
|
||||||
|
|
||||||
|
#include <arpa/inet.h>
|
||||||
|
#include <stdbool.h>
|
||||||
|
|
||||||
|
#include <cpm/CPM.h>
|
||||||
|
#include <termios.h>
|
||||||
|
#include <unistd.h>
|
||||||
|
#include <time.h>
|
||||||
|
|
||||||
|
#define LEAP_SECONDS 5
|
||||||
|
#define READ_BUFFER_SIZE 1024 // Buffer for Serial and Ethernet read function (Used in cp_service)
|
||||||
|
#define STATION_ID 10
|
||||||
|
#define MESSAGE_ID 14
|
||||||
|
#define PROTOCOL_VERSION 1
|
||||||
|
#define CAN_BUFFER_SIZE 24 // Send cmd Buffer
|
||||||
|
#define CAN_SMS_BUFFER_SIZE 25 // Send cmd Buffer for sms transport protocol
|
||||||
|
#define SERIAL_PORT_BUFFER 13 // string buffer serial interface
|
||||||
|
#define BAUDRATE B115200 // default Baudrate
|
||||||
|
#define CRC16_LENGTH 2
|
||||||
|
#define CRC16_START_VALUE 0xFFFF // Start value CRC16 CCITT
|
||||||
|
#define NOF_OBJECTS 127 // Number of Object UMRR-0C
|
||||||
|
#define ROAD_ANGLE 120
|
||||||
|
|
||||||
|
|
||||||
|
#define RADAR_IP "11.11.11.13" // ITS-S interface connected to the radar
|
||||||
|
#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 i32_fdSerial; //Serial Port descriptor
|
||||||
|
char c_portNumber;
|
||||||
|
struct termios s_config;
|
||||||
|
} S_SERIAL_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_devid;
|
||||||
|
u_int8_t u8_partyp;
|
||||||
|
u_int8_t u8_parno;
|
||||||
|
int32_t i32_value;
|
||||||
|
float f_value;
|
||||||
|
u_int8_t u8_action;
|
||||||
|
u_int8_t u8_xor;
|
||||||
|
u_int8_t au8_can_cmd[CAN_BUFFER_SIZE];
|
||||||
|
u_int8_t au8_can_cmd_SMS_TRANSPORT_PROTOCOL[CAN_SMS_BUFFER_SIZE];
|
||||||
|
} S_COMMAND_T;
|
||||||
|
|
||||||
|
typedef struct
|
||||||
|
{
|
||||||
|
u_int16_t u16_startPattern;
|
||||||
|
u_int8_t u8_protocolVersion;
|
||||||
|
u_int8_t u8_headerLength;
|
||||||
|
u_int16_t u16_payloadLength;
|
||||||
|
u_int8_t u8_AppProtocolType;
|
||||||
|
u_int32_t u32_flags;
|
||||||
|
u_int16_t u16_headerCRC16;
|
||||||
|
u_int8_t au8_Payload[1024];
|
||||||
|
u_int8_t au8_Header[1024];
|
||||||
|
u_int16_t u16_PayloadCRC16;
|
||||||
|
u_int16_t u16_indexHeader;
|
||||||
|
u_int16_t u16_indexPayload;
|
||||||
|
} S_SMS_DATA_FRAME_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;
|
||||||
|
|
||||||
|
pthread_mutex_t lock;
|
||||||
|
|
||||||
|
uint8_t type;
|
||||||
|
|
||||||
|
uint64_t next_cpm_max;
|
||||||
|
uint64_t next_cpm_min;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
uint64_t T_GenCpmMin;
|
||||||
|
uint64_t T_GenCpmMax;
|
||||||
|
|
||||||
|
} dissemination_t;
|
||||||
|
|
||||||
|
/* Prototype Functions */
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
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* fc);
|
||||||
|
|
||||||
|
#endif
|
||||||
Loading…
Reference in New Issue