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

Time, gps manipulation

This commit is contained in:
emanuel 2021-04-30 19:16:33 +01:00
parent f07f47a193
commit b7ea3981bf
12 changed files with 206 additions and 247 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/CMakeLists.txt
View File

@ -21,6 +21,7 @@ TARGET_LINK_LIBRARIES(it2s-itss-facilities
-lit2s-asn-ivim
-lit2s-asn-denmv2
-lit2s-asn-cpm
-lit2s-tender
-lm
)

106
src/cam.c
View File

@ -13,6 +13,9 @@
#include <syslog.h>
#include <math.h>
#include <it2s-tender/space.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__)
@ -75,11 +78,6 @@ static int permissions_check(int cid, uint8_t* permissions, uint8_t permissions_
static int mk_cam(facilities_t* facilities, uint8_t *cam_oer, uint32_t *cam_len) {
int rv = 0;
struct timespec systemtime;
if (clock_gettime(CLOCK_REALTIME, &systemtime)) {
syslog_emerg("clock_gettime() failed");
}
CAM_t *cam = calloc(1, sizeof(CAM_t));
cam->header.protocolVersion = 2;
@ -91,18 +89,22 @@ static int mk_cam(facilities_t* facilities, uint8_t *cam_oer, uint32_t *cam_len)
BasicContainer_t* bc = &cam->cam.camParameters.basicContainer;
uint64_t now = it2s_tender_get_clock(&facilities->epv);
lightship_t* lightship = facilities->lightship;
pthread_mutex_lock(&lightship->lock);
if (facilities->station_type != StationType_roadSideUnit) {
pthread_mutex_lock(&facilities->epv.lock);
cam->cam.generationDeltaTime = facilities->epv.timestamp % 64536;
bc->referencePosition.altitude.altitudeValue = facilities->epv.altitude;
bc->referencePosition.altitude.altitudeConfidence = facilities->epv.altitude_conf;
cam->cam.generationDeltaTime = now % 65536;
it2s_tender_lock_space(&facilities->epv);
it2s_tender_get_space(&facilities->epv);
bc->referencePosition.altitude.altitudeValue = facilities->epv.space.altitude;
bc->referencePosition.altitude.altitudeConfidence = facilities->epv.space.altitude_conf;
// Set GPS coordinates
bc->referencePosition.latitude = facilities->epv.latitude;
bc->referencePosition.longitude = facilities->epv.longitude;
bc->referencePosition.latitude = facilities->epv.space.latitude;
bc->referencePosition.longitude = facilities->epv.space.longitude;
bc->referencePosition.positionConfidenceEllipse.semiMinorConfidence = SemiAxisLength_unavailable;
bc->referencePosition.positionConfidenceEllipse.semiMajorConfidence = SemiAxisLength_unavailable;
bc->referencePosition.positionConfidenceEllipse.semiMajorOrientation = HeadingValue_unavailable;
@ -111,14 +113,14 @@ static int mk_cam(facilities_t* facilities, uint8_t *cam_oer, uint32_t *cam_len)
BasicVehicleContainerHighFrequency_t* bvc_hf = &cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency;
// Set speed
bvc_hf->speed.speedValue = facilities->epv.speed;
bvc_hf->speed.speedConfidence = facilities->epv.speed_conf;
bvc_hf->speed.speedValue = facilities->epv.space.speed;
bvc_hf->speed.speedConfidence = facilities->epv.space.speed_conf;
// Set heading
bvc_hf->heading.headingValue = facilities->epv.heading;
bvc_hf->heading.headingConfidence = facilities->epv.heading_conf;
bvc_hf->heading.headingValue = facilities->epv.space.heading;
bvc_hf->heading.headingConfidence = facilities->epv.space.heading_conf;
pthread_mutex_unlock(&facilities->epv.lock);
it2s_tender_unlock_space(&facilities->epv);
bvc_hf->vehicleWidth = 20;
bvc_hf->vehicleLength.vehicleLengthValue = 46;
@ -149,10 +151,7 @@ static int mk_cam(facilities_t* facilities, uint8_t *cam_oer, uint32_t *cam_len)
lightship->pos_history[0]->lon = bc->referencePosition.longitude;
lightship->pos_history[0]->alt = bc->referencePosition.altitude.altitudeValue;
lightship->pos_history[0]->speed = bvc_hf->speed.speedValue;
struct timespec systemtime;
clock_gettime(CLOCK_REALTIME, &systemtime);
long now = (long)((systemtime.tv_sec + LEAP_SECONDS) * 1000 + systemtime.tv_nsec / 1E6);
now -= 1072915200000;
lightship->last_cam = now;
lightship->pos_history[0]->ts = now;
@ -215,18 +214,20 @@ static int mk_cam(facilities_t* facilities, uint8_t *cam_oer, uint32_t *cam_len)
}
} else {
pthread_mutex_lock(&facilities->epv.lock);
cam->cam.generationDeltaTime = facilities->epv.timestamp % 64536;
bc->referencePosition.altitude.altitudeValue = facilities->epv.altitude;
bc->referencePosition.altitude.altitudeConfidence = facilities->epv.altitude_conf;
cam->cam.generationDeltaTime = now % 65536;
it2s_tender_lock_space(&facilities->epv);
it2s_tender_get_space(&facilities->epv);
bc->referencePosition.altitude.altitudeValue = facilities->epv.space.altitude;
bc->referencePosition.altitude.altitudeConfidence = facilities->epv.space.altitude_conf;
// Set GPS coordinates
bc->referencePosition.latitude = facilities->epv.latitude;
bc->referencePosition.longitude = facilities->epv.longitude;
bc->referencePosition.latitude = facilities->epv.space.latitude;
bc->referencePosition.longitude = facilities->epv.space.longitude;
bc->referencePosition.positionConfidenceEllipse.semiMinorConfidence = SemiAxisLength_unavailable;
bc->referencePosition.positionConfidenceEllipse.semiMajorConfidence = SemiAxisLength_unavailable;
bc->referencePosition.positionConfidenceEllipse.semiMajorOrientation = HeadingValue_unavailable;
pthread_mutex_unlock(&facilities->epv.lock);
it2s_tender_unlock_space(&facilities->epv);
cam->cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_rsuContainerHighFrequency;
@ -289,13 +290,10 @@ lightship_t* lightship_init() {
}
int lightship_check(lightship_t* lightship, void* epv_s) {
int lightship_check(lightship_t* lightship, it2s_tender_epv_t* epv) {
int rv = 0;
struct timespec systemtime;
clock_gettime(CLOCK_REALTIME, &systemtime);
long now = (long)((systemtime.tv_sec + LEAP_SECONDS) * 1000 + systemtime.tv_nsec / 1E6);
now -= 1072915200000;
uint64_t now = it2s_tender_get_clock(epv);
pthread_mutex_lock(&lightship->lock);
@ -308,27 +306,27 @@ int lightship_check(lightship_t* lightship, void* epv_s) {
rv = 1;
} else if (now > lightship->next_cam_min) {
epv_t* epv = (epv_t*) epv_s;
it2s_tender_lock_space(epv);
it2s_tender_get_space(epv);
pthread_mutex_lock(&epv->lock);
// Check heading delta > 4º
int diff = epv->heading - lightship->pos_history[0]->heading;
int diff = epv->space.heading - lightship->pos_history[0]->heading;
if (abs(diff) > 40) rv = 1;
if (!rv) {
// Check speed delta > 0.5 m/s
diff = epv->speed - lightship->pos_history[0]->speed;
diff = epv->space.speed - lightship->pos_history[0]->speed;
if (abs(diff) > 50) rv = 1;
if (!rv) {
// Check position delta > 4 m
// TODO make an *accurate* distance calculator using GPS coords
int32_t avg_speed = (epv->speed + lightship->pos_history[0]->speed)/2 / 100; /* cm/s to m/s */
int32_t avg_speed = (epv->space.speed + lightship->pos_history[0]->speed)/2 / 100; /* cm/s to m/s */
uint64_t delta_time = (now - lightship->last_cam) / 1000; /* ms to s */
if (avg_speed * delta_time > 4) rv = 1;
}
}
pthread_mutex_unlock(&epv->lock);
it2s_tender_unlock_space(epv);
}
@ -354,11 +352,8 @@ int lightship_check(lightship_t* lightship, void* epv_s) {
return rv;
}
void lightship_reset_timer(lightship_t* lightship) {
struct timespec systemtime;
clock_gettime(CLOCK_REALTIME, &systemtime);
long now = (long)((systemtime.tv_sec + LEAP_SECONDS) * 1000 + systemtime.tv_nsec / 1E6);
now -= 1072915200000;
void lightship_reset_timer(lightship_t* lightship, it2s_tender_epv_t* epv) {
uint64_t now = it2s_tender_get_clock(epv);
pthread_mutex_lock(&lightship->lock);
@ -375,14 +370,11 @@ void lightship_reset_timer(lightship_t* lightship) {
pthread_mutex_unlock(&lightship->lock);
}
enum CAM_CHECK_R check_cam(void* fc, BTPDataIndication_t *bdi, CAM_t* cam, uint8_t* ssp, uint32_t ssp_len) {
enum CAM_CHECK_R check_cam(void* fc, BTPDataIndication_t *bdi, CAM_t* cam, it2s_tender_epv_t* epv, uint8_t* ssp, uint32_t ssp_len) {
int rv = 0;
lightship_t *lightship = ((facilities_t*) fc)->lightship;
struct timespec systemtime;
clock_gettime(CLOCK_REALTIME, &systemtime);
long now = (long)((systemtime.tv_sec + LEAP_SECONDS) * 1000 + systemtime.tv_nsec / 1E6);
now -= 1072915200000;
uint64_t now = it2s_tender_get_clock(epv);
// Check permissions
if (ssp) {
@ -558,14 +550,14 @@ enum CAM_CHECK_R check_cam(void* fc, BTPDataIndication_t *bdi, CAM_t* cam, uint8
return rv;
}
static int check_pz(lightship_t *lightship, void* epv_s) {
static int check_pz(lightship_t *lightship, it2s_tender_epv_t* epv) {
bool is_inside = false;
epv_t* epv = (epv_t*) epv_s;
pthread_mutex_lock(&epv->lock);
long lat = epv->latitude;
long lon = epv->longitude;
pthread_mutex_unlock(&epv->lock);
it2s_tender_lock_space(epv);
it2s_tender_get_space(epv);
long lat = epv->space.latitude;
long lon = epv->space.longitude;
it2s_tender_unlock_space(epv);
pthread_mutex_lock(&lightship->lock);
@ -604,7 +596,9 @@ void *ca_service(void *fc) {
bdr->btpType = BTPType_btpB;
bdr->gnDestinationAddress.buf = malloc(6);
for (int i = 0; i < 6; ++i) {bdr->gnDestinationAddress.buf[i] = 0xff;}
for (int i = 0; i < 6; ++i) {
bdr->gnDestinationAddress.buf[i] = 0xff;
}
bdr->gnDestinationAddress.size = 6;
bdr->gnPacketTransportType = PacketTransportType_shb;
@ -644,7 +638,7 @@ void *ca_service(void *fc) {
queue_add(facilities->tx_queue, bdr_oer, enc.encoded+1, 3);
pthread_cond_signal(&facilities->tx_queue->trigger);
lightship_reset_timer(facilities->lightship);
lightship_reset_timer(facilities->lightship, &facilities->epv);
}
}

8
src/cam.h
View File

@ -9,6 +9,8 @@
#include <camv2/CAM.h>
#include <itss-transport/BTPDataIndication.h>
#include <it2s-tender/epv.h>
#define POS_HISTORY_MAX_LEN 24
#define PATH_HISTORY_MAX_LEN POS_HISTORY_MAX_LEN-1
@ -82,10 +84,10 @@ typedef struct lightship {
lightship_t* lightship_init();
int lightship_check(lightship_t *lightship, void* epv_s);
void lightship_reset_timer(lightship_t *lightship);
int lightship_check(lightship_t* lightship, it2s_tender_epv_t* epv);
void lightship_reset_timer(lightship_t*lightship, it2s_tender_epv_t* epv);
enum CAM_CHECK_R check_cam(void* fc, BTPDataIndication_t* bdi, CAM_t* cam, uint8_t* ssp, uint32_t ssp_len);
enum CAM_CHECK_R check_cam(void* fc, BTPDataIndication_t* bdi, CAM_t* cam, it2s_tender_epv_t* epv, uint8_t* ssp, uint32_t ssp_len);
void* ca_service(void* fc);
#endif

41
src/config.c
View File

@ -209,7 +209,6 @@ int facilities_config(void* facilities_s) {
facilities->dissemination->T_GenCpmMax = config->facilities.cpm.rsu_obu_period_max;
facilities->dissemination->radar_rotation = config->applications.its_center.rotation;
// Replay
facilities->replay = config->networking.replay.activate;
@ -258,7 +257,9 @@ int facilities_config(void* facilities_s) {
}
}
pthread_mutex_init(&facilities->epv.lock, NULL);
pthread_mutex_init(&facilities->epv.space.lock, NULL);
pthread_mutex_init(&facilities->epv.time.lock, NULL);
facilities->epv.time.resolution = TIME_MILLISECONDS;
ManagementRequest_t* mreq = calloc(1, sizeof(ManagementRequest_t));
mreq->present = ManagementRequest_PR_get;
@ -266,6 +267,10 @@ int facilities_config(void* facilities_s) {
mreq->choice.get.altitude = 1;
mreq->choice.get.heading = 1;
mreq->choice.get.speed = 1;
mreq->choice.get.gpsType = 1;
mreq->choice.get.clockType = 1;
mreq->choice.get.clock = 1;
mreq->choice.get.clockOffset = 1;
void* management_socket = zmq_socket(facilities->zmq.ctx, ZMQ_REQ);
zmq_connect(management_socket, facilities->zmq.management_address);
uint8_t buffer[256];
@ -278,18 +283,28 @@ int facilities_config(void* facilities_s) {
oer_decode(NULL, &asn_DEF_ManagementReply, (void**) &mrep, buffer, 256);
long lat, lon, alt, alt_conf;
if (mrep->returnCode == ManagementReplyReturnCode_accepted && mrep->attributes &&
mrep->attributes->coordinates && mrep->attributes->altitude &&
mrep->attributes->heading && mrep->attributes->speed) {
facilities->epv.latitude = mrep->attributes->coordinates->latitude;
facilities->epv.longitude = mrep->attributes->coordinates->longitude;
facilities->epv.altitude = mrep->attributes->altitude->altitudeValue;
facilities->epv.altitude_conf = mrep->attributes->altitude->altitudeConfidence;
facilities->epv.heading = mrep->attributes->heading->headingValue;
facilities->epv.heading_conf = mrep->attributes->heading->headingConfidence;
facilities->epv.speed = mrep->attributes->speed->speedValue;
facilities->epv.speed_conf = mrep->attributes->speed->speedConfidence;
mrep->attributes->coordinates &&
mrep->attributes->altitude &&
mrep->attributes->heading &&
mrep->attributes->speed &&
mrep->attributes->clockType &&
mrep->attributes->clock &&
mrep->attributes->clockOffset &&
mrep->attributes->gpsType) {
facilities->epv.space.latitude = mrep->attributes->coordinates->latitude;
facilities->epv.space.longitude = mrep->attributes->coordinates->longitude;
facilities->epv.space.altitude = mrep->attributes->altitude->altitudeValue;
facilities->epv.space.altitude_conf = mrep->attributes->altitude->altitudeConfidence;
facilities->epv.space.heading = mrep->attributes->heading->headingValue;
facilities->epv.space.heading_conf = mrep->attributes->heading->headingConfidence;
facilities->epv.space.speed = mrep->attributes->speed->speedValue;
facilities->epv.space.speed_conf = mrep->attributes->speed->speedConfidence;
asn_INTEGER2ulong(&mrep->attributes->timestamp, &facilities->epv.timestamp);
facilities->epv.space.type = *mrep->attributes->gpsType;
facilities->epv.time.type = *mrep->attributes->clockType;
asn_INTEGER2ulong(mrep->attributes->clock, &facilities->epv.time.clock);
asn_INTEGER2ulong(mrep->attributes->clockOffset, &facilities->epv.time.offset);
} else {
syslog_err("[applications] rejected MR attribute request");
rv = 1;

134
src/cpm.c
View File

@ -18,6 +18,8 @@
#include <stdio.h>
#include <signal.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__)
@ -371,15 +373,7 @@ void mk_cpm(facilities_t* facilities,CPM_t* cpm_tx, struct timespec* systemtime)
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
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;
@ -469,38 +463,29 @@ dissemination_t* dissemination_init(){
}
int dissemination_check(dissemination_t* dissemination){
int dissemination_check(dissemination_t* dissemination, it2s_tender_epv_t* epv) {
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;
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)
if (now >= dissemination->next_cpm_max)
rv = 1;
pthread_mutex_unlock(&dissemination->lock);
return rv;
}
void dissemination_reset_timer(dissemination_t* dissemination){
void dissemination_reset_timer(dissemination_t* dissemination, it2s_tender_epv_t* epv){
/* 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;
uint64_t now = it2s_tender_get_clock(epv);
/* Both cases for RSU and OBU */
@ -509,16 +494,12 @@ void dissemination_reset_timer(dissemination_t* dissemination){
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];
@ -583,67 +564,66 @@ void *cp_service(void *fc){
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 (dissemination_check(facilities->dissemination) && facilities->dissemination->active){
if(is_radar_connected){
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;
}
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);
/* 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;
}
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 */
/* 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;
}
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 */
/* 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;
}
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);
/* 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 */
/*Reset Timer for dissemination control */
dissemination_reset_timer(facilities->dissemination);
dissemination_reset_timer(facilities->dissemination, &facilities->epv);
}else{ /* Waiting for Radar to reconnect */
is_radar_connected = waitingIncomingConnection();
}
}
} else { /* Waiting for Radar to reconnect */
is_radar_connected = waitingIncomingConnection();
}
}
}
ASN_STRUCT_FREE(asn_DEF_BTPDataRequest,bdr);

44
src/denm.c
View File

@ -4,6 +4,9 @@
#include <unistd.h>
#include <syslog.h>
#include <it2s-tender/time.h>
#include <it2s-tender/space.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__)
@ -76,7 +79,7 @@ static int permissions_check(int cause_code, uint8_t* permissions, uint8_t permi
}
static enum EVENT_CHECK_R event_check(den_t *den, DENM_t *denm, uint8_t* ssp, uint32_t ssp_len) {
static enum EVENT_CHECK_R event_check(den_t *den, DENM_t *denm, it2s_tender_epv_t* epv, uint8_t* ssp, uint32_t ssp_len) {
int rv = 0;
uint64_t e_detection_time, e_reference_time;
@ -94,12 +97,6 @@ static enum EVENT_CHECK_R event_check(den_t *den, DENM_t *denm, uint8_t* ssp, ui
}
}
struct timespec systemtime;
clock_gettime(CLOCK_REALTIME, &systemtime);
long now = (long) (systemtime.tv_sec * 1000 + systemtime.tv_nsec / 1E6);
now = now - 1072915200000; // Convert EPOCH to 2004/01/01 00:00:000
uint32_t e_validity_duration;
if (denm->denm.management.validityDuration != NULL) {
e_validity_duration = *(uint32_t *) denm->denm.management.validityDuration * 1000; // validityDuration comes in seconds
@ -107,6 +104,8 @@ static enum EVENT_CHECK_R event_check(den_t *den, DENM_t *denm, uint8_t* ssp, ui
e_validity_duration = den->default_event_duration * 1000;
}
uint64_t now = it2s_tender_get_clock(epv);
if (e_detection_time + e_validity_duration < now) {
return EVENT_PASSED;
}
@ -166,12 +165,9 @@ static enum EVENT_CHECK_R event_check(den_t *den, DENM_t *denm, uint8_t* ssp, ui
return EVENT_NEW;
}
static int event_add(den_t *den, DENM_t *denm, int64_t* id) {
static int event_add(den_t *den, DENM_t *denm, it2s_tender_epv_t* epv, int64_t* id) {
struct timespec systemtime;
clock_gettime(CLOCK_REALTIME, &systemtime);
long now = (long) (systemtime.tv_sec * 1000 + systemtime.tv_nsec / 1E6);
now = now - 1072915200000; // Convert EPOCH to 2004/01/01 00:00:000
uint64_t now = it2s_tender_get_clock(epv);
uint64_t e_detection_time, e_reference_time;
asn_INTEGER2ulong((INTEGER_t*) &denm->denm.management.detectionTime, &e_detection_time);
@ -240,12 +236,9 @@ static int event_add(den_t *den, DENM_t *denm, int64_t* id) {
else return 0; // Event added to db
}
static int event_update(den_t *den, DENM_t *denm, int64_t* id) {
static int event_update(den_t *den, DENM_t *denm, it2s_tender_epv_t* epv, int64_t* id) {
struct timespec systemtime;
clock_gettime(CLOCK_REALTIME, &systemtime);
long now = (long) (systemtime.tv_sec * 1000 + systemtime.tv_nsec / 1E6);
now = now - 1072915200000; // Convert EPOCH to 2004/01/01 00:00:000
uint64_t now = it2s_tender_get_clock(epv);
uint64_t e_detection_time, e_reference_time;
asn_INTEGER2ulong((INTEGER_t*) &denm->denm.management.detectionTime, &e_detection_time);
@ -326,12 +319,12 @@ static int event_update(den_t *den, DENM_t *denm, int64_t* id) {
else return 0; // Event updated
}
enum EVENT_CHECK_R event_manage(den_t *den, DENM_t *denm, int64_t* id, uint8_t* ssp, uint32_t ssp_len) {
enum EVENT_CHECK_R event_manage(den_t *den, DENM_t *denm, it2s_tender_epv_t* epv, int64_t* id, uint8_t* ssp, uint32_t ssp_len) {
int rv = 0;
switch (rv = event_check(den, denm, ssp, ssp_len)) {
switch (rv = event_check(den, denm, epv, ssp, ssp_len)) {
case EVENT_NEW:
syslog_debug("[facilities] [den] new event received");
if (event_add(den, denm, id)) {
if (event_add(den, denm, epv, id)) {
syslog_debug("[facilities] [den] failed adding event, max events reached");
ASN_STRUCT_FREE(asn_DEF_DENM, denm);
rv = -1;
@ -347,7 +340,7 @@ enum EVENT_CHECK_R event_manage(den_t *den, DENM_t *denm, int64_t* id, uint8_t*
break;
case EVENT_CANCELLATION:
syslog_debug("[facilities] [den] event cancellation received");
if (event_update(den, denm, id)) {
if (event_update(den, denm, epv, id)) {
syslog_debug("[facilities] [den] failed cancelling event, event not found");
ASN_STRUCT_FREE(asn_DEF_DENM, denm);
rv = EVENT_NUMBER_EXCEEDED;
@ -355,7 +348,7 @@ enum EVENT_CHECK_R event_manage(den_t *den, DENM_t *denm, int64_t* id, uint8_t*
break;
case EVENT_NEGATION:
syslog_debug("[facilities] [den] event negation received");
if (event_update(den, denm, id)) {
if (event_update(den, denm, epv, id)) {
syslog_debug("[facilities] [den] failed negating event, event not found");
ASN_STRUCT_FREE(asn_DEF_DENM, denm);
rv = EVENT_NUMBER_EXCEEDED;
@ -363,7 +356,7 @@ enum EVENT_CHECK_R event_manage(den_t *den, DENM_t *denm, int64_t* id, uint8_t*
break;
case EVENT_UPDATE:
syslog_debug("[facilities] [den] event update received");
if (event_update(den, denm, id)) {
if (event_update(den, denm, epv, id)) {
syslog_debug("[facilities] [den] failed updating event, event not found");
ASN_STRUCT_FREE(asn_DEF_DENM, denm);
rv = EVENT_NUMBER_EXCEEDED;
@ -388,7 +381,6 @@ enum EVENT_CHECK_R event_manage(den_t *den, DENM_t *denm, int64_t* id, uint8_t*
void* den_service(void *fc) {
facilities_t *facilities = (facilities_t *) fc;
struct timespec systemtime;
uint64_t now;
den_t *den = facilities->den;
@ -403,9 +395,7 @@ void* den_service(void *fc) {
uint32_t sleep_count = 0;
while (!facilities->exit) {
clock_gettime(CLOCK_REALTIME, &systemtime);
now = (long)(systemtime.tv_sec * 1000 + systemtime.tv_nsec / 1E6);
now = now - 1072915200000; // Convert EPOCH to 2004/01/01 00:00:000
now = it2s_tender_get_clock(&facilities->epv);
pthread_mutex_lock(&den->lock);
for (int i = 0; i < den->n_max_events; ++i) {

4
src/denm.h
View File

@ -6,6 +6,8 @@
#include <pthread.h>
#include <stdbool.h>
#include <it2s-tender/epv.h>
enum EVENT_STATE {
EVENT_ACTIVE,
EVENT_CANCELLED,
@ -66,7 +68,7 @@ typedef struct cc_ssp_bm {
* @param ssp permissions
* @return 0 if event OK, 1 if event NOK
*/
enum EVENT_CHECK_R event_manage(den_t* den, DENM_t* denm, int64_t* id, uint8_t* ssp, uint32_t ssp_len);
enum EVENT_CHECK_R event_manage(den_t* den, DENM_t* denm, it2s_tender_epv_t* epv, int64_t* id, uint8_t* ssp, uint32_t ssp_len);
void* den_service(void* fc);

42
src/facilities.c
View File

@ -31,6 +31,9 @@
#include <sys/stat.h>
#include <unistd.h>
#include <it2s-tender/space.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__)
@ -109,7 +112,7 @@ static int transport_indication(facilities_t *facilities, void* responder, uint8
// Manage message
switch (bdi->destinationPort) {
case Port_cam:
switch (check_cam(facilities, bdi, its_msg, ssp, ssp_len)) {
switch (check_cam(facilities, bdi, its_msg, &facilities->epv, ssp, ssp_len)) {
case CAM_OK:
fwd = true;
break;
@ -129,7 +132,7 @@ static int transport_indication(facilities_t *facilities, void* responder, uint8
free(xml_denm);
#endif
int64_t id = -1;
switch (event_manage(facilities->den, its_msg, &id, ssp, ssp_len)) {
switch (event_manage(facilities->den, its_msg, &facilities->epv, &id, ssp, ssp_len)) {
case EVENT_NEW:
case EVENT_CANCELLATION:
case EVENT_NEGATION:
@ -146,7 +149,7 @@ static int transport_indication(facilities_t *facilities, void* responder, uint8
break;
case Port_ivim:
switch (service_eval(facilities->infrastructure, SERVICE_IVI, its_msg, &id, ssp, ssp_len)) {
switch (service_eval(facilities->infrastructure, SERVICE_IVI, its_msg, &facilities->epv, &id, ssp, ssp_len)) {
case SERVICE_NEW:
case SERVICE_CANCELLATION:
case SERVICE_NEGATION:
@ -356,27 +359,20 @@ static int management_indication(facilities_t* facilities, void* responder, uint
zmq_send(responder, &code, 1, 0);
if (mi->present == ManagementIndication_PR_attributes) {
pthread_mutex_lock(&facilities->epv.lock);
if (mi->choice.attributes.coordinates) {
facilities->epv.latitude = mi->choice.attributes.coordinates->latitude;
facilities->epv.longitude = mi->choice.attributes.coordinates->longitude;
}
if (mi->choice.attributes.speed) {
facilities->epv.speed = mi->choice.attributes.speed->speedValue;
facilities->epv.speed_conf = mi->choice.attributes.speed->speedConfidence;
}
if (mi->choice.attributes.heading) {
facilities->epv.heading = mi->choice.attributes.heading->headingValue;
facilities->epv.heading_conf = mi->choice.attributes.heading->headingConfidence;
}
if (mi->choice.attributes.altitude) {
facilities->epv.altitude = mi->choice.attributes.altitude->altitudeValue;
facilities->epv.altitude_conf = mi->choice.attributes.altitude->altitudeConfidence;
}
it2s_tender_lock_space(&facilities->epv);
facilities->epv.space.latitude = mi->choice.attributes.coordinates.latitude;
facilities->epv.space.longitude = mi->choice.attributes.coordinates.longitude;
facilities->epv.space.speed = mi->choice.attributes.speed.speedValue;
facilities->epv.space.speed_conf = mi->choice.attributes.speed.speedConfidence;
facilities->epv.space.heading = mi->choice.attributes.heading.headingValue;
facilities->epv.space.heading_conf = mi->choice.attributes.heading.headingConfidence;
facilities->epv.space.altitude = mi->choice.attributes.altitude.altitudeValue;
facilities->epv.space.altitude_conf = mi->choice.attributes.altitude.altitudeConfidence;
it2s_tender_unlock_space(&facilities->epv);
asn_INTEGER2ulong(&mi->choice.attributes.timestamp, &facilities->epv.timestamp);
pthread_mutex_unlock(&facilities->epv.lock);
it2s_tender_lock_time(&facilities->epv);
asn_INTEGER2ulong(&mi->choice.attributes.clock, &facilities->epv.time.clock);
it2s_tender_unlock_time(&facilities->epv);
}
cleanup:

18
src/facilities.h
View File

@ -11,6 +11,8 @@
#include "queue.h"
#include "cpm.h"
#include <it2s-tender/epv.h>
enum ID_CHANGE_STAGE {
ID_CHANGE_INACTIVE,
ID_CHANGE_BLOCKED,
@ -18,20 +20,6 @@ enum ID_CHANGE_STAGE {
ID_CHANGE_COMMIT
};
typedef struct epv {
pthread_mutex_t lock;
uint64_t timestamp;
long latitude;
long longitude;
long altitude;
long altitude_conf;
long speed;
long speed_conf;
long heading;
long heading_conf;
} epv_t;
typedef struct facilities {
pthread_t ca_service;
pthread_t den_service;
@ -79,7 +67,7 @@ typedef struct facilities {
} change;
} id;
epv_t epv;
it2s_tender_epv_t epv;
bool exit;
} facilities_t;

40
src/infrastructure.c
View File

@ -4,6 +4,9 @@
#include <unistd.h>
#include <syslog.h>
#include <it2s-tender/space.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__)
@ -63,13 +66,10 @@ static int permissions_check(int diid, uint8_t* permissions, uint8_t permissions
}
static enum SERVICE_EVAL_R service_check(infrastructure_t* infrastructure, enum SERVICE_TYPE type, void* its_msg, uint8_t* ssp, uint16_t ssp_len) {
static enum SERVICE_EVAL_R service_check(infrastructure_t* infrastructure, enum SERVICE_TYPE type, void* its_msg, it2s_tender_epv_t* epv, uint8_t* ssp, uint16_t ssp_len) {
int rv = 0;
struct timespec systemtime;
clock_gettime(CLOCK_REALTIME, &systemtime);
long now = (long) (systemtime.tv_sec * 1000 + systemtime.tv_nsec / 1E6);
now = now - 1072915200000; // Convert EPOCH to 2004/01/01 00:00:000
uint64_t now = it2s_tender_get_clock(epv);
switch (type) {
case SERVICE_IVI:
@ -259,7 +259,7 @@ static enum SERVICE_EVAL_R service_check(infrastructure_t* infrastructure, enum
return SERVICE_NEW;
}
static int service_add(infrastructure_t* infrastructure, enum SERVICE_TYPE type, void* its_msg, int64_t* id) {
static int service_add(infrastructure_t* infrastructure, enum SERVICE_TYPE type, void* its_msg, it2s_tender_epv_t* epv, int64_t* id) {
switch (type) {
case SERVICE_IVI:
@ -270,10 +270,7 @@ static int service_add(infrastructure_t* infrastructure, enum SERVICE_TYPE type,
IVIM_t* ivim = (IVIM_t*) its_msg;
struct timespec systemtime;
clock_gettime(CLOCK_REALTIME, &systemtime);
long now = (long) (systemtime.tv_sec * 1000 + systemtime.tv_nsec / 1E6);
now = now - 1072915200000; // Convert EPOCH to 2004/01/01 00:00:000
uint64_t now = it2s_tender_get_clock(epv);
uint64_t timestamp, valid_to, valid_from;
if (!ivim->ivi.mandatory.timeStamp) {
@ -348,7 +345,7 @@ static int service_add(infrastructure_t* infrastructure, enum SERVICE_TYPE type,
else return 0; // Services added to db
}
static int service_update(infrastructure_t* infrastructure, enum SERVICE_TYPE type, void* its_msg, int64_t* id) {
static int service_update(infrastructure_t* infrastructure, enum SERVICE_TYPE type, void* its_msg, it2s_tender_epv_t* epv, int64_t* id) {
switch (type) {
case SERVICE_IVI:
@ -359,10 +356,7 @@ static int service_update(infrastructure_t* infrastructure, enum SERVICE_TYPE ty
IVIM_t* ivim = (IVIM_t*) its_msg;
struct timespec systemtime;
clock_gettime(CLOCK_REALTIME, &systemtime);
long now = (long) (systemtime.tv_sec * 1000 + systemtime.tv_nsec / 1E6);
now = now - 1072915200000; // Convert EPOCH to 2004/01/01 00:00:000
uint64_t now = it2s_tender_get_clock(epv);
uint64_t timestamp, valid_to, valid_from;
if (!ivim->ivi.mandatory.timeStamp) {
@ -458,12 +452,12 @@ static int service_update(infrastructure_t* infrastructure, enum SERVICE_TYPE ty
else return 0; // Event updated
}
enum SERVICE_EVAL_R service_eval(infrastructure_t* infrastructure, enum SERVICE_TYPE type, void* its_msg, int64_t* id, uint8_t* ssp, uint16_t ssp_len) {
enum SERVICE_EVAL_R service_eval(infrastructure_t* infrastructure, enum SERVICE_TYPE type, void* its_msg, it2s_tender_epv_t* epv, int64_t* id, uint8_t* ssp, uint16_t ssp_len) {
int rv = 0;
switch (rv = service_check(infrastructure, type, its_msg, ssp, ssp_len)) {
switch (rv = service_check(infrastructure, type, its_msg, epv, ssp, ssp_len)) {
case SERVICE_NEW:
syslog_debug("[facilities] [infrastructure] new service received");
if (service_add(infrastructure, type, its_msg, id)) {
if (service_add(infrastructure, type, its_msg, epv, id)) {
syslog_debug("[facilities] [infrastructure] failed adding service, max services reached");
ASN_STRUCT_FREE(asn_DEF_IVIM, its_msg);
rv = -1;
@ -479,7 +473,7 @@ enum SERVICE_EVAL_R service_eval(infrastructure_t* infrastructure, enum SERVICE_
break;
case SERVICE_CANCELLATION:
syslog_debug("[facilities] [infrastructure] service cancellation received");
if (service_update(infrastructure, type, its_msg, id)) {
if (service_update(infrastructure, type, its_msg, epv, id)) {
syslog_debug("[facilities] [infrastructure] failed cancelling service, event not found");
ASN_STRUCT_FREE(asn_DEF_IVIM, its_msg);
rv = -1;
@ -487,7 +481,7 @@ enum SERVICE_EVAL_R service_eval(infrastructure_t* infrastructure, enum SERVICE_
break;
case SERVICE_NEGATION:
syslog_debug("[facilities] [infrastructure] service negation received");
if (service_update(infrastructure, type, its_msg, id)) {
if (service_update(infrastructure, type, its_msg, epv, id)) {
syslog_debug("[facilities] [infrastructure] failed negating service, service not found");
ASN_STRUCT_FREE(asn_DEF_IVIM, its_msg);
rv = -1;
@ -495,7 +489,7 @@ enum SERVICE_EVAL_R service_eval(infrastructure_t* infrastructure, enum SERVICE_
break;
case SERVICE_UPDATE:
syslog_debug("[facilities] [infrastructure] service update received");
if (service_update(infrastructure, type, its_msg, id)) {
if (service_update(infrastructure, type, its_msg, epv, id)) {
syslog_debug("[facilities] [infrastructure] failed updating service, service not found");
ASN_STRUCT_FREE(asn_DEF_IVIM, its_msg);
rv = -1;
@ -535,9 +529,7 @@ void* infrastructure_service(void *fc) {
uint32_t sleep_count = 0;
while (!facilities->exit) {
clock_gettime(CLOCK_REALTIME, &systemtime);
now = (long)(systemtime.tv_sec * 1000 + systemtime.tv_nsec / 1E6);
now = now - 1072915200000; // Convert EPOCH to 2004/01/01 00:00:000
now = it2s_tender_get_clock(&facilities->epv);
int n_awaiting_services = 0;
pthread_mutex_lock(&infrastructure->lock);

3
src/infrastructure.h
View File

@ -5,6 +5,7 @@
#include <ivim/IVIM.h>
#include <pthread.h>
#include <stdbool.h>
#include <it2s-tender/epv.h>
enum IVI_DIID_TYPE {
IVI_DIID_ViennaCodeConvention,
@ -85,7 +86,7 @@ enum SERVICE_EVAL_R {
SERVICE_BAD_PERMISSIONS
};
enum SERVICE_EVAL_R service_eval(infrastructure_t* infrastructure, enum SERVICE_TYPE type, void* its_msg, int64_t* id, uint8_t* ssp, uint16_t ssp_len);
enum SERVICE_EVAL_R service_eval(infrastructure_t* infrastructure, enum SERVICE_TYPE type, void* its_msg, it2s_tender_epv_t* epv, int64_t* id, uint8_t* ssp, uint16_t ssp_len);
void* infrastructure_service(void* fc);

12
src/requests.c
View File

@ -12,6 +12,8 @@
#include <syslog.h>
#include <cpm/CPM.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__)
@ -118,7 +120,7 @@ int facilities_request_single_message(facilities_t* facilities, void* responder,
if (fdreq->choice.singleMessage.itsMessageType == ItsMessageType_denm) {
managed_msg = true;
uint8_t event_type = event_manage(facilities->den, its_msg, &id, NULL, 0);
uint8_t event_type = event_manage(facilities->den, its_msg, &facilities->epv, &id, NULL, 0);
// Do not free its_msg! event_manage takes care of the msg
// id will get set to another val if EVENT NEW or UPDATE or CANCELLATION or NEGATION
if (event_type != EVENT_NEW &&
@ -206,7 +208,7 @@ int facilities_request_single_message(facilities_t* facilities, void* responder,
} else if (fdreq->choice.singleMessage.itsMessageType == ItsMessageType_ivim) {
managed_msg = true;
uint8_t service_type = service_eval(facilities->infrastructure, SERVICE_IVI, its_msg, &id, NULL, 0);
uint8_t service_type = service_eval(facilities->infrastructure, SERVICE_IVI, its_msg, &facilities->epv, &id, NULL, 0);
if (service_type != SERVICE_NEW &&
service_type != SERVICE_UPDATE &&
@ -227,11 +229,7 @@ int facilities_request_single_message(facilities_t* facilities, void* responder,
uint64_t valid_to, valid_from;
if (!((IVIM_t*) its_msg)->ivi.mandatory.validFrom) {
struct timespec systemtime;
clock_gettime(CLOCK_REALTIME, &systemtime);
long now = (long) (systemtime.tv_sec * 1000 + systemtime.tv_nsec / 1E6);
now = now - 1072915200000; // Convert EPOCH to 2004/01/01 00:00:000
valid_from = now;
valid_from = it2s_tender_get_clock(&facilities->epv);;
} else {
asn_INTEGER2ulong((INTEGER_t*) ((IVIM_t*) its_msg)->ivi.mandatory.validFrom, &valid_from);
}