2017-04-20 14:10:12 +00:00
/*!
2019-03-02 01:21:03 +00:00
* \ file rtklib_pvt_gs . cc
2017-04-20 14:10:12 +00:00
* \ brief Interface of a Position Velocity and Time computation block
* \ author Javier Arribas , 2017. jarribas ( at ) cttc . es
*
2020-07-28 14:57:15 +00:00
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
2017-04-20 14:10:12 +00:00
*
2020-12-30 12:35:06 +00:00
* GNSS - SDR is a Global Navigation Satellite System software - defined receiver .
2017-04-20 14:10:12 +00:00
* This file is part of GNSS - SDR .
*
2020-12-30 12:35:06 +00:00
* Copyright ( C ) 2010 - 2020 ( see AUTHORS file for a list of contributors )
2020-02-08 00:20:02 +00:00
* SPDX - License - Identifier : GPL - 3.0 - or - later
2017-04-20 14:10:12 +00:00
*
2020-07-28 14:57:15 +00:00
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
2017-04-20 14:10:12 +00:00
*/
2019-11-24 12:06:32 +00:00
# include "rtklib_pvt_gs.h"
2019-07-31 16:16:09 +00:00
# include "MATH_CONSTANTS.h"
2019-03-05 07:59:04 +00:00
# include "beidou_dnav_almanac.h"
# include "beidou_dnav_ephemeris.h"
2019-03-05 18:31:41 +00:00
# include "beidou_dnav_iono.h"
# include "beidou_dnav_utc_model.h"
2018-12-02 04:29:11 +00:00
# include "display.h"
2019-03-05 07:59:04 +00:00
# include "galileo_almanac.h"
2018-10-24 15:28:12 +00:00
# include "galileo_almanac_helper.h"
2019-03-05 07:59:04 +00:00
# include "galileo_ephemeris.h"
2019-03-05 18:31:41 +00:00
# include "galileo_iono.h"
# include "galileo_utc_model.h"
2019-03-05 07:59:04 +00:00
# include "geojson_printer.h"
2019-03-05 18:31:41 +00:00
# include "glonass_gnav_almanac.h"
# include "glonass_gnav_ephemeris.h"
# include "glonass_gnav_utc_model.h"
2019-07-31 16:16:09 +00:00
# include "gnss_frequencies.h"
2018-10-30 10:41:15 +00:00
# include "gnss_sdr_create_directory.h"
2020-06-18 09:49:28 +00:00
# include "gnss_sdr_make_unique.h"
2019-03-05 07:59:04 +00:00
# include "gps_almanac.h"
2019-03-05 18:31:41 +00:00
# include "gps_cnav_ephemeris.h"
# include "gps_cnav_iono.h"
# include "gps_cnav_utc_model.h"
2019-03-05 07:59:04 +00:00
# include "gps_ephemeris.h"
2019-03-05 18:31:41 +00:00
# include "gps_iono.h"
# include "gps_utc_model.h"
2019-03-05 07:59:04 +00:00
# include "gpx_printer.h"
# include "kml_printer.h"
2019-03-05 18:31:41 +00:00
# include "monitor_pvt.h"
2019-03-05 07:59:04 +00:00
# include "monitor_pvt_udp_sink.h"
# include "nmea_printer.h"
2018-12-02 04:29:11 +00:00
# include "pvt_conf.h"
2019-03-05 07:59:04 +00:00
# include "rinex_printer.h"
# include "rtcm_printer.h"
# include "rtklib_solver.h"
2019-03-05 18:31:41 +00:00
# include <boost/any.hpp> // for any_cast, any
# include <boost/archive/xml_iarchive.hpp> // for xml_iarchive
# include <boost/archive/xml_oarchive.hpp> // for xml_oarchive
2019-03-05 07:59:04 +00:00
# include <boost/exception/diagnostic_information.hpp>
# include <boost/exception/exception.hpp>
2018-07-21 11:40:52 +00:00
# include <boost/serialization/map.hpp>
2019-03-05 07:59:04 +00:00
# include <boost/serialization/nvp.hpp> // for nvp, make_nvp
# include <glog/logging.h> // for LOG
# include <gnuradio/io_signature.h> // for io_signature
# include <pmt/pmt_sugar.h> // for mp
2019-03-05 18:31:41 +00:00
# include <algorithm> // for sort, unique
2020-03-10 18:56:13 +00:00
# include <cerrno> // for errno
# include <cstring> // for strerror
2019-03-05 18:31:41 +00:00
# include <exception> // for exception
# include <fstream> // for ofstream
2019-03-15 12:31:18 +00:00
# include <iomanip> // for put_time, setprecision
2019-03-05 18:31:41 +00:00
# include <iostream> // for operator<<
2019-03-15 12:31:18 +00:00
# include <locale> // for locale
# include <sstream> // for ostringstream
2019-03-05 18:31:41 +00:00
# include <stdexcept> // for length_error
# include <sys/ipc.h> // for IPC_CREAT
# include <sys/msg.h> // for msgctl
2020-07-19 07:39:32 +00:00
# include <typeinfo> // for std::type_info, typeid
2020-06-18 23:36:36 +00:00
# include <utility> // for pair
2019-06-10 19:41:13 +00:00
2020-04-25 20:17:15 +00:00
# if HAS_GENERIC_LAMBDA
# else
2020-06-08 18:04:01 +00:00
# include <boost/bind/bind.hpp>
2020-04-25 20:17:15 +00:00
# endif
2019-06-10 19:41:13 +00:00
# if HAS_STD_FILESYSTEM
# include <system_error>
namespace errorlib = std ;
2019-06-12 18:39:29 +00:00
# if HAS_STD_FILESYSTEM_EXPERIMENTAL
# include <experimental/filesystem>
namespace fs = std : : experimental : : filesystem ;
# else
# include <filesystem>
namespace fs = std : : filesystem ;
# endif
2019-06-10 19:41:13 +00:00
# else
# include <boost/filesystem/path.hpp>
# include <boost/system/error_code.hpp> // for error_code
namespace fs = boost : : filesystem ;
namespace errorlib = boost : : system ;
# endif
2020-06-12 22:32:40 +00:00
# if USE_OLD_BOOST_MATH_COMMON_FACTOR
2018-07-21 11:57:14 +00:00
# include <boost/math/common_factor_rt.hpp>
namespace bc = boost : : math ;
# else
# include <boost/integer/common_factor_rt.hpp>
namespace bc = boost : : integer ;
# endif
2018-02-26 02:15:53 +00:00
2017-04-20 14:10:12 +00:00
2019-03-02 01:21:03 +00:00
rtklib_pvt_gs_sptr rtklib_make_pvt_gs ( uint32_t nchannels ,
2018-10-27 22:42:28 +00:00
const Pvt_Conf & conf_ ,
2018-12-02 04:29:11 +00:00
const rtk_t & rtk )
2017-04-20 14:10:12 +00:00
{
2019-03-02 01:21:03 +00:00
return rtklib_pvt_gs_sptr ( new rtklib_pvt_gs ( nchannels ,
2018-10-27 22:42:28 +00:00
conf_ ,
2018-03-03 01:03:39 +00:00
rtk ) ) ;
2017-04-20 14:10:12 +00:00
}
2019-03-02 01:21:03 +00:00
rtklib_pvt_gs : : rtklib_pvt_gs ( uint32_t nchannels ,
2018-10-27 22:42:28 +00:00
const Pvt_Conf & conf_ ,
2019-03-02 01:21:03 +00:00
const rtk_t & rtk ) : gr : : sync_block ( " rtklib_pvt_gs " ,
2019-01-28 01:29:43 +00:00
gr : : io_signature : : make ( nchannels , nchannels , sizeof ( Gnss_Synchro ) ) ,
gr : : io_signature : : make ( 0 , 0 , 0 ) )
2017-04-20 14:10:12 +00:00
{
2019-04-23 15:31:26 +00:00
// Send feedback message to observables block with the receiver clock offset
this - > message_port_register_out ( pmt : : mp ( " pvt_to_observables " ) ) ;
2019-07-11 16:39:28 +00:00
// Send PVT status to gnss_flowgraph
this - > message_port_register_out ( pmt : : mp ( " status " ) ) ;
2019-04-23 15:31:26 +00:00
2020-06-18 23:36:36 +00:00
d_mapStringValues [ " 1C " ] = evGPS_1C ;
d_mapStringValues [ " 2S " ] = evGPS_2S ;
d_mapStringValues [ " L5 " ] = evGPS_L5 ;
d_mapStringValues [ " 1B " ] = evGAL_1B ;
d_mapStringValues [ " 5X " ] = evGAL_5X ;
2020-11-07 20:33:26 +00:00
d_mapStringValues [ " E6 " ] = evGAL_E6 ;
2020-07-31 15:58:57 +00:00
d_mapStringValues [ " 7X " ] = evGAL_7X ;
2020-06-18 23:36:36 +00:00
d_mapStringValues [ " 1G " ] = evGLO_1G ;
d_mapStringValues [ " 2G " ] = evGLO_2G ;
d_mapStringValues [ " B1 " ] = evBDS_B1 ;
d_mapStringValues [ " B2 " ] = evBDS_B2 ;
d_mapStringValues [ " B3 " ] = evBDS_B3 ;
2019-07-31 16:16:09 +00:00
2020-06-18 23:36:36 +00:00
d_initial_carrier_phase_offset_estimation_rads = std : : vector < double > ( nchannels , 0.0 ) ;
d_channel_initialized = std : : vector < bool > ( nchannels , false ) ;
2020-02-06 15:56:42 +00:00
2020-06-18 23:36:36 +00:00
d_max_obs_block_rx_clock_offset_ms = conf_ . max_obs_block_rx_clock_offset_ms ;
2020-02-06 15:56:42 +00:00
2018-10-27 22:42:28 +00:00
d_output_rate_ms = conf_ . output_rate_ms ;
d_display_rate_ms = conf_ . display_rate_ms ;
2019-08-01 18:09:35 +00:00
d_report_rate_ms = 1000 ; // report every second PVT to gnss_synchro
2018-10-27 22:42:28 +00:00
d_dump = conf_ . dump ;
2018-10-31 13:56:59 +00:00
d_dump_mat = conf_ . dump_mat and d_dump ;
2018-10-27 22:42:28 +00:00
d_dump_filename = conf_ . dump_filename ;
std : : string dump_ls_pvt_filename = conf_ . dump_filename ;
2018-10-30 10:41:15 +00:00
if ( d_dump )
{
std : : string dump_path ;
// Get path
2019-02-05 00:31:09 +00:00
if ( d_dump_filename . find_last_of ( ' / ' ) ! = std : : string : : npos )
2018-10-30 10:41:15 +00:00
{
2019-02-05 00:31:09 +00:00
std : : string dump_filename_ = d_dump_filename . substr ( d_dump_filename . find_last_of ( ' / ' ) + 1 ) ;
dump_path = d_dump_filename . substr ( 0 , d_dump_filename . find_last_of ( ' / ' ) ) ;
2018-10-30 10:41:15 +00:00
d_dump_filename = dump_filename_ ;
}
else
{
dump_path = std : : string ( " . " ) ;
}
if ( d_dump_filename . empty ( ) )
{
d_dump_filename = " pvt " ;
}
// remove extension if any
2019-02-05 00:31:09 +00:00
if ( d_dump_filename . substr ( 1 ) . find_last_of ( ' . ' ) ! = std : : string : : npos )
2018-10-30 10:41:15 +00:00
{
2019-02-05 00:31:09 +00:00
d_dump_filename = d_dump_filename . substr ( 0 , d_dump_filename . find_last_of ( ' . ' ) ) ;
2018-10-30 10:41:15 +00:00
}
2019-06-10 19:41:13 +00:00
dump_ls_pvt_filename = dump_path + fs : : path : : preferred_separator + d_dump_filename ;
2018-10-30 10:41:15 +00:00
dump_ls_pvt_filename . append ( " .dat " ) ;
// create directory
if ( ! gnss_sdr_create_directory ( dump_path ) )
{
2020-07-07 16:53:50 +00:00
std : : cerr < < " GNSS-SDR cannot create dump file for the PVT block. Wrong permissions? \n " ;
2018-10-30 10:41:15 +00:00
d_dump = false ;
}
}
2017-04-20 14:10:12 +00:00
d_nchannels = nchannels ;
2018-10-30 10:41:15 +00:00
2020-06-18 23:36:36 +00:00
d_type_of_rx = conf_ . type_of_receiver ;
2017-04-20 14:10:12 +00:00
// GPS Ephemeris data message port in
this - > message_port_register_in ( pmt : : mp ( " telemetry " ) ) ;
2020-04-25 20:17:15 +00:00
this - > set_msg_handler ( pmt : : mp ( " telemetry " ) ,
# if HAS_GENERIC_LAMBDA
2020-06-08 18:04:01 +00:00
[ this ] ( auto & & PH1 ) { msg_handler_telemetry ( PH1 ) ; } ) ;
2020-04-25 20:17:15 +00:00
# else
2020-06-12 22:32:40 +00:00
# if USE_BOOST_BIND_PLACEHOLDERS
2020-05-18 16:58:39 +00:00
boost : : bind ( & rtklib_pvt_gs : : msg_handler_telemetry , this , boost : : placeholders : : _1 ) ) ;
2020-05-18 20:50:34 +00:00
# else
boost : : bind ( & rtklib_pvt_gs : : msg_handler_telemetry , this , _1 ) ) ;
# endif
2020-04-25 20:17:15 +00:00
# endif
2018-06-05 19:41:13 +00:00
// initialize kml_printer
2020-07-17 08:48:37 +00:00
const std : : string kml_dump_filename = d_dump_filename ;
2018-10-27 22:42:28 +00:00
d_kml_output_enabled = conf_ . kml_output_enabled ;
2019-06-16 18:27:00 +00:00
d_kml_rate_ms = conf_ . kml_rate_ms ;
if ( d_kml_rate_ms = = 0 )
{
d_kml_output_enabled = false ;
}
2018-10-27 22:42:28 +00:00
if ( d_kml_output_enabled )
2017-05-14 17:59:55 +00:00
{
2020-06-18 09:49:28 +00:00
d_kml_dump = std : : make_unique < Kml_Printer > ( conf_ . kml_output_path ) ;
2018-10-27 22:42:28 +00:00
d_kml_dump - > set_headers ( kml_dump_filename ) ;
2017-05-14 17:59:55 +00:00
}
2017-04-20 14:10:12 +00:00
else
2017-05-14 17:59:55 +00:00
{
2018-10-27 22:42:28 +00:00
d_kml_dump = nullptr ;
2017-05-14 17:59:55 +00:00
}
2017-04-20 14:10:12 +00:00
2018-06-05 19:41:13 +00:00
// initialize gpx_printer
2020-07-17 08:48:37 +00:00
const std : : string gpx_dump_filename = d_dump_filename ;
2018-10-27 22:42:28 +00:00
d_gpx_output_enabled = conf_ . gpx_output_enabled ;
2019-06-16 18:27:00 +00:00
d_gpx_rate_ms = conf_ . gpx_rate_ms ;
if ( d_gpx_rate_ms = = 0 )
{
d_gpx_output_enabled = false ;
}
2018-10-27 22:42:28 +00:00
if ( d_gpx_output_enabled )
2017-08-17 04:40:05 +00:00
{
2020-06-18 09:49:28 +00:00
d_gpx_dump = std : : make_unique < Gpx_Printer > ( conf_ . gpx_output_path ) ;
2018-10-27 22:42:28 +00:00
d_gpx_dump - > set_headers ( gpx_dump_filename ) ;
2017-08-17 04:40:05 +00:00
}
else
{
2018-10-27 22:42:28 +00:00
d_gpx_dump = nullptr ;
2017-08-17 04:40:05 +00:00
}
2018-05-05 05:47:42 +00:00
2018-06-05 19:41:13 +00:00
// initialize geojson_printer
2020-07-17 08:48:37 +00:00
const std : : string geojson_dump_filename = d_dump_filename ;
2018-10-27 22:42:28 +00:00
d_geojson_output_enabled = conf_ . geojson_output_enabled ;
2019-06-16 18:27:00 +00:00
d_geojson_rate_ms = conf_ . geojson_rate_ms ;
if ( d_geojson_rate_ms = = 0 )
{
d_geojson_output_enabled = false ;
}
2018-10-27 22:42:28 +00:00
if ( d_geojson_output_enabled )
2017-05-14 17:59:55 +00:00
{
2020-06-18 09:49:28 +00:00
d_geojson_printer = std : : make_unique < GeoJSON_Printer > ( conf_ . geojson_output_path ) ;
2018-10-27 22:42:28 +00:00
d_geojson_printer - > set_headers ( geojson_dump_filename ) ;
2017-05-14 17:59:55 +00:00
}
2017-04-20 14:10:12 +00:00
else
2017-05-14 17:59:55 +00:00
{
2018-10-27 22:42:28 +00:00
d_geojson_printer = nullptr ;
2017-05-14 17:59:55 +00:00
}
2017-04-20 14:10:12 +00:00
2018-06-05 19:41:13 +00:00
// initialize nmea_printer
2018-11-08 09:02:52 +00:00
d_nmea_output_file_enabled = ( conf_ . nmea_output_file_enabled or conf_ . flag_nmea_tty_port ) ;
2019-06-16 18:27:00 +00:00
d_nmea_rate_ms = conf_ . nmea_rate_ms ;
if ( d_nmea_rate_ms = = 0 )
{
d_nmea_output_file_enabled = false ;
}
2018-11-08 09:02:52 +00:00
if ( d_nmea_output_file_enabled )
2017-05-14 17:59:55 +00:00
{
2020-06-18 09:49:28 +00:00
d_nmea_printer = std : : make_unique < Nmea_Printer > ( conf_ . nmea_dump_filename , conf_ . nmea_output_file_enabled , conf_ . flag_nmea_tty_port , conf_ . nmea_dump_devname , conf_ . nmea_output_file_path ) ;
2017-05-14 17:59:55 +00:00
}
2017-04-20 14:10:12 +00:00
else
2017-05-14 17:59:55 +00:00
{
2018-11-08 09:02:52 +00:00
d_nmea_printer = nullptr ;
2017-05-14 17:59:55 +00:00
}
2017-04-20 14:10:12 +00:00
2018-06-05 19:41:13 +00:00
// initialize rtcm_printer
2020-07-17 08:48:37 +00:00
const std : : string rtcm_dump_filename = d_dump_filename ;
2018-11-02 13:12:31 +00:00
if ( conf_ . flag_rtcm_server or conf_ . flag_rtcm_tty_port or conf_ . rtcm_output_file_enabled )
2017-08-17 04:40:05 +00:00
{
2020-06-18 09:49:28 +00:00
d_rtcm_printer = std : : make_unique < Rtcm_Printer > ( rtcm_dump_filename , conf_ . rtcm_output_file_enabled , conf_ . flag_rtcm_server , conf_ . flag_rtcm_tty_port , conf_ . rtcm_tcp_port , conf_ . rtcm_station_id , conf_ . rtcm_dump_devname , true , conf_ . rtcm_output_file_path ) ;
2018-11-02 13:12:31 +00:00
std : : map < int , int > rtcm_msg_rate_ms = conf_ . rtcm_msg_rate_ms ;
if ( rtcm_msg_rate_ms . find ( 1019 ) ! = rtcm_msg_rate_ms . end ( ) )
{
d_rtcm_MT1019_rate_ms = rtcm_msg_rate_ms [ 1019 ] ;
}
else
{
d_rtcm_MT1019_rate_ms = bc : : lcm ( 5000 , d_output_rate_ms ) ; // default value if not set
}
if ( rtcm_msg_rate_ms . find ( 1020 ) ! = rtcm_msg_rate_ms . end ( ) )
{
d_rtcm_MT1020_rate_ms = rtcm_msg_rate_ms [ 1020 ] ;
}
else
{
d_rtcm_MT1020_rate_ms = bc : : lcm ( 5000 , d_output_rate_ms ) ; // default value if not set
}
if ( rtcm_msg_rate_ms . find ( 1045 ) ! = rtcm_msg_rate_ms . end ( ) )
{
d_rtcm_MT1045_rate_ms = rtcm_msg_rate_ms [ 1045 ] ;
}
else
{
d_rtcm_MT1045_rate_ms = bc : : lcm ( 5000 , d_output_rate_ms ) ; // default value if not set
}
if ( rtcm_msg_rate_ms . find ( 1077 ) ! = rtcm_msg_rate_ms . end ( ) ) // whatever between 1071 and 1077
{
d_rtcm_MT1077_rate_ms = rtcm_msg_rate_ms [ 1077 ] ;
}
else
{
d_rtcm_MT1077_rate_ms = bc : : lcm ( 1000 , d_output_rate_ms ) ; // default value if not set
}
if ( rtcm_msg_rate_ms . find ( 1087 ) ! = rtcm_msg_rate_ms . end ( ) ) // whatever between 1081 and 1087
{
d_rtcm_MT1087_rate_ms = rtcm_msg_rate_ms [ 1087 ] ;
}
else
{
d_rtcm_MT1087_rate_ms = bc : : lcm ( 1000 , d_output_rate_ms ) ; // default value if not set
}
if ( rtcm_msg_rate_ms . find ( 1097 ) ! = rtcm_msg_rate_ms . end ( ) ) // whatever between 1091 and 1097
{
d_rtcm_MT1097_rate_ms = rtcm_msg_rate_ms [ 1097 ] ;
d_rtcm_MSM_rate_ms = rtcm_msg_rate_ms [ 1097 ] ;
}
else
{
d_rtcm_MT1097_rate_ms = bc : : lcm ( 1000 , d_output_rate_ms ) ; // default value if not set
d_rtcm_MSM_rate_ms = bc : : lcm ( 1000 , d_output_rate_ms ) ; // default value if not set
}
2020-06-18 23:36:36 +00:00
d_rtcm_enabled = true ;
2017-08-17 04:40:05 +00:00
}
else
{
2018-11-03 09:50:19 +00:00
d_rtcm_MT1019_rate_ms = 0 ;
d_rtcm_MT1045_rate_ms = 0 ;
d_rtcm_MT1020_rate_ms = 0 ;
d_rtcm_MT1077_rate_ms = 0 ;
d_rtcm_MT1087_rate_ms = 0 ;
d_rtcm_MT1097_rate_ms = 0 ;
d_rtcm_MSM_rate_ms = 0 ;
2020-06-18 23:36:36 +00:00
d_rtcm_enabled = false ;
2018-11-02 13:12:31 +00:00
d_rtcm_printer = nullptr ;
2017-08-17 04:40:05 +00:00
}
2017-04-20 14:10:12 +00:00
2018-06-05 19:41:13 +00:00
// initialize RINEX printer
2020-06-18 23:36:36 +00:00
d_rinex_output_enabled = conf_ . rinex_output_enabled ;
2018-10-27 22:42:28 +00:00
d_rinex_version = conf_ . rinex_version ;
2020-06-18 23:36:36 +00:00
if ( d_rinex_output_enabled )
2017-05-14 17:59:55 +00:00
{
2020-06-18 23:36:36 +00:00
d_rp = std : : make_unique < Rinex_Printer > ( d_rinex_version , conf_ . rinex_output_path , conf_ . rinex_name ) ;
d_rp - > set_pre_2009_file ( conf_ . pre_2009_file ) ;
2017-05-14 17:59:55 +00:00
}
2017-04-20 14:10:12 +00:00
else
2017-05-14 17:59:55 +00:00
{
2020-06-18 23:36:36 +00:00
d_rp = nullptr ;
2017-05-14 17:59:55 +00:00
}
2018-10-27 22:42:28 +00:00
d_rinexobs_rate_ms = conf_ . rinexobs_rate_ms ;
2017-04-20 14:10:12 +00:00
2018-10-27 22:42:28 +00:00
// XML printer
d_xml_storage = conf_ . xml_output_enabled ;
if ( d_xml_storage )
{
2020-06-18 23:36:36 +00:00
d_xml_base_path = conf_ . xml_output_path ;
2019-06-10 19:41:13 +00:00
fs : : path full_path ( fs : : current_path ( ) ) ;
2020-06-18 23:36:36 +00:00
const fs : : path p ( d_xml_base_path ) ;
2019-06-10 19:41:13 +00:00
if ( ! fs : : exists ( p ) )
2018-10-27 22:42:28 +00:00
{
std : : string new_folder ;
2020-12-30 20:49:29 +00:00
for ( const auto & folder : fs : : path ( d_xml_base_path ) )
2018-10-27 22:42:28 +00:00
{
new_folder + = folder . string ( ) ;
2019-06-10 19:41:13 +00:00
errorlib : : error_code ec ;
if ( ! fs : : exists ( new_folder ) )
2018-10-27 22:42:28 +00:00
{
2019-06-10 19:41:13 +00:00
if ( ! fs : : create_directory ( new_folder , ec ) )
2018-10-27 22:42:28 +00:00
{
2020-07-07 16:53:50 +00:00
std : : cout < < " Could not create the " < < new_folder < < " folder. \n " ;
2020-06-18 23:36:36 +00:00
d_xml_base_path = full_path . string ( ) ;
2018-10-27 22:42:28 +00:00
}
}
2019-06-10 19:41:13 +00:00
new_folder + = fs : : path : : preferred_separator ;
2018-10-27 22:42:28 +00:00
}
}
else
{
2020-06-18 23:36:36 +00:00
d_xml_base_path = p . string ( ) ;
2018-10-27 22:42:28 +00:00
}
2020-06-18 23:36:36 +00:00
if ( d_xml_base_path ! = " . " )
2018-10-27 22:42:28 +00:00
{
2020-07-07 16:53:50 +00:00
std : : cout < < " XML files will be stored at " < < d_xml_base_path < < ' \n ' ;
2018-10-27 22:42:28 +00:00
}
2018-06-05 19:41:13 +00:00
2020-06-18 23:36:36 +00:00
d_xml_base_path = d_xml_base_path + fs : : path : : preferred_separator ;
2018-10-27 22:42:28 +00:00
}
2017-04-20 14:10:12 +00:00
2017-04-28 13:38:31 +00:00
d_rx_time = 0.0 ;
2017-04-20 14:10:12 +00:00
d_last_status_print_seg = 0 ;
2019-01-21 10:59:29 +00:00
// PVT MONITOR
2020-06-18 23:36:36 +00:00
d_flag_monitor_pvt_enabled = conf_ . monitor_enabled ;
if ( d_flag_monitor_pvt_enabled )
2019-01-21 10:59:29 +00:00
{
std : : string address_string = conf_ . udp_addresses ;
std : : vector < std : : string > udp_addr_vec = split_string ( address_string , ' _ ' ) ;
std : : sort ( udp_addr_vec . begin ( ) , udp_addr_vec . end ( ) ) ;
udp_addr_vec . erase ( std : : unique ( udp_addr_vec . begin ( ) , udp_addr_vec . end ( ) ) , udp_addr_vec . end ( ) ) ;
2020-06-18 23:36:36 +00:00
d_udp_sink_ptr = std : : make_unique < Monitor_Pvt_Udp_Sink > ( udp_addr_vec , conf_ . udp_port , conf_ . protobuf_enabled ) ;
2019-01-21 10:59:29 +00:00
}
2019-02-18 20:44:19 +00:00
else
{
2020-06-18 23:36:36 +00:00
d_udp_sink_ptr = nullptr ;
2019-02-18 20:44:19 +00:00
}
2019-01-21 10:59:29 +00:00
2017-04-20 14:10:12 +00:00
// Create Sys V message queue
2020-06-18 23:36:36 +00:00
d_first_fix = true ;
d_sysv_msg_key = 1101 ;
2020-07-17 08:48:37 +00:00
const int msgflg = IPC_CREAT | 0666 ;
2020-06-18 23:36:36 +00:00
if ( ( d_sysv_msqid = msgget ( d_sysv_msg_key , msgflg ) ) = = - 1 )
2017-05-14 17:59:55 +00:00
{
2020-07-07 16:53:50 +00:00
std : : cout < < " GNSS-SDR cannot create System V message queues. \n " ;
LOG ( WARNING ) < < " The System V message queue is not available. Error: " < < errno < < " - " < < strerror ( errno ) ;
2017-05-14 17:59:55 +00:00
}
2019-02-18 20:44:19 +00:00
2019-03-15 12:31:18 +00:00
// Display time in local time zone
2019-03-16 11:47:36 +00:00
d_show_local_time_zone = conf_ . show_local_time_zone ;
2019-03-15 12:31:18 +00:00
std : : ostringstream os ;
2019-03-16 19:30:38 +00:00
# ifdef HAS_PUT_TIME
2019-03-19 20:11:21 +00:00
time_t when = std : : time ( nullptr ) ;
auto const tm = * std : : localtime ( & when ) ;
2019-03-15 12:31:18 +00:00
os < < std : : put_time ( & tm , " %z " ) ;
2019-03-16 19:30:38 +00:00
# endif
2019-03-16 11:47:36 +00:00
std : : string utc_diff_str = os . str ( ) ; // in ISO 8601 format: "+HHMM" or "-HHMM"
if ( utc_diff_str . empty ( ) )
{
utc_diff_str = " +0000 " ;
}
2020-07-17 08:48:37 +00:00
const int h = std : : stoi ( utc_diff_str . substr ( 0 , 3 ) , nullptr , 10 ) ;
const int m = std : : stoi ( utc_diff_str [ 0 ] + utc_diff_str . substr ( 3 ) , nullptr , 10 ) ;
2019-03-15 12:31:18 +00:00
d_utc_diff_time = boost : : posix_time : : hours ( h ) + boost : : posix_time : : minutes ( m ) ;
2019-03-16 11:47:36 +00:00
std : : ostringstream os2 ;
2019-03-16 19:30:38 +00:00
# ifdef HAS_PUT_TIME
2019-03-16 11:47:36 +00:00
os2 < < std : : put_time ( & tm , " %Z " ) ;
2019-03-16 19:30:38 +00:00
# endif
2020-07-17 08:48:37 +00:00
const std : : string time_zone_abrv = os2 . str ( ) ;
2019-03-16 11:47:36 +00:00
if ( time_zone_abrv . empty ( ) )
{
if ( utc_diff_str = = " +0000 " )
{
d_local_time_str = " UTC " ;
}
else
{
d_local_time_str = " (UTC " + utc_diff_str . substr ( 0 , 3 ) + " : " + utc_diff_str . substr ( 3 , 2 ) + " ) " ;
}
}
else
{
d_local_time_str = std : : string ( " " ) + time_zone_abrv + " (UTC " + utc_diff_str . substr ( 0 , 3 ) + " : " + utc_diff_str . substr ( 3 , 2 ) + " ) " ;
}
2019-08-01 16:11:36 +00:00
d_waiting_obs_block_rx_clock_offset_correction_msg = false ;
2019-09-06 16:02:40 +00:00
d_enable_rx_clock_correction = conf_ . enable_rx_clock_correction ;
if ( d_enable_rx_clock_correction = = true )
{
2019-09-06 16:39:03 +00:00
// setup two PVT solvers: internal solver for rx clock and user solver
2019-09-06 16:02:40 +00:00
// user PVT solver
2020-07-19 07:39:32 +00:00
d_user_pvt_solver = std : : make_shared < Rtklib_Solver > ( rtk , static_cast < int32_t > ( nchannels ) , dump_ls_pvt_filename , d_dump , d_dump_mat ) ;
2019-09-06 16:02:40 +00:00
d_user_pvt_solver - > set_averaging_depth ( 1 ) ;
2019-09-18 16:25:07 +00:00
d_user_pvt_solver - > set_pre_2009_file ( conf_ . pre_2009_file ) ;
2019-09-06 16:02:40 +00:00
// internal PVT solver, mainly used to estimate the receiver clock
rtk_t internal_rtk = rtk ;
internal_rtk . opt . mode = PMODE_SINGLE ; // use single positioning mode in internal PVT solver
2020-07-19 07:39:32 +00:00
d_internal_pvt_solver = std : : make_shared < Rtklib_Solver > ( internal_rtk , static_cast < int32_t > ( nchannels ) , dump_ls_pvt_filename , false , false ) ;
2019-09-06 16:02:40 +00:00
d_internal_pvt_solver - > set_averaging_depth ( 1 ) ;
2019-09-18 16:25:07 +00:00
d_internal_pvt_solver - > set_pre_2009_file ( conf_ . pre_2009_file ) ;
2019-09-06 16:02:40 +00:00
}
else
{
2019-09-06 16:39:03 +00:00
// only one solver, customized by the user options
2020-07-19 07:39:32 +00:00
d_internal_pvt_solver = std : : make_shared < Rtklib_Solver > ( rtk , static_cast < int32_t > ( nchannels ) , dump_ls_pvt_filename , d_dump , d_dump_mat ) ;
2019-09-06 16:02:40 +00:00
d_internal_pvt_solver - > set_averaging_depth ( 1 ) ;
2019-09-18 16:25:07 +00:00
d_internal_pvt_solver - > set_pre_2009_file ( conf_ . pre_2009_file ) ;
2019-09-06 16:02:40 +00:00
d_user_pvt_solver = d_internal_pvt_solver ;
}
2020-07-19 07:39:32 +00:00
d_gps_ephemeris_sptr_type_hash_code = typeid ( std : : shared_ptr < Gps_Ephemeris > ) . hash_code ( ) ;
d_gps_iono_sptr_type_hash_code = typeid ( std : : shared_ptr < Gps_Iono > ) . hash_code ( ) ;
d_gps_utc_model_sptr_type_hash_code = typeid ( std : : shared_ptr < Gps_Utc_Model > ) . hash_code ( ) ;
d_gps_cnav_ephemeris_sptr_type_hash_code = typeid ( std : : shared_ptr < Gps_CNAV_Ephemeris > ) . hash_code ( ) ;
d_gps_cnav_iono_sptr_type_hash_code = typeid ( std : : shared_ptr < Gps_CNAV_Iono > ) . hash_code ( ) ;
d_gps_cnav_utc_model_sptr_type_hash_code = typeid ( std : : shared_ptr < Gps_CNAV_Utc_Model > ) . hash_code ( ) ;
d_gps_almanac_sptr_type_hash_code = typeid ( std : : shared_ptr < Gps_Almanac > ) . hash_code ( ) ;
d_galileo_ephemeris_sptr_type_hash_code = typeid ( std : : shared_ptr < Galileo_Ephemeris > ) . hash_code ( ) ;
d_galileo_iono_sptr_type_hash_code = typeid ( std : : shared_ptr < Galileo_Iono > ) . hash_code ( ) ;
d_galileo_utc_model_sptr_type_hash_code = typeid ( std : : shared_ptr < Galileo_Utc_Model > ) . hash_code ( ) ;
d_galileo_almanac_helper_sptr_type_hash_code = typeid ( std : : shared_ptr < Galileo_Almanac_Helper > ) . hash_code ( ) ;
d_galileo_almanac_sptr_type_hash_code = typeid ( std : : shared_ptr < Galileo_Almanac > ) . hash_code ( ) ;
d_glonass_gnav_ephemeris_sptr_type_hash_code = typeid ( std : : shared_ptr < Glonass_Gnav_Ephemeris > ) . hash_code ( ) ;
d_glonass_gnav_utc_model_sptr_type_hash_code = typeid ( std : : shared_ptr < Glonass_Gnav_Utc_Model > ) . hash_code ( ) ;
d_glonass_gnav_almanac_sptr_type_hash_code = typeid ( std : : shared_ptr < Glonass_Gnav_Almanac > ) . hash_code ( ) ;
d_beidou_dnav_ephemeris_sptr_type_hash_code = typeid ( std : : shared_ptr < Beidou_Dnav_Ephemeris > ) . hash_code ( ) ;
d_beidou_dnav_iono_sptr_type_hash_code = typeid ( std : : shared_ptr < Beidou_Dnav_Iono > ) . hash_code ( ) ;
d_beidou_dnav_utc_model_sptr_type_hash_code = typeid ( std : : shared_ptr < Beidou_Dnav_Utc_Model > ) . hash_code ( ) ;
d_beidou_dnav_almanac_sptr_type_hash_code = typeid ( std : : shared_ptr < Beidou_Dnav_Almanac > ) . hash_code ( ) ;
2020-06-18 23:36:36 +00:00
d_start = std : : chrono : : system_clock : : now ( ) ;
2017-04-20 14:10:12 +00:00
}
2019-03-02 01:21:03 +00:00
rtklib_pvt_gs : : ~ rtklib_pvt_gs ( )
2017-04-20 14:10:12 +00:00
{
2020-06-25 09:58:01 +00:00
DLOG ( INFO ) < < " PVT block destructor called. " ;
2020-06-18 23:36:36 +00:00
if ( d_sysv_msqid ! = - 1 )
2020-03-10 18:56:13 +00:00
{
2020-06-18 23:36:36 +00:00
msgctl ( d_sysv_msqid , IPC_RMID , nullptr ) ;
2020-03-10 18:56:13 +00:00
}
2019-02-10 20:55:51 +00:00
try
2017-04-20 14:10:12 +00:00
{
2019-02-10 20:55:51 +00:00
if ( d_xml_storage )
2018-03-03 01:03:39 +00:00
{
2019-02-10 20:55:51 +00:00
// save GPS L2CM ephemeris to XML file
2020-06-18 23:36:36 +00:00
std : : string file_name = d_xml_base_path + " gps_cnav_ephemeris.xml " ;
2019-08-01 16:11:36 +00:00
if ( d_internal_pvt_solver - > gps_cnav_ephemeris_map . empty ( ) = = false )
2018-10-27 22:42:28 +00:00
{
2019-02-10 20:55:51 +00:00
std : : ofstream ofs ;
try
{
ofs . open ( file_name . c_str ( ) , std : : ofstream : : trunc | std : : ofstream : : out ) ;
boost : : archive : : xml_oarchive xml ( ofs ) ;
2019-08-01 16:11:36 +00:00
xml < < boost : : serialization : : make_nvp ( " GNSS-SDR_cnav_ephemeris_map " , d_internal_pvt_solver - > gps_cnav_ephemeris_map ) ;
2019-02-10 20:55:51 +00:00
LOG ( INFO ) < < " Saved GPS L2CM or L5 Ephemeris map data " ;
}
2019-02-11 21:00:48 +00:00
catch ( const boost : : archive : : archive_exception & e )
2019-02-10 20:55:51 +00:00
{
LOG ( WARNING ) < < e . what ( ) ;
}
2019-02-14 11:51:43 +00:00
catch ( const std : : exception & e )
2019-02-10 20:55:51 +00:00
{
LOG ( WARNING ) < < e . what ( ) ;
}
2018-10-27 22:42:28 +00:00
}
2019-02-10 20:55:51 +00:00
else
2018-10-27 22:42:28 +00:00
{
2019-02-10 20:55:51 +00:00
LOG ( INFO ) < < " Failed to save GPS L2CM or L5 Ephemeris, map is empty " ;
2018-10-27 22:42:28 +00:00
}
2017-04-20 14:10:12 +00:00
2019-02-10 20:55:51 +00:00
// save GPS L1 CA ephemeris to XML file
2020-06-18 23:36:36 +00:00
file_name = d_xml_base_path + " gps_ephemeris.xml " ;
2019-08-01 16:11:36 +00:00
if ( d_internal_pvt_solver - > gps_ephemeris_map . empty ( ) = = false )
2018-10-27 22:42:28 +00:00
{
2019-02-10 20:55:51 +00:00
std : : ofstream ofs ;
try
{
ofs . open ( file_name . c_str ( ) , std : : ofstream : : trunc | std : : ofstream : : out ) ;
boost : : archive : : xml_oarchive xml ( ofs ) ;
2019-08-01 16:11:36 +00:00
xml < < boost : : serialization : : make_nvp ( " GNSS-SDR_ephemeris_map " , d_internal_pvt_solver - > gps_ephemeris_map ) ;
2019-02-10 20:55:51 +00:00
LOG ( INFO ) < < " Saved GPS L1 CA Ephemeris map data " ;
}
2019-02-11 21:00:48 +00:00
catch ( const boost : : archive : : archive_exception & e )
2019-02-10 20:55:51 +00:00
{
LOG ( WARNING ) < < e . what ( ) ;
}
catch ( const std : : exception & e )
{
LOG ( WARNING ) < < e . what ( ) ;
}
2018-10-27 22:42:28 +00:00
}
2019-02-10 20:55:51 +00:00
else
2018-10-27 22:42:28 +00:00
{
2019-02-10 20:55:51 +00:00
LOG ( INFO ) < < " Failed to save GPS L1 CA Ephemeris, map is empty " ;
2018-10-27 22:42:28 +00:00
}
2017-04-20 14:10:12 +00:00
2019-02-10 20:55:51 +00:00
// save Galileo E1 ephemeris to XML file
2020-06-18 23:36:36 +00:00
file_name = d_xml_base_path + " gal_ephemeris.xml " ;
2019-08-01 16:11:36 +00:00
if ( d_internal_pvt_solver - > galileo_ephemeris_map . empty ( ) = = false )
2018-10-27 22:42:28 +00:00
{
2019-02-10 20:55:51 +00:00
std : : ofstream ofs ;
try
{
ofs . open ( file_name . c_str ( ) , std : : ofstream : : trunc | std : : ofstream : : out ) ;
boost : : archive : : xml_oarchive xml ( ofs ) ;
2019-08-01 16:11:36 +00:00
xml < < boost : : serialization : : make_nvp ( " GNSS-SDR_gal_ephemeris_map " , d_internal_pvt_solver - > galileo_ephemeris_map ) ;
2019-02-10 20:55:51 +00:00
LOG ( INFO ) < < " Saved Galileo E1 Ephemeris map data " ;
}
2019-02-11 21:00:48 +00:00
catch ( const boost : : archive : : archive_exception & e )
2019-02-10 20:55:51 +00:00
{
LOG ( WARNING ) < < e . what ( ) ;
}
catch ( const std : : ofstream : : failure & e )
{
LOG ( WARNING ) < < " Problem opening output XML file " ;
}
catch ( const std : : exception & e )
{
LOG ( WARNING ) < < e . what ( ) ;
}
2018-10-27 22:42:28 +00:00
}
2019-02-10 20:55:51 +00:00
else
2018-10-27 22:42:28 +00:00
{
2019-02-10 20:55:51 +00:00
LOG ( INFO ) < < " Failed to save Galileo E1 Ephemeris, map is empty " ;
2018-10-27 22:42:28 +00:00
}
2017-04-20 14:10:12 +00:00
2019-02-10 20:55:51 +00:00
// save GLONASS GNAV ephemeris to XML file
2020-06-18 23:36:36 +00:00
file_name = d_xml_base_path + " eph_GLONASS_GNAV.xml " ;
2019-08-01 16:11:36 +00:00
if ( d_internal_pvt_solver - > glonass_gnav_ephemeris_map . empty ( ) = = false )
2018-10-27 22:42:28 +00:00
{
2019-02-10 20:55:51 +00:00
std : : ofstream ofs ;
try
{
ofs . open ( file_name . c_str ( ) , std : : ofstream : : trunc | std : : ofstream : : out ) ;
boost : : archive : : xml_oarchive xml ( ofs ) ;
2019-08-01 16:11:36 +00:00
xml < < boost : : serialization : : make_nvp ( " GNSS-SDR_gnav_ephemeris_map " , d_internal_pvt_solver - > glonass_gnav_ephemeris_map ) ;
2019-02-10 20:55:51 +00:00
LOG ( INFO ) < < " Saved GLONASS GNAV Ephemeris map data " ;
}
2019-02-11 21:00:48 +00:00
catch ( const boost : : archive : : archive_exception & e )
2019-02-10 20:55:51 +00:00
{
LOG ( WARNING ) < < e . what ( ) ;
}
catch ( const std : : ofstream : : failure & e )
{
LOG ( WARNING ) < < " Problem opening output XML file " ;
}
2019-02-14 11:51:43 +00:00
catch ( const std : : exception & e )
2019-02-10 20:55:51 +00:00
{
LOG ( WARNING ) < < e . what ( ) ;
}
2018-10-27 22:42:28 +00:00
}
2019-02-10 20:55:51 +00:00
else
2018-10-27 22:42:28 +00:00
{
2019-02-10 20:55:51 +00:00
LOG ( INFO ) < < " Failed to save GLONASS GNAV Ephemeris, map is empty " ;
2018-10-27 22:42:28 +00:00
}
2018-08-09 18:36:11 +00:00
2019-02-10 20:55:51 +00:00
// Save GPS UTC model parameters
2020-06-18 23:36:36 +00:00
file_name = d_xml_base_path + " gps_utc_model.xml " ;
2019-08-01 16:11:36 +00:00
if ( d_internal_pvt_solver - > gps_utc_model . valid )
2018-10-27 22:42:28 +00:00
{
2019-02-10 20:55:51 +00:00
std : : ofstream ofs ;
try
{
ofs . open ( file_name . c_str ( ) , std : : ofstream : : trunc | std : : ofstream : : out ) ;
boost : : archive : : xml_oarchive xml ( ofs ) ;
2019-08-01 16:11:36 +00:00
xml < < boost : : serialization : : make_nvp ( " GNSS-SDR_utc_model " , d_internal_pvt_solver - > gps_utc_model ) ;
2019-02-10 20:55:51 +00:00
LOG ( INFO ) < < " Saved GPS UTC model parameters " ;
}
catch ( const std : : ofstream : : failure & e )
{
LOG ( WARNING ) < < " Problem opening output XML file " ;
}
2019-02-11 21:00:48 +00:00
catch ( const boost : : archive : : archive_exception & e )
2019-02-10 20:55:51 +00:00
{
LOG ( WARNING ) < < e . what ( ) ;
}
2019-02-14 11:51:43 +00:00
catch ( const std : : exception & e )
2019-02-10 20:55:51 +00:00
{
LOG ( WARNING ) < < e . what ( ) ;
}
2018-10-27 22:42:28 +00:00
}
2019-02-10 20:55:51 +00:00
else
2018-10-27 22:42:28 +00:00
{
2019-02-10 20:55:51 +00:00
LOG ( INFO ) < < " Failed to save GPS UTC model parameters, not valid data " ;
2018-10-27 22:42:28 +00:00
}
2018-10-19 11:48:21 +00:00
2019-02-10 20:55:51 +00:00
// Save Galileo UTC model parameters
2020-06-18 23:36:36 +00:00
file_name = d_xml_base_path + " gal_utc_model.xml " ;
2019-08-01 16:11:36 +00:00
if ( d_internal_pvt_solver - > galileo_utc_model . Delta_tLS_6 ! = 0.0 )
2018-10-27 22:42:28 +00:00
{
2019-02-10 20:55:51 +00:00
std : : ofstream ofs ;
try
{
ofs . open ( file_name . c_str ( ) , std : : ofstream : : trunc | std : : ofstream : : out ) ;
boost : : archive : : xml_oarchive xml ( ofs ) ;
2019-08-01 16:11:36 +00:00
xml < < boost : : serialization : : make_nvp ( " GNSS-SDR_gal_utc_model " , d_internal_pvt_solver - > galileo_utc_model ) ;
2019-02-10 20:55:51 +00:00
LOG ( INFO ) < < " Saved Galileo UTC model parameters " ;
}
2019-02-11 21:00:48 +00:00
catch ( const boost : : archive : : archive_exception & e )
2019-02-10 20:55:51 +00:00
{
LOG ( WARNING ) < < e . what ( ) ;
}
catch ( const std : : ofstream : : failure & e )
{
LOG ( WARNING ) < < " Problem opening output XML file " ;
}
2019-02-14 11:51:43 +00:00
catch ( const std : : exception & e )
2019-02-10 20:55:51 +00:00
{
LOG ( WARNING ) < < e . what ( ) ;
}
2018-10-27 22:42:28 +00:00
}
2019-02-10 20:55:51 +00:00
else
2018-10-27 22:42:28 +00:00
{
2019-02-10 20:55:51 +00:00
LOG ( INFO ) < < " Failed to save Galileo UTC model parameters, not valid data " ;
2018-10-27 22:42:28 +00:00
}
2018-10-19 11:48:21 +00:00
2019-02-10 20:55:51 +00:00
// Save GPS iono parameters
2020-06-18 23:36:36 +00:00
file_name = d_xml_base_path + " gps_iono.xml " ;
2019-08-01 16:11:36 +00:00
if ( d_internal_pvt_solver - > gps_iono . valid = = true )
2018-10-27 22:42:28 +00:00
{
2019-02-10 20:55:51 +00:00
std : : ofstream ofs ;
try
{
ofs . open ( file_name . c_str ( ) , std : : ofstream : : trunc | std : : ofstream : : out ) ;
boost : : archive : : xml_oarchive xml ( ofs ) ;
2019-08-01 16:11:36 +00:00
xml < < boost : : serialization : : make_nvp ( " GNSS-SDR_iono_model " , d_internal_pvt_solver - > gps_iono ) ;
2019-02-10 20:55:51 +00:00
LOG ( INFO ) < < " Saved GPS ionospheric model parameters " ;
}
2019-02-11 21:00:48 +00:00
catch ( const boost : : archive : : archive_exception & e )
2019-02-10 20:55:51 +00:00
{
LOG ( WARNING ) < < e . what ( ) ;
}
catch ( const std : : ofstream : : failure & e )
{
LOG ( WARNING ) < < " Problem opening output XML file " ;
}
2019-02-14 11:51:43 +00:00
catch ( const std : : exception & e )
2019-02-10 20:55:51 +00:00
{
LOG ( WARNING ) < < e . what ( ) ;
}
2018-10-27 22:42:28 +00:00
}
2019-02-10 20:55:51 +00:00
else
2018-10-27 22:42:28 +00:00
{
2019-02-10 20:55:51 +00:00
LOG ( INFO ) < < " Failed to save GPS ionospheric model parameters, not valid data " ;
2018-10-27 22:42:28 +00:00
}
2018-10-19 12:48:41 +00:00
2019-02-10 20:55:51 +00:00
// Save GPS CNAV iono parameters
2020-06-18 23:36:36 +00:00
file_name = d_xml_base_path + " gps_cnav_iono.xml " ;
2019-08-01 16:11:36 +00:00
if ( d_internal_pvt_solver - > gps_cnav_iono . valid = = true )
2018-10-27 22:42:28 +00:00
{
2019-02-10 20:55:51 +00:00
std : : ofstream ofs ;
try
{
ofs . open ( file_name . c_str ( ) , std : : ofstream : : trunc | std : : ofstream : : out ) ;
boost : : archive : : xml_oarchive xml ( ofs ) ;
2019-08-01 16:11:36 +00:00
xml < < boost : : serialization : : make_nvp ( " GNSS-SDR_cnav_iono_model " , d_internal_pvt_solver - > gps_cnav_iono ) ;
2019-02-10 20:55:51 +00:00
LOG ( INFO ) < < " Saved GPS CNAV ionospheric model parameters " ;
}
2019-02-11 21:00:48 +00:00
catch ( const boost : : archive : : archive_exception & e )
2019-02-10 20:55:51 +00:00
{
LOG ( WARNING ) < < e . what ( ) ;
}
catch ( const std : : ofstream : : failure & e )
{
LOG ( WARNING ) < < " Problem opening output XML file " ;
}
2019-02-14 11:51:43 +00:00
catch ( const std : : exception & e )
2019-02-10 20:55:51 +00:00
{
LOG ( WARNING ) < < e . what ( ) ;
}
2018-10-27 22:42:28 +00:00
}
2019-02-10 20:55:51 +00:00
else
2018-10-27 22:42:28 +00:00
{
2019-02-10 20:55:51 +00:00
LOG ( INFO ) < < " Failed to save GPS CNAV ionospheric model parameters, not valid data " ;
2018-10-27 22:42:28 +00:00
}
2018-10-20 17:30:32 +00:00
2019-02-10 20:55:51 +00:00
// Save Galileo iono parameters
2020-06-18 23:36:36 +00:00
file_name = d_xml_base_path + " gal_iono.xml " ;
2019-08-01 16:11:36 +00:00
if ( d_internal_pvt_solver - > galileo_iono . ai0_5 ! = 0.0 )
2018-10-27 22:42:28 +00:00
{
2019-02-10 20:55:51 +00:00
std : : ofstream ofs ;
try
{
ofs . open ( file_name . c_str ( ) , std : : ofstream : : trunc | std : : ofstream : : out ) ;
boost : : archive : : xml_oarchive xml ( ofs ) ;
2019-08-01 16:11:36 +00:00
xml < < boost : : serialization : : make_nvp ( " GNSS-SDR_gal_iono_model " , d_internal_pvt_solver - > galileo_iono ) ;
2019-02-10 20:55:51 +00:00
LOG ( INFO ) < < " Saved Galileo ionospheric model parameters " ;
}
2019-02-11 21:00:48 +00:00
catch ( const boost : : archive : : archive_exception & e )
2019-02-10 20:55:51 +00:00
{
LOG ( WARNING ) < < e . what ( ) ;
}
catch ( const std : : ofstream : : failure & e )
{
LOG ( WARNING ) < < " Problem opening output XML file " ;
}
2019-02-14 11:51:43 +00:00
catch ( const std : : exception & e )
2019-02-10 20:55:51 +00:00
{
LOG ( WARNING ) < < e . what ( ) ;
}
2018-10-27 22:42:28 +00:00
}
2019-02-10 20:55:51 +00:00
else
2018-10-27 22:42:28 +00:00
{
2019-02-10 20:55:51 +00:00
LOG ( INFO ) < < " Failed to save Galileo ionospheric model parameters, not valid data " ;
2018-10-27 22:42:28 +00:00
}
2018-10-20 17:30:32 +00:00
2019-02-10 20:55:51 +00:00
// save GPS almanac to XML file
2020-06-18 23:36:36 +00:00
file_name = d_xml_base_path + " gps_almanac.xml " ;
2019-08-01 16:11:36 +00:00
if ( d_internal_pvt_solver - > gps_almanac_map . empty ( ) = = false )
2018-10-27 22:42:28 +00:00
{
2019-02-10 20:55:51 +00:00
std : : ofstream ofs ;
try
{
ofs . open ( file_name . c_str ( ) , std : : ofstream : : trunc | std : : ofstream : : out ) ;
boost : : archive : : xml_oarchive xml ( ofs ) ;
2019-08-01 16:11:36 +00:00
xml < < boost : : serialization : : make_nvp ( " GNSS-SDR_gps_almanac_map " , d_internal_pvt_solver - > gps_almanac_map ) ;
2019-02-10 20:55:51 +00:00
LOG ( INFO ) < < " Saved GPS almanac map data " ;
}
2019-02-11 21:00:48 +00:00
catch ( const boost : : archive : : archive_exception & e )
2019-02-10 20:55:51 +00:00
{
LOG ( WARNING ) < < e . what ( ) ;
}
catch ( const std : : ofstream : : failure & e )
{
LOG ( WARNING ) < < " Problem opening output XML file " ;
}
catch ( const std : : exception & e )
{
LOG ( WARNING ) < < e . what ( ) ;
}
2018-10-27 22:42:28 +00:00
}
2019-02-10 20:55:51 +00:00
else
2018-10-27 22:42:28 +00:00
{
2019-02-10 20:55:51 +00:00
LOG ( INFO ) < < " Failed to save GPS almanac, map is empty " ;
2018-10-27 22:42:28 +00:00
}
2018-10-20 17:30:32 +00:00
2019-02-10 20:55:51 +00:00
// Save Galileo almanac
2020-06-18 23:36:36 +00:00
file_name = d_xml_base_path + " gal_almanac.xml " ;
2019-08-01 16:11:36 +00:00
if ( d_internal_pvt_solver - > galileo_almanac_map . empty ( ) = = false )
2018-10-27 22:42:28 +00:00
{
2019-02-10 20:55:51 +00:00
std : : ofstream ofs ;
try
{
ofs . open ( file_name . c_str ( ) , std : : ofstream : : trunc | std : : ofstream : : out ) ;
boost : : archive : : xml_oarchive xml ( ofs ) ;
2019-08-01 16:11:36 +00:00
xml < < boost : : serialization : : make_nvp ( " GNSS-SDR_gal_almanac_map " , d_internal_pvt_solver - > galileo_almanac_map ) ;
2019-02-10 20:55:51 +00:00
LOG ( INFO ) < < " Saved Galileo almanac data " ;
}
2019-02-11 21:00:48 +00:00
catch ( const boost : : archive : : archive_exception & e )
2019-02-10 20:55:51 +00:00
{
LOG ( WARNING ) < < e . what ( ) ;
}
catch ( const std : : ofstream : : failure & e )
{
LOG ( WARNING ) < < " Problem opening output XML file " ;
}
2019-02-14 11:51:43 +00:00
catch ( const std : : exception & e )
2019-02-10 20:55:51 +00:00
{
LOG ( WARNING ) < < e . what ( ) ;
}
2018-10-27 22:42:28 +00:00
}
2019-02-10 20:55:51 +00:00
else
2018-10-27 22:42:28 +00:00
{
2019-02-10 20:55:51 +00:00
LOG ( INFO ) < < " Failed to save Galileo almanac, not valid data " ;
2018-10-27 22:42:28 +00:00
}
2019-02-10 20:55:51 +00:00
// Save GPS CNAV UTC model parameters
2020-06-18 23:36:36 +00:00
file_name = d_xml_base_path + " gps_cnav_utc_model.xml " ;
2019-08-01 16:11:36 +00:00
if ( d_internal_pvt_solver - > gps_cnav_utc_model . valid )
2018-10-27 22:42:28 +00:00
{
2019-02-10 20:55:51 +00:00
std : : ofstream ofs ;
try
{
ofs . open ( file_name . c_str ( ) , std : : ofstream : : trunc | std : : ofstream : : out ) ;
boost : : archive : : xml_oarchive xml ( ofs ) ;
2019-08-01 16:11:36 +00:00
xml < < boost : : serialization : : make_nvp ( " GNSS-SDR_cnav_utc_model " , d_internal_pvt_solver - > gps_cnav_utc_model ) ;
2019-02-10 20:55:51 +00:00
LOG ( INFO ) < < " Saved GPS CNAV UTC model parameters " ;
}
2019-02-11 21:00:48 +00:00
catch ( const boost : : archive : : archive_exception & e )
2019-02-10 20:55:51 +00:00
{
LOG ( WARNING ) < < e . what ( ) ;
}
catch ( const std : : ofstream : : failure & e )
{
LOG ( WARNING ) < < " Problem opening output XML file " ;
}
2019-02-14 11:51:43 +00:00
catch ( const std : : exception & e )
2019-02-10 20:55:51 +00:00
{
LOG ( WARNING ) < < e . what ( ) ;
}
2018-10-27 22:42:28 +00:00
}
2019-02-10 20:55:51 +00:00
else
2018-10-27 22:42:28 +00:00
{
2019-02-10 20:55:51 +00:00
LOG ( INFO ) < < " Failed to save GPS CNAV UTC model parameters, not valid data " ;
2018-10-27 22:42:28 +00:00
}
2019-02-10 20:55:51 +00:00
// save GLONASS GNAV ephemeris to XML file
2020-06-18 23:36:36 +00:00
file_name = d_xml_base_path + " glo_gnav_ephemeris.xml " ;
2019-08-01 16:11:36 +00:00
if ( d_internal_pvt_solver - > glonass_gnav_ephemeris_map . empty ( ) = = false )
2018-10-27 22:42:28 +00:00
{
2019-02-10 20:55:51 +00:00
std : : ofstream ofs ;
try
{
ofs . open ( file_name . c_str ( ) , std : : ofstream : : trunc | std : : ofstream : : out ) ;
boost : : archive : : xml_oarchive xml ( ofs ) ;
2019-08-01 16:11:36 +00:00
xml < < boost : : serialization : : make_nvp ( " GNSS-SDR_gnav_ephemeris_map " , d_internal_pvt_solver - > glonass_gnav_ephemeris_map ) ;
2019-02-10 20:55:51 +00:00
LOG ( INFO ) < < " Saved GLONASS GNAV ephemeris map data " ;
}
2019-02-11 21:00:48 +00:00
catch ( const boost : : archive : : archive_exception & e )
2019-02-10 20:55:51 +00:00
{
LOG ( WARNING ) < < e . what ( ) ;
}
catch ( const std : : ofstream : : failure & e )
{
LOG ( WARNING ) < < " Problem opening output XML file " ;
}
2019-02-14 11:51:43 +00:00
catch ( const std : : exception & e )
2019-02-10 20:55:51 +00:00
{
LOG ( WARNING ) < < e . what ( ) ;
}
2018-10-27 22:42:28 +00:00
}
2019-02-10 20:55:51 +00:00
else
2018-10-27 22:42:28 +00:00
{
2019-02-10 20:55:51 +00:00
LOG ( INFO ) < < " Failed to save GLONASS GNAV ephemeris, map is empty " ;
2018-10-27 22:42:28 +00:00
}
2019-02-10 20:55:51 +00:00
// save GLONASS UTC model parameters to XML file
2020-06-18 23:36:36 +00:00
file_name = d_xml_base_path + " glo_utc_model.xml " ;
2019-08-01 16:11:36 +00:00
if ( d_internal_pvt_solver - > glonass_gnav_utc_model . valid )
2018-10-27 22:42:28 +00:00
{
2019-02-10 20:55:51 +00:00
std : : ofstream ofs ;
try
{
ofs . open ( file_name . c_str ( ) , std : : ofstream : : trunc | std : : ofstream : : out ) ;
boost : : archive : : xml_oarchive xml ( ofs ) ;
2019-08-01 16:11:36 +00:00
xml < < boost : : serialization : : make_nvp ( " GNSS-SDR_gnav_utc_model " , d_internal_pvt_solver - > glonass_gnav_utc_model ) ;
2019-02-10 20:55:51 +00:00
LOG ( INFO ) < < " Saved GLONASS UTC model parameters " ;
}
2019-02-11 21:00:48 +00:00
catch ( const boost : : archive : : archive_exception & e )
2019-02-10 20:55:51 +00:00
{
LOG ( WARNING ) < < e . what ( ) ;
}
catch ( const std : : ofstream : : failure & e )
{
LOG ( WARNING ) < < " Problem opening output XML file " ;
}
2019-02-14 11:51:43 +00:00
catch ( const std : : exception & e )
2019-02-10 20:55:51 +00:00
{
LOG ( WARNING ) < < e . what ( ) ;
}
2018-10-27 22:42:28 +00:00
}
2019-02-10 20:55:51 +00:00
else
2018-10-27 22:42:28 +00:00
{
2019-02-10 20:55:51 +00:00
LOG ( INFO ) < < " Failed to save GLONASS GNAV ephemeris, not valid data " ;
2018-10-27 22:42:28 +00:00
}
2018-11-29 15:53:40 +00:00
2019-02-10 20:55:51 +00:00
// save BeiDou DNAV ephemeris to XML file
2020-06-18 23:36:36 +00:00
file_name = d_xml_base_path + " bds_dnav_ephemeris.xml " ;
2019-08-01 16:11:36 +00:00
if ( d_internal_pvt_solver - > beidou_dnav_ephemeris_map . empty ( ) = = false )
2018-11-29 15:53:40 +00:00
{
2019-02-10 20:55:51 +00:00
std : : ofstream ofs ;
try
{
ofs . open ( file_name . c_str ( ) , std : : ofstream : : trunc | std : : ofstream : : out ) ;
boost : : archive : : xml_oarchive xml ( ofs ) ;
2019-08-01 16:11:36 +00:00
xml < < boost : : serialization : : make_nvp ( " GNSS-SDR_bds_dnav_ephemeris_map " , d_internal_pvt_solver - > beidou_dnav_ephemeris_map ) ;
2019-02-10 20:55:51 +00:00
LOG ( INFO ) < < " Saved BeiDou DNAV Ephemeris map data " ;
}
2019-02-11 21:00:48 +00:00
catch ( const boost : : archive : : archive_exception & e )
2019-02-10 20:55:51 +00:00
{
LOG ( WARNING ) < < e . what ( ) ;
}
catch ( const std : : ofstream : : failure & e )
{
LOG ( WARNING ) < < " Problem opening output XML file " ;
}
catch ( const std : : exception & e )
{
LOG ( WARNING ) < < e . what ( ) ;
}
2018-10-27 22:42:28 +00:00
}
2019-02-10 20:55:51 +00:00
else
2018-10-27 22:42:28 +00:00
{
2019-02-10 20:55:51 +00:00
LOG ( INFO ) < < " Failed to save BeiDou DNAV Ephemeris, map is empty " ;
2018-11-29 15:53:40 +00:00
}
2019-02-10 20:55:51 +00:00
// Save BeiDou DNAV iono parameters
2020-06-18 23:36:36 +00:00
file_name = d_xml_base_path + " bds_dnav_iono.xml " ;
2019-08-01 16:11:36 +00:00
if ( d_internal_pvt_solver - > beidou_dnav_iono . valid )
2018-11-29 15:53:40 +00:00
{
2019-02-10 20:55:51 +00:00
std : : ofstream ofs ;
try
{
ofs . open ( file_name . c_str ( ) , std : : ofstream : : trunc | std : : ofstream : : out ) ;
boost : : archive : : xml_oarchive xml ( ofs ) ;
2019-08-01 16:11:36 +00:00
xml < < boost : : serialization : : make_nvp ( " GNSS-SDR_bds_dnav_iono_model " , d_internal_pvt_solver - > beidou_dnav_iono ) ;
2019-02-10 20:55:51 +00:00
LOG ( INFO ) < < " Saved BeiDou DNAV ionospheric model parameters " ;
}
2019-02-11 21:00:48 +00:00
catch ( const boost : : archive : : archive_exception & e )
2019-02-10 20:55:51 +00:00
{
LOG ( WARNING ) < < e . what ( ) ;
}
catch ( const std : : ofstream : : failure & e )
{
LOG ( WARNING ) < < " Problem opening output XML file " ;
}
2019-02-14 11:51:43 +00:00
catch ( const std : : exception & e )
2019-02-10 20:55:51 +00:00
{
LOG ( WARNING ) < < e . what ( ) ;
}
2018-11-29 15:53:40 +00:00
}
2019-02-10 20:55:51 +00:00
else
2018-11-29 15:53:40 +00:00
{
2019-02-10 20:55:51 +00:00
LOG ( INFO ) < < " Failed to save BeiDou DNAV ionospheric model parameters, not valid data " ;
2018-11-29 15:53:40 +00:00
}
2019-02-10 20:55:51 +00:00
// save BeiDou DNAV almanac to XML file
2020-06-18 23:36:36 +00:00
file_name = d_xml_base_path + " bds_dnav_almanac.xml " ;
2019-08-01 16:11:36 +00:00
if ( d_internal_pvt_solver - > beidou_dnav_almanac_map . empty ( ) = = false )
2018-11-29 15:53:40 +00:00
{
2019-02-10 20:55:51 +00:00
std : : ofstream ofs ;
try
{
ofs . open ( file_name . c_str ( ) , std : : ofstream : : trunc | std : : ofstream : : out ) ;
boost : : archive : : xml_oarchive xml ( ofs ) ;
2019-08-01 16:11:36 +00:00
xml < < boost : : serialization : : make_nvp ( " GNSS-SDR_bds_dnav_almanac_map " , d_internal_pvt_solver - > beidou_dnav_almanac_map ) ;
2019-02-10 20:55:51 +00:00
LOG ( INFO ) < < " Saved BeiDou DNAV almanac map data " ;
}
2019-02-11 21:00:48 +00:00
catch ( const boost : : archive : : archive_exception & e )
2019-02-10 20:55:51 +00:00
{
LOG ( WARNING ) < < e . what ( ) ;
}
catch ( const std : : ofstream : : failure & e )
{
LOG ( WARNING ) < < " Problem opening output XML file " ;
}
catch ( const std : : exception & e )
{
LOG ( WARNING ) < < e . what ( ) ;
}
2018-11-29 15:53:40 +00:00
}
2019-02-10 20:55:51 +00:00
else
2018-11-29 15:53:40 +00:00
{
2019-02-10 20:55:51 +00:00
LOG ( INFO ) < < " Failed to save BeiDou DNAV almanac, map is empty " ;
2018-11-29 15:53:40 +00:00
}
2019-02-10 20:55:51 +00:00
// Save BeiDou UTC model parameters
2020-06-18 23:36:36 +00:00
file_name = d_xml_base_path + " bds_dnav_utc_model.xml " ;
2019-08-01 16:11:36 +00:00
if ( d_internal_pvt_solver - > beidou_dnav_utc_model . valid )
2018-11-29 15:53:40 +00:00
{
2019-02-10 20:55:51 +00:00
std : : ofstream ofs ;
try
{
ofs . open ( file_name . c_str ( ) , std : : ofstream : : trunc | std : : ofstream : : out ) ;
boost : : archive : : xml_oarchive xml ( ofs ) ;
2019-08-01 16:11:36 +00:00
xml < < boost : : serialization : : make_nvp ( " GNSS-SDR_bds_dnav_utc_model " , d_internal_pvt_solver - > beidou_dnav_utc_model ) ;
2019-02-10 20:55:51 +00:00
LOG ( INFO ) < < " Saved BeiDou DNAV UTC model parameters " ;
}
2019-02-11 21:00:48 +00:00
catch ( const boost : : archive : : archive_exception & e )
2019-02-10 20:55:51 +00:00
{
LOG ( WARNING ) < < e . what ( ) ;
}
catch ( const std : : ofstream : : failure & e )
{
LOG ( WARNING ) < < " Problem opening output XML file " ;
}
2019-02-14 11:51:43 +00:00
catch ( const std : : exception & e )
2019-02-10 20:55:51 +00:00
{
LOG ( WARNING ) < < e . what ( ) ;
}
2018-11-29 15:53:40 +00:00
}
2019-02-10 20:55:51 +00:00
else
2018-11-29 15:53:40 +00:00
{
2019-02-10 20:55:51 +00:00
LOG ( INFO ) < < " Failed to save BeiDou DNAV UTC model parameters, not valid data " ;
2018-10-27 22:42:28 +00:00
}
2018-10-20 17:30:32 +00:00
}
2019-02-10 20:55:51 +00:00
}
catch ( std : : length_error & e )
{
LOG ( WARNING ) < < e . what ( ) ;
2018-10-20 17:30:32 +00:00
}
2017-04-20 14:10:12 +00:00
}
2019-03-02 01:21:03 +00:00
void rtklib_pvt_gs : : msg_handler_telemetry ( const pmt : : pmt_t & msg )
2019-02-26 00:38:48 +00:00
{
try
{
2020-07-19 07:39:32 +00:00
const size_t msg_type_hash_code = pmt : : any_ref ( msg ) . type ( ) . hash_code ( ) ;
// ************************* GPS telemetry *************************
if ( msg_type_hash_code = = d_gps_ephemeris_sptr_type_hash_code )
2019-02-26 00:38:48 +00:00
{
// ### GPS EPHEMERIS ###
2020-08-09 12:03:53 +00:00
const auto gps_eph = boost : : any_cast < std : : shared_ptr < Gps_Ephemeris > > ( pmt : : any_ref ( msg ) ) ;
2019-02-26 00:38:48 +00:00
DLOG ( INFO ) < < " Ephemeris record has arrived from SAT ID "
< < gps_eph - > i_satellite_PRN < < " (Block "
< < gps_eph - > satelliteBlock [ gps_eph - > i_satellite_PRN ] < < " ) "
< < " inserted with Toe= " < < gps_eph - > d_Toe < < " and GPS Week= "
< < gps_eph - > i_GPS_week ;
// update/insert new ephemeris record to the global ephemeris map
2020-11-14 13:44:43 +00:00
if ( d_rp - > is_rinex_header_written ( ) ) // The header is already written, we can now log the navigation message data
2019-05-07 14:39:06 +00:00
{
bool new_annotation = false ;
2019-08-01 16:11:36 +00:00
if ( d_internal_pvt_solver - > gps_ephemeris_map . find ( gps_eph - > i_satellite_PRN ) = = d_internal_pvt_solver - > gps_ephemeris_map . cend ( ) )
2019-05-07 14:39:06 +00:00
{
new_annotation = true ;
}
else
{
2019-08-01 16:11:36 +00:00
if ( d_internal_pvt_solver - > gps_ephemeris_map [ gps_eph - > i_satellite_PRN ] . d_Toe ! = gps_eph - > d_Toe )
2019-05-07 14:39:06 +00:00
{
new_annotation = true ;
}
}
if ( new_annotation = = true )
{
// New record!
std : : map < int32_t , Gps_Ephemeris > new_eph ;
new_eph [ gps_eph - > i_satellite_PRN ] = * gps_eph ;
2020-11-14 00:57:29 +00:00
d_rp - > log_rinex_nav_gps_nav ( d_type_of_rx , new_eph ) ;
2019-05-07 14:39:06 +00:00
}
}
2019-08-01 16:11:36 +00:00
d_internal_pvt_solver - > gps_ephemeris_map [ gps_eph - > i_satellite_PRN ] = * gps_eph ;
2019-09-06 16:02:40 +00:00
if ( d_enable_rx_clock_correction = = true )
{
d_user_pvt_solver - > gps_ephemeris_map [ gps_eph - > i_satellite_PRN ] = * gps_eph ;
}
2019-02-26 00:38:48 +00:00
}
2020-07-19 07:39:32 +00:00
else if ( msg_type_hash_code = = d_gps_iono_sptr_type_hash_code )
2019-02-26 00:38:48 +00:00
{
// ### GPS IONO ###
2020-08-09 12:03:53 +00:00
const auto gps_iono = boost : : any_cast < std : : shared_ptr < Gps_Iono > > ( pmt : : any_ref ( msg ) ) ;
2019-08-01 16:11:36 +00:00
d_internal_pvt_solver - > gps_iono = * gps_iono ;
2019-09-06 16:02:40 +00:00
if ( d_enable_rx_clock_correction = = true )
{
d_user_pvt_solver - > gps_iono = * gps_iono ;
}
2019-02-26 00:38:48 +00:00
DLOG ( INFO ) < < " New IONO record has arrived " ;
}
2020-07-19 07:39:32 +00:00
else if ( msg_type_hash_code = = d_gps_utc_model_sptr_type_hash_code )
2019-02-26 00:38:48 +00:00
{
// ### GPS UTC MODEL ###
2020-08-09 12:03:53 +00:00
const auto gps_utc_model = boost : : any_cast < std : : shared_ptr < Gps_Utc_Model > > ( pmt : : any_ref ( msg ) ) ;
2019-08-01 16:11:36 +00:00
d_internal_pvt_solver - > gps_utc_model = * gps_utc_model ;
2019-09-06 16:02:40 +00:00
if ( d_enable_rx_clock_correction = = true )
{
d_user_pvt_solver - > gps_utc_model = * gps_utc_model ;
}
2019-02-26 00:38:48 +00:00
DLOG ( INFO ) < < " New UTC record has arrived " ;
}
2020-07-19 07:39:32 +00:00
else if ( msg_type_hash_code = = d_gps_cnav_ephemeris_sptr_type_hash_code )
2019-02-26 00:38:48 +00:00
{
// ### GPS CNAV message ###
2020-08-09 12:03:53 +00:00
const auto gps_cnav_ephemeris = boost : : any_cast < std : : shared_ptr < Gps_CNAV_Ephemeris > > ( pmt : : any_ref ( msg ) ) ;
2019-02-26 00:38:48 +00:00
// update/insert new ephemeris record to the global ephemeris map
2020-11-14 13:44:43 +00:00
if ( d_rp - > is_rinex_header_written ( ) ) // The header is already written, we can now log the navigation message data
2019-05-07 14:39:06 +00:00
{
bool new_annotation = false ;
2019-08-01 16:11:36 +00:00
if ( d_internal_pvt_solver - > gps_cnav_ephemeris_map . find ( gps_cnav_ephemeris - > i_satellite_PRN ) = = d_internal_pvt_solver - > gps_cnav_ephemeris_map . cend ( ) )
2019-05-07 14:39:06 +00:00
{
new_annotation = true ;
}
else
{
2019-08-01 16:11:36 +00:00
if ( d_internal_pvt_solver - > gps_cnav_ephemeris_map [ gps_cnav_ephemeris - > i_satellite_PRN ] . d_Toe1 ! = gps_cnav_ephemeris - > d_Toe1 )
2019-05-07 14:39:06 +00:00
{
new_annotation = true ;
}
}
if ( new_annotation = = true )
{
// New record!
std : : map < int32_t , Gps_CNAV_Ephemeris > new_cnav_eph ;
new_cnav_eph [ gps_cnav_ephemeris - > i_satellite_PRN ] = * gps_cnav_ephemeris ;
2020-11-14 00:57:29 +00:00
d_rp - > log_rinex_nav_gps_cnav ( d_type_of_rx , new_cnav_eph ) ;
2019-05-07 14:39:06 +00:00
}
}
2019-08-01 16:11:36 +00:00
d_internal_pvt_solver - > gps_cnav_ephemeris_map [ gps_cnav_ephemeris - > i_satellite_PRN ] = * gps_cnav_ephemeris ;
2019-09-06 16:02:40 +00:00
if ( d_enable_rx_clock_correction = = true )
{
d_user_pvt_solver - > gps_cnav_ephemeris_map [ gps_cnav_ephemeris - > i_satellite_PRN ] = * gps_cnav_ephemeris ;
}
2019-05-07 16:52:08 +00:00
DLOG ( INFO ) < < " New GPS CNAV ephemeris record has arrived " ;
}
2020-07-19 07:39:32 +00:00
else if ( msg_type_hash_code = = d_gps_cnav_iono_sptr_type_hash_code )
2019-05-07 16:52:08 +00:00
{
// ### GPS CNAV IONO ###
2020-08-09 12:03:53 +00:00
const auto gps_cnav_iono = boost : : any_cast < std : : shared_ptr < Gps_CNAV_Iono > > ( pmt : : any_ref ( msg ) ) ;
2019-08-01 16:11:36 +00:00
d_internal_pvt_solver - > gps_cnav_iono = * gps_cnav_iono ;
2019-09-06 16:02:40 +00:00
if ( d_enable_rx_clock_correction = = true )
{
d_user_pvt_solver - > gps_cnav_iono = * gps_cnav_iono ;
}
2019-05-07 16:52:08 +00:00
DLOG ( INFO ) < < " New CNAV IONO record has arrived " ;
}
2020-07-19 07:39:32 +00:00
else if ( msg_type_hash_code = = d_gps_cnav_utc_model_sptr_type_hash_code )
2019-05-07 16:52:08 +00:00
{
// ### GPS CNAV UTC MODEL ###
2020-08-09 12:03:53 +00:00
const auto gps_cnav_utc_model = boost : : any_cast < std : : shared_ptr < Gps_CNAV_Utc_Model > > ( pmt : : any_ref ( msg ) ) ;
2019-08-01 16:11:36 +00:00
d_internal_pvt_solver - > gps_cnav_utc_model = * gps_cnav_utc_model ;
2019-09-06 16:02:40 +00:00
{
d_user_pvt_solver - > gps_cnav_utc_model = * gps_cnav_utc_model ;
}
2019-05-07 16:52:08 +00:00
DLOG ( INFO ) < < " New CNAV UTC record has arrived " ;
}
2018-10-20 17:30:32 +00:00
2020-07-19 07:39:32 +00:00
else if ( msg_type_hash_code = = d_gps_almanac_sptr_type_hash_code )
2019-05-07 16:52:08 +00:00
{
// ### GPS ALMANAC ###
2020-08-09 12:03:53 +00:00
const auto gps_almanac = boost : : any_cast < std : : shared_ptr < Gps_Almanac > > ( pmt : : any_ref ( msg ) ) ;
2019-08-01 16:11:36 +00:00
d_internal_pvt_solver - > gps_almanac_map [ gps_almanac - > i_satellite_PRN ] = * gps_almanac ;
2019-09-06 16:02:40 +00:00
if ( d_enable_rx_clock_correction = = true )
{
d_user_pvt_solver - > gps_almanac_map [ gps_almanac - > i_satellite_PRN ] = * gps_almanac ;
}
2019-05-07 16:52:08 +00:00
DLOG ( INFO ) < < " New GPS almanac record has arrived " ;
}
2018-10-27 22:42:28 +00:00
2020-07-19 07:39:32 +00:00
// *********************** Galileo telemetry ***********************
else if ( msg_type_hash_code = = d_galileo_ephemeris_sptr_type_hash_code )
2019-05-07 16:52:08 +00:00
{
// ### Galileo EPHEMERIS ###
2020-08-09 12:03:53 +00:00
const auto galileo_eph = boost : : any_cast < std : : shared_ptr < Galileo_Ephemeris > > ( pmt : : any_ref ( msg ) ) ;
2019-05-07 16:52:08 +00:00
// insert new ephemeris record
DLOG ( INFO ) < < " Galileo New Ephemeris record inserted in global map with TOW = " < < galileo_eph - > TOW_5
< < " , GALILEO Week Number = " < < galileo_eph - > WN_5
< < " and Ephemeris IOD = " < < galileo_eph - > IOD_ephemeris ;
// update/insert new ephemeris record to the global ephemeris map
2020-11-14 13:44:43 +00:00
if ( d_rp - > is_rinex_header_written ( ) ) // The header is already written, we can now log the navigation message data
2019-05-07 14:39:06 +00:00
{
2019-05-07 16:52:08 +00:00
bool new_annotation = false ;
2019-08-01 16:11:36 +00:00
if ( d_internal_pvt_solver - > galileo_ephemeris_map . find ( galileo_eph - > i_satellite_PRN ) = = d_internal_pvt_solver - > galileo_ephemeris_map . cend ( ) )
2019-05-07 16:52:08 +00:00
{
new_annotation = true ;
}
else
{
2019-08-01 16:11:36 +00:00
if ( d_internal_pvt_solver - > galileo_ephemeris_map [ galileo_eph - > i_satellite_PRN ] . t0e_1 ! = galileo_eph - > t0e_1 )
2019-05-07 14:39:06 +00:00
{
new_annotation = true ;
}
2019-05-07 16:52:08 +00:00
}
if ( new_annotation = = true )
{
// New record!
std : : map < int32_t , Galileo_Ephemeris > new_gal_eph ;
new_gal_eph [ galileo_eph - > i_satellite_PRN ] = * galileo_eph ;
2020-11-14 00:57:29 +00:00
d_rp - > log_rinex_nav_gal_nav ( d_type_of_rx , new_gal_eph ) ;
2019-05-07 14:39:06 +00:00
}
}
2019-08-01 16:11:36 +00:00
d_internal_pvt_solver - > galileo_ephemeris_map [ galileo_eph - > i_satellite_PRN ] = * galileo_eph ;
2019-09-06 16:02:40 +00:00
if ( d_enable_rx_clock_correction = = true )
{
d_user_pvt_solver - > galileo_ephemeris_map [ galileo_eph - > i_satellite_PRN ] = * galileo_eph ;
}
2019-05-07 16:52:08 +00:00
}
2020-07-19 07:39:32 +00:00
else if ( msg_type_hash_code = = d_galileo_iono_sptr_type_hash_code )
2019-05-07 16:52:08 +00:00
{
// ### Galileo IONO ###
2020-08-09 12:03:53 +00:00
const auto galileo_iono = boost : : any_cast < std : : shared_ptr < Galileo_Iono > > ( pmt : : any_ref ( msg ) ) ;
2019-08-01 16:11:36 +00:00
d_internal_pvt_solver - > galileo_iono = * galileo_iono ;
2019-09-06 16:02:40 +00:00
if ( d_enable_rx_clock_correction = = true )
{
d_user_pvt_solver - > galileo_iono = * galileo_iono ;
}
2019-05-07 16:52:08 +00:00
DLOG ( INFO ) < < " New IONO record has arrived " ;
}
2020-07-19 07:39:32 +00:00
else if ( msg_type_hash_code = = d_galileo_utc_model_sptr_type_hash_code )
2019-05-07 16:52:08 +00:00
{
// ### Galileo UTC MODEL ###
2020-08-09 12:03:53 +00:00
const auto galileo_utc_model = boost : : any_cast < std : : shared_ptr < Galileo_Utc_Model > > ( pmt : : any_ref ( msg ) ) ;
2019-08-01 16:11:36 +00:00
d_internal_pvt_solver - > galileo_utc_model = * galileo_utc_model ;
2019-09-06 16:02:40 +00:00
if ( d_enable_rx_clock_correction = = true )
{
d_user_pvt_solver - > galileo_utc_model = * galileo_utc_model ;
}
2019-05-07 16:52:08 +00:00
DLOG ( INFO ) < < " New UTC record has arrived " ;
}
2020-07-19 07:39:32 +00:00
else if ( msg_type_hash_code = = d_galileo_almanac_helper_sptr_type_hash_code )
2019-05-07 16:52:08 +00:00
{
// ### Galileo Almanac ###
2020-08-09 12:03:53 +00:00
const auto galileo_almanac_helper = boost : : any_cast < std : : shared_ptr < Galileo_Almanac_Helper > > ( pmt : : any_ref ( msg ) ) ;
2020-07-19 23:20:15 +00:00
const Galileo_Almanac sv1 = galileo_almanac_helper - > get_almanac ( 1 ) ;
const Galileo_Almanac sv2 = galileo_almanac_helper - > get_almanac ( 2 ) ;
const Galileo_Almanac sv3 = galileo_almanac_helper - > get_almanac ( 3 ) ;
2019-05-07 16:52:08 +00:00
if ( sv1 . i_satellite_PRN ! = 0 )
2018-10-27 22:42:28 +00:00
{
2019-08-01 16:11:36 +00:00
d_internal_pvt_solver - > galileo_almanac_map [ sv1 . i_satellite_PRN ] = sv1 ;
2019-09-06 16:02:40 +00:00
if ( d_enable_rx_clock_correction = = true )
{
d_user_pvt_solver - > galileo_almanac_map [ sv1 . i_satellite_PRN ] = sv1 ;
}
2018-10-27 22:42:28 +00:00
}
2019-05-07 16:52:08 +00:00
if ( sv2 . i_satellite_PRN ! = 0 )
2018-10-27 22:42:28 +00:00
{
2019-08-01 16:11:36 +00:00
d_internal_pvt_solver - > galileo_almanac_map [ sv2 . i_satellite_PRN ] = sv2 ;
2019-09-06 16:02:40 +00:00
if ( d_enable_rx_clock_correction = = true )
{
d_user_pvt_solver - > galileo_almanac_map [ sv2 . i_satellite_PRN ] = sv2 ;
}
2018-10-27 22:42:28 +00:00
}
2019-05-07 16:52:08 +00:00
if ( sv3 . i_satellite_PRN ! = 0 )
2018-10-27 22:42:28 +00:00
{
2019-08-01 16:11:36 +00:00
d_internal_pvt_solver - > galileo_almanac_map [ sv3 . i_satellite_PRN ] = sv3 ;
2019-09-06 16:02:40 +00:00
if ( d_enable_rx_clock_correction = = true )
{
d_user_pvt_solver - > galileo_almanac_map [ sv3 . i_satellite_PRN ] = sv3 ;
}
2019-05-07 16:52:08 +00:00
}
DLOG ( INFO ) < < " New Galileo Almanac data have arrived " ;
}
2020-07-19 07:39:32 +00:00
else if ( msg_type_hash_code = = d_galileo_almanac_sptr_type_hash_code )
2019-05-07 16:52:08 +00:00
{
// ### Galileo Almanac ###
2020-08-09 12:03:53 +00:00
const auto galileo_alm = boost : : any_cast < std : : shared_ptr < Galileo_Almanac > > ( pmt : : any_ref ( msg ) ) ;
2019-05-07 16:52:08 +00:00
// update/insert new almanac record to the global almanac map
2019-08-01 16:11:36 +00:00
d_internal_pvt_solver - > galileo_almanac_map [ galileo_alm - > i_satellite_PRN ] = * galileo_alm ;
2019-09-06 16:02:40 +00:00
if ( d_enable_rx_clock_correction = = true )
{
d_user_pvt_solver - > galileo_almanac_map [ galileo_alm - > i_satellite_PRN ] = * galileo_alm ;
}
2019-05-07 16:52:08 +00:00
}
2019-05-07 14:39:06 +00:00
2020-07-19 07:39:32 +00:00
// **************** GLONASS GNAV Telemetry *************************
else if ( msg_type_hash_code = = d_glonass_gnav_ephemeris_sptr_type_hash_code )
2019-05-07 16:52:08 +00:00
{
// ### GLONASS GNAV EPHEMERIS ###
2020-08-09 12:03:53 +00:00
const auto glonass_gnav_eph = boost : : any_cast < std : : shared_ptr < Glonass_Gnav_Ephemeris > > ( pmt : : any_ref ( msg ) ) ;
2019-05-07 16:52:08 +00:00
// TODO Add GLONASS with gps week number and tow,
// insert new ephemeris record
DLOG ( INFO ) < < " GLONASS GNAV New Ephemeris record inserted in global map with TOW = " < < glonass_gnav_eph - > d_TOW
< < " , Week Number = " < < glonass_gnav_eph - > d_WN
< < " and Ephemeris IOD in UTC = " < < glonass_gnav_eph - > compute_GLONASS_time ( glonass_gnav_eph - > d_t_b )
< < " from SV = " < < glonass_gnav_eph - > i_satellite_slot_number ;
// update/insert new ephemeris record to the global ephemeris map
2020-11-14 13:44:43 +00:00
if ( d_rp - > is_rinex_header_written ( ) ) // The header is already written, we can now log the navigation message data
2019-05-07 16:52:08 +00:00
{
bool new_annotation = false ;
2019-08-01 16:11:36 +00:00
if ( d_internal_pvt_solver - > glonass_gnav_ephemeris_map . find ( glonass_gnav_eph - > i_satellite_PRN ) = = d_internal_pvt_solver - > glonass_gnav_ephemeris_map . cend ( ) )
2019-05-07 14:39:06 +00:00
{
2019-05-07 16:52:08 +00:00
new_annotation = true ;
2019-05-07 14:39:06 +00:00
}
2019-05-07 16:52:08 +00:00
else
2019-05-07 14:39:06 +00:00
{
2019-08-01 16:11:36 +00:00
if ( d_internal_pvt_solver - > glonass_gnav_ephemeris_map [ glonass_gnav_eph - > i_satellite_PRN ] . d_t_b ! = glonass_gnav_eph - > d_t_b )
2019-05-07 14:39:06 +00:00
{
new_annotation = true ;
}
2019-05-07 16:52:08 +00:00
}
if ( new_annotation = = true )
{
// New record!
std : : map < int32_t , Glonass_Gnav_Ephemeris > new_glo_eph ;
new_glo_eph [ glonass_gnav_eph - > i_satellite_PRN ] = * glonass_gnav_eph ;
2020-11-14 00:57:29 +00:00
d_rp - > log_rinex_nav_glo_gnav ( d_type_of_rx , new_glo_eph ) ;
2019-05-07 14:39:06 +00:00
}
}
2019-08-01 16:11:36 +00:00
d_internal_pvt_solver - > glonass_gnav_ephemeris_map [ glonass_gnav_eph - > i_satellite_PRN ] = * glonass_gnav_eph ;
2019-09-06 16:02:40 +00:00
if ( d_enable_rx_clock_correction = = true )
{
d_user_pvt_solver - > glonass_gnav_ephemeris_map [ glonass_gnav_eph - > i_satellite_PRN ] = * glonass_gnav_eph ;
}
2019-05-07 16:52:08 +00:00
}
2020-07-19 07:39:32 +00:00
else if ( msg_type_hash_code = = d_glonass_gnav_utc_model_sptr_type_hash_code )
2019-05-07 16:52:08 +00:00
{
// ### GLONASS GNAV UTC MODEL ###
2020-08-09 12:03:53 +00:00
const auto glonass_gnav_utc_model = boost : : any_cast < std : : shared_ptr < Glonass_Gnav_Utc_Model > > ( pmt : : any_ref ( msg ) ) ;
2019-08-01 16:11:36 +00:00
d_internal_pvt_solver - > glonass_gnav_utc_model = * glonass_gnav_utc_model ;
2019-09-06 16:02:40 +00:00
if ( d_enable_rx_clock_correction = = true )
{
d_user_pvt_solver - > glonass_gnav_utc_model = * glonass_gnav_utc_model ;
}
2019-05-07 16:52:08 +00:00
DLOG ( INFO ) < < " New GLONASS GNAV UTC record has arrived " ;
}
2020-07-19 07:39:32 +00:00
else if ( msg_type_hash_code = = d_glonass_gnav_almanac_sptr_type_hash_code )
2019-05-07 16:52:08 +00:00
{
// ### GLONASS GNAV Almanac ###
2020-08-09 12:03:53 +00:00
const auto glonass_gnav_almanac = boost : : any_cast < std : : shared_ptr < Glonass_Gnav_Almanac > > ( pmt : : any_ref ( msg ) ) ;
2019-08-01 16:11:36 +00:00
d_internal_pvt_solver - > glonass_gnav_almanac = * glonass_gnav_almanac ;
2019-09-06 16:02:40 +00:00
if ( d_enable_rx_clock_correction = = true )
{
d_user_pvt_solver - > glonass_gnav_almanac = * glonass_gnav_almanac ;
}
2019-05-07 16:52:08 +00:00
DLOG ( INFO ) < < " New GLONASS GNAV Almanac has arrived "
< < " , GLONASS GNAV Slot Number = " < < glonass_gnav_almanac - > d_n_A ;
}
2019-02-26 00:38:48 +00:00
2020-07-19 07:39:32 +00:00
// *********************** BeiDou telemetry ************************
else if ( msg_type_hash_code = = d_beidou_dnav_ephemeris_sptr_type_hash_code )
2019-05-07 16:52:08 +00:00
{
// ### Beidou EPHEMERIS ###
2020-08-09 12:03:53 +00:00
const auto bds_dnav_eph = boost : : any_cast < std : : shared_ptr < Beidou_Dnav_Ephemeris > > ( pmt : : any_ref ( msg ) ) ;
2019-05-07 16:52:08 +00:00
DLOG ( INFO ) < < " Ephemeris record has arrived from SAT ID "
< < bds_dnav_eph - > i_satellite_PRN < < " (Block "
< < bds_dnav_eph - > satelliteBlock [ bds_dnav_eph - > i_satellite_PRN ] < < " ) "
< < " inserted with Toe= " < < bds_dnav_eph - > d_Toe < < " and BDS Week= "
< < bds_dnav_eph - > i_BEIDOU_week ;
// update/insert new ephemeris record to the global ephemeris map
2020-11-14 13:44:43 +00:00
if ( d_rp - > is_rinex_header_written ( ) ) // The header is already written, we can now log the navigation message data
2019-05-07 14:39:06 +00:00
{
2019-05-07 16:52:08 +00:00
bool new_annotation = false ;
2019-08-01 16:11:36 +00:00
if ( d_internal_pvt_solver - > beidou_dnav_ephemeris_map . find ( bds_dnav_eph - > i_satellite_PRN ) = = d_internal_pvt_solver - > beidou_dnav_ephemeris_map . cend ( ) )
2019-05-07 16:52:08 +00:00
{
new_annotation = true ;
}
else
{
2019-08-01 16:11:36 +00:00
if ( d_internal_pvt_solver - > beidou_dnav_ephemeris_map [ bds_dnav_eph - > i_satellite_PRN ] . d_Toc ! = bds_dnav_eph - > d_Toc )
2019-05-07 14:39:06 +00:00
{
new_annotation = true ;
}
2019-05-07 16:52:08 +00:00
}
if ( new_annotation = = true )
{
// New record!
std : : map < int32_t , Beidou_Dnav_Ephemeris > new_bds_eph ;
new_bds_eph [ bds_dnav_eph - > i_satellite_PRN ] = * bds_dnav_eph ;
2020-11-14 00:57:29 +00:00
d_rp - > log_rinex_nav_bds_dnav ( d_type_of_rx , new_bds_eph ) ;
2019-05-07 14:39:06 +00:00
}
}
2019-08-01 16:11:36 +00:00
d_internal_pvt_solver - > beidou_dnav_ephemeris_map [ bds_dnav_eph - > i_satellite_PRN ] = * bds_dnav_eph ;
2019-09-06 16:02:40 +00:00
if ( d_enable_rx_clock_correction = = true )
{
d_user_pvt_solver - > beidou_dnav_ephemeris_map [ bds_dnav_eph - > i_satellite_PRN ] = * bds_dnav_eph ;
}
2019-05-07 16:52:08 +00:00
}
2020-07-19 07:39:32 +00:00
else if ( msg_type_hash_code = = d_beidou_dnav_iono_sptr_type_hash_code )
2019-05-07 16:52:08 +00:00
{
// ### BeiDou IONO ###
2020-08-09 12:03:53 +00:00
const auto bds_dnav_iono = boost : : any_cast < std : : shared_ptr < Beidou_Dnav_Iono > > ( pmt : : any_ref ( msg ) ) ;
2019-08-01 16:11:36 +00:00
d_internal_pvt_solver - > beidou_dnav_iono = * bds_dnav_iono ;
2019-09-06 16:02:40 +00:00
if ( d_enable_rx_clock_correction = = true )
{
d_user_pvt_solver - > beidou_dnav_iono = * bds_dnav_iono ;
}
2019-05-07 16:52:08 +00:00
DLOG ( INFO ) < < " New BeiDou DNAV IONO record has arrived " ;
}
2020-07-19 07:39:32 +00:00
else if ( msg_type_hash_code = = d_beidou_dnav_utc_model_sptr_type_hash_code )
2019-05-07 16:52:08 +00:00
{
// ### BeiDou UTC MODEL ###
2020-08-09 12:03:53 +00:00
const auto bds_dnav_utc_model = boost : : any_cast < std : : shared_ptr < Beidou_Dnav_Utc_Model > > ( pmt : : any_ref ( msg ) ) ;
2019-08-01 16:11:36 +00:00
d_internal_pvt_solver - > beidou_dnav_utc_model = * bds_dnav_utc_model ;
2019-09-06 16:02:40 +00:00
if ( d_enable_rx_clock_correction = = true )
{
d_user_pvt_solver - > beidou_dnav_utc_model = * bds_dnav_utc_model ;
}
2019-05-07 16:52:08 +00:00
DLOG ( INFO ) < < " New BeiDou DNAV UTC record has arrived " ;
}
2020-07-19 07:39:32 +00:00
else if ( msg_type_hash_code = = d_beidou_dnav_almanac_sptr_type_hash_code )
2019-05-07 16:52:08 +00:00
{
// ### BeiDou ALMANAC ###
2020-08-09 12:03:53 +00:00
const auto bds_dnav_almanac = boost : : any_cast < std : : shared_ptr < Beidou_Dnav_Almanac > > ( pmt : : any_ref ( msg ) ) ;
2019-08-01 16:11:36 +00:00
d_internal_pvt_solver - > beidou_dnav_almanac_map [ bds_dnav_almanac - > i_satellite_PRN ] = * bds_dnav_almanac ;
2019-09-06 16:02:40 +00:00
if ( d_enable_rx_clock_correction = = true )
{
d_user_pvt_solver - > beidou_dnav_almanac_map [ bds_dnav_almanac - > i_satellite_PRN ] = * bds_dnav_almanac ;
}
2019-05-07 16:52:08 +00:00
DLOG ( INFO ) < < " New BeiDou DNAV almanac record has arrived " ;
}
else
{
LOG ( WARNING ) < < " msg_handler_telemetry unknown object type! " ;
2018-10-20 17:30:32 +00:00
}
}
2020-08-03 06:13:21 +00:00
catch ( const boost : : bad_any_cast & e )
2019-02-26 00:38:48 +00:00
{
2020-08-03 06:13:21 +00:00
LOG ( WARNING ) < < " msg_handler_telemetry Bad any_cast: " < < e . what ( ) ;
2019-02-26 00:38:48 +00:00
}
}
2019-03-02 01:21:03 +00:00
std : : map < int , Gps_Ephemeris > rtklib_pvt_gs : : get_gps_ephemeris_map ( ) const
2019-02-26 00:38:48 +00:00
{
2019-08-01 16:11:36 +00:00
return d_internal_pvt_solver - > gps_ephemeris_map ;
2019-02-26 00:38:48 +00:00
}
2019-03-02 01:21:03 +00:00
std : : map < int , Gps_Almanac > rtklib_pvt_gs : : get_gps_almanac_map ( ) const
2019-02-26 00:38:48 +00:00
{
2019-08-01 16:11:36 +00:00
return d_internal_pvt_solver - > gps_almanac_map ;
2019-02-26 00:38:48 +00:00
}
2019-03-02 01:21:03 +00:00
std : : map < int , Galileo_Ephemeris > rtklib_pvt_gs : : get_galileo_ephemeris_map ( ) const
2019-02-26 00:38:48 +00:00
{
2019-08-01 16:11:36 +00:00
return d_internal_pvt_solver - > galileo_ephemeris_map ;
2019-02-26 00:38:48 +00:00
}
2019-03-02 01:21:03 +00:00
std : : map < int , Galileo_Almanac > rtklib_pvt_gs : : get_galileo_almanac_map ( ) const
2019-02-26 00:38:48 +00:00
{
2019-08-01 16:11:36 +00:00
return d_internal_pvt_solver - > galileo_almanac_map ;
2019-02-26 00:38:48 +00:00
}
2019-03-02 01:21:03 +00:00
std : : map < int , Beidou_Dnav_Ephemeris > rtklib_pvt_gs : : get_beidou_dnav_ephemeris_map ( ) const
2019-02-26 00:38:48 +00:00
{
2019-08-01 16:11:36 +00:00
return d_internal_pvt_solver - > beidou_dnav_ephemeris_map ;
2019-02-26 00:38:48 +00:00
}
2019-03-02 01:21:03 +00:00
std : : map < int , Beidou_Dnav_Almanac > rtklib_pvt_gs : : get_beidou_dnav_almanac_map ( ) const
2019-02-26 00:38:48 +00:00
{
2019-08-01 16:11:36 +00:00
return d_internal_pvt_solver - > beidou_dnav_almanac_map ;
2019-02-26 00:38:48 +00:00
}
2019-03-02 01:21:03 +00:00
void rtklib_pvt_gs : : clear_ephemeris ( )
2019-02-26 00:38:48 +00:00
{
2019-08-01 16:11:36 +00:00
d_internal_pvt_solver - > gps_ephemeris_map . clear ( ) ;
d_internal_pvt_solver - > gps_almanac_map . clear ( ) ;
d_internal_pvt_solver - > galileo_ephemeris_map . clear ( ) ;
d_internal_pvt_solver - > galileo_almanac_map . clear ( ) ;
d_internal_pvt_solver - > beidou_dnav_ephemeris_map . clear ( ) ;
d_internal_pvt_solver - > beidou_dnav_almanac_map . clear ( ) ;
2019-09-06 16:02:40 +00:00
if ( d_enable_rx_clock_correction = = true )
{
d_user_pvt_solver - > gps_ephemeris_map . clear ( ) ;
d_user_pvt_solver - > gps_almanac_map . clear ( ) ;
d_user_pvt_solver - > galileo_ephemeris_map . clear ( ) ;
d_user_pvt_solver - > galileo_almanac_map . clear ( ) ;
d_user_pvt_solver - > beidou_dnav_ephemeris_map . clear ( ) ;
d_user_pvt_solver - > beidou_dnav_almanac_map . clear ( ) ;
}
2017-04-20 14:10:12 +00:00
}
2020-12-30 20:49:29 +00:00
bool rtklib_pvt_gs : : send_sys_v_ttff_msg ( d_ttff_msgbuf ttff ) const
2017-04-20 14:10:12 +00:00
{
2020-06-18 23:36:36 +00:00
if ( d_sysv_msqid ! = - 1 )
2020-03-10 18:56:13 +00:00
{
// Fill Sys V message structures
int msgsend_size ;
2020-06-18 23:36:36 +00:00
d_ttff_msgbuf msg ;
2020-03-10 18:56:13 +00:00
msg . ttff = ttff . ttff ;
msgsend_size = sizeof ( msg . ttff ) ;
msg . mtype = 1 ; // default message ID
// SEND SOLUTION OVER A MESSAGE QUEUE
// non-blocking Sys V message send
2020-06-18 23:36:36 +00:00
msgsnd ( d_sysv_msqid , & msg , msgsend_size , IPC_NOWAIT ) ;
2020-03-10 18:56:13 +00:00
return true ;
}
return false ;
2017-04-20 14:10:12 +00:00
}
2019-03-02 01:21:03 +00:00
bool rtklib_pvt_gs : : save_gnss_synchro_map_xml ( const std : : string & file_name )
2018-10-05 09:49:11 +00:00
{
2020-06-18 23:36:36 +00:00
if ( d_gnss_observables_map . empty ( ) = = false )
2018-10-05 09:49:11 +00:00
{
2018-10-06 18:10:16 +00:00
std : : ofstream ofs ;
2018-10-05 09:49:11 +00:00
try
{
2018-10-06 18:10:16 +00:00
ofs . open ( file_name . c_str ( ) , std : : ofstream : : trunc | std : : ofstream : : out ) ;
2018-10-05 09:49:11 +00:00
boost : : archive : : xml_oarchive xml ( ofs ) ;
2020-06-18 23:36:36 +00:00
xml < < boost : : serialization : : make_nvp ( " GNSS-SDR_gnss_synchro_map " , d_gnss_observables_map ) ;
2018-10-05 09:49:11 +00:00
LOG ( INFO ) < < " Saved gnss_sychro map data " ;
}
2019-02-14 11:51:43 +00:00
catch ( const std : : exception & e )
2018-10-05 09:49:11 +00:00
{
LOG ( WARNING ) < < e . what ( ) ;
return false ;
}
return true ;
}
2018-12-03 21:08:19 +00:00
2019-01-28 01:29:43 +00:00
LOG ( WARNING ) < < " Failed to save gnss_synchro, map is empty " ;
return false ;
2018-10-05 09:49:11 +00:00
}
2018-10-06 18:10:16 +00:00
2019-03-02 01:21:03 +00:00
bool rtklib_pvt_gs : : load_gnss_synchro_map_xml ( const std : : string & file_name )
2018-10-05 09:49:11 +00:00
{
2018-10-06 18:10:16 +00:00
// load from xml (boost serialize)
std : : ifstream ifs ;
2018-10-05 09:49:11 +00:00
try
{
2018-10-06 18:10:16 +00:00
ifs . open ( file_name . c_str ( ) , std : : ifstream : : binary | std : : ifstream : : in ) ;
2018-10-05 09:49:11 +00:00
boost : : archive : : xml_iarchive xml ( ifs ) ;
2020-06-18 23:36:36 +00:00
d_gnss_observables_map . clear ( ) ;
xml > > boost : : serialization : : make_nvp ( " GNSS-SDR_gnss_synchro_map " , d_gnss_observables_map ) ;
2020-07-07 16:53:50 +00:00
// std::cout << "Loaded gnss_synchro map data with " << gnss_synchro_map.size() << " pseudoranges\n";
2018-10-05 09:49:11 +00:00
}
2019-02-14 11:51:43 +00:00
catch ( const std : : exception & e )
2018-10-05 09:49:11 +00:00
{
std : : cout < < e . what ( ) < < " File: " < < file_name ;
return false ;
}
2018-10-06 18:10:16 +00:00
return true ;
2018-10-05 09:49:11 +00:00
}
2019-03-02 01:21:03 +00:00
std : : vector < std : : string > rtklib_pvt_gs : : split_string ( const std : : string & s , char delim ) const
2019-02-18 20:44:19 +00:00
{
std : : vector < std : : string > v ;
std : : stringstream ss ( s ) ;
std : : string item ;
while ( std : : getline ( ss , item , delim ) )
{
* ( std : : back_inserter ( v ) + + ) = item ;
}
return v ;
}
2019-03-02 01:21:03 +00:00
bool rtklib_pvt_gs : : get_latest_PVT ( double * longitude_deg ,
2018-11-06 13:39:57 +00:00
double * latitude_deg ,
double * height_m ,
double * ground_speed_kmh ,
double * course_over_ground_deg ,
2019-02-18 20:44:19 +00:00
time_t * UTC_time ) const
2018-11-06 13:39:57 +00:00
{
2019-09-06 16:02:40 +00:00
if ( d_enable_rx_clock_correction = = true )
2018-11-06 13:39:57 +00:00
{
2019-09-06 16:02:40 +00:00
if ( d_user_pvt_solver - > is_valid_position ( ) )
{
* latitude_deg = d_user_pvt_solver - > get_latitude ( ) ;
* longitude_deg = d_user_pvt_solver - > get_longitude ( ) ;
* height_m = d_user_pvt_solver - > get_height ( ) ;
* ground_speed_kmh = d_user_pvt_solver - > get_speed_over_ground ( ) * 3600.0 / 1000.0 ;
* course_over_ground_deg = d_user_pvt_solver - > get_course_over_ground ( ) ;
* UTC_time = convert_to_time_t ( d_user_pvt_solver - > get_position_UTC_time ( ) ) ;
return true ;
}
}
else
{
if ( d_internal_pvt_solver - > is_valid_position ( ) )
{
* latitude_deg = d_internal_pvt_solver - > get_latitude ( ) ;
* longitude_deg = d_internal_pvt_solver - > get_longitude ( ) ;
* height_m = d_internal_pvt_solver - > get_height ( ) ;
* ground_speed_kmh = d_internal_pvt_solver - > get_speed_over_ground ( ) * 3600.0 / 1000.0 ;
* course_over_ground_deg = d_internal_pvt_solver - > get_course_over_ground ( ) ;
* UTC_time = convert_to_time_t ( d_internal_pvt_solver - > get_position_UTC_time ( ) ) ;
return true ;
}
2018-11-06 13:39:57 +00:00
}
2018-12-03 21:08:19 +00:00
2019-01-28 01:29:43 +00:00
return false ;
2018-11-06 13:39:57 +00:00
}
2018-11-07 18:27:26 +00:00
2019-07-31 16:16:09 +00:00
void rtklib_pvt_gs : : apply_rx_clock_offset ( std : : map < int , Gnss_Synchro > & observables_map ,
double rx_clock_offset_s )
{
2019-08-01 18:09:35 +00:00
// apply corrections according to Rinex 3.04, Table 1: Observation Corrections for Receiver Clock Offset
2019-07-31 16:16:09 +00:00
std : : map < int , Gnss_Synchro > : : iterator observables_iter ;
for ( observables_iter = observables_map . begin ( ) ; observables_iter ! = observables_map . end ( ) ; observables_iter + + )
{
2019-08-01 18:09:35 +00:00
// all observables in the map are valid
2019-07-31 16:16:09 +00:00
observables_iter - > second . RX_time - = rx_clock_offset_s ;
2020-07-05 18:20:02 +00:00
observables_iter - > second . Pseudorange_m - = rx_clock_offset_s * SPEED_OF_LIGHT_M_S ;
2019-07-31 16:16:09 +00:00
2020-06-18 23:36:36 +00:00
switch ( d_mapStringValues [ observables_iter - > second . Signal ] )
2019-07-31 16:16:09 +00:00
{
case evGPS_1C :
case evSBAS_1C :
case evGAL_1B :
2020-07-05 18:20:02 +00:00
observables_iter - > second . Carrier_phase_rads - = rx_clock_offset_s * FREQ1 * TWO_PI ;
2019-07-31 16:16:09 +00:00
break ;
2020-07-09 22:37:55 +00:00
case evGPS_L5 :
2019-07-31 16:16:09 +00:00
case evGAL_5X :
2020-07-05 18:20:02 +00:00
observables_iter - > second . Carrier_phase_rads - = rx_clock_offset_s * FREQ5 * TWO_PI ;
2019-07-31 16:16:09 +00:00
break ;
2020-11-07 20:33:26 +00:00
case evGAL_E6 :
observables_iter - > second . Carrier_phase_rads - = rx_clock_offset_s * FREQ6 * TWO_PI ;
break ;
2020-07-31 15:58:57 +00:00
case evGAL_7X :
observables_iter - > second . Carrier_phase_rads - = rx_clock_offset_s * FREQ7 * TWO_PI ;
break ;
2019-07-31 16:16:09 +00:00
case evGPS_2S :
2020-07-05 18:20:02 +00:00
observables_iter - > second . Carrier_phase_rads - = rx_clock_offset_s * FREQ2 * TWO_PI ;
2019-07-31 16:16:09 +00:00
break ;
case evBDS_B3 :
2020-07-05 18:20:02 +00:00
observables_iter - > second . Carrier_phase_rads - = rx_clock_offset_s * FREQ3_BDS * TWO_PI ;
2019-07-31 16:16:09 +00:00
break ;
case evGLO_1G :
2020-07-05 18:20:02 +00:00
observables_iter - > second . Carrier_phase_rads - = rx_clock_offset_s * FREQ1_GLO * TWO_PI ;
2019-07-31 16:16:09 +00:00
break ;
case evGLO_2G :
2020-07-05 18:20:02 +00:00
observables_iter - > second . Carrier_phase_rads - = rx_clock_offset_s * FREQ2_GLO * TWO_PI ;
2019-07-31 16:16:09 +00:00
break ;
case evBDS_B1 :
2020-07-05 18:20:02 +00:00
observables_iter - > second . Carrier_phase_rads - = rx_clock_offset_s * FREQ1_BDS * TWO_PI ;
2019-07-31 16:16:09 +00:00
break ;
case evBDS_B2 :
2020-07-05 18:20:02 +00:00
observables_iter - > second . Carrier_phase_rads - = rx_clock_offset_s * FREQ2_BDS * TWO_PI ;
2019-07-31 16:16:09 +00:00
break ;
default :
break ;
}
}
}
2019-08-01 18:09:35 +00:00
2020-06-18 23:36:36 +00:00
std : : map < int , Gnss_Synchro > rtklib_pvt_gs : : interpolate_observables ( const std : : map < int , Gnss_Synchro > & observables_map_t0 ,
const std : : map < int , Gnss_Synchro > & observables_map_t1 ,
2019-07-31 16:16:09 +00:00
double rx_time_s )
{
std : : map < int , Gnss_Synchro > interp_observables_map ;
2019-08-01 18:09:35 +00:00
// Linear interpolation: y(t) = y(t0) + (y(t1) - y(t0)) * (t - t0) / (t1 - t0)
2019-08-01 16:11:36 +00:00
// check TOW rollover
double time_factor ;
if ( ( observables_map_t1 . cbegin ( ) - > second . RX_time -
observables_map_t0 . cbegin ( ) - > second . RX_time ) > 0 )
{
time_factor = ( rx_time_s - observables_map_t0 . cbegin ( ) - > second . RX_time ) /
( observables_map_t1 . cbegin ( ) - > second . RX_time -
observables_map_t0 . cbegin ( ) - > second . RX_time ) ;
}
else
{
// TOW rollover situation
time_factor = ( 604800000.0 + rx_time_s - observables_map_t0 . cbegin ( ) - > second . RX_time ) /
( 604800000.0 + observables_map_t1 . cbegin ( ) - > second . RX_time -
observables_map_t0 . cbegin ( ) - > second . RX_time ) ;
}
2019-07-31 16:16:09 +00:00
std : : map < int , Gnss_Synchro > : : const_iterator observables_iter ;
for ( observables_iter = observables_map_t0 . cbegin ( ) ; observables_iter ! = observables_map_t0 . cend ( ) ; observables_iter + + )
{
2019-08-01 18:09:35 +00:00
// 1. Check if the observable exist in t0 and t1
// the map key is the channel ID (see work())
2019-07-31 16:16:09 +00:00
try
{
if ( observables_map_t1 . at ( observables_iter - > first ) . PRN = = observables_iter - > second . PRN )
{
interp_observables_map . insert ( std : : pair < int , Gnss_Synchro > ( observables_iter - > first , observables_iter - > second ) ) ;
2019-08-01 18:09:35 +00:00
interp_observables_map . at ( observables_iter - > first ) . RX_time = rx_time_s ; // interpolation point
2019-07-31 16:16:09 +00:00
interp_observables_map . at ( observables_iter - > first ) . Pseudorange_m + = ( observables_map_t1 . at ( observables_iter - > first ) . Pseudorange_m - observables_iter - > second . Pseudorange_m ) * time_factor ;
interp_observables_map . at ( observables_iter - > first ) . Carrier_phase_rads + = ( observables_map_t1 . at ( observables_iter - > first ) . Carrier_phase_rads - observables_iter - > second . Carrier_phase_rads ) * time_factor ;
interp_observables_map . at ( observables_iter - > first ) . Carrier_Doppler_hz + = ( observables_map_t1 . at ( observables_iter - > first ) . Carrier_Doppler_hz - observables_iter - > second . Carrier_Doppler_hz ) * time_factor ;
}
}
catch ( const std : : out_of_range & oor )
{
2019-08-01 18:09:35 +00:00
// observable does not exist in t1
2019-07-31 16:16:09 +00:00
}
}
return interp_observables_map ;
}
2020-02-11 20:47:13 +00:00
2020-02-06 15:56:42 +00:00
void rtklib_pvt_gs : : initialize_and_apply_carrier_phase_offset ( )
{
2020-02-11 20:47:13 +00:00
// we have a valid PVT. First check if we need to reset the initial carrier phase offsets to match their pseudoranges
2020-02-06 15:56:42 +00:00
std : : map < int , Gnss_Synchro > : : iterator observables_iter ;
2020-06-18 23:36:36 +00:00
for ( observables_iter = d_gnss_observables_map . begin ( ) ; observables_iter ! = d_gnss_observables_map . end ( ) ; observables_iter + + )
2020-02-06 15:56:42 +00:00
{
2020-02-11 20:47:13 +00:00
// check if an initialization is required (new satellite or loss of lock)
// it is set to false by the work function if the gnss_synchro is not valid
2020-06-18 23:36:36 +00:00
if ( d_channel_initialized . at ( observables_iter - > second . Channel_ID ) = = false )
2020-02-06 15:56:42 +00:00
{
double wavelength_m = 0 ;
2020-06-18 23:36:36 +00:00
switch ( d_mapStringValues [ observables_iter - > second . Signal ] )
2020-02-06 15:56:42 +00:00
{
case evGPS_1C :
case evSBAS_1C :
case evGAL_1B :
2020-07-05 18:20:02 +00:00
wavelength_m = SPEED_OF_LIGHT_M_S / FREQ1 ;
2020-02-06 15:56:42 +00:00
break ;
2020-07-09 22:37:55 +00:00
case evGPS_L5 :
2020-02-06 15:56:42 +00:00
case evGAL_5X :
2020-07-05 18:20:02 +00:00
wavelength_m = SPEED_OF_LIGHT_M_S / FREQ5 ;
2020-02-06 15:56:42 +00:00
break ;
2020-11-07 20:33:26 +00:00
case evGAL_E6 :
wavelength_m = SPEED_OF_LIGHT_M_S / FREQ6 ;
break ;
2020-07-31 15:58:57 +00:00
case evGAL_7X :
wavelength_m = SPEED_OF_LIGHT_M_S / FREQ7 ;
break ;
2020-02-06 15:56:42 +00:00
case evGPS_2S :
2020-07-05 18:20:02 +00:00
wavelength_m = SPEED_OF_LIGHT_M_S / FREQ2 ;
2020-02-06 15:56:42 +00:00
break ;
case evBDS_B3 :
2020-07-05 18:20:02 +00:00
wavelength_m = SPEED_OF_LIGHT_M_S / FREQ3_BDS ;
2020-02-06 15:56:42 +00:00
break ;
case evGLO_1G :
2020-07-05 18:20:02 +00:00
wavelength_m = SPEED_OF_LIGHT_M_S / FREQ1_GLO ;
2020-02-06 15:56:42 +00:00
break ;
case evGLO_2G :
2020-07-05 18:20:02 +00:00
wavelength_m = SPEED_OF_LIGHT_M_S / FREQ2_GLO ;
2020-02-06 15:56:42 +00:00
break ;
case evBDS_B1 :
2020-07-05 18:20:02 +00:00
wavelength_m = SPEED_OF_LIGHT_M_S / FREQ1_BDS ;
2020-02-06 15:56:42 +00:00
break ;
case evBDS_B2 :
2020-07-05 18:20:02 +00:00
wavelength_m = SPEED_OF_LIGHT_M_S / FREQ2_BDS ;
2020-02-06 15:56:42 +00:00
break ;
default :
break ;
}
2020-07-19 07:39:32 +00:00
const double wrap_carrier_phase_rad = fmod ( observables_iter - > second . Carrier_phase_rads , TWO_PI ) ;
2020-07-05 18:20:02 +00:00
d_initial_carrier_phase_offset_estimation_rads . at ( observables_iter - > second . Channel_ID ) = TWO_PI * round ( observables_iter - > second . Pseudorange_m / wavelength_m ) - observables_iter - > second . Carrier_phase_rads + wrap_carrier_phase_rad ;
2020-06-18 23:36:36 +00:00
d_channel_initialized . at ( observables_iter - > second . Channel_ID ) = true ;
2020-02-06 15:56:42 +00:00
DLOG ( INFO ) < < " initialized carrier phase at channel " < < observables_iter - > second . Channel_ID ;
}
2020-02-11 20:47:13 +00:00
// apply the carrier phase offset to this satellite
2020-06-18 23:36:36 +00:00
observables_iter - > second . Carrier_phase_rads = observables_iter - > second . Carrier_phase_rads + d_initial_carrier_phase_offset_estimation_rads . at ( observables_iter - > second . Channel_ID ) ;
2020-02-06 15:56:42 +00:00
}
}
2019-08-01 18:09:35 +00:00
2020-02-11 20:47:13 +00:00
2019-03-02 01:21:03 +00:00
int rtklib_pvt_gs : : work ( int noutput_items , gr_vector_const_void_star & input_items ,
2018-03-03 01:03:39 +00:00
gr_vector_void_star & output_items __attribute__ ( ( unused ) ) )
2017-04-20 14:10:12 +00:00
{
2018-08-11 21:03:41 +00:00
for ( int32_t epoch = 0 ; epoch < noutput_items ; epoch + + )
2017-04-20 14:10:12 +00:00
{
2017-05-14 17:59:55 +00:00
bool flag_display_pvt = false ;
bool flag_compute_pvt_output = false ;
bool flag_write_RTCM_1019_output = false ;
2017-08-17 04:40:05 +00:00
bool flag_write_RTCM_1020_output = false ;
2017-05-14 17:59:55 +00:00
bool flag_write_RTCM_1045_output = false ;
bool flag_write_RTCM_MSM_output = false ;
bool flag_write_RINEX_obs_output = false ;
2020-06-18 23:36:36 +00:00
d_gnss_observables_map . clear ( ) ;
2019-02-05 00:31:09 +00:00
const auto * * in = reinterpret_cast < const Gnss_Synchro * * > ( & input_items [ 0 ] ) ; // Get the input buffer pointer
2017-05-14 17:59:55 +00:00
// ############ 1. READ PSEUDORANGES ####
2018-08-11 21:03:41 +00:00
for ( uint32_t i = 0 ; i < d_nchannels ; i + + )
2017-04-20 14:10:12 +00:00
{
2018-03-08 17:32:55 +00:00
if ( in [ i ] [ epoch ] . Flag_valid_pseudorange )
2017-04-28 13:38:31 +00:00
{
2020-07-19 07:39:32 +00:00
const auto tmp_eph_iter_gps = d_internal_pvt_solver - > gps_ephemeris_map . find ( in [ i ] [ epoch ] . PRN ) ;
const auto tmp_eph_iter_gal = d_internal_pvt_solver - > galileo_ephemeris_map . find ( in [ i ] [ epoch ] . PRN ) ;
const auto tmp_eph_iter_cnav = d_internal_pvt_solver - > gps_cnav_ephemeris_map . find ( in [ i ] [ epoch ] . PRN ) ;
const auto tmp_eph_iter_glo_gnav = d_internal_pvt_solver - > glonass_gnav_ephemeris_map . find ( in [ i ] [ epoch ] . PRN ) ;
const auto tmp_eph_iter_bds_dnav = d_internal_pvt_solver - > beidou_dnav_ephemeris_map . find ( in [ i ] [ epoch ] . PRN ) ;
2018-11-29 15:53:40 +00:00
2019-08-01 17:23:38 +00:00
bool store_valid_observable = false ;
2019-08-01 18:09:35 +00:00
if ( tmp_eph_iter_gps ! = d_internal_pvt_solver - > gps_ephemeris_map . cend ( ) )
2019-08-01 17:23:38 +00:00
{
2020-07-19 07:39:32 +00:00
const uint32_t prn_aux = tmp_eph_iter_gps - > second . i_satellite_PRN ;
2019-08-01 17:23:38 +00:00
if ( ( prn_aux = = in [ i ] [ epoch ] . PRN ) and ( std : : string ( in [ i ] [ epoch ] . Signal ) = = " 1C " ) )
{
store_valid_observable = true ;
}
}
2019-08-01 18:09:35 +00:00
if ( tmp_eph_iter_gal ! = d_internal_pvt_solver - > galileo_ephemeris_map . cend ( ) )
2019-08-01 17:23:38 +00:00
{
2020-07-19 07:39:32 +00:00
const uint32_t prn_aux = tmp_eph_iter_gal - > second . i_satellite_PRN ;
2020-07-31 15:58:57 +00:00
if ( ( prn_aux = = in [ i ] [ epoch ] . PRN ) and ( ( std : : string ( in [ i ] [ epoch ] . Signal ) = = " 1B " ) or ( std : : string ( in [ i ] [ epoch ] . Signal ) = = " 5X " ) or ( std : : string ( in [ i ] [ epoch ] . Signal ) = = " 7X " ) ) )
2019-08-01 17:23:38 +00:00
{
store_valid_observable = true ;
}
}
2019-08-01 18:09:35 +00:00
if ( tmp_eph_iter_cnav ! = d_internal_pvt_solver - > gps_cnav_ephemeris_map . cend ( ) )
2019-08-01 17:23:38 +00:00
{
2020-07-19 07:39:32 +00:00
const uint32_t prn_aux = tmp_eph_iter_cnav - > second . i_satellite_PRN ;
2019-08-01 17:23:38 +00:00
if ( ( prn_aux = = in [ i ] [ epoch ] . PRN ) and ( ( std : : string ( in [ i ] [ epoch ] . Signal ) = = " 2S " ) or ( std : : string ( in [ i ] [ epoch ] . Signal ) = = " L5 " ) ) )
{
store_valid_observable = true ;
}
}
2019-08-01 18:09:35 +00:00
if ( tmp_eph_iter_glo_gnav ! = d_internal_pvt_solver - > glonass_gnav_ephemeris_map . cend ( ) )
2019-08-01 17:23:38 +00:00
{
2020-07-19 07:39:32 +00:00
const uint32_t prn_aux = tmp_eph_iter_glo_gnav - > second . i_satellite_PRN ;
2019-08-01 17:23:38 +00:00
if ( ( prn_aux = = in [ i ] [ epoch ] . PRN ) and ( ( std : : string ( in [ i ] [ epoch ] . Signal ) = = " 1G " ) or ( std : : string ( in [ i ] [ epoch ] . Signal ) = = " 2G " ) ) )
{
store_valid_observable = true ;
}
}
2019-08-01 18:09:35 +00:00
if ( tmp_eph_iter_bds_dnav ! = d_internal_pvt_solver - > beidou_dnav_ephemeris_map . cend ( ) )
2019-08-01 17:23:38 +00:00
{
2020-07-19 07:39:32 +00:00
const uint32_t prn_aux = tmp_eph_iter_bds_dnav - > second . i_satellite_PRN ;
2019-08-01 17:23:38 +00:00
if ( ( prn_aux = = in [ i ] [ epoch ] . PRN ) and ( ( std : : string ( in [ i ] [ epoch ] . Signal ) = = " B1 " ) or ( std : : string ( in [ i ] [ epoch ] . Signal ) = = " B3 " ) ) )
{
store_valid_observable = true ;
}
}
if ( store_valid_observable )
2017-05-12 15:58:04 +00:00
{
2017-05-14 17:59:55 +00:00
// store valid observables in a map.
2020-06-18 23:36:36 +00:00
d_gnss_observables_map . insert ( std : : pair < int , Gnss_Synchro > ( i , in [ i ] [ epoch ] ) ) ;
2017-04-20 14:10:12 +00:00
}
2019-08-01 17:23:38 +00:00
2020-06-18 23:36:36 +00:00
if ( d_rtcm_enabled )
2017-05-12 15:58:04 +00:00
{
2018-11-03 09:50:19 +00:00
try
2017-05-14 17:59:55 +00:00
{
2019-08-01 16:11:36 +00:00
if ( d_internal_pvt_solver - > gps_ephemeris_map . empty ( ) = = false )
2018-03-13 10:16:30 +00:00
{
2019-08-01 16:11:36 +00:00
if ( tmp_eph_iter_gps ! = d_internal_pvt_solver - > gps_ephemeris_map . cend ( ) )
2018-11-03 09:50:19 +00:00
{
2019-08-01 16:11:36 +00:00
d_rtcm_printer - > lock_time ( d_internal_pvt_solver - > gps_ephemeris_map . find ( in [ i ] [ epoch ] . PRN ) - > second , in [ i ] [ epoch ] . RX_time , in [ i ] [ epoch ] ) ; // keep track of locking time
2018-11-03 09:50:19 +00:00
}
2018-03-13 10:16:30 +00:00
}
2019-08-01 16:11:36 +00:00
if ( d_internal_pvt_solver - > galileo_ephemeris_map . empty ( ) = = false )
2018-03-13 10:16:30 +00:00
{
2019-08-01 16:11:36 +00:00
if ( tmp_eph_iter_gal ! = d_internal_pvt_solver - > galileo_ephemeris_map . cend ( ) )
2018-11-03 09:50:19 +00:00
{
2019-08-01 16:11:36 +00:00
d_rtcm_printer - > lock_time ( d_internal_pvt_solver - > galileo_ephemeris_map . find ( in [ i ] [ epoch ] . PRN ) - > second , in [ i ] [ epoch ] . RX_time , in [ i ] [ epoch ] ) ; // keep track of locking time
2018-11-03 09:50:19 +00:00
}
2018-03-13 10:16:30 +00:00
}
2019-08-01 16:11:36 +00:00
if ( d_internal_pvt_solver - > gps_cnav_ephemeris_map . empty ( ) = = false )
2018-03-13 10:16:30 +00:00
{
2019-08-01 16:11:36 +00:00
if ( tmp_eph_iter_cnav ! = d_internal_pvt_solver - > gps_cnav_ephemeris_map . cend ( ) )
2018-11-03 09:50:19 +00:00
{
2019-08-01 16:11:36 +00:00
d_rtcm_printer - > lock_time ( d_internal_pvt_solver - > gps_cnav_ephemeris_map . find ( in [ i ] [ epoch ] . PRN ) - > second , in [ i ] [ epoch ] . RX_time , in [ i ] [ epoch ] ) ; // keep track of locking time
2018-11-03 09:50:19 +00:00
}
2018-03-13 10:16:30 +00:00
}
2019-08-01 16:11:36 +00:00
if ( d_internal_pvt_solver - > glonass_gnav_ephemeris_map . empty ( ) = = false )
2018-03-13 10:16:30 +00:00
{
2019-08-01 16:11:36 +00:00
if ( tmp_eph_iter_glo_gnav ! = d_internal_pvt_solver - > glonass_gnav_ephemeris_map . cend ( ) )
2018-11-03 09:50:19 +00:00
{
2019-08-01 16:11:36 +00:00
d_rtcm_printer - > lock_time ( d_internal_pvt_solver - > glonass_gnav_ephemeris_map . find ( in [ i ] [ epoch ] . PRN ) - > second , in [ i ] [ epoch ] . RX_time , in [ i ] [ epoch ] ) ; // keep track of locking time
2018-11-03 09:50:19 +00:00
}
2018-03-13 10:16:30 +00:00
}
2017-07-18 03:09:10 +00:00
}
2018-11-03 09:50:19 +00:00
catch ( const boost : : exception & ex )
2018-08-09 18:36:11 +00:00
{
2020-07-07 16:53:50 +00:00
std : : cout < < " RTCM boost exception: " < < boost : : diagnostic_information ( ex ) < < ' \n ' ;
2018-11-03 09:50:19 +00:00
LOG ( ERROR ) < < " RTCM boost exception: " < < boost : : diagnostic_information ( ex ) ;
}
catch ( const std : : exception & ex )
{
2020-07-07 16:53:50 +00:00
std : : cout < < " RTCM std exception: " < < ex . what ( ) < < ' \n ' ;
2018-11-03 09:50:19 +00:00
LOG ( ERROR ) < < " RTCM std exception: " < < ex . what ( ) ;
2018-08-09 18:36:11 +00:00
}
2018-03-13 10:16:30 +00:00
}
2017-04-20 14:10:12 +00:00
}
2020-02-06 15:56:42 +00:00
else
{
2020-06-18 23:36:36 +00:00
d_channel_initialized . at ( i ) = false ; // the current channel is not reporting valid observable
2020-02-06 15:56:42 +00:00
}
2017-05-14 17:59:55 +00:00
}
2017-04-29 07:25:05 +00:00
2017-05-14 17:59:55 +00:00
// ############ 2 COMPUTE THE PVT ################################
2019-09-06 16:02:40 +00:00
bool flag_pvt_valid = false ;
2020-06-18 23:36:36 +00:00
if ( d_gnss_observables_map . empty ( ) = = false )
2017-05-14 17:59:55 +00:00
{
2020-06-18 23:36:36 +00:00
// LOG(INFO) << "diff raw obs time: " << d_gnss_observables_map.cbegin()->second.RX_time * 1000.0 - old_time_debug;
// old_time_debug = d_gnss_observables_map.cbegin()->second.RX_time * 1000.0;
2019-08-01 16:11:36 +00:00
uint32_t current_RX_time_ms = 0 ;
// #### solve PVT and store the corrected observable set
2020-06-18 23:36:36 +00:00
if ( d_internal_pvt_solver - > get_PVT ( d_gnss_observables_map , false ) )
2017-04-20 14:10:12 +00:00
{
2020-07-19 23:20:15 +00:00
const double Rx_clock_offset_s = d_internal_pvt_solver - > get_time_offset_s ( ) ;
2020-06-18 23:36:36 +00:00
if ( fabs ( Rx_clock_offset_s ) * 1000.0 > d_max_obs_block_rx_clock_offset_ms )
2019-07-31 16:16:09 +00:00
{
2019-08-01 16:11:36 +00:00
if ( ! d_waiting_obs_block_rx_clock_offset_correction_msg )
2019-07-31 16:16:09 +00:00
{
this - > message_port_pub ( pmt : : mp ( " pvt_to_observables " ) , pmt : : make_any ( Rx_clock_offset_s ) ) ;
2019-08-01 16:11:36 +00:00
d_waiting_obs_block_rx_clock_offset_correction_msg = true ;
2019-07-31 16:16:09 +00:00
LOG ( INFO ) < < " Sent clock offset correction to observables: " < < Rx_clock_offset_s < < " [s] " ;
}
2019-08-01 16:11:36 +00:00
}
else
{
2019-09-06 16:02:40 +00:00
if ( d_enable_rx_clock_correction = = true )
2019-07-31 16:16:09 +00:00
{
2019-09-06 16:02:40 +00:00
d_waiting_obs_block_rx_clock_offset_correction_msg = false ;
2020-06-18 23:36:36 +00:00
d_gnss_observables_map_t0 = d_gnss_observables_map_t1 ;
apply_rx_clock_offset ( d_gnss_observables_map , Rx_clock_offset_s ) ;
d_gnss_observables_map_t1 = d_gnss_observables_map ;
2019-08-01 16:11:36 +00:00
2019-09-06 16:02:40 +00:00
// ### select the rx_time and interpolate observables at that time
2020-06-18 23:36:36 +00:00
if ( ! d_gnss_observables_map_t0 . empty ( ) )
2019-07-31 16:16:09 +00:00
{
2020-07-19 23:20:15 +00:00
const auto t0_int_ms = static_cast < uint32_t > ( d_gnss_observables_map_t0 . cbegin ( ) - > second . RX_time * 1000.0 ) ;
const uint32_t adjust_next_20ms = 20 - t0_int_ms % 20 ;
2019-09-06 16:02:40 +00:00
current_RX_time_ms = t0_int_ms + adjust_next_20ms ;
if ( current_RX_time_ms % d_output_rate_ms = = 0 )
{
d_rx_time = static_cast < double > ( current_RX_time_ms ) / 1000.0 ;
2020-06-18 23:36:36 +00:00
// std::cout << " obs time t0: " << d_gnss_observables_map_t0.cbegin()->second.RX_time
// << " t1: " << d_gnss_observables_map_t1.cbegin()->second.RX_time
2020-07-07 16:53:50 +00:00
// << " interp time: " << d_rx_time << '\n';
2020-06-18 23:36:36 +00:00
d_gnss_observables_map = interpolate_observables ( d_gnss_observables_map_t0 ,
d_gnss_observables_map_t1 ,
2019-09-06 16:02:40 +00:00
d_rx_time ) ;
flag_compute_pvt_output = true ;
// d_rx_time = current_RX_time;
// std::cout.precision(17);
2020-07-07 16:53:50 +00:00
// std::cout << "current_RX_time: " << current_RX_time << " map time: " << d_gnss_observables_map.begin()->second.RX_time << '\n';
2019-09-06 16:02:40 +00:00
}
2019-07-31 16:16:09 +00:00
}
}
2019-09-06 16:02:40 +00:00
else
{
2020-06-18 23:36:36 +00:00
d_rx_time = d_gnss_observables_map . begin ( ) - > second . RX_time ;
2019-09-06 16:19:40 +00:00
current_RX_time_ms = static_cast < uint32_t > ( d_rx_time * 1000.0 ) ;
if ( current_RX_time_ms % d_output_rate_ms = = 0 )
{
flag_compute_pvt_output = true ;
// std::cout.precision(17);
2020-07-07 16:53:50 +00:00
// std::cout << "current_RX_time: " << current_RX_time << " map time: " << d_gnss_observables_map.begin()->second.RX_time << '\n';
2019-09-06 16:19:40 +00:00
}
2019-09-06 16:02:40 +00:00
flag_pvt_valid = true ;
}
2019-07-31 16:16:09 +00:00
}
2017-05-12 15:58:04 +00:00
}
2019-08-01 18:09:35 +00:00
// debug code
2019-08-17 13:47:20 +00:00
// else
// {
// DLOG(INFO) << "Internal PVT solver error";
// }
2017-05-14 17:59:55 +00:00
// compute on the fly PVT solution
if ( flag_compute_pvt_output = = true )
2017-05-12 15:58:04 +00:00
{
2020-06-18 23:36:36 +00:00
flag_pvt_valid = d_user_pvt_solver - > get_PVT ( d_gnss_observables_map , false ) ;
2019-09-06 16:02:40 +00:00
}
if ( flag_pvt_valid = = true )
{
2020-02-11 20:47:13 +00:00
// initialize (if needed) the accumulated phase offset and apply it to the active channels
// required to report accumulated phase cycles comparable to pseudoranges
2020-02-06 15:56:42 +00:00
initialize_and_apply_carrier_phase_offset ( ) ;
2020-07-19 07:39:32 +00:00
const double Rx_clock_offset_s = d_user_pvt_solver - > get_time_offset_s ( ) ;
2019-09-06 16:02:40 +00:00
if ( d_enable_rx_clock_correction = = true and fabs ( Rx_clock_offset_s ) > 0.000001 ) // 1us !!
{
LOG ( INFO ) < < " Warning: Rx clock offset at interpolated RX time: " < < Rx_clock_offset_s * 1000.0 < < " [ms] "
< < " at RX time: " < < static_cast < uint32_t > ( d_rx_time * 1000.0 ) < < " [ms] " ;
}
else
2017-05-12 15:58:04 +00:00
{
2019-09-06 16:02:40 +00:00
DLOG ( INFO ) < < " Rx clock offset at interpolated RX time: " < < Rx_clock_offset_s * 1000.0 < < " [s] "
< < " at RX time: " < < static_cast < uint32_t > ( d_rx_time * 1000.0 ) < < " [ms] " ;
// Optional debug code: export observables snapshot for rtklib unit testing
2020-07-07 16:53:50 +00:00
// std::cout << "step 1: save gnss_synchro map\n";
2019-09-06 16:02:40 +00:00
// save_gnss_synchro_map_xml("./gnss_synchro_map.xml");
// getchar(); // stop the execution
// end debug
if ( d_display_rate_ms ! = 0 )
2019-04-23 15:31:26 +00:00
{
2019-09-06 16:02:40 +00:00
if ( current_RX_time_ms % d_display_rate_ms = = 0 )
{
flag_display_pvt = true ;
}
2019-04-23 15:31:26 +00:00
}
2019-09-06 16:02:40 +00:00
if ( d_rtcm_MT1019_rate_ms ! = 0 ) // allows deactivating messages by setting rate = 0
2017-04-20 14:10:12 +00:00
{
2019-09-06 16:02:40 +00:00
if ( current_RX_time_ms % d_rtcm_MT1019_rate_ms = = 0 )
2018-11-08 09:02:52 +00:00
{
2019-09-06 16:02:40 +00:00
flag_write_RTCM_1019_output = true ;
2018-11-08 09:02:52 +00:00
}
2019-09-06 16:02:40 +00:00
}
if ( d_rtcm_MT1020_rate_ms ! = 0 ) // allows deactivating messages by setting rate = 0
{
if ( current_RX_time_ms % d_rtcm_MT1020_rate_ms = = 0 )
2018-11-03 09:50:19 +00:00
{
2019-09-06 16:02:40 +00:00
flag_write_RTCM_1020_output = true ;
2018-11-03 09:50:19 +00:00
}
2019-09-06 16:02:40 +00:00
}
if ( d_rtcm_MT1045_rate_ms ! = 0 )
{
if ( current_RX_time_ms % d_rtcm_MT1045_rate_ms = = 0 )
2018-11-03 09:50:19 +00:00
{
2019-09-06 16:02:40 +00:00
flag_write_RTCM_1045_output = true ;
2018-11-03 09:50:19 +00:00
}
2019-09-06 16:02:40 +00:00
}
// TODO: RTCM 1077, 1087 and 1097 are not used, so, disable the output rates
// if (current_RX_time_ms % d_rtcm_MT1077_rate_ms==0 and d_rtcm_MT1077_rate_ms != 0)
// {
// last_RTCM_1077_output_time = current_RX_time;
// }
// if (current_RX_time_ms % d_rtcm_MT1087_rate_ms==0 and d_rtcm_MT1087_rate_ms != 0)
// {
// last_RTCM_1087_output_time = current_RX_time;
// }
// if (current_RX_time_ms % d_rtcm_MT1097_rate_ms==0 and d_rtcm_MT1097_rate_ms != 0)
// {
// last_RTCM_1097_output_time = current_RX_time;
// }
if ( d_rtcm_MSM_rate_ms ! = 0 )
{
if ( current_RX_time_ms % d_rtcm_MSM_rate_ms = = 0 )
2018-11-03 09:50:19 +00:00
{
2019-09-06 16:02:40 +00:00
flag_write_RTCM_MSM_output = true ;
2018-11-03 09:50:19 +00:00
}
2019-09-06 16:02:40 +00:00
}
if ( d_rinexobs_rate_ms ! = 0 )
{
if ( current_RX_time_ms % static_cast < uint32_t > ( d_rinexobs_rate_ms ) = = 0 )
2018-11-03 09:50:19 +00:00
{
2019-09-06 16:02:40 +00:00
flag_write_RINEX_obs_output = true ;
2018-11-03 09:50:19 +00:00
}
2019-09-06 16:02:40 +00:00
}
2020-06-18 23:36:36 +00:00
if ( d_first_fix = = true )
2019-09-06 16:02:40 +00:00
{
if ( d_show_local_time_zone )
2018-11-03 09:50:19 +00:00
{
2020-07-19 23:20:15 +00:00
const boost : : posix_time : : ptime time_first_solution = d_user_pvt_solver - > get_position_UTC_time ( ) + d_utc_diff_time ;
2019-09-06 16:02:40 +00:00
std : : cout < < " First position fix at " < < time_first_solution < < d_local_time_str ;
2018-11-03 09:50:19 +00:00
}
2019-09-06 16:02:40 +00:00
else
2019-04-23 15:31:26 +00:00
{
2019-09-06 16:02:40 +00:00
std : : cout < < " First position fix at " < < d_user_pvt_solver - > get_position_UTC_time ( ) < < " UTC " ;
2019-04-23 15:31:26 +00:00
}
2019-09-06 16:02:40 +00:00
std : : cout < < " is Lat = " < < d_user_pvt_solver - > get_latitude ( ) < < " [deg], Long = " < < d_user_pvt_solver - > get_longitude ( )
2020-07-07 16:53:50 +00:00
< < " [deg], Height= " < < d_user_pvt_solver - > get_height ( ) < < " [m] \n " ;
2020-06-18 23:36:36 +00:00
d_ttff_msgbuf ttff ;
2019-09-06 16:02:40 +00:00
ttff . mtype = 1 ;
2020-06-18 23:36:36 +00:00
d_end = std : : chrono : : system_clock : : now ( ) ;
std : : chrono : : duration < double > elapsed_seconds = d_end - d_start ;
2019-09-06 16:02:40 +00:00
ttff . ttff = elapsed_seconds . count ( ) ;
send_sys_v_ttff_msg ( ttff ) ;
2020-06-18 23:36:36 +00:00
d_first_fix = false ;
2019-09-06 16:02:40 +00:00
}
if ( d_kml_output_enabled )
{
if ( current_RX_time_ms % d_kml_rate_ms = = 0 )
2019-03-15 12:31:18 +00:00
{
2020-06-18 09:49:28 +00:00
d_kml_dump - > print_position ( d_user_pvt_solver . get ( ) , false ) ;
2019-03-15 12:31:18 +00:00
}
2019-09-06 16:02:40 +00:00
}
if ( d_gpx_output_enabled )
{
if ( current_RX_time_ms % d_gpx_rate_ms = = 0 )
2019-03-15 12:31:18 +00:00
{
2020-06-18 09:49:28 +00:00
d_gpx_dump - > print_position ( d_user_pvt_solver . get ( ) , false ) ;
2019-04-23 15:31:26 +00:00
}
2019-09-06 16:02:40 +00:00
}
if ( d_geojson_output_enabled )
{
if ( current_RX_time_ms % d_geojson_rate_ms = = 0 )
2019-04-23 15:31:26 +00:00
{
2020-06-18 09:49:28 +00:00
d_geojson_printer - > print_position ( d_user_pvt_solver . get ( ) , false ) ;
2019-04-23 15:31:26 +00:00
}
2019-09-06 16:02:40 +00:00
}
if ( d_nmea_output_file_enabled )
{
if ( current_RX_time_ms % d_nmea_rate_ms = = 0 )
2019-04-23 15:31:26 +00:00
{
2020-06-18 09:49:28 +00:00
d_nmea_printer - > Print_Nmea_Line ( d_user_pvt_solver . get ( ) , false ) ;
2019-03-15 12:31:18 +00:00
}
2019-09-06 16:02:40 +00:00
}
2020-06-18 23:36:36 +00:00
if ( d_rinex_output_enabled )
2019-09-06 16:02:40 +00:00
{
2020-11-14 00:57:29 +00:00
d_rp - > print_rinex_annotation ( d_user_pvt_solver . get ( ) , d_gnss_observables_map , d_rx_time , d_type_of_rx , flag_write_RINEX_obs_output ) ;
2019-09-06 16:02:40 +00:00
}
2020-11-14 00:57:29 +00:00
if ( d_rtcm_enabled )
2019-09-06 16:02:40 +00:00
{
2020-11-14 00:57:29 +00:00
d_rtcm_printer - > Print_Rtcm_Messages ( d_user_pvt_solver . get ( ) ,
d_gnss_observables_map ,
d_rx_time ,
d_type_of_rx ,
d_rtcm_MSM_rate_ms ,
d_rtcm_MT1019_rate_ms ,
d_rtcm_MT1020_rate_ms ,
d_rtcm_MT1045_rate_ms ,
d_rtcm_MT1077_rate_ms ,
d_rtcm_MT1097_rate_ms ,
flag_write_RTCM_MSM_output ,
flag_write_RTCM_1019_output ,
flag_write_RTCM_1020_output ,
flag_write_RTCM_1045_output ,
d_enable_rx_clock_correction ) ;
2018-03-13 10:16:30 +00:00
}
2017-05-14 17:59:55 +00:00
}
2017-04-29 07:25:05 +00:00
}
2017-05-14 17:59:55 +00:00
// DEBUG MESSAGE: Display position in console output
2019-08-01 16:11:36 +00:00
if ( d_user_pvt_solver - > is_valid_position ( ) and flag_display_pvt )
2017-05-14 17:59:55 +00:00
{
2019-03-15 12:31:18 +00:00
boost : : posix_time : : ptime time_solution ;
std : : string UTC_solution_str ;
if ( d_show_local_time_zone )
{
2019-08-01 16:11:36 +00:00
time_solution = d_user_pvt_solver - > get_position_UTC_time ( ) + d_utc_diff_time ;
2019-03-16 11:47:36 +00:00
UTC_solution_str = d_local_time_str ;
2019-03-15 12:31:18 +00:00
}
else
{
2019-08-01 16:11:36 +00:00
time_solution = d_user_pvt_solver - > get_position_UTC_time ( ) ;
2019-03-15 12:31:18 +00:00
UTC_solution_str = " UTC " ;
}
2018-06-05 19:41:13 +00:00
std : : streamsize ss = std : : cout . precision ( ) ; // save current precision
2018-06-07 10:23:26 +00:00
std : : cout . setf ( std : : ios : : fixed , std : : ios : : floatfield ) ;
2020-12-30 20:49:29 +00:00
auto * facet = new boost : : posix_time : : time_facet ( " %Y-%b-%d %H:%M:%S.%f %z " ) ;
2018-06-07 10:23:26 +00:00
std : : cout . imbue ( std : : locale ( std : : cout . getloc ( ) , facet ) ) ;
2019-03-15 12:31:18 +00:00
std : : cout
< < TEXT_BOLD_GREEN
< < " Position at " < < time_solution < < UTC_solution_str
2019-08-01 16:11:36 +00:00
< < " using " < < d_user_pvt_solver - > get_num_valid_observations ( )
2019-03-15 12:31:18 +00:00
< < std : : fixed < < std : : setprecision ( 9 )
2019-08-01 16:11:36 +00:00
< < " observations is Lat = " < < d_user_pvt_solver - > get_latitude ( ) < < " [deg], Long = " < < d_user_pvt_solver - > get_longitude ( )
2019-03-15 12:31:18 +00:00
< < std : : fixed < < std : : setprecision ( 3 )
2020-07-07 16:53:50 +00:00
< < " [deg], Height = " < < d_user_pvt_solver - > get_height ( ) < < " [m] " < < TEXT_RESET < < ' \n ' ;
2018-06-07 10:23:26 +00:00
2018-06-05 16:20:55 +00:00
std : : cout < < std : : setprecision ( ss ) ;
2019-08-01 16:11:36 +00:00
DLOG ( INFO ) < < " RX clock offset: " < < d_user_pvt_solver - > get_time_offset_s ( ) < < " [s] " ;
2018-05-18 19:07:12 +00:00
2019-11-04 12:04:46 +00:00
std : : cout
< < TEXT_BOLD_GREEN
< < " Velocity: " < < std : : fixed < < std : : setprecision ( 3 )
< < " East: " < < d_user_pvt_solver - > get_rx_vel ( ) [ 0 ] < < " [m/s], North: " < < d_user_pvt_solver - > get_rx_vel ( ) [ 1 ]
2020-07-07 16:53:50 +00:00
< < " [m/s], Up = " < < d_user_pvt_solver - > get_rx_vel ( ) [ 2 ] < < " [m/s] " < < TEXT_RESET < < ' \n ' ;
2019-11-04 12:04:46 +00:00
std : : cout < < std : : setprecision ( ss ) ;
DLOG ( INFO ) < < " RX clock drift: " < < d_user_pvt_solver - > get_clock_drift_ppm ( ) < < " [ppm] " ;
2018-06-05 19:41:13 +00:00
// boost::posix_time::ptime p_time;
2019-08-01 16:11:36 +00:00
// gtime_t rtklib_utc_time = gpst2time(adjgpsweek(d_user_pvt_solver->gps_ephemeris_map.cbegin()->second.i_GPS_week), d_rx_time);
2018-06-05 19:41:13 +00:00
// p_time = boost::posix_time::from_time_t(rtklib_utc_time.time);
// p_time += boost::posix_time::microseconds(round(rtklib_utc_time.sec * 1e6));
2020-07-07 16:53:50 +00:00
// std::cout << TEXT_MAGENTA << "Observable RX time (GPST) " << boost::posix_time::to_simple_string(p_time) << TEXT_RESET << '\n';
2018-05-18 19:07:12 +00:00
2019-08-01 16:11:36 +00:00
DLOG ( INFO ) < < " Position at " < < boost : : posix_time : : to_simple_string ( d_user_pvt_solver - > get_position_UTC_time ( ) )
< < " UTC using " < < d_user_pvt_solver - > get_num_valid_observations ( ) < < " observations is Lat = " < < d_user_pvt_solver - > get_latitude ( ) < < " [deg], Long = " < < d_user_pvt_solver - > get_longitude ( )
< < " [deg], Height = " < < d_user_pvt_solver - > get_height ( ) < < " [m] " ;
2017-08-16 10:45:00 +00:00
2019-08-01 16:11:36 +00:00
/* std::cout << "Dilution of Precision at " << boost::posix_time::to_simple_string(d_user_pvt_solver->get_position_UTC_time())
< < " UTC using " < < d_user_pvt_solver - > get_num_valid_observations ( ) < < " observations is HDOP = " < < d_user_pvt_solver - > get_hdop ( ) < < " VDOP = "
< < d_user_pvt_solver - > get_vdop ( )
2020-07-07 16:53:50 +00:00
< < " GDOP = " < < d_user_pvt_solver - > get_gdop ( ) < < ' \n ' ; */
2017-05-14 17:59:55 +00:00
}
2019-01-21 10:59:29 +00:00
// PVT MONITOR
2019-08-01 16:11:36 +00:00
if ( d_user_pvt_solver - > is_valid_position ( ) )
2019-01-21 10:59:29 +00:00
{
2020-07-19 07:39:32 +00:00
const std : : shared_ptr < Monitor_Pvt > monitor_pvt = std : : make_shared < Monitor_Pvt > ( d_user_pvt_solver - > get_monitor_pvt ( ) ) ;
2019-07-11 16:39:28 +00:00
2019-08-01 18:09:35 +00:00
// publish new position to the gnss_flowgraph channel status monitor
2019-07-11 16:39:28 +00:00
if ( current_RX_time_ms % d_report_rate_ms = = 0 )
{
this - > message_port_pub ( pmt : : mp ( " status " ) , pmt : : make_any ( monitor_pvt ) ) ;
}
2020-06-18 23:36:36 +00:00
if ( d_flag_monitor_pvt_enabled )
2019-07-11 16:39:28 +00:00
{
2020-06-18 23:36:36 +00:00
d_udp_sink_ptr - > write_monitor_pvt ( monitor_pvt . get ( ) ) ;
2019-07-11 16:39:28 +00:00
}
2019-01-21 10:59:29 +00:00
}
2017-04-28 13:38:31 +00:00
}
2017-04-20 14:10:12 +00:00
}
2017-04-28 13:38:31 +00:00
2017-04-29 07:25:05 +00:00
return noutput_items ;
2017-04-20 14:10:12 +00:00
}