From 3c7b587568b2764dc64b5a52ddfda01a879779b8 Mon Sep 17 00:00:00 2001 From: Raphael Riebl Date: Sun, 27 Oct 2024 03:31:45 +0100 Subject: [PATCH] facilities: copy only encodable PathHistory points --- vanetza/facilities/detail/path_history.tpp | 24 +++++++++++++++------- 1 file changed, 17 insertions(+), 7 deletions(-) diff --git a/vanetza/facilities/detail/path_history.tpp b/vanetza/facilities/detail/path_history.tpp index ddcf745fb..88f6c04ba 100644 --- a/vanetza/facilities/detail/path_history.tpp +++ b/vanetza/facilities/detail/path_history.tpp @@ -25,16 +25,26 @@ void copy(const facilities::PathHistory& src, SomePathSequence& dest) 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) { + auto delta_latitude = round(point.latitude - ref.latitude, tenth_microdegree); // positive: point is north + auto delta_longitude = round(point.longitude - ref.longitude, tenth_microdegree); // positive: point is east + + if (path_points >= scMaxPathPoints) { + // enough path points have been copied + break; + } else if (delta_latitude < -131071 || delta_latitude > 131071) { + // delta latitude value cannot be encoded, skip this point + continue; + } else if (delta_longitude < -131071 || delta_longitude > 131071) { + // delta longitude value cannot be encoded, skip this point + continue; + } else if (delta_time >= scDeltaTimeStepLength && delta_time <= scMaxDeltaTime) { SomePathPoint* path_point = asn1::allocate(); + path_point->pathPosition.deltaLatitude = delta_latitude; + path_point->pathPosition.deltaLongitude = delta_longitude; + path_point->pathPosition.deltaAltitude = DeltaAltitude::DeltaAltitude_unavailable; + path_point->pathDeltaTime = asn1::allocate(); *(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;