diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 879df9a..9ae11f6 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -21,6 +21,7 @@ TARGET_LINK_LIBRARIES(it2s-itss-facilities -lit2s-asn-ivim -lit2s-asn-denmv2 -lit2s-asn-cpm + -lit2s-tender -lm ) diff --git a/src/cam.c b/src/cam.c index 07f68d7..8815971 100644 --- a/src/cam.c +++ b/src/cam.c @@ -13,6 +13,9 @@ #include #include +#include +#include + #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); } } diff --git a/src/cam.h b/src/cam.h index bbfb856..f26e889 100644 --- a/src/cam.h +++ b/src/cam.h @@ -9,6 +9,8 @@ #include #include +#include + #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 diff --git a/src/config.c b/src/config.c index 8399d7e..22a76f7 100644 --- a/src/config.c +++ b/src/config.c @@ -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; diff --git a/src/cpm.c b/src/cpm.c index 964026a..7739b89 100644 --- a/src/cpm.c +++ b/src/cpm.c @@ -18,6 +18,8 @@ #include #include +#include + #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); diff --git a/src/denm.c b/src/denm.c index 2800a8e..0794e02 100644 --- a/src/denm.c +++ b/src/denm.c @@ -4,6 +4,9 @@ #include #include +#include +#include + #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) { diff --git a/src/denm.h b/src/denm.h index f098487..fb5a59c 100644 --- a/src/denm.h +++ b/src/denm.h @@ -6,6 +6,8 @@ #include #include +#include + 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); diff --git a/src/facilities.c b/src/facilities.c index 32d347a..e4fdc55 100644 --- a/src/facilities.c +++ b/src/facilities.c @@ -31,6 +31,9 @@ #include #include +#include +#include + #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: diff --git a/src/facilities.h b/src/facilities.h index cc6fdd3..562c684 100644 --- a/src/facilities.h +++ b/src/facilities.h @@ -11,6 +11,8 @@ #include "queue.h" #include "cpm.h" +#include + 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; diff --git a/src/infrastructure.c b/src/infrastructure.c index 4838f3e..6c544d9 100644 --- a/src/infrastructure.c +++ b/src/infrastructure.c @@ -4,6 +4,9 @@ #include #include +#include +#include + #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); diff --git a/src/infrastructure.h b/src/infrastructure.h index 200ca22..877f950 100644 --- a/src/infrastructure.h +++ b/src/infrastructure.h @@ -5,6 +5,7 @@ #include #include #include +#include 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); diff --git a/src/requests.c b/src/requests.c index fd5d73e..b240c38 100644 --- a/src/requests.c +++ b/src/requests.c @@ -12,6 +12,8 @@ #include #include +#include + #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); }