Merge pull request #233 from khevessy/socktap-gpsd

Use GPSD position when confidence is not available (#232)
riebl · Aug 23, 2024 · a63d1ae · a63d1ae
2 parents 8e01f42 + 122ff3a
commit a63d1ae
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());