Use GPSD position when confidence is not available (#232)

riebl · Aug 23, 2024 · 122ff3a · 122ff3a
1 parent 8e01f42
commit 122ff3a
Show file tree

Hide file tree

Showing 3 changed files with 32 additions and 5 deletions.
diff --git 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
@@ -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
@@ -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());