facilities: add more PathHistory copy functions

vanetza/facilities/cam_functions.hpp

@@ -11,9 +11,12 @@
 // 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
@@ -32,12 +35,23 @@ 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

vanetza/facilities/detail/path_history.ipp

@@ -1,47 +1,37 @@
 #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/facilities/path_history.hpp>
+#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
 {
 
-// TODO:  C2C-CC BSP allows up to 500m history for CAMs, we provide just minimal required history
-void copy(const facilities::PathHistory& ph, ASN1_PREFIXED(BasicVehicleContainerLowFrequency)& container)
-{
-    static const std::size_t scMaxPathPoints = 23;
-    static const boost::posix_time::time_duration scMaxDeltaTime = boost::posix_time::millisec(655350);
-    static const auto scMicrodegree = microdegree;
-
-    const auto& concise_points = ph.getConcisePoints();
-    const facilities::PathPoint& ref = ph.getReferencePoint();
-    std::size_t path_points = 0;
+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");
 
-    for (const PathPoint& point : concise_points) {
-        auto delta_time = ref.time - point.time; // positive: point is in past
-        auto delta_latitude = point.latitude - ref.latitude; // positive: point is north
-        auto delta_longitude = point.longitude - ref.longitude; // positive: point is east
-
-        while (delta_time.total_milliseconds() >= 10 && path_points < scMaxPathPoints) {
-            ::ASN1_PREFIXED(PathPoint)* path_point = asn1::allocate<::ASN1_PREFIXED(PathPoint)>();
-            path_point->pathDeltaTime = asn1::allocate<ASN1_PREFIXED(PathDeltaTime_t)>();
-            *(path_point->pathDeltaTime) = std::min(delta_time, scMaxDeltaTime).total_milliseconds() /
-                10 * PathDeltaTime::PathDeltaTime_tenMilliSecondsInPast;
-            path_point->pathPosition.deltaLatitude = (delta_latitude / scMicrodegree).value() *
-                DeltaLatitude::DeltaLatitude_oneMicrodegreeNorth;
-            path_point->pathPosition.deltaLongitude = (delta_longitude / scMicrodegree).value() *
-                DeltaLongitude::DeltaLongitude_oneMicrodegreeEast;
-            path_point->pathPosition.deltaAltitude = DeltaAltitude::DeltaAltitude_unavailable;
+void copy(const facilities::PathHistory& src, ASN1_PREFIXED(PathHistory_t)& dest)
+{
+    copy<ASN1_PREFIXED(PathHistory_t), ASN1_PREFIXED(PathPoint_t)>(src, dest);
+}
 
-            ASN_SEQUENCE_ADD(&container.pathHistory, path_point);
+#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
 
-            delta_time -= scMaxDeltaTime;
-            ++path_points;
-        }
-    }
+void copy(const facilities::PathHistory& ph, ASN1_PREFIXED(BasicVehicleContainerLowFrequency)& container)
+{
+    copy(ph, container.pathHistory);
 }
 
 } // namespace facilities

vanetza/facilities/detail/path_history.tpp

@@ -0,0 +1,46 @@
+#pragma once
+#include <vanetza/facilities/path_history.hpp>
+#include <vanetza/facilities/path_point.hpp>
+#include <chrono>
+#include <type_traits>
+
+namespace vanetza
+{
+namespace facilities
+{
+
+template<typename SomePathSequence, typename SomePathPoint>
+void copy(const facilities::PathHistory& src, SomePathSequence& dest)
+{
+    using SomePathDeltaTime = typename std::remove_pointer<decltype(SomePathPoint::pathDeltaTime)>::type;
+
+    // TODO:  C2C-CC BSP allows up to 500m history for CAMs, we provide just minimal required history
+    static const std::size_t scMaxPathPoints = 23;
+    static const auto scDeltaTimeStepLength = boost::posix_time::milliseconds(10);
+    static const auto scMaxDeltaTime = scDeltaTimeStepLength * 65535;
+
+    const auto& concise_points = src.getConcisePoints();
+    const facilities::PathPoint& ref = src.getReferencePoint();
+    std::size_t path_points = 0;
+
+    for (const PathPoint& point : concise_points) {
+        auto delta_time = ref.time - point.time; // positive: point is in past
+        auto delta_latitude = point.latitude - ref.latitude; // positive: point is north
+        auto delta_longitude = point.longitude - ref.longitude; // positive: point is east
+
+        if (delta_time >= scDeltaTimeStepLength && delta_time <= scMaxDeltaTime && path_points < scMaxPathPoints) {
+            SomePathPoint* path_point = asn1::allocate<SomePathPoint>();
+            path_point->pathDeltaTime = asn1::allocate<SomePathDeltaTime>();
+            *(path_point->pathDeltaTime) = delta_time.total_milliseconds() / scDeltaTimeStepLength.total_milliseconds();
+            path_point->pathPosition.deltaLatitude = round(delta_latitude, tenth_microdegree);
+            path_point->pathPosition.deltaLongitude = round(delta_longitude, tenth_microdegree);
+            path_point->pathPosition.deltaAltitude = DeltaAltitude::DeltaAltitude_unavailable;
+
+            ASN_SEQUENCE_ADD(&dest, path_point);
+            ++path_points;
+        }
+    }
+}
+
+} // namespace facilities
+} // namespace vanetza

vanetza/facilities/tests/cam_functions.cpp

@@ -1,14 +1,15 @@
 #include <gtest/gtest.h>
+#include <vanetza/facilities/path_history.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>
-#include <vanetza/asn1/its/LowFrequencyContainer.h>
-#include <vanetza/facilities/path_history.hpp>
 
 using namespace vanetza;
 using namespace vanetza::facilities;
@@ -211,36 +212,49 @@ TYPED_TEST(CamFunctionsReferencePosition, copy)
     EXPECT_EQ(dest.altitude.altitudeConfidence, AltitudeConfidence_unavailable);
 }
 
-TYPED_TEST(CamFunctionsReferencePosition, copy_path_history)
+using PathHistoryTypes = ::testing::Types<::PathHistory, Vanetza_ITS2_PathHistory>;
+template<typename T>
+class CamFunctionsPathHistory : public ::testing::Test
 {
+protected:
     vanetza::facilities::PathHistory path_history;
-    vanetza::facilities::PathPoint path_point;
-    path_point.longitude = 40.906 * degree;
-    path_point.latitude = 29.155 * degree;
-    path_point.heading = vanetza::units::Angle { 0.0 * boost::units::degree::degree };
-    path_point.time = boost::posix_time::microsec_clock::universal_time();
-    path_history.addSample(path_point);
-
-    LowFrequencyContainer_t lfc {};
-    lfc.present = LowFrequencyContainer_PR_basicVehicleContainerLowFrequency;
-    BasicVehicleContainerLowFrequency& bvc = lfc.choice.basicVehicleContainerLowFrequency;
-    bvc.vehicleRole = 0;
-    bvc.exteriorLights.size = 1;
-    bvc.exteriorLights.bits_unused = 0;
-    bvc.exteriorLights.buf = static_cast<uint8_t*>(calloc(1, 1));
-    copy(path_history, bvc);
-
-    int size = lfc.choice.basicVehicleContainerLowFrequency.pathHistory.list.count;
+
+    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);
     for (int i = 0; i < size; i++) {
-        auto current_path_point = lfc.choice.basicVehicleContainerLowFrequency.pathHistory.list.array[i];
-        EXPECT_NE(current_path_point->pathDeltaTime, nullptr);
-        EXPECT_TRUE(*current_path_point->pathDeltaTime >= 1);
-        EXPECT_TRUE(*current_path_point->pathDeltaTime <= 65535);
-        EXPECT_TRUE(current_path_point->pathPosition.deltaLatitude >= -131071);
-        EXPECT_TRUE(current_path_point->pathPosition.deltaLatitude <= 131072);
-        EXPECT_TRUE(current_path_point->pathPosition.deltaLongitude >= -131071);
-        EXPECT_TRUE(current_path_point->pathPosition.deltaLongitude <= 131072);
-        EXPECT_TRUE(current_path_point->pathPosition.deltaAltitude >= -12700);
-        EXPECT_TRUE(current_path_point->pathPosition.deltaAltitude <= 12800);
+        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);
     }
 }