diff --git a/src/facilities.c b/src/facilities.c index 2152630..b079bd0 100644 --- a/src/facilities.c +++ b/src/facilities.c @@ -3,6 +3,7 @@ #include "config.h" #include "denm.h" +#include #include #include #include @@ -95,12 +96,12 @@ static int transport_indication(facilities_t *facilities, void* responder, uint8 break; case Port_denm: ; - #ifdef DEBUG - uint8_t* xml_denm = malloc(32768); - asn_enc_rval_t rve = xer_encode_to_buffer(xml_denm, 32768, 0x02, &asn_DEF_DENM, its_msg); - syslog_debug("DENM XER %d: %.*s", (int)rve.encoded, (int)rve.encoded , xml_denm); - free(xml_denm); - #endif +#ifdef DEBUG + uint8_t* xml_denm = malloc(32768); + asn_enc_rval_t rve = xer_encode_to_buffer(xml_denm, 32768, 0x02, &asn_DEF_DENM, its_msg); + syslog_debug("DENM XER %d: %.*s", (int)rve.encoded, (int)rve.encoded , xml_denm); + free(xml_denm); +#endif int64_t id = -1; event_manage(facilities->den, its_msg, &id, ssp); break; @@ -167,7 +168,7 @@ static int facilities_request(facilities_t *facilities, void* responder, uint8_t case FacilitiesDataRequest_PR_singleMessage: ; bool fwd = true; - + uint64_t transmission_duration = 0; uint32_t transmission_interval = 0; uint64_t transmission_start = 0; @@ -220,17 +221,17 @@ static int facilities_request(facilities_t *facilities, void* responder, uint8_t // 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 && - event_type != EVENT_UPDATE && - event_type != EVENT_CANCELLATION && - event_type != EVENT_NEGATION) { - + event_type != EVENT_UPDATE && + event_type != EVENT_CANCELLATION && + event_type != EVENT_NEGATION) { + fwd = false; } if (event_type == EVENT_UPDATE || - event_type == EVENT_CANCELLATION || - event_type == EVENT_NEGATION) { - + event_type == EVENT_CANCELLATION || + event_type == EVENT_NEGATION) { + is_update = true; } @@ -239,12 +240,61 @@ static int facilities_request(facilities_t *facilities, void* responder, uint8_t pthread_mutex_lock(&facilities->lock); ((DENM_t*)its_msg)->header.stationID = facilities->station_id; pthread_mutex_unlock(&facilities->lock); - + + // Set only one trace + if (facilities->station_type != 15) { + pthread_mutex_lock(&facilities->lightship->lock); + if (facilities->lightship->pos_history_len > 0) { + + if (!((DENM_t*)its_msg)->denm.location) { + ((DENM_t*)its_msg)->denm.location = calloc(1, sizeof(LocationContainer_t)); + } + ((DENM_t*)its_msg)->denm.location->traces.list.count = 1; + ((DENM_t*)its_msg)->denm.location->traces.list.size = 1 * sizeof(void*); + ((DENM_t*)its_msg)->denm.location->traces.list.array = malloc(1 * sizeof(void*)); + ((DENM_t*)its_msg)->denm.location->traces.list.array[0] = calloc(1, sizeof(PathHistory_t)); + PathHistory_t* ph = ((DENM_t*)its_msg)->denm.location->traces.list.array[0]; + + pos_vector_t** pos_history = facilities->lightship->pos_history; + uint16_t pos_history_len = facilities->lightship->pos_history_len; + + ph->list.array = malloc((pos_history_len-1) * sizeof(void*)); + ph->list.count = pos_history_len-1; + ph->list.size = (pos_history_len-1) * sizeof(void*); + + for (int i = 0; i < pos_history_len-1; ++i) { + ph->list.array[i] = calloc(1,sizeof(PathPoint_t)); + + if (pos_history[i+1]->alt != AltitudeValue_unavailable && pos_history[i]->alt != AltitudeValue_unavailable) { + ph->list.array[i]->pathPosition.deltaAltitude = pos_history[i+1]->alt - pos_history[i]->alt; + } else { + ph->list.array[i]->pathPosition.deltaAltitude = DeltaAltitude_unavailable; + } + + if (pos_history[i+1]->lat != Latitude_unavailable && pos_history[i]->lat != Latitude_unavailable) { + ph->list.array[i]->pathPosition.deltaLatitude = pos_history[i+1]->lat - pos_history[i]->lat; + } else { + ph->list.array[i]->pathPosition.deltaLatitude = DeltaLatitude_unavailable; + } + + if (pos_history[i+1]->lon != Longitude_unavailable && pos_history[i]->lon != Longitude_unavailable) { + ph->list.array[i]->pathPosition.deltaLongitude = pos_history[i+1]->lon - pos_history[i]->lon; + } else { + ph->list.array[i]->pathPosition.deltaLongitude = DeltaLongitude_unavailable; + } + + ph->list.array[i]->pathDeltaTime = calloc(1,sizeof(PathDeltaTime_t)); + *ph->list.array[i]->pathDeltaTime = (pos_history[i]->ts - pos_history[i+1]->ts)/10; + } + } + pthread_mutex_unlock(&facilities->lightship->lock); + } + // get, set retransmission, duration if ( ((DENM_t*)its_msg)->denm.management.transmissionInterval ) { transmission_interval = *( (uint32_t*) ((DENM_t*)its_msg)->denm.management.transmissionInterval ); - + if ( ((DENM_t*)its_msg)->denm.management.validityDuration ) { transmission_duration = *( (uint32_t*) ((DENM_t*)its_msg)->denm.management.validityDuration ) * 1000; } else { @@ -258,17 +308,17 @@ static int facilities_request(facilities_t *facilities, void* responder, uint8_t uint8_t service_type = service_eval(facilities->infrastructure, SERVICE_IVI, its_msg, &id, NULL); if (service_type != SERVICE_NEW && - service_type != SERVICE_UPDATE && - service_type != SERVICE_CANCELLATION && - service_type != SERVICE_NEGATION) { - + service_type != SERVICE_UPDATE && + service_type != SERVICE_CANCELLATION && + service_type != SERVICE_NEGATION) { + fwd = false; } if (service_type == SERVICE_UPDATE || - service_type == SERVICE_CANCELLATION || - service_type == SERVICE_NEGATION) { - + service_type == SERVICE_CANCELLATION || + service_type == SERVICE_NEGATION) { + is_update = true; } @@ -323,9 +373,14 @@ static int facilities_request(facilities_t *facilities, void* responder, uint8_t bdr->btpType = BTPType_btpB; - bdr->data.buf = malloc(fdreq->choice.singleMessage.data.size); - memcpy(bdr->data.buf, fdreq->choice.singleMessage.data.buf, fdreq->choice.singleMessage.data.size); - bdr->data.size = fdreq->choice.singleMessage.data.size; + bdr->data.buf = malloc(2048); + enc = uper_encode_to_buffer(its_msg_def, NULL, its_msg, bdr->data.buf, 2048); + if (enc.encoded == -1) { + syslog_err("[facilities] failed encoding ITS message into UPER (%s)", enc.failed_type->name); + rv = 1; + goto cleanup; + } + bdr->data.size = (enc.encoded + 7) / 8; bdr->gnDestinationAddress.buf = malloc(6); for (int i = 0; i < 6; ++i) bdr->gnDestinationAddress.buf[i] = 0xff; @@ -356,7 +411,6 @@ static int facilities_request(facilities_t *facilities, void* responder, uint8_t *bdr->gnSecurityProfile = 1; } - // Encode ITS message into OER uint8_t bdr_oer[2048]; bdr_oer[0] = 4; // [facilities] service id enc = oer_encode_to_buffer(&asn_DEF_BTPDataRequest, NULL, bdr, bdr_oer + 1, 2047); @@ -460,8 +514,8 @@ static int facilities_request(facilities_t *facilities, void* responder, uint8_t rv = 1; goto cleanup; } - - cleanup: + +cleanup: if (rv) { fdres->code = FacilitiesResultCode_rejected; uint8_t fdres_oer[32]; @@ -498,14 +552,29 @@ static int security_indication(facilities_t *facilities, void* responder_secured switch (si->choice.idChangeEvent.command) { case SecurityIdChangeEventType_prepare: pthread_mutex_lock(&facilities->lock); + pthread_mutex_lock(&facilities->lightship->lock); break; case SecurityIdChangeEventType_commit: ; - + + // Change Station ID facilities->station_id = rand(); pthread_mutex_unlock(&facilities->lock); + // Reset lightship + for (int i = 0; i < facilities->lightship->pos_history_len; ++i) { + free(facilities->lightship->pos_history[i]); + } + facilities->lightship->pos_history_len = 0; + + facilities->lightship->last_cam = 0; + facilities->lightship->last_cam_lfc = 0; + facilities->lightship->next_cam_max = 0; + facilities->lightship->next_cam_min = 0; + + pthread_mutex_unlock(&facilities->lightship->lock); + break; case SecurityIdChangeEventType_abort: pthread_mutex_unlock(&facilities->lock);