diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 0ab98c1..e6fbdbf 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -10,9 +10,9 @@ TARGET_LINK_LIBRARIES(it2s-itss-facilities -lit2s-asn-itss-facilities -lit2s-asn-itss-transport -lzmq - -ltoml -lit2s-gps -lpthread + -ltoml -lit2s-asn-camv2 -lit2s-asn-ivim -lit2s-asn-denmv2 diff --git a/src/cam.c b/src/cam.c index b243511..3d95987 100644 --- a/src/cam.c +++ b/src/cam.c @@ -2,6 +2,7 @@ #include "facilities.h" #include +#include #include #include @@ -55,56 +56,56 @@ static SpeedConfidence_t getSpeedConfidence(uint32_t conf) { } -static int mk_cam(uint8_t *cam, uint32_t *cam_len) { +static int mk_cam(facilities_t* facilities, uint8_t *cam, uint32_t *cam_len) { int rv = 0; - struct timespec st; - if (clock_gettime(CLOCK_REALTIME, &st)) { + struct timespec systemtime; + if (clock_gettime(CLOCK_REALTIME, &systemtime)) { syslog_emerg("clock_gettime() failed"); } + struct it2s_gps_data gps_data; + it2s_gps_read(&gps_data); + CAM_t *cam_tx = calloc(1, sizeof(CAM_t)); cam_tx->header.protocolVersion = 2; cam_tx->header.messageID = ItsPduHeader__messageID_cam; cam_tx->header.stationID = 0; - cam_tx->cam.camParameters.basicContainer.stationType = StationType_passengerCar; - cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.vehicleWidth = 20; - cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.vehicleLength.vehicleLengthValue = 46; - cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.vehicleLength.vehicleLengthConfidenceIndication = VehicleLengthConfidenceIndication_unavailable; - cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.longitudinalAcceleration.longitudinalAccelerationValue = LongitudinalAccelerationValue_unavailable; - cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.longitudinalAcceleration.longitudinalAccelerationConfidence = AccelerationConfidence_unavailable; + cam_tx->cam.camParameters.basicContainer.stationType = facilities->station_type; - cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.driveDirection = DriveDirection_unavailable; - cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.curvature.curvatureValue = CurvatureValue_unavailable; - cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.curvature.curvatureConfidence = CurvatureConfidence_unavailable; - cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.yawRate.yawRateValue = YawRateValue_unavailable; - cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.yawRate.yawRateConfidence = YawRateConfidence_unavailable; + if (facilities->station_type != StationType_roadSideUnit) { + cam_tx->cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_basicVehicleContainerHighFrequency; + cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.vehicleWidth = 20; + cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.vehicleLength.vehicleLengthValue = 46; + cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.vehicleLength.vehicleLengthConfidenceIndication = VehicleLengthConfidenceIndication_unavailable; + cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.longitudinalAcceleration.longitudinalAccelerationValue = LongitudinalAccelerationValue_unavailable; + cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.longitudinalAcceleration.longitudinalAccelerationConfidence = AccelerationConfidence_unavailable; - cam_tx->cam.camParameters.lowFrequencyContainer = calloc(1, sizeof(struct LowFrequencyContainer)); - cam_tx->cam.camParameters.lowFrequencyContainer->present = LowFrequencyContainer_PR_basicVehicleContainerLowFrequency; - cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.vehicleRole = VehicleRole_default; - cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.exteriorLights.buf = calloc(1, sizeof(uint8_t) * 1); - cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.exteriorLights.buf[0] = 1 << 6; - cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.exteriorLights.bits_unused = 5; - cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.exteriorLights.size = 1; - *(cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.exteriorLights.buf) = 0x00; - cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory.list.count = 0; - cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory.list.size = 0; + cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.driveDirection = DriveDirection_unavailable; + cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.curvature.curvatureValue = CurvatureValue_unavailable; + cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.curvature.curvatureConfidence = CurvatureConfidence_unavailable; + cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.yawRate.yawRateValue = YawRateValue_unavailable; + cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.yawRate.yawRateConfidence = YawRateConfidence_unavailable; + + if (!isnan(gps_data.gps_track)) { + cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading.headingValue = ((uint32_t)(gps_data.gps_track * 10)); + cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading.headingConfidence = getHeadingConfidence((uint32_t)(gps_data.gps_epd * 10)); + } else { + cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading.headingValue = HeadingValue_unavailable; + cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading.headingConfidence = HeadingConfidence_unavailable; + + } - cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory.list.count = 1; - cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory.list.size = 1; - cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory.list.array = calloc(1, sizeof(struct PathPoint*)); - cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory.list.array[0] = calloc(1, sizeof(struct PathPoint)); - cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory.list.array[0]->pathDeltaTime = calloc(1, sizeof(PathDeltaTime_t)); - *(cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory.list.array[0]->pathDeltaTime) = PathDeltaTime_tenMilliSecondsInPast; - cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory.list.array[0]->pathPosition.deltaAltitude = DeltaAltitude_unavailable; - cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory.list.array[0]->pathPosition.deltaLatitude = DeltaLatitude_unavailable; - cam_tx->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory.list.array[0]->pathPosition.deltaLongitude = DeltaLongitude_unavailable; - - struct timespec systemtime; - if (clock_gettime(CLOCK_REALTIME, &systemtime)) { - syslog_emerg("clock_gettime() failed"); + if (!isnan(gps_data.gps_speed)) { + cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.speed.speedValue = ((uint32_t)(gps_data.gps_speed * 100)); // cm/s + cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.speed.speedConfidence = getSpeedConfidence((uint32_t)(gps_data.gps_eps * 100)); + } else { + cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.speed.speedValue = SpeedValue_unavailable; + cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.speed.speedConfidence = SpeedConfidence_unavailable; + } + } else { + cam_tx->cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_rsuContainerHighFrequency; } long generationdeltatime = (long)((systemtime.tv_sec + LEAP_SECONDS) * 1000 + systemtime.tv_nsec / 1E6); @@ -112,53 +113,33 @@ static int mk_cam(uint8_t *cam, uint32_t *cam_len) { generationdeltatime = generationdeltatime % 64536; cam_tx->cam.generationDeltaTime = generationdeltatime; - struct it2s_gps_data gps_data; - it2s_gps_read(&gps_data); if (!isnan(gps_data.gps_altitude)) { cam_tx->cam.camParameters.basicContainer.referencePosition.altitude.altitudeValue = (int32_t)((gps_data.gps_altitude) * 10); cam_tx->cam.camParameters.basicContainer.referencePosition.altitude.altitudeConfidence = getAltitudeConfidence(gps_data.gps_epv); } else { - cam_tx->cam.camParameters.basicContainer.referencePosition.altitude.altitudeValue = AltitudeValue_unavailable; - cam_tx->cam.camParameters.basicContainer.referencePosition.altitude.altitudeConfidence = AltitudeConfidence_unavailable; + cam_tx->cam.camParameters.basicContainer.referencePosition.altitude.altitudeValue = AltitudeValue_unavailable; + cam_tx->cam.camParameters.basicContainer.referencePosition.altitude.altitudeConfidence = AltitudeConfidence_unavailable; } if (!isnan(gps_data.gps_latitude)) { cam_tx->cam.camParameters.basicContainer.referencePosition.latitude = (int32_t)((gps_data.gps_latitude) * 10000000); } else { - cam_tx->cam.camParameters.basicContainer.referencePosition.latitude = DeltaLatitude_unavailable; + cam_tx->cam.camParameters.basicContainer.referencePosition.latitude = DeltaLatitude_unavailable; } if (!isnan(gps_data.gps_longitude)) { cam_tx->cam.camParameters.basicContainer.referencePosition.longitude = (int32_t)((gps_data.gps_longitude) * 10000000); } else { - cam_tx->cam.camParameters.basicContainer.referencePosition.longitude = Longitude_unavailable; + cam_tx->cam.camParameters.basicContainer.referencePosition.longitude = Longitude_unavailable; } cam_tx->cam.camParameters.basicContainer.referencePosition.positionConfidenceEllipse.semiMinorConfidence = SemiAxisLength_unavailable; cam_tx->cam.camParameters.basicContainer.referencePosition.positionConfidenceEllipse.semiMajorConfidence = SemiAxisLength_unavailable; cam_tx->cam.camParameters.basicContainer.referencePosition.positionConfidenceEllipse.semiMajorOrientation = HeadingValue_unavailable; - - cam_tx->cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_basicVehicleContainerHighFrequency; - if (!isnan(gps_data.gps_track)) { - cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading.headingValue = ((uint32_t)(gps_data.gps_track * 10)); - cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading.headingConfidence = getHeadingConfidence((uint32_t)(gps_data.gps_epd * 10)); - } else { - cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading.headingValue = HeadingValue_unavailable; - cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading.headingConfidence = HeadingConfidence_unavailable; - - } - - if (!isnan(gps_data.gps_speed)) { - cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.speed.speedValue = ((uint32_t)(gps_data.gps_speed * 100)); // cm/s - cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.speed.speedConfidence = getSpeedConfidence((uint32_t)(gps_data.gps_eps * 100)); - } else { - cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.speed.speedValue = SpeedValue_unavailable; - cam_tx->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.speed.speedConfidence = SpeedConfidence_unavailable; - } - asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_CAM, NULL, cam_tx, cam, 256); if (enc.encoded == -1) { + syslog_err("[facilities] [ca] failed encoding CAM (%s)", enc.failed_type->name); rv = 1; goto cleanup; } @@ -171,6 +152,82 @@ cleanup: } +lightship_t* lightship_init() { + lightship_t* lightship = (lightship_t*) calloc(1, sizeof(lightship_t)); + + pthread_mutex_init(&lightship->lock, NULL); + + return lightship; +} + + +int lightship_check(lightship_t* lightship) { + int rv = 0; + + struct timespec systemtime; + clock_gettime(CLOCK_REALTIME, &systemtime); + long now = (long)((systemtime.tv_sec + LEAP_SECONDS) * 1000 + systemtime.tv_nsec / 1E6); + now -= 1072915200000; + + pthread_mutex_lock(&lightship->lock); + + if (lightship->type == StationType_roadSideUnit) { // RSU + if (lightship->is_vehicle_near && now > lightship->next_cam) { + rv = 1; + } + } else { // Vehicle + if (now > lightship->next_cam) { + rv = 1; + } + } + + + pthread_mutex_unlock(&lightship->lock); + + return rv; +} + +void lightship_reset_timer(lightship_t* lightship) { + struct timespec systemtime; + clock_gettime(CLOCK_REALTIME, &systemtime); + long now = (long)((systemtime.tv_sec + LEAP_SECONDS) * 1000 + systemtime.tv_nsec / 1E6); + now -= 1072915200000; + + pthread_mutex_lock(&lightship->lock); + + if (lightship->type != StationType_roadSideUnit) { // Vehicle + lightship->next_cam = now + VEHICLE_GEN_CAM_MAX; + } else { // RSU + if (now > lightship->last_vehicle + RSU_VEHICLE_PERMANENCE) { + lightship->is_vehicle_near = false; + } + lightship->next_cam = now + RSU_GEN_CAM_MIN; + } + + pthread_mutex_unlock(&lightship->lock); +} + +int check_cam(void* fc, BTPDataIndication_t *bdi, CAM_t* cam) { + int rv = 0; + facilities_t *facilities = (facilities_t*) fc; + + struct timespec systemtime; + clock_gettime(CLOCK_REALTIME, &systemtime); + long now = (long)((systemtime.tv_sec + LEAP_SECONDS) * 1000 + systemtime.tv_nsec / 1E6); + now -= 1072915200000; + + pthread_mutex_lock(&facilities->lightship->lock); + if (facilities->lightship->type == StationType_roadSideUnit) { + if (bdi->stationType != StationType_roadSideUnit && bdi->isNeighbour) { + facilities->lightship->last_vehicle = now; + facilities->lightship->is_vehicle_near = true; + } + } + pthread_mutex_unlock(&facilities->lightship->lock); + + return rv; +} + void *ca_service(void *fc) { int rv = 0; @@ -199,22 +256,26 @@ void *ca_service(void *fc) { uint8_t bdr_oer[256]; bdr_oer[0] = 4; // Facilities while (!facilities->exit) { - rv = mk_cam(bdr->data.buf, (uint32_t *) &bdr->data.size); - if (rv) { - syslog_err("[facilities] make cam failed (%d)", rv); - continue; + usleep(1000*50); + + if ( lightship_check(facilities->lightship) ) { + rv = mk_cam(facilities, bdr->data.buf, (uint32_t *) &bdr->data.size); + if (rv) { + continue; + } + + asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_BTPDataRequest, NULL, bdr, bdr_oer+1, 255); + if (enc.encoded == -1) { + syslog_err("[facilities] encoding BTPDataRequest for cam failed"); + continue; + } + + queue_add(facilities->tx_queue, bdr_oer, enc.encoded+1, 3); + pthread_cond_signal(&facilities->tx_queue->trigger); + + lightship_reset_timer(facilities->lightship); } - asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_BTPDataRequest, NULL, bdr, bdr_oer+1, 255); - if (enc.encoded == -1) { - syslog_err("[facilities] encoding BTPDataRequest for cam failed"); - continue; - } - - queue_add(facilities->tx_queue, bdr_oer, enc.encoded+1, 3); - pthread_cond_signal(&facilities->tx_queue->trigger); - - usleep(1000*1000); } ASN_STRUCT_FREE(asn_DEF_BTPDataRequest, bdr); diff --git a/src/cam.h b/src/cam.h index d69c35f..58c3527 100644 --- a/src/cam.h +++ b/src/cam.h @@ -2,7 +2,37 @@ #define FACILITIES_CAM_H #include +#include +#include +#include -void *ca_service(void *fc); +#include +#include + +#define VEHICLE_GEN_CAM_MIN 100 +#define VEHICLE_GEN_CAM_MAX 1000 + +#define RSU_GEN_CAM_MIN 1000 +#define RSU_VEHICLE_PERMANENCE 5000 + +typedef struct lightship { + pthread_mutex_t lock; + + uint8_t type; + + uint64_t next_cam; + + bool is_vehicle_near; + uint64_t last_vehicle; + +} lightship_t; + +lightship_t* lightship_init(); + +int lightship_check(lightship_t *lightship); +void lightship_reset_timer(lightship_t *lightship); + +int check_cam(void* fc, BTPDataIndication_t* bdi, CAM_t* cam); +void* ca_service(void* fc); #endif diff --git a/src/config.h b/src/config.h index 144afbb..fd914b1 100644 --- a/src/config.h +++ b/src/config.h @@ -1,6 +1,6 @@ #ifndef ITSS_CONFIG_H #define ITSS_CONFIG_H -int itss_config(void *itss_s, char * config_file); +int itss_config(void *facilities_s, char * config_file); #endif diff --git a/src/facilities.c b/src/facilities.c index c23a9b9..4b9e016 100644 --- a/src/facilities.c +++ b/src/facilities.c @@ -77,18 +77,29 @@ static int transport_indication(facilities_t *facilities, void* responder, uint8 } // Manage message - if (bdi->destinationPort == Port_denm) { - #ifdef DEBUG - uint8_t* xml_denm = malloc(4096); - asn_enc_rval_t rve = xer_encode_to_buffer(xml_denm, 4096, 0x02, &asn_DEF_DENM, its_msg); - syslog_debug("DENM XER %d: %.*s", (int)rve.encoded, (int)rve.encoded , xml_denm); - free(xml_denm); - #endif + switch (bdi->destinationPort) { + case Port_cam: - event_manage(facilities->den, its_msg); + if (facilities->station_type == 15) { // Currently only RSUs need to check CAMs + check_cam(facilities, bdi, its_msg); + } + + break; + case Port_denm: + #ifdef DEBUG + uint8_t* xml_denm = malloc(4096); + asn_enc_rval_t rve = xer_encode_to_buffer(xml_denm, 4096, 0x02, &asn_DEF_DENM, its_msg); + syslog_debug("DENM XER %d: %.*s", (int)rve.encoded, (int)rve.encoded , xml_denm); + free(xml_denm); + #endif + event_manage(facilities->den, its_msg); + + break; + default: + break; } - // Forward to app + // Forward to application fdi = calloc(1, sizeof(FacilitiesDataIndication_t)); fdi->itssMessageType = bdi->destinationPort; @@ -356,6 +367,9 @@ int main() { facilities.tx_queue = queue_init(); + facilities.lightship = lightship_init(); + facilities.lightship->type = facilities.station_type; + facilities.den = calloc(1, sizeof(den_t)); // Tx diff --git a/src/facilities.h b/src/facilities.h index 94b69a5..fbfbb24 100644 --- a/src/facilities.h +++ b/src/facilities.h @@ -4,6 +4,7 @@ #include #include +#include "cam.h" #include "denm.h" #include "queue.h" @@ -22,7 +23,10 @@ typedef struct facilities { // Transmitter queue_t* tx_queue; - // DENM + // CA + lightship_t* lightship; + + // DEN den_t *den; int station_type;