mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-12-15 12:40:35 +00:00
Merge branch 'next' of https://github.com/gnss-sdr/gnss-sdr into fpga
This commit is contained in:
Marc Majoral
commit
3e46f658f6
@ -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
|
||||
|
||||
17
README.md
17
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
|
||||
$ tar xvfz v2.2.1.tar.gz
|
||||
$ cd gflags-2.2.1
|
||||
$ wget https://github.com/gflags/gflags/archive/v2.2.2.tar.gz
|
||||
$ tar xvfz v2.2.2.tar.gz
|
||||
$ 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
|
||||
~~~~~~
|
||||
|
||||
69
cmake/Modules/FindPugiXML.cmake
Normal file
69
cmake/Modules/FindPugiXML.cmake
Normal file
@ -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)
|
||||
@ -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:float" name="d_WNa"/>
|
||||
<xs:element type="xs:float" name="d_IODa"/>
|
||||
<xs:element type="xs:byte" name="i_Toa"/>
|
||||
<xs:element type="xs:byte" name="i_WNa"/>
|
||||
<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:float" name="E1B_HS"/>
|
||||
<xs:element type="xs:float" name="E5a_HS"/>
|
||||
<xs:element type="xs:byte" name="E5b_HS"/>
|
||||
<xs:element type="xs:byte" name="E1B_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"/>
|
||||
|
||||
@ -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"/>
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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;
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@ -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
|
||||
|
||||
|
||||
@ -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
|
||||
<< "xmlns=\"http://www.topografix.com/GPX/1/1\"" << std::endl
|
||||
<< "xmlns:xsi=\"http://www.w3.org/2001/XMLSchema-instance\">" << std::endl
|
||||
<< "<trk>" << std::endl
|
||||
<< indent << "<name>Position fixes computed by GNSS-SDR v" << GNSS_SDR_VERSION << "</name>" << std::endl
|
||||
<< indent << "<desc>GNSS-SDR position log generated at " << pt << " (local time)</desc>" << std::endl
|
||||
<< indent << "<trkseg>" << 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
|
||||
<< indent << "xmlns=\"http://www.topografix.com/GPX/1/1\"" << std::endl
|
||||
<< indent << "xmlns:gpxx=\"http://www.garmin.com/xmlschemas/GpxExtensions/v3\"" << std::endl
|
||||
<< indent << "xmlns:gpxtpx=\"http://www.garmin.com/xmlschemas/TrackPointExtension/v2\"" << std::endl
|
||||
<< indent << "xmlns:xsi=\"http://www.w3.org/2001/XMLSchema-instance\">" << std::endl
|
||||
<< indent << "<trk>" << 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
|
||||
<< "</trk>" << std::endl
|
||||
gpx_file << indent << indent << "</trkseg>" << std::endl
|
||||
<< indent << "</trk>" << std::endl
|
||||
<< "</gpx>";
|
||||
gpx_file.close();
|
||||
return true;
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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
|
||||
<< " <Document>" << std::endl
|
||||
<< " <name>GNSS Track</name>" << std::endl
|
||||
<< " <description>GNSS-SDR Receiver position log file created at " << pt
|
||||
<< " </description>" << std::endl
|
||||
<< "<Style id=\"yellowLineGreenPoly\">" << std::endl
|
||||
<< " <LineStyle>" << std::endl
|
||||
<< " <color>7f00ffff</color>" << std::endl
|
||||
<< " <width>1</width>" << std::endl
|
||||
<< " </LineStyle>" << std::endl
|
||||
<< "<PolyStyle>" << std::endl
|
||||
<< " <color>7f00ff00</color>" << std::endl
|
||||
<< "</PolyStyle>" << std::endl
|
||||
<< "</Style>" << std::endl
|
||||
<< "<Placemark>" << std::endl
|
||||
<< "<name>GNSS-SDR PVT</name>" << std::endl
|
||||
<< "<description>GNSS-SDR position log</description>" << std::endl
|
||||
<< "<styleUrl>#yellowLineGreenPoly</styleUrl>" << std::endl
|
||||
<< "<LineString>" << std::endl
|
||||
<< "<extrude>0</extrude>" << std::endl
|
||||
<< "<tessellate>1</tessellate>" << std::endl
|
||||
<< "<altitudeMode>absolute</altitudeMode>" << std::endl
|
||||
<< "<coordinates>" << std::endl;
|
||||
<< "<kml xmlns=\"http://www.opengis.net/kml/2.2\" xmlns:gx=\"http://www.google.com/kml/ext/2.2\">" << std::endl
|
||||
<< indent << "<Document>" << std::endl
|
||||
<< indent << indent << "<name>GNSS Track</name>" << std::endl
|
||||
<< indent << indent << "<description><![CDATA[" << std::endl
|
||||
<< indent << indent << indent << "<table>" << std::endl
|
||||
<< indent << indent << indent << indent << "<tr><td>GNSS-SDR Receiver position log file created at " << pt << "</td></tr>" << std::endl
|
||||
<< indent << indent << indent << indent << "<tr><td>https://gnss-sdr.org/</td></tr>" << std::endl
|
||||
<< indent << indent << indent << "</table>" << std::endl
|
||||
<< indent << indent << "]]></description>" << std::endl
|
||||
<< indent << indent << "<!-- Normal track style -->" << std::endl
|
||||
<< indent << indent << "<Style id=\"track_n\">" << std::endl
|
||||
<< indent << indent << indent << "<IconStyle>" << std::endl
|
||||
<< indent << indent << indent << indent << "<color>ff00ffff</color>" << std::endl
|
||||
<< indent << indent << indent << indent << "<scale>0.3</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 << indent << "<LabelStyle>" << std::endl
|
||||
<< indent << indent << indent << indent << "<scale>0</scale>" << std::endl
|
||||
<< indent << indent << indent << "</LabelStyle>" << std::endl
|
||||
<< indent << indent << "</Style>" << 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
|
||||
<< "</LineString>" << std::endl
|
||||
<< "</Placemark>" << std::endl
|
||||
<< "</Document>" << std::endl
|
||||
tmp_file.close();
|
||||
|
||||
kml_file << indent << indent << "</Folder>"
|
||||
<< 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
|
||||
|
||||
@ -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();
|
||||
};
|
||||
|
||||
|
||||
@ -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));
|
||||
this->set_visible_satellites_Az(i, *azim);
|
||||
this->set_visible_satellites_El(i, *elev);
|
||||
this->set_visible_satellites_Distance(i, *dist);
|
||||
topocent(azim, elev, dist, pos.subvec(0, 2), Rot_X - pos.subvec(0, 2));
|
||||
|
||||
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)
|
||||
{
|
||||
|
||||
@ -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;
|
||||
|
||||
// GPRMC (RMC-Recommended,Minimum Specific GNSS Data)
|
||||
std::string sentence_header;
|
||||
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";
|
||||
unsigned char buff[1024] = {0};
|
||||
outnmea_rmc(buff, &d_PVT_data->pvt_sol);
|
||||
sentence_str << buff;
|
||||
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;
|
||||
sentence_header = "$GPGSA,";
|
||||
sentence_str << sentence_header;
|
||||
|
||||
// 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";
|
||||
unsigned char buff[1024] = {0};
|
||||
outnmea_gsa(buff, &d_PVT_data->pvt_sol, d_PVT_data->pvt_ssat);
|
||||
sentence_str << buff;
|
||||
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;
|
||||
|
||||
// GPGGA (Global Positioning System Fixed Data)
|
||||
std::string sentence_header;
|
||||
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";
|
||||
unsigned char buff[1024] = {0};
|
||||
outnmea_gga(buff, &d_PVT_data->pvt_sol);
|
||||
sentence_str << buff;
|
||||
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
|
||||
}
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
|
||||
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;
|
||||
//todo: refactor this class. Mix of duplicated functions, use either RTKLIB geodetic functions or geofunctions.h
|
||||
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];
|
||||
}
|
||||
}
|
||||
|
||||
@ -52,6 +52,8 @@ private:
|
||||
double d_latitude_d; // RX position Latitude WGS84 [deg]
|
||||
double d_longitude_d; // RX position Longitude WGS84 [deg]
|
||||
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
|
||||
*
|
||||
|
||||
@ -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();
|
||||
glonass_gnav_ephemeris_iter != eph_map.end();
|
||||
for (glonass_gnav_ephemeris_iter = eph_map.cbegin();
|
||||
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();
|
||||
observables_iter != observables.end();
|
||||
for (observables_iter = observables.cbegin();
|
||||
observables_iter != observables.cend();
|
||||
observables_iter++)
|
||||
{
|
||||
numSatellitesObserved++;
|
||||
}
|
||||
line += Rinex_Printer::rightJustify(boost::lexical_cast<std::string>(numSatellitesObserved), 3);
|
||||
for (observables_iter = observables.begin();
|
||||
observables_iter != observables.end();
|
||||
for (observables_iter = observables.cbegin();
|
||||
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();
|
||||
observables_iter != observables.end();
|
||||
for (observables_iter = observables.cbegin();
|
||||
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();
|
||||
observables_iter != observables.end();
|
||||
for (observables_iter = observables.cbegin();
|
||||
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();
|
||||
observables_iter != observables.end();
|
||||
for (observables_iter = observables.cbegin();
|
||||
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();
|
||||
observables_iter != observables.end();
|
||||
for (observables_iter = observables.cbegin();
|
||||
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();
|
||||
observables_iter != observablesR1C.end();
|
||||
for (observables_iter = observablesR1C.cbegin();
|
||||
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();
|
||||
observables_iter != observablesR2C.end();
|
||||
for (observables_iter = observablesR2C.cbegin();
|
||||
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();
|
||||
observables_iter != observablesG1C.end();
|
||||
for (observables_iter = observablesG1C.cbegin();
|
||||
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();
|
||||
observables_iter != observablesR1C.end();
|
||||
for (observables_iter = observablesR1C.cbegin();
|
||||
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();
|
||||
observables_iter != observablesR2C.end();
|
||||
for (observables_iter = observablesR2C.cbegin();
|
||||
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();
|
||||
observables_iter != observablesG1C.end();
|
||||
for (observables_iter = observablesG1C.cbegin();
|
||||
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();
|
||||
observables_iter != observables.end();
|
||||
for (observables_iter = observables.cbegin();
|
||||
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();
|
||||
observables_iter != observablesR1C.end();
|
||||
for (observables_iter = observablesR1C.cbegin();
|
||||
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();
|
||||
observables_iter != observablesR2C.end();
|
||||
for (observables_iter = observablesR2C.cbegin();
|
||||
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();
|
||||
observables_iter != observablesG2S.end();
|
||||
for (observables_iter = observablesG2S.cbegin();
|
||||
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();
|
||||
observables_iter != observables.end();
|
||||
for (observables_iter = observables.cbegin();
|
||||
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();
|
||||
observables_iter != observablesR1C.end();
|
||||
for (observables_iter = observablesR1C.cbegin();
|
||||
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();
|
||||
observables_iter != observablesR2C.end();
|
||||
for (observables_iter = observablesR2C.cbegin();
|
||||
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();
|
||||
observables_iter != observablesE1B.end();
|
||||
for (observables_iter = observablesE1B.cbegin();
|
||||
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();
|
||||
observables_iter != observablesE1B.end();
|
||||
for (observables_iter = observablesE1B.cbegin();
|
||||
observables_iter != observablesE1B.cend();
|
||||
observables_iter++)
|
||||
{
|
||||
uint32_t prn_ = observables_iter->second.PRN;
|
||||
|
||||
@ -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][1] = SPEED_OF_LIGHT / FREQ2; /* L2 */
|
||||
nav_data.lam[i][2] = SPEED_OF_LIGHT / FREQ5; /* L5/E5 */
|
||||
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][2] = SPEED_OF_LIGHT / FREQ5; // L5/E5
|
||||
}
|
||||
|
||||
result = rtkpos(&rtk_, obs_data, valid_obs + glo_valid_obs, &nav_data);
|
||||
@ -794,7 +799,11 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
|
||||
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]
|
||||
|
||||
@ -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();
|
||||
|
||||
|
||||
@ -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()
|
||||
|
||||
@ -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})
|
||||
|
||||
@ -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;
|
||||
@ -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,26 +40,39 @@
|
||||
|
||||
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)),
|
||||
d_nitems(nitems),
|
||||
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_open_valve == false)
|
||||
{
|
||||
if (d_ncopied_items >= d_nitems)
|
||||
{
|
||||
@ -66,15 +80,25 @@ int gnss_sdr_valve::work(int noutput_items,
|
||||
d_queue->handle(cmf->GetQueueMessage(200, 0));
|
||||
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));
|
||||
//for(long long i = 0; i++; i<n)
|
||||
// {
|
||||
// output_items[i] = input_items[i];
|
||||
// }
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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_);
|
||||
DLOG(INFO) << "valve(" << valve_->unique_id() << ")";
|
||||
|
||||
for (uint32_t i = 0; i < (n_channels_); i++)
|
||||
{
|
||||
valve_vec_.push_back(gnss_sdr_make_valve(sizeof(gr_complex), samples_, queue_));
|
||||
if (dump_)
|
||||
{
|
||||
sink_ = gr::blocks::file_sink::make(sizeof(gr_complex), dump_filename_.c_str());
|
||||
DLOG(INFO) << "file_sink(" << sink_->unique_id() << ")";
|
||||
sink_vec_.push_back(gr::blocks::file_sink::make(sizeof(gr_complex), dump_filename_.c_str()));
|
||||
}
|
||||
if (enable_throttle_control_)
|
||||
{
|
||||
throttle_ = gr::blocks::throttle::make(sizeof(gr_complex), sampling_frequency_);
|
||||
throttle_vec_.push_back(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(endian_, 0, unpack_spir_, 0);
|
||||
top_block->connect(deint_, sel_ch_ - 1, endian_vec_.at(sel_ch_ - 1), 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++;
|
||||
}
|
||||
}
|
||||
}
|
||||
for (uint32_t i = 0; i < n_channels_; i++)
|
||||
{
|
||||
if (enable_throttle_control_)
|
||||
{
|
||||
top_block->connect(unpack_spir_, 0, throttle_, 0);
|
||||
top_block->connect(throttle_, 0, valve_, 0);
|
||||
top_block->connect(unpack_spir_vec_.at(i), 0, throttle_vec_.at(i), 0);
|
||||
top_block->connect(throttle_vec_.at(i), 0, valve_vec_.at(i), 0);
|
||||
}
|
||||
else
|
||||
{
|
||||
top_block->connect(unpack_spir_, 0, valve_, 0);
|
||||
top_block->connect(unpack_spir_vec_.at(i), 0, valve_vec_.at(i), 0);
|
||||
}
|
||||
if (dump_)
|
||||
{
|
||||
top_block->connect(valve_, 0, sink_, 0);
|
||||
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(endian_, 0, unpack_spir_, 0);
|
||||
top_block->disconnect(deint_, sel_ch_ - 1, endian_vec_.at(sel_ch_ - 1), 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++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (uint32_t i = 0; i < (n_channels_); i++)
|
||||
{
|
||||
if (enable_throttle_control_)
|
||||
{
|
||||
top_block->disconnect(unpack_spir_, 0, throttle_, 0);
|
||||
top_block->disconnect(throttle_, 0, valve_, 0);
|
||||
top_block->disconnect(unpack_spir_vec_.at(i), 0, throttle_vec_.at(i), 0);
|
||||
top_block->disconnect(throttle_vec_.at(i), 0, valve_vec_.at(i), 0);
|
||||
}
|
||||
else
|
||||
{
|
||||
top_block->disconnect(unpack_spir_, 0, valve_, 0);
|
||||
top_block->disconnect(unpack_spir_vec_.at(i), 0, valve_vec_.at(i), 0);
|
||||
}
|
||||
if (dump_)
|
||||
{
|
||||
top_block->disconnect(valve_, 0, sink_, 0);
|
||||
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_;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (enable_throttle_control_)
|
||||
{
|
||||
return throttle_;
|
||||
}
|
||||
else
|
||||
{
|
||||
return unpack_spir_;
|
||||
}
|
||||
}
|
||||
return valve_vec_.at(0);
|
||||
}
|
||||
|
||||
@ -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_;
|
||||
boost::shared_ptr<gr::block> valve_;
|
||||
gr::blocks::file_sink::sptr sink_;
|
||||
gr::blocks::throttle::sptr throttle_;
|
||||
std::vector<unpack_spir_gss6450_samples_sptr> unpack_spir_vec_;
|
||||
std::vector<boost::shared_ptr<gr::block>> valve_vec_;
|
||||
std::vector<gr::blocks::file_sink::sptr> sink_vec_;
|
||||
std::vector<gr::blocks::throttle::sptr> throttle_vec_;
|
||||
gr::msg_queue::sptr queue_;
|
||||
size_t item_size_;
|
||||
};
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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");
|
||||
|
||||
|
||||
@ -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");
|
||||
|
||||
|
||||
@ -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_ */
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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}
|
||||
)
|
||||
|
||||
@ -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
|
||||
*
|
||||
@ -156,9 +163,9 @@ int ControlThread::run()
|
||||
sysv_queue_thread_ = boost::thread(&ControlThread::sysv_queue_listener, this);
|
||||
|
||||
// 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 gps_almanac_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_almanacl_xml", gps_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_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 ####################
|
||||
|
||||
@ -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>
|
||||
|
||||
class GNSSFlowgraph;
|
||||
class ConfigurationInterface;
|
||||
#include <armadillo>
|
||||
|
||||
|
||||
/*!
|
||||
@ -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
|
||||
*/
|
||||
|
||||
@ -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";
|
||||
//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
|
||||
|
||||
LOG(INFO) << "TC request flowgraph coldstart";
|
||||
//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";
|
||||
//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
|
||||
LOG(INFO) << "TC request flowgraph hotstart";
|
||||
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";
|
||||
//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
|
||||
|
||||
LOG(INFO) << "TC request flowgraph warmstart";
|
||||
//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_)
|
||||
|
||||
@ -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();
|
||||
|
||||
@ -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,17 +83,75 @@ 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
|
||||
//todo: store it in a member variable
|
||||
if (commandLine.size() > 5)
|
||||
{
|
||||
//read commandline time parameter
|
||||
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)
|
||||
{
|
||||
@ -90,14 +162,37 @@ std::string TcpCmdInterface::hotstart(const std::vector<std::string> &commandLin
|
||||
{
|
||||
response = "ERROR\n";
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
response = "ERROR: time parameter not found, please use hotstart %d/%m/%Y %H:%M:%S\n";
|
||||
}
|
||||
return response;
|
||||
}
|
||||
|
||||
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
|
||||
//todo: store it in a member variable
|
||||
if (commandLine.size() > 5)
|
||||
{
|
||||
std::string tmp_str;
|
||||
//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)
|
||||
{
|
||||
@ -108,6 +203,12 @@ std::string TcpCmdInterface::warmstart(const std::vector<std::string> &commandLi
|
||||
{
|
||||
response = "ERROR\n";
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
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";
|
||||
|
||||
@ -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_ */
|
||||
|
||||
@ -34,9 +34,9 @@
|
||||
Galileo_Almanac::Galileo_Almanac()
|
||||
{
|
||||
i_satellite_PRN = 0U;
|
||||
d_Toa = 0.0;
|
||||
d_WNa = 0.0;
|
||||
d_IODa = 0.0;
|
||||
i_Toa = 0;
|
||||
i_WNa = 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;
|
||||
E1B_HS = 0.0;
|
||||
E5a_HS = 0.0;
|
||||
E5b_HS = 0;
|
||||
E1B_HS = 0;
|
||||
E5a_HS = 0;
|
||||
}
|
||||
|
||||
@ -42,10 +42,10 @@ class Galileo_Almanac
|
||||
{
|
||||
public:
|
||||
uint32_t i_satellite_PRN; //!< SV PRN NUMBER
|
||||
double d_Toa;
|
||||
double d_WNa;
|
||||
double d_IODa;
|
||||
double d_Delta_i;
|
||||
int32_t i_Toa;
|
||||
int32_t i_WNa;
|
||||
int32_t i_IODa;
|
||||
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;
|
||||
double E1B_HS;
|
||||
double E5a_HS;
|
||||
int32_t E5b_HS;
|
||||
int32_t E1B_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(d_WNa);
|
||||
ar& BOOST_SERIALIZATION_NVP(d_IODa);
|
||||
ar& BOOST_SERIALIZATION_NVP(i_Toa);
|
||||
ar& BOOST_SERIALIZATION_NVP(i_WNa);
|
||||
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);
|
||||
|
||||
@ -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;
|
||||
t0a_7 = 0.0;
|
||||
WN_a_7 = 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;
|
||||
E1B_HS_8 = 0.0;
|
||||
E5a_HS_8 = 0.0;
|
||||
E5b_HS_8 = 0;
|
||||
E1B_HS_8 = 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;
|
||||
t0a_9 = 0.0;
|
||||
WN_a_9 = 0;
|
||||
t0a_9 = 0;
|
||||
M0_9 = 0.0;
|
||||
af0_9 = 0.0;
|
||||
af1_9 = 0.0;
|
||||
E5b_HS_9 = 0.0;
|
||||
E1B_HS_9 = 0.0;
|
||||
E5a_HS_9 = 0.0;
|
||||
E5b_HS_9 = 0;
|
||||
E1B_HS_9 = 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;
|
||||
E1B_HS_10 = 0.0;
|
||||
E5a_HS_10 = 0.0;
|
||||
E5b_HS_10 = 0;
|
||||
E1B_HS_10 = 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.d_WNa = this->WN_a_7;
|
||||
galileo_almanac.d_IODa = this->IOD_a_7;
|
||||
galileo_almanac.i_Toa = this->t0a_7;
|
||||
galileo_almanac.i_WNa = this->WN_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.d_WNa = this->WN_a_9;
|
||||
galileo_almanac.d_IODa = this->IOD_a_9;
|
||||
galileo_almanac.i_Toa = this->t0a_9;
|
||||
galileo_almanac.i_WNa = this->WN_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.d_WNa = this->WN_a_9;
|
||||
galileo_almanac.d_IODa = this->IOD_a_10;
|
||||
galileo_almanac.i_Toa = this->t0a_9;
|
||||
galileo_almanac.i_WNa = this->WN_a_9;
|
||||
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;
|
||||
|
||||
@ -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;
|
||||
double E1B_HS_8;
|
||||
double E5a_HS_8;
|
||||
int32_t E5b_HS_8;
|
||||
int32_t E1B_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;
|
||||
double E1B_HS_9;
|
||||
double E5a_HS_9;
|
||||
int32_t E5b_HS_9;
|
||||
int32_t E1B_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;
|
||||
double E1B_HS_10;
|
||||
double E5a_HS_10;
|
||||
int32_t E5b_HS_10;
|
||||
int32_t E1B_HS_10;
|
||||
int32_t E5a_HS_10;
|
||||
|
||||
Galileo_Almanac_Helper(); //!< Default constructor
|
||||
Galileo_Almanac get_almanac(int i);
|
||||
|
||||
@ -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;
|
||||
E1B_HS_5 = 0.0;
|
||||
E5b_DVS_5 = 0.0;
|
||||
E1B_DVS_5 = 0.0;
|
||||
E5b_HS_5 = 0;
|
||||
E1B_HS_5 = 0;
|
||||
E5b_DVS_5 = 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;
|
||||
E1B_HS_8 = 0.0;
|
||||
E5b_HS_8 = 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;
|
||||
E1B_HS_9 = 0.0;
|
||||
E5b_HS_9 = 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;
|
||||
E1B_HS_10 = 0.0;
|
||||
E5b_HS_10 = 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;
|
||||
|
||||
@ -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
|
||||
double E1B_HS_5; //!< E1B Signal Health Status
|
||||
double E5b_DVS_5; //!< E5b Data Validity Status
|
||||
double E1B_DVS_5; //!< E1B Data Validity Status
|
||||
int32_t E5b_HS_5; //!< E5b Signal Health Status
|
||||
int32_t E1B_HS_5; //!< E1B Signal Health Status
|
||||
int32_t E5b_DVS_5; //!< E5b 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;
|
||||
double E1B_HS_8;
|
||||
int32_t E5b_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;
|
||||
double E1B_HS_9;
|
||||
int32_t E5b_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;
|
||||
double E1B_HS_10;
|
||||
int32_t E5b_HS_10;
|
||||
int32_t E1B_HS_10;
|
||||
|
||||
// GST-GPS conversion
|
||||
double A_0G_10; //!< Constant term of the offset Delta t systems
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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));
|
||||
d_Toa = d_Toa * T_OA_LSB;
|
||||
i_Toa = static_cast<int32_t>(read_navigation_unsigned(subframe_bits, T_OA));
|
||||
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));
|
||||
|
||||
@ -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_WN_A; //!< Modulo 256 of the GPS week number to which the almanac reference time (d_Toa) is referenced
|
||||
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 (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
|
||||
|
||||
@ -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}
|
||||
|
||||
@ -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}
|
||||
)
|
||||
|
||||
|
||||
@ -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");
|
||||
|
||||
@ -18,7 +18,6 @@
|
||||
|
||||
|
||||
set(SYSTEM_TESTING_LIB_SOURCES
|
||||
geofunctions.cc
|
||||
spirent_motion_csv_dump_reader.cc
|
||||
rtklib_solver_dump_reader.cc
|
||||
)
|
||||
|
||||
@ -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");
|
||||
|
||||
@ -455,70 +455,7 @@ 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)
|
||||
{
|
||||
//fall back to read receiver KML output (position only)
|
||||
std::fstream myfile(PositionSystemTest::generated_kml_file, std::ios_base::in);
|
||||
ASSERT_TRUE(myfile.is_open()) << "No valid kml file could be opened";
|
||||
std::string line;
|
||||
// Skip header
|
||||
std::getline(myfile, line);
|
||||
bool is_header = true;
|
||||
while (is_header)
|
||||
{
|
||||
std::getline(myfile, line);
|
||||
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_ecef;
|
||||
pv_Geo_to_ECEF(degtorad(lat), degtorad(longitude), h, arma::vec{0, 0, 0}, tmp_r_ecef, tmp_v_ecef);
|
||||
R_eb_e.insert_cols(current_epoch, tmp_r_ecef);
|
||||
arma::vec tmp_r_enu = {0, 0, 0};
|
||||
cart2utm(tmp_r_ecef, utm_zone, tmp_r_enu);
|
||||
R_eb_enu.insert_cols(current_epoch, tmp_r_enu);
|
||||
// std::cout << "lat = " << lat << ", longitude = " << longitude << " h = " << h << std::endl;
|
||||
// std::cout << "E = " << east << ", N = " << north << " U = " << up << std::endl;
|
||||
// getchar();
|
||||
}
|
||||
}
|
||||
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();
|
||||
@ -552,10 +489,8 @@ void PositionSystemTest::check_results()
|
||||
current_epoch++;
|
||||
}
|
||||
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));
|
||||
|
||||
@ -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
|
||||
pvt_solution->set_position_UTC_time(pt);
|
||||
boost::posix_time::hours(22) + boost::posix_time::minutes(54) + boost::posix_time::seconds(46));
|
||||
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->set_rx_pos(pos);
|
||||
|
||||
pvt_solution->set_valid_position(true);
|
||||
pvt_solution->pvt_sol.rr[0] = -2282104.0; //49.27416667;
|
||||
pvt_solution->pvt_sol.rr[1] = -3489369.0; //-123.18533333;
|
||||
pvt_solution->pvt_sol.rr[2] = 4810507.0; // 0
|
||||
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");
|
||||
}
|
||||
}
|
||||
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");
|
||||
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();
|
||||
|
||||
@ -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);
|
||||
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,6 +746,34 @@ 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;
|
||||
std::chrono::time_point<std::chrono::system_clock> start, end;
|
||||
if (acq_to_trk_delay_samples > 0)
|
||||
{
|
||||
EXPECT_NO_THROW({
|
||||
start = std::chrono::system_clock::now();
|
||||
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({
|
||||
@ -728,11 +781,11 @@ TEST_F(TrackingPullInTest, ValidationOfResults)
|
||||
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
|
||||
|
||||
//********************************
|
||||
|
||||
@ -43,6 +43,7 @@ include_directories(
|
||||
${GNURADIO_BLOCKS_INCLUDE_DIRS}
|
||||
${ARMADILLO_INCLUDE_DIRS}
|
||||
${Boost_INCLUDE_DIRS}
|
||||
${PUGIXML_INCLUDE_DIR}
|
||||
${VOLK_GNSSSDR_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user