Add Geohash of PVT solution to internal logs

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

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<Geohash>()),
       d_gps_ephemeris_sptr_type_hash_code(typeid(std::shared_ptr<Gps_Ephemeris>).hash_code()),
       d_gps_iono_sptr_type_hash_code(typeid(std::shared_ptr<Gps_Iono>).hash_code()),
       d_gps_utc_model_sptr_type_hash_code(typeid(std::shared_ptr<Gps_Utc_Model>).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()

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<GnssTime> d_TimeChannelTagTimestamps;
 
     boost::posix_time::time_duration d_utc_diff_time;
+    std::unique_ptr<Geohash> d_geohash;
 
     size_t d_gps_ephemeris_sptr_type_hash_code;
     size_t d_gps_iono_sptr_type_hash_code;

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)

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 <algorithm>
+#include <cctype>
+#include <cmath>
+#include <cstddef>
+#include <limits>
+#include <stdexcept>
+
+
+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<double>::epsilon()) &&
+                        (std::fabs(posn[1] - lon) < std::numeric_limits<double>::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<size_t>(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<double, 2> 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<double, 2> 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<double, 4> 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};
+}

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 <array>
+#include <string>
+
+/** \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<double, 2> decode(std::string geohash) const;
+
+private:
+    /*
+     * Returns SW/NE latitude/longitude bounds of specified geohash.
+     */
+    std::array<double, 4> bounds(std::string geohash) const;
+    std::string base32;
+};
+
+/** \} */
+/** \} */
+#endif  // GNSS_SDR_GEOHASH_H

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"

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"));
+}

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
  *