diff --git a/vanetza/asn1/Dockerfile b/vanetza/asn1/Dockerfile new file mode 100644 index 000000000..7cac0cdc9 --- /dev/null +++ b/vanetza/asn1/Dockerfile @@ -0,0 +1,13 @@ +FROM alpine:3 AS base + +FROM base AS build +ARG ASN1C_REPO=https://github.com/mouse07410/asn1c.git +ARG ASN1C_VERSION=18e5650 +RUN apk add --no-cache autoconf automake bison build-base flex gcc git libtool +RUN git clone $ASN1C_REPO /asn1c-source && cd /asn1c-source && git checkout ${ASN1C_VERSION} +WORKDIR /asn1c-source +RUN autoreconf -iv && ./configure --prefix /asn1c && make -j $(nproc) install + +FROM base AS runtime +ENV ASN1C_FLAGS="-fcompound-names -fincludes-quoted -no-gen-example" +COPY --from=build /asn1c /asn1c diff --git a/vanetza/facilities/cam_functions.cpp b/vanetza/facilities/cam_functions.cpp index b1bd0ef3a..950e70725 100644 --- a/vanetza/facilities/cam_functions.cpp +++ b/vanetza/facilities/cam_functions.cpp @@ -1,16 +1,10 @@ #include #include -#include -#include #include -#include #include #include #include #include -#include -#include -#undef min namespace vanetza { @@ -21,67 +15,6 @@ using vanetza::units::Angle; static const auto microdegree = units::degree * units::si::micro; -// TODO: C2C-CC BSP allows up to 500m history for CAMs, we provide just minimal required history -void copy(const facilities::PathHistory& ph, BasicVehicleContainerLowFrequency& container) -{ - static const std::size_t scMaxPathPoints = 23; - static const boost::posix_time::time_duration scMaxDeltaTime = boost::posix_time::millisec(655350); - static const auto scMicrodegree = microdegree; - - const auto& concise_points = ph.getConcisePoints(); - const facilities::PathPoint& ref = ph.getReferencePoint(); - std::size_t path_points = 0; - - for (const PathPoint& point : concise_points) { - auto delta_time = ref.time - point.time; // positive: point is in past - auto delta_latitude = point.latitude - ref.latitude; // positive: point is north - auto delta_longitude = point.longitude - ref.longitude; // positive: point is east - - while (!delta_time.is_negative() && path_points < scMaxPathPoints) { - ::PathPoint* path_point = asn1::allocate<::PathPoint>(); - path_point->pathDeltaTime = asn1::allocate(); - *(path_point->pathDeltaTime) = std::min(delta_time, scMaxDeltaTime).total_milliseconds() / - 10 * PathDeltaTime::PathDeltaTime_tenMilliSecondsInPast; - path_point->pathPosition.deltaLatitude = (delta_latitude / scMicrodegree).value() * - DeltaLatitude::DeltaLatitude_oneMicrodegreeNorth; - path_point->pathPosition.deltaLongitude = (delta_longitude / scMicrodegree).value() * - DeltaLongitude::DeltaLongitude_oneMicrodegreeEast; - path_point->pathPosition.deltaAltitude = DeltaAltitude::DeltaAltitude_unavailable; - - ASN_SEQUENCE_ADD(&container.pathHistory, path_point); - - delta_time -= scMaxDeltaTime; - ++path_points; - } - } -} - -bool similar_heading(const Heading& a, const Heading& b, Angle limit) -{ - // HeadingValues are tenth of degree (900 equals 90 degree east) - static_assert(HeadingValue_wgs84East == 900, "HeadingValue interpretation fails"); - - bool result = false; - if (is_available(a) && is_available(b)) { - using vanetza::units::degree; - const Angle angle_a { a.headingValue / 10.0 * degree }; - const Angle angle_b { b.headingValue / 10.0 * degree }; - result = similar_heading(angle_a, angle_b, limit); - } - - return result; -} - -bool similar_heading(const Heading& a, Angle b, Angle limit) -{ - bool result = false; - if (is_available(a)) { - using vanetza::units::degree; - result = similar_heading(Angle { a.headingValue / 10.0 * degree}, b, limit); - } - return result; -} - bool similar_heading(Angle a, Angle b, Angle limit) { using namespace boost::units; @@ -92,54 +25,6 @@ bool similar_heading(Angle a, Angle b, Angle limit) return abs_diff <= limit || abs_diff >= full_circle - limit; } -units::Length distance(const ReferencePosition_t& a, const ReferencePosition_t& b) -{ - using geonet::GeodeticPosition; - using units::GeoAngle; - - auto length = units::Length::from_value(std::numeric_limits::quiet_NaN()); - if (is_available(a) && is_available(b)) { - GeodeticPosition geo_a { - GeoAngle { a.latitude / Latitude_oneMicrodegreeNorth * microdegree }, - GeoAngle { a.longitude / Longitude_oneMicrodegreeEast * microdegree } - }; - GeodeticPosition geo_b { - GeoAngle { b.latitude / Latitude_oneMicrodegreeNorth * microdegree }, - GeoAngle { b.longitude / Longitude_oneMicrodegreeEast * microdegree } - }; - length = geonet::distance(geo_a, geo_b); - } - return length; -} - -units::Length distance(const ReferencePosition_t& a, units::GeoAngle lat, units::GeoAngle lon) -{ - using geonet::GeodeticPosition; - using units::GeoAngle; - - auto length = units::Length::from_value(std::numeric_limits::quiet_NaN()); - if (is_available(a)) { - GeodeticPosition geo_a { - GeoAngle { a.latitude / Latitude_oneMicrodegreeNorth * microdegree }, - GeoAngle { a.longitude / Longitude_oneMicrodegreeEast * microdegree } - }; - GeodeticPosition geo_b { lat, lon }; - length = geonet::distance(geo_a, geo_b); - } - return length; -} - -bool is_available(const Heading& hd) -{ - return hd.headingValue != HeadingValue_unavailable; -} - -bool is_available(const ReferencePosition& pos) -{ - return pos.latitude != Latitude_unavailable && pos.longitude != Longitude_unavailable; -} - - template long round(const boost::units::quantity& q, const U& u) { @@ -147,42 +32,6 @@ long round(const boost::units::quantity& q, const U& u) return std::round(v.value()); } -void copy(const PositionFix& position, ReferencePosition& reference_position) { - reference_position.longitude = round(position.longitude, microdegree) * Longitude_oneMicrodegreeEast; - reference_position.latitude = round(position.latitude, microdegree) * Latitude_oneMicrodegreeNorth; - if (std::isfinite(position.confidence.semi_major.value()) - && std::isfinite(position.confidence.semi_minor.value())) - { - if ((position.confidence.semi_major.value() * 100 < SemiAxisLength_outOfRange) - && (position.confidence.semi_minor.value() * 100 < SemiAxisLength_outOfRange) - && (position.confidence.orientation.value() * 10 < HeadingValue_unavailable)) - { - reference_position.positionConfidenceEllipse.semiMajorConfidence = position.confidence.semi_major.value() * 100; // Value in centimeters - reference_position.positionConfidenceEllipse.semiMinorConfidence = position.confidence.semi_minor.value() * 100; - reference_position.positionConfidenceEllipse.semiMajorOrientation = (position.confidence.orientation.value()) * 10; // Value from 0 to 3600 - } - else - { - reference_position.positionConfidenceEllipse.semiMajorConfidence = SemiAxisLength_outOfRange; - reference_position.positionConfidenceEllipse.semiMinorConfidence = SemiAxisLength_outOfRange; - reference_position.positionConfidenceEllipse.semiMajorOrientation = HeadingValue_unavailable; - } - } - else - { - reference_position.positionConfidenceEllipse.semiMajorConfidence = SemiAxisLength_unavailable; - reference_position.positionConfidenceEllipse.semiMinorConfidence = SemiAxisLength_unavailable; - reference_position.positionConfidenceEllipse.semiMajorOrientation = HeadingValue_unavailable; - } - if (position.altitude) { - reference_position.altitude.altitudeValue = to_altitude_value(position.altitude->value()); - reference_position.altitude.altitudeConfidence = to_altitude_confidence(position.altitude->confidence()); - } else { - reference_position.altitude.altitudeValue = AltitudeValue_unavailable; - reference_position.altitude.altitudeConfidence = AltitudeConfidence_unavailable; - } -} - AltitudeConfidence_t to_altitude_confidence(units::Length confidence) { const double alt_con = confidence / units::si::meter; @@ -234,309 +83,49 @@ AltitudeValue_t to_altitude_value(units::Length alt) } } -bool check_service_specific_permissions(const asn1::Cam& cam, security::CamPermissions ssp) -{ - using security::CamPermission; - using security::CamPermissions; - - CamPermissions required_permissions; - const CamParameters_t& params = cam->cam.camParameters; - - if (params.highFrequencyContainer.present == HighFrequencyContainer_PR_rsuContainerHighFrequency) { - const RSUContainerHighFrequency_t& rsu = params.highFrequencyContainer.choice.rsuContainerHighFrequency; - if (rsu.protectedCommunicationZonesRSU) { - required_permissions.add(CamPermission::CEN_DSRC_Tolling_Zone); - } - } - - if (const SpecialVehicleContainer_t* special = params.specialVehicleContainer) { - const EmergencyContainer_t* emergency = nullptr; - const SafetyCarContainer_t* safety = nullptr; - const RoadWorksContainerBasic_t* roadworks = nullptr; - - switch (special->present) { - case SpecialVehicleContainer_PR_publicTransportContainer: - required_permissions.add(CamPermission::Public_Transport); - break; - case SpecialVehicleContainer_PR_specialTransportContainer: - required_permissions.add(CamPermission::Special_Transport); - break; - case SpecialVehicleContainer_PR_dangerousGoodsContainer: - required_permissions.add(CamPermission::Dangerous_Goods); - break; - case SpecialVehicleContainer_PR_roadWorksContainerBasic: - required_permissions.add(CamPermission::Roadwork); - roadworks = &special->choice.roadWorksContainerBasic; - break; - case SpecialVehicleContainer_PR_rescueContainer: - required_permissions.add(CamPermission::Rescue); - break; - case SpecialVehicleContainer_PR_emergencyContainer: - required_permissions.add(CamPermission::Emergency); - emergency = &special->choice.emergencyContainer; - break; - case SpecialVehicleContainer_PR_safetyCarContainer: - required_permissions.add(CamPermission::Safety_Car); - safety = &special->choice.safetyCarContainer; - break; - case SpecialVehicleContainer_PR_NOTHING: - default: - break; - } +} // namespace facilities +} // namespace vanetza - if (emergency && emergency->emergencyPriority && emergency->emergencyPriority->size == 1) { - // testing bit strings from asn1c is such a mess... - assert(emergency->emergencyPriority->buf); - uint8_t bits = *emergency->emergencyPriority->buf; - if (bits & (1 << (7 - EmergencyPriority_requestForRightOfWay))) { - required_permissions.add(CamPermission::Request_For_Right_Of_Way); - } - if (bits & (1 << (7 - EmergencyPriority_requestForFreeCrossingAtATrafficLight))) { - required_permissions.add(CamPermission::Request_For_Free_Crossing_At_Traffic_Light); - } - } +#define ASN1_PREFIX ASN1_RELEASE1_PREFIX +#define ITS_RELEASE 1 +#include "detail/cam.ipp" +#include "detail/heading.ipp" +#include "detail/path_history.ipp" +#include "detail/reference_position.ipp" - if (roadworks && roadworks->closedLanes) { - required_permissions.add(CamPermission::Closed_Lanes); - } +#undef ASN1_PREFIX +#undef ITS_RELEASE - if (safety && safety->trafficRule) { - switch (*safety->trafficRule) { - case TrafficRule_noPassing: - required_permissions.add(CamPermission::No_Passing); - break; - case TrafficRule_noPassingForTrucks: - required_permissions.add(CamPermission::No_Passing_For_Trucks); - break; - default: - break; - } - } +#define ASN1_PREFIX ASN1_RELEASE2_PREFIX +#define ITS_RELEASE 2 +#include "detail/cam.ipp" +#include "detail/heading.ipp" +#include "detail/path_history.ipp" +#include "detail/reference_position.ipp" - if (safety && safety->speedLimit) { - required_permissions.add(CamPermission::Speed_Limit); - } - } +namespace vanetza +{ +namespace facilities +{ - return ssp.has(required_permissions); +bool check_service_specific_permissions(const asn1::r1::Cam& cam, security::CamPermissions ssp) +{ + return check_service_specific_permissions(cam->cam.camParameters, ssp); } -void print_indented(std::ostream& os, const asn1::Cam& message, const std::string& indent, unsigned level) +bool check_service_specific_permissions(const asn1::r2::Cam& cam, security::CamPermissions ssp) { - auto prefix = [&](const char* field) -> std::ostream& { - for (unsigned i = 0; i < level; ++i) { - os << indent; - } - os << field << ": "; - return os; - }; - - const ItsPduHeader_t& header = message->header; - prefix("ITS PDU Header") << "\n"; - ++level; - prefix("Protocol Version") << header.protocolVersion << "\n"; - prefix("Message ID") << header.messageID << "\n"; - prefix("Station ID") << header.stationID << "\n"; - --level; - - const CoopAwareness_t& cam = message->cam; - prefix("CoopAwarensess") << "\n"; - ++level; - prefix("Generation Delta Time") << cam.generationDeltaTime << "\n"; - - prefix("Basic Container") << "\n"; - ++level; - const BasicContainer_t& basic = cam.camParameters.basicContainer; - prefix("Station Type") << basic.stationType << "\n"; - prefix("Reference Position") << "\n"; - ++level; - prefix("Longitude") << basic.referencePosition.longitude << "\n"; - prefix("Latitude") << basic.referencePosition.latitude << "\n"; - prefix("Semi Major Orientation") << basic.referencePosition.positionConfidenceEllipse.semiMajorOrientation << "\n"; - prefix("Semi Major Confidence") << basic.referencePosition.positionConfidenceEllipse.semiMajorConfidence << "\n"; - prefix("Semi Minor Confidence") << basic.referencePosition.positionConfidenceEllipse.semiMinorConfidence << "\n"; - prefix("Altitude [Confidence]") << basic.referencePosition.altitude.altitudeValue - << " [" << basic.referencePosition.altitude.altitudeConfidence << "]\n"; - --level; - --level; - - if (cam.camParameters.highFrequencyContainer.present == HighFrequencyContainer_PR_basicVehicleContainerHighFrequency) { - prefix("High Frequency Container [Basic Vehicle]") << "\n"; - ++level; - const BasicVehicleContainerHighFrequency& bvc = - cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency; - prefix("Heading [Confidence]") << bvc.heading.headingValue - << " [" << bvc.heading.headingConfidence << "]\n"; - prefix("Speed [Confidence]") << bvc.speed.speedValue - << " [" << bvc.speed.speedConfidence << "]\n"; - prefix("Drive Direction") << bvc.driveDirection << "\n"; - prefix("Longitudinal Acceleration [Confidence]") << bvc.longitudinalAcceleration.longitudinalAccelerationValue - << " [" << bvc.longitudinalAcceleration.longitudinalAccelerationConfidence << "]\n"; - prefix("Vehicle Length [Confidence Indication]") << bvc.vehicleLength.vehicleLengthValue - << " [" << bvc.vehicleLength.vehicleLengthConfidenceIndication << "]\n"; - prefix("Vehicle Width") << bvc.vehicleWidth << "\n"; - prefix("Curvature [Confidence]") << bvc.curvature.curvatureValue - << " [" << bvc.curvature.curvatureConfidence << "]\n"; - prefix("Curvature Calculation Mode") << bvc.curvatureCalculationMode << "\n"; - prefix("Yaw Rate [Confidence]") << bvc.yawRate.yawRateValue - << " [" << bvc.yawRate.yawRateConfidence << "]\n"; - --level; - } else if (cam.camParameters.highFrequencyContainer.present == HighFrequencyContainer_PR_rsuContainerHighFrequency) { - prefix("High Frequency Container [RSU]") << "\n"; - const RSUContainerHighFrequency_t& rsu = cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency; - if (nullptr != rsu.protectedCommunicationZonesRSU && nullptr != rsu.protectedCommunicationZonesRSU->list.array) { - ++level; - int size = rsu.protectedCommunicationZonesRSU->list.count; - for (int i = 0; i < size; i++) - { - prefix("Protected Zone") << "\n"; - ++level; - prefix("Type") << rsu.protectedCommunicationZonesRSU->list.array[i]->protectedZoneType << "\n"; - if (rsu.protectedCommunicationZonesRSU->list.array[i]->expiryTime - && nullptr != rsu.protectedCommunicationZonesRSU->list.array[i]->expiryTime->buf - && rsu.protectedCommunicationZonesRSU->list.array[i]->expiryTime->size > 0) - prefix("Expiry Time") << (unsigned) rsu.protectedCommunicationZonesRSU->list.array[i]->expiryTime->buf[0] << "\n"; - prefix("Latitude") << rsu.protectedCommunicationZonesRSU->list.array[i]->protectedZoneLatitude << "\n"; - prefix("Longitude") << rsu.protectedCommunicationZonesRSU->list.array[i]->protectedZoneLongitude << "\n"; - if (nullptr != rsu.protectedCommunicationZonesRSU->list.array[i]->protectedZoneRadius) - prefix("Radius") << *(rsu.protectedCommunicationZonesRSU->list.array[i]->protectedZoneRadius) << "\n"; - if (nullptr != rsu.protectedCommunicationZonesRSU->list.array[i]->protectedZoneRadius) - prefix("ID") << *(rsu.protectedCommunicationZonesRSU->list.array[i]->protectedZoneID) << "\n"; - --level; - } - --level; - } - } else { - prefix("High Frequency Container") << "empty\n"; - } - - if (nullptr != cam.camParameters.lowFrequencyContainer) { - if (cam.camParameters.lowFrequencyContainer->present == LowFrequencyContainer_PR_basicVehicleContainerLowFrequency) { - prefix("Low Frequency Container") << "\n"; - const BasicVehicleContainerLowFrequency_t& lfc = - cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency; - ++level; - prefix("Vehicle Role") << (lfc.vehicleRole) << "\n"; - - if (nullptr != lfc.exteriorLights.buf && lfc.exteriorLights.size > 0) - prefix("Exterior Lights") << unsigned(*(lfc.exteriorLights.buf)) << "\n"; - if (nullptr != lfc.pathHistory.list.array) { - int size = lfc.pathHistory.list.count; - for (int i = 0; i < size; i++) - { - prefix("Path history point") << "\n"; - ++level; - prefix("Latitude") << (lfc.pathHistory.list.array[i]->pathPosition.deltaLatitude) << "\n"; - prefix("Longitude") << (lfc.pathHistory.list.array[i]->pathPosition.deltaLongitude) << "\n"; - prefix("Altitude") << (lfc.pathHistory.list.array[i]->pathPosition.deltaAltitude) << "\n"; - if (lfc.pathHistory.list.array[i]->pathDeltaTime) - prefix("Delta time") << *(lfc.pathHistory.list.array[i]->pathDeltaTime) << "\n"; - --level; - } - } - --level; - } - else // LowFrequencyContainer_PR_NOTHING - prefix("Low Frequency Container") << "present but empty" << "\n"; - } - else - prefix("Low Frequency Container") << "not present" << "\n"; + return check_service_specific_permissions(cam->cam.camParameters, ssp); +} - if (nullptr != cam.camParameters.specialVehicleContainer) { - if (cam.camParameters.specialVehicleContainer->present == SpecialVehicleContainer_PR_publicTransportContainer) { - prefix("Special Vehicle Container [Public Transport]") << "\n"; - PublicTransportContainer_t& ptc = cam.camParameters.specialVehicleContainer->choice.publicTransportContainer; - ++level; - prefix("Embarkation Status") << ptc.embarkationStatus << "\n"; - if (ptc.ptActivation) { - prefix("PT Activation Type") << ptc.ptActivation->ptActivationType << "\n"; - if (0 != ptc.ptActivation->ptActivationData.size) { - int size = ptc.ptActivation->ptActivationData.size; - for (int i = 0; i < ptc.ptActivation->ptActivationData.size; i++) - prefix("PT Activation Data") << (unsigned) ptc.ptActivation->ptActivationData.buf[i] << "\n"; - } - } - --level; - } else if (cam.camParameters.specialVehicleContainer->present == SpecialVehicleContainer_PR_specialTransportContainer) { - prefix("Special Vehicle Container [Special Transport]") << "\n"; - SpecialTransportContainer_t& stc = cam.camParameters.specialVehicleContainer->choice.specialTransportContainer; - ++level; - if (nullptr != stc.specialTransportType.buf && stc.specialTransportType.size > 0) - prefix("Type") << (unsigned) stc.specialTransportType.buf[0] << "\n"; - if (nullptr != stc.lightBarSirenInUse.buf && stc.lightBarSirenInUse.size > 0) - prefix("Light Bar Siren in Use") << (unsigned) stc.lightBarSirenInUse.buf[0] << "\n"; - --level; - } else if (cam.camParameters.specialVehicleContainer->present == SpecialVehicleContainer_PR_dangerousGoodsContainer) { - prefix("Special Vehicle Container [Dangerous Goods]") << "\n"; - DangerousGoodsContainer_t& dgc = cam.camParameters.specialVehicleContainer->choice.dangerousGoodsContainer; - ++level; - prefix("Dangerous Goods Basic Type") << (unsigned)dgc.dangerousGoodsBasic << "\n"; - --level; - } else if (cam.camParameters.specialVehicleContainer->present == SpecialVehicleContainer_PR_roadWorksContainerBasic) { - prefix("Special Vehicle Container [Road Works]") << "\n"; - RoadWorksContainerBasic_t& rwc = cam.camParameters.specialVehicleContainer->choice.roadWorksContainerBasic; - ++level; - if (nullptr != rwc.roadworksSubCauseCode) - prefix("Sub Cause Code") << *(rwc.roadworksSubCauseCode) << "\n"; - if (nullptr != rwc.lightBarSirenInUse.buf && rwc.lightBarSirenInUse.size > 0) - prefix("Light Bar Siren in Use") << (unsigned) rwc.lightBarSirenInUse.buf[0] << "\n"; - if (nullptr != rwc.closedLanes) { - if (rwc.closedLanes->innerhardShoulderStatus) - prefix("Inner Hard Shoulder Status") << *(rwc.closedLanes->innerhardShoulderStatus) << "\n"; - if (rwc.closedLanes->outerhardShoulderStatus) - prefix("Outer Hard Shoulder Status") << *(rwc.closedLanes->outerhardShoulderStatus) << "\n"; - if (rwc.closedLanes->drivingLaneStatus && nullptr != rwc.closedLanes->drivingLaneStatus->buf - && rwc.closedLanes->drivingLaneStatus->size > 0) - prefix("Driving Lane Status") << (unsigned) rwc.closedLanes->drivingLaneStatus->buf[0] << "\n"; - } - --level; - } else if (cam.camParameters.specialVehicleContainer->present == SpecialVehicleContainer_PR_rescueContainer) { - prefix("Special Vehicle Container [Rescue]") << "\n"; - RescueContainer_t& rc = cam.camParameters.specialVehicleContainer->choice.rescueContainer; - ++level; - if (nullptr != rc.lightBarSirenInUse.buf && rc.lightBarSirenInUse.size > 0) - prefix("Light Bar Siren in Use") << (unsigned) rc.lightBarSirenInUse.buf[0] << "\n"; - --level; - } else if (cam.camParameters.specialVehicleContainer->present == SpecialVehicleContainer_PR_emergencyContainer) { - prefix("Special Vehicle Container [Emergency]") << "\n"; - EmergencyContainer_t& ec = cam.camParameters.specialVehicleContainer->choice.emergencyContainer; - ++level; - if (nullptr != ec.lightBarSirenInUse.buf && ec.lightBarSirenInUse.size > 0) - prefix("Light Bar Siren in Use") << (unsigned) ec.lightBarSirenInUse.buf[0] << "\n"; - if (nullptr != ec.incidentIndication) { - prefix("Incident Indication Cause Code") << ec.incidentIndication->causeCode << "\n"; - prefix("Incident Indication Sub Cause Code") << ec.incidentIndication->subCauseCode << "\n"; - } - if (nullptr != ec.emergencyPriority && nullptr != ec.emergencyPriority->buf - && ec.emergencyPriority->size > 0) { - prefix("Emergency Priority") << (unsigned) ec.emergencyPriority->buf[0] << "\n"; - } - --level; - } else if (cam.camParameters.specialVehicleContainer->present == SpecialVehicleContainer_PR_safetyCarContainer) { - prefix("Special Vehicle Container [Safety Car]") << "\n"; - SafetyCarContainer_t& sc = cam.camParameters.specialVehicleContainer->choice.safetyCarContainer; - ++level; - if (nullptr != sc.lightBarSirenInUse.buf && sc.lightBarSirenInUse.size > 0) - prefix("Light Bar Siren in Use") << (unsigned) sc.lightBarSirenInUse.buf[0] << "\n"; - if (nullptr != sc.incidentIndication) { - prefix("Incident Indication Cause Code") << sc.incidentIndication->causeCode << "\n"; - prefix("Incident Indication Sub Cause Code") << sc.incidentIndication->subCauseCode << "\n"; - } - if (nullptr != sc.trafficRule) { - prefix("Traffic Rule") << *(sc.trafficRule) << "\n"; - } - if (nullptr != sc.speedLimit) { - prefix("Speed Limit") << *(sc.speedLimit) << "\n"; - } - --level; - } - else // SpecialVehicleContainer_PR_NOTHING - prefix("Special Vehicle Container") << ("present but empty") << "\n"; - } - else - prefix("Special Vehicle Container") << "not present" << "\n"; +void print_indented(std::ostream& os, const asn1::r1::Cam& cam, const std::string& indent, unsigned start) +{ + print_indented(os, cam.content(), indent, start); +} - --level; +void print_indented(std::ostream& os, const asn1::r2::Cam& cam, const std::string& indent, unsigned start) +{ + print_indented(os, cam.content(), indent, start); } } // namespace facilities diff --git a/vanetza/facilities/cam_functions.hpp b/vanetza/facilities/cam_functions.hpp index 8c765fead..abc10a2d7 100644 --- a/vanetza/facilities/cam_functions.hpp +++ b/vanetza/facilities/cam_functions.hpp @@ -3,8 +3,6 @@ #include #include -#include -#include #include #include #include @@ -12,12 +10,20 @@ // forward declaration of asn1c generated struct struct BasicVehicleContainerLowFrequency; +struct Heading; +struct ReferencePosition; +struct Vanetza_ITS2_BasicVehicleContainerLowFrequency; +struct Vanetza_ITS2_Heading; +struct Vanetza_ITS2_ReferencePosition; namespace vanetza { -// forward declaration of CAM message wrapper -namespace asn1 { class Cam; } +// forward declaration of CAM message wrappers +namespace asn1 { + namespace r1 { class Cam; } + namespace r2 { class Cam; } +} namespace facilities { @@ -30,6 +36,7 @@ class PathHistory; * \param ASN.1 CAM container (destination) */ void copy(const PathHistory&, BasicVehicleContainerLowFrequency&); +void copy(const PathHistory&, Vanetza_ITS2_BasicVehicleContainerLowFrequency&); /** * Check if difference of two given heading values is within a limit @@ -40,6 +47,8 @@ void copy(const PathHistory&, BasicVehicleContainerLowFrequency&); */ bool similar_heading(const Heading& a, const Heading& b, units::Angle limit); bool similar_heading(const Heading& a, units::Angle b, units::Angle limit); +bool similar_heading(const Vanetza_ITS2_Heading& a, const Vanetza_ITS2_Heading&b, units::Angle limit); +bool similar_heading(const Vanetza_ITS2_Heading& a, units::Angle b, units::Angle limit); bool similar_heading(units::Angle a, units::Angle b, units::Angle limit); /** @@ -48,28 +57,37 @@ bool similar_heading(units::Angle a, units::Angle b, units::Angle limit); * \param b another position * \return distance between given positions (or NaN if some position is unavailable) */ -units::Length distance(const ReferencePosition_t& a, const ReferencePosition_t& b); -units::Length distance(const ReferencePosition_t& a, units::GeoAngle lat, units::GeoAngle lon); +units::Length distance(const ReferencePosition& a, const ReferencePosition& b); +units::Length distance(const ReferencePosition& a, units::GeoAngle lat, units::GeoAngle lon); +units::Length distance(const Vanetza_ITS2_ReferencePosition& a, const Vanetza_ITS2_ReferencePosition& b); +units::Length distance(const Vanetza_ITS2_ReferencePosition& a, units::GeoAngle lat, units::GeoAngle lon); /** * Check if ASN.1 data element indicates unavailable value * \return true if value is available */ bool is_available(const Heading&); -bool is_available(const ReferencePosition_t&); +bool is_available(const Vanetza_ITS2_Heading&); +bool is_available(const ReferencePosition&); +bool is_available(const Vanetza_ITS2_ReferencePosition&); /** * Copy position information into a ReferencePosition structure from CDD */ void copy(const PositionFix&, ReferencePosition&); +void copy(const PositionFix&, Vanetza_ITS2_ReferencePosition&); /** * Convert altitude to AltitudeValue from CDD + * + * It is safe to cast AltitudeValue_t to Vanetza_ITS2_AltitudeValue_t. */ AltitudeValue_t to_altitude_value(units::Length); /** * Convert altitude confidence to AltitudeConfidence from CDD + * + * It is safe to cast AltitudeConfidence_t to Vanetza_ITS2_AltitudeConfidence_t. */ AltitudeConfidence_t to_altitude_confidence(units::Length); @@ -79,7 +97,8 @@ AltitudeConfidence_t to_altitude_confidence(units::Length); * \param ssp CA service specific permissions * \return true if no forbidden data elements are included */ -bool check_service_specific_permissions(const asn1::Cam& cam, security::CamPermissions ssp); +bool check_service_specific_permissions(const asn1::r1::Cam& cam, security::CamPermissions ssp); +bool check_service_specific_permissions(const asn1::r2::Cam& cam, security::CamPermissions ssp); /** * Print CAM content with indentation of nested fields @@ -91,7 +110,8 @@ bool check_service_specific_permissions(const asn1::Cam& cam, security::CamPermi * This function is an idea of Erik de Britto e Silva (erikbritto@github) * from University of Antwerp - erik.debrittoesilva@uantwerpen.be */ -void print_indented(std::ostream& os, const asn1::Cam& cam, const std::string& indent = "\t", unsigned start = 0); +void print_indented(std::ostream& os, const asn1::r1::Cam& cam, const std::string& indent = "\t", unsigned start = 0); +void print_indented(std::ostream& os, const asn1::r2::Cam& cam, const std::string& indent = "\t", unsigned start = 0); } // namespace facilities } // namespace vanetza diff --git a/vanetza/facilities/detail/cam.ipp b/vanetza/facilities/detail/cam.ipp new file mode 100644 index 000000000..3a6d13307 --- /dev/null +++ b/vanetza/facilities/detail/cam.ipp @@ -0,0 +1,376 @@ +#include +#include +#include +#include + +ASSERT_EQUAL_TYPE(AltitudeConfidence_t); +ASSERT_EQUAL_ENUM(AltitudeConfidence_alt_000_01); +ASSERT_EQUAL_ENUM(AltitudeConfidence_alt_200_00); +ASSERT_EQUAL_ENUM(AltitudeConfidence_outOfRange); +ASSERT_EQUAL_ENUM(AltitudeConfidence_unavailable); + +ASSERT_EQUAL_TYPE(AltitudeValue_t); +ASSERT_EQUAL_ENUM(AltitudeValue_unavailable); + +ASSERT_EQUAL_TYPE(DeltaAltitude_t); +ASSERT_EQUAL_ENUM(DeltaAltitude_unavailable); + +ASSERT_EQUAL_TYPE(DeltaLatitude_t); +ASSERT_EQUAL_ENUM(DeltaLatitude_unavailable); + +ASSERT_EQUAL_TYPE(DeltaLongitude_t); +ASSERT_EQUAL_ENUM(DeltaLongitude_unavailable); + +ASSERT_EQUAL_TYPE(Latitude_t); +ASSERT_EQUAL_ENUM(Latitude_unavailable); + +ASSERT_EQUAL_TYPE(Longitude_t); +ASSERT_EQUAL_ENUM(Longitude_unavailable); + +ASSERT_EQUAL_TYPE(PathDeltaTime_t); + +namespace vanetza +{ +namespace facilities +{ + +bool check_service_specific_permissions(const ASN1_PREFIXED(CamParameters_t)& params, security::CamPermissions ssp) +{ + using security::CamPermission; + using security::CamPermissions; + + CamPermissions required_permissions; + + if (params.highFrequencyContainer.present == ASN1_PREFIXED(HighFrequencyContainer_PR_rsuContainerHighFrequency)) { + const ASN1_PREFIXED(RSUContainerHighFrequency_t)& rsu = params.highFrequencyContainer.choice.rsuContainerHighFrequency; + if (rsu.protectedCommunicationZonesRSU) { + required_permissions.add(CamPermission::CEN_DSRC_Tolling_Zone); + } + } + + if (const ASN1_PREFIXED(SpecialVehicleContainer_t)* special = params.specialVehicleContainer) { + const ASN1_PREFIXED(EmergencyContainer_t)* emergency = nullptr; + const ASN1_PREFIXED(SafetyCarContainer_t)* safety = nullptr; + const ASN1_PREFIXED(RoadWorksContainerBasic_t)* roadworks = nullptr; + + switch (special->present) { + case ASN1_PREFIXED(SpecialVehicleContainer_PR_publicTransportContainer): + required_permissions.add(CamPermission::Public_Transport); + break; + case ASN1_PREFIXED(SpecialVehicleContainer_PR_specialTransportContainer): + required_permissions.add(CamPermission::Special_Transport); + break; + case ASN1_PREFIXED(SpecialVehicleContainer_PR_dangerousGoodsContainer): + required_permissions.add(CamPermission::Dangerous_Goods); + break; + case ASN1_PREFIXED(SpecialVehicleContainer_PR_roadWorksContainerBasic): + required_permissions.add(CamPermission::Roadwork); + roadworks = &special->choice.roadWorksContainerBasic; + break; + case ASN1_PREFIXED(SpecialVehicleContainer_PR_rescueContainer): + required_permissions.add(CamPermission::Rescue); + break; + case ASN1_PREFIXED(SpecialVehicleContainer_PR_emergencyContainer): + required_permissions.add(CamPermission::Emergency); + emergency = &special->choice.emergencyContainer; + break; + case ASN1_PREFIXED(SpecialVehicleContainer_PR_safetyCarContainer): + required_permissions.add(CamPermission::Safety_Car); + safety = &special->choice.safetyCarContainer; + break; + default: + break; + } + + if (emergency && emergency->emergencyPriority && emergency->emergencyPriority->size == 1) { + // testing bit strings from asn1c is such a mess... + assert(emergency->emergencyPriority->buf); + uint8_t bits = *emergency->emergencyPriority->buf; + if (bits & (1 << (7 - ASN1_PREFIXED(EmergencyPriority_requestForRightOfWay)))) { + required_permissions.add(CamPermission::Request_For_Right_Of_Way); + } + if (bits & (1 << (7 - ASN1_PREFIXED(EmergencyPriority_requestForFreeCrossingAtATrafficLight)))) { + required_permissions.add(CamPermission::Request_For_Free_Crossing_At_Traffic_Light); + } + } + + if (roadworks && roadworks->closedLanes) { + required_permissions.add(CamPermission::Closed_Lanes); + } + + if (safety && safety->trafficRule) { + switch (*safety->trafficRule) { + case ASN1_PREFIXED(TrafficRule_noPassing): + required_permissions.add(CamPermission::No_Passing); + break; + case ASN1_PREFIXED(TrafficRule_noPassingForTrucks): + required_permissions.add(CamPermission::No_Passing_For_Trucks); + break; + default: + break; + } + } + + if (safety && safety->speedLimit) { + required_permissions.add(CamPermission::Speed_Limit); + } + } + + return ssp.has(required_permissions); +} + +void print_indented(std::ostream& os, const ASN1_PREFIXED(CAM_t)* message, const std::string& indent, unsigned level) +{ + auto prefix = [&](const char* field) -> std::ostream& { + for (unsigned i = 0; i < level; ++i) { + os << indent; + } + os << field << ": "; + return os; + }; + + const ASN1_PREFIXED(ItsPduHeader_t)& header = message->header; + prefix("ITS PDU Header") << "\n"; + ++level; + prefix("Protocol Version") << header.protocolVersion << "\n"; + #if ITS_RELEASE == 1 + prefix("Message ID") << header.messageID << "\n"; + prefix("Station ID") << header.stationID << "\n"; + #else + prefix("Message ID") << header.messageId << "\n"; + prefix("Station ID") << header.stationId << "\n"; + #endif + --level; + + #if ITS_RELEASE == 1 + const ASN1_PREFIXED(CoopAwareness_t)& cam = message->cam; + #else + const ASN1_PREFIXED(CamPayload_t)& cam = message->cam; + #endif + prefix("CoopAwareness") << "\n"; + ++level; + prefix("Generation Delta Time") << cam.generationDeltaTime << "\n"; + + prefix("Basic Container") << "\n"; + ++level; + const ASN1_PREFIXED(BasicContainer_t)& basic = cam.camParameters.basicContainer; + prefix("Station Type") << basic.stationType << "\n"; + prefix("Reference Position") << "\n"; + ++level; + prefix("Longitude") << basic.referencePosition.longitude << "\n"; + prefix("Latitude") << basic.referencePosition.latitude << "\n"; + #if ITS_RELEASE == 1 + prefix("Semi Major Orientation") << basic.referencePosition.positionConfidenceEllipse.semiMajorOrientation << "\n"; + prefix("Semi Major Confidence") << basic.referencePosition.positionConfidenceEllipse.semiMajorConfidence << "\n"; + prefix("Semi Minor Confidence") << basic.referencePosition.positionConfidenceEllipse.semiMinorConfidence << "\n"; + #else + prefix("Semi Major Axis Orientation") << basic.referencePosition.positionConfidenceEllipse.semiMajorAxisOrientation << "\n"; + prefix("Semi Major Axis Length") << basic.referencePosition.positionConfidenceEllipse.semiMajorAxisLength << "\n"; + prefix("Semi Minor Axis Length") << basic.referencePosition.positionConfidenceEllipse.semiMinorAxisLength << "\n"; + #endif + + prefix("Altitude [Confidence]") << basic.referencePosition.altitude.altitudeValue + << " [" << basic.referencePosition.altitude.altitudeConfidence << "]\n"; + --level; + --level; + + if (cam.camParameters.highFrequencyContainer.present == ASN1_PREFIXED(HighFrequencyContainer_PR_basicVehicleContainerHighFrequency)) { + prefix("High Frequency Container [Basic Vehicle]") << "\n"; + ++level; + const ASN1_PREFIXED(BasicVehicleContainerHighFrequency)& bvc = + cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency; + prefix("Heading [Confidence]") << bvc.heading.headingValue + << " [" << bvc.heading.headingConfidence << "]\n"; + prefix("Speed [Confidence]") << bvc.speed.speedValue + << " [" << bvc.speed.speedConfidence << "]\n"; + prefix("Drive Direction") << bvc.driveDirection << "\n"; + #if ITS_RELEASE == 1 + prefix("Longitudinal Acceleration [Confidence]") << bvc.longitudinalAcceleration.longitudinalAccelerationValue + << " [" << bvc.longitudinalAcceleration.longitudinalAccelerationConfidence << "]\n"; + #else + prefix("Longitudinal Acceleration [Confidence]") << bvc.longitudinalAcceleration.value + << " [" << bvc.longitudinalAcceleration.confidence << "]\n"; + #endif + prefix("Vehicle Length [Confidence Indication]") << bvc.vehicleLength.vehicleLengthValue + << " [" << bvc.vehicleLength.vehicleLengthConfidenceIndication << "]\n"; + prefix("Vehicle Width") << bvc.vehicleWidth << "\n"; + prefix("Curvature [Confidence]") << bvc.curvature.curvatureValue + << " [" << bvc.curvature.curvatureConfidence << "]\n"; + prefix("Curvature Calculation Mode") << bvc.curvatureCalculationMode << "\n"; + prefix("Yaw Rate [Confidence]") << bvc.yawRate.yawRateValue + << " [" << bvc.yawRate.yawRateConfidence << "]\n"; + --level; + } else if (cam.camParameters.highFrequencyContainer.present == ASN1_PREFIXED(HighFrequencyContainer_PR_rsuContainerHighFrequency)) { + prefix("High Frequency Container [RSU]") << "\n"; + const ASN1_PREFIXED(RSUContainerHighFrequency_t)& rsu = cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency; + if (nullptr != rsu.protectedCommunicationZonesRSU && nullptr != rsu.protectedCommunicationZonesRSU->list.array) { + ++level; + int size = rsu.protectedCommunicationZonesRSU->list.count; + for (int i = 0; i < size; i++) + { + prefix("Protected Zone") << "\n"; + ++level; + prefix("Type") << rsu.protectedCommunicationZonesRSU->list.array[i]->protectedZoneType << "\n"; + if (rsu.protectedCommunicationZonesRSU->list.array[i]->expiryTime + && nullptr != rsu.protectedCommunicationZonesRSU->list.array[i]->expiryTime->buf + && rsu.protectedCommunicationZonesRSU->list.array[i]->expiryTime->size > 0) + prefix("Expiry Time") << (unsigned) rsu.protectedCommunicationZonesRSU->list.array[i]->expiryTime->buf[0] << "\n"; + prefix("Latitude") << rsu.protectedCommunicationZonesRSU->list.array[i]->protectedZoneLatitude << "\n"; + prefix("Longitude") << rsu.protectedCommunicationZonesRSU->list.array[i]->protectedZoneLongitude << "\n"; + if (nullptr != rsu.protectedCommunicationZonesRSU->list.array[i]->protectedZoneRadius) + prefix("Radius") << *(rsu.protectedCommunicationZonesRSU->list.array[i]->protectedZoneRadius) << "\n"; + if (nullptr != rsu.protectedCommunicationZonesRSU->list.array[i]->protectedZoneRadius) + #if ITS_RELEASE == 1 + prefix("ID") << *(rsu.protectedCommunicationZonesRSU->list.array[i]->protectedZoneID) << "\n"; + #else + prefix("ID") << *(rsu.protectedCommunicationZonesRSU->list.array[i]->protectedZoneId) << "\n"; + #endif + --level; + } + --level; + } + } else { + prefix("High Frequency Container") << "empty\n"; + } + + if (nullptr != cam.camParameters.lowFrequencyContainer) { + if (cam.camParameters.lowFrequencyContainer->present == ASN1_PREFIXED(LowFrequencyContainer_PR_basicVehicleContainerLowFrequency)) { + prefix("Low Frequency Container") << "\n"; + const ASN1_PREFIXED(BasicVehicleContainerLowFrequency_t)& lfc = + cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency; + ++level; + prefix("Vehicle Role") << (lfc.vehicleRole) << "\n"; + + if (nullptr != lfc.exteriorLights.buf && lfc.exteriorLights.size > 0) + prefix("Exterior Lights") << unsigned(*(lfc.exteriorLights.buf)) << "\n"; + if (nullptr != lfc.pathHistory.list.array) { + int size = lfc.pathHistory.list.count; + for (int i = 0; i < size; i++) + { + prefix("Path history point") << "\n"; + ++level; + prefix("Latitude") << (lfc.pathHistory.list.array[i]->pathPosition.deltaLatitude) << "\n"; + prefix("Longitude") << (lfc.pathHistory.list.array[i]->pathPosition.deltaLongitude) << "\n"; + prefix("Altitude") << (lfc.pathHistory.list.array[i]->pathPosition.deltaAltitude) << "\n"; + if (lfc.pathHistory.list.array[i]->pathDeltaTime) + prefix("Delta time") << *(lfc.pathHistory.list.array[i]->pathDeltaTime) << "\n"; + --level; + } + } + --level; + } + else // LowFrequencyContainer_PR_NOTHING + prefix("Low Frequency Container") << "present but empty" << "\n"; + } + else + prefix("Low Frequency Container") << "not present" << "\n"; + + if (nullptr != cam.camParameters.specialVehicleContainer) { + if (cam.camParameters.specialVehicleContainer->present == ASN1_PREFIXED(SpecialVehicleContainer_PR_publicTransportContainer)) { + prefix("Special Vehicle Container [Public Transport]") << "\n"; + ASN1_PREFIXED(PublicTransportContainer_t)& ptc = cam.camParameters.specialVehicleContainer->choice.publicTransportContainer; + ++level; + prefix("Embarkation Status") << ptc.embarkationStatus << "\n"; + if (ptc.ptActivation) { + prefix("PT Activation Type") << ptc.ptActivation->ptActivationType << "\n"; + if (0 != ptc.ptActivation->ptActivationData.size) { + int size = ptc.ptActivation->ptActivationData.size; + for (int i = 0; i < ptc.ptActivation->ptActivationData.size; i++) + prefix("PT Activation Data") << (unsigned) ptc.ptActivation->ptActivationData.buf[i] << "\n"; + } + } + --level; + } else if (cam.camParameters.specialVehicleContainer->present == ASN1_PREFIXED(SpecialVehicleContainer_PR_specialTransportContainer)) { + prefix("Special Vehicle Container [Special Transport]") << "\n"; + ASN1_PREFIXED(SpecialTransportContainer_t)& stc = cam.camParameters.specialVehicleContainer->choice.specialTransportContainer; + ++level; + if (nullptr != stc.specialTransportType.buf && stc.specialTransportType.size > 0) + prefix("Type") << (unsigned) stc.specialTransportType.buf[0] << "\n"; + if (nullptr != stc.lightBarSirenInUse.buf && stc.lightBarSirenInUse.size > 0) + prefix("Light Bar Siren in Use") << (unsigned) stc.lightBarSirenInUse.buf[0] << "\n"; + --level; + } else if (cam.camParameters.specialVehicleContainer->present == ASN1_PREFIXED(SpecialVehicleContainer_PR_dangerousGoodsContainer)) { + prefix("Special Vehicle Container [Dangerous Goods]") << "\n"; + ASN1_PREFIXED(DangerousGoodsContainer_t)& dgc = cam.camParameters.specialVehicleContainer->choice.dangerousGoodsContainer; + ++level; + prefix("Dangerous Goods Basic Type") << (unsigned)dgc.dangerousGoodsBasic << "\n"; + --level; + } else if (cam.camParameters.specialVehicleContainer->present == ASN1_PREFIXED(SpecialVehicleContainer_PR_roadWorksContainerBasic)) { + prefix("Special Vehicle Container [Road Works]") << "\n"; + ASN1_PREFIXED(RoadWorksContainerBasic_t)& rwc = cam.camParameters.specialVehicleContainer->choice.roadWorksContainerBasic; + ++level; + if (nullptr != rwc.roadworksSubCauseCode) + prefix("Sub Cause Code") << *(rwc.roadworksSubCauseCode) << "\n"; + if (nullptr != rwc.lightBarSirenInUse.buf && rwc.lightBarSirenInUse.size > 0) + prefix("Light Bar Siren in Use") << (unsigned) rwc.lightBarSirenInUse.buf[0] << "\n"; + if (nullptr != rwc.closedLanes) { + if (rwc.closedLanes->innerhardShoulderStatus) + prefix("Inner Hard Shoulder Status") << *(rwc.closedLanes->innerhardShoulderStatus) << "\n"; + if (rwc.closedLanes->outerhardShoulderStatus) + prefix("Outer Hard Shoulder Status") << *(rwc.closedLanes->outerhardShoulderStatus) << "\n"; + if (rwc.closedLanes->drivingLaneStatus && nullptr != rwc.closedLanes->drivingLaneStatus->buf + && rwc.closedLanes->drivingLaneStatus->size > 0) + prefix("Driving Lane Status") << (unsigned) rwc.closedLanes->drivingLaneStatus->buf[0] << "\n"; + } + --level; + } else if (cam.camParameters.specialVehicleContainer->present == ASN1_PREFIXED(SpecialVehicleContainer_PR_rescueContainer)) { + prefix("Special Vehicle Container [Rescue]") << "\n"; + ASN1_PREFIXED(RescueContainer_t)& rc = cam.camParameters.specialVehicleContainer->choice.rescueContainer; + ++level; + if (nullptr != rc.lightBarSirenInUse.buf && rc.lightBarSirenInUse.size > 0) + prefix("Light Bar Siren in Use") << (unsigned) rc.lightBarSirenInUse.buf[0] << "\n"; + --level; + } else if (cam.camParameters.specialVehicleContainer->present == ASN1_PREFIXED(SpecialVehicleContainer_PR_emergencyContainer)) { + prefix("Special Vehicle Container [Emergency]") << "\n"; + ASN1_PREFIXED(EmergencyContainer_t)& ec = cam.camParameters.specialVehicleContainer->choice.emergencyContainer; + ++level; + if (nullptr != ec.lightBarSirenInUse.buf && ec.lightBarSirenInUse.size > 0) + prefix("Light Bar Siren in Use") << (unsigned) ec.lightBarSirenInUse.buf[0] << "\n"; + if (nullptr != ec.incidentIndication) { + #if ITS_RELEASE == 1 + prefix("Incident Indication Cause Code") << ec.incidentIndication->causeCode << "\n"; + prefix("Incident Indication Sub Cause Code") << ec.incidentIndication->subCauseCode << "\n"; + #else + prefix("Incident Indication Cause Code V2") << ec.incidentIndication->ccAndScc.present << "\n"; + prefix("Incident Indication Sub Cause Code V2") << ec.incidentIndication->ccAndScc.choice.reserved0 << "\n"; + #endif + } + if (nullptr != ec.emergencyPriority && nullptr != ec.emergencyPriority->buf + && ec.emergencyPriority->size > 0) { + prefix("Emergency Priority") << (unsigned) ec.emergencyPriority->buf[0] << "\n"; + } + --level; + } else if (cam.camParameters.specialVehicleContainer->present == ASN1_PREFIXED(SpecialVehicleContainer_PR_safetyCarContainer)) { + prefix("Special Vehicle Container [Safety Car]") << "\n"; + ASN1_PREFIXED(SafetyCarContainer_t)& sc = cam.camParameters.specialVehicleContainer->choice.safetyCarContainer; + ++level; + if (nullptr != sc.lightBarSirenInUse.buf && sc.lightBarSirenInUse.size > 0) + prefix("Light Bar Siren in Use") << (unsigned) sc.lightBarSirenInUse.buf[0] << "\n"; + if (nullptr != sc.incidentIndication) { + #if ITS_RELEASE == 1 + prefix("Incident Indication Cause Code") << sc.incidentIndication->causeCode << "\n"; + prefix("Incident Indication Sub Cause Code") << sc.incidentIndication->subCauseCode << "\n"; + #else + prefix("Incident Indication Cause Code V2") << sc.incidentIndication->ccAndScc.present << "\n"; + prefix("Incident Indication Sub Cause Code V2") << sc.incidentIndication->ccAndScc.choice.reserved0 << "\n"; + #endif + } + if (nullptr != sc.trafficRule) { + prefix("Traffic Rule") << *(sc.trafficRule) << "\n"; + } + if (nullptr != sc.speedLimit) { + prefix("Speed Limit") << *(sc.speedLimit) << "\n"; + } + --level; + } + else // SpecialVehicleContainer_PR_NOTHING + prefix("Special Vehicle Container") << ("present but empty") << "\n"; + } + else + prefix("Special Vehicle Container") << "not present" << "\n"; + + --level; +} + +} // namespace facilities +} // namespace vanetza diff --git a/vanetza/facilities/detail/heading.ipp b/vanetza/facilities/detail/heading.ipp new file mode 100644 index 000000000..8a9a8deac --- /dev/null +++ b/vanetza/facilities/detail/heading.ipp @@ -0,0 +1,49 @@ +#include +#include +#include +#include + +ASSERT_EQUAL_ENUM(HeadingValue_wgs84North); +ASSERT_EQUAL_ENUM(HeadingValue_wgs84East); +ASSERT_EQUAL_ENUM(HeadingValue_wgs84South); +ASSERT_EQUAL_ENUM(HeadingValue_wgs84West); +ASSERT_EQUAL_ENUM(HeadingValue_unavailable); + +namespace vanetza +{ +namespace facilities +{ + +bool is_available(const ASN1_PREFIXED(Heading)& hd) +{ + return hd.headingValue != ASN1_PREFIXED(HeadingValue_unavailable); +} + +bool similar_heading(const ASN1_PREFIXED(Heading)& a, const ASN1_PREFIXED(Heading)& b, Angle limit) +{ + // HeadingValues are tenth of degree (900 equals 90 degree east) + static_assert(ASN1_PREFIXED(HeadingValue_wgs84East) == 900, "HeadingValue interpretation fails"); + + bool result = false; + if (is_available(a) && is_available(b)) { + using vanetza::units::degree; + const Angle angle_a { a.headingValue / 10.0 * degree }; + const Angle angle_b { b.headingValue / 10.0 * degree }; + result = similar_heading(angle_a, angle_b, limit); + } + + return result; +} + +bool similar_heading(const ASN1_PREFIXED(Heading)& a, Angle b, Angle limit) +{ + bool result = false; + if (is_available(a)) { + using vanetza::units::degree; + result = similar_heading(Angle { a.headingValue / 10.0 * degree}, b, limit); + } + return result; +} + +} // namespace facilities +} // namespace vanetza diff --git a/vanetza/facilities/detail/macros.ipp b/vanetza/facilities/detail/macros.ipp new file mode 100644 index 000000000..368594b9b --- /dev/null +++ b/vanetza/facilities/detail/macros.ipp @@ -0,0 +1,29 @@ +#pragma once + +#define ASN1_RELEASE2_PREFIX Vanetza_ITS2_ +#define ASN1_RELEASE1_PREFIX + +#define ASN1_CONCAT(x, y) ASN1_CONCAT_AGAIN(x, y) +#define ASN1_CONCAT_AGAIN(x, y) x ## y + +#define ASN1_RELEASE1_NAME(name) ASN1_CONCAT(ASN1_RELEASE1_PREFIX, name) +#define ASN1_RELEASE2_NAME(name) ASN1_CONCAT(ASN1_RELEASE2_PREFIX, name) + +/** + * Prepend code generation prefix to an ASN.1 name or type. + */ +#define ASN1_PREFIXED(name) ASN1_CONCAT(ASN1_PREFIX, name) + +/** + * Check that enum name has equal value in both releases. + */ +#define ASSERT_EQUAL_ENUM(name) \ + static_assert(int(ASN1_RELEASE1_NAME(name)) == int(ASN1_RELEASE2_NAME(name)), \ + #name " mismatch between release 1 and 2"); + +/** + * Check that types are equal in both releases + */ +#define ASSERT_EQUAL_TYPE(name) \ + static_assert(std::is_same::value, \ + #name " type mismatch between release 1 and 2"); diff --git a/vanetza/facilities/detail/path_history.ipp b/vanetza/facilities/detail/path_history.ipp new file mode 100644 index 000000000..46a971706 --- /dev/null +++ b/vanetza/facilities/detail/path_history.ipp @@ -0,0 +1,48 @@ +#include +#include +#include +#include +#include + +namespace vanetza +{ +namespace facilities +{ + +// TODO: C2C-CC BSP allows up to 500m history for CAMs, we provide just minimal required history +void copy(const facilities::PathHistory& ph, ASN1_PREFIXED(BasicVehicleContainerLowFrequency)& container) +{ + static const std::size_t scMaxPathPoints = 23; + static const boost::posix_time::time_duration scMaxDeltaTime = boost::posix_time::millisec(655350); + static const auto scMicrodegree = microdegree; + + const auto& concise_points = ph.getConcisePoints(); + const facilities::PathPoint& ref = ph.getReferencePoint(); + std::size_t path_points = 0; + + for (const PathPoint& point : concise_points) { + auto delta_time = ref.time - point.time; // positive: point is in past + auto delta_latitude = point.latitude - ref.latitude; // positive: point is north + auto delta_longitude = point.longitude - ref.longitude; // positive: point is east + + while (!delta_time.is_negative() && path_points < scMaxPathPoints) { + ::ASN1_PREFIXED(PathPoint)* path_point = asn1::allocate<::ASN1_PREFIXED(PathPoint)>(); + path_point->pathDeltaTime = asn1::allocate(); + *(path_point->pathDeltaTime) = std::min(delta_time, scMaxDeltaTime).total_milliseconds() / + 10 * PathDeltaTime::PathDeltaTime_tenMilliSecondsInPast; + path_point->pathPosition.deltaLatitude = (delta_latitude / scMicrodegree).value() * + DeltaLatitude::DeltaLatitude_oneMicrodegreeNorth; + path_point->pathPosition.deltaLongitude = (delta_longitude / scMicrodegree).value() * + DeltaLongitude::DeltaLongitude_oneMicrodegreeEast; + path_point->pathPosition.deltaAltitude = DeltaAltitude::DeltaAltitude_unavailable; + + ASN_SEQUENCE_ADD(&container.pathHistory, path_point); + + delta_time -= scMaxDeltaTime; + ++path_points; + } + } +} + +} // namespace facilities +} // namespace vanetza diff --git a/vanetza/facilities/detail/reference_position.ipp b/vanetza/facilities/detail/reference_position.ipp new file mode 100644 index 000000000..0336cfce4 --- /dev/null +++ b/vanetza/facilities/detail/reference_position.ipp @@ -0,0 +1,92 @@ +#include +#include +#include +#include +#include +#include + +namespace vanetza +{ +namespace facilities +{ + +units::Length distance(const ASN1_PREFIXED(ReferencePosition_t)& a, const ASN1_PREFIXED(ReferencePosition_t)& b) +{ + using geonet::GeodeticPosition; + using units::GeoAngle; + + auto length = units::Length::from_value(std::numeric_limits::quiet_NaN()); + if (is_available(a) && is_available(b)) { + GeodeticPosition geo_a { + GeoAngle { a.latitude / Latitude_oneMicrodegreeNorth * microdegree }, + GeoAngle { a.longitude / Longitude_oneMicrodegreeEast * microdegree } + }; + GeodeticPosition geo_b { + GeoAngle { b.latitude / Latitude_oneMicrodegreeNorth * microdegree }, + GeoAngle { b.longitude / Longitude_oneMicrodegreeEast * microdegree } + }; + length = geonet::distance(geo_a, geo_b); + } + return length; +} + +units::Length distance(const ASN1_PREFIXED(ReferencePosition_t)& a, units::GeoAngle lat, units::GeoAngle lon) +{ + using geonet::GeodeticPosition; + using units::GeoAngle; + + auto length = units::Length::from_value(std::numeric_limits::quiet_NaN()); + if (is_available(a)) { + GeodeticPosition geo_a { + GeoAngle { a.latitude / Latitude_oneMicrodegreeNorth * microdegree }, + GeoAngle { a.longitude / Longitude_oneMicrodegreeEast * microdegree } + }; + GeodeticPosition geo_b { lat, lon }; + length = geonet::distance(geo_a, geo_b); + } + return length; +} + +bool is_available(const ASN1_PREFIXED(ReferencePosition)& pos) +{ + return pos.latitude != ASN1_PREFIXED(Latitude_unavailable) && pos.longitude != ASN1_PREFIXED(Longitude_unavailable); +} + +void copy(const PositionFix& position, ASN1_PREFIXED(ReferencePosition)& reference_position) { + reference_position.longitude = round(position.longitude, microdegree) * Longitude_oneMicrodegreeEast; + reference_position.latitude = round(position.latitude, microdegree) * Latitude_oneMicrodegreeNorth; + if (std::isfinite(position.confidence.semi_major.value()) + && std::isfinite(position.confidence.semi_minor.value())) + { + if ((position.confidence.semi_major.value() * 100 < ASN1_PREFIXED(SemiAxisLength_outOfRange)) + && (position.confidence.semi_minor.value() * 100 < ASN1_PREFIXED(SemiAxisLength_outOfRange)) + && (position.confidence.orientation.value() * 10 < ASN1_PREFIXED(HeadingValue_unavailable))) + { + reference_position.positionConfidenceEllipse.semiMajorConfidence = position.confidence.semi_major.value() * 100; // Value in centimeters + reference_position.positionConfidenceEllipse.semiMinorConfidence = position.confidence.semi_minor.value() * 100; + reference_position.positionConfidenceEllipse.semiMajorOrientation = (position.confidence.orientation.value()) * 10; // Value from 0 to 3600 + } + else + { + reference_position.positionConfidenceEllipse.semiMajorConfidence = ASN1_PREFIXED(SemiAxisLength_outOfRange); + reference_position.positionConfidenceEllipse.semiMinorConfidence = ASN1_PREFIXED(SemiAxisLength_outOfRange); + reference_position.positionConfidenceEllipse.semiMajorOrientation = ASN1_PREFIXED(HeadingValue_unavailable); + } + } + else + { + reference_position.positionConfidenceEllipse.semiMajorConfidence = ASN1_PREFIXED(SemiAxisLength_unavailable); + reference_position.positionConfidenceEllipse.semiMinorConfidence = ASN1_PREFIXED(SemiAxisLength_unavailable); + reference_position.positionConfidenceEllipse.semiMajorOrientation = ASN1_PREFIXED(HeadingValue_unavailable); + } + if (position.altitude) { + reference_position.altitude.altitudeValue = to_altitude_value(position.altitude->value()); + reference_position.altitude.altitudeConfidence = to_altitude_confidence(position.altitude->confidence()); + } else { + reference_position.altitude.altitudeValue = ASN1_PREFIXED(AltitudeValue_unavailable); + reference_position.altitude.altitudeConfidence = ASN1_PREFIXED(AltitudeConfidence_unavailable); + } +} + +} // namespace facilities +} // namespace vanetza diff --git a/vanetza/facilities/tests/cam_functions.cpp b/vanetza/facilities/tests/cam_functions.cpp index 65bead042..dc93e705b 100644 --- a/vanetza/facilities/tests/cam_functions.cpp +++ b/vanetza/facilities/tests/cam_functions.cpp @@ -1,4 +1,8 @@ #include +#include +#include +#include +#include #include #include #include @@ -24,7 +28,14 @@ ::testing::AssertionResult NearDistance(const Length& a, const Length& b, Length } } -TEST(CamFunctions, similar_heading) +using HeadingTypes = ::testing::Types; +template +class CamFunctionsHeading : public ::testing::Test +{ +}; +TYPED_TEST_SUITE(CamFunctionsHeading, HeadingTypes); + +TEST(CamFunctionsHeading, similar_heading) { Angle a = 3 * si::radian; Angle b = 2 * si::radian; @@ -50,9 +61,11 @@ TEST(CamFunctions, similar_heading) EXPECT_FALSE(similar_heading(a, a, limit)); } -TEST(CamFunctions, similar_heading_unavailable1) +TYPED_TEST(CamFunctionsHeading, similar_heading_unavailable1) { - Heading a; + using SomeHeading = TypeParam; + + SomeHeading a; a.headingValue = HeadingValue_unavailable; Angle b = 2.0 * si::radian; Angle limit = 10 * si::radian; @@ -71,11 +84,13 @@ TEST(CamFunctions, similar_heading_unavailable1) EXPECT_TRUE(similar_heading(a, b, limit)); } -TEST(CamFunctions, similar_heading_unavailable2) +TYPED_TEST(CamFunctionsHeading, similar_heading_unavailable2) { - Heading a; + using SomeHeading = TypeParam; + + SomeHeading a; a.headingValue = HeadingValue_unavailable; - Heading b; + SomeHeading b; b.headingValue = HeadingValue_unavailable; Angle limit = 10 * si::radian; EXPECT_FALSE(is_available(a)); @@ -93,44 +108,55 @@ TEST(CamFunctions, similar_heading_unavailable2) EXPECT_TRUE(similar_heading(b, a, limit)); } -TEST(CamFunctions, distance_reference_positions) +using ReferencePositionTypes = ::testing::Types; +template +class CamFunctionsReferencePosition : public ::testing::Test +{ +}; +TYPED_TEST_SUITE(CamFunctionsReferencePosition, ReferencePositionTypes); + +TYPED_TEST(CamFunctionsReferencePosition, distance_reference_positions) { - ReferencePosition_t pos1; + using SomeReferencePosition = TypeParam; + + SomeReferencePosition pos1; pos1.latitude = microdegree(6, 21.23) * Latitude_oneMicrodegreeSouth; pos1.longitude = microdegree(33, 22.12) * Longitude_oneMicrodegreeWest; - ReferencePosition_t pos2; + SomeReferencePosition pos2; pos2.latitude = microdegree(6, 22.48) * Latitude_oneMicrodegreeSouth; pos2.longitude = microdegree(33, 22.55) * Longitude_oneMicrodegreeWest; EXPECT_TRUE(NearDistance(distance(pos1, pos2), 2440.0 * si::meter , 10.0 * si::meter)); - ReferencePosition_t pos3; + SomeReferencePosition pos3; pos3.latitude = microdegree(37, 17.3) * Latitude_oneMicrodegreeNorth; pos3.longitude = microdegree(0, 13.14) * Longitude_oneMicrodegreeWest; - ReferencePosition_t pos4; + SomeReferencePosition pos4; pos4.latitude = microdegree(37, 17.19) * Latitude_oneMicrodegreeNorth; pos4.longitude = microdegree(0, 9.45) * Longitude_oneMicrodegreeEast; EXPECT_TRUE(NearDistance(distance(pos3, pos4), 33390.0 * si::meter , 10.0 * si::meter)); - ReferencePosition_t pos5; + SomeReferencePosition pos5; pos5.latitude = microdegree(0, 19.24) * Latitude_oneMicrodegreeSouth; pos5.longitude = microdegree(83, 37.32) * Longitude_oneMicrodegreeEast; - ReferencePosition_t pos6; + SomeReferencePosition pos6; pos6.latitude = microdegree(0, 27.15) * Latitude_oneMicrodegreeNorth; pos6.longitude = microdegree(83, 04.45) * Longitude_oneMicrodegreeEast; EXPECT_TRUE(NearDistance(distance(pos5, pos6), 105010.0 * si::meter , 10.0 * si::meter)); - ReferencePosition_t pos7; + SomeReferencePosition pos7; pos7.latitude = microdegree(48, 45.56) * Latitude_oneMicrodegreeNorth; pos7.longitude = microdegree(11, 26.01) * Longitude_oneMicrodegreeEast; - ReferencePosition_t pos8; + SomeReferencePosition pos8; pos8.latitude = microdegree(48, 45.566) * Latitude_oneMicrodegreeNorth; pos8.longitude = microdegree(11, 26.04) * Longitude_oneMicrodegreeEast; EXPECT_TRUE(NearDistance(distance(pos7, pos8), 38.0 * si::meter , 0.5 * si::meter)); } -TEST(CamFunctions, distance_refpos_latlon) +TYPED_TEST(CamFunctionsReferencePosition, distance_refpos_latlon) { - ReferencePosition_t refpos; + using SomeReferencePosition = TypeParam; + + SomeReferencePosition refpos; refpos.latitude = microdegree(6, 21.23) * Latitude_oneMicrodegreeSouth; refpos.longitude = microdegree(33, 22.12) * Longitude_oneMicrodegreeWest; GeoAngle lat = -(6 + (22.48 / 60.0)) * degree; @@ -138,10 +164,12 @@ TEST(CamFunctions, distance_refpos_latlon) EXPECT_TRUE(NearDistance(distance(refpos, lat, lon), 2440.0 * si::meter , 10.0 * si::meter)); } -TEST(CamFunctions, distance_unavailable) +TYPED_TEST(CamFunctionsReferencePosition, distance_unavailable) { - ReferencePosition_t pos1 {}; - ReferencePosition_t pos2 {}; + using SomeReferencePosition = TypeParam; + + SomeReferencePosition pos1 {}; + SomeReferencePosition pos2 {}; EXPECT_TRUE(is_available(pos1)); EXPECT_TRUE(is_available(pos2)); EXPECT_FALSE(std::isnan(distance(pos1, pos2).value())); @@ -157,3 +185,26 @@ TEST(CamFunctions, distance_unavailable) EXPECT_TRUE(std::isnan(distance(pos1, pos2).value())); EXPECT_TRUE(std::isnan(distance(pos2, pos1).value())); } + +TYPED_TEST(CamFunctionsReferencePosition, copy) +{ + using SomeReferencePosition = TypeParam; + + PositionFix src; + src.latitude = 1.23 * vanetza::units::degree; + src.longitude = 4.56 * vanetza::units::degree; + src.confidence.orientation = vanetza::units::TrueNorth::from_value(10.0); + src.confidence.semi_major = 20 * vanetza::units::si::meter; + src.confidence.semi_minor = 15 * vanetza::units::si::meter; + + SomeReferencePosition dest; + copy(src, dest); + + EXPECT_EQ(dest.latitude, 123 * 100000); + EXPECT_EQ(dest.longitude, 456 * 100000); + EXPECT_EQ(dest.positionConfidenceEllipse.semiMajorConfidence, 20 * 100); + EXPECT_EQ(dest.positionConfidenceEllipse.semiMinorConfidence, 15 * 100); + EXPECT_EQ(dest.positionConfidenceEllipse.semiMajorOrientation, 10 * 10); + EXPECT_EQ(dest.altitude.altitudeValue, AltitudeValue_unavailable); + EXPECT_EQ(dest.altitude.altitudeConfidence, AltitudeConfidence_unavailable); +}