diff --git a/src/cpm.c b/src/cpm.c index fdbbf31..06dae62 100644 --- a/src/cpm.c +++ b/src/cpm.c @@ -20,6 +20,7 @@ #include #include +#include #define syslog_info(msg, ...) syslog(LOG_INFO, msg, ##__VA_ARGS__) #define syslog_emerg(msg, ...) syslog(LOG_EMERG, "%s:%d [" msg "]", __func__, __LINE__, ##__VA_ARGS__) @@ -529,15 +530,24 @@ static int mk_cpm(facilities_t* facilities, uint8_t *bdr_oer, uint32_t *bdr_len, uint64_t generationDeltaTime = it2s_tender_get_clock(&facilities->epv) % 65536; // generationDeltaTime = TimestampIts mod 65 536 + int32_t lat, lon, alt, alt_conf; + pthread_mutex_lock(&facilities->epv.space.lock); + it2s_tender_get_space(&facilities->epv); + lat = facilities->epv.space.latitude; + lon = facilities->epv.space.longitude; + alt = facilities->epv.space.altitude; + alt_conf = facilities->epv.space.altitude_conf; + pthread_mutex_unlock(&facilities->epv.space.lock); + cpm_tx->cpm.generationDeltaTime = generationDeltaTime; cpm_tx->cpm.cpmParameters.managementContainer.stationType = StationType_roadSideUnit; - cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.latitude = facilities->epv.space.latitude; - cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.longitude = facilities->epv.space.longitude; - cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.positionConfidenceEllipse.semiMajorConfidence = 100; - cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.positionConfidenceEllipse.semiMinorConfidence = 100; - cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.positionConfidenceEllipse.semiMajorOrientation = HeadingValue_wgs84North; - cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.altitude.altitudeValue = 4000; - cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.altitude.altitudeConfidence = AltitudeConfidence_alt_005_00; + cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.latitude = lat; + cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.longitude = lon; + cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.positionConfidenceEllipse.semiMajorConfidence = 100; // TODO + cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.positionConfidenceEllipse.semiMinorConfidence = 100; // TODO + cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.positionConfidenceEllipse.semiMajorOrientation = HeadingValue_wgs84North; // TODO + cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.altitude.altitudeValue = alt; + cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.altitude.altitudeConfidence = alt_conf; if(dissemination_check(facilities->dissemination,&facilities->epv,0) == 1){ /* Sensor Information Container Inclusion Management */