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

Update for ITS-S Management SDU compability

This commit is contained in:
emanuel 2022-03-01 12:10:53 +00:00
parent d0c692d523
commit 119224cde5
5 changed files with 70 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
src/config.c
View File

@ -444,39 +444,40 @@ int facilities_config(void* facilities_s) {
ManagementReply_t* mrep = calloc(1, sizeof(ManagementReply_t));
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 &&
mrep->attributes->clockType &&
mrep->attributes->clock &&
mrep->attributes->clockOffset &&
mrep->attributes->gpsType &&
(!!mrep->attributes->trajectory == mreq->choice.attributes.choice.get.trajectory)) {
facilities->epv.space.latitude = mrep->attributes->coordinates->latitude;
facilities->epv.space.latitude_conf = mrep->attributes->coordinates->latitudeConfidence;
facilities->epv.space.longitude = mrep->attributes->coordinates->longitude;
facilities->epv.space.longitude_conf = mrep->attributes->coordinates->longitudeConfidence;
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;
if (mrep->returnCode == ManagementReplyReturnCode_accepted &&
mrep->data &&
mrep->data->choice.attributes.coordinates &&
mrep->data->choice.attributes.altitude &&
mrep->data->choice.attributes.heading &&
mrep->data->choice.attributes.speed &&
mrep->data->choice.attributes.clockType &&
mrep->data->choice.attributes.clock &&
mrep->data->choice.attributes.clockOffset &&
mrep->data->choice.attributes.gpsType &&
(!!mrep->data->choice.attributes.trajectory == mreq->choice.attributes.choice.get.trajectory)) {
facilities->epv.space.latitude = mrep->data->choice.attributes.coordinates->latitude;
facilities->epv.space.latitude_conf = mrep->data->choice.attributes.coordinates->latitudeConfidence;
facilities->epv.space.longitude = mrep->data->choice.attributes.coordinates->longitude;
facilities->epv.space.longitude_conf = mrep->data->choice.attributes.coordinates->longitudeConfidence;
facilities->epv.space.altitude = mrep->data->choice.attributes.altitude->altitudeValue;
facilities->epv.space.altitude_conf = mrep->data->choice.attributes.altitude->altitudeConfidence;
facilities->epv.space.heading = mrep->data->choice.attributes.heading->headingValue;
facilities->epv.space.heading_conf = mrep->data->choice.attributes.heading->headingConfidence;
facilities->epv.space.speed = mrep->data->choice.attributes.speed->speedValue;
facilities->epv.space.speed_conf = mrep->data->choice.attributes.speed->speedConfidence;
facilities->epv.space.type = *mrep->attributes->gpsType;
facilities->epv.time.type = *mrep->attributes->clockType;
facilities->epv.space.type = *mrep->data->choice.attributes.gpsType;
facilities->epv.time.type = *mrep->data->choice.attributes.clockType;
asn_INTEGER2ulong(mrep->attributes->clock, &facilities->epv.time.clock);
asn_INTEGER2ulong(mrep->attributes->clockOffset, &facilities->epv.time.offset);
asn_INTEGER2ulong(mrep->data->choice.attributes.clock, &facilities->epv.time.clock);
asn_INTEGER2ulong(mrep->data->choice.attributes.clockOffset, &facilities->epv.time.offset);
if (config->facilities.dcm.activate) {
facilities->epv.trajectory.len = mrep->attributes->trajectory->list.count;
for (int i = 0; i < mrep->attributes->trajectory->list.count; ++i) {
facilities->epv.trajectory.path[i].latitude = mrep->attributes->trajectory->list.array[i]->latitude;
facilities->epv.trajectory.path[i].longitude = mrep->attributes->trajectory->list.array[i]->longitude;
asn_INTEGER2ulong(&mrep->attributes->trajectory->list.array[i]->timestamp, &facilities->epv.trajectory.path[i].timestamp);
facilities->epv.trajectory.len = mrep->data->choice.attributes.trajectory->list.count;
for (int i = 0; i < mrep->data->choice.attributes.trajectory->list.count; ++i) {
facilities->epv.trajectory.path[i].latitude = mrep->data->choice.attributes.trajectory->list.array[i]->latitude;
facilities->epv.trajectory.path[i].longitude = mrep->data->choice.attributes.trajectory->list.array[i]->longitude;
asn_INTEGER2ulong(&mrep->data->choice.attributes.trajectory->list.array[i]->timestamp, &facilities->epv.trajectory.path[i].timestamp);
}
}
} else {

2
src/facilities.h
View File

@ -66,7 +66,7 @@ typedef struct facilities {
// TP
tolling_s tolling;
// DC
// PC
coordination_s coordination;
int station_type;

9
src/pcm.c
View File

@ -425,7 +425,11 @@ int pcm_check(void* fc, PCM_t* pcm) {
if (intersecting) {
syslog_info("[facilities] [pc] intersection danger with %ld @ (%f, %f) in %ld ms",
pcm->header.stationID, trajectoryA[index].latitude/1.0e7, trajectoryA[index].longitude/1.0e7, trajectoryA[index].timestamp - now);
pcm->header.stationID,
trajectoryA[index].latitude/1.0e7,
trajectoryA[index].longitude/1.0e7,
trajectoryA[index].timestamp - now);
pcm_check_intersection_detected(facilities, pcm, &coordination->neighbours[ni]);
}
}
@ -473,7 +477,7 @@ static int mk_pcm(facilities_t* facilities, uint8_t* pcm_uper, uint16_t* pcm_upe
pcm->pcm.currentPosition.longitude = lon;
asn_ulong2INTEGER(&pcm->pcm.currentPosition.timestamp, now);
if (facilities->station_type == 15) {
if (facilities->station_type == StationType_roadSideUnit) {
pcm->pcm.maneuverContainer.present = ManeuverContainer_PR_rsu;
ManeuverRSUContainer_t* mrc = &pcm->pcm.maneuverContainer.choice.rsu;
mrc->recommendedTrajectories = NULL; // TODO
@ -580,7 +584,6 @@ void* pc_service(void* fc) {
usleep(50 * 1000);
}
return NULL;
}

10
src/pcm.h
View File

@ -3,6 +3,8 @@
#include <stdbool.h>
#include <pcm/PCM.h>
#include <it2s-tender/geodesy.h>
#define MC_MAX_NEIGHBOURS 32
#define MC_RESOLUTION_TIMEOUT 1000
@ -24,10 +26,16 @@ typedef struct coordination {
uint64_t t_last_send_pcm;
uint64_t pcm_period_min;
uint64_t pcm_period_max;
mc_neighbour_s neighbours[MC_MAX_NEIGHBOURS];
uint8_t neighbours_len;
struct {
uint64_t id;
uint8_t link[32];
it2s_tender_region_t region;
} chain;
} coordination_s;
int pcm_check(void* fc, PCM_t* pcm);

47
test/tester.c
View File

@ -47,55 +47,56 @@ int init(void* ctx) {
switch (mreq->choice.attributes.present) {
case ManagementRequestAttributes_PR_get:
mrep->returnCode = ManagementReplyReturnCode_accepted;
mrep->attributes = calloc(1, sizeof(ManagementReplyAttributes_t));
mrep->data = calloc(1, sizeof(ManagementReplyData_t));
mrep->data->present = ManagementReplyData_PR_attributes;
if (mreq->choice.attributes.choice.get.stationID) {
mrep->attributes->stationID = malloc(sizeof(long));
*mrep->attributes->stationID = 78;
mrep->data->choice.attributes.stationID = malloc(sizeof(long));
*mrep->data->choice.attributes.stationID = 78;
}
if (mreq->choice.attributes.choice.get.gpsType) {
mrep->attributes->gpsType = malloc(sizeof(long));
*mrep->attributes->gpsType = 0;
mrep->data->choice.attributes.gpsType = malloc(sizeof(long));
*mrep->data->choice.attributes.gpsType = 0;
}
if (mreq->choice.attributes.choice.get.clockType) {
mrep->attributes->clockType = malloc(sizeof(long));
*mrep->attributes->clockType = 0;
mrep->data->choice.attributes.clockType = malloc(sizeof(long));
*mrep->data->choice.attributes.clockType = 0;
}
if (mreq->choice.attributes.choice.get.clock) {
mrep->attributes->clock = calloc(1, sizeof(TimestampIts_t));
asn_ulong2INTEGER(mrep->attributes->clock, 666);
mrep->data->choice.attributes.clock = calloc(1, sizeof(TimestampIts_t));
asn_ulong2INTEGER(mrep->data->choice.attributes.clock, 666);
}
if (mreq->choice.attributes.choice.get.clockOffset) {
mrep->attributes->clockOffset = calloc(1, sizeof(TimestampIts_t));
asn_ulong2INTEGER(mrep->attributes->clockOffset, 0);
mrep->data->choice.attributes.clockOffset = calloc(1, sizeof(TimestampIts_t));
asn_ulong2INTEGER(mrep->data->choice.attributes.clockOffset, 0);
}
if (mreq->choice.attributes.choice.get.coordinates) {
mrep->attributes->coordinates = calloc(1, sizeof(WGS84Coordinates_t));
mrep->attributes->coordinates->latitude = 0;
mrep->attributes->coordinates->longitude = 0;
mrep->data->choice.attributes.coordinates = calloc(1, sizeof(WGS84Coordinates_t));
mrep->data->choice.attributes.coordinates->latitude = 0;
mrep->data->choice.attributes.coordinates->longitude = 0;
}
if (mreq->choice.attributes.choice.get.altitude) {
mrep->attributes->altitude = calloc(1, sizeof(Altitude_t));
mrep->attributes->altitude->altitudeValue = 0;
mrep->attributes->altitude->altitudeConfidence = 1;
mrep->data->choice.attributes.altitude = calloc(1, sizeof(Altitude_t));
mrep->data->choice.attributes.altitude->altitudeValue = 0;
mrep->data->choice.attributes.altitude->altitudeConfidence = 1;
}
if (mreq->choice.attributes.choice.get.speed) {
mrep->attributes->speed = calloc(1, sizeof(Speed_t));
mrep->attributes->speed->speedValue = 0;
mrep->attributes->speed->speedConfidence = 1;
mrep->data->choice.attributes.speed = calloc(1, sizeof(Speed_t));
mrep->data->choice.attributes.speed->speedValue = 0;
mrep->data->choice.attributes.speed->speedConfidence = 1;
}
if (mreq->choice.attributes.choice.get.heading) {
mrep->attributes->heading = calloc(1, sizeof(Heading_t));
mrep->attributes->heading->headingValue = 0;
mrep->attributes->heading->headingConfidence = 1;
mrep->data->choice.attributes.heading = calloc(1, sizeof(Heading_t));
mrep->data->choice.attributes.heading->headingValue = 0;
mrep->data->choice.attributes.heading->headingConfidence = 1;
}
break;