From 122ff3a2e1060903f236b89d5b5a8fb1f5ddc407 Mon Sep 17 00:00:00 2001
From: Karel Hevessy <karel.hevessy@machsystems.cz>
Date: Fri, 23 Aug 2024 09:44:53 +0200
Subject: [PATCH] Use GPSD position when confidence is not available
 (riebl#232)

---
 tools/socktap/cam_application.cpp       |  2 +-
 tools/socktap/gps_position_provider.cpp |  8 +++++++-
 vanetza/facilities/cam_functions.cpp    | 27 ++++++++++++++++++++++---
 3 files changed, 32 insertions(+), 5 deletions(-)

diff --git a/tools/socktap/cam_application.cpp b/tools/socktap/cam_application.cpp
index f7e40e6fe..a221b8a8e 100644
--- a/tools/socktap/cam_application.cpp
+++ b/tools/socktap/cam_application.cpp
@@ -79,7 +79,7 @@ void CamApplication::on_timer(Clock::time_point)
 
     auto position = positioning_.position_fix();
 
-    if (!position.confidence) {
+    if (!std::isfinite(position.latitude.value()) || !std::isfinite(position.latitude.value())) {
         std::cerr << "Skipping CAM, because no good position is available, yet." << std::endl;
         return;
     }
diff --git a/tools/socktap/gps_position_provider.cpp b/tools/socktap/gps_position_provider.cpp
index 5928514d2..f8710d107 100644
--- a/tools/socktap/gps_position_provider.cpp
+++ b/tools/socktap/gps_position_provider.cpp
@@ -72,6 +72,9 @@ GpsPositionProvider::GpsPositionProvider(boost::asio::io_service& io, const std:
         throw GpsPositioningException(errno);
     }
     gps_stream(&gps_data, WATCH_ENABLE | WATCH_JSON, nullptr);
+    using namespace vanetza::units;
+    fetched_position_fix.latitude = GeoAngle::from_value(std::numeric_limits<GeoAngle::value_type>::infinity());
+    fetched_position_fix.longitude = GeoAngle::from_value(std::numeric_limits<GeoAngle::value_type>::infinity());
     schedule_timer();
 }
 
@@ -118,8 +121,8 @@ void GpsPositionProvider::fetch_position_fix()
         throw GpsPositioningException(errno);
     }
 
+    using namespace vanetza::units;
     if (gpsd_has_useful_fix(gps_data)) {
-        using namespace vanetza::units;
         static const TrueNorth north = TrueNorth::from_value(0.0);
 
         fetched_position_fix.timestamp = convert_gps_time(gps_data.fix.time);
@@ -146,6 +149,9 @@ void GpsPositionProvider::fetch_position_fix()
         } else {
             fetched_position_fix.altitude = boost::none;
         }
+    } else {
+        fetched_position_fix.latitude = GeoAngle::from_value(std::numeric_limits<GeoAngle::value_type>::infinity());
+        fetched_position_fix.longitude = GeoAngle::from_value(std::numeric_limits<GeoAngle::value_type>::infinity());
     }
 }
 
diff --git a/vanetza/facilities/cam_functions.cpp b/vanetza/facilities/cam_functions.cpp
index 7baf4c9c3..22b539ce0 100644
--- a/vanetza/facilities/cam_functions.cpp
+++ b/vanetza/facilities/cam_functions.cpp
@@ -150,9 +150,30 @@ long round(const boost::units::quantity<T>& q, const U& u)
 void copy(const PositionFix& position, ReferencePosition& reference_position) {
     reference_position.longitude = round(position.longitude, microdegree) * Longitude_oneMicrodegreeEast;
     reference_position.latitude = round(position.latitude, microdegree) * Latitude_oneMicrodegreeNorth;
-    reference_position.positionConfidenceEllipse.semiMajorOrientation = HeadingValue_unavailable;
-    reference_position.positionConfidenceEllipse.semiMajorConfidence = SemiAxisLength_unavailable;
-    reference_position.positionConfidenceEllipse.semiMinorConfidence = SemiAxisLength_unavailable;
+    if (std::isfinite(position.confidence.semi_major.value())
+        && std::isfinite(position.confidence.semi_major.value()))
+    {
+        if ((position.confidence.semi_major.value() * 100 < SemiAxisLength_outOfRange)
+            && (position.confidence.semi_major.value() * 100 < SemiAxisLength_outOfRange)
+            && (position.confidence.orientation.value() * 10 < HeadingValue_unavailable))
+        {
+            reference_position.positionConfidenceEllipse.semiMajorConfidence = position.confidence.semi_major.value() * 100;    // Value in centimeters
+            reference_position.positionConfidenceEllipse.semiMinorConfidence = position.confidence.semi_minor.value() * 100;
+            reference_position.positionConfidenceEllipse.semiMajorOrientation = (position.confidence.orientation.value()) * 10; // Value from 0 to 3600
+        }
+        else
+        {
+            reference_position.positionConfidenceEllipse.semiMajorConfidence = SemiAxisLength_outOfRange;
+            reference_position.positionConfidenceEllipse.semiMinorConfidence = SemiAxisLength_outOfRange;
+            reference_position.positionConfidenceEllipse.semiMajorOrientation = HeadingValue_unavailable;
+        }
+    }
+    else
+    {
+        reference_position.positionConfidenceEllipse.semiMajorConfidence = SemiAxisLength_unavailable;
+        reference_position.positionConfidenceEllipse.semiMinorConfidence = SemiAxisLength_unavailable;
+        reference_position.positionConfidenceEllipse.semiMajorOrientation = HeadingValue_unavailable;
+    }
     if (position.altitude) {
         reference_position.altitude.altitudeValue = to_altitude_value(position.altitude->value());
         reference_position.altitude.altitudeConfidence = to_altitude_confidence(position.altitude->confidence());