From 1b2087944ece1d3e0a0575105360825b7dcb81c7 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 17 Mar 2023 10:39:22 +0100 Subject: [PATCH] Add Geohash of PVT solution to internal logs --- docs/CHANGELOG.md | 2 + .../PVT/gnuradio_blocks/rtklib_pvt_gs.cc | 4 +- .../PVT/gnuradio_blocks/rtklib_pvt_gs.h | 2 + src/algorithms/PVT/libs/CMakeLists.txt | 2 + src/algorithms/PVT/libs/geohash.cc | 195 ++++++++++++++++++ src/algorithms/PVT/libs/geohash.h | 74 +++++++ src/tests/test_main.cc | 1 + .../pvt/geohash_test.cc | 38 ++++ .../pvt/nmea_printer_test.cc | 2 +- 9 files changed, 318 insertions(+), 2 deletions(-) create mode 100644 src/algorithms/PVT/libs/geohash.cc create mode 100644 src/algorithms/PVT/libs/geohash.h create mode 100644 src/tests/unit-tests/signal-processing-blocks/pvt/geohash_test.cc diff --git a/docs/CHANGELOG.md b/docs/CHANGELOG.md index fafe79b12..aff709642 100644 --- a/docs/CHANGELOG.md +++ b/docs/CHANGELOG.md @@ -111,6 +111,8 @@ All notable changes to GNSS-SDR will be documented in this file. - New configuration parameter `PVT.use_unhealthy_sats`, set by default to `false`, allows processing observables of satellites that report an unhealthy status in the navigation message if set to `true`. +- Added the [Geohash](https://en.wikipedia.org/wiki/Geohash) of the PVT solution + in the internal logs. - Allowed the CMake project to be a sub-project. See the definitions of concepts and metrics at diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index 500db4d4c..9d2b843b3 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -28,6 +28,7 @@ #include "galileo_has_data.h" #include "galileo_iono.h" #include "galileo_utc_model.h" +#include "geohash.h" #include "geojson_printer.h" #include "glonass_gnav_almanac.h" #include "glonass_gnav_ephemeris.h" @@ -124,6 +125,7 @@ rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels, gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro)), gr::io_signature::make(0, 0, 0)), d_dump_filename(conf_.dump_filename), + d_geohash(std::make_unique()), d_gps_ephemeris_sptr_type_hash_code(typeid(std::shared_ptr).hash_code()), d_gps_iono_sptr_type_hash_code(typeid(std::shared_ptr).hash_code()), d_gps_utc_model_sptr_type_hash_code(typeid(std::shared_ptr).hash_code()), @@ -2432,7 +2434,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item LOG(INFO) << "Position at " << boost::posix_time::to_simple_string(d_user_pvt_solver->get_position_UTC_time()) << " UTC using " << d_user_pvt_solver->get_num_valid_observations() << " observations is Lat = " << d_user_pvt_solver->get_latitude() << " [deg], Long = " << d_user_pvt_solver->get_longitude() << " [deg], Height = " << d_user_pvt_solver->get_height() << " [m]"; - + LOG(INFO) << "geohash: " << d_geohash->encode(d_user_pvt_solver->get_latitude(), d_user_pvt_solver->get_longitude()); /* std::cout << "Dilution of Precision at " << boost::posix_time::to_simple_string(d_user_pvt_solver->get_position_UTC_time()) << " UTC using "<< d_user_pvt_solver->get_num_valid_observations() <<" observations is HDOP = " << d_user_pvt_solver->get_hdop() << " VDOP = " << d_user_pvt_solver->get_vdop() diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h index 94ffb458a..747c31c65 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h @@ -50,6 +50,7 @@ class Beidou_Dnav_Ephemeris; class Galileo_Almanac; class Galileo_Ephemeris; class Galileo_HAS_data; +class Geohash; class GeoJSON_Printer; class Gps_Almanac; class Gps_Ephemeris; @@ -204,6 +205,7 @@ private: std::queue d_TimeChannelTagTimestamps; boost::posix_time::time_duration d_utc_diff_time; + std::unique_ptr d_geohash; size_t d_gps_ephemeris_sptr_type_hash_code; size_t d_gps_iono_sptr_type_hash_code; diff --git a/src/algorithms/PVT/libs/CMakeLists.txt b/src/algorithms/PVT/libs/CMakeLists.txt index 8e2785144..2460a3d4b 100644 --- a/src/algorithms/PVT/libs/CMakeLists.txt +++ b/src/algorithms/PVT/libs/CMakeLists.txt @@ -22,6 +22,7 @@ set(PVT_LIB_SOURCES monitor_pvt_udp_sink.cc monitor_ephemeris_udp_sink.cc has_simple_printer.cc + geohash.cc ) set(PVT_LIB_HEADERS @@ -43,6 +44,7 @@ set(PVT_LIB_HEADERS serdes_gps_eph.h monitor_ephemeris_udp_sink.h has_simple_printer.h + geohash.h ) list(SORT PVT_LIB_HEADERS) diff --git a/src/algorithms/PVT/libs/geohash.cc b/src/algorithms/PVT/libs/geohash.cc new file mode 100644 index 000000000..3682a7e0e --- /dev/null +++ b/src/algorithms/PVT/libs/geohash.cc @@ -0,0 +1,195 @@ +/*! + * \file geohash.cc + * \brief Implementation of a class for geohash encoding / decoding + * \author Carles Fernandez-Prades, 2023. cfernandez(at)cttc.es + * + * + * ----------------------------------------------------------------------------- + * + * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. + * This file is part of GNSS-SDR. + * + * Copyright (C) 2010-2023 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + +#include "geohash.h" +#include +#include +#include +#include +#include +#include + + +Geohash::Geohash() +{ + base32 = "0123456789bcdefghjkmnpqrstuvwxyz"; +} + +std::string Geohash::encode(double lat, double lon, int precision) const +{ + // infer precision? + if (precision == -1) + { + // refine geohash until it matches precision of supplied lat/lon + for (int p = 1; p <= 12; ++p) + { + const auto hash = Geohash::encode(lat, lon, p); + const auto posn = Geohash::decode(hash); + + if ((std::fabs(posn[0] - lat) < std::numeric_limits::epsilon()) && + (std::fabs(posn[1] - lon) < std::numeric_limits::epsilon())) + { + return hash; + } + } + precision = 12; // set to maximum + } + + if (std::isnan(lat) || std::isnan(lon) || precision < 1) + { + throw std::invalid_argument("Invalid geohash"); + } + + int idx = 0; // index into base32 map + int bit = 0; // each char holds 5 bits + bool evenBit = true; + std::string geohash = ""; + + double latMin = -90.0; + double latMax = 90.0; + double lonMin = -180.0; + double lonMax = 180.0; + + while (geohash.length() < static_cast(precision)) + { + if (evenBit) + { + // bisect E-W longitude + const double lonMid = (lonMin + lonMax) / 2.0; + if (lon >= lonMid) + { + idx = idx * 2 + 1; + lonMin = lonMid; + } + else + { + idx = idx * 2; + lonMax = lonMid; + } + } + else + { + // bisect N-S latitude + const double latMid = (latMin + latMax) / 2.0; + if (lat >= latMid) + { + idx = idx * 2 + 1; + latMin = latMid; + } + else + { + idx = idx * 2; + latMax = latMid; + } + } + evenBit = !evenBit; + + if (++bit == 5) + { + // 5 bits gives us a character: append it and start over + geohash += base32[idx]; + bit = 0; + idx = 0; + } + } + + return geohash; +} + + +std::array Geohash::decode(std::string geohash) const +{ + const auto bounds = Geohash::bounds(geohash); + + const double latMin = bounds[0]; + const double lonMin = bounds[1]; + const double latMax = bounds[2]; + const double lonMax = bounds[3]; + + // cell centre + double lat = (latMin + latMax) / 2.0; + double lon = (lonMin + lonMax) / 2.0; + + // round to close to centre without excessive precision: ⌊2-log10(Δ°)⌋ decimal places + std::array latlon{}; + latlon[0] = std::floor(lat * std::pow(10, std::floor(2 - std::log10(latMax - latMin)))); + latlon[1] = std::floor(lon * std::pow(10, std::floor(2 - std::log10(lonMax - lonMin)))); + + return latlon; +} + + +std::array Geohash::bounds(std::string geohash) const +{ + if (geohash.length() == 0) + { + throw std::runtime_error("Invalid geohash"); + } + + std::transform(geohash.begin(), geohash.end(), geohash.begin(), + [](unsigned char c) { return std::tolower(c); }); + + bool evenBit = true; + double latMin = -90.0; + double latMax = 90.0; + double lonMin = -180.0; + double lonMax = 180.0; + + for (size_t i = 0; i < geohash.length(); i++) + { + char chr = geohash[i]; + int idx = base32.find(chr); + if (idx == -1) + { + throw std::runtime_error("Invalid geohash"); + } + + for (int n = 4; n >= 0; n--) + { + int bitN = idx >> n & 1; + if (evenBit) + { + // longitude + double lonMid = (lonMin + lonMax) / 2; + if (bitN == 1) + { + lonMin = lonMid; + } + else + { + lonMax = lonMid; + } + } + else + { + // latitude + double latMid = (latMin + latMax) / 2; + if (bitN == 1) + { + latMin = latMid; + } + else + { + latMax = latMid; + } + } + evenBit = !evenBit; + } + } + + return {latMin, lonMin, latMax, lonMax}; +} \ No newline at end of file diff --git a/src/algorithms/PVT/libs/geohash.h b/src/algorithms/PVT/libs/geohash.h new file mode 100644 index 000000000..019dbaea5 --- /dev/null +++ b/src/algorithms/PVT/libs/geohash.h @@ -0,0 +1,74 @@ +/*! + * \file geohash.h + * \brief Interface of a class that encodes / decodes geohashes + * \author Carles Fernandez-Prades, 2023. cfernandez(at)cttc.es + * + * + * ----------------------------------------------------------------------------- + * + * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. + * This file is part of GNSS-SDR. + * + * Copyright (C) 2010-2023 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + + +#ifndef GNSS_SDR_GEOHASH_H +#define GNSS_SDR_GEOHASH_H + +#include +#include + +/** \addtogroup PVT + * \{ */ +/** \addtogroup PVT_libs + * \{ */ + +/*! + * \brief Class for geohash encoding / decoding + * See https://en.wikipedia.org/wiki/Geohash + */ +class Geohash +{ +public: + Geohash(); + + /** + * Encodes latitude/longitude to geohash, either to specified precision or + * to automatically evaluated precision. + * + * @param {double} lat - Latitude in degrees. + * @param {double} lon - Longitude in degrees. + * @param {int} [precision] - Number of characters in resulting geohash. + * @returns {string} Geohash of supplied latitude/longitude. + * @throws Invalid geohash. + * + */ + std::string encode(double lat, double lon, int precision = -1) const; + + /** + * Decode geohash to latitude/longitude (location is approximate centre of + * geohash cell, to reasonable precision). + * + * @param {string} geohash - Geohash string to be converted to + * latitude/longitude. + * @returns {lat, lon} (Center of) geohashed location. + * @throws Invalid geohash. + * + */ + std::array decode(std::string geohash) const; + +private: + /* + * Returns SW/NE latitude/longitude bounds of specified geohash. + */ + std::array bounds(std::string geohash) const; + std::string base32; +}; + +/** \} */ +/** \} */ +#endif // GNSS_SDR_GEOHASH_H diff --git a/src/tests/test_main.cc b/src/tests/test_main.cc index 15b5ffacc..3769756aa 100644 --- a/src/tests/test_main.cc +++ b/src/tests/test_main.cc @@ -72,6 +72,7 @@ DECLARE_string(log_dir); #include "unit-tests/signal-processing-blocks/adapter/adapter_test.cc" #include "unit-tests/signal-processing-blocks/adapter/pass_through_test.cc" #include "unit-tests/signal-processing-blocks/libs/item_type_helpers_test.cc" +#include "unit-tests/signal-processing-blocks/pvt/geohash_test.cc" #include "unit-tests/signal-processing-blocks/pvt/nmea_printer_test.cc" #include "unit-tests/signal-processing-blocks/pvt/rinex_printer_test.cc" #include "unit-tests/signal-processing-blocks/pvt/rtcm_printer_test.cc" diff --git a/src/tests/unit-tests/signal-processing-blocks/pvt/geohash_test.cc b/src/tests/unit-tests/signal-processing-blocks/pvt/geohash_test.cc new file mode 100644 index 000000000..bb1168a08 --- /dev/null +++ b/src/tests/unit-tests/signal-processing-blocks/pvt/geohash_test.cc @@ -0,0 +1,38 @@ +/*! + * \file geohash_test.cc + * \brief Implements Unit Tests for the Geohash class. + * \author Carles Fernandez-Prades, 2023. cfernandez(at)cttc.es + * + * ----------------------------------------------------------------------------- + * + * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. + * This file is part of GNSS-SDR. + * + * Copyright (C) 2010-2023 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + +#include "geohash.h" + +TEST(Geohash_Test, Encode) +{ + Geohash gh = Geohash(); + std::string hash; + EXPECT_NO_THROW(hash = gh.encode(52.205, 0.119, 7)); + + EXPECT_EQ(0, hash.compare("u120fxw")); + + EXPECT_THROW(gh.encode(52.205, 0.119, 0), std::invalid_argument); +} + +TEST(Geohash_Test, precision) +{ + Geohash gh = Geohash(); + std::string hash; + EXPECT_NO_THROW(hash = gh.encode(52.205, 0.119, 6)); + EXPECT_EQ(0, hash.compare("u120fx")); + EXPECT_NO_THROW(hash = gh.encode(52.205, 0.119, 5)); + EXPECT_EQ(0, hash.compare("u120f")); +} \ No newline at end of file diff --git a/src/tests/unit-tests/signal-processing-blocks/pvt/nmea_printer_test.cc b/src/tests/unit-tests/signal-processing-blocks/pvt/nmea_printer_test.cc index b4dfc483f..99c2bc0a3 100644 --- a/src/tests/unit-tests/signal-processing-blocks/pvt/nmea_printer_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/pvt/nmea_printer_test.cc @@ -1,5 +1,5 @@ /*! - * \file nma_printer_test.cc + * \file nmea_printer_test.cc * \brief Implements Unit Tests for the Nmea_Printer class. * \author Carles Fernandez-Prades, 2017. cfernandez(at)cttc.es *