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

Set event trace in OBU (App) generated DENMs

This commit is contained in:
emanuel 2021-03-12 14:27:41 +00:00
parent 80e532aefd
commit cf8b0dc5fa
1 changed files with 99 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

83
src/facilities.c
View File

@ -3,6 +3,7 @@
#include "config.h"
#include "denm.h"
#include <camv2/xer_encoder.h>
#include <itss-transport/BTPDataRequest.h>
#include <itss-transport/BTPDataIndication.h>
#include <itss-facilities/FacilitiesDataIndication.h>
@ -95,12 +96,12 @@ static int transport_indication(facilities_t *facilities, void* responder, uint8
break;
case Port_denm:
;
#ifdef DEBUG
#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
#endif
int64_t id = -1;
event_manage(facilities->den, its_msg, &id, ssp);
break;
@ -240,6 +241,55 @@ static int facilities_request(facilities_t *facilities, void* responder, uint8_t
((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 ) {
@ -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);
@ -461,7 +515,7 @@ static int facilities_request(facilities_t *facilities, void* responder, uint8_t
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);