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

Modded SAEMs

This commit is contained in:
emanuel 2021-06-14 17:41:16 +01:00
parent 64962a0646
commit aace2a33db
6 changed files with 89 additions and 135 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

120
src/cam.c
View File

@ -4,6 +4,7 @@
#include <camv2/INTEGER.h>
#include <itss-transport/BTPDataRequest.h>
#include <itss-transport/BTPDataIndication.h>
#include <itss-management/ManagementRequest.h>
#include <camv2/CAM.h>
#include <stdint.h>
@ -240,30 +241,30 @@ static int mk_cam(facilities_t* facilities, uint8_t *cam_oer, uint32_t *cam_len)
cam->cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_rsuContainerHighFrequency;
if (facilities->lightship->pz_len > 0) {
if (facilities->lightship->protected_zones.pz_len > 0) {
cam->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU = calloc(1, sizeof(ProtectedCommunicationZonesRSU_t));
ProtectedCommunicationZonesRSU_t *pzs = cam->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU;
pzs->list.count = facilities->lightship->pz_len;
pzs->list.size = facilities->lightship->pz_len * sizeof(void*);
pzs->list.array = malloc(facilities->lightship->pz_len * sizeof(void*));
for (int i = 0; i < facilities->lightship->pz_len; ++i) {
pzs->list.count = facilities->lightship->protected_zones.pz_len;
pzs->list.size = facilities->lightship->protected_zones.pz_len * sizeof(void*);
pzs->list.array = malloc(facilities->lightship->protected_zones.pz_len * sizeof(void*));
for (int i = 0; i < facilities->lightship->protected_zones.pz_len; ++i) {
pzs->list.array[i] = calloc(1, sizeof(ProtectedCommunicationZone_t));
pzs->list.array[i]->protectedZoneLatitude = facilities->lightship->pz[i]->protectedZoneLatitude;
pzs->list.array[i]->protectedZoneLongitude = facilities->lightship->pz[i]->protectedZoneLongitude;
pzs->list.array[i]->protectedZoneType = facilities->lightship->pz[i]->protectedZoneType;
pzs->list.array[i]->protectedZoneLatitude = facilities->lightship->protected_zones.pz[i]->protectedZoneLatitude;
pzs->list.array[i]->protectedZoneLongitude = facilities->lightship->protected_zones.pz[i]->protectedZoneLongitude;
pzs->list.array[i]->protectedZoneType = facilities->lightship->protected_zones.pz[i]->protectedZoneType;
if (facilities->lightship->pz[i]->expiryTime) {
pzs->list.array[i]->expiryTime->size = facilities->lightship->pz[i]->expiryTime->size;
pzs->list.array[i]->expiryTime->buf = malloc(facilities->lightship->pz[i]->expiryTime->size);
memcpy(pzs->list.array[i]->expiryTime->buf, facilities->lightship->pz[i]->expiryTime->buf, facilities->lightship->pz[i]->expiryTime->size);
if (facilities->lightship->protected_zones.pz[i]->expiryTime) {
pzs->list.array[i]->expiryTime->size = facilities->lightship->protected_zones.pz[i]->expiryTime->size;
pzs->list.array[i]->expiryTime->buf = malloc(facilities->lightship->protected_zones.pz[i]->expiryTime->size);
memcpy(pzs->list.array[i]->expiryTime->buf, facilities->lightship->protected_zones.pz[i]->expiryTime->buf, facilities->lightship->protected_zones.pz[i]->expiryTime->size);
}
if (facilities->lightship->pz[i]->protectedZoneID) {
if (facilities->lightship->protected_zones.pz[i]->protectedZoneID) {
pzs->list.array[i]->protectedZoneID = malloc(8);
*pzs->list.array[i]->protectedZoneID = *facilities->lightship->pz[i]->protectedZoneID;
*pzs->list.array[i]->protectedZoneID = *facilities->lightship->protected_zones.pz[i]->protectedZoneID;
}
if (facilities->lightship->pz[i]->protectedZoneRadius) {
if (facilities->lightship->protected_zones.pz[i]->protectedZoneRadius) {
pzs->list.array[i]->protectedZoneRadius = malloc(8);
*pzs->list.array[i]->protectedZoneRadius = *facilities->lightship->pz[i]->protectedZoneRadius;
*pzs->list.array[i]->protectedZoneRadius = *facilities->lightship->protected_zones.pz[i]->protectedZoneRadius;
}
}
}
@ -290,7 +291,7 @@ cleanup:
lightship_t* lightship_init() {
lightship_t* lightship = (lightship_t*) calloc(1, sizeof(lightship_t));
lightship->pz = calloc(256 , sizeof(void*));
lightship->protected_zones.pz = calloc(256 , sizeof(void*));
pthread_mutex_init(&lightship->lock, NULL);
lightship->pos_history = malloc(POS_HISTORY_MAX_LEN * sizeof(void*));
@ -340,16 +341,16 @@ int lightship_check(lightship_t* lightship, it2s_tender_epv_t* epv) {
// Remove expired PZs
for (int i = 0; i < lightship->pz_len; ++i) {
for (int i = 0; i < lightship->protected_zones.pz_len; ++i) {
uint64_t expiry;
if (lightship->pz[i]->expiryTime) {
asn_INTEGER2ulong(lightship->pz[i]->expiryTime, &expiry);
if (lightship->protected_zones.pz[i]->expiryTime) {
asn_INTEGER2ulong(lightship->protected_zones.pz[i]->expiryTime, &expiry);
if (now >= expiry) {
ASN_STRUCT_FREE(asn_DEF_ProtectedCommunicationZone, lightship->pz[i]);
for (int j = i; j < lightship->pz_len - 1; ++j) {
lightship->pz[j] = lightship->pz[j+1];
ASN_STRUCT_FREE(asn_DEF_ProtectedCommunicationZone, lightship->protected_zones.pz[i]);
for (int j = i; j < lightship->protected_zones.pz_len - 1; ++j) {
lightship->protected_zones.pz[j] = lightship->protected_zones.pz[j+1];
}
--lightship->pz_len;
--lightship->protected_zones.pz_len;
}
}
}
@ -381,6 +382,7 @@ 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, it2s_tender_epv_t* epv, uint8_t* ssp, uint32_t ssp_len) {
int rv = 0;
facilities_t* facilities = (facilities_t*) fc;
lightship_t *lightship = ((facilities_t*) fc)->lightship;
uint64_t now = it2s_tender_get_clock(epv);
@ -518,39 +520,69 @@ enum CAM_CHECK_R check_cam(void* fc, BTPDataIndication_t *bdi, CAM_t* cam, it2s_
if (cam->cam.camParameters.basicContainer.stationType == StationType_roadSideUnit &&
cam->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU) {
ProtectedCommunicationZonesRSU_t *pzs = cam->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU;
if (pzs->list.count > 0 && pzs->list.count + lightship->pz_len < 255) {
if (pzs->list.count > 0 && pzs->list.count + lightship->protected_zones.pz_len < 255) {
bool new_pz = false;
for (int k = 0; k < pzs->list.count; ++k) {
bool found = false;
for (int j = 0; j < lightship->pz_len; ++j) {
if (lightship->pz[j]->protectedZoneLatitude == pzs->list.array[k]->protectedZoneLatitude &&
lightship->pz[j]->protectedZoneLongitude == pzs->list.array[k]->protectedZoneLongitude) {
for (int j = 0; j < lightship->protected_zones.pz_len; ++j) {
if (lightship->protected_zones.pz[j]->protectedZoneLatitude == pzs->list.array[k]->protectedZoneLatitude &&
lightship->protected_zones.pz[j]->protectedZoneLongitude == pzs->list.array[k]->protectedZoneLongitude) {
found = true;
break;
}
}
if (found) continue;
lightship->pz[lightship->pz_len] = calloc(1, sizeof(ProtectedCommunicationZone_t));
lightship->pz[lightship->pz_len]->protectedZoneLatitude = pzs->list.array[k]->protectedZoneLatitude;
lightship->pz[lightship->pz_len]->protectedZoneLongitude = pzs->list.array[k]->protectedZoneLongitude;
lightship->pz[lightship->pz_len]->protectedZoneType = pzs->list.array[k]->protectedZoneType;
new_pz = true;
lightship->protected_zones.pz[lightship->protected_zones.pz_len] = calloc(1, sizeof(ProtectedCommunicationZone_t));
lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneLatitude = pzs->list.array[k]->protectedZoneLatitude;
lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneLongitude = pzs->list.array[k]->protectedZoneLongitude;
lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneType = pzs->list.array[k]->protectedZoneType;
if (pzs->list.array[k]->expiryTime) {
lightship->pz[lightship->pz_len]->expiryTime->size = pzs->list.array[k]->expiryTime->size;
lightship->pz[lightship->pz_len]->expiryTime->buf = malloc(pzs->list.array[k]->expiryTime->size);
memcpy(lightship->pz[lightship->pz_len]->expiryTime->buf, pzs->list.array[k]->expiryTime->buf, pzs->list.array[k]->expiryTime->size);
lightship->protected_zones.pz[lightship->protected_zones.pz_len]->expiryTime->size = pzs->list.array[k]->expiryTime->size;
lightship->protected_zones.pz[lightship->protected_zones.pz_len]->expiryTime->buf = malloc(pzs->list.array[k]->expiryTime->size);
memcpy(lightship->protected_zones.pz[lightship->protected_zones.pz_len]->expiryTime->buf, pzs->list.array[k]->expiryTime->buf, pzs->list.array[k]->expiryTime->size);
}
if (pzs->list.array[k]->protectedZoneID) {
lightship->pz[lightship->pz_len]->protectedZoneID = malloc(8);
*lightship->pz[lightship->pz_len]->protectedZoneID = *pzs->list.array[k]->protectedZoneID;
lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneID = malloc(8);
*lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneID = *pzs->list.array[k]->protectedZoneID;
}
if (pzs->list.array[k]->protectedZoneRadius) {
lightship->pz[lightship->pz_len]->protectedZoneRadius = malloc(8);
*lightship->pz[lightship->pz_len]->protectedZoneRadius = *pzs->list.array[k]->protectedZoneRadius;
lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneRadius = malloc(8);
*lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneRadius = *pzs->list.array[k]->protectedZoneRadius;
}
++lightship->pz_len;
++lightship->protected_zones.pz_len;
}
// Inform [management]
if (new_pz) {
uint8_t b_oer[512];
ManagementRequest_t* mreq = calloc(1, sizeof(ManagementRequest_t));
mreq->present = ManagementRequest_PR_attributes;
mreq->choice.attributes.present = ManagementRequestAttributes_PR_set;
mreq->choice.attributes.choice.set.protectedZones = calloc(1, sizeof(struct protectedZones));
mreq->choice.attributes.choice.set.protectedZones->list.count = lightship->protected_zones.pz_len;
mreq->choice.attributes.choice.set.protectedZones->list.size = lightship->protected_zones.pz_len * sizeof(void*);
mreq->choice.attributes.choice.set.protectedZones->list.array = calloc(lightship->protected_zones.pz_len, sizeof(void*));
for (int p = 0; p < lightship->protected_zones.pz_len; ++p) {
asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_ProtectedCommunicationZone, NULL, lightship->protected_zones.pz[p], b_oer, 512);
oer_decode(NULL, &asn_DEF_ProtectedCommunicationZone, (void**) &mreq->choice.attributes.choice.set.protectedZones->list.array[p], b_oer, enc.encoded);
}
asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_ManagementRequest, NULL, mreq, b_oer, 512);
void* management_socket = zmq_socket(facilities->zmq.ctx, ZMQ_REQ);
int wait_ms = 1000;
zmq_setsockopt(management_socket, ZMQ_RCVTIMEO, &wait_ms, sizeof(int));
zmq_connect(management_socket, facilities->zmq.management_address);
zmq_send(management_socket, b_oer, enc.encoded, 0);
syslog_debug("[facilities] [ca]-> sending MReq.attributes.set.protectedZones to ->[management]");
uint8_t code;
zmq_recv(management_socket, &code, 1, 0);
zmq_close(management_socket);
}
}
}
@ -571,12 +603,12 @@ static int check_pz(lightship_t *lightship, it2s_tender_epv_t* epv) {
pthread_mutex_lock(&lightship->lock);
for (int i = 0; i < lightship->pz_len; ++i) {
double d = haversine(lat, lon, (double) lightship->pz[i]->protectedZoneLatitude/10000000.0, (double) lightship->pz[i]->protectedZoneLongitude/10000000.0);
for (int i = 0; i < lightship->protected_zones.pz_len; ++i) {
double d = haversine(lat, lon, (double) lightship->protected_zones.pz[i]->protectedZoneLatitude/10000000.0, (double) lightship->protected_zones.pz[i]->protectedZoneLongitude/10000000.0);
int pz_radius = 50;
if (lightship->pz[i]->protectedZoneRadius) {
pz_radius = *lightship->pz[i]->protectedZoneRadius;
if (lightship->protected_zones.pz[i]->protectedZoneRadius) {
pz_radius = *lightship->protected_zones.pz[i]->protectedZoneRadius;
}
if (d < pz_radius) {
is_inside = true;

4
src/cam.h
View File

@ -69,7 +69,6 @@ typedef struct lightship {
pos_vector_t** pos_history;
uint16_t pos_history_len;
bool is_vehicle_near;
uint64_t last_vehicle;
@ -78,8 +77,11 @@ typedef struct lightship {
uint32_t rsu_gen_min;
uint32_t rsu_vehicle_permanence;
struct {
ProtectedCommunicationZone_t ** pz;
uint16_t pz_len;
} protected_zones;
} lightship_t;
lightship_t* lightship_init();

53
src/config.c
View File

@ -292,8 +292,8 @@ int facilities_config(void* facilities_s) {
asn_dec_rval_t dec = xer_decode(NULL, &asn_DEF_ProtectedCommunicationZone, (void**) &zone, pz_xml, size);
if (!dec.code) {
facilities->lightship->pz[i] = zone;
++facilities->lightship->pz_len;
facilities->lightship->protected_zones.pz[i] = zone;
++facilities->lightship->protected_zones.pz_len;
++i;
syslog_debug("[facilities] [config] loaded protection zone @ (%ld, %ld)", zone->protectedZoneLatitude, zone->protectedZoneLongitude);
} else {
@ -306,55 +306,6 @@ int facilities_config(void* facilities_s) {
}
}
// Announcements
if (facilities->station_type == 15) {
int i = 0;
//DIR *d = opendir(config->facilities.announcements.path);
DIR *d = opendir("/etc/it2s/announcements");
struct dirent *dir;
char file[256];
char si_xml[2048];
if (d) {
while ((dir = readdir(d)) != NULL && i < 16) {
if (dir->d_name[0] == '.') continue;
//sprintf(file, "%s/%s", config->facilities.announcements.path, dir->d_name);
sprintf(file, "%s/%s", "/etc/it2s/announcements", dir->d_name);
FILE *fp = fopen(file, "r");
if (!fp) continue;
fseek(fp, 0, SEEK_END);
uint16_t size = ftell(fp);
fseek(fp, 0, SEEK_SET);
if (!size) {
fclose(fp);
continue;
}
if (!fread(si_xml, 1, size, fp)) {
fclose(fp);
continue;
}
fclose(fp);
ServiceInfo_t *si = calloc(1, sizeof(ServiceInfo_t));
asn_dec_rval_t dec = xer_decode(NULL, &asn_DEF_ServiceInfo, (void**) &si, si_xml, size);
if (!dec.code) {
facilities->press.providing[i] = si;
++facilities->press.providing_len;
++i;
syslog_debug("[facilities] [config] loaded service announcement '%s'", dir->d_name);
} else {
syslog_err("[facilities] [config] failure to decode service announcement '%s'", dir->d_name);
ASN_STRUCT_FREE(asn_DEF_ServiceInfo, si);
}
}
closedir(d);
}
}
pthread_mutex_init(&facilities->epv.space.lock, NULL);
pthread_mutex_init(&facilities->epv.time.lock, NULL);
facilities->epv.time.resolution = TIME_MILLISECONDS;

4
src/facilities.c
View File

@ -514,9 +514,7 @@ int main() {
pthread_create(&facilities.cp_service, NULL, cp_service, (void*) &facilities);
// SA
if (facilities.press.providing_len > 0) {
pthread_create(&facilities.sa_service, NULL, sa_service, (void*) &facilities);
}
//pthread_create(&facilities.sa_service, NULL, sa_service, (void*) &facilities);
uint8_t buffer[PACKET_MAX_LEN];
syslog_info("[facilities] listening");

31
src/sa.c
View File

@ -23,7 +23,7 @@ SAEM_CODE_R saem_check(void* fc, press_t* press, SAEM_t* saem) {
int rv = 0;
if (saem->header.messageID != ItsPduHeader__messageID_saem) {
if (saem->header.messageID != messageID_saem) {
return SAEM_INVALID_HEADER_MESSAGE_ID;
}
@ -74,35 +74,12 @@ SAEM_CODE_R saem_check(void* fc, press_t* press, SAEM_t* saem) {
uint16_t ci_index = si->channelIndex;
// TODO chOptions
for (int o = 0; o < si->chOptions.extensions->list.count; ++o) {
Extension_34P0_t* sie = (Extension_34P0_t*) si->chOptions.extensions->list.array[o];
switch (sie->value.present) {
case Extension_34P0__value_PR_TwoDLocation:
press->available[index]->position.latitude = sie->value.choice.TwoDLocation.latitude;
press->available[index]->position.longitude = sie->value.choice.TwoDLocation.longitude;
break;
case Extension_34P0__value_PR_ThreeDLocation:
press->available[index]->position.latitude = sie->value.choice.ThreeDLocation.latitude;
press->available[index]->position.longitude = sie->value.choice.ThreeDLocation.longitude;
press->available[index]->position.altitude = sie->value.choice.ThreeDLocation.elevation;
break;
case Extension_34P0__value_PR_RepeatRate:
break;
case Extension_34P0__value_PR_AdvertiserIdentifier:
break;
default:
break;
}
}
// TODO channelInfos
if (saem->sam.body.channelInfos) {
if (saem->sam.body.channelInfos->list.count >= ci_index + 1) {
}
}
}
}
@ -125,7 +102,7 @@ int mk_saem(facilities_t* facilities, uint8_t* b_saem, uint32_t* b_saem_len) {
/* header */
saem->header.protocolVersion = 2;
saem->header.messageID = ItsPduHeader__messageID_saem;
saem->header.messageID = messageID_saem;
pthread_mutex_lock(&facilities->id.lock);
saem->header.stationID = facilities->id.value;
@ -142,8 +119,8 @@ int mk_saem(facilities_t* facilities, uint8_t* b_saem, uint32_t* b_saem_len) {
uint8_t buf[1024];
for (int i = 0; i < facilities->press.providing_len; ++i) {
saem->sam.body.serviceInfos->list.array[i] = calloc(1, sizeof(ServiceInfo_t));
asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_ServiceInfo, NULL, facilities->press.providing[i], buf, 1024);
oer_decode(NULL, &asn_DEF_ServiceInfo, (void**) &saem->sam.body.serviceInfos->list.array[i], buf, enc.encoded);
saem->sam.body.serviceInfos->list.array[i]->serviceID.present = VarLengthNumber_PR_content;
saem->sam.body.serviceInfos->list.array[i]->serviceID.choice.content = facilities->press.providing[i]->its_aid;
}
asn_enc_rval_t enc = asn_encode_to_buffer(NULL, ATS_UNALIGNED_CANONICAL_PER, &asn_DEF_SAEM, saem, b_saem, *b_saem_len);

8
src/sa.h
View File

@ -12,19 +12,13 @@ typedef struct announcement {
uint64_t station_id;
uint64_t timestamp;
struct {
int32_t latitude;
int32_t longitude;
int16_t altitude;
} position;
} announcement_t;
typedef struct press {
announcement_t* available[MAX_ANNOUNCEMENTS_LEN];
uint16_t available_len;
ServiceInfo_t* providing[16];
announcement_t* providing[16];
uint8_t providing_len;
} press_t;