Keep the colleague's microbu-esp32c5 tree in this repository
obu-firmware builds against vanetza-idf from microbu-esp32c5/external, but that tree was gitignored, so a clone of this repository could not build the firmware it ships. It is now committed here as ordinary files in its own folder, microbu-esp32c5/: the colleague's commit cf4b99f plus the V2X2MAP bridge's signature verification (--trust) used on the bench. Nothing is fetched from or pushed to the colleague's repository; this repository and its remotes carry everything. The folder's own .gitignore keeps build output, downloaded components and private key material out, as it did there; the committed file set is identical to that repository's tracked files. The ESP32-C5 is still flashed from obu-firmware/, which only takes vanetza-idf from microbu-esp32c5/, so the two stay separate folders. FLASHING.md says how to take a newer version of the colleague's tree (copy it over the folder, rebuild, test, commit).
This commit is contained in:
Ashin Walpola
@@ -0,0 +1,375 @@
|
||||
#include <vanetza/asn1/its/CAM.h>
|
||||
#include <vanetza/asn1/its/r2/CAM.h>
|
||||
#include <vanetza/facilities/detail/macros.ipp>
|
||||
#include <iostream>
|
||||
|
||||
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) {
|
||||
for (size_t 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
|
||||
@@ -0,0 +1,49 @@
|
||||
#include <vanetza/asn1/its/Heading.h>
|
||||
#include <vanetza/asn1/its/r2/Heading.h>
|
||||
#include <vanetza/facilities/detail/macros.ipp>
|
||||
#include <vanetza/units/angle.hpp>
|
||||
|
||||
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
|
||||
@@ -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<ASN1_RELEASE1_NAME(name), ASN1_RELEASE2_NAME(name)>::value, \
|
||||
#name " type mismatch between release 1 and 2");
|
||||
+38
@@ -0,0 +1,38 @@
|
||||
#include <vanetza/asn1/asn1c_wrapper.hpp>
|
||||
#include <vanetza/asn1/its/BasicVehicleContainerLowFrequency.h>
|
||||
#include <vanetza/asn1/its/PathHistory.h>
|
||||
#include <vanetza/asn1/its/r2/BasicVehicleContainerLowFrequency.h>
|
||||
#include <vanetza/asn1/its/r2/Path.h>
|
||||
#include <vanetza/asn1/its/r2/PathHistory.h>
|
||||
#include <vanetza/facilities/detail/macros.ipp>
|
||||
#include <vanetza/facilities/detail/path_history.tpp>
|
||||
|
||||
namespace vanetza
|
||||
{
|
||||
namespace facilities
|
||||
{
|
||||
|
||||
static_assert(DeltaLongitude_oneMicrodegreeEast == 10, "DeltaLongitude is an integer number of tenth microdegrees");
|
||||
static_assert(DeltaLatitude_oneMicrodegreeNorth == 10, "DeltaLatitude is an integer number of tenth microdegrees");
|
||||
static_assert(PathDeltaTime_tenMilliSecondsInPast == 1, "PathDeltaTime encodes 10ms steps");
|
||||
|
||||
void copy(const facilities::PathHistory& src, ASN1_PREFIXED(PathHistory_t)& dest)
|
||||
{
|
||||
copy<ASN1_PREFIXED(PathHistory_t), ASN1_PREFIXED(PathPoint_t)>(src, dest);
|
||||
}
|
||||
|
||||
#if ITS_RELEASE != 1
|
||||
// no Path_t in ITS Release 1 ASN.1
|
||||
void copy(const facilities::PathHistory& src, ASN1_PREFIXED(Path_t)& dest)
|
||||
{
|
||||
copy<ASN1_PREFIXED(Path_t), ASN1_PREFIXED(PathPoint_t)>(src, dest);
|
||||
}
|
||||
#endif
|
||||
|
||||
void copy(const facilities::PathHistory& ph, ASN1_PREFIXED(BasicVehicleContainerLowFrequency)& container)
|
||||
{
|
||||
copy(ph, container.pathHistory);
|
||||
}
|
||||
|
||||
} // namespace facilities
|
||||
} // namespace vanetza
|
||||
+63
@@ -0,0 +1,63 @@
|
||||
#pragma once
|
||||
#include <vanetza/facilities/path_history.hpp>
|
||||
#include <vanetza/facilities/path_point.hpp>
|
||||
#include <chrono>
|
||||
#include <type_traits>
|
||||
|
||||
namespace vanetza
|
||||
{
|
||||
namespace facilities
|
||||
{
|
||||
|
||||
// C2C-CC BSP CAM trace limits (RS_BSP_318)
|
||||
static const units::Length cCamTraceMinLength = 200.0 * units::si::meter;
|
||||
static constexpr std::size_t cCamTraceMaxPoints = 23;
|
||||
|
||||
template<typename SomePathSequence, typename SomePathPoint>
|
||||
void copy(const facilities::PathHistory& src, SomePathSequence& dest,
|
||||
units::Length min_distance = cCamTraceMinLength, std::size_t max_points = cCamTraceMaxPoints)
|
||||
{
|
||||
using SomePathDeltaTime = typename std::remove_pointer<decltype(SomePathPoint::pathDeltaTime)>::type;
|
||||
|
||||
static const auto scDeltaTimeStepLength = boost::posix_time::milliseconds(10);
|
||||
static const auto scMaxDeltaTime = scDeltaTimeStepLength * 65535;
|
||||
|
||||
// ETSI TS 102 894-2: first PathPoint relative to the reference position, each subsequent
|
||||
// one relative to the previous PathPoint (incremental deltas); newest first (RS_BSP_287).
|
||||
facilities::PathPoint prev = src.getReferencePoint();
|
||||
bool first_point = true; // only the first PathPoint may be clamped (stationary marker)
|
||||
|
||||
for (const PathPoint& point : src.getConcisePointsMinLength(min_distance, max_points)) {
|
||||
auto delta_time = prev.time - point.time; // positive: point is in past
|
||||
auto delta_latitude = round(point.latitude - prev.latitude, tenth_microdegree);
|
||||
auto delta_longitude = round(point.longitude - prev.longitude, tenth_microdegree);
|
||||
|
||||
if (delta_latitude < -131071 || delta_latitude > 131071) {
|
||||
continue; // delta latitude not encodable
|
||||
} else if (delta_longitude < -131071 || delta_longitude > 131071) {
|
||||
continue; // delta longitude not encodable
|
||||
} else if (delta_time >= scDeltaTimeStepLength) {
|
||||
if (delta_time > scMaxDeltaTime) {
|
||||
if (first_point) {
|
||||
delta_time = scMaxDeltaTime; // RS_BSP_289: clamp the first PathPoint (stationary marker)
|
||||
} else {
|
||||
break; // later overflow (old stationary) drops the disconnected older trail
|
||||
}
|
||||
}
|
||||
SomePathPoint* path_point = asn1::allocate<SomePathPoint>();
|
||||
path_point->pathPosition.deltaLatitude = delta_latitude;
|
||||
path_point->pathPosition.deltaLongitude = delta_longitude;
|
||||
path_point->pathPosition.deltaAltitude = DeltaAltitude::DeltaAltitude_unavailable;
|
||||
|
||||
path_point->pathDeltaTime = asn1::allocate<SomePathDeltaTime>();
|
||||
*(path_point->pathDeltaTime) = delta_time.total_milliseconds() / scDeltaTimeStepLength.total_milliseconds();
|
||||
|
||||
ASN_SEQUENCE_ADD(&dest, path_point);
|
||||
prev = point;
|
||||
first_point = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace facilities
|
||||
} // namespace vanetza
|
||||
+96
@@ -0,0 +1,96 @@
|
||||
#include <vanetza/asn1/its/ReferencePosition.h>
|
||||
#include <vanetza/asn1/its/r2/ReferencePosition.h>
|
||||
#include <vanetza/facilities/detail/macros.ipp>
|
||||
#include <vanetza/geodesy/geodesy.hpp>
|
||||
#include <vanetza/units/length.hpp>
|
||||
#include <limits>
|
||||
|
||||
namespace vanetza
|
||||
{
|
||||
namespace facilities
|
||||
{
|
||||
|
||||
static_assert(Longitude_oneMicrodegreeEast == 10, "Longitude is an integer number of tenth microdegrees");
|
||||
static_assert(Latitude_oneMicrodegreeNorth == 10, "Latitude is an integer number of tenth microdegrees");
|
||||
|
||||
units::Length distance(const ASN1_PREFIXED(ReferencePosition_t)& a, const ASN1_PREFIXED(ReferencePosition_t)& b)
|
||||
{
|
||||
using geodesy::GeodeticPosition;
|
||||
using units::GeoAngle;
|
||||
|
||||
auto length = units::Length::from_value(std::numeric_limits<double>::quiet_NaN());
|
||||
if (is_available(a) && is_available(b)) {
|
||||
GeodeticPosition geo_a {
|
||||
GeoAngle { a.latitude * tenth_microdegree },
|
||||
GeoAngle { a.longitude * tenth_microdegree }
|
||||
};
|
||||
GeodeticPosition geo_b {
|
||||
GeoAngle { b.latitude * tenth_microdegree },
|
||||
GeoAngle { b.longitude * tenth_microdegree }
|
||||
};
|
||||
length = geodesy::distance(geo_a, geo_b);
|
||||
}
|
||||
return length;
|
||||
}
|
||||
|
||||
units::Length distance(const ASN1_PREFIXED(ReferencePosition_t)& a, units::GeoAngle lat, units::GeoAngle lon)
|
||||
{
|
||||
using geodesy::GeodeticPosition;
|
||||
using units::GeoAngle;
|
||||
|
||||
auto length = units::Length::from_value(std::numeric_limits<double>::quiet_NaN());
|
||||
if (is_available(a)) {
|
||||
GeodeticPosition geo_a {
|
||||
GeoAngle { a.latitude * tenth_microdegree },
|
||||
GeoAngle { a.longitude * tenth_microdegree }
|
||||
};
|
||||
GeodeticPosition geo_b { lat, lon };
|
||||
length = geodesy::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, tenth_microdegree);
|
||||
reference_position.latitude = round(position.latitude, tenth_microdegree);
|
||||
if (std::isfinite(position.confidence.semi_major.value())
|
||||
&& std::isfinite(position.confidence.semi_minor.value()))
|
||||
{
|
||||
if ((position.confidence.semi_major.value() * 100 < static_cast<long>(ASN1_PREFIXED(SemiAxisLength_outOfRange)))
|
||||
&& (position.confidence.semi_minor.value() * 100 < static_cast<long>(ASN1_PREFIXED(SemiAxisLength_outOfRange)))
|
||||
&& (position.confidence.orientation.value() * 10 < static_cast<long>(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
|
||||
Reference in New Issue
Block a user