diff --git a/src/config.c b/src/config.c index b962d94..065f3be 100644 --- a/src/config.c +++ b/src/config.c @@ -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 { diff --git a/src/facilities.h b/src/facilities.h index a10120c..407d9f3 100644 --- a/src/facilities.h +++ b/src/facilities.h @@ -66,7 +66,7 @@ typedef struct facilities { // TP tolling_s tolling; - // DC + // PC coordination_s coordination; int station_type; diff --git a/src/pcm.c b/src/pcm.c index ea2c73d..3d979fd 100644 --- a/src/pcm.c +++ b/src/pcm.c @@ -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; } diff --git a/src/pcm.h b/src/pcm.h index 362ac78..7f1e92c 100644 --- a/src/pcm.h +++ b/src/pcm.h @@ -3,6 +3,8 @@ #include #include +#include + #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); diff --git a/test/tester.c b/test/tester.c index e105ee5..e0ea1cd 100644 --- a/test/tester.c +++ b/test/tester.c @@ -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;