Merge branch 'next' of https://github.com/gnss-sdr/gnss-sdr into fpga

CMakeLists.txt

@@ -345,14 +345,14 @@ set(GNSSSDR_MATIO_MIN_VERSION "1.5.3")
 ################################################################################
 # Versions to download and build (but not installed) if not found
 ################################################################################
-set(GNSSSDR_GFLAGS_LOCAL_VERSION "2.2.1")
+set(GNSSSDR_GFLAGS_LOCAL_VERSION "2.2.2")
 set(GNSSSDR_GLOG_LOCAL_VERSION "0.3.5")
 set(GNSSSDR_ARMADILLO_LOCAL_VERSION "9.200.x")
 set(GNSSSDR_GTEST_LOCAL_VERSION "1.8.1")
 set(GNSSSDR_GNSS_SIM_LOCAL_VERSION "master")
 set(GNSSSDR_GPSTK_LOCAL_VERSION "2.10.6")
 set(GNSSSDR_MATIO_LOCAL_VERSION "1.5.13")
-
+set(GNSSSDR_PUGIXML_LOCAL_VERSION "1.9")
 
 
 ################################################################################
@@ -1216,6 +1216,53 @@ if(NOT MATIO_FOUND OR MATIO_VERSION_STRING VERSION_LESS ${GNSSSDR_MATIO_MIN_VERS
 endif(NOT MATIO_FOUND OR MATIO_VERSION_STRING VERSION_LESS ${GNSSSDR_MATIO_MIN_VERSION})
 
 
+################################################################################
+# PugiXML - https://pugixml.org/
+################################################################################
+find_package(PugiXML QUIET)
+if(PugiXML_FOUND)
+    message(STATUS "PugiXML has been found. Reading of GSA Galileo almanac XML files enabled.")
+else(PugiXML_FOUND)
+    message(STATUS " PugiXML v${GNSSSDR_PUGIXML_LOCAL_VERSION} will be downloaded and built automatically when doing '${CMAKE_MAKE_PROGRAM_PRETTY_NAME}'.")
+    set(PUGIXML_COMPILER -DCMAKE_CXX_COMPILER=${CMAKE_CXX_COMPILER} -DCMAKE_C_COMPILER=${CMAKE_C_COMPILER})
+    set(TOOLCHAIN_ARG "")
+    if(EXISTS $ENV{OECORE_TARGET_SYSROOT})
+        set(PUGIXML_COMPILER "")
+        set(TOOLCHAIN_ARG "-DCMAKE_TOOLCHAIN_FILE=${CMAKE_CURRENT_SOURCE_DIR}/cmake/Toolchains/oe-sdk_cross.cmake")
+    endif(EXISTS $ENV{OECORE_TARGET_SYSROOT})
+    if(CMAKE_VERSION VERSION_LESS 3.2)
+        ExternalProject_Add(
+            pugixml-${GNSSSDR_PUGIXML_LOCAL_VERSION}
+            PREFIX ${CMAKE_CURRENT_BINARY_DIR}/pugixml-${GNSSSDR_PUGIXML_LOCAL_VERSION}
+            GIT_REPOSITORY https://github.com/zeux/pugixml
+            GIT_TAG v${GNSSSDR_PUGIXML_LOCAL_VERSION}
+            SOURCE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/thirdparty/pugixml/pugixml-${GNSSSDR_PUGIXML_LOCAL_VERSION}
+            BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR}/pugixml-${GNSSSDR_PUGIXML_LOCAL_VERSION}
+            CMAKE_ARGS ${PUGIXML_COMPILER} ${TOOLCHAIN_ARG}
+            UPDATE_COMMAND ""
+            PATCH_COMMAND ""
+            INSTALL_COMMAND ""
+        )
+    else(CMAKE_VERSION VERSION_LESS 3.2)
+        ExternalProject_Add(
+            pugixml-${GNSSSDR_PUGIXML_LOCAL_VERSION}
+            PREFIX ${CMAKE_CURRENT_BINARY_DIR}/pugixml-${GNSSSDR_PUGIXML_LOCAL_VERSION}
+            GIT_REPOSITORY https://github.com/zeux/pugixml
+            GIT_TAG v${GNSSSDR_PUGIXML_LOCAL_VERSION}
+            SOURCE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/thirdparty/pugixml/pugixml-${GNSSSDR_PUGIXML_LOCAL_VERSION}
+            BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR}/pugixml-${GNSSSDR_PUGIXML_LOCAL_VERSION}
+            CMAKE_ARGS ${PUGIXML_COMPILER} ${TOOLCHAIN_ARG}
+            UPDATE_COMMAND ""
+            PATCH_COMMAND ""
+            BUILD_BYPRODUCTS ${CMAKE_CURRENT_BINARY_DIR}/pugixml-${GNSSSDR_PUGIXML_LOCAL_VERSION}/${CMAKE_FIND_LIBRARY_PREFIXES}pugixml${CMAKE_STATIC_LIBRARY_SUFFIX}
+            INSTALL_COMMAND ""
+        )
+    endif(CMAKE_VERSION VERSION_LESS 3.2)
+    set(PUGIXML_LIBRARY ${CMAKE_CURRENT_BINARY_DIR}/pugixml-${GNSSSDR_PUGIXML_LOCAL_VERSION}/${CMAKE_FIND_LIBRARY_PREFIXES}pugixml${CMAKE_STATIC_LIBRARY_SUFFIX} )
+    set(PUGIXML_INCLUDE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/thirdparty/pugixml/pugixml-${GNSSSDR_PUGIXML_LOCAL_VERSION}/src )
+    set(PUGIXML_LOCAL true)
+endif(PugiXML_FOUND)
+
 
 ################################################################################
 # USRP Hardware Driver (UHD) - OPTIONAL

README.md

@@ -64,7 +64,8 @@ $ sudo apt-get install build-essential cmake git libboost-dev libboost-date-time
        libboost-system-dev libboost-filesystem-dev libboost-thread-dev libboost-chrono-dev \
        libboost-serialization-dev liblog4cpp5-dev libuhd-dev gnuradio-dev gr-osmosdr \
        libblas-dev liblapack-dev libarmadillo-dev libgflags-dev libgoogle-glog-dev \
-       libgnutls-openssl-dev libpcap-dev python-mako python-six libmatio-dev libgtest-dev
+       libgnutls-openssl-dev libpcap-dev python-mako python-six libmatio-dev libpugixml-dev \
+       libgtest-dev
 ~~~~~~
 
 Please note that the required files from `libgtest-dev` were moved to `googletest` in Debian 9 "stretch" and Ubuntu 18.04 "bionic", and moved back again to `libgtest-dev` in Debian 10 "buster" and Ubuntu 18.10 "cosmic".
@@ -85,7 +86,7 @@ $ sudo yum install make automake gcc gcc-c++ kernel-devel cmake git boost-devel
        boost-date-time boost-system boost-filesystem boost-thread boost-chrono \
        boost-serialization log4cpp-devel gnuradio-devel gr-osmosdr-devel \
        blas-devel lapack-devel matio-devel armadillo-devel gflags-devel \
-       glog-devel openssl-devel libpcap-devel python-mako python-six
+       glog-devel openssl-devel libpcap-devel python-mako python-six pugixml-devel
 ~~~~~~
 
 Once you have installed these packages, you can jump directly to [download the source code and build GNSS-SDR](#download-and-build-linux).
@@ -102,7 +103,7 @@ $ sudo yum install make automake gcc gcc-c++ kernel-devel libtool \
        hdf5-devel cmake git boost-devel boost-date-time boost-system \
        boost-filesystem boost-thread boost-chrono boost-serialization \
        log4cpp-devel gnuradio-devel gr-osmosdr-devel blas-devel lapack-devel \
-       armadillo-devel openssl-devel libpcap-devel python-mako python-six
+       armadillo-devel openssl-devel libpcap-devel python-mako python-six pugixml-devel
 ~~~~~~
 
 Once you have installed these packages, you can jump directly to [download the source code and build GNSS-SDR](#download-and-build-linux).
@@ -113,7 +114,7 @@ If you are using Arch Linux (with base-devel group installed):
 
 ~~~~~~
 $ pacman -S cmake git boost boost-libs log4cpp libvolk gnuradio gnuradio-osmosdr \
-       blas lapack gflags google-glog openssl python2-mako python2-six \
+       blas lapack gflags google-glog openssl pugixml python2-mako python2-six \
        libmatio libpcap gtest
 ~~~~~~
 
@@ -201,9 +202,9 @@ The full stop separated from ```cmake``` by a space is important. [CMake](https:
 #### Install [Gflags](https://github.com/gflags/gflags "Gflags' Homepage"), a commandline flags processing module for C++:
 
 ~~~~~~
-$ wget https://github.com/gflags/gflags/archive/v2.2.1.tar.gz
+$ wget https://github.com/gflags/gflags/archive/v2.2.2.tar.gz
-$ tar xvfz v2.2.1.tar.gz
+$ tar xvfz v2.2.2.tar.gz
-$ cd gflags-2.2.1
+$ cd gflags-2.2.2
 $ cmake -DBUILD_SHARED_LIBS=ON -DBUILD_STATIC_LIBS=OFF -DBUILD_gflags_nothreads_LIB=OFF .
 $ make
 $ sudo make install
@@ -547,6 +548,7 @@ $ sudo port install google-glog +gflags
 $ sudo port install py27-mako
 $ sudo port install py27-six
 $ sudo port install matio
+$ sudo port install pugixml
 ~~~~~~
 
 You also might need to activate a Python installation. The list of installed versions can be retrieved with:
@@ -586,6 +588,7 @@ $ brew install armadillo
 $ brew install glog gflags gnutls
 $ brew install gnuradio
 $ brew install libmatio
+$ brew install pugixml
 $ pip install mako
 $ pip install six
 ~~~~~~

cmake/Modules/FindPugiXML.cmake

@@ -0,0 +1,69 @@
+# Copyright (C) 2011-2018 (see AUTHORS file for a list of contributors)
+#
+# This file is part of GNSS-SDR.
+#
+# GNSS-SDR is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# GNSS-SDR is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
+
+
+# Find the pugixml XML parsing library.
+#
+# Sets the usual variables expected for find_package scripts:
+#
+# PUGIXML_INCLUDE_DIR - header location
+# PUGIXML_LIBRARIES - library to link against
+# PUGIXML_FOUND - true if pugixml was found.
+
+
+find_path (PUGIXML_INCLUDE_DIR
+           NAMES pugixml.hpp
+           PATHS ${PUGIXML_HOME}/include
+           /usr/include
+           /usr/local/include
+           /opt/local/include)
+
+find_library (PUGIXML_LIBRARY
+              NAMES pugixml
+              PATHS ${PUGIXML_HOME}/lib
+              /usr/lib/x86_64-linux-gnu
+              /usr/lib/aarch64-linux-gnu
+              /usr/lib/arm-linux-gnueabi
+              /usr/lib/arm-linux-gnueabihf
+              /usr/lib/i386-linux-gnu
+              /usr/lib/mips-linux-gnu
+              /usr/lib/mips64el-linux-gnuabi64
+              /usr/lib/mipsel-linux-gnu
+              /usr/lib/powerpc64le-linux-gnu
+              /usr/lib/s390x-linux-gnu
+              /usr/local/lib
+              /opt/local/lib
+              /usr/lib
+              /usr/lib64
+              /usr/local/lib64 )
+
+# Support the REQUIRED and QUIET arguments, and set PUGIXML_FOUND if found.
+include (FindPackageHandleStandardArgs)
+FIND_PACKAGE_HANDLE_STANDARD_ARGS (PugiXML DEFAULT_MSG PUGIXML_LIBRARY
+                                   PUGIXML_INCLUDE_DIR)
+
+if (PUGIXML_FOUND)
+    set (PUGIXML_LIBRARIES ${PUGIXML_LIBRARY})
+    if (NOT PugiXML_FIND_QUIETLY)
+        message (STATUS "PugiXML include = ${PUGIXML_INCLUDE_DIR}")
+        message (STATUS "PugiXML library = ${PUGIXML_LIBRARY}")
+    endif (NOT PugiXML_FIND_QUIETLY)
+else (PUGIXML_FOUND)
+    message (STATUS "PugiXML not found.")
+endif(PUGIXML_FOUND)
+
+mark_as_advanced (PUGIXML_LIBRARY PUGIXML_INCLUDE_DIR)

docs/xml-schemas/gal_almanac_map.xsd

@@ -15,9 +15,9 @@
                       <xs:complexType mixed="true">
                         <xs:sequence>
                           <xs:element type="xs:byte" name="i_satellite_PRN"/>
-                          <xs:element type="xs:float" name="d_Toa"/>
+                          <xs:element type="xs:byte" name="i_Toa"/>
-                          <xs:element type="xs:float" name="d_WNa"/>
+                          <xs:element type="xs:byte" name="i_WNa"/>
-                          <xs:element type="xs:float" name="d_IODa"/>
+                          <xs:element type="xs:byte" name="i_IODa"/>
                           <xs:element type="xs:float" name="d_Delta_i"/>
                           <xs:element type="xs:float" name="d_M_0"/>
                           <xs:element type="xs:float" name="d_e_eccentricity"/>
@@ -27,9 +27,9 @@
                           <xs:element type="xs:float" name="d_OMEGA_DOT"/>
                           <xs:element type="xs:float" name="d_A_f0"/>
                           <xs:element type="xs:float" name="d_A_f1"/>
-                          <xs:element type="xs:float" name="E5b_HS"/>
+                          <xs:element type="xs:byte" name="E5b_HS"/>
-                          <xs:element type="xs:float" name="E1B_HS"/>
+                          <xs:element type="xs:byte" name="E1B_HS"/>
-                          <xs:element type="xs:float" name="E5a_HS"/>
+                          <xs:element type="xs:byte" name="E5a_HS"/>
                         </xs:sequence>
                         <xs:attribute type="xs:byte" name="class_id" use="optional"/>
                         <xs:attribute type="xs:byte" name="tracking_level" use="optional"/>

docs/xml-schemas/gps_almanac_map.xsd

@@ -16,7 +16,7 @@
                         <xs:sequence>
                           <xs:element type="xs:byte" name="i_satellite_PRN"/>
                           <xs:element type="xs:float" name="d_Delta_i"/>
-                          <xs:element type="xs:float" name="d_Toa"/>
+                          <xs:element type="xs:byte" name="i_Toa"/>
                           <xs:element type="xs:byte" name="i_WNa"/>
                           <xs:element type="xs:float" name="d_M_0"/>
                           <xs:element type="xs:float" name="d_e_eccentricity"/>

src/algorithms/PVT/adapters/rtklib_pvt.cc

@@ -48,6 +48,27 @@ namespace bc = boost::integer;
 
 using google::LogMessage;
 
+
+bool RtklibPvt::get_latest_PVT(double* longitude_deg,
+    double* latitude_deg,
+    double* height_m,
+    double* ground_speed_kmh,
+    double* course_over_ground_deg,
+    time_t* UTC_time)
+{
+    return pvt_->get_latest_PVT(longitude_deg,
+        latitude_deg,
+        height_m,
+        ground_speed_kmh,
+        course_over_ground_deg,
+        UTC_time);
+}
+
+void RtklibPvt::clear_ephemeris()
+{
+    pvt_->clear_ephemeris();
+}
+
 RtklibPvt::RtklibPvt(ConfigurationInterface* configuration,
     std::string role,
     unsigned int in_streams,
@@ -516,6 +537,30 @@ RtklibPvt::~RtklibPvt()
 }
 
 
+std::map<int, Gps_Ephemeris> RtklibPvt::get_gps_ephemeris() const
+{
+    return pvt_->get_gps_ephemeris_map();
+}
+
+
+std::map<int, Galileo_Ephemeris> RtklibPvt::get_galileo_ephemeris() const
+{
+    return pvt_->get_galileo_ephemeris_map();
+}
+
+
+std::map<int, Gps_Almanac> RtklibPvt::get_gps_almanac() const
+{
+    return pvt_->get_gps_almanac_map();
+}
+
+
+std::map<int, Galileo_Almanac> RtklibPvt::get_galileo_almanac() const
+{
+    return pvt_->get_galileo_almanac_map();
+}
+
+
 void RtklibPvt::connect(gr::top_block_sptr top_block)
 {
     if (top_block)

src/algorithms/PVT/adapters/rtklib_pvt.h

@@ -57,12 +57,18 @@ public:
         return role_;
     }
 
-    //!  Returns "RTKLIB_Pvt"
+    //!  Returns "RTKLIB_PVT"
     inline std::string implementation() override
     {
         return "RTKLIB_PVT";
     }
 
+    void clear_ephemeris() override;
+    std::map<int, Gps_Ephemeris> get_gps_ephemeris() const override;
+    std::map<int, Galileo_Ephemeris> get_galileo_ephemeris() const override;
+    std::map<int, Gps_Almanac> get_gps_almanac() const override;
+    std::map<int, Galileo_Almanac> get_galileo_almanac() const override;
+
     void connect(gr::top_block_sptr top_block) override;
     void disconnect(gr::top_block_sptr top_block) override;
     gr::basic_block_sptr get_left_block() override;
@@ -79,6 +85,13 @@ public:
         return sizeof(gr_complex);
     }
 
+    bool get_latest_PVT(double* longitude_deg,
+        double* latitude_deg,
+        double* height_m,
+        double* ground_speed_kmh,
+        double* course_over_ground_deg,
+        time_t* UTC_time) override;
+
 private:
     rtklib_pvt_cc_sptr pvt_;
     rtk_t rtk;

src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h

@@ -31,7 +31,7 @@
 #ifndef GNSS_SDR_RTKLIB_PVT_CC_H
 #define GNSS_SDR_RTKLIB_PVT_CC_H
 
-
+#include "gps_ephemeris.h"
 #include "nmea_printer.h"
 #include "kml_printer.h"
 #include "gpx_printer.h"
@@ -40,6 +40,8 @@
 #include "rtcm_printer.h"
 #include "pvt_conf.h"
 #include "rtklib_solver.h"
+#include <boost/date_time/posix_time/posix_time.hpp>
+#include <boost/date_time/gregorian/gregorian.hpp>
 #include <gnuradio/sync_block.h>
 #include <sys/types.h>
 #include <sys/ipc.h>
@@ -60,7 +62,7 @@ rtklib_pvt_cc_sptr rtklib_make_pvt_cc(uint32_t n_channels,
     rtk_t& rtk);
 
 /*!
- * \brief This class implements a block that computes the PVT solution with Galileo E1 signals
+ * \brief This class implements a block that computes the PVT solution using the RTKLIB integrated library
  */
 class rtklib_pvt_cc : public gr::sync_block
 {
@@ -109,8 +111,9 @@ private:
     bool d_geojson_output_enabled;
     bool d_gpx_output_enabled;
     bool d_kml_output_enabled;
+    bool d_nmea_output_file_enabled;
 
-    std::shared_ptr<rtklib_solver> d_ls_pvt;
+    std::shared_ptr<rtklib_solver> d_pvt_solver;
 
     std::map<int, Gnss_Synchro> gnss_observables_map;
     bool observables_pairCompare_min(const std::pair<int, Gnss_Synchro>& a, const std::pair<int, Gnss_Synchro>& b);
@@ -135,6 +138,11 @@ private:
     bool d_xml_storage;
     std::string xml_base_path;
 
+    inline std::time_t to_time_t(boost::posix_time::ptime pt)
+    {
+        return (pt - boost::posix_time::ptime(boost::gregorian::date(1970, 1, 1))).total_seconds();
+    }
+
 
 public:
     rtklib_pvt_cc(uint32_t nchannels,
@@ -142,11 +150,32 @@ public:
         rtk_t& rtk);
 
     /*!
-     * \brief Get latest set of GPS L1 ephemeris from PVT block
+     * \brief Get latest set of ephemeris from PVT block
      *
-     * It is used to save the assistance data at the receiver shutdown
      */
-    std::map<int, Gps_Ephemeris> get_GPS_L1_ephemeris_map();
+    std::map<int, Gps_Ephemeris> get_gps_ephemeris_map() const;
+
+    std::map<int, Gps_Almanac> get_gps_almanac_map() const;
+
+    std::map<int, Galileo_Ephemeris> get_galileo_ephemeris_map() const;
+
+    std::map<int, Galileo_Almanac> get_galileo_almanac_map() const;
+
+    /*!
+     * \brief Clear all ephemeris information and the almanacs for GPS and Galileo
+     *
+     */
+    void clear_ephemeris();
+
+    /*!
+     * \brief Get the latest Position WGS84 [deg], Ground Velocity, Course over Ground, and UTC Time, if available
+     */
+    bool get_latest_PVT(double* longitude_deg,
+        double* latitude_deg,
+        double* height_m,
+        double* ground_speed_kmh,
+        double* course_over_ground_deg,
+        time_t* UTC_time);
 
     ~rtklib_pvt_cc();  //!< Default destructor
 

src/algorithms/PVT/libs/gpx_printer.cc

@@ -138,13 +138,15 @@ bool Gpx_Printer::set_headers(std::string filename, bool time_tag_name)
             gpx_file << std::setprecision(14);
             gpx_file << "<?xml version=\"1.0\" encoding=\"UTF-8\"?>" << std::endl
                      << "<gpx version=\"1.1\" creator=\"GNSS-SDR\"" << std::endl
-                     << "xsi:schemaLocation=\"http://www.topografix.com/GPX/1/1 http://www.topografix.com/GPX/1/1/gpx.xsd\"" << std::endl
+                     << indent << "xsi:schemaLocation=\"http://www.topografix.com/GPX/1/1 http://www.topografix.com/GPX/1/1/gpx.xsd http://www.garmin.com/xmlschemas/GpxExtensions/v3 http://www.garmin.com/xmlschemas/GpxExtensionsv3.xsd http://www.garmin.com/xmlschemas/TrackPointExtension/v2 http://www.garmin.com/xmlschemas/TrackPointExtensionv2.xsd\"" << std::endl
-                     << "xmlns=\"http://www.topografix.com/GPX/1/1\"" << std::endl
+                     << indent << "xmlns=\"http://www.topografix.com/GPX/1/1\"" << std::endl
-                     << "xmlns:xsi=\"http://www.w3.org/2001/XMLSchema-instance\">" << std::endl
+                     << indent << "xmlns:gpxx=\"http://www.garmin.com/xmlschemas/GpxExtensions/v3\"" << std::endl
-                     << "<trk>" << std::endl
+                     << indent << "xmlns:gpxtpx=\"http://www.garmin.com/xmlschemas/TrackPointExtension/v2\"" << std::endl
-                     << indent << "<name>Position fixes computed by GNSS-SDR v" << GNSS_SDR_VERSION << "</name>" << std::endl
+                     << indent << "xmlns:xsi=\"http://www.w3.org/2001/XMLSchema-instance\">" << std::endl
-                     << indent << "<desc>GNSS-SDR position log generated at " << pt << " (local time)</desc>" << std::endl
+                     << indent << "<trk>" << std::endl
-                     << indent << "<trkseg>" << std::endl;
+                     << indent << indent << "<name>Position fixes computed by GNSS-SDR v" << GNSS_SDR_VERSION << "</name>" << std::endl
+                     << indent << indent << "<desc>GNSS-SDR position log generated at " << pt << " (local time)</desc>" << std::endl
+                     << indent << indent << "<trkseg>" << std::endl;
             return true;
         }
     else
@@ -164,6 +166,9 @@ bool Gpx_Printer::print_position(const std::shared_ptr<rtklib_solver>& position,
     positions_printed = true;
     std::shared_ptr<rtklib_solver> position_ = position;
 
+    double speed_over_ground = position_->get_speed_over_ground();    // expressed in m/s
+    double course_over_ground = position_->get_course_over_ground();  // expressed in deg
+
     double hdop = position_->get_hdop();
     double vdop = position_->get_vdop();
     double pdop = position_->get_pdop();
@@ -187,9 +192,13 @@ bool Gpx_Printer::print_position(const std::shared_ptr<rtklib_solver>& position,
 
     if (gpx_file.is_open())
         {
-            gpx_file << indent << indent << "<trkpt lon=\"" << longitude << "\" lat=\"" << latitude << "\"><ele>" << height << "</ele>"
+            gpx_file << indent << indent << indent << "<trkpt lon=\"" << longitude << "\" lat=\"" << latitude << "\"><ele>" << height << "</ele>"
                      << "<time>" << utc_time << "</time>"
-                     << "<hdop>" << hdop << "</hdop><vdop>" << vdop << "</vdop><pdop>" << pdop << "</pdop></trkpt>" << std::endl;
+                     << "<hdop>" << hdop << "</hdop><vdop>" << vdop << "</vdop><pdop>" << pdop << "</pdop>"
+                     << "<extensions><gpxtpx:TrackPointExtension>"
+                     << "<gpxtpx:speed>" << speed_over_ground << "</gpxtpx:speed>"
+                     << "<gpxtpx:course>" << course_over_ground << "</gpxtpx:course>"
+                     << "</gpxtpx:TrackPointExtension></extensions></trkpt>" << std::endl;
             return true;
         }
     else
@@ -203,8 +212,8 @@ bool Gpx_Printer::close_file()
 {
     if (gpx_file.is_open())
         {
-            gpx_file << indent << "</trkseg>" << std::endl
+            gpx_file << indent << indent << "</trkseg>" << std::endl
-                     << "</trk>" << std::endl
+                     << indent << "</trk>" << std::endl
                      << "</gpx>";
             gpx_file.close();
             return true;

src/algorithms/PVT/libs/hybrid_ls_pvt.cc

@@ -150,8 +150,6 @@ bool hybrid_ls_pvt::get_PVT(std::map<int, Gnss_Synchro> gnss_observables_map, do
                                 // 4- fill the observations vector with the corrected observables
                                 obs.resize(valid_obs + 1, 1);
                                 obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + SV_clock_bias_s * GALILEO_C_m_s - this->get_time_offset_s() * GALILEO_C_m_s;
-                                this->set_visible_satellites_ID(valid_obs, galileo_ephemeris_iter->second.i_satellite_PRN);
-                                this->set_visible_satellites_CN0_dB(valid_obs, gnss_observables_iter->second.CN0_dB_hz);
 
                                 Galileo_week_number = galileo_ephemeris_iter->second.WN_5;  //for GST
                                 GST = galileo_ephemeris_iter->second.Galileo_System_Time(Galileo_week_number, hybrid_current_time);
@@ -213,8 +211,6 @@ bool hybrid_ls_pvt::get_PVT(std::map<int, Gnss_Synchro> gnss_observables_map, do
                                         double Code_bias_m = P1_P2 / (1.0 - Gamma);
                                         obs.resize(valid_obs + 1, 1);
                                         obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + dtr * GPS_C_m_s - Code_bias_m - this->get_time_offset_s() * GPS_C_m_s;
-                                        this->set_visible_satellites_ID(valid_obs, gps_ephemeris_iter->second.i_satellite_PRN);
-                                        this->set_visible_satellites_CN0_dB(valid_obs, gnss_observables_iter->second.CN0_dB_hz);
 
                                         // SV ECEF DEBUG OUTPUT
                                         LOG(INFO) << "(new)ECEF GPS L1 CA satellite SV ID=" << gps_ephemeris_iter->second.i_satellite_PRN
@@ -265,8 +261,6 @@ bool hybrid_ls_pvt::get_PVT(std::map<int, Gnss_Synchro> gnss_observables_map, do
                                         // 4- fill the observations vector with the corrected observables
                                         obs.resize(valid_obs + 1, 1);
                                         obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + dtr * GPS_C_m_s + SV_clock_bias_s * GPS_C_m_s;
-                                        this->set_visible_satellites_ID(valid_obs, gps_cnav_ephemeris_iter->second.i_satellite_PRN);
-                                        this->set_visible_satellites_CN0_dB(valid_obs, gnss_observables_iter->second.CN0_dB_hz);
 
                                         GPS_week = gps_cnav_ephemeris_iter->second.i_GPS_week;
                                         GPS_week = GPS_week % 1024;  //Necessary due to the increase of WN bits in CNAV message (10 in GPS NAV and 13 in CNAV)

src/algorithms/PVT/libs/kml_printer.cc

@@ -2,6 +2,7 @@
  * \file kml_printer.cc
  * \brief Implementation of a class that prints PVT information to a kml file
  * \author Javier Arribas, 2011. jarribas(at)cttc.es
+ *         Álvaro Cebrián Juan, 2018. acebrianjuan(at)gmail.com
  *
  *
  * -------------------------------------------------------------------------
@@ -43,6 +44,7 @@ using google::LogMessage;
 Kml_Printer::Kml_Printer(const std::string& base_path)
 {
     positions_printed = false;
+    indent = "  ";
     kml_base_path = base_path;
     boost::filesystem::path full_path(boost::filesystem::current_path());
     const boost::filesystem::path p(kml_base_path);
@@ -74,6 +76,14 @@ Kml_Printer::Kml_Printer(const std::string& base_path)
         }
 
     kml_base_path = kml_base_path + boost::filesystem::path::preferred_separator;
+
+    boost::filesystem::path tmp_base_path = boost::filesystem::temp_directory_path();
+    boost::filesystem::path tmp_filename = boost::filesystem::unique_path();
+    boost::filesystem::path tmp_file = tmp_base_path / tmp_filename;
+
+    tmp_file_str = tmp_file.string();
+
+    point_id = 0;
 }
 
 
@@ -127,36 +137,73 @@ bool Kml_Printer::set_headers(std::string filename, bool time_tag_name)
     kml_filename = kml_base_path + kml_filename;
     kml_file.open(kml_filename.c_str());
 
-    if (kml_file.is_open())
+    tmp_file.open(tmp_file_str.c_str());
+
+    if (kml_file.is_open() && tmp_file.is_open())
         {
             DLOG(INFO) << "KML printer writing on " << filename.c_str();
             // Set iostream numeric format and precision
             kml_file.setf(kml_file.fixed, kml_file.floatfield);
             kml_file << std::setprecision(14);
+
+            tmp_file.setf(tmp_file.fixed, tmp_file.floatfield);
+            tmp_file << std::setprecision(14);
+
             kml_file << "<?xml version=\"1.0\" encoding=\"UTF-8\"?>" << std::endl
-                     << "<kml xmlns=\"http://www.opengis.net/kml/2.2\">" << std::endl
+                     << "<kml xmlns=\"http://www.opengis.net/kml/2.2\" xmlns:gx=\"http://www.google.com/kml/ext/2.2\">" << std::endl
-                     << "    <Document>" << std::endl
+                     << indent << "<Document>" << std::endl
-                     << "    <name>GNSS Track</name>" << std::endl
+                     << indent << indent << "<name>GNSS Track</name>" << std::endl
-                     << "    <description>GNSS-SDR Receiver position log file created at " << pt
+                     << indent << indent << "<description><![CDATA[" << std::endl
-                     << "    </description>" << std::endl
+                     << indent << indent << indent << "<table>" << std::endl
-                     << "<Style id=\"yellowLineGreenPoly\">" << std::endl
+                     << indent << indent << indent << indent << "<tr><td>GNSS-SDR Receiver position log file created at " << pt << "</td></tr>" << std::endl
-                     << " <LineStyle>" << std::endl
+                     << indent << indent << indent << indent << "<tr><td>https://gnss-sdr.org/</td></tr>" << std::endl
-                     << "     <color>7f00ffff</color>" << std::endl
+                     << indent << indent << indent << "</table>" << std::endl
-                     << "        <width>1</width>" << std::endl
+                     << indent << indent << "]]></description>" << std::endl
-                     << "    </LineStyle>" << std::endl
+                     << indent << indent << "<!-- Normal track style -->" << std::endl
-                     << "<PolyStyle>" << std::endl
+                     << indent << indent << "<Style id=\"track_n\">" << std::endl
-                     << "    <color>7f00ff00</color>" << std::endl
+                     << indent << indent << indent << "<IconStyle>" << std::endl
-                     << "</PolyStyle>" << std::endl
+                     << indent << indent << indent << indent << "<color>ff00ffff</color>" << std::endl
-                     << "</Style>" << std::endl
+                     << indent << indent << indent << indent << "<scale>0.3</scale>" << std::endl
-                     << "<Placemark>" << std::endl
+                     << indent << indent << indent << indent << "<Icon>" << std::endl
-                     << "<name>GNSS-SDR PVT</name>" << std::endl
+                     << indent << indent << indent << indent << indent << "<href>http://maps.google.com/mapfiles/kml/shapes/shaded_dot.png</href>" << std::endl
-                     << "<description>GNSS-SDR position log</description>" << std::endl
+                     << indent << indent << indent << indent << "</Icon>" << std::endl
-                     << "<styleUrl>#yellowLineGreenPoly</styleUrl>" << std::endl
+                     << indent << indent << indent << "</IconStyle>" << std::endl
-                     << "<LineString>" << std::endl
+                     << indent << indent << indent << "<LabelStyle>" << std::endl
-                     << "<extrude>0</extrude>" << std::endl
+                     << indent << indent << indent << indent << "<scale>0</scale>" << std::endl
-                     << "<tessellate>1</tessellate>" << std::endl
+                     << indent << indent << indent << "</LabelStyle>" << std::endl
-                     << "<altitudeMode>absolute</altitudeMode>" << std::endl
+                     << indent << indent << "</Style>" << std::endl
-                     << "<coordinates>" << std::endl;
+                     << indent << indent << "<!-- Highlighted track style -->" << std::endl
+                     << indent << indent << "<Style id=\"track_h\">" << std::endl
+                     << indent << indent << indent << "<IconStyle>" << std::endl
+                     << indent << indent << indent << indent << "<color>ff00ffff</color>" << std::endl
+                     << indent << indent << indent << indent << "<scale>1</scale>" << std::endl
+                     << indent << indent << indent << indent << "<Icon>" << std::endl
+                     << indent << indent << indent << indent << indent << "<href>http://maps.google.com/mapfiles/kml/shapes/shaded_dot.png</href>" << std::endl
+                     << indent << indent << indent << indent << "</Icon>" << std::endl
+                     << indent << indent << indent << "</IconStyle>" << std::endl
+                     << indent << indent << "</Style>" << std::endl
+                     << indent << indent << "<StyleMap id=\"track\">" << std::endl
+                     << indent << indent << indent << "<Pair>" << std::endl
+                     << indent << indent << indent << indent << "<key>normal</key>" << std::endl
+                     << indent << indent << indent << indent << "<styleUrl>#track_n</styleUrl>" << std::endl
+                     << indent << indent << indent << "</Pair>" << std::endl
+                     << indent << indent << indent << "<Pair>" << std::endl
+                     << indent << indent << indent << indent << "<key>highlight</key>" << std::endl
+                     << indent << indent << indent << indent << "<styleUrl>#track_h</styleUrl>" << std::endl
+                     << indent << indent << indent << "</Pair>" << std::endl
+                     << indent << indent << "</StyleMap>" << std::endl
+                     << indent << indent << "<Style id=\"yellowLineGreenPoly\">" << std::endl
+                     << indent << indent << indent << "<LineStyle>" << std::endl
+                     << indent << indent << indent << indent << "<color>7f00ffff</color>" << std::endl
+                     << indent << indent << indent << indent << "<width>1</width>" << std::endl
+                     << indent << indent << indent << "</LineStyle>" << std::endl
+                     << indent << indent << indent << "<PolyStyle>" << std::endl
+                     << indent << indent << indent << indent << "<color>7f00ff00</color>" << std::endl
+                     << indent << indent << indent << "</PolyStyle>" << std::endl
+                     << indent << indent << "</Style>" << std::endl
+                     << indent << indent << "<Folder>" << std::endl
+                     << indent << indent << indent << "<name>Points</name>" << std::endl;
+
             return true;
         }
     else
@@ -167,7 +214,7 @@ bool Kml_Printer::set_headers(std::string filename, bool time_tag_name)
 }
 
 
-bool Kml_Printer::print_position(const std::shared_ptr<Pvt_Solution>& position, bool print_average_values)
+bool Kml_Printer::print_position(const std::shared_ptr<rtklib_solver>& position, bool print_average_values)
 {
     double latitude;
     double longitude;
@@ -175,7 +222,18 @@ bool Kml_Printer::print_position(const std::shared_ptr<Pvt_Solution>& position,
 
     positions_printed = true;
 
-    std::shared_ptr<Pvt_Solution> position_ = position;
+    std::shared_ptr<rtklib_solver> position_ = position;
+
+    double speed_over_ground = position_->get_speed_over_ground();    // expressed in m/s
+    double course_over_ground = position_->get_course_over_ground();  // expressed in deg
+
+    double hdop = position_->get_hdop();
+    double vdop = position_->get_vdop();
+    double pdop = position_->get_pdop();
+    std::string utc_time = to_iso_extended_string(position_->get_position_UTC_time());
+    if (utc_time.length() < 23) utc_time += ".";
+    utc_time.resize(23, '0');  // time up to ms
+    utc_time.append("Z");      // UTC time zone
 
     if (print_average_values == false)
         {
@@ -190,9 +248,38 @@ bool Kml_Printer::print_position(const std::shared_ptr<Pvt_Solution>& position,
             height = position_->get_avg_height();
         }
 
-    if (kml_file.is_open())
+    if (kml_file.is_open() && tmp_file.is_open())
         {
-            kml_file << longitude << "," << latitude << "," << height << std::endl;
+            point_id++;
+            kml_file << indent << indent << indent << "<Placemark>" << std::endl
+                     << indent << indent << indent << indent << "<name>" << point_id << "</name>" << std::endl
+                     << indent << indent << indent << indent << "<snippet/>" << std::endl
+                     << indent << indent << indent << indent << "<description><![CDATA[" << std::endl
+                     << indent << indent << indent << indent << indent << "<table>" << std::endl
+                     << indent << indent << indent << indent << indent << indent << "<tr><td>Time:</td><td>" << utc_time << "</td></tr>" << std::endl
+                     << indent << indent << indent << indent << indent << indent << "<tr><td>Longitude:</td><td>" << longitude << "</td><td>deg</td></tr>" << std::endl
+                     << indent << indent << indent << indent << indent << indent << "<tr><td>Latitude:</td><td>" << latitude << "</td><td>deg</td></tr>" << std::endl
+                     << indent << indent << indent << indent << indent << indent << "<tr><td>Altitude:</td><td>" << height << "</td><td>m</td></tr>" << std::endl
+                     << indent << indent << indent << indent << indent << indent << "<tr><td>Speed:</td><td>" << speed_over_ground << "</td><td>m/s</td></tr>" << std::endl
+                     << indent << indent << indent << indent << indent << indent << "<tr><td>Course:</td><td>" << course_over_ground << "</td><td>deg</td></tr>" << std::endl
+                     << indent << indent << indent << indent << indent << indent << "<tr><td>HDOP:</td><td>" << hdop << "</td></tr>" << std::endl
+                     << indent << indent << indent << indent << indent << indent << "<tr><td>VDOP:</td><td>" << vdop << "</td></tr>" << std::endl
+                     << indent << indent << indent << indent << indent << indent << "<tr><td>PDOP:</td><td>" << pdop << "</td></tr>" << std::endl
+                     << indent << indent << indent << indent << indent << "</table>" << std::endl
+                     << indent << indent << indent << indent << "]]></description>" << std::endl
+                     << indent << indent << indent << indent << "<TimeStamp>" << std::endl
+                     << indent << indent << indent << indent << indent << "<when>" << utc_time << "</when>" << std::endl
+                     << indent << indent << indent << indent << "</TimeStamp>" << std::endl
+                     << indent << indent << indent << indent << "<styleUrl>#track</styleUrl>" << std::endl
+                     << indent << indent << indent << indent << "<Point>" << std::endl
+                     << indent << indent << indent << indent << indent << "<altitudeMode>absolute</altitudeMode>" << std::endl
+                     << indent << indent << indent << indent << indent << "<coordinates>" << longitude << "," << latitude << "," << height << "</coordinates>" << std::endl
+                     << indent << indent << indent << indent << "</Point>" << std::endl
+                     << indent << indent << indent << "</Placemark>" << std::endl;
+
+            tmp_file << indent << indent << indent << indent << indent
+                     << longitude << "," << latitude << "," << height << std::endl;
+
             return true;
         }
     else
@@ -204,14 +291,32 @@ bool Kml_Printer::print_position(const std::shared_ptr<Pvt_Solution>& position,
 
 bool Kml_Printer::close_file()
 {
-    if (kml_file.is_open())
+    if (kml_file.is_open() && tmp_file.is_open())
         {
-            kml_file << "</coordinates>" << std::endl
+            tmp_file.close();
-                     << "</LineString>" << std::endl
+
-                     << "</Placemark>" << std::endl
+            kml_file << indent << indent << "</Folder>"
-                     << "</Document>" << std::endl
+                     << indent << indent << "<Placemark>" << std::endl
+                     << indent << indent << indent << "<name>Path</name>" << std::endl
+                     << indent << indent << indent << "<styleUrl>#yellowLineGreenPoly</styleUrl>" << std::endl
+                     << indent << indent << indent << "<LineString>" << std::endl
+                     << indent << indent << indent << indent << "<extrude>0</extrude>" << std::endl
+                     << indent << indent << indent << indent << "<tessellate>1</tessellate>" << std::endl
+                     << indent << indent << indent << indent << "<altitudeMode>absolute</altitudeMode>" << std::endl
+                     << indent << indent << indent << indent << "<coordinates>" << std::endl;
+
+            // Copy the contents of tmp_file into kml_file
+            std::ifstream src(tmp_file_str, std::ios::binary);
+            kml_file << src.rdbuf();
+
+            kml_file << indent << indent << indent << indent << "</coordinates>" << std::endl
+                     << indent << indent << indent << "</LineString>" << std::endl
+                     << indent << indent << "</Placemark>" << std::endl
+                     << indent << "</Document>" << std::endl
                      << "</kml>";
+
             kml_file.close();
+
             return true;
         }
     else

src/algorithms/PVT/libs/kml_printer.h

@@ -2,7 +2,7 @@
  * \file kml_printer.h
  * \brief Interface of a class that prints PVT information to a kml file
  * \author Javier Arribas, 2011. jarribas(at)cttc.es
- *
+ *         Álvaro Cebrián Juan, 2018. acebrianjuan(at)gmail.com
  *
  * -------------------------------------------------------------------------
  *
@@ -34,6 +34,7 @@
 #define GNSS_SDR_KML_PRINTER_H_
 
 #include "pvt_solution.h"
+#include "rtklib_solver.h"
 #include <fstream>
 #include <memory>
 #include <string>
@@ -48,15 +49,19 @@ class Kml_Printer
 {
 private:
     std::ofstream kml_file;
+    std::ofstream tmp_file;
     bool positions_printed;
     std::string kml_filename;
     std::string kml_base_path;
+    std::string tmp_file_str;
+    unsigned int point_id;
+    std::string indent;
 
 public:
     Kml_Printer(const std::string& base_path = std::string("."));
     ~Kml_Printer();
     bool set_headers(std::string filename, bool time_tag_name = true);
-    bool print_position(const std::shared_ptr<Pvt_Solution>& position, bool print_average_values);
+    bool print_position(const std::shared_ptr<rtklib_solver>& position, bool print_average_values);
     bool close_file();
 };
 

src/algorithms/PVT/libs/ls_pvt.cc

@@ -31,6 +31,7 @@
 
 #include "ls_pvt.h"
 #include "GPS_L1_CA.h"
+#include "geofunctions.h"
 #include <glog/logging.h>
 #include <exception>
 #include <stdexcept>
@@ -235,15 +236,12 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat& satpos, const arma::vec& obs,
                             double* azim = 0;
                             double* elev = 0;
                             double* dist = 0;
-                            Ls_Pvt::topocent(azim, elev, dist, pos.subvec(0, 2), Rot_X - pos.subvec(0, 2));
+                            topocent(azim, elev, dist, pos.subvec(0, 2), Rot_X - pos.subvec(0, 2));
-                            this->set_visible_satellites_Az(i, *azim);
-                            this->set_visible_satellites_El(i, *elev);
-                            this->set_visible_satellites_Distance(i, *dist);
 
                             if (traveltime < 0.1 && nmbOfSatellites > 3)
                                 {
                                     //--- Find receiver's height
-                                    Ls_Pvt::togeod(&dphi, &dlambda, &h, 6378137.0, 298.257223563, pos(0), pos(1), pos(2));
+                                    togeod(&dphi, &dlambda, &h, 6378137.0, 298.257223563, pos(0), pos(1), pos(2));
                                     // Add troposphere correction if the receiver is below the troposphere
                                     if (h > 15000)
                                         {
@@ -253,7 +251,7 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat& satpos, const arma::vec& obs,
                                     else
                                         {
                                             //--- Find delay due to troposphere (in meters)
-                                            Ls_Pvt::tropo(&trop, sin(this->get_visible_satellites_El(i) * GPS_PI / 180.0), h / 1000.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0);
+                                            Ls_Pvt::tropo(&trop, sin(*elev * GPS_PI / 180.0), h / 1000.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0);
                                             if (trop > 5.0) trop = 0.0;  //check for erratic values
                                         }
                                 }
@@ -280,9 +278,6 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat& satpos, const arma::vec& obs,
                 }
         }
 
-    //-- compute the Dilution Of Precision values
-    //this->set_Q(arma::inv(arma::htrans(A) * A));
-
     // check the consistency of the PVT solution
     if (((fabs(pos(3)) * 1000.0) / GPS_C_m_s) > GPS_STARTOFFSET_ms * 2)
         {

src/algorithms/PVT/libs/nmea_printer.cc

@@ -34,6 +34,7 @@
  */
 
 #include "nmea_printer.h"
+#include "rtklib_solution.h"
 #include <boost/date_time/posix_time/posix_time.hpp>
 #include <boost/filesystem/operations.hpp>   // for create_directories, exists
 #include <boost/filesystem/path.hpp>         // for path, operator<<
@@ -376,99 +377,10 @@ std::string Nmea_Printer::get_UTC_NMEA_time(boost::posix_time::ptime d_position_
 std::string Nmea_Printer::get_GPRMC()
 {
     // Sample -> $GPRMC,161229.487,A,3723.2475,N,12158.3416,W,0.13,309.62,120598,*10
-    bool valid_fix = d_PVT_data->is_valid_position();
-
-    // ToDo: Compute speed and course over ground
-    double speed_over_ground_knots = 0;
-    double course_over_ground_deg = 0;
-
-    // boost::posix_time::ptime d_position_UTC_time=boost::posix_time::microsec_clock::universal_time();
-
     std::stringstream sentence_str;
-
+    unsigned char buff[1024] = {0};
-    // GPRMC (RMC-Recommended,Minimum Specific GNSS Data)
+    outnmea_rmc(buff, &d_PVT_data->pvt_sol);
-    std::string sentence_header;
+    sentence_str << buff;
-    sentence_header = "$GPRMC,";
-    sentence_str << sentence_header;
-
-    // UTC Time: hhmmss.sss
-    sentence_str << get_UTC_NMEA_time(d_PVT_data->get_position_UTC_time());
-
-    // Status: A: data valid, V: data NOT valid
-    if (valid_fix == true)
-        {
-            sentence_str << ",A";
-        }
-    else
-        {
-            sentence_str << ",V";
-        };
-
-    if (print_avg_pos == true)
-        {
-            // Latitude ddmm.mmmm,(N or S)
-            sentence_str << "," << latitude_to_hm(d_PVT_data->get_avg_latitude());
-            // longitude dddmm.mmmm,(E or W)
-            sentence_str << "," << longitude_to_hm(d_PVT_data->get_avg_longitude());
-        }
-    else
-        {
-            // Latitude ddmm.mmmm,(N or S)
-            sentence_str << "," << latitude_to_hm(d_PVT_data->get_latitude());
-            // longitude dddmm.mmmm,(E or W)
-            sentence_str << "," << longitude_to_hm(d_PVT_data->get_longitude());
-        }
-
-    // Speed over ground (knots)
-    sentence_str << ",";
-    sentence_str.setf(std::ios::fixed, std::ios::floatfield);
-    sentence_str.precision(2);
-    sentence_str << speed_over_ground_knots;
-
-    // course over ground (degrees)
-    sentence_str << ",";
-    sentence_str.setf(std::ios::fixed, std::ios::floatfield);
-    sentence_str.precision(2);
-    sentence_str << course_over_ground_deg;
-
-    // Date ddmmyy
-    boost::gregorian::date sentence_date = d_PVT_data->get_position_UTC_time().date();
-    unsigned int year = sentence_date.year();
-    unsigned int day = sentence_date.day();
-    unsigned int month = sentence_date.month();
-
-    sentence_str << ",";
-    sentence_str.width(2);
-    sentence_str.fill('0');
-    sentence_str << day;
-    sentence_str.width(2);
-    sentence_str.fill('0');
-    sentence_str << month;
-
-    std::stringstream year_strs;
-    year_strs << std::dec << year;
-    sentence_str << std::dec << year_strs.str().substr(2);
-
-    // Magnetic Variation (degrees)
-    // ToDo: Implement magnetic compass
-    sentence_str << ",";
-
-    // Magnetic Variation (E or W)
-    // ToDo: Implement magnetic compass
-    sentence_str << ",";
-
-    // Checksum
-    char checksum;
-    std::string tmpstr;
-    tmpstr = sentence_str.str();
-    checksum = checkSum(tmpstr.substr(1));
-    sentence_str << "*";
-    sentence_str.width(2);
-    sentence_str.fill('0');
-    sentence_str << std::hex << static_cast<int>(checksum);
-
-    // end NMEA sentence
-    sentence_str << "\r\n";
     return sentence_str.str();
 }
 
@@ -477,82 +389,10 @@ std::string Nmea_Printer::get_GPGSA()
 {
     // $GPGSA,A,3,07,02,26,27,09,04,15, , , , , ,1.8,1.0,1.5*33
     // GSA-GNSS DOP and Active Satellites
-    bool valid_fix = d_PVT_data->is_valid_position();
-    int n_sats_used = d_PVT_data->get_num_valid_observations();
-    double pdop = d_PVT_data->get_pdop();
-    double hdop = d_PVT_data->get_hdop();
-    double vdop = d_PVT_data->get_vdop();
-
     std::stringstream sentence_str;
-    std::string sentence_header;
+    unsigned char buff[1024] = {0};
-    sentence_header = "$GPGSA,";
+    outnmea_gsa(buff, &d_PVT_data->pvt_sol, d_PVT_data->pvt_ssat);
-    sentence_str << sentence_header;
+    sentence_str << buff;
-
-    // mode1:
-    // (M) Manual-forced to operate in 2D or 3D mode
-    // (A) Automatic-allowed to automatically switch 2D/3D
-    std::string mode1 = "M";
-    sentence_str << mode1;
-
-    // mode2:
-    // 1 fix not available
-    // 2 fix 2D
-    // 3 fix 3D
-    if (valid_fix == true)
-        {
-            sentence_str << ",3";
-        }
-    else
-        {
-            sentence_str << ",1";
-        };
-
-    // Used satellites
-    for (int i = 0; i < 12; i++)
-        {
-            sentence_str << ",";
-            if (i < n_sats_used)
-                {
-                    sentence_str.width(2);
-                    sentence_str.fill('0');
-                    sentence_str << d_PVT_data->get_visible_satellites_ID(i);
-                }
-        }
-
-    // PDOP
-    sentence_str << ",";
-    sentence_str.setf(std::ios::fixed, std::ios::floatfield);
-    sentence_str.width(2);
-    sentence_str.precision(1);
-    sentence_str.fill('0');
-    sentence_str << pdop;
-    // HDOP
-    sentence_str << ",";
-    sentence_str.setf(std::ios::fixed, std::ios::floatfield);
-    sentence_str.width(2);
-    sentence_str.precision(1);
-    sentence_str.fill('0');
-    sentence_str << hdop;
-    // VDOP
-    sentence_str << ",";
-    sentence_str.setf(std::ios::fixed, std::ios::floatfield);
-    sentence_str.width(2);
-    sentence_str.precision(1);
-    sentence_str.fill('0');
-    sentence_str << vdop;
-
-    // Checksum
-    char checksum;
-    std::string tmpstr;
-    tmpstr = sentence_str.str();
-    checksum = checkSum(tmpstr.substr(1));
-    sentence_str << "*";
-    sentence_str.width(2);
-    sentence_str.fill('0');
-    sentence_str << std::hex << static_cast<int>(checksum);
-
-    // end NMEA sentence
-    sentence_str << "\r\n";
     return sentence_str.str();
 }
 
@@ -560,199 +400,22 @@ std::string Nmea_Printer::get_GPGSA()
 std::string Nmea_Printer::get_GPGSV()
 {
     // GSV-GNSS Satellites in View
-    // Notice that NMEA 2.1 only supports 12 channels
-    int n_sats_used = d_PVT_data->get_num_valid_observations();
-    std::stringstream sentence_str;
-    std::stringstream frame_str;
-    std::string sentence_header;
-    sentence_header = "$GPGSV,";
-    char checksum;
-    std::string tmpstr;
-
-    // 1st step: How many GPGSV frames we need? (up to 3)
-    // Each frame contains up to 4 satellites
-    int n_frames;
-    n_frames = std::ceil((static_cast<double>(n_sats_used)) / 4.0);
-
-    // generate the frames
-    int current_satellite = 0;
-    for (int i = 1; i < (n_frames + 1); i++)
-        {
-            frame_str.str("");
-            frame_str << sentence_header;
-
-            // number of messages
-            frame_str << n_frames;
-
-            // message number
-            frame_str << ",";
-            frame_str << i;
-
-            // total number of satellites in view
-            frame_str << ",";
-            frame_str.width(2);
-            frame_str.fill('0');
-            frame_str << std::dec << n_sats_used;
-
-            // satellites info
-            for (int j = 0; j < 4; j++)
-                {
-                    // write satellite info
-                    frame_str << ",";
-                    frame_str.width(2);
-                    frame_str.fill('0');
-                    frame_str << std::dec << d_PVT_data->get_visible_satellites_ID(current_satellite);
-
-                    frame_str << ",";
-                    frame_str.width(2);
-                    frame_str.fill('0');
-                    frame_str << std::dec << static_cast<int>(d_PVT_data->get_visible_satellites_El(current_satellite));
-
-                    frame_str << ",";
-                    frame_str.width(3);
-                    frame_str.fill('0');
-                    frame_str << std::dec << static_cast<int>(d_PVT_data->get_visible_satellites_Az(current_satellite));
-
-                    frame_str << ",";
-                    frame_str.width(2);
-                    frame_str.fill('0');
-                    frame_str << std::dec << static_cast<int>(d_PVT_data->get_visible_satellites_CN0_dB(current_satellite));
-
-                    current_satellite++;
-
-                    if (current_satellite == n_sats_used)
-                        {
-                            break;
-                        }
-                }
-
-            // frame checksum
-            tmpstr = frame_str.str();
-            checksum = checkSum(tmpstr.substr(1));
-            frame_str << "*";
-            frame_str.width(2);
-            frame_str.fill('0');
-            frame_str << std::hex << static_cast<int>(checksum);
-
-            // end NMEA sentence
-            frame_str << "\r\n";
-
-            //add frame to sentence
-            sentence_str << frame_str.str();
-        }
-    return sentence_str.str();
     // $GPGSV,2,1,07,07,79,048,42,02,51,062,43,26,36,256,42,27,27,138,42*71
+    // Notice that NMEA 2.1 only supports 12 channels
+    std::stringstream sentence_str;
+    unsigned char buff[1024] = {0};
+    outnmea_gsv(buff, &d_PVT_data->pvt_sol, d_PVT_data->pvt_ssat);
+    sentence_str << buff;
+    return sentence_str.str();
 }
 
 
 std::string Nmea_Printer::get_GPGGA()
 {
-    // boost::posix_time::ptime d_position_UTC_time=boost::posix_time::microsec_clock::universal_time();
-    bool valid_fix = d_PVT_data->is_valid_position();
-    int n_channels = d_PVT_data->get_num_valid_observations();  //d_nchannels
-    double hdop = d_PVT_data->get_hdop();
-    double MSL_altitude;
-
-    if (d_PVT_data->is_averaging() == true)
-        {
-            MSL_altitude = d_PVT_data->get_avg_height();
-        }
-    else
-        {
-            MSL_altitude = d_PVT_data->get_height();
-        }
-
     std::stringstream sentence_str;
-
+    unsigned char buff[1024] = {0};
-    // GPGGA (Global Positioning System Fixed Data)
+    outnmea_gga(buff, &d_PVT_data->pvt_sol);
-    std::string sentence_header;
+    sentence_str << buff;
-    sentence_header = "$GPGGA,";
-    sentence_str << sentence_header;
-
-    // UTC Time: hhmmss.sss
-    sentence_str << get_UTC_NMEA_time(d_PVT_data->get_position_UTC_time());
-
-    if (d_PVT_data->is_averaging() == true)
-        {
-            // Latitude ddmm.mmmm,(N or S)
-            sentence_str << "," << latitude_to_hm(d_PVT_data->get_avg_latitude());
-            // longitude dddmm.mmmm,(E or W)
-            sentence_str << "," << longitude_to_hm(d_PVT_data->get_avg_longitude());
-        }
-    else
-        {
-            // Latitude ddmm.mmmm,(N or S)
-            sentence_str << "," << latitude_to_hm(d_PVT_data->get_latitude());
-            // longitude dddmm.mmmm,(E or W)
-            sentence_str << "," << longitude_to_hm(d_PVT_data->get_longitude());
-        }
-
-    // Position fix indicator
-    // 0 - Fix not available or invalid
-    // 1 - GPS SPS Mode, fix valid
-    // 2 - Differential GPS, SPS Mode, fix valid
-    // 3-5 - Not supported
-    // 6 - Dead Reckoning Mode, fix valid
-    // ToDo: Update PVT module to identify the fix mode
-
-    if (valid_fix == true)
-        {
-            sentence_str << ",1";
-        }
-    else
-        {
-            sentence_str << ",0";
-        }
-
-    // Number of satellites used in PVT
-    sentence_str << ",";
-    if (n_channels < 10)
-        {
-            sentence_str << '0' << n_channels;
-        }
-    else
-        {
-            sentence_str << n_channels;
-        }
-
-    // HDOP
-    sentence_str << ",";
-    sentence_str.setf(std::ios::fixed, std::ios::floatfield);
-    sentence_str.width(2);
-    sentence_str.precision(1);
-    sentence_str.fill('0');
-    sentence_str << hdop;
-
-    // MSL Altitude
-    sentence_str << ",";
-    sentence_str.precision(1);
-    sentence_str << MSL_altitude;
-    sentence_str << ",M";
-
-    // Geoid-to-ellipsoid separation. Ellipsoid altitude = MSL Altitude + Geoid Separation.
-    // ToDo: Compute this value
-    sentence_str << ",";
-    sentence_str << "0.0";
-    sentence_str << ",M";
-
-    // Age of Diff. Corr.  (Seconds) Null fields when DGPS is not used
-    // Diff. Ref. Station ID (0000)
-    // ToDo: Implement this fields for Differential GPS
-    sentence_str << ",";
-    sentence_str << "0.0,0000";
-
-    // Checksum
-    char checksum;
-    std::string tmpstr;
-    tmpstr = sentence_str.str();
-    checksum = checkSum(tmpstr.substr(1));
-    sentence_str << "*";
-    sentence_str.width(2);
-    sentence_str.fill('0');
-    sentence_str << std::hex << static_cast<int>(checksum);
-
-    // end NMEA sentence
-    sentence_str << "\r\n";
     return sentence_str.str();
     // $GPGGA,104427.591,5920.7009,N,01803.2938,E,1,05,3.3,78.2,M,23.2,M,0.0,0000*4A
 }

src/algorithms/PVT/libs/pvt_solution.cc

@@ -31,6 +31,7 @@
 
 #include "pvt_solution.h"
 #include "GPS_L1_CA.h"
+#include "geofunctions.h"
 #include <glog/logging.h>
 #include <exception>
 
@@ -43,6 +44,8 @@ Pvt_Solution::Pvt_Solution()
     d_latitude_d = 0.0;
     d_longitude_d = 0.0;
     d_height_m = 0.0;
+    d_speed_over_ground_m_s = 0.0;
+    d_course_over_ground_d = 0.0;
     d_avg_latitude_d = 0.0;
     d_avg_longitude_d = 0.0;
     d_avg_height_m = 0.0;
@@ -126,125 +129,7 @@ int Pvt_Solution::cart2geo(double X, double Y, double Z, int elipsoid_selection)
     d_latitude_d = phi * 180.0 / GPS_PI;
     d_longitude_d = lambda * 180.0 / GPS_PI;
     d_height_m = h;
-    return 0;
+    //todo: refactor this class. Mix of duplicated functions, use either RTKLIB geodetic functions or geofunctions.h
-}
-
-
-int Pvt_Solution::togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z)
-{
-    /* Subroutine to calculate geodetic coordinates latitude, longitude,
-       height given Cartesian coordinates X,Y,Z, and reference ellipsoid
-       values semi-major axis (a) and the inverse of flattening (finv).
-
-       The output units of angular quantities will be in decimal degrees
-       (15.5 degrees not 15 deg 30 min). The output units of h will be the
-       same as the units of X,Y,Z,a.
-
-       Inputs:
-               a           - semi-major axis of the reference ellipsoid
-               finv        - inverse of flattening of the reference ellipsoid
-               X,Y,Z       - Cartesian coordinates
-
-       Outputs:
-               dphi        - latitude
-               dlambda     - longitude
-               h           - height above reference ellipsoid
-
-       Based in a Matlab function by Kai Borre
-     */
-
-    *h = 0;
-    double tolsq = 1.e-10;  // tolerance to accept convergence
-    int maxit = 10;         // max number of iterations
-    double rtd = 180.0 / GPS_PI;
-
-    // compute square of eccentricity
-    double esq;
-    if (finv < 1.0E-20)
-        {
-            esq = 0.0;
-        }
-    else
-        {
-            esq = (2.0 - 1.0 / finv) / finv;
-        }
-
-    // first guess
-    double P = sqrt(X * X + Y * Y);  // P is distance from spin axis
-
-    //direct calculation of longitude
-    if (P > 1.0E-20)
-        {
-            *dlambda = atan2(Y, X) * rtd;
-        }
-    else
-        {
-            *dlambda = 0.0;
-        }
-
-    // correct longitude bound
-    if (*dlambda < 0)
-        {
-            *dlambda = *dlambda + 360.0;
-        }
-
-    double r = sqrt(P * P + Z * Z);  // r is distance from origin (0,0,0)
-
-    double sinphi;
-    if (r > 1.0E-20)
-        {
-            sinphi = Z / r;
-        }
-    else
-        {
-            sinphi = 0.0;
-        }
-    *dphi = asin(sinphi);
-
-    // initial value of height  =  distance from origin minus
-    // approximate distance from origin to surface of ellipsoid
-    if (r < 1.0E-20)
-        {
-            *h = 0;
-            return 1;
-        }
-
-    *h = r - a * (1 - sinphi * sinphi / finv);
-
-    // iterate
-    double cosphi;
-    double N_phi;
-    double dP;
-    double dZ;
-    double oneesq = 1.0 - esq;
-
-    for (int i = 0; i < maxit; i++)
-        {
-            sinphi = sin(*dphi);
-            cosphi = cos(*dphi);
-
-            // compute radius of curvature in prime vertical direction
-            N_phi = a / sqrt(1 - esq * sinphi * sinphi);
-
-            //    compute residuals in P and Z
-            dP = P - (N_phi + (*h)) * cosphi;
-            dZ = Z - (N_phi * oneesq + (*h)) * sinphi;
-
-            //    update height and latitude
-            *h = *h + (sinphi * dZ + cosphi * dP);
-            *dphi = *dphi + (cosphi * dZ - sinphi * dP) / (N_phi + (*h));
-
-            //     test for convergence
-            if ((dP * dP + dZ * dZ) < tolsq)
-                {
-                    break;
-                }
-            if (i == (maxit - 1))
-                {
-                    LOG(WARNING) << "The computation of geodetic coordinates did not converge";
-                }
-        }
-    *dphi = (*dphi) * rtd;
     return 0;
 }
 
@@ -356,73 +241,6 @@ int Pvt_Solution::tropo(double *ddr_m, double sinel, double hsta_km, double p_mb
 }
 
 
-int Pvt_Solution::topocent(double *Az, double *El, double *D, const arma::vec &x, const arma::vec &dx)
-{
-    /*  Transformation of vector dx into topocentric coordinate
-      system with origin at x
-         Inputs:
-            x           - vector origin coordinates (in ECEF system [X; Y; Z;])
-            dx          - vector ([dX; dY; dZ;]).
-
-         Outputs:
-            D           - vector length. Units like the input
-            Az          - azimuth from north positive clockwise, degrees
-            El          - elevation angle, degrees
-
-            Based on a Matlab function by Kai Borre
-     */
-
-    double lambda;
-    double phi;
-    double h;
-    double dtr = GPS_PI / 180.0;
-    double a = 6378137.0;         // semi-major axis of the reference ellipsoid WGS-84
-    double finv = 298.257223563;  // inverse of flattening of the reference ellipsoid WGS-84
-
-    // Transform x into geodetic coordinates
-    Pvt_Solution::togeod(&phi, &lambda, &h, a, finv, x(0), x(1), x(2));
-
-    double cl = cos(lambda * dtr);
-    double sl = sin(lambda * dtr);
-    double cb = cos(phi * dtr);
-    double sb = sin(phi * dtr);
-
-    arma::mat F = {{-sl, -sb * cl, cb * cl},
-        {cl, -sb * sl, cb * sl},
-        {0, 0, cb, sb}};
-
-    arma::vec local_vector;
-
-    local_vector = arma::htrans(F) * dx;
-
-    double E = local_vector(0);
-    double N = local_vector(1);
-    double U = local_vector(2);
-
-    double hor_dis;
-    hor_dis = sqrt(E * E + N * N);
-
-    if (hor_dis < 1.0E-20)
-        {
-            *Az = 0;
-            *El = 90;
-        }
-    else
-        {
-            *Az = atan2(E, N) / dtr;
-            *El = atan2(U, hor_dis) / dtr;
-        }
-
-    if (*Az < 0)
-        {
-            *Az = *Az + 360.0;
-        }
-
-    *D = sqrt(dx(0) * dx(0) + dx(1) * dx(1) + dx(2) * dx(2));
-    return 0;
-}
-
-
 void Pvt_Solution::set_averaging_depth(int depth)
 {
     d_averaging_depth = depth;
@@ -517,6 +335,30 @@ double Pvt_Solution::get_height() const
 }
 
 
+double Pvt_Solution::get_speed_over_ground() const
+{
+    return d_speed_over_ground_m_s;
+}
+
+
+void Pvt_Solution::set_speed_over_ground(double speed_m_s)
+{
+    d_speed_over_ground_m_s = speed_m_s;
+}
+
+
+void Pvt_Solution::set_course_over_ground(double cog_deg)
+{
+    d_course_over_ground_d = cog_deg;
+}
+
+
+double Pvt_Solution::get_course_over_ground() const
+{
+    return d_course_over_ground_d;
+}
+
+
 double Pvt_Solution::get_avg_latitude() const
 {
     return d_avg_latitude_d;
@@ -540,6 +382,7 @@ bool Pvt_Solution::is_averaging() const
     return d_flag_averaging;
 }
 
+
 bool Pvt_Solution::is_valid_position() const
 {
     return b_valid_position;
@@ -589,172 +432,3 @@ void Pvt_Solution::set_num_valid_observations(int num)
 {
     d_valid_observations = num;
 }
-
-
-bool Pvt_Solution::set_visible_satellites_ID(size_t index, unsigned int prn)
-{
-    if (index >= PVT_MAX_CHANNELS)
-        {
-            LOG(WARNING) << "Setting sat ID to channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")";
-            return false;
-        }
-    else
-        {
-            if (prn >= PVT_MAX_PRN)
-                {
-                    LOG(WARNING) << "Setting to channel " << index << " a PRN of " << prn << " (the maximum is " << PVT_MAX_PRN << ")";
-                    return false;
-                }
-            else
-                {
-                    d_visible_satellites_IDs[index] = prn;
-                    return true;
-                }
-        }
-}
-
-
-unsigned int Pvt_Solution::get_visible_satellites_ID(size_t index) const
-{
-    if (index >= PVT_MAX_CHANNELS)
-        {
-            LOG(WARNING) << "Getting sat ID for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")";
-            return 0;
-        }
-    else
-        {
-            return d_visible_satellites_IDs[index];
-        }
-}
-
-
-bool Pvt_Solution::set_visible_satellites_El(size_t index, double el)
-{
-    if (index >= PVT_MAX_CHANNELS)
-        {
-            LOG(WARNING) << "Setting sat elevation for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")";
-            return false;
-        }
-    else
-        {
-            if (el > 90.0)
-                {
-                    LOG(WARNING) << "Setting a sat elevation > 90 [degrees]. Saturating to 90";
-                    d_visible_satellites_El[index] = 90.0;
-                }
-            else
-                {
-                    if (el < -90.0)
-                        {
-                            LOG(WARNING) << "Setting a sat elevation < -90 [degrees]. Saturating to -90";
-                            d_visible_satellites_El[index] = -90.0;
-                        }
-                    else
-                        {
-                            d_visible_satellites_El[index] = el;
-                        }
-                }
-            return true;
-        }
-}
-
-
-double Pvt_Solution::get_visible_satellites_El(size_t index) const
-{
-    if (index >= PVT_MAX_CHANNELS)
-        {
-            LOG(WARNING) << "Getting sat elevation for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")";
-            return 0.0;
-        }
-    else
-        {
-            return d_visible_satellites_El[index];
-        }
-}
-
-
-bool Pvt_Solution::set_visible_satellites_Az(size_t index, double az)
-{
-    if (index >= PVT_MAX_CHANNELS)
-        {
-            LOG(WARNING) << "Getting sat azimuth for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")";
-            return false;
-        }
-    else
-        {
-            d_visible_satellites_Az[index] = az;
-            return true;
-        }
-}
-
-
-double Pvt_Solution::get_visible_satellites_Az(size_t index) const
-{
-    if (index >= PVT_MAX_CHANNELS)
-        {
-            LOG(WARNING) << "Getting sat azimuth for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")";
-            return 0.0;
-        }
-    else
-        {
-            return d_visible_satellites_Az[index];
-        }
-}
-
-
-bool Pvt_Solution::set_visible_satellites_Distance(size_t index, double dist)
-{
-    if (index >= PVT_MAX_CHANNELS)
-        {
-            LOG(WARNING) << "Setting sat distance for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")";
-            return false;
-        }
-    else
-        {
-            d_visible_satellites_Distance[index] = dist;
-            return true;
-        }
-}
-
-
-double Pvt_Solution::get_visible_satellites_Distance(size_t index) const
-{
-    if (index >= PVT_MAX_CHANNELS)
-        {
-            LOG(WARNING) << "Getting sat distance for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")";
-            return 0.0;
-        }
-    else
-        {
-            return d_visible_satellites_Distance[index];
-        }
-}
-
-
-bool Pvt_Solution::set_visible_satellites_CN0_dB(size_t index, double cn0)
-{
-    if (index >= PVT_MAX_CHANNELS)
-        {
-            LOG(WARNING) << "Setting sat Cn0 for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")";
-            return false;
-        }
-    else
-        {
-            d_visible_satellites_CN0_dB[index] = cn0;
-            return true;
-        }
-}
-
-
-double Pvt_Solution::get_visible_satellites_CN0_dB(size_t index) const
-{
-    if (index >= PVT_MAX_CHANNELS)
-        {
-            LOG(WARNING) << "Getting received CN0 for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")";
-            return 0.0;
-        }
-    else
-        {
-            return d_visible_satellites_CN0_dB[index];
-        }
-}

src/algorithms/PVT/libs/pvt_solution.h

@@ -49,9 +49,11 @@ class Pvt_Solution
 private:
     double d_rx_dt_s;  // RX time offset [s]
 
-    double d_latitude_d;   // RX position Latitude WGS84 [deg]
+    double d_latitude_d;             // RX position Latitude WGS84 [deg]
-    double d_longitude_d;  // RX position Longitude WGS84 [deg]
+    double d_longitude_d;            // RX position Longitude WGS84 [deg]
-    double d_height_m;     // RX position height WGS84 [m]
+    double d_height_m;               // RX position height WGS84 [m]
+    double d_speed_over_ground_m_s;  // RX speed over ground [m/s]
+    double d_course_over_ground_d;   // RX course over ground [deg]
 
     double d_avg_latitude_d;   // Averaged latitude in degrees
     double d_avg_longitude_d;  // Averaged longitude in degrees
@@ -70,12 +72,6 @@ private:
     boost::posix_time::ptime d_position_UTC_time;
     int d_valid_observations;
 
-    int d_visible_satellites_IDs[PVT_MAX_CHANNELS] = {};          // Array with the IDs of the valid satellites
-    double d_visible_satellites_El[PVT_MAX_CHANNELS] = {};        // Array with the LOS Elevation of the valid satellites
-    double d_visible_satellites_Az[PVT_MAX_CHANNELS] = {};        // Array with the LOS Azimuth of the valid satellites
-    double d_visible_satellites_Distance[PVT_MAX_CHANNELS] = {};  // Array with the LOS Distance of the valid satellites
-    double d_visible_satellites_CN0_dB[PVT_MAX_CHANNELS] = {};    // Array with the IDs of the valid satellites
-
 public:
     Pvt_Solution();
 
@@ -86,6 +82,12 @@ public:
     double get_longitude() const;  //!< Get RX position Longitude WGS84 [deg]
     double get_height() const;     //!< Get RX position height WGS84 [m]
 
+    double get_speed_over_ground() const;          //!< Get RX speed over ground [m/s]
+    void set_speed_over_ground(double speed_m_s);  //!< Set RX speed over ground [m/s]
+
+    double get_course_over_ground() const;        //!< Get RX course over ground [deg]
+    void set_course_over_ground(double cog_deg);  //!< Set RX course over ground [deg]
+
     double get_avg_latitude() const;   //!< Get RX position averaged Latitude WGS84 [deg]
     double get_avg_longitude() const;  //!< Get RX position averaged Longitude WGS84 [deg]
     double get_avg_height() const;     //!< Get RX position averaged height WGS84 [m]
@@ -102,21 +104,6 @@ public:
     int get_num_valid_observations() const;    //!< Get the number of valid pseudorange observations (valid satellites)
     void set_num_valid_observations(int num);  //!< Set the number of valid pseudorange observations (valid satellites)
 
-    bool set_visible_satellites_ID(size_t index, unsigned int prn);  //!< Set the ID of the visible satellite index channel
-    unsigned int get_visible_satellites_ID(size_t index) const;      //!< Get the ID of the visible satellite index channel
-
-    bool set_visible_satellites_El(size_t index, double el);  //!< Set the LOS Elevation, in degrees, of the visible satellite index channel
-    double get_visible_satellites_El(size_t index) const;     //!< Get the LOS Elevation, in degrees, of the visible satellite index channel
-
-    bool set_visible_satellites_Az(size_t index, double az);  //!< Set the LOS Azimuth, in degrees, of the visible satellite index channel
-    double get_visible_satellites_Az(size_t index) const;     //!< Get the LOS Azimuth, in degrees, of the visible satellite index channel
-
-    bool set_visible_satellites_Distance(size_t index, double dist);  //!< Set the LOS Distance of the visible satellite index channel
-    double get_visible_satellites_Distance(size_t index) const;       //!< Get the LOS Distance of the visible satellite index channel
-
-    bool set_visible_satellites_CN0_dB(size_t index, double cn0);  //!< Set the CN0 in dB of the visible satellite index channel
-    double get_visible_satellites_CN0_dB(size_t index) const;      //!< Get the CN0 in dB of the visible satellite index channel
-
     //averaging
     void perform_pos_averaging();
     void set_averaging_depth(int depth);  //!< Set length of averaging window
@@ -142,41 +129,6 @@ public:
       */
     int cart2geo(double X, double Y, double Z, int elipsoid_selection);
 
-    /*!
-      * \brief Transformation of vector dx into topocentric coordinate system with origin at x
-      *
-      * \param[in] x    Vector origin coordinates (in ECEF system [X; Y; Z;])
-      * \param[in] dx   Vector ([dX; dY; dZ;]).
-      *
-      * \param[out] D   Vector length. Units like the input
-      * \param[out] Az  Azimuth from north positive clockwise, degrees
-      * \param[out] El  Elevation angle, degrees
-      *
-      * Based on a Matlab function by Kai Borre
-      */
-    int topocent(double *Az, double *El, double *D, const arma::vec &x, const arma::vec &dx);
-
-    /*!
-      * \brief Subroutine to calculate geodetic coordinates latitude, longitude,
-      * height given Cartesian coordinates X,Y,Z, and reference ellipsoid
-      * values semi-major axis (a) and the inverse of flattening (finv).
-      *
-      *  The output units of angular quantities will be in decimal degrees
-      *  (15.5 degrees not 15 deg 30 min). The output units of h will be the
-      *  same as the units of X,Y,Z,a.
-      *
-      *  \param[in] a           - semi-major axis of the reference ellipsoid
-      *  \param[in] finv        - inverse of flattening of the reference ellipsoid
-      *  \param[in] X,Y,Z       - Cartesian coordinates
-      *
-      *  \param[out] dphi        - latitude
-      *  \param[out] dlambda     - longitude
-      *  \param[out] h           - height above reference ellipsoid
-      *
-      * Based in a Matlab function by Kai Borre
-      */
-    int togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z);
-
     /*!
       * \brief Tropospheric correction
       *

src/algorithms/PVT/libs/rinex_printer.cc

@@ -3296,8 +3296,8 @@ void Rinex_Printer::log_rinex_nav(std::fstream& out, const std::map<int32_t, Glo
     std::string line;
     std::map<int32_t, Glonass_Gnav_Ephemeris>::const_iterator glonass_gnav_ephemeris_iter;
 
-    for (glonass_gnav_ephemeris_iter = eph_map.begin();
+    for (glonass_gnav_ephemeris_iter = eph_map.cbegin();
-         glonass_gnav_ephemeris_iter != eph_map.end();
+         glonass_gnav_ephemeris_iter != eph_map.cend();
          glonass_gnav_ephemeris_iter++)
         {
             // -------- SV / EPOCH / SV CLK
@@ -7114,15 +7114,15 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Glonass_Gnav_Ephemeri
             //Number of satellites observed in current epoch
             int32_t numSatellitesObserved = 0;
             std::map<int32_t, Gnss_Synchro>::const_iterator observables_iter;
-            for (observables_iter = observables.begin();
+            for (observables_iter = observables.cbegin();
-                 observables_iter != observables.end();
+                 observables_iter != observables.cend();
                  observables_iter++)
                 {
                     numSatellitesObserved++;
                 }
             line += Rinex_Printer::rightJustify(boost::lexical_cast<std::string>(numSatellitesObserved), 3);
-            for (observables_iter = observables.begin();
+            for (observables_iter = observables.cbegin();
-                 observables_iter != observables.end();
+                 observables_iter != observables.cend();
                  observables_iter++)
                 {
                     line += satelliteSystem["GLONASS"];
@@ -7135,8 +7135,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Glonass_Gnav_Ephemeri
             Rinex_Printer::lengthCheck(line);
             out << line << std::endl;
 
-            for (observables_iter = observables.begin();
+            for (observables_iter = observables.cbegin();
-                 observables_iter != observables.end();
+                 observables_iter != observables.cend();
                  observables_iter++)
                 {
                     std::string lineObs;
@@ -7218,8 +7218,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Glonass_Gnav_Ephemeri
             //Number of satellites observed in current epoch
             int32_t numSatellitesObserved = 0;
             std::map<int32_t, Gnss_Synchro>::const_iterator observables_iter;
-            for (observables_iter = observables.begin();
+            for (observables_iter = observables.cbegin();
-                 observables_iter != observables.end();
+                 observables_iter != observables.cend();
                  observables_iter++)
                 {
                     numSatellitesObserved++;
@@ -7233,8 +7233,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Glonass_Gnav_Ephemeri
             Rinex_Printer::lengthCheck(line);
             out << line << std::endl;
 
-            for (observables_iter = observables.begin();
+            for (observables_iter = observables.cbegin();
-                 observables_iter != observables.end();
+                 observables_iter != observables.cend();
                  observables_iter++)
                 {
                     std::string lineObs;
@@ -7390,8 +7390,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_ep
     std::map<int32_t, Gnss_Synchro> observablesR2C;
     std::map<int32_t, Gnss_Synchro>::const_iterator observables_iter;
 
-    for (observables_iter = observables.begin();
+    for (observables_iter = observables.cbegin();
-         observables_iter != observables.end();
+         observables_iter != observables.cend();
          observables_iter++)
         {
             std::string system_(&observables_iter->second.System, 1);
@@ -7413,8 +7413,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_ep
     std::multimap<uint32_t, Gnss_Synchro> total_glo_map;
     std::set<uint32_t> available_glo_prns;
     std::set<uint32_t>::iterator it;
-    for (observables_iter = observablesR1C.begin();
+    for (observables_iter = observablesR1C.cbegin();
-         observables_iter != observablesR1C.end();
+         observables_iter != observablesR1C.cend();
          observables_iter++)
         {
             uint32_t prn_ = observables_iter->second.PRN;
@@ -7426,8 +7426,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_ep
                 }
         }
 
-    for (observables_iter = observablesR2C.begin();
+    for (observables_iter = observablesR2C.cbegin();
-         observables_iter != observablesR2C.end();
+         observables_iter != observablesR2C.cend();
          observables_iter++)
         {
             uint32_t prn_ = observables_iter->second.PRN;
@@ -7446,8 +7446,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_ep
     if (version == 2)
         {
             // Add list of GPS satellites
-            for (observables_iter = observablesG1C.begin();
+            for (observables_iter = observablesG1C.cbegin();
-                 observables_iter != observablesG1C.end();
+                 observables_iter != observablesG1C.cend();
                  observables_iter++)
                 {
                     line += satelliteSystem["GPS"];
@@ -7455,8 +7455,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_ep
                     line += boost::lexical_cast<std::string>(static_cast<int32_t>(observables_iter->second.PRN));
                 }
             // Add list of GLONASS L1 satellites
-            for (observables_iter = observablesR1C.begin();
+            for (observables_iter = observablesR1C.cbegin();
-                 observables_iter != observablesR1C.end();
+                 observables_iter != observablesR1C.cend();
                  observables_iter++)
                 {
                     line += satelliteSystem["GLONASS"];
@@ -7464,8 +7464,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_ep
                     line += boost::lexical_cast<std::string>(static_cast<int32_t>(observables_iter->second.PRN));
                 }
             // Add list of GLONASS L2 satellites
-            for (observables_iter = observablesR2C.begin();
+            for (observables_iter = observablesR2C.cbegin();
-                 observables_iter != observablesR2C.end();
+                 observables_iter != observablesR2C.cend();
                  observables_iter++)
                 {
                     line += satelliteSystem["GLONASS"];
@@ -7480,8 +7480,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_ep
     // -------- OBSERVATION record
     std::string s;
     std::string lineObs;
-    for (observables_iter = observablesG1C.begin();
+    for (observables_iter = observablesG1C.cbegin();
-         observables_iter != observablesG1C.end();
+         observables_iter != observablesG1C.cend();
          observables_iter++)
         {
             lineObs.clear();
@@ -7664,8 +7664,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_CNAV_Ephemeris& g
     std::map<int32_t, Gnss_Synchro> observablesR2C;
     std::map<int32_t, Gnss_Synchro>::const_iterator observables_iter;
 
-    for (observables_iter = observables.begin();
+    for (observables_iter = observables.cbegin();
-         observables_iter != observables.end();
+         observables_iter != observables.cend();
          observables_iter++)
         {
             std::string system_(&observables_iter->second.System, 1);
@@ -7687,8 +7687,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_CNAV_Ephemeris& g
     std::multimap<uint32_t, Gnss_Synchro> total_glo_map;
     std::set<uint32_t> available_glo_prns;
     std::set<uint32_t>::iterator it;
-    for (observables_iter = observablesR1C.begin();
+    for (observables_iter = observablesR1C.cbegin();
-         observables_iter != observablesR1C.end();
+         observables_iter != observablesR1C.cend();
          observables_iter++)
         {
             uint32_t prn_ = observables_iter->second.PRN;
@@ -7700,8 +7700,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_CNAV_Ephemeris& g
                 }
         }
 
-    for (observables_iter = observablesR2C.begin();
+    for (observables_iter = observablesR2C.cbegin();
-         observables_iter != observablesR2C.end();
+         observables_iter != observablesR2C.cend();
          observables_iter++)
         {
             uint32_t prn_ = observables_iter->second.PRN;
@@ -7725,8 +7725,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_CNAV_Ephemeris& g
     // -------- OBSERVATION record
     std::string s;
     std::string lineObs;
-    for (observables_iter = observablesG2S.begin();
+    for (observables_iter = observablesG2S.cbegin();
-         observables_iter != observablesG2S.end();
+         observables_iter != observablesG2S.cend();
          observables_iter++)
         {
             lineObs.clear();
@@ -7904,8 +7904,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Galileo_Ephemeris& ga
     std::map<int32_t, Gnss_Synchro> observablesR2C;
     std::map<int32_t, Gnss_Synchro>::const_iterator observables_iter;
 
-    for (observables_iter = observables.begin();
+    for (observables_iter = observables.cbegin();
-         observables_iter != observables.end();
+         observables_iter != observables.cend();
          observables_iter++)
         {
             std::string system_(&observables_iter->second.System, 1);
@@ -7927,8 +7927,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Galileo_Ephemeris& ga
     std::multimap<uint32_t, Gnss_Synchro> total_glo_map;
     std::set<uint32_t> available_glo_prns;
     std::set<uint32_t>::iterator it;
-    for (observables_iter = observablesR1C.begin();
+    for (observables_iter = observablesR1C.cbegin();
-         observables_iter != observablesR1C.end();
+         observables_iter != observablesR1C.cend();
          observables_iter++)
         {
             uint32_t prn_ = observables_iter->second.PRN;
@@ -7939,8 +7939,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Galileo_Ephemeris& ga
                     available_glo_prns.insert(prn_);
                 }
         }
-    for (observables_iter = observablesR2C.begin();
+    for (observables_iter = observablesR2C.cbegin();
-         observables_iter != observablesR2C.end();
+         observables_iter != observablesR2C.cend();
          observables_iter++)
         {
             uint32_t prn_ = observables_iter->second.PRN;
@@ -7966,8 +7966,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Galileo_Ephemeris& ga
 
     std::string s;
     std::string lineObs;
-    for (observables_iter = observablesE1B.begin();
+    for (observables_iter = observablesE1B.cbegin();
-         observables_iter != observablesE1B.end();
+         observables_iter != observablesE1B.cend();
          observables_iter++)
         {
             lineObs.clear();
@@ -8747,8 +8747,8 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Galileo_Ephemeris& ep
     std::set<uint32_t>::iterator it;
     if (found_1B != std::string::npos)
         {
-            for (observables_iter = observablesE1B.begin();
+            for (observables_iter = observablesE1B.cbegin();
-                 observables_iter != observablesE1B.end();
+                 observables_iter != observablesE1B.cend();
                  observables_iter++)
                 {
                     uint32_t prn_ = observables_iter->second.PRN;

src/algorithms/PVT/libs/rtklib_solver.cc

@@ -53,6 +53,7 @@
 
 #include "rtklib_solver.h"
 #include "rtklib_conversions.h"
+#include "rtklib_solution.h"
 #include "GPS_L1_CA.h"
 #include "Galileo_E1.h"
 #include "GLONASS_L1_L2_CA.h"
@@ -74,7 +75,11 @@ rtklib_solver::rtklib_solver(int nchannels, std::string dump_filename, bool flag
     rtk_ = rtk;
     for (unsigned int i = 0; i < 4; i++) dop_[i] = 0.0;
     pvt_sol = {{0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, '0', '0', '0', 0, 0, 0};
-
+    ssat_t ssat0 = {0, 0, {0.0}, {0.0}, {0.0}, {'0'}, {'0'}, {'0'}, {'0'}, {'0'}, {}, {}, {}, {}, 0.0, 0.0, 0.0, 0.0, {{{0, 0}}, {{0, 0}}}, {{}, {}}};
+    for (unsigned int i = 0; i < MAXSAT; i++)
+        {
+            pvt_ssat[i] = ssat0;
+        }
     // ############# ENABLE DATA FILE LOG #################
     if (d_flag_dump_enabled == true)
         {
@@ -772,9 +777,9 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
 
             for (int i = 0; i < MAXSAT; i++)
                 {
-                    nav_data.lam[i][0] = SPEED_OF_LIGHT / FREQ1; /* L1/E1 */
+                    nav_data.lam[i][0] = SPEED_OF_LIGHT / FREQ1;  // L1/E1
-                    nav_data.lam[i][1] = SPEED_OF_LIGHT / FREQ2; /* L2 */
+                    nav_data.lam[i][1] = SPEED_OF_LIGHT / FREQ2;  // L2
-                    nav_data.lam[i][2] = SPEED_OF_LIGHT / FREQ5; /* L5/E5 */
+                    nav_data.lam[i][2] = SPEED_OF_LIGHT / FREQ5;  // L5/E5
                 }
 
             result = rtkpos(&rtk_, obs_data, valid_obs + glo_valid_obs, &nav_data);
@@ -783,18 +788,22 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
                 {
                     LOG(INFO) << "RTKLIB rtkpos error";
                     DLOG(INFO) << "RTKLIB rtkpos error message: " << rtk_.errbuf;
-                    this->set_time_offset_s(0.0);  //reset rx time estimation
+                    this->set_time_offset_s(0.0);  // reset rx time estimation
                     this->set_num_valid_observations(0);
                 }
             else
                 {
-                    this->set_num_valid_observations(rtk_.sol.ns);  //record the number of valid satellites used by the PVT solver
+                    this->set_num_valid_observations(rtk_.sol.ns);  // record the number of valid satellites used by the PVT solver
                     pvt_sol = rtk_.sol;
                     // DOP computation
                     unsigned int used_sats = 0;
                     for (unsigned int i = 0; i < MAXSAT; i++)
                         {
-                            if (rtk_.ssat[i].vs == 1) used_sats++;
+                            pvt_ssat[i] = rtk_.ssat[i];
+                            if (rtk_.ssat[i].vs == 1)
+                                {
+                                    used_sats++;
+                                }
                         }
 
                     std::vector<double> azel;
@@ -809,8 +818,8 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
                                     index_aux++;
                                 }
                         }
-                    if (index_aux > 0) dops(index_aux, azel.data(), 0.0, dop_);
 
+                    if (index_aux > 0) dops(index_aux, azel.data(), 0.0, dop_);
                     this->set_valid_position(true);
                     arma::vec rx_position_and_time(4);
                     rx_position_and_time(0) = pvt_sol.rr[0];  // [m]
@@ -827,6 +836,22 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
                             rx_position_and_time(3) = pvt_sol.dtr[0] / GPS_C_m_s;  // the receiver clock offset is expressed in [meters], so we convert it into [s]
                         }
                     this->set_rx_pos(rx_position_and_time.rows(0, 2));  // save ECEF position for the next iteration
+
+                    //compute Ground speed and COG
+                    double ground_speed_ms = 0.0;
+                    double pos[3];
+                    double enuv[3];
+                    ecef2pos(pvt_sol.rr, pos);
+                    ecef2enu(pos, &pvt_sol.rr[3], enuv);
+                    this->set_speed_over_ground(norm_rtk(enuv, 2));
+                    double new_cog;
+                    if (ground_speed_ms >= 1.0)
+                        {
+                            new_cog = atan2(enuv[0], enuv[1]) * R2D;
+                            if (new_cog < 0.0) new_cog += 360.0;
+                            this->set_course_over_ground(new_cog);
+                        }
+
                     //observable fix:
                     //double offset_s = this->get_time_offset_s();
                     //this->set_time_offset_s(offset_s + (rx_position_and_time(3) / GPS_C_m_s));  // accumulate the rx time error for the next iteration [meters]->[seconds]

src/algorithms/PVT/libs/rtklib_solver.h

@@ -86,6 +86,7 @@ private:
 
 public:
     sol_t pvt_sol;
+    ssat_t pvt_ssat[MAXSAT];
     rtklib_solver(int nchannels, std::string dump_filename, bool flag_dump_to_file, bool flag_dump_to_mat, rtk_t& rtk);
     ~rtklib_solver();
 

src/algorithms/channel/libs/channel_fsm.cc

@@ -59,7 +59,6 @@ bool ChannelFsm::Event_stop_channel()
     switch (d_state)
         {
         case 0:  //already in stanby
-            return true;
             break;
         case 1:  //acquisition
             d_state = 0;
@@ -70,8 +69,9 @@ bool ChannelFsm::Event_stop_channel()
             stop_tracking();
             break;
         default:
-            return true;
+            break;
         }
+    return true;
 }
 
 bool ChannelFsm::Event_start_acquisition()

src/algorithms/libs/CMakeLists.txt

@@ -39,6 +39,7 @@ set(GNSS_SPLIBS_SOURCES
         conjugate_sc.cc
         conjugate_ic.cc
         gnss_sdr_create_directory.cc
+        geofunctions.cc
 )
 
 set(GNSS_SPLIBS_HEADERS
@@ -63,6 +64,7 @@ set(GNSS_SPLIBS_HEADERS
         conjugate_ic.h
         gnss_sdr_create_directory.h
         gnss_circular_deque.h
+        geofunctions.h
 )
 
 
@@ -101,6 +103,7 @@ include_directories(
     ${Boost_INCLUDE_DIRS}
     ${GLOG_INCLUDE_DIRS}
     ${GFlags_INCLUDE_DIRS}
+    ${ARMADILLO_INCLUDE_DIRS}
     ${GNURADIO_RUNTIME_INCLUDE_DIRS}
     ${GNURADIO_BLOCKS_INCLUDE_DIRS}
     ${VOLK_INCLUDE_DIRS}
@@ -128,6 +131,7 @@ target_link_libraries(gnss_sp_libs ${GNURADIO_RUNTIME_LIBRARIES}
     ${VOLK_LIBRARIES} ${ORC_LIBRARIES}
     ${VOLK_GNSSSDR_LIBRARIES}  ${ORC_LIBRARIES}
     ${GFlags_LIBS}
+    ${ARMADILLO_LIBRARIES}
     ${GNURADIO_BLOCKS_LIBRARIES}
     ${GNURADIO_FFT_LIBRARIES}
     ${GNURADIO_FILTER_LIBRARIES}
@@ -136,7 +140,9 @@ target_link_libraries(gnss_sp_libs ${GNURADIO_RUNTIME_LIBRARIES}
 )
 
 if(NOT VOLK_GNSSSDR_FOUND)
-    add_dependencies(gnss_sp_libs volk_gnsssdr_module)
+    add_dependencies(gnss_sp_libs volk_gnsssdr_module armadillo-${armadillo_RELEASE})
+else(NOT VOLK_GNSSSDR_FOUND)
+    add_dependencies(gnss_sp_libs armadillo-${armadillo_RELEASE})
 endif(NOT VOLK_GNSSSDR_FOUND)
 
 if(${GFLAGS_GREATER_20})

src/algorithms/libs/geofunctions.cc

@@ -301,7 +301,7 @@ double mstokph(double MetersPerSeconds)
 arma::vec CTM_to_Euler(const arma::mat &C)
 {
     // Calculate Euler angles using (2.23)
-    arma::mat CTM = C;
+    arma::mat CTM(C);
     arma::vec eul = arma::zeros(3, 1);
     eul(0) = atan2(CTM(1, 2), CTM(2, 2));  // roll
     if (CTM(0, 2) < -1.0) CTM(0, 2) = -1.0;

src/algorithms/libs/gnss_sdr_valve.cc

@@ -2,6 +2,7 @@
  * \file gnss_sdr_valve.cc
  * \brief Implementation of a GNU Radio block that sends a STOP message to the
  * control queue right after a specific number of samples have passed through it.
+ * \author Javier Arribas, 2018. jarribas(at)cttc.es
  * \author Carlos Aviles, 2010. carlos.avilesr(at)googlemail.com
  *
  *
@@ -39,42 +40,65 @@
 
 gnss_sdr_valve::gnss_sdr_valve(size_t sizeof_stream_item,
     unsigned long long nitems,
-    gr::msg_queue::sptr queue) : gr::sync_block("valve",
+    gr::msg_queue::sptr queue, bool stop_flowgraph) : gr::sync_block("valve",
-                                     gr::io_signature::make(1, 1, sizeof_stream_item),
+                                                          gr::io_signature::make(1, 1, sizeof_stream_item),
-                                     gr::io_signature::make(1, 1, sizeof_stream_item)),
+                                                          gr::io_signature::make(1, 1, sizeof_stream_item)),
-                                 d_nitems(nitems),
+                                                      d_nitems(nitems),
-                                 d_ncopied_items(0),
+                                                      d_ncopied_items(0),
-                                 d_queue(queue)
+                                                      d_queue(queue),
+                                                      d_stop_flowgraph(stop_flowgraph)
 {
+    d_open_valve = false;
 }
 
 
-boost::shared_ptr<gr::block> gnss_sdr_make_valve(size_t sizeof_stream_item, unsigned long long nitems, gr::msg_queue::sptr queue)
+boost::shared_ptr<gr::block> gnss_sdr_make_valve(size_t sizeof_stream_item, unsigned long long nitems, gr::msg_queue::sptr queue, bool stop_flowgraph)
 {
-    boost::shared_ptr<gnss_sdr_valve> valve_(new gnss_sdr_valve(sizeof_stream_item, nitems, queue));
+    boost::shared_ptr<gnss_sdr_valve> valve_(new gnss_sdr_valve(sizeof_stream_item, nitems, queue, stop_flowgraph));
     return valve_;
 }
 
+boost::shared_ptr<gr::block> gnss_sdr_make_valve(size_t sizeof_stream_item, unsigned long long nitems, gr::msg_queue::sptr queue)
+{
+    boost::shared_ptr<gnss_sdr_valve> valve_(new gnss_sdr_valve(sizeof_stream_item, nitems, queue, false));
+    return valve_;
+}
 
+void gnss_sdr_valve::open_valve()
+{
+    d_open_valve = true;
+}
 int gnss_sdr_valve::work(int noutput_items,
     gr_vector_const_void_star &input_items,
     gr_vector_void_star &output_items)
 {
-    if (d_ncopied_items >= d_nitems)
+    if (d_open_valve == false)
         {
-            ControlMessageFactory *cmf = new ControlMessageFactory();
+            if (d_ncopied_items >= d_nitems)
-            d_queue->handle(cmf->GetQueueMessage(200, 0));
+                {
-            LOG(INFO) << "Stopping receiver, " << d_ncopied_items << " samples processed";
+                    ControlMessageFactory *cmf = new ControlMessageFactory();
-            delete cmf;
+                    d_queue->handle(cmf->GetQueueMessage(200, 0));
-            return -1;  // Done!
+                    LOG(INFO) << "Stopping receiver, " << d_ncopied_items << " samples processed";
+                    delete cmf;
+                    if (d_stop_flowgraph)
+                        {
+                            return -1;  // Done!
+                        }
+                    else
+                        {
+                            usleep(1000000);
+                            return 0;  //do not produce or consume
+                        }
+                }
+            unsigned long long n = std::min(d_nitems - d_ncopied_items, static_cast<long long unsigned int>(noutput_items));
+            if (n == 0) return 0;
+            memcpy(output_items[0], input_items[0], n * input_signature()->sizeof_stream_item(0));
+            d_ncopied_items += n;
+            return n;
+        }
+    else
+        {
+            memcpy(output_items[0], input_items[0], noutput_items * input_signature()->sizeof_stream_item(0));
+            return noutput_items;
         }
-    unsigned long long n = std::min(d_nitems - d_ncopied_items, static_cast<long long unsigned int>(noutput_items));
-    if (n == 0) return 0;
-    memcpy(output_items[0], input_items[0], n * input_signature()->sizeof_stream_item(0));
-    //for(long long i = 0; i++; i<n)
-    //    {
-    //        output_items[i] = input_items[i];
-    //    }
-    d_ncopied_items += n;
-    return n;
 }

src/algorithms/libs/gnss_sdr_valve.h

@@ -2,6 +2,7 @@
  * \file gnss_sdr_valve.h
  * \brief  Interface of a GNU Radio block that sends a STOP message to the
  * control queue right after a specific number of samples have passed through it.
+ * \author Javier Arribas, 2018. jarribas(at)cttc.es
  * \author Carlos Aviles, 2010. carlos.avilesr(at)googlemail.com
  *
  * -------------------------------------------------------------------------
@@ -37,10 +38,13 @@
 #include <gnuradio/msg_queue.h>
 #include <boost/shared_ptr.hpp>
 
-
 boost::shared_ptr<gr::block> gnss_sdr_make_valve(size_t sizeof_stream_item,
     unsigned long long nitems,
     gr::msg_queue::sptr queue);
+boost::shared_ptr<gr::block> gnss_sdr_make_valve(size_t sizeof_stream_item,
+    unsigned long long nitems,
+    gr::msg_queue::sptr queue,
+    bool stop_flowgraph);
 /*!
  * \brief Implementation of a GNU Radio block that sends a STOP message to the
  * control queue right after a specific number of samples have passed through it.
@@ -50,14 +54,24 @@ class gnss_sdr_valve : public gr::sync_block
     friend boost::shared_ptr<gr::block> gnss_sdr_make_valve(size_t sizeof_stream_item,
         unsigned long long nitems,
         gr::msg_queue::sptr queue);
-    gnss_sdr_valve(size_t sizeof_stream_item,
+    friend boost::shared_ptr<gr::block> gnss_sdr_make_valve(size_t sizeof_stream_item,
         unsigned long long nitems,
-        gr::msg_queue::sptr queue);
+        gr::msg_queue::sptr queue,
+        bool stop_flowgraph);
+
+
     unsigned long long d_nitems;
     unsigned long long d_ncopied_items;
     gr::msg_queue::sptr d_queue;
+    bool d_stop_flowgraph;
+    bool d_open_valve;
 
 public:
+    gnss_sdr_valve(size_t sizeof_stream_item,
+        unsigned long long nitems,
+        gr::msg_queue::sptr queue, bool stop_flowgraph);
+    void open_valve();
+
     int work(int noutput_items,
         gr_vector_const_void_star &input_items,
         gr_vector_void_star &output_items);

src/algorithms/libs/rtklib/rtklib_conversions.cc

@@ -116,7 +116,7 @@ geph_t eph_to_rtklib(const Glonass_Gnav_Ephemeris& glonass_gnav_eph, const Glona
 eph_t eph_to_rtklib(const Galileo_Ephemeris& gal_eph)
 {
     eph_t rtklib_sat = {0, 0, 0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {0, 0}, 0.0, 0.0, 0.0, 0.0, 0.0,
-        0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0 };
+        0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0};
     //Galileo is the third satellite system for RTKLIB, so, add the required offset to discriminate Galileo ephemeris
     rtklib_sat.sat = gal_eph.i_satellite_PRN + NSATGPS + NSATGLO;
     rtklib_sat.A = gal_eph.A_1 * gal_eph.A_1;
@@ -174,7 +174,7 @@ eph_t eph_to_rtklib(const Galileo_Ephemeris& gal_eph)
 eph_t eph_to_rtklib(const Gps_Ephemeris& gps_eph)
 {
     eph_t rtklib_sat = {0, 0, 0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {0, 0}, 0.0, 0.0, 0.0, 0.0, 0.0,
-        0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0 };
+        0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0};
     rtklib_sat.sat = gps_eph.i_satellite_PRN;
     rtklib_sat.A = gps_eph.d_sqrt_A * gps_eph.d_sqrt_A;
     rtklib_sat.M0 = gps_eph.d_M_0;
@@ -231,7 +231,7 @@ eph_t eph_to_rtklib(const Gps_Ephemeris& gps_eph)
 eph_t eph_to_rtklib(const Gps_CNAV_Ephemeris& gps_cnav_eph)
 {
     eph_t rtklib_sat = {0, 0, 0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {0, 0}, 0.0, 0.0, 0.0, 0.0, 0.0,
-        0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0 };
+        0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0};
     rtklib_sat.sat = gps_cnav_eph.i_satellite_PRN;
     const double A_REF = 26559710.0;  // See IS-GPS-200H,  pp. 170
     rtklib_sat.A = A_REF + gps_cnav_eph.d_DELTA_A;
@@ -291,3 +291,55 @@ eph_t eph_to_rtklib(const Gps_CNAV_Ephemeris& gps_cnav_eph)
 
     return rtklib_sat;
 }
+
+alm_t alm_to_rtklib(const Gps_Almanac& gps_alm)
+{
+    alm_t rtklib_alm;
+
+    rtklib_alm = {0, 0, 0, 0, {0, 0}, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+
+    rtklib_alm.sat = gps_alm.i_satellite_PRN;
+    rtklib_alm.svh = gps_alm.i_SV_health;
+    rtklib_alm.svconf = gps_alm.i_AS_status;
+    rtklib_alm.week = gps_alm.i_WNa;
+    rtklib_alm.toa = gpst2time(gps_alm.i_WNa, gps_alm.i_Toa);
+    rtklib_alm.A = gps_alm.d_sqrt_A * gps_alm.d_sqrt_A;
+    rtklib_alm.e = gps_alm.d_e_eccentricity;
+    rtklib_alm.i0 = gps_alm.d_Delta_i + 0.3;
+    rtklib_alm.OMG0 = gps_alm.d_OMEGA0;
+    rtklib_alm.OMGd = gps_alm.d_OMEGA_DOT;
+    rtklib_alm.omg = gps_alm.d_OMEGA;
+    rtklib_alm.M0 = gps_alm.d_M_0;
+    rtklib_alm.f0 = gps_alm.d_A_f0;
+    rtklib_alm.f1 = gps_alm.d_A_f1;
+    rtklib_alm.toas = gps_alm.i_Toa;
+
+
+    return rtklib_alm;
+}
+alm_t alm_to_rtklib(const Galileo_Almanac& gal_alm)
+{
+    alm_t rtklib_alm;
+
+    rtklib_alm = {0, 0, 0, 0, {0, 0}, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+
+    rtklib_alm.sat = gal_alm.i_satellite_PRN;
+    rtklib_alm.svh = gal_alm.E1B_HS;
+    rtklib_alm.svconf = gal_alm.E1B_HS;
+    rtklib_alm.week = gal_alm.i_WNa;
+    rtklib_alm.toa = gpst2time(gal_alm.i_WNa, gal_alm.i_Toa);
+    rtklib_alm.A = 5440.588203494 + gal_alm.d_Delta_sqrt_A;
+    rtklib_alm.A = rtklib_alm.A * rtklib_alm.A;
+    rtklib_alm.e = gal_alm.d_e_eccentricity;
+    rtklib_alm.i0 = gal_alm.d_Delta_i + 0.31111;
+    rtklib_alm.OMG0 = gal_alm.d_OMEGA0;
+    rtklib_alm.OMGd = gal_alm.d_OMEGA_DOT;
+    rtklib_alm.omg = gal_alm.d_OMEGA;
+    rtklib_alm.M0 = gal_alm.d_M_0;
+    rtklib_alm.f0 = gal_alm.d_A_f0;
+    rtklib_alm.f1 = gal_alm.d_A_f1;
+    rtklib_alm.toas = gal_alm.i_Toa;
+
+
+    return rtklib_alm;
+}

src/algorithms/libs/rtklib/rtklib_conversions.h

@@ -38,10 +38,16 @@
 #include "gps_cnav_ephemeris.h"
 #include "glonass_gnav_ephemeris.h"
 #include "glonass_gnav_utc_model.h"
+#include "gps_almanac.h"
+#include "galileo_almanac.h"
 
 eph_t eph_to_rtklib(const Galileo_Ephemeris& gal_eph);
 eph_t eph_to_rtklib(const Gps_Ephemeris& gps_eph);
 eph_t eph_to_rtklib(const Gps_CNAV_Ephemeris& gps_cnav_eph);
+
+alm_t alm_to_rtklib(const Gps_Almanac& gps_alm);
+alm_t alm_to_rtklib(const Galileo_Almanac& gal_alm);
+
 /*!
  * \brief Transforms a Glonass_Gnav_Ephemeris to its RTKLIB counterpart
  * \param glonass_gnav_eph GLONASS GNAV Ephemeris structure

src/algorithms/signal_source/adapters/spir_gss6450_file_signal_source.cc

@@ -59,7 +59,7 @@ SpirGSS6450FileSignalSource::SpirGSS6450FileSignalSource(ConfigurationInterface*
     adc_bits_ = configuration->property(role + ".adc_bits", 4);
     n_channels_ = configuration->property(role + ".total_channels", 1);
     sel_ch_ = configuration->property(role + ".sel_ch", 1);
-    item_size_ = sizeof(int);
+    item_size_ = sizeof(int32_t);
     int64_t bytes_seek = configuration->property(role + ".bytes_to_skip", 65536);
     double sample_size_byte = static_cast<double>(adc_bits_) / 4.0;
 
@@ -69,17 +69,22 @@ SpirGSS6450FileSignalSource::SpirGSS6450FileSignalSource(ConfigurationInterface*
         }
     if (n_channels_ > 1)
         {
-            for (uint32_t i = 0; i < (n_channels_ - 1); i++)
+            for (uint32_t i = 0; i < n_channels_; i++)
                 {
-                    null_sinks_.push_back(gr::blocks::null_sink::make(item_size_));
+                    null_sinks_.push_back(gr::blocks::null_sink::make(sizeof(gr_complex)));
+                    unpack_spir_vec_.push_back(make_unpack_spir_gss6450_samples(adc_bits_));
+                    if (endian_swap_)
+                        {
+                            endian_vec_.push_back(gr::blocks::endian_swap::make(item_size_));
+                        }
                 }
-            DLOG(INFO) << "NUMBER OF NULL SINKS = " << null_sinks_.size();
         }
     try
         {
             file_source_ = gr::blocks::file_source::make(item_size_, filename_.c_str(), repeat_);
             file_source_->seek(bytes_seek / item_size_, SEEK_SET);
-            unpack_spir_ = make_unpack_spir_gss6450_samples(adc_bits_);
+
+
             deint_ = gr::blocks::deinterleave::make(item_size_);
         }
     catch (const std::exception& e)
@@ -143,22 +148,19 @@ SpirGSS6450FileSignalSource::SpirGSS6450FileSignalSource(ConfigurationInterface*
     LOG(INFO) << "Total number samples to be processed= " << samples_ << " GNSS signal duration= " << signal_duration_s << " [s]";
     std::cout << "GNSS signal recorded time to be processed: " << signal_duration_s << " [s]" << std::endl;
 
-    valve_ = gnss_sdr_make_valve(sizeof(gr_complex), samples_, queue_);
+    for (uint32_t i = 0; i < (n_channels_); i++)
-    DLOG(INFO) << "valve(" << valve_->unique_id() << ")";
+        {
+            valve_vec_.push_back(gnss_sdr_make_valve(sizeof(gr_complex), samples_, queue_));
+            if (dump_)
+                {
+                    sink_vec_.push_back(gr::blocks::file_sink::make(sizeof(gr_complex), dump_filename_.c_str()));
+                }
+            if (enable_throttle_control_)
+                {
+                    throttle_vec_.push_back(gr::blocks::throttle::make(sizeof(gr_complex), sampling_frequency_));
+                }
+        }
 
-    if (dump_)
-        {
-            sink_ = gr::blocks::file_sink::make(sizeof(gr_complex), dump_filename_.c_str());
-            DLOG(INFO) << "file_sink(" << sink_->unique_id() << ")";
-        }
-    if (enable_throttle_control_)
-        {
-            throttle_ = gr::blocks::throttle::make(sizeof(gr_complex), sampling_frequency_);
-        }
-    if (endian_swap_)
-        {
-            endian_ = gr::blocks::endian_swap::make(item_size_);
-        }
     DLOG(INFO) << "File source filename " << filename_;
     DLOG(INFO) << "Samples " << samples_;
     DLOG(INFO) << "Sampling frequency " << sampling_frequency_;
@@ -188,15 +190,17 @@ void SpirGSS6450FileSignalSource::connect(gr::top_block_sptr top_block)
     if (samples_ > 0)
         {
             top_block->connect(file_source_, 0, deint_, 0);
+
             if (endian_swap_)
                 {
-                    top_block->connect(deint_, sel_ch_ - 1, endian_, 0);
+                    top_block->connect(deint_, sel_ch_ - 1, endian_vec_.at(sel_ch_ - 1), 0);
-                    top_block->connect(endian_, 0, unpack_spir_, 0);
+                    top_block->connect(endian_vec_.at(sel_ch_ - 1), 0, unpack_spir_vec_.at(sel_ch_ - 1), 0);
                 }
             else
                 {
-                    top_block->connect(deint_, sel_ch_ - 1, unpack_spir_, 0);
+                    top_block->connect(deint_, sel_ch_ - 1, unpack_spir_vec_.at(sel_ch_ - 1), 0);
                 }
+
             if (n_channels_ > 1)
                 {
                     uint32_t aux = 0;
@@ -204,23 +208,37 @@ void SpirGSS6450FileSignalSource::connect(gr::top_block_sptr top_block)
                         {
                             if (i != (sel_ch_ - 1))
                                 {
-                                    top_block->connect(deint_, i, null_sinks_.at(aux), 0);
+                                    if (endian_swap_)
+                                        {
+                                            top_block->connect(deint_, i, endian_vec_.at(i), 0);
+                                            top_block->connect(endian_vec_.at(i), 0, unpack_spir_vec_.at(i), 0);
+                                        }
+                                    else
+                                        {
+                                            top_block->connect(deint_, i, unpack_spir_vec_.at(i), 0);
+                                        }
+
                                     aux++;
                                 }
                         }
                 }
-            if (enable_throttle_control_)
+            for (uint32_t i = 0; i < n_channels_; i++)
                 {
-                    top_block->connect(unpack_spir_, 0, throttle_, 0);
+                    if (enable_throttle_control_)
-                    top_block->connect(throttle_, 0, valve_, 0);
+                        {
-                }
+                            top_block->connect(unpack_spir_vec_.at(i), 0, throttle_vec_.at(i), 0);
-            else
+                            top_block->connect(throttle_vec_.at(i), 0, valve_vec_.at(i), 0);
-                {
+                        }
-                    top_block->connect(unpack_spir_, 0, valve_, 0);
+                    else
-                }
+                        {
-            if (dump_)
+                            top_block->connect(unpack_spir_vec_.at(i), 0, valve_vec_.at(i), 0);
-                {
+                        }
-                    top_block->connect(valve_, 0, sink_, 0);
+                    if (dump_)
+                        {
+                            top_block->connect(valve_vec_.at(i), 0, sink_vec_.at(i), 0);
+                        }
+
+                    top_block->connect(valve_vec_.at(i), 0, null_sinks_.at(i), 0);
                 }
         }
     else
@@ -237,12 +255,12 @@ void SpirGSS6450FileSignalSource::disconnect(gr::top_block_sptr top_block)
             top_block->disconnect(file_source_, 0, deint_, 0);
             if (endian_swap_)
                 {
-                    top_block->disconnect(deint_, sel_ch_ - 1, endian_, 0);
+                    top_block->disconnect(deint_, sel_ch_ - 1, endian_vec_.at(sel_ch_ - 1), 0);
-                    top_block->disconnect(endian_, 0, unpack_spir_, 0);
+                    top_block->disconnect(endian_vec_.at(sel_ch_ - 1), 0, unpack_spir_vec_.at(sel_ch_ - 1), 0);
                 }
             else
                 {
-                    top_block->disconnect(deint_, sel_ch_ - 1, unpack_spir_, 0);
+                    top_block->disconnect(deint_, sel_ch_ - 1, unpack_spir_vec_.at(sel_ch_ - 1), 0);
                 }
             if (n_channels_ > 1)
                 {
@@ -251,23 +269,38 @@ void SpirGSS6450FileSignalSource::disconnect(gr::top_block_sptr top_block)
                         {
                             if (i != (sel_ch_ - 1))
                                 {
-                                    top_block->disconnect(deint_, i, null_sinks_.at(aux), 0);
+                                    if (endian_swap_)
+                                        {
+                                            top_block->disconnect(deint_, i, endian_vec_.at(i), 0);
+                                            top_block->disconnect(endian_vec_.at(i), 0, unpack_spir_vec_.at(i), 0);
+                                        }
+                                    else
+                                        {
+                                            top_block->disconnect(deint_, i, unpack_spir_vec_.at(i), 0);
+                                        }
+
                                     aux++;
                                 }
                         }
                 }
-            if (enable_throttle_control_)
+
+            for (uint32_t i = 0; i < (n_channels_); i++)
                 {
-                    top_block->disconnect(unpack_spir_, 0, throttle_, 0);
+                    if (enable_throttle_control_)
-                    top_block->disconnect(throttle_, 0, valve_, 0);
+                        {
-                }
+                            top_block->disconnect(unpack_spir_vec_.at(i), 0, throttle_vec_.at(i), 0);
-            else
+                            top_block->disconnect(throttle_vec_.at(i), 0, valve_vec_.at(i), 0);
-                {
+                        }
-                    top_block->disconnect(unpack_spir_, 0, valve_, 0);
+                    else
-                }
+                        {
-            if (dump_)
+                            top_block->disconnect(unpack_spir_vec_.at(i), 0, valve_vec_.at(i), 0);
-                {
+                        }
-                    top_block->disconnect(valve_, 0, sink_, 0);
+                    if (dump_)
+                        {
+                            top_block->disconnect(valve_vec_.at(i), 0, sink_vec_.at(i), 0);
+                        }
+
+                    top_block->disconnect(valve_vec_.at(i), 0, null_sinks_.at(i), 0);
                 }
         }
     else
@@ -283,22 +316,12 @@ gr::basic_block_sptr SpirGSS6450FileSignalSource::get_left_block()
     return gr::blocks::file_source::sptr();
 }
 
+gr::basic_block_sptr SpirGSS6450FileSignalSource::get_right_block(int RF_channel)
+{
+    return valve_vec_.at(RF_channel);
+}
 
 gr::basic_block_sptr SpirGSS6450FileSignalSource::get_right_block()
 {
-    if (samples_ > 0)
+    return valve_vec_.at(0);
-        {
-            return valve_;
-        }
-    else
-        {
-            if (enable_throttle_control_)
-                {
-                    return throttle_;
-                }
-            else
-                {
-                    return unpack_spir_;
-                }
-        }
 }

src/algorithms/signal_source/adapters/spir_gss6450_file_signal_source.h

@@ -79,6 +79,7 @@ public:
     void connect(gr::top_block_sptr top_block) override;
     void disconnect(gr::top_block_sptr top_block) override;
     gr::basic_block_sptr get_left_block() override;
+    gr::basic_block_sptr get_right_block(int RF_channel) override;
     gr::basic_block_sptr get_right_block() override;
 
     inline std::string filename() const
@@ -124,12 +125,12 @@ private:
     uint32_t sel_ch_;
     gr::blocks::file_source::sptr file_source_;
     gr::blocks::deinterleave::sptr deint_;
-    gr::blocks::endian_swap::sptr endian_;
+    std::vector<gr::blocks::endian_swap::sptr> endian_vec_;
     std::vector<gr::blocks::null_sink::sptr> null_sinks_;
-    unpack_spir_gss6450_samples_sptr unpack_spir_;
+    std::vector<unpack_spir_gss6450_samples_sptr> unpack_spir_vec_;
-    boost::shared_ptr<gr::block> valve_;
+    std::vector<boost::shared_ptr<gr::block>> valve_vec_;
-    gr::blocks::file_sink::sptr sink_;
+    std::vector<gr::blocks::file_sink::sptr> sink_vec_;
-    gr::blocks::throttle::sptr throttle_;
+    std::vector<gr::blocks::throttle::sptr> throttle_vec_;
     gr::msg_queue::sptr queue_;
     size_t item_size_;
 };

src/algorithms/signal_source/gnuradio_blocks/unpack_spir_gss6450_samples.cc

@@ -40,7 +40,7 @@ unpack_spir_gss6450_samples_sptr make_unpack_spir_gss6450_samples(unsigned int a
 
 
 unpack_spir_gss6450_samples::unpack_spir_gss6450_samples(unsigned int adc_nbit) : gr::sync_interpolator("unpack_spir_gss6450_samples",
-                                                                                      gr::io_signature::make(1, 1, sizeof(int)),
+                                                                                      gr::io_signature::make(1, 1, sizeof(int32_t)),
                                                                                       gr::io_signature::make(1, 1, sizeof(gr_complex)), 16 / adc_nbit)
 {
     adc_bits = adc_nbit;

src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc

@@ -50,7 +50,7 @@
 #include "GPS_L5.h"
 #include "gps_l5_signal.h"
 #include "gnss_sdr_create_directory.h"
-#include <boost/lexical_cast.hpp>
+#include <boost/filesystem/path.hpp>
 #include <glog/logging.h>
 #include <gnuradio/io_signature.h>
 #include <matio.h>
@@ -1091,7 +1091,7 @@ int32_t dll_pll_veml_tracking::save_matfile()
     std::ifstream dump_file;
     std::string dump_filename_ = d_dump_filename;
     // add channel number to the filename
-    dump_filename_.append(boost::lexical_cast<std::string>(d_channel));
+    dump_filename_.append(std::to_string(d_channel));
     // add extension
     dump_filename_.append(".dat");
     std::cout << "Generating .mat file for " << dump_filename_ << std::endl;
@@ -1335,7 +1335,7 @@ void dll_pll_veml_tracking::set_channel(uint32_t channel)
         {
             std::string dump_filename_ = d_dump_filename;
             // add channel number to the filename
-            dump_filename_.append(boost::lexical_cast<std::string>(d_channel));
+            dump_filename_.append(std::to_string(d_channel));
             // add extension
             dump_filename_.append(".dat");
 

src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.cc

@@ -48,7 +48,7 @@
 #include "GPS_L5.h"
 #include "gps_l5_signal.h"
 #include "gnss_sdr_create_directory.h"
-#include <boost/lexical_cast.hpp>
+#include <boost/filesystem/path.hpp>
 #include <glog/logging.h>
 #include <gnuradio/io_signature.h>
 #include <matio.h>
@@ -1011,7 +1011,7 @@ int32_t dll_pll_veml_tracking_fpga::save_matfile()
     std::ifstream dump_file;
     std::string dump_filename_ = d_dump_filename;
     // add channel number to the filename
-    dump_filename_.append(boost::lexical_cast<std::string>(d_channel));
+    dump_filename_.append(std::to_string(d_channel));
     // add extension
     dump_filename_.append(".dat");
     std::cout << "Generating .mat file for " << dump_filename_ << std::endl;
@@ -1239,7 +1239,7 @@ void dll_pll_veml_tracking_fpga::set_channel(uint32_t channel)
         {
             std::string dump_filename_ = d_dump_filename;
             // add channel number to the filename
-            dump_filename_.append(boost::lexical_cast<std::string>(d_channel));
+            dump_filename_.append(std::to_string(d_channel));
             // add extension
             dump_filename_.append(".dat");
 

src/core/interfaces/pvt_interface.h

@@ -38,6 +38,10 @@
 #define GNSS_SDR_PVT_INTERFACE_H_
 
 #include "gnss_block_interface.h"
+#include "gps_ephemeris.h"
+#include "galileo_ephemeris.h"
+#include "gps_almanac.h"
+#include "galileo_almanac.h"
 
 /*!
  * \brief This class represents an interface to a PVT block.
@@ -52,6 +56,18 @@ class PvtInterface : public GNSSBlockInterface
 {
 public:
     virtual void reset() = 0;
+    virtual void clear_ephemeris() = 0;
+    virtual std::map<int, Gps_Ephemeris> get_gps_ephemeris() const = 0;
+    virtual std::map<int, Galileo_Ephemeris> get_galileo_ephemeris() const = 0;
+    virtual std::map<int, Gps_Almanac> get_gps_almanac() const = 0;
+    virtual std::map<int, Galileo_Almanac> get_galileo_almanac() const = 0;
+
+    virtual bool get_latest_PVT(double* longitude_deg,
+        double* latitude_deg,
+        double* height_m,
+        double* ground_speed_kmh,
+        double* course_over_ground_deg,
+        time_t* UTC_time) = 0;
 };
 
 #endif /* GNSS_SDR_PVT_INTERFACE_H_ */

src/core/libs/CMakeLists.txt

@@ -45,6 +45,7 @@ include_directories(
     ${GLOG_INCLUDE_DIRS}
     ${GFlags_INCLUDE_DIRS}
     ${Boost_INCLUDE_DIRS}
+    ${PUGIXML_INCLUDE_DIR}
 )
 
 list(SORT CORE_LIBS_HEADERS)
@@ -52,4 +53,8 @@ list(SORT CORE_LIBS_SOURCES)
 
 add_library(rx_core_lib ${CORE_LIBS_SOURCES} ${CORE_LIBS_HEADERS})
 source_group(Headers FILES ${CORE_LIBS_HEADERS})
-target_link_libraries(rx_core_lib supl_library) 
+target_link_libraries(rx_core_lib supl_library ${PUGIXML_LIBRARY})
+
+if(PUGIXML_LOCAL)
+    add_dependencies(rx_core_lib pugixml-${GNSSSDR_PUGIXML_LOCAL_VERSION})
+endif(PUGIXML_LOCAL)

src/core/libs/gnss_sdr_supl_client.cc

@@ -32,6 +32,7 @@
  */
 
 #include "gnss_sdr_supl_client.h"
+#include <pugixml.hpp>
 #include <cmath>
 #include <utility>
 
@@ -267,7 +268,7 @@ void gnss_sdr_supl_client::read_supl_data()
                     gps_almanac_iterator->second.d_OMEGA0 = static_cast<double>(a->OMEGA_0) * pow(2.0, -23);
                     gps_almanac_iterator->second.d_sqrt_A = static_cast<double>(a->A_sqrt) * pow(2.0, -11);
                     gps_almanac_iterator->second.d_OMEGA_DOT = static_cast<double>(a->OMEGA_dot) * pow(2.0, -38);
-                    gps_almanac_iterator->second.d_Toa = static_cast<double>(a->toa) * pow(2.0, 12);
+                    gps_almanac_iterator->second.i_Toa = static_cast<int32_t>(a->toa) * pow(2.0, 12);
                     gps_almanac_iterator->second.d_e_eccentricity = static_cast<double>(a->e) * pow(2.0, -21);
                     gps_almanac_iterator->second.d_M_0 = static_cast<double>(a->M0) * pow(2.0, -23);
                 }
@@ -842,11 +843,61 @@ bool gnss_sdr_supl_client::load_gal_almanac_xml(const std::string file_name)
             boost::archive::xml_iarchive xml(ifs);
             gal_almanac_map.clear();
             xml >> boost::serialization::make_nvp("GNSS-SDR_gal_almanac_map", this->gal_almanac_map);
-            LOG(INFO) << "Loaded Galileo almanac map data with " << this->gal_almanac_map.size() << " satellites";
         }
     catch (std::exception& e)
         {
-            LOG(WARNING) << e.what() << "File: " << file_name;
+            // Maybe the file is from https://www.gsc-europa.eu/system-status/almanac-data ?
+            return this->read_gal_almanac_from_gsa(file_name);
+        }
+    LOG(INFO) << "Loaded Galileo almanac map data with " << this->gal_almanac_map.size() << " satellites";
+    return true;
+}
+
+
+bool gnss_sdr_supl_client::read_gal_almanac_from_gsa(const std::string file_name)
+{
+    pugi::xml_document doc;
+    pugi::xml_parse_result result = doc.load_file(file_name.c_str());
+    if (!result)
+        {
+            LOG(WARNING) << "Error loading file " << file_name << ":" << result.description();
+            return false;
+        }
+    for (pugi::xml_node almanac : doc.child("signalData")
+                                      .child("body")
+                                      .child("Almanacs")
+                                      .children("svAlmanac"))
+        {
+            Galileo_Almanac gal_alm;
+            try
+                {
+                    uint32_t prn = static_cast<uint32_t>(std::stoi(almanac.child_value("SVID")));
+                    gal_alm.i_satellite_PRN = prn;
+                    gal_alm.i_Toa = std::stoi(almanac.child("almanac").child_value("t0a"));
+                    gal_alm.i_WNa = std::stoi(almanac.child("almanac").child_value("wna"));
+                    gal_alm.i_IODa = std::stoi(almanac.child("almanac").child_value("iod"));
+                    gal_alm.d_Delta_i = std::stod(almanac.child("almanac").child_value("deltai"));
+                    gal_alm.d_M_0 = std::stod(almanac.child("almanac").child_value("deltai"));
+                    gal_alm.d_e_eccentricity = std::stod(almanac.child("almanac").child_value("ecc"));
+                    gal_alm.d_Delta_sqrt_A = std::stod(almanac.child("almanac").child_value("aSqRoot"));
+                    gal_alm.d_OMEGA0 = std::stod(almanac.child("almanac").child_value("omega0"));
+                    gal_alm.d_OMEGA = std::stod(almanac.child("almanac").child_value("w"));
+                    gal_alm.d_OMEGA_DOT = std::stod(almanac.child("almanac").child_value("omegaDot"));
+                    gal_alm.d_A_f0 = std::stod(almanac.child("almanac").child_value("af0"));
+                    gal_alm.d_A_f1 = std::stod(almanac.child("almanac").child_value("af1"));
+                    gal_alm.E5b_HS = std::stoi(almanac.child("svINavSignalStatus").child_value("statusE5b"));
+                    gal_alm.E1B_HS = std::stoi(almanac.child("svINavSignalStatus").child_value("statusE1B"));
+                    gal_alm.E5a_HS = std::stoi(almanac.child("svFNavSignalStatus").child_value("statusE5a"));
+
+                    this->gal_almanac_map[static_cast<int>(prn)] = gal_alm;
+                }
+            catch (const std::exception& e)
+                {
+                    std::cerr << e.what() << std::endl;
+                }
+        }
+    if (this->gal_almanac_map.empty())
+        {
             return false;
         }
     return true;

src/core/libs/gnss_sdr_supl_client.h

@@ -77,6 +77,7 @@ private:
     supl_ctx_t ctx;
     // assistance data
     supl_assist_t assist;
+    bool read_gal_almanac_from_gsa(const std::string file_name);
 
 public:
     // SUPL SERVER INFO

src/core/receiver/CMakeLists.txt

@@ -157,6 +157,7 @@ include_directories(
      ${GFlags_INCLUDE_DIRS}
      ${Boost_INCLUDE_DIRS}
      ${GNURADIO_RUNTIME_INCLUDE_DIRS}
+     ${PUGIXML_INCLUDE_DIR}
      ${OPT_RECEIVER_INCLUDE_DIRS}
      ${VOLK_GNSSSDR_INCLUDE_DIRS}
 )

src/core/receiver/control_thread.cc

@@ -35,6 +35,7 @@
 #include "control_thread.h"
 #include "concurrent_queue.h"
 #include "concurrent_map.h"
+#include "pvt_interface.h"
 #include "control_message_factory.h"
 #include "file_configuration.h"
 #include "gnss_flowgraph.h"
@@ -49,6 +50,10 @@
 #include "gps_almanac.h"
 #include "glonass_gnav_ephemeris.h"
 #include "glonass_gnav_utc_model.h"
+#include "geofunctions.h"
+#include "rtklib_rtkcmn.h"
+#include "rtklib_conversions.h"
+#include "rtklib_ephemeris.h"
 #include <boost/lexical_cast.hpp>
 #include <boost/chrono.hpp>
 #include <glog/logging.h>
@@ -89,6 +94,7 @@ ControlThread::ControlThread(std::shared_ptr<ConfigurationInterface> configurati
 {
     configuration_ = configuration;
     delete_configuration_ = false;
+    restart_ = false;
     init();
 }
 
@@ -107,6 +113,7 @@ void ControlThread::telecommand_listener()
     cmd_interface_.run_cmd_server(tcp_cmd_port);
 }
 
+
 /*
  * Runs the control thread that manages the receiver control plane
  *
@@ -149,16 +156,16 @@ int ControlThread::run()
             return 0;
         }
 
-    //launch GNSS assistance process AFTER the flowgraph is running because the GNURadio asynchronous queues must be already running to transport msgs
+    // launch GNSS assistance process AFTER the flowgraph is running because the GNU Radio asynchronous queues must be already running to transport msgs
     assist_GNSS();
     // start the keyboard_listener thread
     keyboard_thread_ = boost::thread(&ControlThread::keyboard_listener, this);
     sysv_queue_thread_ = boost::thread(&ControlThread::sysv_queue_listener, this);
 
-    //start the telecommand listener thread
+    // start the telecommand listener thread
+    cmd_interface_.set_pvt(flowgraph_->get_pvt());
     cmd_interface_thread_ = boost::thread(&ControlThread::telecommand_listener, this);
 
-
     bool enable_FPGA = configuration_->property("Channel.enable_FPGA", false);
     if (enable_FPGA == true)
         {
@@ -234,8 +241,8 @@ bool ControlThread::read_assistance_from_XML()
     std::string cnav_utc_xml_filename = configuration_->property("GNSS-SDR.SUPL_cnav_utc_model_xml", cnav_utc_default_xml_filename);
     std::string eph_glo_xml_filename = configuration_->property("GNSS-SDR.SUPL_glo_ephemeris_xml", eph_glo_gnav_default_xml_filename);
     std::string glo_utc_xml_filename = configuration_->property("GNSS-SDR.SUPL_glo_utc_model_xml", glo_utc_default_xml_filename);
-    std::string gal_almanac_xml_filename = configuration_->property("GNSS-SDR.SUPL_gal_almanacl_xml", gal_almanac_default_xml_filename);
+    std::string gal_almanac_xml_filename = configuration_->property("GNSS-SDR.SUPL_gal_almanac_xml", gal_almanac_default_xml_filename);
-    std::string gps_almanac_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_almanacl_xml", gps_almanac_default_xml_filename);
+    std::string gps_almanac_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_almanac_xml", gps_almanac_default_xml_filename);
 
     if (configuration_->property("GNSS-SDR.AGNSS_XML_enabled", false) == true)
         {
@@ -251,7 +258,8 @@ bool ControlThread::read_assistance_from_XML()
             cnav_utc_xml_filename = configuration_->property("GNSS-SDR.AGNSS_cnav_utc_model_xml", cnav_utc_default_xml_filename);
             eph_glo_xml_filename = configuration_->property("GNSS-SDR.AGNSS_glo_ephemeris_xml", eph_glo_gnav_default_xml_filename);
             glo_utc_xml_filename = configuration_->property("GNSS-SDR.AGNSS_glo_utc_model_xml", glo_utc_default_xml_filename);
-            gal_almanac_xml_filename = configuration_->property("GNSS-SDR.AGNSS_gal_almanacl_xml", gal_almanac_default_xml_filename);
+            gal_almanac_xml_filename = configuration_->property("GNSS-SDR.AGNSS_gal_almanac_xml", gal_almanac_default_xml_filename);
+            gps_almanac_xml_filename = configuration_->property("GNSS-SDR.AGNSS_gps_almanac_xml", gps_almanac_default_xml_filename);
         }
 
     std::cout << "Trying to read GNSS ephemeris from XML file(s)..." << std::endl;
@@ -693,6 +701,10 @@ void ControlThread::process_control_messages()
                 }
             else
                 {
+                    if (control_messages_->at(i)->who == 300)  //some TC commands require also actions from controlthread
+                        {
+                            apply_action(control_messages_->at(i)->what);
+                        }
                     flowgraph_->apply_action(control_messages_->at(i)->who, control_messages_->at(i)->what);
                 }
             processed_control_messages_++;
@@ -704,26 +716,181 @@ void ControlThread::process_control_messages()
 
 void ControlThread::apply_action(unsigned int what)
 {
+    std::shared_ptr<PvtInterface> pvt_ptr;
+    std::vector<std::pair<int, Gnss_Satellite>> visible_satellites;
     switch (what)
         {
         case 0:
-            DLOG(INFO) << "Received action STOP";
+            LOG(INFO) << "Received action STOP";
             stop_ = true;
             applied_actions_++;
             break;
         case 1:
-            DLOG(INFO) << "Received action RESTART";
+            LOG(INFO) << "Received action RESTART";
             stop_ = true;
             restart_ = true;
             applied_actions_++;
             break;
+        case 11:
+            LOG(INFO) << "Receiver action COLDSTART";
+            //delete all ephemeris and almanac information from maps (also the PVT map queue)
+            pvt_ptr = flowgraph_->get_pvt();
+            pvt_ptr->clear_ephemeris();
+            //todo: reorder the satellite queues to the receiver default startup order.
+            //This is required to allow repeatability. Otherwise the satellite search order will depend on the last tracked satellites
+            break;
+        case 12:
+            LOG(INFO) << "Receiver action HOTSTART";
+            visible_satellites = get_visible_sats(cmd_interface_.get_utc_time(), cmd_interface_.get_LLH());
+            //reorder the satellite queue to acquire first those visible satellites
+            flowgraph_->priorize_satellites(visible_satellites);
+            //start again the satellite acquisitions (done in chained applyaction to flowgraph)
+            break;
+        case 13:
+            LOG(INFO) << "Receiver action WARMSTART";
+            //delete all ephemeris and almanac information from maps (also the PVT map queue)
+            pvt_ptr = flowgraph_->get_pvt();
+            pvt_ptr->clear_ephemeris();
+            //load the ephemeris and the almanac from XML files (receiver assistance)
+            read_assistance_from_XML();
+            //call here the function that computes the set of visible satellites and its elevation
+            //for the date and time specified by the warmstart command and the assisted position
+            get_visible_sats(cmd_interface_.get_utc_time(), cmd_interface_.get_LLH());
+            //reorder the satellite queue to acquire first those visible satellites
+            flowgraph_->priorize_satellites(visible_satellites);
+            //start again the satellite acquisitions (done in chained applyaction to flowgraph)
+            break;
         default:
-            DLOG(INFO) << "Unrecognized action.";
+            LOG(INFO) << "Unrecognized action.";
             break;
         }
 }
 
 
+std::vector<std::pair<int, Gnss_Satellite>> ControlThread::get_visible_sats(time_t rx_utc_time, const arma::vec &LLH)
+{
+    // 1. Compute rx ECEF position from LLH WGS84
+    arma::vec LLH_rad = arma::vec{degtorad(LLH(0)), degtorad(LLH(1)), LLH(2)};
+    arma::mat C_tmp = arma::zeros(3, 3);
+    arma::vec r_eb_e = arma::zeros(3, 1);
+    arma::vec v_eb_e = arma::zeros(3, 1);
+    Geo_to_ECEF(LLH_rad, arma::vec{0, 0, 0}, C_tmp, r_eb_e, v_eb_e, C_tmp);
+
+    // 2. Compute rx GPS time from UTC time
+    gtime_t utc_gtime;
+    utc_gtime.time = rx_utc_time;
+    utc_gtime.sec = 0;
+    gtime_t gps_gtime = utc2gpst(utc_gtime);
+
+    // 3. loop through all the available ephemeris or almanac and compute satellite positions and elevations
+    // store visible satellites in a vector of pairs <int,Gnss_Satellite> to associate an elevation to the each satellite
+    std::vector<std::pair<int, Gnss_Satellite>> available_satellites;
+
+    std::shared_ptr<PvtInterface> pvt_ptr = flowgraph_->get_pvt();
+    struct tm tstruct;
+    char buf[80];
+    tstruct = *gmtime(&rx_utc_time);
+    strftime(buf, sizeof(buf), "%d/%m/%Y %H:%M:%S ", &tstruct);
+    std::string str_time = std::string(buf);
+    std::cout << "Get visible satellites at " << str_time
+              << " UTC, assuming RX position " << LLH(0) << " [deg], " << LLH(1) << " [deg], " << LLH(2) << " [m]" << std::endl;
+
+    std::map<int, Gps_Ephemeris> gps_eph_map = pvt_ptr->get_gps_ephemeris();
+    for (std::map<int, Gps_Ephemeris>::iterator it = gps_eph_map.begin(); it != gps_eph_map.end(); ++it)
+        {
+            eph_t rtklib_eph = eph_to_rtklib(it->second);
+            double r_sat[3];
+            double clock_bias_s;
+            double sat_pos_variance_m2;
+            eph2pos(gps_gtime, &rtklib_eph, &r_sat[0], &clock_bias_s,
+                &sat_pos_variance_m2);
+            double Az, El, dist_m;
+            arma::vec r_sat_eb_e = arma::vec{r_sat[0], r_sat[1], r_sat[2]};
+            arma::vec dx = r_sat_eb_e - r_eb_e;
+            topocent(&Az, &El, &dist_m, r_eb_e, dx);
+            // push sat
+            if (El > 0)
+                {
+                    std::cout << "Using GPS Ephemeris: Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl;
+                    available_satellites.push_back(std::pair<int, Gnss_Satellite>(floor(El),
+                        (Gnss_Satellite(std::string("GPS"), it->second.i_satellite_PRN))));
+                }
+        }
+
+    std::map<int, Galileo_Ephemeris> gal_eph_map = pvt_ptr->get_galileo_ephemeris();
+    for (std::map<int, Galileo_Ephemeris>::iterator it = gal_eph_map.begin(); it != gal_eph_map.end(); ++it)
+        {
+            eph_t rtklib_eph = eph_to_rtklib(it->second);
+            double r_sat[3];
+            double clock_bias_s;
+            double sat_pos_variance_m2;
+            eph2pos(gps_gtime, &rtklib_eph, &r_sat[0], &clock_bias_s,
+                &sat_pos_variance_m2);
+            double Az, El, dist_m;
+            arma::vec r_sat_eb_e = arma::vec{r_sat[0], r_sat[1], r_sat[2]};
+            arma::vec dx = r_sat_eb_e - r_eb_e;
+            topocent(&Az, &El, &dist_m, r_eb_e, dx);
+            // push sat
+            if (El > 0)
+                {
+                    std::cout << "Using Galileo Ephemeris: Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl;
+                    available_satellites.push_back(std::pair<int, Gnss_Satellite>(floor(El),
+                        (Gnss_Satellite(std::string("Galileo"), it->second.i_satellite_PRN))));
+                }
+        }
+
+    std::map<int, Gps_Almanac> gps_alm_map = pvt_ptr->get_gps_almanac();
+    for (std::map<int, Gps_Almanac>::iterator it = gps_alm_map.begin(); it != gps_alm_map.end(); ++it)
+        {
+            alm_t rtklib_alm = alm_to_rtklib(it->second);
+            double r_sat[3];
+            double clock_bias_s;
+            double sat_pos_variance_m2;
+            alm2pos(gps_gtime, &rtklib_alm, &r_sat[0], &clock_bias_s);
+            double Az, El, dist_m;
+            arma::vec r_sat_eb_e = arma::vec{r_sat[0], r_sat[1], r_sat[2]};
+            arma::vec dx = r_sat_eb_e - r_eb_e;
+            topocent(&Az, &El, &dist_m, r_eb_e, dx);
+            // push sat
+            if (El > 0)
+                {
+                    std::cout << "Using GPS Almanac:  Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl;
+                    available_satellites.push_back(std::pair<int, Gnss_Satellite>(floor(El),
+                        (Gnss_Satellite(std::string("GPS"), it->second.i_satellite_PRN))));
+                }
+        }
+
+    std::map<int, Galileo_Almanac> gal_alm_map = pvt_ptr->get_galileo_almanac();
+    for (std::map<int, Galileo_Almanac>::iterator it = gal_alm_map.begin(); it != gal_alm_map.end(); ++it)
+        {
+            alm_t rtklib_alm = alm_to_rtklib(it->second);
+            double r_sat[3];
+            double clock_bias_s;
+            double sat_pos_variance_m2;
+            alm2pos(gps_gtime, &rtklib_alm, &r_sat[0], &clock_bias_s);
+            double Az, El, dist_m;
+            arma::vec r_sat_eb_e = arma::vec{r_sat[0], r_sat[1], r_sat[2]};
+            arma::vec dx = r_sat_eb_e - r_eb_e;
+            topocent(&Az, &El, &dist_m, r_eb_e, dx);
+            std::cout << "Using Galileo Almanac:  Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl;
+            // push sat
+            if (El > 0)
+                {
+                    std::cout << "Using GPS Almanac:  Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl;
+                    available_satellites.push_back(std::pair<int, Gnss_Satellite>(floor(El),
+                        (Gnss_Satellite(std::string("Galileo"), it->second.i_satellite_PRN))));
+                }
+        }
+
+    // sort the visible satellites in ascending order of elevation
+    std::sort(available_satellites.begin(), available_satellites.end(), [](const std::pair<int, Gnss_Satellite> &a, const std::pair<int, Gnss_Satellite> &b) {  // use lambda. Cleaner and easier to read
+        return a.first < b.first;
+    });
+    // std::reverse(available_satellites.begin(), available_satellites.end());
+    return available_satellites;
+}
+
+
 void ControlThread::gps_acq_assist_data_collector()
 {
     // ############ 1.bis READ EPHEMERIS/UTC_MODE/IONO QUEUE ####################

src/core/receiver/control_thread.h

@@ -35,16 +35,17 @@
 #ifndef GNSS_SDR_CONTROL_THREAD_H_
 #define GNSS_SDR_CONTROL_THREAD_H_
 
+#include "gnss_satellite.h"
 #include "control_message_factory.h"
 #include "gnss_sdr_supl_client.h"
 #include "tcp_cmd_interface.h"
+#include "gnss_flowgraph.h"
+#include "configuration_interface.h"
 #include <boost/thread.hpp>
 #include <gnuradio/msg_queue.h>
 #include <memory>
 #include <vector>
-
+#include <armadillo>
-class GNSSFlowgraph;
-class ConfigurationInterface;
 
 
 /*!
@@ -143,6 +144,12 @@ private:
      */
     void gps_acq_assist_data_collector();
 
+    /*
+     * Compute elevations for the specified time and position for all the available satellites in ephemeris and almanac queues
+     * returns a vector filled with the available satellites ordered from high elevation to low elevation angle.
+     */
+    std::vector<std::pair<int, Gnss_Satellite>> get_visible_sats(time_t rx_utc_time, const arma::vec& LLH);
+
     /*
      * Read initial GNSS assistance from SUPL server or local XML files
      */

src/core/receiver/gnss_flowgraph.cc

@@ -1103,11 +1103,7 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what)
             acq_channels_count_ = 0;  //all channels are in stanby now
             break;
         case 11:  //request coldstart mode
-            LOG(INFO) << "TC request coldstart";
+            LOG(INFO) << "TC request flowgraph coldstart";
-            //todo: delete all ephemeris and almanac information from maps (also the PVT map queue)
-            //todo: reorder the satellite queues to the receiver default startup order.
-            //This is required to allow repeatability. Otherwise the satellite search order will depend on the last tracked satellites
-
             //start again the satellite acquisitions
             for (unsigned int i = 0; i < channels_count_; i++)
                 {
@@ -1136,11 +1132,7 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what)
                 }
             break;
         case 12:  //request hotstart mode
-            LOG(INFO) << "TC request hotstart";
+            LOG(INFO) << "TC request flowgraph hotstart";
-            //todo: call here the function that computes the set of visible satellites and its elevation
-            //for the date and time specified by the hotstart command and the last available PVT
-            //todo: reorder the satellite queue to acquire first those visible satellites
-            //start again the satellite acquisitions
             for (unsigned int i = 0; i < channels_count_; i++)
                 {
                     unsigned int ch_index = (who + i + 1) % channels_count_;
@@ -1168,13 +1160,7 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what)
                 }
             break;
         case 13:  //request warmstart mode
-            LOG(INFO) << "TC request warmstart";
+            LOG(INFO) << "TC request flowgraph warmstart";
-            //todo: delete all ephemeris and almanac information from maps (also the PVT map queue)
-            //todo: load the ephemeris and the almanac from XML files (receiver assistance)
-            //todo: call here the function that computes the set of visible satellites and its elevation
-            //for the date and time specified by the warmstart command and the assisted position
-            //todo: reorder the satellite queue to acquire first those visible satellites
-
             //start again the satellite acquisitions
             for (unsigned int i = 0; i < channels_count_; i++)
                 {
@@ -1209,6 +1195,60 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what)
 }
 
 
+void GNSSFlowgraph::priorize_satellites(std::vector<std::pair<int, Gnss_Satellite>> visible_satellites)
+{
+    size_t old_size;
+    Gnss_Signal gs;
+    for (std::vector<std::pair<int, Gnss_Satellite>>::iterator it = visible_satellites.begin(); it != visible_satellites.end(); ++it)
+        {
+            if (it->second.get_system() == "GPS")
+                {
+                    gs = Gnss_Signal(it->second, "1C");
+                    old_size = available_GPS_1C_signals_.size();
+                    available_GPS_1C_signals_.remove(gs);
+                    if (old_size > available_GPS_1C_signals_.size())
+                        {
+                            available_GPS_1C_signals_.push_front(gs);
+                        }
+
+                    gs = Gnss_Signal(it->second, "2S");
+                    old_size = available_GPS_2S_signals_.size();
+                    available_GPS_2S_signals_.remove(gs);
+                    if (old_size > available_GPS_2S_signals_.size())
+                        {
+                            available_GPS_2S_signals_.push_front(gs);
+                        }
+
+                    gs = Gnss_Signal(it->second, "L5");
+                    old_size = available_GPS_L5_signals_.size();
+                    available_GPS_L5_signals_.remove(gs);
+                    if (old_size > available_GPS_L5_signals_.size())
+                        {
+                            available_GPS_L5_signals_.push_front(gs);
+                        }
+                }
+            else if (it->second.get_system() == "Galileo")
+                {
+                    gs = Gnss_Signal(it->second, "1B");
+                    old_size = available_GAL_1B_signals_.size();
+                    available_GAL_1B_signals_.remove(gs);
+                    if (old_size > available_GAL_1B_signals_.size())
+                        {
+                            available_GAL_1B_signals_.push_front(gs);
+                        }
+
+                    gs = Gnss_Signal(it->second, "5X");
+                    old_size = available_GAL_5X_signals_.size();
+                    available_GAL_5X_signals_.remove(gs);
+                    if (old_size > available_GAL_5X_signals_.size())
+                        {
+                            available_GAL_5X_signals_.push_front(gs);
+                        }
+                }
+        }
+}
+
+
 void GNSSFlowgraph::set_configuration(std::shared_ptr<ConfigurationInterface> configuration)
 {
     if (running_)

src/core/receiver/gnss_flowgraph.h

@@ -41,6 +41,11 @@
 #include "gnss_signal.h"
 #include "gnss_sdr_sample_counter.h"
 #include "gnss_synchro_monitor.h"
+#include "gnss_block_interface.h"
+#include "pvt_interface.h"
+#include "channel_interface.h"
+#include "configuration_interface.h"
+#include "gnss_block_factory.h"
 #include <gnuradio/top_block.h>
 #include <gnuradio/msg_queue.h>
 #include <list>
@@ -55,10 +60,6 @@
 #include "gnss_sdr_fpga_sample_counter.h"
 #endif
 
-class GNSSBlockInterface;
-class ChannelInterface;
-class ConfigurationInterface;
-class GNSSBlockFactory;
 
 /*! \brief This class represents a GNSS flow graph.
  *
@@ -129,6 +130,19 @@ public:
      */
     bool send_telemetry_msg(pmt::pmt_t msg);
 
+    /*!
+     * \brief Returns a smart pointer to the PVT object
+     */
+    std::shared_ptr<PvtInterface> get_pvt()
+    {
+        return std::dynamic_pointer_cast<PvtInterface>(pvt_);
+    }
+
+    /*!
+     * \brief Priorize visible satellites in the specified vector
+     */
+    void priorize_satellites(std::vector<std::pair<int, Gnss_Satellite>> visible_satellites);
+
 private:
     void init();  // Populates the SV PRN list available for acquisition and tracking
     void set_signals_list();

src/core/receiver/tcp_cmd_interface.cc

@@ -32,8 +32,22 @@
 #include "tcp_cmd_interface.h"
 #include "control_message_factory.h"
 #include <functional>
+#include <sstream>
 
 
+void TcpCmdInterface::set_pvt(std::shared_ptr<PvtInterface> PVT_sptr)
+{
+    PVT_sptr_ = PVT_sptr;
+}
+time_t TcpCmdInterface::get_utc_time()
+{
+    return receiver_utc_time_;
+}
+
+arma::vec TcpCmdInterface::get_LLH()
+{
+    return arma::vec{rx_latitude_, rx_longitude_, rx_altitude_};
+}
 std::string TcpCmdInterface::reset(const std::vector<std::string> &commandLine)
 {
     std::string response;
@@ -69,26 +83,90 @@ std::string TcpCmdInterface::standby(const std::vector<std::string> &commandLine
 
 std::string TcpCmdInterface::status(const std::vector<std::string> &commandLine)
 {
-    std::string response;
+    std::stringstream str_stream;
     //todo: implement the receiver status report
-    response = "Not implemented\n";
+
-    return response;
+    //    str_stream << "-------------------------------------------------------\n";
+    //    str_stream << "ch | sys | sig | mode | Tlm | Eph | Doppler | CN0 |\n";
+    //    str_stream << "   |     |     |      |     |     |  [Hz]   | [dB - Hz] |\n";
+    //    str_stream << "-------------------------------------------------------\n";
+    //    int n_ch = 10;
+    //    for (int n = 0; n < n_ch; n++)
+    //        {
+    //            str_stream << n << "GPS | L1CA | TRK | YES | YES | 23412.4 | 44.3 |\n";
+    //        }
+    //    str_stream << "--------------------------------------------------------\n";
+
+    double longitude_deg, latitude_deg, height_m, ground_speed_kmh, course_over_ground_deg;
+    time_t UTC_time;
+    if (PVT_sptr_->get_latest_PVT(&longitude_deg,
+            &latitude_deg,
+            &height_m,
+            &ground_speed_kmh,
+            &course_over_ground_deg,
+            &UTC_time) == true)
+        {
+            struct tm tstruct;
+            char buf1[80];
+            tstruct = *gmtime(&UTC_time);
+            strftime(buf1, sizeof(buf1), "%d/%m/%Y %H:%M:%S", &tstruct);
+            std::string str_time = std::string(buf1);
+            str_stream << "- Receiver UTC Time: " << str_time << std::endl;
+            str_stream << std::setprecision(9);
+            str_stream << "- Receiver Position WGS84 [Lat, Long, H]: "
+                       << latitude_deg << ", "
+                       << longitude_deg << ", ";
+            str_stream << std::setprecision(3);
+            str_stream << height_m << std::endl;
+            str_stream << std::setprecision(1);
+            str_stream << "- Receiver Speed over Ground [km/h]: " << ground_speed_kmh << std::endl;
+            str_stream << "- Receiver Course over ground [deg]: " << course_over_ground_deg << std::endl;
+        }
+    else
+        {
+            str_stream << "No PVT information available.\n";
+        }
+
+    return str_stream.str();
 }
 
 std::string TcpCmdInterface::hotstart(const std::vector<std::string> &commandLine)
 {
     std::string response;
-    //todo: read and parse the command line parameter: dd/mm/yyyy HH:MM:SS
+    if (commandLine.size() > 5)
-    //todo: store it in a member variable
-    std::unique_ptr<ControlMessageFactory> cmf(new ControlMessageFactory());
-    if (control_queue_ != nullptr)
         {
-            control_queue_->handle(cmf->GetQueueMessage(300, 12));  //send the standby message (who=300,what=12)
+            //read commandline time parameter
-            response = "OK\n";
+            struct tm tm;
+            strptime(commandLine.at(1).c_str(), "%d/%m/%Y %H:%M:%S", &tm);
+            receiver_utc_time_ = timegm(&tm);
+
+
+            //read latitude, longitude, and height
+            rx_latitude_ = std::stod(commandLine.at(3).c_str());
+            rx_longitude_ = std::stod(commandLine.at(4).c_str());
+            rx_altitude_ = std::stod(commandLine.at(5).c_str());
+
+            if (std::isnan(rx_latitude_) || std::isnan(rx_longitude_) || std::isnan(rx_altitude_))
+                {
+                    response = "ERROR: position malformed\n";
+                }
+            else
+                {
+                    std::unique_ptr<ControlMessageFactory> cmf(new ControlMessageFactory());
+                    if (control_queue_ != nullptr)
+                        {
+                            control_queue_->handle(cmf->GetQueueMessage(300, 12));  //send the standby message (who=300,what=12)
+                            response = "OK\n";
+                        }
+                    else
+                        {
+                            response = "ERROR\n";
+                        }
+                }
         }
     else
         {
-            response = "ERROR\n";
+            response = "ERROR: time parameter not found, please use hotstart %d/%m/%Y %H:%M:%S\n";
         }
     return response;
 }
@@ -96,17 +174,40 @@ std::string TcpCmdInterface::hotstart(const std::vector<std::string> &commandLin
 std::string TcpCmdInterface::warmstart(const std::vector<std::string> &commandLine)
 {
     std::string response;
-    //todo: read and parse the command line parameter: dd/mm/yyyy HH:MM:SS
+    if (commandLine.size() > 5)
-    //todo: store it in a member variable
-    std::unique_ptr<ControlMessageFactory> cmf(new ControlMessageFactory());
-    if (control_queue_ != nullptr)
         {
-            control_queue_->handle(cmf->GetQueueMessage(300, 13));  //send the standby message (who=300,what=13)
+            std::string tmp_str;
-            response = "OK\n";
+            //read commandline time parameter
+            struct tm tm;
+            tmp_str = commandLine.at(1) + commandLine.at(2);
+            strptime(tmp_str.c_str(), "%d/%m/%Y %H:%M:%S", &tm);
+            receiver_utc_time_ = timegm(&tm);
+
+            //read latitude, longitude, and height
+            rx_latitude_ = std::stod(commandLine.at(3).c_str());
+            rx_longitude_ = std::stod(commandLine.at(4).c_str());
+            rx_altitude_ = std::stod(commandLine.at(5).c_str());
+            if (std::isnan(rx_latitude_) || std::isnan(rx_longitude_) || std::isnan(rx_altitude_))
+                {
+                    response = "ERROR: position malformed\n";
+                }
+            else
+                {
+                    std::unique_ptr<ControlMessageFactory> cmf(new ControlMessageFactory());
+                    if (control_queue_ != nullptr)
+                        {
+                            control_queue_->handle(cmf->GetQueueMessage(300, 13));  //send the standby message (who=300,what=13)
+                            response = "OK\n";
+                        }
+                    else
+                        {
+                            response = "ERROR\n";
+                        }
+                }
         }
     else
         {
-            response = "ERROR\n";
+            response = "ERROR: time parameter not found, please use warmstart %d/%m/%Y %H:%M:%S Lat Long H\n";
         }
     return response;
 }
@@ -154,6 +255,10 @@ TcpCmdInterface::TcpCmdInterface()
     register_functions();
     keep_running_ = true;
     control_queue_ = nullptr;
+    rx_latitude_ = 0;
+    rx_longitude_ = 0;
+    rx_altitude_ = 0;
+    receiver_utc_time_ = 0;
 }
 
 
@@ -218,6 +323,10 @@ void TcpCmdInterface::run_cmd_server(int tcp_port)
                                                             response = functions[cmd_vector.at(0)](cmd_vector);
                                                         }
                                                 }
+                                            catch (const std::bad_function_call &ex)
+                                                {
+                                                    response = "ERROR: command not found \n ";
+                                                }
                                             catch (const std::exception &ex)
                                                 {
                                                     response = "ERROR: command execution error: " + std::string(ex.what()) + "\n";

src/core/receiver/tcp_cmd_interface.h

@@ -31,6 +31,7 @@
 #ifndef GNSS_SDR_TCPCMDINTERFACE_H_
 #define GNSS_SDR_TCPCMDINTERFACE_H_
 
+#include "pvt_interface.h"
 #include <functional>
 #include <iostream>
 #include <string>
@@ -42,7 +43,8 @@
 #include <cstdint>
 #include <gnuradio/message.h>
 #include <gnuradio/msg_queue.h>
-
+#include <armadillo>
+#include <time.h>
 
 class TcpCmdInterface
 {
@@ -51,7 +53,16 @@ public:
     virtual ~TcpCmdInterface();
     void run_cmd_server(int tcp_port);
     void set_msg_queue(gr::msg_queue::sptr control_queue);
+    /*!
+     * \brief gets the UTC time parsed from the last TC command issued
+     */
+    time_t get_utc_time();
+    /*!
+     * \brief gets the Latitude, Longitude and Altitude vector from the last TC command issued
+     */
+    arma::vec get_LLH();
 
+    void set_pvt(std::shared_ptr<PvtInterface> PVT_sptr);
 
 private:
     std::unordered_map<std::string, std::function<std::string(const std::vector<std::string> &)>>
@@ -68,6 +79,14 @@ private:
 
     gr::msg_queue::sptr control_queue_;
     bool keep_running_;
+
+    time_t receiver_utc_time_;
+
+    double rx_latitude_;
+    double rx_longitude_;
+    double rx_altitude_;
+
+    std::shared_ptr<PvtInterface> PVT_sptr_;
 };
 
 #endif /* GNSS_SDR_TCPCMDINTERFACE_H_ */

src/core/system_parameters/galileo_almanac.cc

@@ -34,9 +34,9 @@
 Galileo_Almanac::Galileo_Almanac()
 {
     i_satellite_PRN = 0U;
-    d_Toa = 0.0;
+    i_Toa = 0;
-    d_WNa = 0.0;
+    i_WNa = 0;
-    d_IODa = 0.0;
+    i_IODa = 0;
     d_Delta_i = 0.0;
     d_M_0 = 0.0;
     d_e_eccentricity = 0.0;
@@ -46,7 +46,7 @@ Galileo_Almanac::Galileo_Almanac()
     d_OMEGA_DOT = 0.0;
     d_A_f0 = 0.0;
     d_A_f1 = 0.0;
-    E5b_HS = 0.0;
+    E5b_HS = 0;
-    E1B_HS = 0.0;
+    E1B_HS = 0;
-    E5a_HS = 0.0;
+    E5a_HS = 0;
 }

src/core/system_parameters/galileo_almanac.h

@@ -42,10 +42,10 @@ class Galileo_Almanac
 {
 public:
     uint32_t i_satellite_PRN;  //!< SV PRN NUMBER
-    double d_Toa;
+    int32_t i_Toa;
-    double d_WNa;
+    int32_t i_WNa;
-    double d_IODa;
+    int32_t i_IODa;
-    double d_Delta_i;
+    double d_Delta_i;         //!< Inclination at reference time relative to i0 = 56º [semi-circles]
     double d_M_0;             //!< Mean Anomaly at Reference Time [semi-circles]
     double d_e_eccentricity;  //!< Eccentricity [dimensionless]
     double d_Delta_sqrt_A;    //!< Square Root of the Semi-Major Axis [sqrt(m)]
@@ -54,9 +54,9 @@ public:
     double d_OMEGA_DOT;       //!< Rate of Right Ascension [semi-circles/s]
     double d_A_f0;            //!< Coefficient 0 of code phase offset model [s]
     double d_A_f1;            //!< Coefficient 1 of code phase offset model [s/s]
-    double E5b_HS;
+    int32_t E5b_HS;
-    double E1B_HS;
+    int32_t E1B_HS;
-    double E5a_HS;
+    int32_t E5a_HS;
 
     /*!
      * Default constructor
@@ -71,9 +71,9 @@ public:
             {
             };
         ar& BOOST_SERIALIZATION_NVP(i_satellite_PRN);
-        ar& BOOST_SERIALIZATION_NVP(d_Toa);
+        ar& BOOST_SERIALIZATION_NVP(i_Toa);
-        ar& BOOST_SERIALIZATION_NVP(d_WNa);
+        ar& BOOST_SERIALIZATION_NVP(i_WNa);
-        ar& BOOST_SERIALIZATION_NVP(d_IODa);
+        ar& BOOST_SERIALIZATION_NVP(i_IODa);
         ar& BOOST_SERIALIZATION_NVP(d_Delta_i);
         ar& BOOST_SERIALIZATION_NVP(d_M_0);
         ar& BOOST_SERIALIZATION_NVP(d_e_eccentricity);

src/core/system_parameters/galileo_almanac_helper.cc

@@ -34,8 +34,8 @@ Galileo_Almanac_Helper::Galileo_Almanac_Helper()
 {
     // Word type 7: Almanac for SVID1 (1/2), almanac reference time and almanac reference week number
     IOD_a_7 = 0;
-    WN_a_7 = 0.0;
+    WN_a_7 = 0;
-    t0a_7 = 0.0;
+    t0a_7 = 0;
     SVID1_7 = 0;
     DELTA_A_7 = 0.0;
     e_7 = 0.0;
@@ -49,9 +49,9 @@ Galileo_Almanac_Helper::Galileo_Almanac_Helper()
     IOD_a_8 = 0;
     af0_8 = 0.0;
     af1_8 = 0.0;
-    E5b_HS_8 = 0.0;
+    E5b_HS_8 = 0;
-    E1B_HS_8 = 0.0;
+    E1B_HS_8 = 0;
-    E5a_HS_8 = 0.0;
+    E5a_HS_8 = 0;
     SVID2_8 = 0;
     DELTA_A_8 = 0.0;
     e_8 = 0.0;
@@ -62,14 +62,14 @@ Galileo_Almanac_Helper::Galileo_Almanac_Helper()
 
     // Word type 9: Almanac for SVID2 (2/2) and SVID3 (1/2)
     IOD_a_9 = 0;
-    WN_a_9 = 0.0;
+    WN_a_9 = 0;
-    t0a_9 = 0.0;
+    t0a_9 = 0;
     M0_9 = 0.0;
     af0_9 = 0.0;
     af1_9 = 0.0;
-    E5b_HS_9 = 0.0;
+    E5b_HS_9 = 0;
-    E1B_HS_9 = 0.0;
+    E1B_HS_9 = 0;
-    E5a_HS_9 = 0.0;
+    E5a_HS_9 = 0;
     SVID3_9 = 0;
     DELTA_A_9 = 0.0;
     e_9 = 0.0;
@@ -83,9 +83,9 @@ Galileo_Almanac_Helper::Galileo_Almanac_Helper()
     M0_10 = 0.0;
     af0_10 = 0.0;
     af1_10 = 0.0;
-    E5b_HS_10 = 0.0;
+    E5b_HS_10 = 0;
-    E1B_HS_10 = 0.0;
+    E1B_HS_10 = 0;
-    E5a_HS_10 = 0.0;
+    E5a_HS_10 = 0;
 }
 
 Galileo_Almanac Galileo_Almanac_Helper::get_almanac(int i)
@@ -95,9 +95,9 @@ Galileo_Almanac Galileo_Almanac_Helper::get_almanac(int i)
         {
         case 1:
             galileo_almanac.i_satellite_PRN = this->SVID1_7;
-            galileo_almanac.d_Toa = this->t0a_7;
+            galileo_almanac.i_Toa = this->t0a_7;
-            galileo_almanac.d_WNa = this->WN_a_7;
+            galileo_almanac.i_WNa = this->WN_a_7;
-            galileo_almanac.d_IODa = this->IOD_a_7;
+            galileo_almanac.i_IODa = this->IOD_a_7;
             galileo_almanac.d_Delta_i = this->delta_i_7;
             galileo_almanac.d_M_0 = this->M0_7;
             galileo_almanac.d_e_eccentricity = this->e_7;
@@ -114,9 +114,9 @@ Galileo_Almanac Galileo_Almanac_Helper::get_almanac(int i)
 
         case 2:
             galileo_almanac.i_satellite_PRN = this->SVID2_8;
-            galileo_almanac.d_Toa = this->t0a_9;
+            galileo_almanac.i_Toa = this->t0a_9;
-            galileo_almanac.d_WNa = this->WN_a_9;
+            galileo_almanac.i_WNa = this->WN_a_9;
-            galileo_almanac.d_IODa = this->IOD_a_9;
+            galileo_almanac.i_IODa = this->IOD_a_9;
             galileo_almanac.d_Delta_i = this->delta_i_8;
             galileo_almanac.d_M_0 = this->M0_9;
             galileo_almanac.d_e_eccentricity = this->e_8;
@@ -126,16 +126,15 @@ Galileo_Almanac Galileo_Almanac_Helper::get_almanac(int i)
             galileo_almanac.d_OMEGA_DOT = this->Omega_dot_8;
             galileo_almanac.d_A_f0 = this->af0_9;
             galileo_almanac.d_A_f1 = this->af1_9;
-            galileo_almanac.E5b_HS = this->E5b_HS_9;
             galileo_almanac.E1B_HS = this->E1B_HS_9;
             galileo_almanac.E5a_HS = this->E5a_HS_9;
             break;
 
         case 3:
             galileo_almanac.i_satellite_PRN = this->SVID3_9;
-            galileo_almanac.d_Toa = this->t0a_9;
+            galileo_almanac.i_Toa = this->t0a_9;
-            galileo_almanac.d_WNa = this->WN_a_9;
+            galileo_almanac.i_WNa = this->WN_a_9;
-            galileo_almanac.d_IODa = this->IOD_a_10;
+            galileo_almanac.i_IODa = this->IOD_a_10;
             galileo_almanac.d_Delta_i = this->delta_i_9;
             galileo_almanac.d_M_0 = this->M0_10;
             galileo_almanac.d_e_eccentricity = this->e_9;

src/core/system_parameters/galileo_almanac_helper.h

@@ -44,7 +44,7 @@ class Galileo_Almanac_Helper
 public:
     // Word type 7: Almanac for SVID1 (1/2), almanac reference time and almanac reference week number
     int32_t IOD_a_7;
-    double WN_a_7;
+    int32_t WN_a_7;
     double t0a_7;
     int32_t SVID1_7;
     double DELTA_A_7;
@@ -59,9 +59,9 @@ public:
     int32_t IOD_a_8;
     double af0_8;
     double af1_8;
-    double E5b_HS_8;
+    int32_t E5b_HS_8;
-    double E1B_HS_8;
+    int32_t E1B_HS_8;
-    double E5a_HS_8;
+    int32_t E5a_HS_8;
     int32_t SVID2_8;
     double DELTA_A_8;
     double e_8;
@@ -72,14 +72,14 @@ public:
 
     // Word type 9: Almanac for SVID2 (2/2) and SVID3 (1/2)
     int32_t IOD_a_9;
-    double WN_a_9;
+    int32_t WN_a_9;
     double t0a_9;
     double M0_9;
     double af0_9;
     double af1_9;
-    double E5b_HS_9;
+    int32_t E5b_HS_9;
-    double E1B_HS_9;
+    int32_t E1B_HS_9;
-    double E5a_HS_9;
+    int32_t E5a_HS_9;
     int32_t SVID3_9;
     double DELTA_A_9;
     double e_9;
@@ -93,9 +93,9 @@ public:
     double M0_10;
     double af0_10;
     double af1_10;
-    double E5b_HS_10;
+    int32_t E5b_HS_10;
-    double E1B_HS_10;
+    int32_t E1B_HS_10;
-    double E5a_HS_10;
+    int32_t E5a_HS_10;
 
     Galileo_Almanac_Helper();  //!< Default constructor
     Galileo_Almanac get_almanac(int i);

src/core/system_parameters/galileo_navigation_message.cc

@@ -123,10 +123,10 @@ void Galileo_Navigation_Message::reset()
     Region5_flag_5 = false;
     BGD_E1E5a_5 = 0.0;
     BGD_E1E5b_5 = 0.0;
-    E5b_HS_5 = 0.0;
+    E5b_HS_5 = 0;
-    E1B_HS_5 = 0.0;
+    E1B_HS_5 = 0;
-    E5b_DVS_5 = 0.0;
+    E5b_DVS_5 = 0;
-    E1B_DVS_5 = 0.0;
+    E1B_DVS_5 = 0;
 
     // GST
     WN_5 = 0.0;
@@ -146,7 +146,7 @@ void Galileo_Navigation_Message::reset()
 
     // Word type 7: Almanac for SVID1 (1/2), almanac reference time and almanac reference week number
     IOD_a_7 = 0;
-    WN_a_7 = 0.0;
+    WN_a_7 = 0;
     t0a_7 = 0.0;
     SVID1_7 = 0;
     DELTA_A_7 = 0.0;
@@ -161,8 +161,8 @@ void Galileo_Navigation_Message::reset()
     IOD_a_8 = 0;
     af0_8 = 0.0;
     af1_8 = 0.0;
-    E5b_HS_8 = 0.0;
+    E5b_HS_8 = 0;
-    E1B_HS_8 = 0.0;
+    E1B_HS_8 = 0;
     SVID2_8 = 0;
     DELTA_A_8 = 0.0;
     e_8 = 0.0;
@@ -173,13 +173,13 @@ void Galileo_Navigation_Message::reset()
 
     // Word type 9: Almanac for SVID2 (2/2) and SVID3 (1/2)
     IOD_a_9 = 0;
-    WN_a_9 = 0.0;
+    WN_a_9 = 0;
     t0a_9 = 0.0;
     M0_9 = 0.0;
     af0_9 = 0.0;
     af1_9 = 0.0;
-    E5b_HS_9 = 0.0;
+    E5b_HS_9 = 0;
-    E1B_HS_9 = 0.0;
+    E1B_HS_9 = 0;
     SVID3_9 = 0;
     DELTA_A_9 = 0.0;
     e_9 = 0.0;
@@ -193,8 +193,8 @@ void Galileo_Navigation_Message::reset()
     M0_10 = 0.0;
     af0_10 = 0.0;
     af1_10 = 0.0;
-    E5b_HS_10 = 0.0;
+    E5b_HS_10 = 0;
-    E1B_HS_10 = 0.0;
+    E1B_HS_10 = 0;
 
     // GST-GPS
     A_0G_10 = 0.0;
@@ -781,13 +781,13 @@ int32_t Galileo_Navigation_Message::page_jk_decoder(const char *data_jk)
             BGD_E1E5b_5 = static_cast<double>(read_navigation_signed(data_jk_bits, BGD_E1E5b_5_bit));
             BGD_E1E5b_5 = BGD_E1E5b_5 * BGD_E1E5b_5_LSB;
             DLOG(INFO) << "BGD_E1E5b_5= " << BGD_E1E5b_5;
-            E5b_HS_5 = static_cast<double>(read_navigation_unsigned(data_jk_bits, E5b_HS_5_bit));
+            E5b_HS_5 = static_cast<int32_t>(read_navigation_unsigned(data_jk_bits, E5b_HS_5_bit));
             DLOG(INFO) << "E5b_HS_5= " << E5b_HS_5;
-            E1B_HS_5 = static_cast<double>(read_navigation_unsigned(data_jk_bits, E1B_HS_5_bit));
+            E1B_HS_5 = static_cast<int32_t>(read_navigation_unsigned(data_jk_bits, E1B_HS_5_bit));
             DLOG(INFO) << "E1B_HS_5= " << E1B_HS_5;
-            E5b_DVS_5 = static_cast<double>(read_navigation_unsigned(data_jk_bits, E5b_DVS_5_bit));
+            E5b_DVS_5 = static_cast<int32_t>(read_navigation_unsigned(data_jk_bits, E5b_DVS_5_bit));
             DLOG(INFO) << "E5b_DVS_5= " << E5b_DVS_5;
-            E1B_DVS_5 = static_cast<double>(read_navigation_unsigned(data_jk_bits, E1B_DVS_5_bit));
+            E1B_DVS_5 = static_cast<int32_t>(read_navigation_unsigned(data_jk_bits, E1B_DVS_5_bit));
             DLOG(INFO) << "E1B_DVS_5= " << E1B_DVS_5;
             // GST
             WN_5 = static_cast<double>(read_navigation_unsigned(data_jk_bits, WN_5_bit));
@@ -833,7 +833,7 @@ int32_t Galileo_Navigation_Message::page_jk_decoder(const char *data_jk)
         case 7:  // Word type 7: Almanac for SVID1 (1/2), almanac reference time and almanac reference week number
             IOD_a_7 = static_cast<double>(read_navigation_unsigned(data_jk_bits, IOD_a_7_bit));
             DLOG(INFO) << "IOD_a_7= " << IOD_a_7;
-            WN_a_7 = static_cast<double>(read_navigation_unsigned(data_jk_bits, WN_a_7_bit));
+            WN_a_7 = static_cast<int32_t>(read_navigation_unsigned(data_jk_bits, WN_a_7_bit));
             DLOG(INFO) << "WN_a_7= " << WN_a_7;
             t0a_7 = static_cast<double>(read_navigation_unsigned(data_jk_bits, t0a_7_bit));
             t0a_7 = t0a_7 * t0a_7_LSB;
@@ -874,11 +874,11 @@ int32_t Galileo_Navigation_Message::page_jk_decoder(const char *data_jk)
             af1_8 = static_cast<double>(read_navigation_signed(data_jk_bits, af1_8_bit));
             af1_8 = af1_8 * af1_8_LSB;
             DLOG(INFO) << "af1_8= " << af1_8;
-            E5b_HS_8 = static_cast<double>(read_navigation_unsigned(data_jk_bits, E5b_HS_8_bit));
+            E5b_HS_8 = static_cast<int32_t>(read_navigation_unsigned(data_jk_bits, E5b_HS_8_bit));
             DLOG(INFO) << "E5b_HS_8= " << E5b_HS_8;
-            E1B_HS_8 = static_cast<double>(read_navigation_unsigned(data_jk_bits, E1B_HS_8_bit));
+            E1B_HS_8 = static_cast<int32_t>(read_navigation_unsigned(data_jk_bits, E1B_HS_8_bit));
             DLOG(INFO) << "E1B_HS_8= " << E1B_HS_8;
-            SVID2_8 = static_cast<double>(read_navigation_unsigned(data_jk_bits, SVID2_8_bit));
+            SVID2_8 = static_cast<int32_t>(read_navigation_unsigned(data_jk_bits, SVID2_8_bit));
             DLOG(INFO) << "SVID2_8= " << SVID2_8;
             DELTA_A_8 = static_cast<double>(read_navigation_signed(data_jk_bits, DELTA_A_8_bit));
             DELTA_A_8 = DELTA_A_8 * DELTA_A_8_LSB;
@@ -905,7 +905,7 @@ int32_t Galileo_Navigation_Message::page_jk_decoder(const char *data_jk)
         case 9:  // Word type 9: Almanac for SVID2 (2/2) and SVID3 (1/2)
             IOD_a_9 = static_cast<double>(read_navigation_unsigned(data_jk_bits, IOD_a_9_bit));
             DLOG(INFO) << "IOD_a_9= " << IOD_a_9;
-            WN_a_9 = static_cast<double>(read_navigation_unsigned(data_jk_bits, WN_a_9_bit));
+            WN_a_9 = static_cast<int32_t>(read_navigation_unsigned(data_jk_bits, WN_a_9_bit));
             DLOG(INFO) << "WN_a_9= " << WN_a_9;
             t0a_9 = static_cast<double>(read_navigation_unsigned(data_jk_bits, t0a_9_bit));
             t0a_9 = t0a_9 * t0a_9_LSB;
@@ -919,11 +919,11 @@ int32_t Galileo_Navigation_Message::page_jk_decoder(const char *data_jk)
             af1_9 = static_cast<double>(read_navigation_signed(data_jk_bits, af1_9_bit));
             af1_9 = af1_9 * af1_9_LSB;
             DLOG(INFO) << "af1_9= " << af1_9;
-            E5b_HS_9 = static_cast<double>(read_navigation_unsigned(data_jk_bits, E5b_HS_9_bit));
+            E5b_HS_9 = static_cast<int32_t>(read_navigation_unsigned(data_jk_bits, E5b_HS_9_bit));
             DLOG(INFO) << "E5b_HS_9= " << E5b_HS_9;
-            E1B_HS_9 = static_cast<double>(read_navigation_unsigned(data_jk_bits, E1B_HS_9_bit));
+            E1B_HS_9 = static_cast<int32_t>(read_navigation_unsigned(data_jk_bits, E1B_HS_9_bit));
             DLOG(INFO) << "E1B_HS_9= " << E1B_HS_9;
-            SVID3_9 = static_cast<double>(read_navigation_unsigned(data_jk_bits, SVID3_9_bit));
+            SVID3_9 = static_cast<int32_t>(read_navigation_unsigned(data_jk_bits, SVID3_9_bit));
             DLOG(INFO) << "SVID3_9= " << SVID3_9;
             DELTA_A_9 = static_cast<double>(read_navigation_signed(data_jk_bits, DELTA_A_9_bit));
             DELTA_A_9 = DELTA_A_9 * DELTA_A_9_LSB;
@@ -959,9 +959,9 @@ int32_t Galileo_Navigation_Message::page_jk_decoder(const char *data_jk)
             af1_10 = static_cast<double>(read_navigation_signed(data_jk_bits, af1_10_bit));
             af1_10 = af1_10 * af1_10_LSB;
             DLOG(INFO) << "af1_10= " << af1_10;
-            E5b_HS_10 = static_cast<double>(read_navigation_unsigned(data_jk_bits, E5b_HS_10_bit));
+            E5b_HS_10 = static_cast<int32_t>(read_navigation_unsigned(data_jk_bits, E5b_HS_10_bit));
             DLOG(INFO) << "E5b_HS_10= " << E5b_HS_10;
-            E1B_HS_10 = static_cast<double>(read_navigation_unsigned(data_jk_bits, E1B_HS_10_bit));
+            E1B_HS_10 = static_cast<int32_t>(read_navigation_unsigned(data_jk_bits, E1B_HS_10_bit));
             DLOG(INFO) << "E1B_HS_10= " << E1B_HS_10;
             A_0G_10 = static_cast<double>(read_navigation_signed(data_jk_bits, A_0G_10_bit));
             A_0G_10 = A_0G_10 * A_0G_10_LSB;

src/core/system_parameters/galileo_navigation_message.h

@@ -143,10 +143,10 @@ public:
     double BGD_E1E5a_5;   //!< E1-E5a Broadcast Group Delay [s]
     double BGD_E1E5b_5;   //!< E1-E5b Broadcast Group Delay [s]
 
-    double E5b_HS_5;   //!< E5b Signal Health Status
+    int32_t E5b_HS_5;   //!< E5b Signal Health Status
-    double E1B_HS_5;   //!< E1B Signal Health Status
+    int32_t E1B_HS_5;   //!< E1B Signal Health Status
-    double E5b_DVS_5;  //!< E5b Data Validity Status
+    int32_t E5b_DVS_5;  //!< E5b Data Validity Status
-    double E1B_DVS_5;  //!< E1B Data Validity Status
+    int32_t E1B_DVS_5;  //!< E1B Data Validity Status
 
     // GST
     double WN_5;
@@ -166,7 +166,7 @@ public:
 
     // Word type 7: Almanac for SVID1 (1/2), almanac reference time and almanac reference week number
     int32_t IOD_a_7;
-    double WN_a_7;
+    int32_t WN_a_7;
     double t0a_7;
     int32_t SVID1_7;
     double DELTA_A_7;
@@ -181,8 +181,8 @@ public:
     int32_t IOD_a_8;
     double af0_8;
     double af1_8;
-    double E5b_HS_8;
+    int32_t E5b_HS_8;
-    double E1B_HS_8;
+    int32_t E1B_HS_8;
     int32_t SVID2_8;
     double DELTA_A_8;
     double e_8;
@@ -193,13 +193,13 @@ public:
 
     // Word type 9: Almanac for SVID2 (2/2) and SVID3 (1/2)
     int32_t IOD_a_9;
-    double WN_a_9;
+    int32_t WN_a_9;
     double t0a_9;
     double M0_9;
     double af0_9;
     double af1_9;
-    double E5b_HS_9;
+    int32_t E5b_HS_9;
-    double E1B_HS_9;
+    int32_t E1B_HS_9;
     int32_t SVID3_9;
     double DELTA_A_9;
     double e_9;
@@ -213,8 +213,8 @@ public:
     double M0_10;
     double af0_10;
     double af1_10;
-    double E5b_HS_10;
+    int32_t E5b_HS_10;
-    double E1B_HS_10;
+    int32_t E1B_HS_10;
 
     // GST-GPS conversion
     double A_0G_10;   //!< Constant term of the offset Delta t systems

src/core/system_parameters/gps_almanac.cc

@@ -36,7 +36,7 @@ Gps_Almanac::Gps_Almanac()
 {
     i_satellite_PRN = 0U;
     d_Delta_i = 0.0;
-    d_Toa = 0.0;
+    i_Toa = 0.0;
     i_WNa = 0;
     d_M_0 = 0.0;
     d_e_eccentricity = 0.0;

src/core/system_parameters/gps_almanac.h

@@ -45,7 +45,7 @@ class Gps_Almanac
 public:
     uint32_t i_satellite_PRN;  //!< SV PRN NUMBER
     double d_Delta_i;          //!< Inclination Angle at Reference Time (relative to i_0 = 0.30 semi-circles)
-    double d_Toa;              //!< Almanac data reference time of week (Ref. 20.3.3.4.3 IS-GPS-200E) [s]
+    int32_t i_Toa;             //!< Almanac data reference time of week (Ref. 20.3.3.4.3 IS-GPS-200E) [s]
     int32_t i_WNa;             //!< Almanac week number
     double d_M_0;              //!< Mean Anomaly at Reference Time [semi-circles]
     double d_e_eccentricity;   //!< Eccentricity [dimensionless]
@@ -72,7 +72,7 @@ public:
             };
         ar& BOOST_SERIALIZATION_NVP(i_satellite_PRN);
         ar& BOOST_SERIALIZATION_NVP(d_Delta_i);
-        ar& BOOST_SERIALIZATION_NVP(d_Toa);
+        ar& BOOST_SERIALIZATION_NVP(i_Toa);
         ar& BOOST_SERIALIZATION_NVP(i_WNa);
         ar& BOOST_SERIALIZATION_NVP(d_M_0);
         ar& BOOST_SERIALIZATION_NVP(d_e_eccentricity);

src/core/system_parameters/gps_navigation_message.cc

@@ -125,7 +125,7 @@ void Gps_Navigation_Message::reset()
     d_DeltaT_LSF = 0.0;
 
     // Almanac
-    d_Toa = 0.0;
+    i_Toa = 0;
     i_WN_A = 0;
     for (int32_t i = 1; i < 32; i++)
         {
@@ -433,8 +433,8 @@ int32_t Gps_Navigation_Message::subframe_decoder(char *subframe)
                 }
             if (SV_page_5 == 51)  // Page 25 (from Table 20-V. Data IDs and SV IDs in Subframes 4 and 5, IS-GPS-200H, page 110)
                 {
-                    d_Toa = static_cast<double>(read_navigation_unsigned(subframe_bits, T_OA));
+                    i_Toa = static_cast<int32_t>(read_navigation_unsigned(subframe_bits, T_OA));
-                    d_Toa = d_Toa * T_OA_LSB;
+                    i_Toa = i_Toa * T_OA_LSB;
                     i_WN_A = static_cast<int32_t>(read_navigation_unsigned(subframe_bits, WN_A));
                     almanacHealth[1] = static_cast<int32_t>(read_navigation_unsigned(subframe_bits, HEALTH_SV1));
                     almanacHealth[2] = static_cast<int32_t>(read_navigation_unsigned(subframe_bits, HEALTH_SV2));

src/core/system_parameters/gps_navigation_message.h

@@ -109,8 +109,8 @@ public:
     double d_A_f2;  //!< Coefficient 2 of code phase offset model [s/s^2]
 
     // Almanac
-    double d_Toa;                              //!< Almanac reference time [s]
+    int32_t i_Toa;                             //!< Almanac reference time [s]
-    int32_t i_WN_A;                            //!< Modulo 256 of the GPS week number to which the almanac reference time (d_Toa) is referenced
+    int32_t i_WN_A;                            //!< Modulo 256 of the GPS week number to which the almanac reference time (i_Toa) is referenced
     std::map<int32_t, int32_t> almanacHealth;  //!< Map that stores the health information stored in the almanac
 
     std::map<int32_t, std::string> satelliteBlock;  //!< Map that stores to which block the PRN belongs http://www.navcen.uscg.gov/?Do=constellationStatus

src/main/CMakeLists.txt

@@ -50,6 +50,7 @@ include_directories(
      ${CMAKE_SOURCE_DIR}/src/core/system_parameters
      ${CMAKE_SOURCE_DIR}/src/core/interfaces
      ${CMAKE_SOURCE_DIR}/src/core/receiver
+     ${CMAKE_SOURCE_DIR}/src/core/monitor
      ${CMAKE_SOURCE_DIR}/src/core/libs
      ${CMAKE_SOURCE_DIR}/src/core/libs/supl
      ${CMAKE_SOURCE_DIR}/src/core/libs/supl/asn-rrlp
@@ -60,6 +61,7 @@ include_directories(
      ${ARMADILLO_INCLUDE_DIRS}
      ${Boost_INCLUDE_DIRS}
      ${GNURADIO_RUNTIME_INCLUDE_DIRS}
+     ${PUGIXML_INCLUDE_DIR}
      ${GNSS_SDR_OPTIONAL_HEADERS}
      ${VOLK_GNSSSDR_INCLUDE_DIRS}
      ${OPT_GNSSSDR_INCLUDE_DIRS}

src/tests/CMakeLists.txt

@@ -376,6 +376,7 @@ set(LIST_INCLUDE_DIRS
      ${VOLK_INCLUDE_DIRS}
      ${VOLK_GNSSSDR_INCLUDE_DIRS}
      ${MATIO_INCLUDE_DIRS}
+     ${PUGIXML_INCLUDE_DIR}
      ${GNSS_SDR_TEST_OPTIONAL_HEADERS}
 )
 

src/tests/common-files/tracking_tests_flags.h

@@ -67,6 +67,9 @@ DEFINE_double(acq_Delay_error_chips_start, 2.0, "Acquisition Code Delay error st
 DEFINE_double(acq_Delay_error_chips_stop, -2.0, "Acquisition Code Delay error stop sweep value [Chips]");
 DEFINE_double(acq_Delay_error_chips_step, -0.1, "Acquisition Code Delay error sweep step value [Chips]");
 
+DEFINE_double(acq_to_trk_delay_s, 0.0, "Acquisition to Tracking delay value [s]");
+
+
 DEFINE_int64(skip_samples, 0, "Skip an initial transitory in the processed signal file capture [samples]");
 
 DEFINE_int32(plot_detail_level, 0, "Specify the desired plot detail (0,1,2): 0 - Minimum plots (default) 2 - Plot all tracking parameters");

src/tests/system-tests/libs/CMakeLists.txt

@@ -18,7 +18,6 @@
 
 
 set(SYSTEM_TESTING_LIB_SOURCES
-    geofunctions.cc
     spirent_motion_csv_dump_reader.cc
     rtklib_solver_dump_reader.cc
 )

src/tests/system-tests/libs/position_test_flags.h

@@ -37,7 +37,6 @@
 DEFINE_string(config_file_ptest, std::string(""), "File containing the configuration parameters for the position test.");
 DEFINE_bool(plot_position_test, false, "Plots results of with gnuplot");
 DEFINE_bool(static_scenario, true, "Compute figures of merit for static user position (DRMS, CEP, etc..)");
-DEFINE_bool(use_pvt_solver_dump, false, "Use PVT solver binary dump or fall back to KML PVT file (contains only position information)");
 DEFINE_bool(use_ref_motion_file, false, "Enable or disable the use of a reference file containing the true receiver position, velocity and acceleration.");
 DEFINE_int32(ref_motion_file_type, 1, "Type of reference motion file: 1- Spirent CSV motion file");
 DEFINE_string(ref_motion_filename, std::string("motion.csv"), "Path and filename for the reference motion file");

src/tests/system-tests/position_test.cc

@@ -455,107 +455,42 @@ void PositionSystemTest::check_results()
     ref_R_eb_e.insert_cols(0, true_r_eb_e);
     arma::vec ref_r_enu = {0, 0, 0};
     cart2utm(true_r_eb_e, utm_zone, ref_r_enu);
-    if (!FLAGS_use_pvt_solver_dump)
+
+    rtklib_solver_dump_reader pvt_reader;
+    pvt_reader.open_obs_file(FLAGS_pvt_solver_dump_filename);
+    int64_t n_epochs = pvt_reader.num_epochs();
+    R_eb_e = arma::zeros(3, n_epochs);
+    V_eb_e = arma::zeros(3, n_epochs);
+    LLH = arma::zeros(3, n_epochs);
+    receiver_time_s = arma::zeros(n_epochs, 1);
+    int64_t current_epoch = 0;
+    while (pvt_reader.read_binary_obs())
         {
-            //fall back to read receiver KML output (position only)
+            receiver_time_s(current_epoch) = pvt_reader.RX_time - pvt_reader.clk_offset_s;
-            std::fstream myfile(PositionSystemTest::generated_kml_file, std::ios_base::in);
+            R_eb_e(0, current_epoch) = pvt_reader.rr[0];
-            ASSERT_TRUE(myfile.is_open()) << "No valid kml file could be opened";
+            R_eb_e(1, current_epoch) = pvt_reader.rr[1];
-            std::string line;
+            R_eb_e(2, current_epoch) = pvt_reader.rr[2];
-            // Skip header
+            V_eb_e(0, current_epoch) = pvt_reader.rr[3];
-            std::getline(myfile, line);
+            V_eb_e(1, current_epoch) = pvt_reader.rr[4];
-            bool is_header = true;
+            V_eb_e(2, current_epoch) = pvt_reader.rr[5];
-            while (is_header)
+            LLH(0, current_epoch) = pvt_reader.latitude;
-                {
+            LLH(1, current_epoch) = pvt_reader.longitude;
-                    std::getline(myfile, line);
+            LLH(2, current_epoch) = pvt_reader.height;
-                    ASSERT_FALSE(myfile.eof()) << "No valid kml file found.";
-                    std::size_t found = line.find("<coordinates>");
-                    if (found != std::string::npos) is_header = false;
-                }
-            bool is_data = true;
-            //read data
-            int64_t current_epoch = 0;
-            while (is_data)
-                {
-                    if (!std::getline(myfile, line))
-                        {
-                            is_data = false;
-                            break;
-                        }
-                    std::size_t found = line.find("</coordinates>");
-                    if (found != std::string::npos)
-                        is_data = false;
-                    else
-                        {
-                            std::string str2;
-                            std::istringstream iss(line);
-                            double value;
-                            double lat = 0.0;
-                            double longitude = 0.0;
-                            double h = 0.0;
-                            for (int i = 0; i < 3; i++)
-                                {
-                                    std::getline(iss, str2, ',');
-                                    value = std::stod(str2);
-                                    if (i == 0) longitude = value;
-                                    if (i == 1) lat = value;
-                                    if (i == 2) h = value;
-                                }
 
-                            arma::vec tmp_v_ecef;
+            arma::vec tmp_r_enu = {0, 0, 0};
-                            arma::vec tmp_r_ecef;
+            cart2utm(R_eb_e.col(current_epoch), utm_zone, tmp_r_enu);
-                            pv_Geo_to_ECEF(degtorad(lat), degtorad(longitude), h, arma::vec{0, 0, 0}, tmp_r_ecef, tmp_v_ecef);
+            R_eb_enu.insert_cols(current_epoch, tmp_r_enu);
-                            R_eb_e.insert_cols(current_epoch, tmp_r_ecef);
+
-                            arma::vec tmp_r_enu = {0, 0, 0};
+            // debug check
-                            cart2utm(tmp_r_ecef, utm_zone, tmp_r_enu);
+            // std::cout << "t1: " << pvt_reader.RX_time << std::endl;
-                            R_eb_enu.insert_cols(current_epoch, tmp_r_enu);
+            // std::cout << "t2: " << pvt_reader.TOW_at_current_symbol_ms << std::endl;
-                            //                            std::cout << "lat = " << lat << ", longitude = " << longitude << "  h = " << h << std::endl;
+            // std::cout << "offset: " << pvt_reader.clk_offset_s << std::endl;
-                            //                            std::cout << "E = " << east << ", N = " << north << " U = " << up << std::endl;
+            // getchar();
-                            //                            getchar();
+            current_epoch++;
-                        }
-                }
-            myfile.close();
-            ASSERT_FALSE(R_eb_e.n_cols == 0) << "KML file is empty";
-        }
-    else
-        {
-            //use complete binary dump from pvt solver
-            rtklib_solver_dump_reader pvt_reader;
-            pvt_reader.open_obs_file(FLAGS_pvt_solver_dump_filename);
-            int64_t n_epochs = pvt_reader.num_epochs();
-            R_eb_e = arma::zeros(3, n_epochs);
-            V_eb_e = arma::zeros(3, n_epochs);
-            LLH = arma::zeros(3, n_epochs);
-            receiver_time_s = arma::zeros(n_epochs, 1);
-            int64_t current_epoch = 0;
-            while (pvt_reader.read_binary_obs())
-                {
-                    receiver_time_s(current_epoch) = pvt_reader.RX_time - pvt_reader.clk_offset_s;
-                    R_eb_e(0, current_epoch) = pvt_reader.rr[0];
-                    R_eb_e(1, current_epoch) = pvt_reader.rr[1];
-                    R_eb_e(2, current_epoch) = pvt_reader.rr[2];
-                    V_eb_e(0, current_epoch) = pvt_reader.rr[3];
-                    V_eb_e(1, current_epoch) = pvt_reader.rr[4];
-                    V_eb_e(2, current_epoch) = pvt_reader.rr[5];
-                    LLH(0, current_epoch) = pvt_reader.latitude;
-                    LLH(1, current_epoch) = pvt_reader.longitude;
-                    LLH(2, current_epoch) = pvt_reader.height;
-
-                    arma::vec tmp_r_enu = {0, 0, 0};
-                    cart2utm(R_eb_e.col(current_epoch), utm_zone, tmp_r_enu);
-                    R_eb_enu.insert_cols(current_epoch, tmp_r_enu);
-
-                    //debug check
-                    //                    std::cout << "t1: " << pvt_reader.RX_time << std::endl;
-                    //                    std::cout << "t2: " << pvt_reader.TOW_at_current_symbol_ms << std::endl;
-                    //                    std::cout << "offset: " << pvt_reader.clk_offset_s << std::endl;
-                    //                    getchar();
-                    current_epoch++;
-                }
-            ASSERT_FALSE(current_epoch == 0) << "PVT dump is empty";
         }
+    ASSERT_FALSE(current_epoch == 0) << "PVT dump is empty";
 
     // compute results
-
     if (FLAGS_static_scenario)
         {
             double sigma_E_2_precision = arma::var(R_eb_enu.row(0));

src/tests/unit-tests/signal-processing-blocks/pvt/nmea_printer_test.cc

@@ -160,13 +160,20 @@ TEST_F(NmeaPrinterTest, PrintLine)
     std::shared_ptr<rtklib_solver> pvt_solution = std::make_shared<rtklib_solver>(12, "filename", false, false, rtk);
 
     boost::posix_time::ptime pt(boost::gregorian::date(1994, boost::date_time::Nov, 19),
-        boost::posix_time::hours(22) + boost::posix_time::minutes(54) + boost::posix_time::seconds(46));  // example from http://aprs.gids.nl/nmea/#rmc
+        boost::posix_time::hours(22) + boost::posix_time::minutes(54) + boost::posix_time::seconds(46));
-    pvt_solution->set_position_UTC_time(pt);
+    std::time_t tim = (pt - boost::posix_time::ptime(boost::gregorian::date(1970, 1, 1))).total_seconds();
+    gtime_t gtime;
+    gtime.time = tim;
+    gtime.sec = 0.0;
 
-    arma::vec pos = {49.27416667, -123.18533333, 0};
+    pvt_solution->pvt_sol.rr[0] = -2282104.0;  //49.27416667;
-    pvt_solution->set_rx_pos(pos);
+    pvt_solution->pvt_sol.rr[1] = -3489369.0;  //-123.18533333;
-
+    pvt_solution->pvt_sol.rr[2] = 4810507.0;   // 0
-    pvt_solution->set_valid_position(true);
+    pvt_solution->pvt_sol.rr[3] = 0.0;
+    pvt_solution->pvt_sol.rr[4] = 0.0;
+    pvt_solution->pvt_sol.rr[5] = 0.0;
+    pvt_solution->pvt_sol.stat = 1;  // SOLQ_FIX
+    pvt_solution->pvt_sol.time = gtime;
 
     bool flag_nmea_output_file = true;
     ASSERT_NO_THROW({
@@ -184,46 +191,7 @@ TEST_F(NmeaPrinterTest, PrintLine)
                     std::size_t found = line.find(GPRMC);
                     if (found != std::string::npos)
                         {
-                            EXPECT_EQ(line, "$GPRMC,225446.000,A,4916.4500,N,12311.1199,W,0.00,0.00,191194,,*1c\r");
+                            EXPECT_EQ(line, "$GPRMC,225436.00,A,4916.4497617,N,12311.1202744,W,0.00,0.00,191194,0.0,E,D*21\r");
-                        }
-                }
-            test_file.close();
-        }
-    EXPECT_EQ(0, remove(filename.c_str())) << "Failure deleting a temporary file.";
-}
-
-
-TEST_F(NmeaPrinterTest, PrintLineLessthan10min)
-{
-    std::string filename("nmea_test.nmea");
-    std::shared_ptr<rtklib_solver> pvt_solution = std::make_shared<rtklib_solver>(12, "filename", false, false, rtk);
-
-    boost::posix_time::ptime pt(boost::gregorian::date(1994, boost::date_time::Nov, 19),
-        boost::posix_time::hours(22) + boost::posix_time::minutes(54) + boost::posix_time::seconds(46));  // example from http://aprs.gids.nl/nmea/#rmc
-    pvt_solution->set_position_UTC_time(pt);
-
-    arma::vec pos = {49.07416667, -123.02527778, 0};
-    pvt_solution->set_rx_pos(pos);
-
-    pvt_solution->set_valid_position(true);
-
-    bool flag_nmea_output_file = true;
-    ASSERT_NO_THROW({
-        std::shared_ptr<Nmea_Printer> nmea_printer = std::make_shared<Nmea_Printer>(filename, flag_nmea_output_file, false, "");
-        nmea_printer->Print_Nmea_Line(pvt_solution, false);
-    }) << "Failure printing NMEA messages.";
-
-    std::ifstream test_file(filename);
-    std::string line;
-    std::string GPRMC("$GPRMC");
-    if (test_file.is_open())
-        {
-            while (getline(test_file, line))
-                {
-                    std::size_t found = line.find(GPRMC);
-                    if (found != std::string::npos)
-                        {
-                            EXPECT_EQ(line, "$GPRMC,225446.000,A,4904.4500,N,12301.5166,W,0.00,0.00,191194,,*1a\r");
                         }
                 }
             test_file.close();

src/tests/unit-tests/signal-processing-blocks/tracking/tracking_pull-in_test.cc

@@ -33,7 +33,9 @@
 
 #include "GPS_L1_CA.h"
 #include "gnss_block_factory.h"
+#include "control_message_factory.h"
 #include "tracking_interface.h"
+#include "gnss_sdr_valve.h"
 #include "gps_l2_m_pcps_acquisition.h"
 #include "gps_l1_ca_pcps_acquisition.h"
 #include "gps_l1_ca_pcps_acquisition_fine_doppler.h"
@@ -56,6 +58,7 @@
 #include <gnuradio/blocks/null_sink.h>
 #include <gnuradio/blocks/skiphead.h>
 #include <gnuradio/blocks/head.h>
+#include <gnuradio/msg_queue.h>
 #include <gtest/gtest.h>
 #include <chrono>
 #include <cstdint>
@@ -186,6 +189,8 @@ public:
     std::shared_ptr<InMemoryConfiguration> config;
     Gnss_Synchro gnss_synchro;
     size_t item_size;
+
+    gr::msg_queue::sptr queue;
 };
 
 
@@ -658,6 +663,18 @@ TEST_F(TrackingPullInTest, ValidationOfResults)
                       << " [Hz], estimated Initial code delay " << true_acq_delay_samples << " [Samples]"
                       << " Acquisition SampleStamp is " << acq_samplestamp_map.find(FLAGS_test_satellite_PRN)->second << std::endl;
         }
+
+    // create the msg queue for valve
+
+    queue = gr::msg_queue::make(0);
+    boost::shared_ptr<gnss_sdr_valve> reseteable_valve;
+    long long int acq_to_trk_delay_samples = ceil(static_cast<double>(FLAGS_fs_gen_sps) * FLAGS_acq_to_trk_delay_s);
+    boost::shared_ptr<gnss_sdr_valve> resetable_valve_(new gnss_sdr_valve(sizeof(gr_complex), acq_to_trk_delay_samples, queue, false));
+
+    std::shared_ptr<ControlMessageFactory> control_message_factory_;
+    std::shared_ptr<std::vector<std::shared_ptr<ControlMessage>>> control_messages_;
+
+
     //CN0 LOOP
     std::vector<std::vector<double>> pull_in_results_v_v;
 
@@ -710,7 +727,15 @@ TEST_F(TrackingPullInTest, ValidationOfResults)
                                 gr::blocks::head::sptr head_samples = gr::blocks::head::make(sizeof(gr_complex), baseband_sampling_freq * FLAGS_duration);
                                 top_block->connect(file_source, 0, gr_interleaved_char_to_complex, 0);
                                 top_block->connect(gr_interleaved_char_to_complex, 0, head_samples, 0);
-                                top_block->connect(head_samples, 0, tracking->get_left_block(), 0);
+                                if (acq_to_trk_delay_samples > 0)
+                                    {
+                                        top_block->connect(head_samples, 0, resetable_valve_, 0);
+                                        top_block->connect(resetable_valve_, 0, tracking->get_left_block(), 0);
+                                    }
+                                else
+                                    {
+                                        top_block->connect(head_samples, 0, tracking->get_left_block(), 0);
+                                    }
                                 top_block->connect(tracking->get_right_block(), 0, sink, 0);
                                 top_block->msg_connect(tracking->get_right_block(), pmt::mp("events"), msg_rx, pmt::mp("events"));
                                 file_source->seek(2 * FLAGS_skip_samples, 0);  //skip head. ibyte, two bytes per complex sample
@@ -721,18 +746,46 @@ TEST_F(TrackingPullInTest, ValidationOfResults)
                             //***** STEP 5: Perform the signal tracking and read the results *****
                             //********************************************************************
                             std::cout << "--- START TRACKING WITH PULL-IN ERROR: " << acq_doppler_error_hz_values.at(current_acq_doppler_error_idx) << " [Hz] and " << acq_delay_error_chips_values.at(current_acq_doppler_error_idx).at(current_acq_code_error_idx) << " [Chips] ---" << std::endl;
-                            tracking->start_tracking();
                             std::chrono::time_point<std::chrono::system_clock> start, end;
-                            EXPECT_NO_THROW({
+                            if (acq_to_trk_delay_samples > 0)
-                                start = std::chrono::system_clock::now();
+                                {
-                                top_block->run();  // Start threads and wait
+                                    EXPECT_NO_THROW({
-                                end = std::chrono::system_clock::now();
+                                        start = std::chrono::system_clock::now();
-                            }) << "Failure running the top_block.";
+                                        std::cout << "--- SIMULATING A PULL-IN DELAY OF " << FLAGS_acq_to_trk_delay_s << " SECONDS ---\n";
+                                        top_block->start();
+                                        std::cout << " Waiting for valve...\n";
+                                        //wait the valve message indicating the circulation of the amount of samples of the delay
+                                        gr::message::sptr queue_message = queue->delete_head();
+                                        if (queue_message != 0)
+                                            {
+                                                control_messages_ = control_message_factory_->GetControlMessages(queue_message);
+                                            }
+                                        else
+                                            {
+                                                control_messages_->clear();
+                                            }
+                                        std::cout << " Starting tracking...\n";
+                                        tracking->start_tracking();
+                                        resetable_valve_->open_valve();
+                                        std::cout << " Waiting flowgraph..\n";
+                                        top_block->wait();
+                                        end = std::chrono::system_clock::now();
+                                    }) << "Failure running the top_block.";
+                                }
+                            else
+                                {
+                                    tracking->start_tracking();
+                                    std::chrono::time_point<std::chrono::system_clock> start, end;
+                                    EXPECT_NO_THROW({
+                                        start = std::chrono::system_clock::now();
+                                        top_block->run();  // Start threads and wait
+                                        end = std::chrono::system_clock::now();
+                                    }) << "Failure running the top_block.";
+                                }
 
                             std::chrono::duration<double> elapsed_seconds = end - start;
                             std::cout << "Signal tracking completed in " << elapsed_seconds.count() << " seconds" << std::endl;
 
-
                             pull_in_results_v.push_back(msg_rx->rx_message != 3);  //save last asynchronous tracking message in order to detect a loss of lock
 
                             //********************************

src/utils/front-end-cal/CMakeLists.txt

@@ -43,6 +43,7 @@ include_directories(
     ${GNURADIO_BLOCKS_INCLUDE_DIRS}
     ${ARMADILLO_INCLUDE_DIRS}
     ${Boost_INCLUDE_DIRS}
+    ${PUGIXML_INCLUDE_DIR}
     ${VOLK_GNSSSDR_INCLUDE_DIRS}
 )