Skip to content

Commit

Permalink
socktap: refactor GPS position provider
Browse files Browse the repository at this point in the history 
Avoid blocking read when using gpsd with socket communication.
Check data set by gpsd more carefully, i.e. use isfinite instead of isnan
and check if GPS time (integer) is set.
  • Loading branch information
@riebl
riebl committed Jan 15, 2024
1 parent 5a04054 commit 25d67e7
Show file tree
Hide file tree
Showing 2 changed files with 56 additions and 43 deletions.
93 changes: 52 additions & 41 deletions tools/socktap/gps_position_provider.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,11 +25,6 @@ int gpsd_read(gps_data_t& data)
#endif
}

constexpr bool gpsd_has_useful_fix(const gps_data_t& data)
{
return data.fix.mode >= MODE_2D;
}

constexpr double gpsd_get_altitude(const gps_data_t& data)
{
#if GPSD_API_MAJOR_VERSION > 8
Expand Down Expand Up @@ -66,19 +61,19 @@ GpsPositionProvider::GpsPositionProvider(boost::asio::io_service& io) :
}

GpsPositionProvider::GpsPositionProvider(boost::asio::io_service& io, const std::string& hostname, const std::string& port) :
timer_(io)
timer_(io), uses_shm_(hostname == gpsd::shared_memory)
{
if (gps_open(hostname.c_str(), port.c_str(), &gps_data)) {
if (gps_open(hostname.c_str(), port.c_str(), &gps_data_)) {
throw GpsPositioningException(errno);
}
gps_stream(&gps_data, WATCH_ENABLE | WATCH_JSON, nullptr);
gps_stream(&gps_data_, WATCH_ENABLE | WATCH_JSON, nullptr);
schedule_timer();
}

GpsPositionProvider::~GpsPositionProvider()
{
gps_stream(&gps_data, WATCH_DISABLE, nullptr);
gps_close(&gps_data);
gps_stream(&gps_data_, WATCH_DISABLE, nullptr);
gps_close(&gps_data_);
}

GpsPositionProvider::GpsPositioningException::GpsPositioningException(int err) :
Expand All @@ -88,7 +83,7 @@ GpsPositionProvider::GpsPositioningException::GpsPositioningException(int err) :

const vanetza::PositionFix& GpsPositionProvider::position_fix()
{
return fetched_position_fix;
return fetched_position_fix_;
}

void GpsPositionProvider::schedule_timer()
Expand All @@ -110,42 +105,58 @@ void GpsPositionProvider::on_timer(const boost::system::error_code& ec)
void GpsPositionProvider::fetch_position_fix()
{
int gps_read_rc = 0;
do {
gps_read_rc = gpsd_read(gps_data);
} while (gps_read_rc > 0 && gps_data.devices.ndevices > 0);
if (uses_shm_ || gps_waiting(&gps_data_, 0)) {
// reading is not expected to block now
gps_read_rc = gpsd_read(gps_data_);
}

if (gps_read_rc < 0) {
throw GpsPositioningException(errno);
} else if (gps_read_rc > 0) {
apply_gps_data(gps_data_);
}
}

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);
fetched_position_fix.latitude = gps_data.fix.latitude * degree;
fetched_position_fix.longitude = gps_data.fix.longitude * degree;
fetched_position_fix.speed.assign(gps_data.fix.speed * si::meter_per_second, gps_data.fix.eps * si::meter_per_second);
fetched_position_fix.course.assign(north + gps_data.fix.track * degree, north + gps_data.fix.epd * degree);
if (!std::isnan(gps_data.fix.epx) && !std::isnan(gps_data.fix.epy)) {
if (gps_data.fix.epx > gps_data.fix.epy) {
fetched_position_fix.confidence.semi_minor = gps_data.fix.epy * si::meter;
fetched_position_fix.confidence.semi_major = gps_data.fix.epx * si::meter;
fetched_position_fix.confidence.orientation = north + 90.0 * degree;
} else {
fetched_position_fix.confidence.semi_minor = gps_data.fix.epx * si::meter;
fetched_position_fix.confidence.semi_major = gps_data.fix.epy * si::meter;
fetched_position_fix.confidence.orientation = north;
}
} else {
fetched_position_fix.confidence = vanetza::PositionConfidence();
}
if (gps_data.fix.mode == MODE_3D) {
fetched_position_fix.altitude = vanetza::ConfidentQuantity<vanetza::units::Length> {
gpsd_get_altitude(gps_data) * si::meter, gps_data.fix.epv * si::meter };
bool GpsPositionProvider::apply_gps_data(const gps_data_t& gps_data)
{
if ((gps_data.set & MODE_SET) != MODE_SET) {
// no mode set at all
return false;
} else if ((gps_data.set & TIME_SET) != TIME_SET) {
// mandatory GPS time is missing (fix.time field)
return false;
} else if (gps_data.fix.mode < MODE_2D) {
// latitude and longitude unavailable
return false;
}

using namespace vanetza::units;
static const TrueNorth north = TrueNorth::from_value(0.0);

fetched_position_fix_.timestamp = convert_gps_time(gps_data.fix.time);
fetched_position_fix_.latitude = gps_data.fix.latitude * degree;
fetched_position_fix_.longitude = gps_data.fix.longitude * degree;
fetched_position_fix_.speed.assign(gps_data.fix.speed * si::meter_per_second, gps_data.fix.eps * si::meter_per_second);
fetched_position_fix_.course.assign(north + gps_data.fix.track * degree, north + gps_data.fix.epd * degree);
if (std::isfinite(gps_data.fix.epx) && std::isfinite(gps_data.fix.epy)) {
if (gps_data.fix.epx > gps_data.fix.epy) {
fetched_position_fix_.confidence.semi_minor = gps_data.fix.epy * si::meter;
fetched_position_fix_.confidence.semi_major = gps_data.fix.epx * si::meter;
fetched_position_fix_.confidence.orientation = north + 90.0 * degree;
} else {
fetched_position_fix.altitude = boost::none;
fetched_position_fix_.confidence.semi_minor = gps_data.fix.epx * si::meter;
fetched_position_fix_.confidence.semi_major = gps_data.fix.epy * si::meter;
fetched_position_fix_.confidence.orientation = north;
}
} else {
fetched_position_fix_.confidence = vanetza::PositionConfidence();
}
if (gps_data.fix.mode == MODE_3D) {
fetched_position_fix_.altitude = vanetza::ConfidentQuantity<vanetza::units::Length> {
gpsd_get_altitude(gps_data) * si::meter, gps_data.fix.epv * si::meter };
} else {
fetched_position_fix_.altitude = boost::none;
}
}

return true;
}
6 changes: 4 additions & 2 deletions tools/socktap/gps_position_provider.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,10 +29,12 @@ class GpsPositionProvider : public vanetza::PositionProvider
private:
void schedule_timer();
void on_timer(const boost::system::error_code& ec);
bool apply_gps_data(const gps_data_t&);

boost::asio::steady_timer timer_;
gps_data_t gps_data;
vanetza::PositionFix fetched_position_fix;
bool uses_shm_;
gps_data_t gps_data_;
vanetza::PositionFix fetched_position_fix_;
};

namespace gpsd
Expand Down

0 comments on commit 25d67e7

Please sign in to comment.