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).

microbu-esp32c5/external/vanetza-idf/vanetza/facilities/CMakeLists.txt

@@ -0,0 +1,12 @@
+set(CXX_SOURCES
+    cam_functions.cpp
+    path_history.cpp
+    path_point.cpp
+)
+
+add_vanetza_component(facilities ${CXX_SOURCES})
+target_link_libraries(facilities PUBLIC Boost::date_time asn1 security)
+target_link_libraries(facilities PRIVATE geodesy)
+
+add_test_subdirectory(tests)
+

microbu-esp32c5/external/vanetza-idf/vanetza/facilities/cam_functions.cpp

@@ -0,0 +1,134 @@
+#include <vanetza/asn1/cam.hpp>
+#include <vanetza/facilities/cam_functions.hpp>
+#include <boost/algorithm/clamp.hpp>
+#include <boost/math/constants/constants.hpp>
+#include <boost/units/cmath.hpp>
+#include <boost/units/systems/si/prefixes.hpp>
+#include <boost/units/systems/angle/degrees.hpp>
+
+namespace vanetza
+{
+namespace facilities
+{
+
+using vanetza::units::Angle;
+
+static const auto microdegree = units::degree * units::si::micro;
+static const auto tenth_microdegree = units::si::deci * microdegree;
+
+bool similar_heading(Angle a, Angle b, Angle limit)
+{
+    using namespace boost::units;
+    using boost::math::double_constants::pi;
+
+    static const Angle full_circle = 2.0 * pi * si::radian;
+    const Angle abs_diff = fmod(abs(a - b), full_circle);
+    return abs_diff <= limit || abs_diff >= full_circle - limit;
+}
+
+template<typename T, typename U>
+long round(const boost::units::quantity<T>& q, const U&)
+{
+    boost::units::quantity<U> v { q };
+    return std::round(v.value());
+}
+
+AltitudeConfidence_t to_altitude_confidence(units::Length confidence)
+{
+    const double alt_con = confidence / units::si::meter;
+
+    if (alt_con < 0 || std::isnan(alt_con)) {
+        return AltitudeConfidence_unavailable;
+    } else if (alt_con <= 0.01) {
+        return AltitudeConfidence_alt_000_01;
+    } else if (alt_con <= 0.02) {
+        return AltitudeConfidence_alt_000_02;
+    } else if (alt_con <= 0.05) {
+        return AltitudeConfidence_alt_000_05;
+    } else if (alt_con <= 0.1) {
+        return AltitudeConfidence_alt_000_10;
+    } else if (alt_con <= 0.2) {
+        return AltitudeConfidence_alt_000_20;
+    } else if (alt_con <= 0.5) {
+        return AltitudeConfidence_alt_000_50;
+    } else if (alt_con <= 1.0) {
+        return AltitudeConfidence_alt_001_00;
+    } else if (alt_con <= 2.0) {
+        return AltitudeConfidence_alt_002_00;
+    } else if (alt_con <= 5.0) {
+        return AltitudeConfidence_alt_005_00;
+    } else if (alt_con <= 10.0) {
+        return AltitudeConfidence_alt_010_00;
+    } else if (alt_con <= 20.0) {
+        return AltitudeConfidence_alt_020_00;
+    } else if (alt_con <= 50.0) {
+        return AltitudeConfidence_alt_050_00;
+    } else if (alt_con <= 100.0) {
+        return AltitudeConfidence_alt_100_00;
+    } else if (alt_con <= 200.0) {
+        return AltitudeConfidence_alt_200_00;
+    } else {
+        return AltitudeConfidence_outOfRange;
+    }
+}
+
+AltitudeValue_t to_altitude_value(units::Length alt)
+{
+    using boost::units::isnan;
+    static_assert(AltitudeValue_oneCentimeter == 1, "AltitudeValue encodes an integer number of centimeters");
+
+    if (!isnan(alt)) {
+        alt = boost::algorithm::clamp(alt, -1000.0 * units::si::meter, 8000.0 * units::si::meter);
+        return round(alt, units::si::centi * units::si::meter);
+    } else {
+        return AltitudeValue_unavailable;
+    }
+}
+
+} // namespace facilities
+} // namespace vanetza
+
+#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"
+
+#undef ASN1_PREFIX
+#undef ITS_RELEASE
+
+#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"
+
+namespace vanetza
+{
+namespace facilities
+{
+
+bool check_service_specific_permissions(const asn1::r1::Cam& cam, security::CamPermissions ssp)
+{
+    return check_service_specific_permissions(cam->cam.camParameters, ssp);
+}
+
+bool check_service_specific_permissions(const asn1::r2::Cam& cam, security::CamPermissions ssp)
+{
+    return check_service_specific_permissions(cam->cam.camParameters, ssp);
+}
+
+void print_indented(std::ostream& os, const asn1::r1::Cam& cam, const std::string& indent, unsigned start)
+{
+    print_indented(os, cam.content(), indent, start);
+}
+
+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
+} // namespace vanetza

microbu-esp32c5/external/vanetza-idf/vanetza/facilities/cam_functions.hpp

@@ -0,0 +1,133 @@
+#ifndef CAM_FUNCTIONS_HPP_PUFKBEM8
+#define CAM_FUNCTIONS_HPP_PUFKBEM8
+
+#include <vanetza/asn1/its/AltitudeConfidence.h>
+#include <vanetza/asn1/its/AltitudeValue.h>
+#include <vanetza/common/position_fix.hpp>
+#include <vanetza/security/cam_ssp.hpp>
+#include <vanetza/units/angle.hpp>
+#include <vanetza/units/length.hpp>
+
+// forward declaration of asn1c generated struct
+struct BasicVehicleContainerLowFrequency;
+struct Heading;
+struct PathHistory;
+struct ReferencePosition;
+struct Vanetza_ITS2_BasicVehicleContainerLowFrequency;
+struct Vanetza_ITS2_Heading;
+struct Vanetza_ITS2_Path;
+struct Vanetza_ITS2_PathHistory;
+struct Vanetza_ITS2_ReferencePosition;
+
+namespace vanetza
+{
+
+// forward declaration of CAM message wrappers
+namespace asn1 {
+    namespace r1 { class Cam; }
+    namespace r2 { class Cam; }
+}
+
+namespace facilities
+{
+
+class PathHistory;
+
+/**
+ * Copy PathHistory into BasicVehicleContainerLowFrequency's pathHistory element
+ * \deprecated use function with PathHistory destination instead
+ * \param Facilities' path history object (source)
+ * \param ASN.1 CAM container (destination)
+ */
+void copy(const PathHistory&, BasicVehicleContainerLowFrequency&);
+void copy(const PathHistory&, Vanetza_ITS2_BasicVehicleContainerLowFrequency&);
+
+/**
+ * Copy facilities::PathHistory into an ASN.1 PathHistory structure
+ * 
+ * \param src source path history
+ * \param dest destination path history
+ */
+void copy(const PathHistory& src, ::PathHistory&);
+void copy(const PathHistory& src, Vanetza_ITS2_PathHistory&);
+void copy(const PathHistory& src, Vanetza_ITS2_Path&);
+
+/**
+ * Check if difference of two given heading values is within a limit
+ * \param a one heading
+ * \param b another heading
+ * \param limit maximum difference (positive)
+ * \return true if similar enough
+ */
+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);
+
+/**
+ * Calculate distance between positions
+ * \param a one position
+ * \param b another position
+ * \return distance between given positions (or NaN if some position is unavailable)
+ */
+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 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);
+
+/**
+ * Check if a CAM contains only allowed data elements
+ * \param cam CA message
+ * \param ssp CA service specific permissions
+ * \return true if no forbidden data elements are included
+ */
+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
+ * \param os output stream
+ * \param cam CA message
+ * \param indent indentation marker, by default one tab per level
+ * \param start initial level of indentation
+ *
+ * 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::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
+
+#endif /* CAM_FUNCTIONS_HPP_PUFKBEM8 */

microbu-esp32c5/external/vanetza-idf/vanetza/facilities/detail/cam.ipp

@@ -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

microbu-esp32c5/external/vanetza-idf/vanetza/facilities/detail/heading.ipp

@@ -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

microbu-esp32c5/external/vanetza-idf/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<ASN1_RELEASE1_NAME(name), ASN1_RELEASE2_NAME(name)>::value, \
+    #name " type mismatch between release 1 and 2");

microbu-esp32c5/external/vanetza-idf/vanetza/facilities/detail/path_history.ipp

@@ -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

microbu-esp32c5/external/vanetza-idf/vanetza/facilities/detail/path_history.tpp

@@ -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

microbu-esp32c5/external/vanetza-idf/vanetza/facilities/detail/reference_position.ipp

@@ -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

microbu-esp32c5/external/vanetza-idf/vanetza/facilities/path_history.cpp

@@ -0,0 +1,161 @@
+#include <vanetza/facilities/path_history.hpp>
+#include <vanetza/units/angle.hpp>
+#include <vanetza/units/length.hpp>
+#include <boost/units/cmath.hpp>
+#include <cassert>
+
+namespace vanetza {
+namespace facilities {
+
+const units::Length cTraceAllowableError = 0.47 * units::si::meter;
+const units::Length cTraceMaxDeltaDistance = 22.5 * units::si::meter;
+const units::Angle cTraceDeltaPhi = units::Angle(1.0 * units::degree);
+
+PathHistory::PathHistory() : PathHistory(Parameters{})
+{
+}
+
+PathHistory::PathHistory(const Parameters& params) :
+    m_params(params), m_samples(3)
+{
+}
+
+const PathPoint& PathHistory::starting() const
+{
+    assert(!m_concise.empty());
+    return m_concise.front();
+}
+
+const PathPoint& PathHistory::previous() const
+{
+    assert(m_samples.size() > 1);
+    return m_samples[1];
+}
+
+const PathPoint& PathHistory::next() const
+{
+    assert(!m_samples.empty());
+    return m_samples.front();
+}
+
+void PathHistory::addSample(const PathPoint& point)
+{
+    m_samples.push_front(point);
+    if (m_concise.empty()) {
+        m_concise.push_front(m_samples.front());
+    }
+
+    updateConcisePoints();
+    truncateConcisePoints();
+}
+
+void PathHistory::clear()
+{
+    m_samples.clear();
+    m_concise.clear();
+}
+
+const PathPoint& PathHistory::getReferencePoint() const
+{
+    static const PathPoint scDefaultPathPoint = PathPoint();
+
+    if (m_samples.empty()) {
+        return scDefaultPathPoint;
+    } else {
+        return m_samples.front();
+    }
+}
+
+void PathHistory::updateConcisePoints()
+{
+    if (m_samples.full()) {
+        const auto actual_chord_length = chord_length(starting(), next());
+        units::Length actual_error;
+        if (actual_chord_length > m_params.chord_length_threshold) {
+            actual_error = m_params.allowable_error + 1.0 * units::si::meter;
+        } else {
+            const units::Angle delta_phi = next().heading - starting().heading;
+            if (abs(delta_phi) < m_params.small_delta_phi) {
+                actual_error = 0.0 * units::si::meter;
+            } else {
+                const units::Length estimated_radius = actual_chord_length / (2 * sin(delta_phi * 0.5));
+                const units::Length d = estimated_radius * cos(0.5 * delta_phi);
+                actual_error = estimated_radius - d;
+            }
+        }
+
+        if (actual_error > m_params.allowable_error) {
+            m_concise.push_front(previous());
+        }
+    }
+}
+
+void PathHistory::truncateConcisePoints()
+{
+    units::Length distance = 0.0 * units::si::meter;
+    if (m_concise.size() > 2) {
+        auto previous = m_concise.begin();
+        auto current = ++m_concise.begin();
+        for (; current != m_concise.end(); ++previous, ++current) {
+            distance += chord_length(*previous, *current);
+            if (distance >= m_params.retention_distance) {
+                m_concise.erase(++current, m_concise.end());
+                break;
+            }
+        }
+    }
+}
+
+boost::iterator_range<std::list<PathPoint>::const_iterator>
+PathHistory::getConcisePointsMinLength(units::Length distance) const
+{
+    return getConcisePointsMinLength(distance, m_concise.size());
+}
+
+boost::iterator_range<std::list<PathPoint>::const_iterator>
+PathHistory::getConcisePointsMinLength(units::Length distance, std::size_t max_points) const
+{
+    units::Length covered = 0.0 * units::si::meter;
+    std::size_t count = 0;
+    const PathPoint* previous = nullptr;
+    auto cut = m_concise.begin();
+    for (; cut != m_concise.end() && count < max_points; ++cut, ++count) {
+        if (previous != nullptr) {
+            covered += chord_length(*previous, *cut);
+        }
+        previous = &*cut;
+        if (covered >= distance) {
+            ++cut; // include the point reaching the distance
+            break;
+        }
+    }
+    return { m_concise.begin(), cut };
+}
+
+boost::iterator_range<std::list<PathPoint>::const_iterator>
+PathHistory::getConcisePointsMaxLength(units::Length distance) const
+{
+    return getConcisePointsMaxLength(distance, m_concise.size());
+}
+
+boost::iterator_range<std::list<PathPoint>::const_iterator>
+PathHistory::getConcisePointsMaxLength(units::Length distance, std::size_t max_points) const
+{
+    units::Length covered = 0.0 * units::si::meter;
+    std::size_t count = 0;
+    const PathPoint* previous = nullptr;
+    auto cut = m_concise.begin();
+    for (; cut != m_concise.end() && count < max_points; ++cut, ++count) {
+        if (previous != nullptr) {
+            covered += chord_length(*previous, *cut);
+            if (covered > distance) {
+                break; // exclude the point beyond the distance
+            }
+        }
+        previous = &*cut;
+    }
+    return { m_concise.begin(), cut };
+}
+
+} // namespace facilities
+} // namespace vanetza

microbu-esp32c5/external/vanetza-idf/vanetza/facilities/path_history.hpp

@@ -0,0 +1,103 @@
+#ifndef PATH_HISTORY_HPP_1ITSMS5I
+#define PATH_HISTORY_HPP_1ITSMS5I
+
+#include <vanetza/facilities/path_point.hpp>
+#include <boost/circular_buffer.hpp>
+#include <boost/range/iterator_range.hpp>
+#include <cstddef>
+#include <list>
+
+namespace vanetza
+{
+namespace facilities
+{
+
+// C2C-CC BSP RS_BSP_318 path history (Method One) parameters
+extern const units::Length cTraceAllowableError;
+extern const units::Length cTraceMaxDeltaDistance;
+extern const units::Angle cTraceDeltaPhi;
+
+/**
+ * Implementation of Path History Reference Design (Method One)
+ * \see NHTSA Document "VSC-A Final Report: Appendix B-2" from September 2011
+ */
+class PathHistory
+{
+public:
+    struct Parameters
+    {
+        units::Length allowable_error = cTraceAllowableError;
+        units::Length chord_length_threshold = cTraceMaxDeltaDistance;
+        units::Angle small_delta_phi = cTraceDeltaPhi;
+        units::Length retention_distance = 500.0 * units::si::meter;
+    };
+
+    PathHistory();
+    explicit PathHistory(const Parameters& params);
+
+    /**
+     * Consider one further path point for inclusion into path history
+     * \param a path point, expected to be newer than any previously given point
+     */
+    void addSample(const PathPoint&);
+
+    /**
+     * Drop all samples and concise points, e.g. on pseudonym (AT) change
+     */
+    void clear();
+
+    /**
+     * Get current reference point, i.e. last provided path point
+     * \return current reference point (fallback is a default constructed PathPoint)
+     */
+    const PathPoint& getReferencePoint() const;
+
+    /**
+     * Get concise list of path points
+     * \note previously given path points are only included if the algorithm
+     *       presented in above mentioned document as "Method One" selects them
+     * \return list of path points, some given points might be omitted
+     */
+    const std::list<PathPoint>& getConcisePoints() const { return m_concise; }
+
+    /**
+     * Newest concise points covering at least a distance (crossing point included)
+     * \param distance minimum distance to cover
+     * \return view of the newest concise points
+     */
+    boost::iterator_range<std::list<PathPoint>::const_iterator>
+    getConcisePointsMinLength(units::Length distance) const;
+
+    /// As above but capped at max_points
+    boost::iterator_range<std::list<PathPoint>::const_iterator>
+    getConcisePointsMinLength(units::Length distance, std::size_t max_points) const;
+
+    /**
+     * Newest concise points covering at most a distance (crossing point excluded)
+     * \param distance maximum distance to cover
+     * \return view of the newest concise points
+     */
+    boost::iterator_range<std::list<PathPoint>::const_iterator>
+    getConcisePointsMaxLength(units::Length distance) const;
+
+    /// As above but capped at max_points
+    boost::iterator_range<std::list<PathPoint>::const_iterator>
+    getConcisePointsMaxLength(units::Length distance, std::size_t max_points) const;
+
+private:
+    void updateConcisePoints();
+    void truncateConcisePoints();
+    const PathPoint& starting() const;
+    const PathPoint& previous() const;
+    const PathPoint& next() const;
+
+    Parameters m_params;
+    boost::circular_buffer<PathPoint> m_samples;
+    std::list<PathPoint> m_concise;
+};
+
+} // namespace facilities
+} // namespace vanetza
+
+#endif /* PATH_HISTORY_HPP_1ITSMS5I */
+

microbu-esp32c5/external/vanetza-idf/vanetza/facilities/path_point.cpp

@@ -0,0 +1,29 @@
+#include <vanetza/facilities/path_point.hpp>
+#include <boost/units/cmath.hpp>
+#include <boost/units/systems/si/prefixes.hpp>
+
+namespace vanetza
+{
+namespace facilities
+{
+
+const units::Length cREarthMeridian = units::Length(6378.137 * units::si::kilo * units::si::meters);
+
+PathPoint::PathPoint()
+{
+}
+
+units::Length chord_length(const PathPoint& a, const PathPoint& b)
+{
+    const units::Angle lat1(a.latitude);
+    const units::Angle lon1(a.longitude);
+    const units::Angle lat2(b.latitude);
+    const units::Angle lon2(b.longitude);
+
+    return cREarthMeridian *
+        acos(cos(lat1) * cos(lat2) * cos(lon1 - lon2) + sin(lat1) * sin(lat2))
+        / units::si::radian ;
+}
+
+} // namespace facilities
+} // namespace vanetza

microbu-esp32c5/external/vanetza-idf/vanetza/facilities/path_point.hpp

@@ -0,0 +1,37 @@
+#ifndef PATH_POINT_HPP_LXQ9YZKI
+#define PATH_POINT_HPP_LXQ9YZKI
+
+#include <vanetza/units/angle.hpp>
+#include <vanetza/units/length.hpp>
+#include <boost/date_time/posix_time/posix_time.hpp>
+
+namespace vanetza
+{
+namespace facilities
+{
+
+extern const units::Length cREarthMeridian;
+
+struct PathPoint
+{
+    PathPoint();
+
+    units::GeoAngle latitude;
+    units::GeoAngle longitude;
+    units::Angle heading;
+    boost::posix_time::ptime time;
+};
+
+/**
+ * Calculate chord length between two points
+ * \param path point A
+ * \param path point B
+ * \return chord length
+ */
+units::Length chord_length(const PathPoint&, const PathPoint&);
+
+} // namespace facilities
+} // namespace vanetza
+
+#endif /* PATH_POINT_HPP_LXQ9YZKI */
+

microbu-esp32c5/external/vanetza-idf/vanetza/facilities/tests/CMakeLists.txt

@@ -0,0 +1,6 @@
+include(UseGTest)
+configure_gtest_directory(LINK_LIBRARIES facilities)
+
+add_gtest(CamFunctions cam_functions.cpp LINK_LIBRARIES asn1_its_r2)
+add_gtest(PathHistory path_history.cpp)
+add_gtest(PathPoint path_point.cpp)

microbu-esp32c5/external/vanetza-idf/vanetza/facilities/tests/cam_functions.cpp

@@ -0,0 +1,340 @@
+#include <gtest/gtest.h>
+#include <vanetza/facilities/path_history.hpp>
+#include <vanetza/asn1/type_traits.hpp>
+#include <vanetza/asn1/its/Heading.h>
+#include <vanetza/asn1/its/PathHistory.h>
+#include <vanetza/asn1/its/ReferencePosition.h>
+#include <vanetza/asn1/its/r2/Heading.h>
+#include <vanetza/asn1/its/r2/PathHistory.h>
+#include <vanetza/asn1/its/r2/ReferencePosition.h>
+#include <vanetza/facilities/cam_functions.hpp>
+#include <boost/units/cmath.hpp>
+#include <boost/units/io.hpp>
+#include <cmath>
+
+namespace vanetza
+{
+namespace asn1
+{
+
+template<> struct asn1_type_traits<::PathHistory>
+{
+    static asn_TYPE_descriptor_t& descriptor() { return ::asn_DEF_PathHistory; }
+};
+
+template<> struct asn1_type_traits<::Vanetza_ITS2_PathHistory>
+{
+    static asn_TYPE_descriptor_t& descriptor() { return ::asn_DEF_Vanetza_ITS2_PathHistory; }
+};
+
+} // namespace asn1
+} // namespace vanetza
+
+using namespace vanetza;
+using namespace vanetza::facilities;
+using namespace vanetza::units;
+
+constexpr long latitude(double degree, double arc_minute)
+{
+    return std::round(1e7 * (degree + arc_minute / 60.0));
+}
+
+constexpr long longitude(double degree, double arc_minute)
+{
+    return std::round(1e7 * (degree + arc_minute / 60.0));
+}
+
+::testing::AssertionResult NearDistance(const Length& a, const Length& b, Length delta)
+{
+    using namespace boost::units;
+    const auto diff = abs(a - b);
+    if (diff < delta) {
+        return ::testing::AssertionSuccess();
+    } else {
+        return ::testing::AssertionFailure() << "actual difference " << diff << " exceeds delta of " << delta;
+    }
+}
+
+using HeadingTypes = ::testing::Types<Heading, Vanetza_ITS2_Heading>;
+template<typename T>
+class CamFunctionsHeading : public ::testing::Test
+{
+};
+TYPED_TEST_SUITE(CamFunctionsHeading, HeadingTypes);
+
+TEST(CamFunctionsHeading, similar_heading)
+{
+    Angle a = 3 * si::radian;
+    Angle b = 2 * si::radian;
+    Angle limit = 0.5 * si::radian;
+    EXPECT_FALSE(similar_heading(a, b, limit));
+    EXPECT_FALSE(similar_heading(b, a, limit));
+
+    limit = 1.0 * si::radian;
+    EXPECT_TRUE(similar_heading(a, b, limit));
+    EXPECT_TRUE(similar_heading(b, a, limit));
+
+    a = 6.1 * si::radian;
+    b = 0.2 * si::radian;
+    limit = 0.4 * si::radian;
+    EXPECT_TRUE(similar_heading(a, b, limit));
+    EXPECT_TRUE(similar_heading(b, a, limit));
+
+    limit = 0.3 * si::radian;
+    EXPECT_FALSE(similar_heading(a, b, limit));
+    EXPECT_FALSE(similar_heading(b, a, limit));
+
+    limit = -1.0 * si::radian;
+    EXPECT_FALSE(similar_heading(a, a, limit));
+}
+
+TYPED_TEST(CamFunctionsHeading, similar_heading_unavailable1)
+{
+    using SomeHeading = TypeParam;
+
+    SomeHeading a;
+    a.headingValue = HeadingValue_unavailable;
+    Angle b = 2.0 * si::radian;
+    Angle limit = 10 * si::radian;
+    EXPECT_FALSE(is_available(a));
+    EXPECT_FALSE(similar_heading(a, b, limit));
+
+    a.headingValue = 2 * HeadingValue_wgs84East;
+    EXPECT_TRUE(is_available(a));
+    EXPECT_TRUE(similar_heading(a, b, limit));
+
+    b = 0.0 * si::radian;
+    limit = 3.14 * si::radian;
+    EXPECT_FALSE(similar_heading(a, b, limit));
+
+    limit = 3.15 * si::radian;
+    EXPECT_TRUE(similar_heading(a, b, limit));
+}
+
+TYPED_TEST(CamFunctionsHeading, similar_heading_unavailable2)
+{
+    using SomeHeading = TypeParam;
+
+    SomeHeading a;
+    a.headingValue = HeadingValue_unavailable;
+    SomeHeading b;
+    b.headingValue = HeadingValue_unavailable;
+    Angle limit = 10 * si::radian;
+    EXPECT_FALSE(is_available(a));
+    EXPECT_FALSE(is_available(b));
+    EXPECT_FALSE(similar_heading(a, b, limit));
+
+    b.headingValue = 200;
+    EXPECT_TRUE(is_available(b));
+    EXPECT_FALSE(similar_heading(a, b, limit));
+    EXPECT_FALSE(similar_heading(b, a, limit));
+
+    a.headingValue = 300;
+    EXPECT_TRUE(is_available(a));
+    EXPECT_TRUE(similar_heading(a, b, limit));
+    EXPECT_TRUE(similar_heading(b, a, limit));
+}
+
+using ReferencePositionTypes = ::testing::Types<ReferencePosition_t, Vanetza_ITS2_ReferencePosition_t>;
+template<typename T>
+class CamFunctionsReferencePosition : public ::testing::Test
+{
+};
+TYPED_TEST_SUITE(CamFunctionsReferencePosition, ReferencePositionTypes);
+
+TYPED_TEST(CamFunctionsReferencePosition, distance_reference_positions)
+{
+    using SomeReferencePosition = TypeParam;
+
+    SomeReferencePosition pos1;
+    pos1.latitude = -latitude(6, 21.23);
+    pos1.longitude = -longitude(33, 22.12);
+    SomeReferencePosition pos2;
+    pos2.latitude = -latitude(6, 22.48);
+    pos2.longitude = -longitude(33, 22.55);
+    EXPECT_TRUE(NearDistance(distance(pos1, pos2), 2440.0 * si::meter , 10.0 * si::meter));
+
+    SomeReferencePosition pos3;
+    pos3.latitude = latitude(37, 17.3);
+    pos3.longitude = -longitude(0, 13.14);
+    SomeReferencePosition pos4;
+    pos4.latitude = latitude(37, 17.19);
+    pos4.longitude = longitude(0, 9.45);
+    EXPECT_TRUE(NearDistance(distance(pos3, pos4), 33390.0 * si::meter , 100.0 * si::meter));
+
+    SomeReferencePosition pos5;
+    pos5.latitude = -latitude(0, 19.24);
+    pos5.longitude = longitude(83, 37.32);
+    SomeReferencePosition pos6;
+    pos6.latitude = latitude(0, 27.15);
+    pos6.longitude = longitude(83, 04.45);
+    EXPECT_TRUE(NearDistance(distance(pos5, pos6), 105010.0 * si::meter , 300.0 * si::meter));
+
+    SomeReferencePosition pos7;
+    pos7.latitude = latitude(48, 45.56);
+    pos7.longitude = longitude(11, 26.01);
+    SomeReferencePosition pos8;
+    pos8.latitude = latitude(48, 45.566);
+    pos8.longitude = longitude(11, 26.04);
+    EXPECT_TRUE(NearDistance(distance(pos7, pos8), 38.0 * si::meter , 0.5 * si::meter));
+}
+
+TYPED_TEST(CamFunctionsReferencePosition, distance_refpos_latlon)
+{
+    using SomeReferencePosition = TypeParam;
+
+    SomeReferencePosition refpos;
+    refpos.latitude = -latitude(6, 21.23);
+    refpos.longitude = -longitude(33, 22.12);
+    GeoAngle lat = -(6 + (22.48 / 60.0)) * degree;
+    GeoAngle lon = -(33 + (22.55 / 60.0)) * degree;
+    EXPECT_TRUE(NearDistance(distance(refpos, lat, lon), 2440.0 * si::meter , 10.0 * si::meter));
+}
+
+TYPED_TEST(CamFunctionsReferencePosition, distance_unavailable)
+{
+    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()));
+
+    pos1.latitude = Latitude_unavailable;
+    EXPECT_FALSE(is_available(pos1));
+    EXPECT_TRUE(std::isnan(distance(pos1, pos2).value()));
+    EXPECT_TRUE(std::isnan(distance(pos2, pos1).value()));
+
+    pos1.latitude = 0;
+    pos1.longitude = Longitude_unavailable;
+    EXPECT_FALSE(is_available(pos1));
+    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);
+}
+
+using PathHistoryTypes = ::testing::Types<::PathHistory, Vanetza_ITS2_PathHistory>;
+template<typename T>
+class CamFunctionsPathHistory : public ::testing::Test
+{
+protected:
+    vanetza::facilities::PathHistory path_history;
+
+    void add_sample(double lat, double lon, const std::string& time)
+    {
+        vanetza::facilities::PathPoint path_point;
+        path_point.latitude = lat * degree;
+        path_point.longitude = lon * degree;
+        path_point.time = boost::posix_time::from_iso_string(time);
+        path_history.addSample(path_point);
+    }
+};
+TYPED_TEST_SUITE(CamFunctionsPathHistory, PathHistoryTypes);
+
+TYPED_TEST(CamFunctionsPathHistory, copy_path_history)
+{
+    using SomePathHistory = TypeParam;
+
+    this->add_sample(40.906, 29.155, "20241027T031000");
+    this->add_sample(40.907, 29.156, "20241027T031010");
+    this->add_sample(40.908, 29.157, "20241027T031020");
+
+    SomePathHistory dest_path_history = {}; // zero-initialize struct
+    copy(this->path_history, dest_path_history);
+
+    int size = dest_path_history.list.count;
+    EXPECT_EQ(size, 2);
+    // incremental encoding: each point relative to the previous
+    EXPECT_EQ(dest_path_history.list.array[0]->pathPosition.deltaLatitude,
+              dest_path_history.list.array[1]->pathPosition.deltaLatitude);
+    EXPECT_EQ(*dest_path_history.list.array[0]->pathDeltaTime, 1000); // 10 s
+    EXPECT_EQ(*dest_path_history.list.array[1]->pathDeltaTime, 1000); // 10 s
+    for (int i = 0; i < size; i++) {
+        auto current_path_point = dest_path_history.list.array[i];
+        ASSERT_NE(current_path_point->pathDeltaTime, nullptr);
+
+        // check ASN.1 constraints
+        EXPECT_GE(*current_path_point->pathDeltaTime, 1);
+        EXPECT_LE(*current_path_point->pathDeltaTime, 65535);
+        EXPECT_GE(current_path_point->pathPosition.deltaLatitude, -131071);
+        EXPECT_LE(current_path_point->pathPosition.deltaLatitude, 131072);
+        EXPECT_GE(current_path_point->pathPosition.deltaLongitude, -131071);
+        EXPECT_LE(current_path_point->pathPosition.deltaLongitude, 131072);
+        EXPECT_GE(current_path_point->pathPosition.deltaAltitude, -12700);
+        EXPECT_LE(current_path_point->pathPosition.deltaAltitude, 12800);
+    }
+
+    asn1::reset(dest_path_history);
+}
+
+TYPED_TEST(CamFunctionsPathHistory, copy_clamps_stationary_delta_time)
+{
+    using SomePathHistory = TypeParam;
+
+    this->add_sample(40.906, 29.155, "20241027T031000");
+    this->add_sample(40.907, 29.156, "20241027T031010");
+    this->add_sample(40.908, 29.157, "20241027T031020");
+    // parked ~20 min at the last position: reference far past the max PathDeltaTime
+    this->add_sample(40.908, 29.157, "20241027T033000");
+
+    SomePathHistory dest = {}; // zero-initialize struct
+    copy(this->path_history, dest);
+
+    ASSERT_EQ(dest.list.count, 3); // whole trace kept alive
+    ASSERT_NE(dest.list.array[0]->pathDeltaTime, nullptr);
+    EXPECT_EQ(*dest.list.array[0]->pathDeltaTime, 65535); // only the first point is clamped
+    for (int i = 1; i < dest.list.count; i++) {
+        ASSERT_NE(dest.list.array[i]->pathDeltaTime, nullptr);
+        EXPECT_LT(*dest.list.array[i]->pathDeltaTime, 65535); // later points keep their short gaps
+    }
+
+    asn1::reset(dest);
+}
+
+TYPED_TEST(CamFunctionsPathHistory, copy_truncates_after_long_stop)
+{
+    using SomePathHistory = TypeParam;
+
+    // moved, parked ~21 min, then resumed: the stop is a >655 s gap in the middle
+    this->add_sample(40.9000, 29.1500, "20241027T031000"); // pre-stop trail
+    this->add_sample(40.9005, 29.1505, "20241027T031010");
+    this->add_sample(40.9010, 29.1510, "20241027T031020");
+    this->add_sample(40.9015, 29.1515, "20241027T033100"); // resumed ~21 min later
+    this->add_sample(40.9020, 29.1520, "20241027T033110");
+    this->add_sample(40.9025, 29.1525, "20241027T033120");
+
+    SomePathHistory dest = {}; // zero-initialize struct
+    copy(this->path_history, dest);
+
+    // trail is cut at the discontinuity, not carried across it with a clamped mid gap
+    ASSERT_GT(dest.list.count, 0);
+    for (int i = 0; i < dest.list.count; i++) {
+        ASSERT_NE(dest.list.array[i]->pathDeltaTime, nullptr);
+        EXPECT_LT(*dest.list.array[i]->pathDeltaTime, 65535);
+    }
+
+    asn1::reset(dest);
+}

microbu-esp32c5/external/vanetza-idf/vanetza/facilities/tests/path_history.cpp

@@ -0,0 +1,223 @@
+#include <vanetza/facilities/path_history.hpp>
+#include <gtest/gtest.h>
+
+using vanetza::facilities::PathHistory;
+using vanetza::facilities::PathPoint;
+namespace units = vanetza::units;
+
+#define EXPECT_PATHPOINT_EQ(a, b) \
+    EXPECT_DOUBLE_EQ(a.latitude.value(), b.latitude.value()); \
+    EXPECT_DOUBLE_EQ(a.longitude.value(), b.longitude.value()); \
+    EXPECT_DOUBLE_EQ(a.heading.value(), b.heading.value()); \
+    EXPECT_EQ(a.time, b.time)
+
+const units::GeoAngle cOneMeterLatitude = 1.0 / 111320.0 * units::degrees;
+
+TEST(PathHistory, reference_point) {
+    PathHistory ph;
+    const PathPoint default_pp;
+    EXPECT_PATHPOINT_EQ(default_pp, ph.getReferencePoint());
+
+    PathPoint first_pp;
+    first_pp.longitude = 34.4 * units::degrees;
+    first_pp.latitude = -10.3 * units::degrees;
+    first_pp.heading = units::Angle { 48.3 * units::degrees };
+    first_pp.time = boost::posix_time::time_from_string("2014-11-21 11:11:48");
+
+    ph.addSample(first_pp);
+    EXPECT_PATHPOINT_EQ(first_pp, ph.getReferencePoint());
+
+    PathPoint second_pp;
+    second_pp.longitude = 34.4 * units::degrees;
+    second_pp.latitude = -10.3 * units::degrees;
+    second_pp.heading = units::Angle { 48.3 * units::degrees };
+    second_pp.time = boost::posix_time::time_from_string("2014-11-21 11:11:48.1");
+
+    ph.addSample(second_pp);
+    EXPECT_PATHPOINT_EQ(second_pp, ph.getReferencePoint());
+}
+
+TEST(PathHistory, concise_points_of_equal_samples) {
+    PathHistory ph;
+    EXPECT_EQ(0, ph.getConcisePoints().size());
+
+    PathPoint first_pp;
+    first_pp.longitude = -3.45 * units::degrees;
+    first_pp.latitude = 32.98 * units::degrees;
+    ph.addSample(first_pp);
+    ASSERT_EQ(1, ph.getConcisePoints().size());
+    EXPECT_PATHPOINT_EQ(first_pp, ph.getConcisePoints().front());
+
+    for (unsigned i = 0; i < 10; ++i) {
+        ph.addSample(first_pp);
+        EXPECT_EQ(1, ph.getConcisePoints().size());
+    }
+}
+
+TEST(PathHistory, concise_points_chord_length_threshold) {
+    PathHistory ph;
+
+    PathPoint pp;
+    pp.longitude = 0.0 * units::degrees;
+    pp.latitude = 0.0  * units::degrees;
+    ph.addSample(pp);
+    pp.latitude += cOneMeterLatitude;
+    ph.addSample(pp);
+    pp.latitude += cOneMeterLatitude;
+    ph.addSample(pp);
+    EXPECT_EQ(1, ph.getConcisePoints().size());
+
+    pp.latitude += cOneMeterLatitude;
+    ph.addSample(pp);
+    EXPECT_EQ(1, ph.getConcisePoints().size());
+
+    pp.latitude += 20.0 * cOneMeterLatitude;
+    ph.addSample(pp);
+    ASSERT_EQ(2, ph.getConcisePoints().size());
+    EXPECT_DOUBLE_EQ(3.0 * cOneMeterLatitude.value(),
+            ph.getConcisePoints().front().latitude.value());
+
+    pp.latitude += 10.0 * cOneMeterLatitude;
+    ph.addSample(pp);
+    EXPECT_EQ(3, ph.getConcisePoints().size());
+    EXPECT_DOUBLE_EQ(23.0 * cOneMeterLatitude.value(),
+            ph.getConcisePoints().front().latitude.value());
+
+    pp.latitude += 10.0 * cOneMeterLatitude;
+    ph.addSample(pp);
+    EXPECT_EQ(3, ph.getConcisePoints().size());
+
+    pp.latitude += 3.0 * cOneMeterLatitude;
+    ph.addSample(pp);
+    EXPECT_EQ(4, ph.getConcisePoints().size());
+    EXPECT_DOUBLE_EQ(43.0 * cOneMeterLatitude.value(),
+            ph.getConcisePoints().front().latitude.value());
+
+    pp.latitude += 23.0 * cOneMeterLatitude;
+    ph.addSample(pp);
+    EXPECT_EQ(5, ph.getConcisePoints().size());
+    EXPECT_DOUBLE_EQ(46.0 * cOneMeterLatitude.value(),
+            ph.getConcisePoints().front().latitude.value());
+}
+
+TEST(PathHistory, concise_points_actual_error_threshold) {
+    PathHistory ph;
+    PathPoint pp;
+    ph.addSample(pp);
+
+    pp.heading += units::Angle(5.0 * units::degrees);
+    ph.addSample(pp);
+    pp.latitude += 5.0 * cOneMeterLatitude;
+    ph.addSample(pp);
+    EXPECT_EQ(1, ph.getConcisePoints().size());
+
+    pp.latitude += 10.0 * units::degrees;
+    pp.longitude += 10.0 * units::degrees;
+    ph.addSample(pp);
+    EXPECT_EQ(2, ph.getConcisePoints().size());
+}
+
+TEST(PathHistory, concise_points_truncation) {
+    PathHistory::Parameters params;
+    params.retention_distance = 200.0 * units::si::meter;
+    PathHistory ph(params);
+    PathPoint pp;
+    pp.latitude = 0.0 * units::degree;
+    pp.longitude = 0.0 * units::degree;
+    ph.addSample(pp);
+
+    pp.latitude += 25.0 * cOneMeterLatitude;
+    ph.addSample(pp);
+    pp.latitude += 25.0 * cOneMeterLatitude;
+    ph.addSample(pp);
+    EXPECT_EQ(2, ph.getConcisePoints().size());
+
+    pp.latitude += 25.0 * cOneMeterLatitude;
+    ph.addSample(pp);
+    EXPECT_EQ(3, ph.getConcisePoints().size());
+
+    pp.latitude += 205.0 * cOneMeterLatitude;
+    ph.addSample(pp);
+    ASSERT_EQ(4, ph.getConcisePoints().size());
+    EXPECT_DOUBLE_EQ(0.0 * cOneMeterLatitude.value(),
+            ph.getConcisePoints().back().latitude.value());
+    EXPECT_DOUBLE_EQ(75.0 * cOneMeterLatitude.value(),
+            ph.getConcisePoints().front().latitude.value());
+
+    ph.addSample(pp);
+    ASSERT_EQ(2, ph.getConcisePoints().size());
+    EXPECT_DOUBLE_EQ(75.0 * cOneMeterLatitude.value(),
+            ph.getConcisePoints().back().latitude.value());
+    EXPECT_DOUBLE_EQ(280.0 * cOneMeterLatitude.value(),
+            ph.getConcisePoints().front().latitude.value());
+}
+
+TEST(PathHistory, clear_resets) {
+    PathHistory ph;
+    PathPoint pp;
+    pp.latitude = 0.0 * units::degrees;
+    pp.longitude = 0.0 * units::degrees;
+    for (unsigned i = 0; i < 5; ++i) {
+        pp.latitude += 25.0 * cOneMeterLatitude;
+        ph.addSample(pp);
+    }
+    ASSERT_GT(ph.getConcisePoints().size(), 1u);
+
+    ph.clear();
+    EXPECT_EQ(0u, ph.getConcisePoints().size());
+    EXPECT_PATHPOINT_EQ(PathPoint(), ph.getReferencePoint());
+
+    // usable again, no stale points carried across the reset
+    ph.addSample(pp);
+    EXPECT_EQ(1u, ph.getConcisePoints().size());
+}
+
+TEST(PathHistory, custom_retention_keeps_more) {
+    PathHistory::Parameters short_params;
+    short_params.retention_distance = 200.0 * units::si::meter;
+    PathHistory short_hist(short_params);
+
+    PathHistory::Parameters long_params;
+    long_params.retention_distance = 500.0 * units::si::meter;
+    PathHistory long_hist(long_params);
+
+    PathPoint pp;
+    pp.latitude = 0.0 * units::degrees;
+    pp.longitude = 0.0 * units::degrees;
+    short_hist.addSample(pp);
+    long_hist.addSample(pp);
+    for (unsigned i = 0; i < 20; ++i) {
+        pp.latitude += 25.0 * cOneMeterLatitude; // ~25 m steps, ~500 m total
+        short_hist.addSample(pp);
+        long_hist.addSample(pp);
+    }
+    EXPECT_GT(long_hist.getConcisePoints().size(), short_hist.getConcisePoints().size());
+}
+
+TEST(PathHistory, concise_points_retrieval_limits) {
+    PathHistory ph;
+    PathPoint pp;
+    pp.latitude = 0.0 * units::degrees;
+    pp.longitude = 0.0 * units::degrees;
+    ph.addSample(pp);
+    for (unsigned i = 0; i < 6; ++i) {
+        pp.latitude += 25.0 * cOneMeterLatitude; // beyond chord threshold: a concise point each
+        ph.addSample(pp);
+    }
+    const std::list<PathPoint>& full = ph.getConcisePoints();
+    ASSERT_GT(full.size(), 3u);
+
+    // concise points ~25 m apart, so at a 30 m threshold:
+    // "covering" includes the point crossing 30 m, "within" excludes it
+    const auto covering = ph.getConcisePointsMinLength(30.0 * units::si::meter);
+    const auto within = ph.getConcisePointsMaxLength(30.0 * units::si::meter);
+    EXPECT_EQ(3, std::distance(covering.begin(), covering.end()));
+    EXPECT_EQ(2, std::distance(within.begin(), within.end()));
+    EXPECT_DOUBLE_EQ(full.front().latitude.value(), covering.front().latitude.value());
+    EXPECT_DOUBLE_EQ(full.front().latitude.value(), within.front().latitude.value());
+
+    // optional point limit keeps the newest points
+    const auto capped = ph.getConcisePointsMinLength(10000.0 * units::si::meter, 2);
+    EXPECT_EQ(2, std::distance(capped.begin(), capped.end()));
+    EXPECT_DOUBLE_EQ(full.front().latitude.value(), capped.front().latitude.value());
+}

microbu-esp32c5/external/vanetza-idf/vanetza/facilities/tests/path_point.cpp

@@ -0,0 +1,20 @@
+#include <vanetza/facilities/path_point.hpp>
+#include <gtest/gtest.h>
+
+namespace units = vanetza::units;
+using vanetza::facilities::PathPoint;
+
+TEST(PathPoint, chord_length) {
+    PathPoint a;
+    a.longitude = 12.14094444 * units::degrees;
+    a.latitude = 49.53858333 * units::degrees;
+
+    PathPoint b;
+    b.longitude = 12.27191666 * units::degrees;
+    b.latitude = 49.72394444 * units::degrees;
+
+    EXPECT_EQ(chord_length(a, b), chord_length(b, a));
+
+    units::Length length = chord_length(a, b);
+    EXPECT_NEAR(22692.54, length / units::si::meters, 0.01);
+}