From dd7b1f9f6ae318ddac362cb34ccdff9762d13487 Mon Sep 17 00:00:00 2001 From: "M.A. Gomez" Date: Wed, 12 Jul 2023 17:26:45 +0200 Subject: [PATCH] [ADD] estatic_measures_sd flag --- src/algorithms/PVT/adapters/rtklib_pvt.cc | 1975 ++++++------ src/algorithms/PVT/libs/pvt_conf.h | 219 +- src/algorithms/PVT/libs/pvt_kf.cc | 309 +- src/algorithms/PVT/libs/pvt_kf.h | 140 +- src/algorithms/PVT/libs/rtklib_solver.cc | 3559 +++++++++++---------- 5 files changed, 3111 insertions(+), 3091 deletions(-) mode change 100644 => 100755 src/algorithms/PVT/adapters/rtklib_pvt.cc mode change 100644 => 100755 src/algorithms/PVT/libs/pvt_conf.h mode change 100644 => 100755 src/algorithms/PVT/libs/pvt_kf.cc mode change 100644 => 100755 src/algorithms/PVT/libs/pvt_kf.h mode change 100644 => 100755 src/algorithms/PVT/libs/rtklib_solver.cc diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.cc b/src/algorithms/PVT/adapters/rtklib_pvt.cc old mode 100644 new mode 100755 index 9bf6174ab..ad967d953 --- a/src/algorithms/PVT/adapters/rtklib_pvt.cc +++ b/src/algorithms/PVT/adapters/rtklib_pvt.cc @@ -1,987 +1,988 @@ -/*! - * \file rtklib_pvt.cc - * \brief Interface of a Position Velocity and Time computation block - * \author Javier Arribas, 2017. jarribas(at)cttc.es - * - * ----------------------------------------------------------------------------- - * - * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. - * This file is part of GNSS-SDR. - * - * Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors) - * SPDX-License-Identifier: GPL-3.0-or-later - * - * ----------------------------------------------------------------------------- - */ - - -#include "rtklib_pvt.h" -#include "MATH_CONSTANTS.h" // for D2R -#include "configuration_interface.h" // for ConfigurationInterface -#include "galileo_almanac.h" // for Galileo_Almanac -#include "galileo_ephemeris.h" // for Galileo_Ephemeris -#include "gnss_sdr_flags.h" // for FLAGS_RINEX_version -#include "gnss_sdr_string_literals.h" // for std::string_literals -#include "gps_almanac.h" // for Gps_Almanac -#include "gps_ephemeris.h" // for Gps_Ephemeris -#include "pvt_conf.h" // for Pvt_Conf -#include "rtklib_rtkpos.h" // for rtkfree, rtkinit -#include // for LOG -#include // for std::cout -#if USE_STD_COMMON_FACTOR -#include -namespace bc = std; -#else -#if USE_OLD_BOOST_MATH_COMMON_FACTOR -#include -namespace bc = boost::math; -#else -#include -namespace bc = boost::integer; -#endif -#endif - -using namespace std::string_literals; - -Rtklib_Pvt::Rtklib_Pvt(const ConfigurationInterface* configuration, - const std::string& role, - unsigned int in_streams, - unsigned int out_streams) : role_(role), - in_streams_(in_streams), - out_streams_(out_streams) -{ - Pvt_Conf pvt_output_parameters = Pvt_Conf(); - // dump parameters - const std::string default_dump_filename("./pvt.dat"); - const std::string default_nmea_dump_filename("./nmea_pvt.nmea"); - const std::string default_nmea_dump_devname("/dev/tty1"); - const std::string default_rtcm_dump_devname("/dev/pts/1"); - DLOG(INFO) << "role " << role; - pvt_output_parameters.dump = configuration->property(role + ".dump", false); - pvt_output_parameters.dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); - pvt_output_parameters.dump_mat = configuration->property(role + ".dump_mat", true); - - pvt_output_parameters.rtk_trace_level = configuration->property(role + ".rtk_trace_level"s, 0); - - // Flag to postprocess old gnss records (older than 2009) and avoid wrong week rollover - pvt_output_parameters.pre_2009_file = configuration->property("GNSS-SDR.pre_2009_file", false); - - // output rate - pvt_output_parameters.observable_interval_ms = configuration->property("GNSS-SDR.observable_interval_ms", pvt_output_parameters.observable_interval_ms); - - pvt_output_parameters.output_rate_ms = bc::lcm(static_cast(pvt_output_parameters.observable_interval_ms), configuration->property(role + ".output_rate_ms", 500)); - - // display rate - pvt_output_parameters.display_rate_ms = bc::lcm(pvt_output_parameters.output_rate_ms, configuration->property(role + ".display_rate_ms", 500)); - - // PVT KF settings - pvt_output_parameters.enable_pvt_kf = configuration->property(role + ".enable_pvt_kf", false); - pvt_output_parameters.measures_ecef_pos_sd_m = configuration->property(role + ".kf_measures_ecef_pos_sd_m", 1.0); - pvt_output_parameters.measures_ecef_vel_sd_ms = configuration->property(role + ".kf_measures_ecef_vel_sd_ms", 0.1); - pvt_output_parameters.system_ecef_pos_sd_m = configuration->property(role + ".kf_system_ecef_pos_sd_m", 0.01); - pvt_output_parameters.system_ecef_vel_sd_ms = configuration->property(role + ".kf_system_ecef_vel_sd_ms", 0.001); - - // NMEA Printer settings - pvt_output_parameters.flag_nmea_tty_port = configuration->property(role + ".flag_nmea_tty_port", false); - pvt_output_parameters.nmea_dump_filename = configuration->property(role + ".nmea_dump_filename", default_nmea_dump_filename); - pvt_output_parameters.nmea_dump_devname = configuration->property(role + ".nmea_dump_devname", default_nmea_dump_devname); - - // RINEX version - pvt_output_parameters.rinex_version = configuration->property(role + ".rinex_version", 3); - if (FLAGS_RINEX_version == "3.01" || FLAGS_RINEX_version == "3.02" || FLAGS_RINEX_version == "3") - { - pvt_output_parameters.rinex_version = 3; - } - else if (FLAGS_RINEX_version == "2.10" || FLAGS_RINEX_version == "2.11" || FLAGS_RINEX_version == "2") - { - pvt_output_parameters.rinex_version = 2; - } - pvt_output_parameters.rinexobs_rate_ms = bc::lcm(configuration->property(role + ".rinexobs_rate_ms", 1000), pvt_output_parameters.output_rate_ms); - pvt_output_parameters.rinex_name = configuration->property(role + ".rinex_name", std::string("-")); - if (FLAGS_RINEX_name != "-") - { - pvt_output_parameters.rinex_name = FLAGS_RINEX_name; - } - - // RTCM Printer settings - pvt_output_parameters.flag_rtcm_tty_port = configuration->property(role + ".flag_rtcm_tty_port", false); - pvt_output_parameters.rtcm_dump_devname = configuration->property(role + ".rtcm_dump_devname", default_rtcm_dump_devname); - pvt_output_parameters.flag_rtcm_server = configuration->property(role + ".flag_rtcm_server", false); - pvt_output_parameters.rtcm_tcp_port = configuration->property(role + ".rtcm_tcp_port", 2101); - pvt_output_parameters.rtcm_station_id = configuration->property(role + ".rtcm_station_id", 1234); - // RTCM message rates: least common multiple with output_rate_ms - const int rtcm_MT1019_rate_ms = bc::lcm(configuration->property(role + ".rtcm_MT1019_rate_ms", 5000), pvt_output_parameters.output_rate_ms); - const int rtcm_MT1020_rate_ms = bc::lcm(configuration->property(role + ".rtcm_MT1020_rate_ms", 5000), pvt_output_parameters.output_rate_ms); - const int rtcm_MT1045_rate_ms = bc::lcm(configuration->property(role + ".rtcm_MT1045_rate_ms", 5000), pvt_output_parameters.output_rate_ms); - const int rtcm_MSM_rate_ms = bc::lcm(configuration->property(role + ".rtcm_MSM_rate_ms", 1000), pvt_output_parameters.output_rate_ms); - const int rtcm_MT1077_rate_ms = bc::lcm(configuration->property(role + ".rtcm_MT1077_rate_ms", rtcm_MSM_rate_ms), pvt_output_parameters.output_rate_ms); - const int rtcm_MT1087_rate_ms = bc::lcm(configuration->property(role + ".rtcm_MT1087_rate_ms", rtcm_MSM_rate_ms), pvt_output_parameters.output_rate_ms); - const int rtcm_MT1097_rate_ms = bc::lcm(configuration->property(role + ".rtcm_MT1097_rate_ms", rtcm_MSM_rate_ms), pvt_output_parameters.output_rate_ms); - - pvt_output_parameters.rtcm_msg_rate_ms[1019] = rtcm_MT1019_rate_ms; - pvt_output_parameters.rtcm_msg_rate_ms[1020] = rtcm_MT1020_rate_ms; - pvt_output_parameters.rtcm_msg_rate_ms[1045] = rtcm_MT1045_rate_ms; - for (int k = 1071; k < 1078; k++) // All GPS MSM - { - pvt_output_parameters.rtcm_msg_rate_ms[k] = rtcm_MT1077_rate_ms; - } - for (int k = 1081; k < 1088; k++) // All GLONASS MSM - { - pvt_output_parameters.rtcm_msg_rate_ms[k] = rtcm_MT1087_rate_ms; - } - for (int k = 1091; k < 1098; k++) // All Galileo MSM - { - pvt_output_parameters.rtcm_msg_rate_ms[k] = rtcm_MT1097_rate_ms; - } - - // Advanced Nativation Protocol Printer settings - pvt_output_parameters.an_output_enabled = configuration->property(role + ".an_output_enabled", pvt_output_parameters.an_output_enabled); - pvt_output_parameters.an_dump_devname = configuration->property(role + ".an_dump_devname", default_nmea_dump_devname); - if (pvt_output_parameters.an_output_enabled && pvt_output_parameters.flag_nmea_tty_port) - { - if (pvt_output_parameters.nmea_dump_devname == pvt_output_parameters.an_dump_devname) - { - std::cerr << "Warning: NMEA an Advanced Nativation printers set to write to the same serial port.\n" - << "Please make sure that PVT.an_dump_devname and PVT.an_dump_devname are different.\n" - << "Shutting down the NMEA serial output.\n"; - pvt_output_parameters.flag_nmea_tty_port = false; - } - } - if (pvt_output_parameters.an_output_enabled && pvt_output_parameters.flag_rtcm_tty_port) - { - if (pvt_output_parameters.rtcm_dump_devname == pvt_output_parameters.an_dump_devname) - { - std::cerr << "Warning: RTCM an Advanced Nativation printers set to write to the same serial port.\n" - << "Please make sure that PVT.an_dump_devname and .rtcm_dump_devname are different.\n" - << "Shutting down the RTCM serial output.\n"; - pvt_output_parameters.flag_rtcm_tty_port = false; - } - } - - pvt_output_parameters.kml_rate_ms = bc::lcm(configuration->property(role + ".kml_rate_ms", pvt_output_parameters.kml_rate_ms), pvt_output_parameters.output_rate_ms); - pvt_output_parameters.gpx_rate_ms = bc::lcm(configuration->property(role + ".gpx_rate_ms", pvt_output_parameters.gpx_rate_ms), pvt_output_parameters.output_rate_ms); - pvt_output_parameters.geojson_rate_ms = bc::lcm(configuration->property(role + ".geojson_rate_ms", pvt_output_parameters.geojson_rate_ms), pvt_output_parameters.output_rate_ms); - pvt_output_parameters.nmea_rate_ms = bc::lcm(configuration->property(role + ".nmea_rate_ms", pvt_output_parameters.nmea_rate_ms), pvt_output_parameters.output_rate_ms); - pvt_output_parameters.an_rate_ms = configuration->property(role + ".an_rate_ms", pvt_output_parameters.an_rate_ms); - - // Infer the type of receiver - /* - * TYPE | RECEIVER - * 0 | Unknown - * 1 | GPS L1 C/A - * 2 | GPS L2C - * 3 | GPS L5 - * 4 | Galileo E1B - * 5 | Galileo E5a - * 6 | Galileo E5b - * 7 | GPS L1 C/A + GPS L2C - * 8 | GPS L1 C/A + GPS L5 - * 9 | GPS L1 C/A + Galileo E1B - * 10 | GPS L1 C/A + Galileo E5a - * 11 | GPS L1 C/A + Galileo E5b - * 12 | Galileo E1B + GPS L2C - * 13 | Galileo E5a + GPS L5 - * 14 | Galileo E1B + Galileo E5a - * 15 | Galileo E1B + Galileo E5b - * 16 | GPS L2C + GPS L5 - * 17 | GPS L2C + Galileo E5a - * 18 | GPS L2C + Galileo E5b - * 19 | Galileo E5a + Galileo E5b - * 20 | GPS L5 + Galileo E5b - * 21 | GPS L1 C/A + Galileo E1B + GPS L2C - * 22 | GPS L1 C/A + Galileo E1B + GPS L5 - * 23 | GLONASS L1 C/A - * 24 | GLONASS L2 C/A - * 25 | GLONASS L1 C/A + GLONASS L2 C/A - * 26 | GPS L1 C/A + GLONASS L1 C/A - * 27 | Galileo E1B + GLONASS L1 C/A - * 28 | GPS L2C + GLONASS L1 C/A - * 29 | GPS L1 C/A + GLONASS L2 C/A - * 30 | Galileo E1B + GLONASS L2 C/A - * 31 | GPS L2C + GLONASS L2 C/A - * 32 | GPS L1 C/A + Galileo E1B + GPS L5 + Galileo E5a - * 33 | GPS L1 C/A + Galileo E1B + Galileo E5a - * - * Skipped previous values to avoid overlapping - * 100 | Galileo E6B - * 101 | Galileo E1B + Galileo E6B - * 102 | Galileo E5a + Galileo E6B - * 103 | Galileo E5b + Galileo E6B - * 104 | Galileo E1B + Galileo E5a + Galileo E6B - * 105 | Galileo E1B + Galileo E5b + Galileo E6B - * 106 | GPS L1 C/A + Galileo E1B + Galileo E6B - * 107 | GPS L1 C/A + Galileo E6B - * Skipped previous values to avoid overlapping - * 500 | BeiDou B1I - * 501 | BeiDou B1I + GPS L1 C/A - * 502 | BeiDou B1I + Galileo E1B - * 503 | BeiDou B1I + GLONASS L1 C/A - * 504 | BeiDou B1I + GPS L1 C/A + Galileo E1B - * 505 | BeiDou B1I + GPS L1 C/A + GLONASS L1 C/A + Galileo E1B - * 506 | BeiDou B1I + Beidou B3I - * Skipped previous values to avoid overlapping - * 600 | BeiDou B3I - * 601 | BeiDou B3I + GPS L2C - * 602 | BeiDou B3I + GLONASS L2 C/A - * 603 | BeiDou B3I + GPS L2C + GLONASS L2 C/A - * 604 | BeiDou B3I + GPS L1 C/A - * 605 | BeiDou B3I + Galileo E1B - * 606 | BeiDou B3I + GLONASS L1 C/A - * 607 | BeiDou B3I + GPS L1 C/A + Galileo E1B - * 608 | BeiDou B3I + GPS L1 C/A + Galileo E1B + BeiDou B1I - * 609 | BeiDou B3I + GPS L1 C/A + Galileo E1B + GLONASS L1 C/A - * 610 | BeiDou B3I + GPS L1 C/A + Galileo E1B + GLONASS L1 C/A + BeiDou B1I - * - * 1000 | GPS L1 C/A + GPS L2C + GPS L5 - * 1001 | GPS L1 C/A + Galileo E1B + GPS L2C + GPS L5 + Galileo E5a - */ - const int gps_1C_count = configuration->property("Channels_1C.count", 0); - const int gps_2S_count = configuration->property("Channels_2S.count", 0); - const int gps_L5_count = configuration->property("Channels_L5.count", 0); - const int gal_1B_count = configuration->property("Channels_1B.count", 0); - const int gal_E5a_count = configuration->property("Channels_5X.count", 0); - const int gal_E5b_count = configuration->property("Channels_7X.count", 0); - const int gal_E6_count = configuration->property("Channels_E6.count", 0); - const int glo_1G_count = configuration->property("Channels_1G.count", 0); - const int glo_2G_count = configuration->property("Channels_2G.count", 0); - const int bds_B1_count = configuration->property("Channels_B1.count", 0); - const int bds_B3_count = configuration->property("Channels_B3.count", 0); - - if ((gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 1; // L1 - } - if ((gps_1C_count == 0) && (gps_2S_count != 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 2; // GPS L2C - } - if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count != 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 3; // L5 - } - if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 4; // E1 - } - if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 5; // E5a - } - if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 6; - } - if ((gps_1C_count != 0) && (gps_2S_count != 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 7; // GPS L1 C/A + GPS L2C - } - if ((gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count != 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 8; // L1+L5 - } - if ((gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 9; // L1+E1 - } - if ((gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 10; // GPS L1 C/A + Galileo E5a - } - if ((gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 11; - } - if ((gps_1C_count == 0) && (gps_2S_count != 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 12; // Galileo E1B + GPS L2C - } - if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count != 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 13; // L5+E5a - } - if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 14; // Galileo E1B + Galileo E5a - } - if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 15; - } - if ((gps_1C_count == 0) && (gps_2S_count != 0) && (gps_L5_count != 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 16; // GPS L2C + GPS L5 - } - if ((gps_1C_count == 0) && (gps_2S_count != 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 17; // GPS L2C + Galileo E5a - } - if ((gps_1C_count == 0) && (gps_2S_count != 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 18; // GPS L2C + Galileo E5b - } - if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count != 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 19; // Galileo E5a + Galileo E5b - } - if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count != 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 20; // GPS L5 + Galileo E5b - } - if ((gps_1C_count != 0) && (gps_2S_count != 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 21; // GPS L1 C/A + Galileo E1B + GPS L2C - } - if ((gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count != 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 22; // GPS L1 C/A + Galileo E1B + GPS L5 - } - if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count != 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 23; // GLONASS L1 C/A - } - if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count != 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 24; // GLONASS L2 C/A - } - if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count != 0) && (glo_2G_count != 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 25; // GLONASS L1 C/A + GLONASS L2 C/A - } - if ((gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count != 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 26; // GPS L1 C/A + GLONASS L1 C/A - } - if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count != 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 27; // Galileo E1B + GLONASS L1 C/A - } - if ((gps_1C_count == 0) && (gps_2S_count != 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count != 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 28; // GPS L2C + GLONASS L1 C/A - } - if ((gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count != 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 29; // GPS L1 C/A + GLONASS L2 C/A - } - if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count != 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 30; // Galileo E1B + GLONASS L2 C/A - } - if ((gps_1C_count == 0) && (gps_2S_count != 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count != 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 31; // GPS L2C + GLONASS L2 C/A - } - if ((gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count != 0) && (gal_1B_count != 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 32; // L1+E1+L5+E5a - } - if ((gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 33; // L1+E1+E5a - } - // Galileo E6 - if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count != 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 100; // Galileo E6B - } - if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count != 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 101; // Galileo E1B + Galileo E6B - } - if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (gal_E6_count != 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 102; // Galileo E5a + Galileo E6B - } - if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0) && (gal_E6_count != 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 103; // Galileo E5b + Galileo E6B - } - if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (gal_E6_count != 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 104; // Galileo E1B + Galileo E5a + Galileo E6B - } - if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0) && (gal_E6_count != 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 105; // Galileo E1B + Galileo E5b + Galileo E6B - } - if ((gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count != 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 106; // GPS L1 C/A + Galileo E1B + Galileo E6B - } - if ((gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count != 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 107; // GPS L1 C/A + Galileo E6B - } - // BeiDou B1I Receiver - if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count != 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 500; // Beidou B1I - } - if ((gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count != 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 501; // Beidou B1I + GPS L1 C/A - } - if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count != 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 502; // Beidou B1I + Galileo E1B - } - if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count != 0) && (glo_2G_count == 0) && (bds_B1_count != 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 503; // Beidou B1I + GLONASS L1 C/A - } - if ((gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count != 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 504; // Beidou B1I + GPS L1 C/A + Galileo E1B - } - if ((gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count != 0) && (glo_2G_count == 0) && (bds_B1_count != 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 505; // Beidou B1I + GPS L1 C/A + GLONASS L1 C/A + Galileo E1B - } - if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count != 0) && (bds_B3_count != 0)) - { - pvt_output_parameters.type_of_receiver = 506; // Beidou B1I + Beidou B3I - } - // BeiDou B3I Receiver - if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count != 0)) - { - pvt_output_parameters.type_of_receiver = 600; // Beidou B3I - } - if ((gps_1C_count != 0) && (gps_2S_count != 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count != 0)) - { - pvt_output_parameters.type_of_receiver = 601; // Beidou B3I + GPS L2C - } - if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count != 0) && (bds_B1_count == 0) && (bds_B3_count != 0)) - { - pvt_output_parameters.type_of_receiver = 602; // Beidou B3I + GLONASS L2 C/A - } - if ((gps_1C_count == 0) && (gps_2S_count != 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count != 0) && (glo_2G_count != 0) && (bds_B1_count == 0) && (bds_B3_count != 0)) - { - pvt_output_parameters.type_of_receiver = 603; // Beidou B3I + GPS L2C + GLONASS L2 C/A - } - if ((gps_1C_count != 0) && (gps_2S_count != 0) && (gps_L5_count != 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 1000; // GPS L1 + GPS L2C + GPS L5 - } - if ((gps_1C_count != 0) && (gps_2S_count != 0) && (gps_L5_count != 0) && (gal_1B_count != 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) - { - pvt_output_parameters.type_of_receiver = 1001; // GPS L1 + Galileo E1B + GPS L2C + GPS L5 + Galileo E5a - } - - // RTKLIB PVT solver options - // Settings 1 - int positioning_mode = -1; - const std::string default_pos_mode("Single"); - const std::string positioning_mode_str = configuration->property(role + ".positioning_mode", default_pos_mode); // (PMODE_XXX) see src/algorithms/libs/rtklib/rtklib.h - if (positioning_mode_str == "Single") - { - positioning_mode = PMODE_SINGLE; - } - if (positioning_mode_str == "Static") - { - positioning_mode = PMODE_STATIC; - } - if (positioning_mode_str == "Kinematic") - { - positioning_mode = PMODE_KINEMA; - } - if (positioning_mode_str == "PPP_Static") - { - positioning_mode = PMODE_PPP_STATIC; - } - if (positioning_mode_str == "PPP_Kinematic") - { - positioning_mode = PMODE_PPP_KINEMA; - } - - if (positioning_mode == -1) - { - // warn user and set the default - std::cout << "WARNING: Bad specification of positioning mode.\n" - << "positioning_mode possible values: Single / Static / Kinematic / PPP_Static / PPP_Kinematic\n" - << "positioning_mode specified value: " << positioning_mode_str << "\n" - << "Setting positioning_mode to Single\n" - << std::flush; - positioning_mode = PMODE_SINGLE; - } - - int num_bands = 0; - - if ((gps_1C_count > 0) || (gal_1B_count > 0) || (glo_1G_count > 0) || (bds_B1_count > 0)) - { - num_bands += 1; - } - if ((gps_2S_count > 0) || (glo_2G_count > 0) || (bds_B3_count > 0)) - { - num_bands += 1; - } - if (gal_E6_count > 0) - { - num_bands += 1; - } - if ((gal_E5a_count > 0) || (gps_L5_count > 0)) - { - num_bands += 1; - } - if (gal_E5b_count > 0) - { - num_bands += 1; - } - if (num_bands > 3) - { - LOG(WARNING) << "Too much bands: The PVT engine can only handle 3 bands, but " << num_bands << " were set"; - num_bands = 3; - } - - int number_of_frequencies = configuration->property(role + ".num_bands", num_bands); /* (1:L1, 2:L1+L2, 3:L1+L2+L5) */ - if ((number_of_frequencies < 1) || (number_of_frequencies > 3)) - { - // warn user and set the default - number_of_frequencies = num_bands; - } - - double elevation_mask = configuration->property(role + ".elevation_mask", 15.0); - if ((elevation_mask < 0.0) || (elevation_mask > 90.0)) - { - // warn user and set the default - LOG(WARNING) << "Erroneous Elevation Mask. Setting to default value of 15.0 degrees"; - elevation_mask = 15.0; - } - - int dynamics_model = configuration->property(role + ".dynamics_model", 0); /* dynamics model (0:none, 1:velocity, 2:accel) */ - if ((dynamics_model < 0) || (dynamics_model > 2)) - { - // warn user and set the default - LOG(WARNING) << "Erroneous Dynamics Model configuration. Setting to default value of (0:none)"; - dynamics_model = 0; - } - - const std::string default_iono_model("OFF"); - const std::string iono_model_str = configuration->property(role + ".iono_model", default_iono_model); /* (IONOOPT_XXX) see src/algorithms/libs/rtklib/rtklib.h */ - int iono_model = -1; - if (iono_model_str == "OFF") - { - iono_model = IONOOPT_OFF; - } - if (iono_model_str == "Broadcast") - { - iono_model = IONOOPT_BRDC; - } - if (iono_model_str == "SBAS") - { - iono_model = IONOOPT_SBAS; - } - if (iono_model_str == "Iono-Free-LC") - { - iono_model = IONOOPT_IFLC; - } - if (iono_model_str == "Estimate_STEC") - { - iono_model = IONOOPT_EST; - } - if (iono_model_str == "IONEX") - { - iono_model = IONOOPT_TEC; - } - if (iono_model == -1) - { - // warn user and set the default - std::cout << "WARNING: Bad specification of ionospheric model.\n" - << "iono_model possible values: OFF / Broadcast / SBAS / Iono-Free-LC / Estimate_STEC / IONEX\n" - << "iono_model specified value: " << iono_model_str << "\n" - << "Setting iono_model to OFF\n" - << std::flush; - iono_model = IONOOPT_OFF; /* 0: ionosphere option: correction off */ - } - - const std::string default_trop_model("OFF"); - int trop_model = -1; - const std::string trop_model_str = configuration->property(role + ".trop_model", default_trop_model); /* (TROPOPT_XXX) see src/algorithms/libs/rtklib/rtklib.h */ - if (trop_model_str == "OFF") - { - trop_model = TROPOPT_OFF; - } - if (trop_model_str == "Saastamoinen") - { - trop_model = TROPOPT_SAAS; - } - if (trop_model_str == "SBAS") - { - trop_model = TROPOPT_SBAS; - } - if (trop_model_str == "Estimate_ZTD") - { - trop_model = TROPOPT_EST; - } - if (trop_model_str == "Estimate_ZTD_Grad") - { - trop_model = TROPOPT_ESTG; - } - if (trop_model == -1) - { - // warn user and set the default - std::cout << "WARNING: Bad specification of tropospheric model.\n" - << "trop_model possible values: OFF / Saastamoinen / SBAS / Estimate_ZTD / Estimate_ZTD_Grad\n" - << "trop_model specified value: " << trop_model_str << "\n" - << "Setting trop_model to OFF\n" - << std::flush; - trop_model = TROPOPT_OFF; - } - - /* RTKLIB positioning options */ - int sat_PCV = 0; /* Set whether the satellite antenna PCV (phase center variation) model is used or not. This feature requires a Satellite Antenna PCV File. */ - int rec_PCV = 0; /* Set whether the receiver antenna PCV (phase center variation) model is used or not. This feature requires a Receiver Antenna PCV File. */ - - /* Set whether the phase windup correction for PPP modes is applied or not. Only applicable to PPP‐* modes.*/ - const int phwindup = configuration->property(role + ".phwindup", 0); - - /* Set whether the GPS Block IIA satellites in eclipse are excluded or not. - The eclipsing Block IIA satellites often degrade the PPP solutions due to unpredicted behavior of yaw‐attitude. Only applicable to PPP‐* modes.*/ - const int reject_GPS_IIA = configuration->property(role + ".reject_GPS_IIA", 0); - - /* Set whether RAIM (receiver autonomous integrity monitoring) FDE (fault detection and exclusion) feature is enabled or not. - In case of RAIM FDE enabled, a satellite is excluded if SSE (sum of squared errors) of residuals is over a threshold. - The excluded satellite is selected to indicate the minimum SSE. */ - const int raim_fde = configuration->property(role + ".raim_fde", 0); - - const int earth_tide = configuration->property(role + ".earth_tide", 0); - - int nsys = 0; - if ((gps_1C_count > 0) || (gps_2S_count > 0) || (gps_L5_count > 0)) - { - nsys += SYS_GPS; - } - if ((gal_1B_count > 0) || (gal_E5a_count > 0) || (gal_E5b_count > 0) || (gal_E6_count > 0)) - { - nsys += SYS_GAL; - } - if ((glo_1G_count > 0) || (glo_2G_count > 0)) - { - nsys += SYS_GLO; - } - if ((bds_B1_count > 0) || (bds_B3_count > 0)) - { - nsys += SYS_BDS; - } - - int navigation_system = configuration->property(role + ".navigation_system", nsys); /* (SYS_XXX) see src/algorithms/libs/rtklib/rtklib.h */ - if ((navigation_system < 1) || (navigation_system > 255)) /* GPS: 1 SBAS: 2 GPS+SBAS: 3 Galileo: 8 Galileo+GPS: 9 GPS+SBAS+Galileo: 11 All: 255 */ - { - // warn user and set the default - LOG(WARNING) << "Erroneous Navigation System. Setting to default value of (0:none)"; - navigation_system = nsys; - } - - // Settings 2 - const std::string default_gps_ar("Continuous"); - const std::string integer_ambiguity_resolution_gps_str = configuration->property(role + ".AR_GPS", default_gps_ar); /* Integer Ambiguity Resolution mode for GPS (0:off,1:continuous,2:instantaneous,3:fix and hold,4:ppp-ar) */ - int integer_ambiguity_resolution_gps = -1; - if (integer_ambiguity_resolution_gps_str == "OFF") - { - integer_ambiguity_resolution_gps = ARMODE_OFF; - } - if (integer_ambiguity_resolution_gps_str == "Continuous") - { - integer_ambiguity_resolution_gps = ARMODE_CONT; - } - if (integer_ambiguity_resolution_gps_str == "Instantaneous") - { - integer_ambiguity_resolution_gps = ARMODE_INST; - } - if (integer_ambiguity_resolution_gps_str == "Fix-and-Hold") - { - integer_ambiguity_resolution_gps = ARMODE_FIXHOLD; - } - if (integer_ambiguity_resolution_gps_str == "PPP-AR") - { - integer_ambiguity_resolution_gps = ARMODE_PPPAR; - } - if (integer_ambiguity_resolution_gps == -1) - { - // warn user and set the default - std::cout << "WARNING: Bad specification of GPS ambiguity resolution method.\n" - << "AR_GPS possible values: OFF / Continuous / Instantaneous / Fix-and-Hold / PPP-AR\n" - << "AR_GPS specified value: " << integer_ambiguity_resolution_gps_str << "\n" - << "Setting AR_GPS to OFF\n" - << std::flush; - integer_ambiguity_resolution_gps = ARMODE_OFF; - } - - int integer_ambiguity_resolution_glo = configuration->property(role + ".AR_GLO", 1); /* Integer Ambiguity Resolution mode for GLONASS (0:off,1:on,2:auto cal,3:ext cal) */ - if ((integer_ambiguity_resolution_glo < 0) || (integer_ambiguity_resolution_glo > 3)) - { - // warn user and set the default - LOG(WARNING) << "Erroneous Integer Ambiguity Resolution for GLONASS . Setting to default value of (1:on)"; - integer_ambiguity_resolution_glo = 1; - } - - int integer_ambiguity_resolution_bds = configuration->property(role + ".AR_DBS", 1); /* Integer Ambiguity Resolution mode for BEIDOU (0:off,1:on) */ - if ((integer_ambiguity_resolution_bds < 0) || (integer_ambiguity_resolution_bds > 1)) - { - // warn user and set the default - LOG(WARNING) << "Erroneous Integer Ambiguity Resolution for BEIDOU . Setting to default value of (1:on)"; - integer_ambiguity_resolution_bds = 1; - } - - const double min_ratio_to_fix_ambiguity = configuration->property(role + ".min_ratio_to_fix_ambiguity", 3.0); /* Set the integer ambiguity validation threshold for ratio‐test, - which uses the ratio of squared residuals of the best integer vector to the second‐best vector. */ - - const int min_lock_to_fix_ambiguity = configuration->property(role + ".min_lock_to_fix_ambiguity", 0); /* Set the minimum lock count to fix integer ambiguity.FLAGS_RINEX_version. - If the lock count is less than the value, the ambiguity is excluded from the fixed integer vector. */ - - const double min_elevation_to_fix_ambiguity = configuration->property(role + ".min_elevation_to_fix_ambiguity", 0.0); /* Set the minimum elevation (deg) to fix integer ambiguity. - If the elevation of the satellite is less than the value, the ambiguity is excluded from the fixed integer vector. */ - - const int outage_reset_ambiguity = configuration->property(role + ".outage_reset_ambiguity", 5); /* Set the outage count to reset ambiguity. If the data outage count is over the value, the estimated ambiguity is reset to the initial value. */ - - const double slip_threshold = configuration->property(role + ".slip_threshold", 0.05); /* set the cycle‐slip threshold (m) of geometry‐free LC carrier‐phase difference between epochs */ - - const double threshold_reject_gdop = configuration->property(role + ".threshold_reject_gdop", 30.0); /* reject threshold of GDOP. If the GDOP is over the value, the observable is excluded for the estimation process as an outlier. */ - - const double threshold_reject_innovation = configuration->property(role + ".threshold_reject_innovation", 30.0); /* reject threshold of innovation (m). If the innovation is over the value, the observable is excluded for the estimation process as an outlier. */ - - const int number_filter_iter = configuration->property(role + ".number_filter_iter", 1); /* Set the number of iteration in the measurement update of the estimation filter. - If the baseline length is very short like 1 m, the iteration may be effective to handle - the nonlinearity of measurement equation. */ - - // Statistics - const double bias_0 = configuration->property(role + ".bias_0", 30.0); - - const double iono_0 = configuration->property(role + ".iono_0", 0.03); - - const double trop_0 = configuration->property(role + ".trop_0", 0.3); - - const double sigma_bias = configuration->property(role + ".sigma_bias", 1e-4); /* Set the process noise standard deviation of carrier‐phase - bias (ambiguity) (cycle/sqrt(s)) */ - - const double sigma_iono = configuration->property(role + ".sigma_iono", 1e-3); /* Set the process noise standard deviation of vertical ionospheric delay per 10 km baseline (m/sqrt(s)). */ - - const double sigma_trop = configuration->property(role + ".sigma_trop", 1e-4); /* Set the process noise standard deviation of zenith tropospheric delay (m/sqrt(s)). */ - - const double sigma_acch = configuration->property(role + ".sigma_acch", 1e-1); /* Set the process noise standard deviation of the receiver acceleration as - the horizontal component. (m/s2/sqrt(s)). If Receiver Dynamics is set to OFF, they are not used. */ - - const double sigma_accv = configuration->property(role + ".sigma_accv", 1e-2); /* Set the process noise standard deviation of the receiver acceleration as - the vertical component. (m/s2/sqrt(s)). If Receiver Dynamics is set to OFF, they are not used. */ - - const double sigma_pos = configuration->property(role + ".sigma_pos", 0.0); - - const double code_phase_error_ratio_l1 = configuration->property(role + ".code_phase_error_ratio_l1", 100.0); - const double code_phase_error_ratio_l2 = configuration->property(role + ".code_phase_error_ratio_l2", 100.0); - const double code_phase_error_ratio_l5 = configuration->property(role + ".code_phase_error_ratio_l5", 100.0); - const double carrier_phase_error_factor_a = configuration->property(role + ".carrier_phase_error_factor_a", 0.003); - const double carrier_phase_error_factor_b = configuration->property(role + ".carrier_phase_error_factor_b", 0.003); - - const bool bancroft_init = configuration->property(role + ".bancroft_init", true); - - snrmask_t snrmask = {{}, {{}, {}}}; - - prcopt_t rtklib_configuration_options = { - positioning_mode, /* positioning mode (PMODE_XXX) see src/algorithms/libs/rtklib/rtklib.h */ - 0, /* solution type (0:forward,1:backward,2:combined) */ - number_of_frequencies, /* number of frequencies (1:L1, 2:L1+L2, 3:L1+L2+L5)*/ - navigation_system, /* navigation system */ - elevation_mask * D2R, /* elevation mask angle (degrees) */ - snrmask, /* snrmask_t snrmask SNR mask */ - 0, /* satellite ephemeris/clock (EPHOPT_XXX) */ - integer_ambiguity_resolution_gps, /* AR mode (0:off,1:continuous,2:instantaneous,3:fix and hold,4:ppp-ar) */ - integer_ambiguity_resolution_glo, /* GLONASS AR mode (0:off,1:on,2:auto cal,3:ext cal) */ - integer_ambiguity_resolution_bds, /* BeiDou AR mode (0:off,1:on) */ - outage_reset_ambiguity, /* obs outage count to reset bias */ - min_lock_to_fix_ambiguity, /* min lock count to fix ambiguity */ - 10, /* min fix count to hold ambiguity */ - 1, /* max iteration to resolve ambiguity */ - iono_model, /* ionosphere option (IONOOPT_XXX) */ - trop_model, /* troposphere option (TROPOPT_XXX) */ - dynamics_model, /* dynamics model (0:none, 1:velocity, 2:accel) */ - earth_tide, /* earth tide correction (0:off,1:solid,2:solid+otl+pole) */ - number_filter_iter, /* number of filter iteration */ - 0, /* code smoothing window size (0:none) */ - 0, /* interpolate reference obs (for post mission) */ - 0, /* sbssat_t sbssat SBAS correction options */ - 0, /* sbsion_t sbsion[MAXBAND+1] SBAS satellite selection (0:all) */ - 0, /* rover position for fixed mode */ - 0, /* base position for relative mode */ - /* 0:pos in prcopt, 1:average of single pos, */ - /* 2:read from file, 3:rinex header, 4:rtcm pos */ - {code_phase_error_ratio_l1, code_phase_error_ratio_l2, code_phase_error_ratio_l5}, /* eratio[NFREQ] code/phase error ratio */ - {100.0, carrier_phase_error_factor_a, carrier_phase_error_factor_b, 0.0, 1.0}, /* err[5]: measurement error factor [0]:reserved, [1-3]:error factor a/b/c of phase (m) , [4]:doppler frequency (hz) */ - {bias_0, iono_0, trop_0}, /* std[3]: initial-state std [0]bias,[1]iono [2]trop*/ - {sigma_bias, sigma_iono, sigma_trop, sigma_acch, sigma_accv, sigma_pos}, /* prn[6] process-noise std */ - 5e-12, /* sclkstab: satellite clock stability (sec/sec) */ - {min_ratio_to_fix_ambiguity, 0.9999, 0.25, 0.1, 0.05, 0.0, 0.0, 0.0}, /* thresar[8]: AR validation threshold */ - min_elevation_to_fix_ambiguity, /* elevation mask of AR for rising satellite (deg) */ - 0.0, /* elevation mask to hold ambiguity (deg) */ - slip_threshold, /* slip threshold of geometry-free phase (m) */ - 30.0, /* max difference of time (sec) */ - threshold_reject_innovation, /* reject threshold of innovation (m) */ - threshold_reject_gdop, /* reject threshold of gdop */ - {}, /* double baseline[2] baseline length constraint {const,sigma} (m) */ - {}, /* double ru[3] rover position for fixed mode {x,y,z} (ecef) (m) */ - {}, /* double rb[3] base position for relative mode {x,y,z} (ecef) (m) */ - {"", ""}, /* char anttype[2][MAXANT] antenna types {rover,base} */ - {{}, {}}, /* double antdel[2][3] antenna delta {{rov_e,rov_n,rov_u},{ref_e,ref_n,ref_u}} */ - {}, /* pcv_t pcvr[2] receiver antenna parameters {rov,base} */ - {}, /* unsigned char exsats[MAXSAT] excluded satellites (1:excluded, 2:included) */ - 0, /* max averaging epoches */ - 0, /* initialize by restart */ - 1, /* output single by dgps/float/fix/ppp outage */ - {"", ""}, /* char rnxopt[2][256] rinex options {rover,base} */ - {sat_PCV, rec_PCV, phwindup, reject_GPS_IIA, raim_fde}, /* posopt[6] positioning options [0]: satellite and receiver antenna PCV model; [1]: interpolate antenna parameters; [2]: apply phase wind-up correction for PPP modes; [3]: exclude measurements of GPS Block IIA satellites satellite [4]: RAIM FDE (fault detection and exclusion) [5]: handle day-boundary clock jump */ - 0, /* solution sync mode (0:off,1:on) */ - {{}, {}}, /* odisp[2][6*11] ocean tide loading parameters {rov,base} */ - {{}, {{}, {}}, {{}, {}}, {}, {}}, /* exterr_t exterr extended receiver error model */ - 0, /* disable L2-AR */ - {}, /* char pppopt[256] ppp option "-GAP_RESION=" default gap to reset iono parameters (ep) */ - bancroft_init /* enable Bancroft initialization for the first iteration of the PVT computation, useful in some geometries */ - }; - - rtkinit(&rtk, &rtklib_configuration_options); - - // Outputs - const bool default_output_enabled = configuration->property(role + ".output_enabled", true); - pvt_output_parameters.output_enabled = default_output_enabled; - pvt_output_parameters.rinex_output_enabled = configuration->property(role + ".rinex_output_enabled", default_output_enabled); - pvt_output_parameters.gpx_output_enabled = configuration->property(role + ".gpx_output_enabled", default_output_enabled); - pvt_output_parameters.geojson_output_enabled = configuration->property(role + ".geojson_output_enabled", default_output_enabled); - pvt_output_parameters.kml_output_enabled = configuration->property(role + ".kml_output_enabled", default_output_enabled); - pvt_output_parameters.xml_output_enabled = configuration->property(role + ".xml_output_enabled", default_output_enabled); - pvt_output_parameters.nmea_output_file_enabled = configuration->property(role + ".nmea_output_file_enabled", default_output_enabled); - pvt_output_parameters.rtcm_output_file_enabled = configuration->property(role + ".rtcm_output_file_enabled", false); - - const std::string default_output_path = configuration->property(role + ".output_path", std::string(".")); - pvt_output_parameters.output_path = default_output_path; - pvt_output_parameters.rinex_output_path = configuration->property(role + ".rinex_output_path", default_output_path); - pvt_output_parameters.gpx_output_path = configuration->property(role + ".gpx_output_path", default_output_path); - pvt_output_parameters.geojson_output_path = configuration->property(role + ".geojson_output_path", default_output_path); - pvt_output_parameters.kml_output_path = configuration->property(role + ".kml_output_path", default_output_path); - pvt_output_parameters.xml_output_path = configuration->property(role + ".xml_output_path", default_output_path); - pvt_output_parameters.nmea_output_file_path = configuration->property(role + ".nmea_output_file_path", default_output_path); - pvt_output_parameters.rtcm_output_file_path = configuration->property(role + ".rtcm_output_file_path", default_output_path); - - // Read PVT MONITOR Configuration - pvt_output_parameters.monitor_enabled = configuration->property(role + ".enable_monitor", false); - pvt_output_parameters.udp_addresses = configuration->property(role + ".monitor_client_addresses", std::string("127.0.0.1")); - pvt_output_parameters.udp_port = configuration->property(role + ".monitor_udp_port", 1234); - pvt_output_parameters.protobuf_enabled = configuration->property(role + ".enable_protobuf", true); - if (configuration->property("Monitor.enable_protobuf", false) == true) - { - pvt_output_parameters.protobuf_enabled = true; - } - - // Read EPHEMERIS MONITOR Configuration - pvt_output_parameters.monitor_ephemeris_enabled = configuration->property(role + ".enable_monitor_ephemeris", false); - pvt_output_parameters.udp_eph_addresses = configuration->property(role + ".monitor_ephemeris_client_addresses", std::string("127.0.0.1")); - pvt_output_parameters.udp_eph_port = configuration->property(role + ".monitor_ephemeris_udp_port", 1234); - - // Show time in local zone - pvt_output_parameters.show_local_time_zone = configuration->property(role + ".show_local_time_zone", false); - - // Enable or disable rx clock correction in observables - pvt_output_parameters.enable_rx_clock_correction = configuration->property(role + ".enable_rx_clock_correction", false); - - // Set maximum clock offset allowed if pvt_output_parameters.enable_rx_clock_correction = false - pvt_output_parameters.max_obs_block_rx_clock_offset_ms = configuration->property(role + ".max_clock_offset_ms", pvt_output_parameters.max_obs_block_rx_clock_offset_ms); - - // Source timetag - pvt_output_parameters.log_source_timetag = configuration->property(role + ".log_timetag", pvt_output_parameters.log_source_timetag); - pvt_output_parameters.log_source_timetag_file = configuration->property(role + ".log_source_timetag_file", pvt_output_parameters.log_source_timetag_file); - - // Use E6 for PVT - pvt_output_parameters.use_e6_for_pvt = configuration->property(role + ".use_e6_for_pvt", pvt_output_parameters.use_e6_for_pvt); - pvt_output_parameters.use_has_corrections = configuration->property(role + ".use_has_corrections", pvt_output_parameters.use_has_corrections); - - // Use unhealthy satellites - pvt_output_parameters.use_unhealthy_sats = configuration->property(role + ".use_unhealthy_sats", pvt_output_parameters.use_unhealthy_sats); - - // make PVT object - pvt_ = rtklib_make_pvt_gs(in_streams_, pvt_output_parameters, rtk); - DLOG(INFO) << "pvt(" << pvt_->unique_id() << ")"; - if (out_streams_ > 0) - { - LOG(ERROR) << "The PVT block does not have an output stream"; - } -} - - -Rtklib_Pvt::~Rtklib_Pvt() -{ - DLOG(INFO) << "PVT adapter destructor called."; - rtkfree(&rtk); -} - - -bool Rtklib_Pvt::get_latest_PVT(double* longitude_deg, - double* latitude_deg, - double* height_m, - double* ground_speed_kmh, - double* course_over_ground_deg, - time_t* UTC_time) -{ - return pvt_->get_latest_PVT(longitude_deg, - latitude_deg, - height_m, - ground_speed_kmh, - course_over_ground_deg, - UTC_time); -} - - -void Rtklib_Pvt::clear_ephemeris() -{ - pvt_->clear_ephemeris(); -} - - -std::map Rtklib_Pvt::get_gps_ephemeris() const -{ - return pvt_->get_gps_ephemeris_map(); -} - - -std::map Rtklib_Pvt::get_galileo_ephemeris() const -{ - return pvt_->get_galileo_ephemeris_map(); -} - - -std::map Rtklib_Pvt::get_gps_almanac() const -{ - return pvt_->get_gps_almanac_map(); -} - - -std::map Rtklib_Pvt::get_galileo_almanac() const -{ - return pvt_->get_galileo_almanac_map(); -} - - -void Rtklib_Pvt::connect(gr::top_block_sptr top_block) -{ - if (top_block) - { /* top_block is not null */ - }; - // Nothing to connect internally - DLOG(INFO) << "nothing to connect internally"; -} - - -void Rtklib_Pvt::disconnect(gr::top_block_sptr top_block) -{ - if (top_block) - { /* top_block is not null */ - }; - // Nothing to disconnect -} - - -gr::basic_block_sptr Rtklib_Pvt::get_left_block() -{ - return pvt_; -} - - -gr::basic_block_sptr Rtklib_Pvt::get_right_block() -{ - return nullptr; // this is a sink, nothing downstream -} +/*! + * \file rtklib_pvt.cc + * \brief Interface of a Position Velocity and Time computation block + * \author Javier Arribas, 2017. jarribas(at)cttc.es + * + * ----------------------------------------------------------------------------- + * + * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. + * This file is part of GNSS-SDR. + * + * Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + + +#include "rtklib_pvt.h" +#include "MATH_CONSTANTS.h" // for D2R +#include "configuration_interface.h" // for ConfigurationInterface +#include "galileo_almanac.h" // for Galileo_Almanac +#include "galileo_ephemeris.h" // for Galileo_Ephemeris +#include "gnss_sdr_flags.h" // for FLAGS_RINEX_version +#include "gnss_sdr_string_literals.h" // for std::string_literals +#include "gps_almanac.h" // for Gps_Almanac +#include "gps_ephemeris.h" // for Gps_Ephemeris +#include "pvt_conf.h" // for Pvt_Conf +#include "rtklib_rtkpos.h" // for rtkfree, rtkinit +#include // for LOG +#include // for std::cout +#if USE_STD_COMMON_FACTOR +#include +namespace bc = std; +#else +#if USE_OLD_BOOST_MATH_COMMON_FACTOR +#include +namespace bc = boost::math; +#else +#include +namespace bc = boost::integer; +#endif +#endif + +using namespace std::string_literals; + +Rtklib_Pvt::Rtklib_Pvt(const ConfigurationInterface* configuration, + const std::string& role, + unsigned int in_streams, + unsigned int out_streams) : role_(role), + in_streams_(in_streams), + out_streams_(out_streams) +{ + Pvt_Conf pvt_output_parameters = Pvt_Conf(); + // dump parameters + const std::string default_dump_filename("./pvt.dat"); + const std::string default_nmea_dump_filename("./nmea_pvt.nmea"); + const std::string default_nmea_dump_devname("/dev/tty1"); + const std::string default_rtcm_dump_devname("/dev/pts/1"); + DLOG(INFO) << "role " << role; + pvt_output_parameters.dump = configuration->property(role + ".dump", false); + pvt_output_parameters.dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); + pvt_output_parameters.dump_mat = configuration->property(role + ".dump_mat", true); + + pvt_output_parameters.rtk_trace_level = configuration->property(role + ".rtk_trace_level"s, 0); + + // Flag to postprocess old gnss records (older than 2009) and avoid wrong week rollover + pvt_output_parameters.pre_2009_file = configuration->property("GNSS-SDR.pre_2009_file", false); + + // output rate + pvt_output_parameters.observable_interval_ms = configuration->property("GNSS-SDR.observable_interval_ms", pvt_output_parameters.observable_interval_ms); + + pvt_output_parameters.output_rate_ms = bc::lcm(static_cast(pvt_output_parameters.observable_interval_ms), configuration->property(role + ".output_rate_ms", 500)); + + // display rate + pvt_output_parameters.display_rate_ms = bc::lcm(pvt_output_parameters.output_rate_ms, configuration->property(role + ".display_rate_ms", 500)); + + // PVT KF settings + pvt_output_parameters.enable_pvt_kf = configuration->property(role + ".enable_pvt_kf", false); + pvt_output_parameters.estatic_measures_sd = configuration->property(role + ".estatic_measures_sd", false); + pvt_output_parameters.measures_ecef_pos_sd_m = configuration->property(role + ".kf_measures_ecef_pos_sd_m", 1.0); + pvt_output_parameters.measures_ecef_vel_sd_ms = configuration->property(role + ".kf_measures_ecef_vel_sd_ms", 0.1); + pvt_output_parameters.system_ecef_pos_sd_m = configuration->property(role + ".kf_system_ecef_pos_sd_m", 0.01); + pvt_output_parameters.system_ecef_vel_sd_ms = configuration->property(role + ".kf_system_ecef_vel_sd_ms", 0.001); + + // NMEA Printer settings + pvt_output_parameters.flag_nmea_tty_port = configuration->property(role + ".flag_nmea_tty_port", false); + pvt_output_parameters.nmea_dump_filename = configuration->property(role + ".nmea_dump_filename", default_nmea_dump_filename); + pvt_output_parameters.nmea_dump_devname = configuration->property(role + ".nmea_dump_devname", default_nmea_dump_devname); + + // RINEX version + pvt_output_parameters.rinex_version = configuration->property(role + ".rinex_version", 3); + if (FLAGS_RINEX_version == "3.01" || FLAGS_RINEX_version == "3.02" || FLAGS_RINEX_version == "3") + { + pvt_output_parameters.rinex_version = 3; + } + else if (FLAGS_RINEX_version == "2.10" || FLAGS_RINEX_version == "2.11" || FLAGS_RINEX_version == "2") + { + pvt_output_parameters.rinex_version = 2; + } + pvt_output_parameters.rinexobs_rate_ms = bc::lcm(configuration->property(role + ".rinexobs_rate_ms", 1000), pvt_output_parameters.output_rate_ms); + pvt_output_parameters.rinex_name = configuration->property(role + ".rinex_name", std::string("-")); + if (FLAGS_RINEX_name != "-") + { + pvt_output_parameters.rinex_name = FLAGS_RINEX_name; + } + + // RTCM Printer settings + pvt_output_parameters.flag_rtcm_tty_port = configuration->property(role + ".flag_rtcm_tty_port", false); + pvt_output_parameters.rtcm_dump_devname = configuration->property(role + ".rtcm_dump_devname", default_rtcm_dump_devname); + pvt_output_parameters.flag_rtcm_server = configuration->property(role + ".flag_rtcm_server", false); + pvt_output_parameters.rtcm_tcp_port = configuration->property(role + ".rtcm_tcp_port", 2101); + pvt_output_parameters.rtcm_station_id = configuration->property(role + ".rtcm_station_id", 1234); + // RTCM message rates: least common multiple with output_rate_ms + const int rtcm_MT1019_rate_ms = bc::lcm(configuration->property(role + ".rtcm_MT1019_rate_ms", 5000), pvt_output_parameters.output_rate_ms); + const int rtcm_MT1020_rate_ms = bc::lcm(configuration->property(role + ".rtcm_MT1020_rate_ms", 5000), pvt_output_parameters.output_rate_ms); + const int rtcm_MT1045_rate_ms = bc::lcm(configuration->property(role + ".rtcm_MT1045_rate_ms", 5000), pvt_output_parameters.output_rate_ms); + const int rtcm_MSM_rate_ms = bc::lcm(configuration->property(role + ".rtcm_MSM_rate_ms", 1000), pvt_output_parameters.output_rate_ms); + const int rtcm_MT1077_rate_ms = bc::lcm(configuration->property(role + ".rtcm_MT1077_rate_ms", rtcm_MSM_rate_ms), pvt_output_parameters.output_rate_ms); + const int rtcm_MT1087_rate_ms = bc::lcm(configuration->property(role + ".rtcm_MT1087_rate_ms", rtcm_MSM_rate_ms), pvt_output_parameters.output_rate_ms); + const int rtcm_MT1097_rate_ms = bc::lcm(configuration->property(role + ".rtcm_MT1097_rate_ms", rtcm_MSM_rate_ms), pvt_output_parameters.output_rate_ms); + + pvt_output_parameters.rtcm_msg_rate_ms[1019] = rtcm_MT1019_rate_ms; + pvt_output_parameters.rtcm_msg_rate_ms[1020] = rtcm_MT1020_rate_ms; + pvt_output_parameters.rtcm_msg_rate_ms[1045] = rtcm_MT1045_rate_ms; + for (int k = 1071; k < 1078; k++) // All GPS MSM + { + pvt_output_parameters.rtcm_msg_rate_ms[k] = rtcm_MT1077_rate_ms; + } + for (int k = 1081; k < 1088; k++) // All GLONASS MSM + { + pvt_output_parameters.rtcm_msg_rate_ms[k] = rtcm_MT1087_rate_ms; + } + for (int k = 1091; k < 1098; k++) // All Galileo MSM + { + pvt_output_parameters.rtcm_msg_rate_ms[k] = rtcm_MT1097_rate_ms; + } + + // Advanced Nativation Protocol Printer settings + pvt_output_parameters.an_output_enabled = configuration->property(role + ".an_output_enabled", pvt_output_parameters.an_output_enabled); + pvt_output_parameters.an_dump_devname = configuration->property(role + ".an_dump_devname", default_nmea_dump_devname); + if (pvt_output_parameters.an_output_enabled && pvt_output_parameters.flag_nmea_tty_port) + { + if (pvt_output_parameters.nmea_dump_devname == pvt_output_parameters.an_dump_devname) + { + std::cerr << "Warning: NMEA an Advanced Nativation printers set to write to the same serial port.\n" + << "Please make sure that PVT.an_dump_devname and PVT.an_dump_devname are different.\n" + << "Shutting down the NMEA serial output.\n"; + pvt_output_parameters.flag_nmea_tty_port = false; + } + } + if (pvt_output_parameters.an_output_enabled && pvt_output_parameters.flag_rtcm_tty_port) + { + if (pvt_output_parameters.rtcm_dump_devname == pvt_output_parameters.an_dump_devname) + { + std::cerr << "Warning: RTCM an Advanced Nativation printers set to write to the same serial port.\n" + << "Please make sure that PVT.an_dump_devname and .rtcm_dump_devname are different.\n" + << "Shutting down the RTCM serial output.\n"; + pvt_output_parameters.flag_rtcm_tty_port = false; + } + } + + pvt_output_parameters.kml_rate_ms = bc::lcm(configuration->property(role + ".kml_rate_ms", pvt_output_parameters.kml_rate_ms), pvt_output_parameters.output_rate_ms); + pvt_output_parameters.gpx_rate_ms = bc::lcm(configuration->property(role + ".gpx_rate_ms", pvt_output_parameters.gpx_rate_ms), pvt_output_parameters.output_rate_ms); + pvt_output_parameters.geojson_rate_ms = bc::lcm(configuration->property(role + ".geojson_rate_ms", pvt_output_parameters.geojson_rate_ms), pvt_output_parameters.output_rate_ms); + pvt_output_parameters.nmea_rate_ms = bc::lcm(configuration->property(role + ".nmea_rate_ms", pvt_output_parameters.nmea_rate_ms), pvt_output_parameters.output_rate_ms); + pvt_output_parameters.an_rate_ms = configuration->property(role + ".an_rate_ms", pvt_output_parameters.an_rate_ms); + + // Infer the type of receiver + /* + * TYPE | RECEIVER + * 0 | Unknown + * 1 | GPS L1 C/A + * 2 | GPS L2C + * 3 | GPS L5 + * 4 | Galileo E1B + * 5 | Galileo E5a + * 6 | Galileo E5b + * 7 | GPS L1 C/A + GPS L2C + * 8 | GPS L1 C/A + GPS L5 + * 9 | GPS L1 C/A + Galileo E1B + * 10 | GPS L1 C/A + Galileo E5a + * 11 | GPS L1 C/A + Galileo E5b + * 12 | Galileo E1B + GPS L2C + * 13 | Galileo E5a + GPS L5 + * 14 | Galileo E1B + Galileo E5a + * 15 | Galileo E1B + Galileo E5b + * 16 | GPS L2C + GPS L5 + * 17 | GPS L2C + Galileo E5a + * 18 | GPS L2C + Galileo E5b + * 19 | Galileo E5a + Galileo E5b + * 20 | GPS L5 + Galileo E5b + * 21 | GPS L1 C/A + Galileo E1B + GPS L2C + * 22 | GPS L1 C/A + Galileo E1B + GPS L5 + * 23 | GLONASS L1 C/A + * 24 | GLONASS L2 C/A + * 25 | GLONASS L1 C/A + GLONASS L2 C/A + * 26 | GPS L1 C/A + GLONASS L1 C/A + * 27 | Galileo E1B + GLONASS L1 C/A + * 28 | GPS L2C + GLONASS L1 C/A + * 29 | GPS L1 C/A + GLONASS L2 C/A + * 30 | Galileo E1B + GLONASS L2 C/A + * 31 | GPS L2C + GLONASS L2 C/A + * 32 | GPS L1 C/A + Galileo E1B + GPS L5 + Galileo E5a + * 33 | GPS L1 C/A + Galileo E1B + Galileo E5a + * + * Skipped previous values to avoid overlapping + * 100 | Galileo E6B + * 101 | Galileo E1B + Galileo E6B + * 102 | Galileo E5a + Galileo E6B + * 103 | Galileo E5b + Galileo E6B + * 104 | Galileo E1B + Galileo E5a + Galileo E6B + * 105 | Galileo E1B + Galileo E5b + Galileo E6B + * 106 | GPS L1 C/A + Galileo E1B + Galileo E6B + * 107 | GPS L1 C/A + Galileo E6B + * Skipped previous values to avoid overlapping + * 500 | BeiDou B1I + * 501 | BeiDou B1I + GPS L1 C/A + * 502 | BeiDou B1I + Galileo E1B + * 503 | BeiDou B1I + GLONASS L1 C/A + * 504 | BeiDou B1I + GPS L1 C/A + Galileo E1B + * 505 | BeiDou B1I + GPS L1 C/A + GLONASS L1 C/A + Galileo E1B + * 506 | BeiDou B1I + Beidou B3I + * Skipped previous values to avoid overlapping + * 600 | BeiDou B3I + * 601 | BeiDou B3I + GPS L2C + * 602 | BeiDou B3I + GLONASS L2 C/A + * 603 | BeiDou B3I + GPS L2C + GLONASS L2 C/A + * 604 | BeiDou B3I + GPS L1 C/A + * 605 | BeiDou B3I + Galileo E1B + * 606 | BeiDou B3I + GLONASS L1 C/A + * 607 | BeiDou B3I + GPS L1 C/A + Galileo E1B + * 608 | BeiDou B3I + GPS L1 C/A + Galileo E1B + BeiDou B1I + * 609 | BeiDou B3I + GPS L1 C/A + Galileo E1B + GLONASS L1 C/A + * 610 | BeiDou B3I + GPS L1 C/A + Galileo E1B + GLONASS L1 C/A + BeiDou B1I + * + * 1000 | GPS L1 C/A + GPS L2C + GPS L5 + * 1001 | GPS L1 C/A + Galileo E1B + GPS L2C + GPS L5 + Galileo E5a + */ + const int gps_1C_count = configuration->property("Channels_1C.count", 0); + const int gps_2S_count = configuration->property("Channels_2S.count", 0); + const int gps_L5_count = configuration->property("Channels_L5.count", 0); + const int gal_1B_count = configuration->property("Channels_1B.count", 0); + const int gal_E5a_count = configuration->property("Channels_5X.count", 0); + const int gal_E5b_count = configuration->property("Channels_7X.count", 0); + const int gal_E6_count = configuration->property("Channels_E6.count", 0); + const int glo_1G_count = configuration->property("Channels_1G.count", 0); + const int glo_2G_count = configuration->property("Channels_2G.count", 0); + const int bds_B1_count = configuration->property("Channels_B1.count", 0); + const int bds_B3_count = configuration->property("Channels_B3.count", 0); + + if ((gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 1; // L1 + } + if ((gps_1C_count == 0) && (gps_2S_count != 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 2; // GPS L2C + } + if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count != 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 3; // L5 + } + if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 4; // E1 + } + if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 5; // E5a + } + if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 6; + } + if ((gps_1C_count != 0) && (gps_2S_count != 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 7; // GPS L1 C/A + GPS L2C + } + if ((gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count != 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 8; // L1+L5 + } + if ((gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 9; // L1+E1 + } + if ((gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 10; // GPS L1 C/A + Galileo E5a + } + if ((gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 11; + } + if ((gps_1C_count == 0) && (gps_2S_count != 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 12; // Galileo E1B + GPS L2C + } + if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count != 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 13; // L5+E5a + } + if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 14; // Galileo E1B + Galileo E5a + } + if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 15; + } + if ((gps_1C_count == 0) && (gps_2S_count != 0) && (gps_L5_count != 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 16; // GPS L2C + GPS L5 + } + if ((gps_1C_count == 0) && (gps_2S_count != 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 17; // GPS L2C + Galileo E5a + } + if ((gps_1C_count == 0) && (gps_2S_count != 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 18; // GPS L2C + Galileo E5b + } + if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count != 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 19; // Galileo E5a + Galileo E5b + } + if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count != 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 20; // GPS L5 + Galileo E5b + } + if ((gps_1C_count != 0) && (gps_2S_count != 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 21; // GPS L1 C/A + Galileo E1B + GPS L2C + } + if ((gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count != 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 22; // GPS L1 C/A + Galileo E1B + GPS L5 + } + if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count != 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 23; // GLONASS L1 C/A + } + if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count != 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 24; // GLONASS L2 C/A + } + if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count != 0) && (glo_2G_count != 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 25; // GLONASS L1 C/A + GLONASS L2 C/A + } + if ((gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count != 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 26; // GPS L1 C/A + GLONASS L1 C/A + } + if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count != 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 27; // Galileo E1B + GLONASS L1 C/A + } + if ((gps_1C_count == 0) && (gps_2S_count != 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count != 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 28; // GPS L2C + GLONASS L1 C/A + } + if ((gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count != 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 29; // GPS L1 C/A + GLONASS L2 C/A + } + if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count != 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 30; // Galileo E1B + GLONASS L2 C/A + } + if ((gps_1C_count == 0) && (gps_2S_count != 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count != 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 31; // GPS L2C + GLONASS L2 C/A + } + if ((gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count != 0) && (gal_1B_count != 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 32; // L1+E1+L5+E5a + } + if ((gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 33; // L1+E1+E5a + } + // Galileo E6 + if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count != 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 100; // Galileo E6B + } + if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count != 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 101; // Galileo E1B + Galileo E6B + } + if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (gal_E6_count != 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 102; // Galileo E5a + Galileo E6B + } + if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0) && (gal_E6_count != 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 103; // Galileo E5b + Galileo E6B + } + if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (gal_E6_count != 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 104; // Galileo E1B + Galileo E5a + Galileo E6B + } + if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0) && (gal_E6_count != 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 105; // Galileo E1B + Galileo E5b + Galileo E6B + } + if ((gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count != 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 106; // GPS L1 C/A + Galileo E1B + Galileo E6B + } + if ((gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count != 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 107; // GPS L1 C/A + Galileo E6B + } + // BeiDou B1I Receiver + if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count != 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 500; // Beidou B1I + } + if ((gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count != 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 501; // Beidou B1I + GPS L1 C/A + } + if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count != 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 502; // Beidou B1I + Galileo E1B + } + if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count != 0) && (glo_2G_count == 0) && (bds_B1_count != 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 503; // Beidou B1I + GLONASS L1 C/A + } + if ((gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count != 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 504; // Beidou B1I + GPS L1 C/A + Galileo E1B + } + if ((gps_1C_count != 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count != 0) && (glo_2G_count == 0) && (bds_B1_count != 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 505; // Beidou B1I + GPS L1 C/A + GLONASS L1 C/A + Galileo E1B + } + if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count != 0) && (bds_B3_count != 0)) + { + pvt_output_parameters.type_of_receiver = 506; // Beidou B1I + Beidou B3I + } + // BeiDou B3I Receiver + if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count != 0)) + { + pvt_output_parameters.type_of_receiver = 600; // Beidou B3I + } + if ((gps_1C_count != 0) && (gps_2S_count != 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count != 0)) + { + pvt_output_parameters.type_of_receiver = 601; // Beidou B3I + GPS L2C + } + if ((gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count != 0) && (bds_B1_count == 0) && (bds_B3_count != 0)) + { + pvt_output_parameters.type_of_receiver = 602; // Beidou B3I + GLONASS L2 C/A + } + if ((gps_1C_count == 0) && (gps_2S_count != 0) && (gps_L5_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count != 0) && (glo_2G_count != 0) && (bds_B1_count == 0) && (bds_B3_count != 0)) + { + pvt_output_parameters.type_of_receiver = 603; // Beidou B3I + GPS L2C + GLONASS L2 C/A + } + if ((gps_1C_count != 0) && (gps_2S_count != 0) && (gps_L5_count != 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 1000; // GPS L1 + GPS L2C + GPS L5 + } + if ((gps_1C_count != 0) && (gps_2S_count != 0) && (gps_L5_count != 0) && (gal_1B_count != 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0) && (gal_E6_count == 0) && (glo_1G_count == 0) && (glo_2G_count == 0) && (bds_B1_count == 0) && (bds_B3_count == 0)) + { + pvt_output_parameters.type_of_receiver = 1001; // GPS L1 + Galileo E1B + GPS L2C + GPS L5 + Galileo E5a + } + + // RTKLIB PVT solver options + // Settings 1 + int positioning_mode = -1; + const std::string default_pos_mode("Single"); + const std::string positioning_mode_str = configuration->property(role + ".positioning_mode", default_pos_mode); // (PMODE_XXX) see src/algorithms/libs/rtklib/rtklib.h + if (positioning_mode_str == "Single") + { + positioning_mode = PMODE_SINGLE; + } + if (positioning_mode_str == "Static") + { + positioning_mode = PMODE_STATIC; + } + if (positioning_mode_str == "Kinematic") + { + positioning_mode = PMODE_KINEMA; + } + if (positioning_mode_str == "PPP_Static") + { + positioning_mode = PMODE_PPP_STATIC; + } + if (positioning_mode_str == "PPP_Kinematic") + { + positioning_mode = PMODE_PPP_KINEMA; + } + + if (positioning_mode == -1) + { + // warn user and set the default + std::cout << "WARNING: Bad specification of positioning mode.\n" + << "positioning_mode possible values: Single / Static / Kinematic / PPP_Static / PPP_Kinematic\n" + << "positioning_mode specified value: " << positioning_mode_str << "\n" + << "Setting positioning_mode to Single\n" + << std::flush; + positioning_mode = PMODE_SINGLE; + } + + int num_bands = 0; + + if ((gps_1C_count > 0) || (gal_1B_count > 0) || (glo_1G_count > 0) || (bds_B1_count > 0)) + { + num_bands += 1; + } + if ((gps_2S_count > 0) || (glo_2G_count > 0) || (bds_B3_count > 0)) + { + num_bands += 1; + } + if (gal_E6_count > 0) + { + num_bands += 1; + } + if ((gal_E5a_count > 0) || (gps_L5_count > 0)) + { + num_bands += 1; + } + if (gal_E5b_count > 0) + { + num_bands += 1; + } + if (num_bands > 3) + { + LOG(WARNING) << "Too much bands: The PVT engine can only handle 3 bands, but " << num_bands << " were set"; + num_bands = 3; + } + + int number_of_frequencies = configuration->property(role + ".num_bands", num_bands); /* (1:L1, 2:L1+L2, 3:L1+L2+L5) */ + if ((number_of_frequencies < 1) || (number_of_frequencies > 3)) + { + // warn user and set the default + number_of_frequencies = num_bands; + } + + double elevation_mask = configuration->property(role + ".elevation_mask", 15.0); + if ((elevation_mask < 0.0) || (elevation_mask > 90.0)) + { + // warn user and set the default + LOG(WARNING) << "Erroneous Elevation Mask. Setting to default value of 15.0 degrees"; + elevation_mask = 15.0; + } + + int dynamics_model = configuration->property(role + ".dynamics_model", 0); /* dynamics model (0:none, 1:velocity, 2:accel) */ + if ((dynamics_model < 0) || (dynamics_model > 2)) + { + // warn user and set the default + LOG(WARNING) << "Erroneous Dynamics Model configuration. Setting to default value of (0:none)"; + dynamics_model = 0; + } + + const std::string default_iono_model("OFF"); + const std::string iono_model_str = configuration->property(role + ".iono_model", default_iono_model); /* (IONOOPT_XXX) see src/algorithms/libs/rtklib/rtklib.h */ + int iono_model = -1; + if (iono_model_str == "OFF") + { + iono_model = IONOOPT_OFF; + } + if (iono_model_str == "Broadcast") + { + iono_model = IONOOPT_BRDC; + } + if (iono_model_str == "SBAS") + { + iono_model = IONOOPT_SBAS; + } + if (iono_model_str == "Iono-Free-LC") + { + iono_model = IONOOPT_IFLC; + } + if (iono_model_str == "Estimate_STEC") + { + iono_model = IONOOPT_EST; + } + if (iono_model_str == "IONEX") + { + iono_model = IONOOPT_TEC; + } + if (iono_model == -1) + { + // warn user and set the default + std::cout << "WARNING: Bad specification of ionospheric model.\n" + << "iono_model possible values: OFF / Broadcast / SBAS / Iono-Free-LC / Estimate_STEC / IONEX\n" + << "iono_model specified value: " << iono_model_str << "\n" + << "Setting iono_model to OFF\n" + << std::flush; + iono_model = IONOOPT_OFF; /* 0: ionosphere option: correction off */ + } + + const std::string default_trop_model("OFF"); + int trop_model = -1; + const std::string trop_model_str = configuration->property(role + ".trop_model", default_trop_model); /* (TROPOPT_XXX) see src/algorithms/libs/rtklib/rtklib.h */ + if (trop_model_str == "OFF") + { + trop_model = TROPOPT_OFF; + } + if (trop_model_str == "Saastamoinen") + { + trop_model = TROPOPT_SAAS; + } + if (trop_model_str == "SBAS") + { + trop_model = TROPOPT_SBAS; + } + if (trop_model_str == "Estimate_ZTD") + { + trop_model = TROPOPT_EST; + } + if (trop_model_str == "Estimate_ZTD_Grad") + { + trop_model = TROPOPT_ESTG; + } + if (trop_model == -1) + { + // warn user and set the default + std::cout << "WARNING: Bad specification of tropospheric model.\n" + << "trop_model possible values: OFF / Saastamoinen / SBAS / Estimate_ZTD / Estimate_ZTD_Grad\n" + << "trop_model specified value: " << trop_model_str << "\n" + << "Setting trop_model to OFF\n" + << std::flush; + trop_model = TROPOPT_OFF; + } + + /* RTKLIB positioning options */ + int sat_PCV = 0; /* Set whether the satellite antenna PCV (phase center variation) model is used or not. This feature requires a Satellite Antenna PCV File. */ + int rec_PCV = 0; /* Set whether the receiver antenna PCV (phase center variation) model is used or not. This feature requires a Receiver Antenna PCV File. */ + + /* Set whether the phase windup correction for PPP modes is applied or not. Only applicable to PPP‐* modes.*/ + const int phwindup = configuration->property(role + ".phwindup", 0); + + /* Set whether the GPS Block IIA satellites in eclipse are excluded or not. + The eclipsing Block IIA satellites often degrade the PPP solutions due to unpredicted behavior of yaw‐attitude. Only applicable to PPP‐* modes.*/ + const int reject_GPS_IIA = configuration->property(role + ".reject_GPS_IIA", 0); + + /* Set whether RAIM (receiver autonomous integrity monitoring) FDE (fault detection and exclusion) feature is enabled or not. + In case of RAIM FDE enabled, a satellite is excluded if SSE (sum of squared errors) of residuals is over a threshold. + The excluded satellite is selected to indicate the minimum SSE. */ + const int raim_fde = configuration->property(role + ".raim_fde", 0); + + const int earth_tide = configuration->property(role + ".earth_tide", 0); + + int nsys = 0; + if ((gps_1C_count > 0) || (gps_2S_count > 0) || (gps_L5_count > 0)) + { + nsys += SYS_GPS; + } + if ((gal_1B_count > 0) || (gal_E5a_count > 0) || (gal_E5b_count > 0) || (gal_E6_count > 0)) + { + nsys += SYS_GAL; + } + if ((glo_1G_count > 0) || (glo_2G_count > 0)) + { + nsys += SYS_GLO; + } + if ((bds_B1_count > 0) || (bds_B3_count > 0)) + { + nsys += SYS_BDS; + } + + int navigation_system = configuration->property(role + ".navigation_system", nsys); /* (SYS_XXX) see src/algorithms/libs/rtklib/rtklib.h */ + if ((navigation_system < 1) || (navigation_system > 255)) /* GPS: 1 SBAS: 2 GPS+SBAS: 3 Galileo: 8 Galileo+GPS: 9 GPS+SBAS+Galileo: 11 All: 255 */ + { + // warn user and set the default + LOG(WARNING) << "Erroneous Navigation System. Setting to default value of (0:none)"; + navigation_system = nsys; + } + + // Settings 2 + const std::string default_gps_ar("Continuous"); + const std::string integer_ambiguity_resolution_gps_str = configuration->property(role + ".AR_GPS", default_gps_ar); /* Integer Ambiguity Resolution mode for GPS (0:off,1:continuous,2:instantaneous,3:fix and hold,4:ppp-ar) */ + int integer_ambiguity_resolution_gps = -1; + if (integer_ambiguity_resolution_gps_str == "OFF") + { + integer_ambiguity_resolution_gps = ARMODE_OFF; + } + if (integer_ambiguity_resolution_gps_str == "Continuous") + { + integer_ambiguity_resolution_gps = ARMODE_CONT; + } + if (integer_ambiguity_resolution_gps_str == "Instantaneous") + { + integer_ambiguity_resolution_gps = ARMODE_INST; + } + if (integer_ambiguity_resolution_gps_str == "Fix-and-Hold") + { + integer_ambiguity_resolution_gps = ARMODE_FIXHOLD; + } + if (integer_ambiguity_resolution_gps_str == "PPP-AR") + { + integer_ambiguity_resolution_gps = ARMODE_PPPAR; + } + if (integer_ambiguity_resolution_gps == -1) + { + // warn user and set the default + std::cout << "WARNING: Bad specification of GPS ambiguity resolution method.\n" + << "AR_GPS possible values: OFF / Continuous / Instantaneous / Fix-and-Hold / PPP-AR\n" + << "AR_GPS specified value: " << integer_ambiguity_resolution_gps_str << "\n" + << "Setting AR_GPS to OFF\n" + << std::flush; + integer_ambiguity_resolution_gps = ARMODE_OFF; + } + + int integer_ambiguity_resolution_glo = configuration->property(role + ".AR_GLO", 1); /* Integer Ambiguity Resolution mode for GLONASS (0:off,1:on,2:auto cal,3:ext cal) */ + if ((integer_ambiguity_resolution_glo < 0) || (integer_ambiguity_resolution_glo > 3)) + { + // warn user and set the default + LOG(WARNING) << "Erroneous Integer Ambiguity Resolution for GLONASS . Setting to default value of (1:on)"; + integer_ambiguity_resolution_glo = 1; + } + + int integer_ambiguity_resolution_bds = configuration->property(role + ".AR_DBS", 1); /* Integer Ambiguity Resolution mode for BEIDOU (0:off,1:on) */ + if ((integer_ambiguity_resolution_bds < 0) || (integer_ambiguity_resolution_bds > 1)) + { + // warn user and set the default + LOG(WARNING) << "Erroneous Integer Ambiguity Resolution for BEIDOU . Setting to default value of (1:on)"; + integer_ambiguity_resolution_bds = 1; + } + + const double min_ratio_to_fix_ambiguity = configuration->property(role + ".min_ratio_to_fix_ambiguity", 3.0); /* Set the integer ambiguity validation threshold for ratio‐test, + which uses the ratio of squared residuals of the best integer vector to the second‐best vector. */ + + const int min_lock_to_fix_ambiguity = configuration->property(role + ".min_lock_to_fix_ambiguity", 0); /* Set the minimum lock count to fix integer ambiguity.FLAGS_RINEX_version. + If the lock count is less than the value, the ambiguity is excluded from the fixed integer vector. */ + + const double min_elevation_to_fix_ambiguity = configuration->property(role + ".min_elevation_to_fix_ambiguity", 0.0); /* Set the minimum elevation (deg) to fix integer ambiguity. + If the elevation of the satellite is less than the value, the ambiguity is excluded from the fixed integer vector. */ + + const int outage_reset_ambiguity = configuration->property(role + ".outage_reset_ambiguity", 5); /* Set the outage count to reset ambiguity. If the data outage count is over the value, the estimated ambiguity is reset to the initial value. */ + + const double slip_threshold = configuration->property(role + ".slip_threshold", 0.05); /* set the cycle‐slip threshold (m) of geometry‐free LC carrier‐phase difference between epochs */ + + const double threshold_reject_gdop = configuration->property(role + ".threshold_reject_gdop", 30.0); /* reject threshold of GDOP. If the GDOP is over the value, the observable is excluded for the estimation process as an outlier. */ + + const double threshold_reject_innovation = configuration->property(role + ".threshold_reject_innovation", 30.0); /* reject threshold of innovation (m). If the innovation is over the value, the observable is excluded for the estimation process as an outlier. */ + + const int number_filter_iter = configuration->property(role + ".number_filter_iter", 1); /* Set the number of iteration in the measurement update of the estimation filter. + If the baseline length is very short like 1 m, the iteration may be effective to handle + the nonlinearity of measurement equation. */ + + // Statistics + const double bias_0 = configuration->property(role + ".bias_0", 30.0); + + const double iono_0 = configuration->property(role + ".iono_0", 0.03); + + const double trop_0 = configuration->property(role + ".trop_0", 0.3); + + const double sigma_bias = configuration->property(role + ".sigma_bias", 1e-4); /* Set the process noise standard deviation of carrier‐phase + bias (ambiguity) (cycle/sqrt(s)) */ + + const double sigma_iono = configuration->property(role + ".sigma_iono", 1e-3); /* Set the process noise standard deviation of vertical ionospheric delay per 10 km baseline (m/sqrt(s)). */ + + const double sigma_trop = configuration->property(role + ".sigma_trop", 1e-4); /* Set the process noise standard deviation of zenith tropospheric delay (m/sqrt(s)). */ + + const double sigma_acch = configuration->property(role + ".sigma_acch", 1e-1); /* Set the process noise standard deviation of the receiver acceleration as + the horizontal component. (m/s2/sqrt(s)). If Receiver Dynamics is set to OFF, they are not used. */ + + const double sigma_accv = configuration->property(role + ".sigma_accv", 1e-2); /* Set the process noise standard deviation of the receiver acceleration as + the vertical component. (m/s2/sqrt(s)). If Receiver Dynamics is set to OFF, they are not used. */ + + const double sigma_pos = configuration->property(role + ".sigma_pos", 0.0); + + const double code_phase_error_ratio_l1 = configuration->property(role + ".code_phase_error_ratio_l1", 100.0); + const double code_phase_error_ratio_l2 = configuration->property(role + ".code_phase_error_ratio_l2", 100.0); + const double code_phase_error_ratio_l5 = configuration->property(role + ".code_phase_error_ratio_l5", 100.0); + const double carrier_phase_error_factor_a = configuration->property(role + ".carrier_phase_error_factor_a", 0.003); + const double carrier_phase_error_factor_b = configuration->property(role + ".carrier_phase_error_factor_b", 0.003); + + const bool bancroft_init = configuration->property(role + ".bancroft_init", true); + + snrmask_t snrmask = {{}, {{}, {}}}; + + prcopt_t rtklib_configuration_options = { + positioning_mode, /* positioning mode (PMODE_XXX) see src/algorithms/libs/rtklib/rtklib.h */ + 0, /* solution type (0:forward,1:backward,2:combined) */ + number_of_frequencies, /* number of frequencies (1:L1, 2:L1+L2, 3:L1+L2+L5)*/ + navigation_system, /* navigation system */ + elevation_mask * D2R, /* elevation mask angle (degrees) */ + snrmask, /* snrmask_t snrmask SNR mask */ + 0, /* satellite ephemeris/clock (EPHOPT_XXX) */ + integer_ambiguity_resolution_gps, /* AR mode (0:off,1:continuous,2:instantaneous,3:fix and hold,4:ppp-ar) */ + integer_ambiguity_resolution_glo, /* GLONASS AR mode (0:off,1:on,2:auto cal,3:ext cal) */ + integer_ambiguity_resolution_bds, /* BeiDou AR mode (0:off,1:on) */ + outage_reset_ambiguity, /* obs outage count to reset bias */ + min_lock_to_fix_ambiguity, /* min lock count to fix ambiguity */ + 10, /* min fix count to hold ambiguity */ + 1, /* max iteration to resolve ambiguity */ + iono_model, /* ionosphere option (IONOOPT_XXX) */ + trop_model, /* troposphere option (TROPOPT_XXX) */ + dynamics_model, /* dynamics model (0:none, 1:velocity, 2:accel) */ + earth_tide, /* earth tide correction (0:off,1:solid,2:solid+otl+pole) */ + number_filter_iter, /* number of filter iteration */ + 0, /* code smoothing window size (0:none) */ + 0, /* interpolate reference obs (for post mission) */ + 0, /* sbssat_t sbssat SBAS correction options */ + 0, /* sbsion_t sbsion[MAXBAND+1] SBAS satellite selection (0:all) */ + 0, /* rover position for fixed mode */ + 0, /* base position for relative mode */ + /* 0:pos in prcopt, 1:average of single pos, */ + /* 2:read from file, 3:rinex header, 4:rtcm pos */ + {code_phase_error_ratio_l1, code_phase_error_ratio_l2, code_phase_error_ratio_l5}, /* eratio[NFREQ] code/phase error ratio */ + {100.0, carrier_phase_error_factor_a, carrier_phase_error_factor_b, 0.0, 1.0}, /* err[5]: measurement error factor [0]:reserved, [1-3]:error factor a/b/c of phase (m) , [4]:doppler frequency (hz) */ + {bias_0, iono_0, trop_0}, /* std[3]: initial-state std [0]bias,[1]iono [2]trop*/ + {sigma_bias, sigma_iono, sigma_trop, sigma_acch, sigma_accv, sigma_pos}, /* prn[6] process-noise std */ + 5e-12, /* sclkstab: satellite clock stability (sec/sec) */ + {min_ratio_to_fix_ambiguity, 0.9999, 0.25, 0.1, 0.05, 0.0, 0.0, 0.0}, /* thresar[8]: AR validation threshold */ + min_elevation_to_fix_ambiguity, /* elevation mask of AR for rising satellite (deg) */ + 0.0, /* elevation mask to hold ambiguity (deg) */ + slip_threshold, /* slip threshold of geometry-free phase (m) */ + 30.0, /* max difference of time (sec) */ + threshold_reject_innovation, /* reject threshold of innovation (m) */ + threshold_reject_gdop, /* reject threshold of gdop */ + {}, /* double baseline[2] baseline length constraint {const,sigma} (m) */ + {}, /* double ru[3] rover position for fixed mode {x,y,z} (ecef) (m) */ + {}, /* double rb[3] base position for relative mode {x,y,z} (ecef) (m) */ + {"", ""}, /* char anttype[2][MAXANT] antenna types {rover,base} */ + {{}, {}}, /* double antdel[2][3] antenna delta {{rov_e,rov_n,rov_u},{ref_e,ref_n,ref_u}} */ + {}, /* pcv_t pcvr[2] receiver antenna parameters {rov,base} */ + {}, /* unsigned char exsats[MAXSAT] excluded satellites (1:excluded, 2:included) */ + 0, /* max averaging epoches */ + 0, /* initialize by restart */ + 1, /* output single by dgps/float/fix/ppp outage */ + {"", ""}, /* char rnxopt[2][256] rinex options {rover,base} */ + {sat_PCV, rec_PCV, phwindup, reject_GPS_IIA, raim_fde}, /* posopt[6] positioning options [0]: satellite and receiver antenna PCV model; [1]: interpolate antenna parameters; [2]: apply phase wind-up correction for PPP modes; [3]: exclude measurements of GPS Block IIA satellites satellite [4]: RAIM FDE (fault detection and exclusion) [5]: handle day-boundary clock jump */ + 0, /* solution sync mode (0:off,1:on) */ + {{}, {}}, /* odisp[2][6*11] ocean tide loading parameters {rov,base} */ + {{}, {{}, {}}, {{}, {}}, {}, {}}, /* exterr_t exterr extended receiver error model */ + 0, /* disable L2-AR */ + {}, /* char pppopt[256] ppp option "-GAP_RESION=" default gap to reset iono parameters (ep) */ + bancroft_init /* enable Bancroft initialization for the first iteration of the PVT computation, useful in some geometries */ + }; + + rtkinit(&rtk, &rtklib_configuration_options); + + // Outputs + const bool default_output_enabled = configuration->property(role + ".output_enabled", true); + pvt_output_parameters.output_enabled = default_output_enabled; + pvt_output_parameters.rinex_output_enabled = configuration->property(role + ".rinex_output_enabled", default_output_enabled); + pvt_output_parameters.gpx_output_enabled = configuration->property(role + ".gpx_output_enabled", default_output_enabled); + pvt_output_parameters.geojson_output_enabled = configuration->property(role + ".geojson_output_enabled", default_output_enabled); + pvt_output_parameters.kml_output_enabled = configuration->property(role + ".kml_output_enabled", default_output_enabled); + pvt_output_parameters.xml_output_enabled = configuration->property(role + ".xml_output_enabled", default_output_enabled); + pvt_output_parameters.nmea_output_file_enabled = configuration->property(role + ".nmea_output_file_enabled", default_output_enabled); + pvt_output_parameters.rtcm_output_file_enabled = configuration->property(role + ".rtcm_output_file_enabled", false); + + const std::string default_output_path = configuration->property(role + ".output_path", std::string(".")); + pvt_output_parameters.output_path = default_output_path; + pvt_output_parameters.rinex_output_path = configuration->property(role + ".rinex_output_path", default_output_path); + pvt_output_parameters.gpx_output_path = configuration->property(role + ".gpx_output_path", default_output_path); + pvt_output_parameters.geojson_output_path = configuration->property(role + ".geojson_output_path", default_output_path); + pvt_output_parameters.kml_output_path = configuration->property(role + ".kml_output_path", default_output_path); + pvt_output_parameters.xml_output_path = configuration->property(role + ".xml_output_path", default_output_path); + pvt_output_parameters.nmea_output_file_path = configuration->property(role + ".nmea_output_file_path", default_output_path); + pvt_output_parameters.rtcm_output_file_path = configuration->property(role + ".rtcm_output_file_path", default_output_path); + + // Read PVT MONITOR Configuration + pvt_output_parameters.monitor_enabled = configuration->property(role + ".enable_monitor", false); + pvt_output_parameters.udp_addresses = configuration->property(role + ".monitor_client_addresses", std::string("127.0.0.1")); + pvt_output_parameters.udp_port = configuration->property(role + ".monitor_udp_port", 1234); + pvt_output_parameters.protobuf_enabled = configuration->property(role + ".enable_protobuf", true); + if (configuration->property("Monitor.enable_protobuf", false) == true) + { + pvt_output_parameters.protobuf_enabled = true; + } + + // Read EPHEMERIS MONITOR Configuration + pvt_output_parameters.monitor_ephemeris_enabled = configuration->property(role + ".enable_monitor_ephemeris", false); + pvt_output_parameters.udp_eph_addresses = configuration->property(role + ".monitor_ephemeris_client_addresses", std::string("127.0.0.1")); + pvt_output_parameters.udp_eph_port = configuration->property(role + ".monitor_ephemeris_udp_port", 1234); + + // Show time in local zone + pvt_output_parameters.show_local_time_zone = configuration->property(role + ".show_local_time_zone", false); + + // Enable or disable rx clock correction in observables + pvt_output_parameters.enable_rx_clock_correction = configuration->property(role + ".enable_rx_clock_correction", false); + + // Set maximum clock offset allowed if pvt_output_parameters.enable_rx_clock_correction = false + pvt_output_parameters.max_obs_block_rx_clock_offset_ms = configuration->property(role + ".max_clock_offset_ms", pvt_output_parameters.max_obs_block_rx_clock_offset_ms); + + // Source timetag + pvt_output_parameters.log_source_timetag = configuration->property(role + ".log_timetag", pvt_output_parameters.log_source_timetag); + pvt_output_parameters.log_source_timetag_file = configuration->property(role + ".log_source_timetag_file", pvt_output_parameters.log_source_timetag_file); + + // Use E6 for PVT + pvt_output_parameters.use_e6_for_pvt = configuration->property(role + ".use_e6_for_pvt", pvt_output_parameters.use_e6_for_pvt); + pvt_output_parameters.use_has_corrections = configuration->property(role + ".use_has_corrections", pvt_output_parameters.use_has_corrections); + + // Use unhealthy satellites + pvt_output_parameters.use_unhealthy_sats = configuration->property(role + ".use_unhealthy_sats", pvt_output_parameters.use_unhealthy_sats); + + // make PVT object + pvt_ = rtklib_make_pvt_gs(in_streams_, pvt_output_parameters, rtk); + DLOG(INFO) << "pvt(" << pvt_->unique_id() << ")"; + if (out_streams_ > 0) + { + LOG(ERROR) << "The PVT block does not have an output stream"; + } +} + + +Rtklib_Pvt::~Rtklib_Pvt() +{ + DLOG(INFO) << "PVT adapter destructor called."; + rtkfree(&rtk); +} + + +bool Rtklib_Pvt::get_latest_PVT(double* longitude_deg, + double* latitude_deg, + double* height_m, + double* ground_speed_kmh, + double* course_over_ground_deg, + time_t* UTC_time) +{ + return pvt_->get_latest_PVT(longitude_deg, + latitude_deg, + height_m, + ground_speed_kmh, + course_over_ground_deg, + UTC_time); +} + + +void Rtklib_Pvt::clear_ephemeris() +{ + pvt_->clear_ephemeris(); +} + + +std::map Rtklib_Pvt::get_gps_ephemeris() const +{ + return pvt_->get_gps_ephemeris_map(); +} + + +std::map Rtklib_Pvt::get_galileo_ephemeris() const +{ + return pvt_->get_galileo_ephemeris_map(); +} + + +std::map Rtklib_Pvt::get_gps_almanac() const +{ + return pvt_->get_gps_almanac_map(); +} + + +std::map Rtklib_Pvt::get_galileo_almanac() const +{ + return pvt_->get_galileo_almanac_map(); +} + + +void Rtklib_Pvt::connect(gr::top_block_sptr top_block) +{ + if (top_block) + { /* top_block is not null */ + }; + // Nothing to connect internally + DLOG(INFO) << "nothing to connect internally"; +} + + +void Rtklib_Pvt::disconnect(gr::top_block_sptr top_block) +{ + if (top_block) + { /* top_block is not null */ + }; + // Nothing to disconnect +} + + +gr::basic_block_sptr Rtklib_Pvt::get_left_block() +{ + return pvt_; +} + + +gr::basic_block_sptr Rtklib_Pvt::get_right_block() +{ + return nullptr; // this is a sink, nothing downstream +} diff --git a/src/algorithms/PVT/libs/pvt_conf.h b/src/algorithms/PVT/libs/pvt_conf.h old mode 100644 new mode 100755 index bf4a3501a..f194c0596 --- a/src/algorithms/PVT/libs/pvt_conf.h +++ b/src/algorithms/PVT/libs/pvt_conf.h @@ -1,109 +1,110 @@ -/*! - * \file pvt_conf.h - * \brief Class that contains all the configuration parameters for the PVT block - * \author Carles Fernandez, 2018. cfernandez(at)cttc.es - * - * ----------------------------------------------------------------------------- - * - * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. - * This file is part of GNSS-SDR. - * - * Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors) - * SPDX-License-Identifier: GPL-3.0-or-later - * - * ----------------------------------------------------------------------------- - */ - -#ifndef GNSS_SDR_PVT_CONF_H -#define GNSS_SDR_PVT_CONF_H - -#include -#include -#include - -/** \addtogroup PVT - * \{ */ -/** \addtogroup PVT_libs - * \{ */ - - -class Pvt_Conf -{ -public: - std::map rtcm_msg_rate_ms; - - std::string rinex_name = std::string("-"); - std::string dump_filename; - std::string nmea_dump_filename; - std::string nmea_dump_devname; - std::string rtcm_dump_devname; - std::string an_dump_devname; - std::string output_path = std::string("."); - std::string rinex_output_path = std::string("."); - std::string gpx_output_path = std::string("."); - std::string geojson_output_path = std::string("."); - std::string nmea_output_file_path = std::string("."); - std::string kml_output_path = std::string("."); - std::string xml_output_path = std::string("."); - std::string rtcm_output_file_path = std::string("."); - std::string udp_addresses; - std::string udp_eph_addresses; - std::string log_source_timetag_file; - - uint32_t type_of_receiver = 0; - uint32_t observable_interval_ms = 20; - - int32_t output_rate_ms = 0; - int32_t display_rate_ms = 0; - int32_t kml_rate_ms = 1000; - int32_t gpx_rate_ms = 1000; - int32_t geojson_rate_ms = 1000; - int32_t nmea_rate_ms = 1000; - int32_t rinex_version = 0; - int32_t rinexobs_rate_ms = 0; - int32_t an_rate_ms = 1000; - int32_t max_obs_block_rx_clock_offset_ms = 40; - int udp_port = 0; - int udp_eph_port = 0; - int rtk_trace_level = 0; - - uint16_t rtcm_tcp_port = 0; - uint16_t rtcm_station_id = 0; - - bool flag_nmea_tty_port = false; - bool flag_rtcm_server = false; - bool flag_rtcm_tty_port = false; - bool output_enabled = true; - bool rinex_output_enabled = true; - bool gpx_output_enabled = true; - bool geojson_output_enabled = true; - bool nmea_output_file_enabled = true; - bool an_output_enabled = false; - bool kml_output_enabled = true; - bool xml_output_enabled = true; - bool rtcm_output_file_enabled = true; - bool monitor_enabled = false; - bool monitor_ephemeris_enabled = false; - bool protobuf_enabled = true; - bool enable_rx_clock_correction = true; - bool show_local_time_zone = false; - bool pre_2009_file = false; - bool dump = false; - bool dump_mat = true; - bool log_source_timetag; - bool use_e6_for_pvt = true; - bool use_has_corrections = true; - bool use_unhealthy_sats = false; - - // PVT KF parameters - bool enable_pvt_kf = false; - double measures_ecef_pos_sd_m = 1.0; - double measures_ecef_vel_sd_ms = 0.1; - double system_ecef_pos_sd_m = 0.01; - double system_ecef_vel_sd_ms = 0.001; -}; - - -/** \} */ -/** \} */ -#endif // GNSS_SDR_PVT_CONF_H +/*! + * \file pvt_conf.h + * \brief Class that contains all the configuration parameters for the PVT block + * \author Carles Fernandez, 2018. cfernandez(at)cttc.es + * + * ----------------------------------------------------------------------------- + * + * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. + * This file is part of GNSS-SDR. + * + * Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_PVT_CONF_H +#define GNSS_SDR_PVT_CONF_H + +#include +#include +#include + +/** \addtogroup PVT + * \{ */ +/** \addtogroup PVT_libs + * \{ */ + + +class Pvt_Conf +{ +public: + std::map rtcm_msg_rate_ms; + + std::string rinex_name = std::string("-"); + std::string dump_filename; + std::string nmea_dump_filename; + std::string nmea_dump_devname; + std::string rtcm_dump_devname; + std::string an_dump_devname; + std::string output_path = std::string("."); + std::string rinex_output_path = std::string("."); + std::string gpx_output_path = std::string("."); + std::string geojson_output_path = std::string("."); + std::string nmea_output_file_path = std::string("."); + std::string kml_output_path = std::string("."); + std::string xml_output_path = std::string("."); + std::string rtcm_output_file_path = std::string("."); + std::string udp_addresses; + std::string udp_eph_addresses; + std::string log_source_timetag_file; + + uint32_t type_of_receiver = 0; + uint32_t observable_interval_ms = 20; + + int32_t output_rate_ms = 0; + int32_t display_rate_ms = 0; + int32_t kml_rate_ms = 1000; + int32_t gpx_rate_ms = 1000; + int32_t geojson_rate_ms = 1000; + int32_t nmea_rate_ms = 1000; + int32_t rinex_version = 0; + int32_t rinexobs_rate_ms = 0; + int32_t an_rate_ms = 1000; + int32_t max_obs_block_rx_clock_offset_ms = 40; + int udp_port = 0; + int udp_eph_port = 0; + int rtk_trace_level = 0; + + uint16_t rtcm_tcp_port = 0; + uint16_t rtcm_station_id = 0; + + bool flag_nmea_tty_port = false; + bool flag_rtcm_server = false; + bool flag_rtcm_tty_port = false; + bool output_enabled = true; + bool rinex_output_enabled = true; + bool gpx_output_enabled = true; + bool geojson_output_enabled = true; + bool nmea_output_file_enabled = true; + bool an_output_enabled = false; + bool kml_output_enabled = true; + bool xml_output_enabled = true; + bool rtcm_output_file_enabled = true; + bool monitor_enabled = false; + bool monitor_ephemeris_enabled = false; + bool protobuf_enabled = true; + bool enable_rx_clock_correction = true; + bool show_local_time_zone = false; + bool pre_2009_file = false; + bool dump = false; + bool dump_mat = true; + bool log_source_timetag; + bool use_e6_for_pvt = true; + bool use_has_corrections = true; + bool use_unhealthy_sats = false; + + // PVT KF parameters + bool enable_pvt_kf = false; + bool estatic_measures_sd = false; + double measures_ecef_pos_sd_m = 1.0; + double measures_ecef_vel_sd_ms = 0.1; + double system_ecef_pos_sd_m = 0.01; + double system_ecef_vel_sd_ms = 0.001; +}; + + +/** \} */ +/** \} */ +#endif // GNSS_SDR_PVT_CONF_H diff --git a/src/algorithms/PVT/libs/pvt_kf.cc b/src/algorithms/PVT/libs/pvt_kf.cc old mode 100644 new mode 100755 index 9a47732a0..17336e0fa --- a/src/algorithms/PVT/libs/pvt_kf.cc +++ b/src/algorithms/PVT/libs/pvt_kf.cc @@ -1,147 +1,162 @@ -/*! - * \file pvt_kf.cc - * \brief Kalman Filter for Position and Velocity - * \author Javier Arribas, 2023. jarribas(at)cttc.es - * - * - * ----------------------------------------------------------------------------- - * - * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. - * This file is part of GNSS-SDR. - * - * Copyright (C) 2010-2023 (see AUTHORS file for a list of contributors) - * SPDX-License-Identifier: GPL-3.0-or-later - * - * ----------------------------------------------------------------------------- - */ - -#include "pvt_kf.h" -#include - - -void Pvt_Kf::init_Kf(const arma::vec& p, - const arma::vec& v, - const arma::vec& res_p, - double solver_interval_s, - double measures_ecef_pos_sd_m, - double measures_ecef_vel_sd_ms, - double system_ecef_pos_sd_m, - double system_ecef_vel_sd_ms) -{ - // Kalman Filter class variables - const double Ti = solver_interval_s; - - d_F = {{1.0, 0.0, 0.0, Ti, 0.0, 0.0}, - {0.0, 1.0, 0.0, 0.0, Ti, 0.0}, - {0.0, 0.0, 1.0, 0.0, 0.0, Ti}, - {0.0, 0.0, 0.0, 1.0, 0.0, 0.0}, - {0.0, 0.0, 0.0, 0.0, 1.0, 0.0}, - {0.0, 0.0, 0.0, 0.0, 0.0, 1.0}}; - - d_H = arma::eye(6, 6); - - // measurement matrix static covariances - d_R = {{res_p[0], res_p[3], res_p[5], 0.0, 0.0, 0.0}, - {res_p[3], res_p[1], res_p[4], 0.0, 0.0, 0.0}, - {res_p[4], res_p[5], res_p[2], 0.0, 0.0, 0.0}, - {0.0, 0.0, 0.0, pow(measures_ecef_vel_sd_ms, 2.0), 0.0, 0.0}, - {0.0, 0.0, 0.0, 0.0, pow(measures_ecef_vel_sd_ms, 2.0), 0.0}, - {0.0, 0.0, 0.0, 0.0, 0.0, pow(measures_ecef_vel_sd_ms, 2.0)}}; - - // system covariance matrix (static) - d_Q = {{pow(system_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0, 0.0, 0.0}, - {0.0, pow(system_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0, 0.0}, - {0.0, 0.0, pow(system_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0}, - {0.0, 0.0, 0.0, pow(system_ecef_vel_sd_ms, 2.0), 0.0, 0.0}, - {0.0, 0.0, 0.0, 0.0, pow(system_ecef_vel_sd_ms, 2.0), 0.0}, - {0.0, 0.0, 0.0, 0.0, 0.0, pow(system_ecef_vel_sd_ms, 2.0)}}; - - // initial Kalman covariance matrix - d_P_old_old = {{pow(system_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0, 0.0, 0.0}, - {0.0, pow(system_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0, 0.0}, - {0.0, 0.0, pow(system_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0}, - {0.0, 0.0, 0.0, pow(system_ecef_vel_sd_ms, 2.0), 0.0, 0.0}, - {0.0, 0.0, 0.0, 0.0, pow(system_ecef_vel_sd_ms, 2.0), 0.0}, - {0.0, 0.0, 0.0, 0.0, 0.0, pow(system_ecef_vel_sd_ms, 2.0)}}; - - // states: position ecef [m], velocity ecef [m/s] - d_x_old_old = arma::zeros(6); - d_x_old_old.subvec(0, 2) = p; - d_x_old_old.subvec(3, 5) = v; - - d_initialized = true; - - DLOG(INFO) << "Ti: " << Ti; - DLOG(INFO) << "F: " << d_F; - DLOG(INFO) << "H: " << d_H; - DLOG(INFO) << "R: " << d_R; - DLOG(INFO) << "Q: " << d_Q; - DLOG(INFO) << "P: " << d_P_old_old; - DLOG(INFO) << "x: " << d_x_old_old; -} - - -bool Pvt_Kf::is_initialized() const -{ - return d_initialized; -} - - -void Pvt_Kf::reset_Kf() -{ - d_initialized = false; -} - - -void Pvt_Kf::run_Kf(const arma::vec& p, const arma::vec& v, const arma::vec& res_p) -{ - if (d_initialized) - { - // Kalman loop - // Prediction - d_x_new_old = d_F * d_x_old_old; - d_P_new_old = d_F * d_P_old_old * d_F.t() + d_Q; - - // Measurement update - try - { - // Measurement residuals - d_R(0, 0) = res_p[0]; - d_R(0, 1) = res_p[3]; - d_R(0, 2) = res_p[5]; - d_R(1, 0) = res_p[3]; - d_R(1, 1) = res_p[1]; - d_R(1, 2) = res_p[4]; - d_R(2, 0) = res_p[5]; - d_R(2, 1) = res_p[4]; - d_R(2, 2) = res_p[2]; - - // Measurement update - arma::vec z = arma::join_cols(p, v); - arma::mat K = d_P_new_old * d_H.t() * arma::inv(d_H * d_P_new_old * d_H.t() + d_R); // Kalman gain - - d_x_new_new = d_x_new_old + K * (z - d_H * d_x_new_old); - arma::mat A = (arma::eye(6, 6) - K * d_H); - d_P_new_new = A * d_P_new_old * A.t() + K * d_R * K.t(); - - // prepare data for next KF epoch - d_x_old_old = d_x_new_new; - d_P_old_old = d_P_new_new; - } - catch (...) - { - d_x_new_new = d_x_new_old; - this->reset_Kf(); - } - } -} - - -void Pvt_Kf::get_pv_Kf(arma::vec& p, arma::vec& v) const -{ - if (d_initialized) - { - p = d_x_new_new.subvec(0, 2); - v = d_x_new_new.subvec(3, 5); - } -} +/*! + * \file pvt_kf.cc + * \brief Kalman Filter for Position and Velocity + * \author Javier Arribas, 2023. jarribas(at)cttc.es + * + * + * ----------------------------------------------------------------------------- + * + * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. + * This file is part of GNSS-SDR. + * + * Copyright (C) 2010-2023 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + +#include "pvt_kf.h" +#include + + +void Pvt_Kf::init_Kf(const arma::vec& p, + const arma::vec& v, + const arma::vec& res_p, + double solver_interval_s, + bool estatic_measures_sd, + double measures_ecef_pos_sd_m, + double measures_ecef_vel_sd_ms, + double system_ecef_pos_sd_m, + double system_ecef_vel_sd_ms) +{ + // Kalman Filter class variables + const double Ti = solver_interval_s; + + d_F = {{1.0, 0.0, 0.0, Ti, 0.0, 0.0}, + {0.0, 1.0, 0.0, 0.0, Ti, 0.0}, + {0.0, 0.0, 1.0, 0.0, 0.0, Ti}, + {0.0, 0.0, 0.0, 1.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, 0.0, 1.0, 0.0}, + {0.0, 0.0, 0.0, 0.0, 0.0, 1.0}}; + + d_H = arma::eye(6, 6); + + // measurement matrix static covariances + if(estatic_measures_sd){ + d_R = {{pow(measures_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, pow(measures_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, pow(measures_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, pow(measures_ecef_vel_sd_ms, 2.0), 0.0, 0.0}, + {0.0, 0.0, 0.0, 0.0, pow(measures_ecef_vel_sd_ms, 2.0), 0.0}, + {0.0, 0.0, 0.0, 0.0, 0.0, pow(measures_ecef_vel_sd_ms, 2.0)}}; + + d_static = true; + + }else{ + d_R = {{res_p[0], res_p[3], res_p[5], 0.0, 0.0, 0.0}, + {res_p[3], res_p[1], res_p[4], 0.0, 0.0, 0.0}, + {res_p[4], res_p[5], res_p[2], 0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, pow(measures_ecef_vel_sd_ms, 2.0), 0.0, 0.0}, + {0.0, 0.0, 0.0, 0.0, pow(measures_ecef_vel_sd_ms, 2.0), 0.0}, + {0.0, 0.0, 0.0, 0.0, 0.0, pow(measures_ecef_vel_sd_ms, 2.0)}}; + + d_static = false; + } + // system covariance matrix (static) + d_Q = {{pow(system_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, pow(system_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, pow(system_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, pow(system_ecef_vel_sd_ms, 2.0), 0.0, 0.0}, + {0.0, 0.0, 0.0, 0.0, pow(system_ecef_vel_sd_ms, 2.0), 0.0}, + {0.0, 0.0, 0.0, 0.0, 0.0, pow(system_ecef_vel_sd_ms, 2.0)}}; + + // initial Kalman covariance matrix + d_P_old_old = {{pow(system_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0, 0.0, 0.0}, + {0.0, pow(system_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0, 0.0}, + {0.0, 0.0, pow(system_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, pow(system_ecef_vel_sd_ms, 2.0), 0.0, 0.0}, + {0.0, 0.0, 0.0, 0.0, pow(system_ecef_vel_sd_ms, 2.0), 0.0}, + {0.0, 0.0, 0.0, 0.0, 0.0, pow(system_ecef_vel_sd_ms, 2.0)}}; + + // states: position ecef [m], velocity ecef [m/s] + d_x_old_old = arma::zeros(6); + d_x_old_old.subvec(0, 2) = p; + d_x_old_old.subvec(3, 5) = v; + + d_initialized = true; + + DLOG(INFO) << "Ti: " << Ti; + DLOG(INFO) << "F: " << d_F; + DLOG(INFO) << "H: " << d_H; + DLOG(INFO) << "R: " << d_R; + DLOG(INFO) << "Q: " << d_Q; + DLOG(INFO) << "P: " << d_P_old_old; + DLOG(INFO) << "x: " << d_x_old_old; +} + + +bool Pvt_Kf::is_initialized() const +{ + return d_initialized; +} + + +void Pvt_Kf::reset_Kf() +{ + d_initialized = false; +} + + +void Pvt_Kf::run_Kf(const arma::vec& p, const arma::vec& v, const arma::vec& res_p) +{ + if (d_initialized) + { + // Kalman loop + // Prediction + d_x_new_old = d_F * d_x_old_old; + d_P_new_old = d_F * d_P_old_old * d_F.t() + d_Q; + + // Measurement update + try + { + if(!d_static){ + // Measurement residuals update + d_R(0, 0) = res_p[0]; + d_R(0, 1) = res_p[3]; + d_R(0, 2) = res_p[5]; + d_R(1, 0) = res_p[3]; + d_R(1, 1) = res_p[1]; + d_R(1, 2) = res_p[4]; + d_R(2, 0) = res_p[5]; + d_R(2, 1) = res_p[4]; + d_R(2, 2) = res_p[2]; + } + // Measurement update + arma::vec z = arma::join_cols(p, v); + arma::mat K = d_P_new_old * d_H.t() * arma::inv(d_H * d_P_new_old * d_H.t() + d_R); // Kalman gain + + d_x_new_new = d_x_new_old + K * (z - d_H * d_x_new_old); + arma::mat A = (arma::eye(6, 6) - K * d_H); + d_P_new_new = A * d_P_new_old * A.t() + K * d_R * K.t(); + + // prepare data for next KF epoch + d_x_old_old = d_x_new_new; + d_P_old_old = d_P_new_new; + } + catch (...) + { + d_x_new_new = d_x_new_old; + this->reset_Kf(); + } + } +} + + +void Pvt_Kf::get_pv_Kf(arma::vec& p, arma::vec& v) const +{ + if (d_initialized) + { + p = d_x_new_new.subvec(0, 2); + v = d_x_new_new.subvec(3, 5); + } +} diff --git a/src/algorithms/PVT/libs/pvt_kf.h b/src/algorithms/PVT/libs/pvt_kf.h old mode 100644 new mode 100755 index 4d15c45f9..e051922ee --- a/src/algorithms/PVT/libs/pvt_kf.h +++ b/src/algorithms/PVT/libs/pvt_kf.h @@ -1,69 +1,71 @@ -/*! - * \file pvt_kf.h - * \brief Kalman Filter for Position and Velocity - * \author Javier Arribas, 2023. jarribas(at)cttc.es - * - * - * ----------------------------------------------------------------------------- - * - * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. - * This file is part of GNSS-SDR. - * - * Copyright (C) 2010-2023 (see AUTHORS file for a list of contributors) - * SPDX-License-Identifier: GPL-3.0-or-later - * - * ----------------------------------------------------------------------------- - */ - -#ifndef GNSS_SDR_PVT_KF_H -#define GNSS_SDR_PVT_KF_H - -#include - -/** \addtogroup PVT - * \{ */ -/** \addtogroup PVT_libs - * \{ */ - - -/*! - * \brief Kalman Filter for Position and Velocity - * - */ -class Pvt_Kf -{ -public: - Pvt_Kf() = default; - virtual ~Pvt_Kf() = default; - void init_Kf(const arma::vec& p, - const arma::vec& v, - const arma::vec& res_p, - double solver_interval_s, - double measures_ecef_pos_sd_m, - double measures_ecef_vel_sd_ms, - double system_ecef_pos_sd_m, - double system_ecef_vel_sd_ms); - bool is_initialized() const; - void run_Kf(const arma::vec& p, const arma::vec& v, const arma::vec& res_p); - void get_pv_Kf(arma::vec& p, arma::vec& v) const; - void reset_Kf(); - -private: - // Kalman Filter class variables - arma::mat d_F; - arma::mat d_H; - arma::mat d_R; - arma::mat d_Q; - arma::mat d_P_old_old; - arma::mat d_P_new_old; - arma::mat d_P_new_new; - arma::vec d_x_old_old; - arma::vec d_x_new_old; - arma::vec d_x_new_new; - bool d_initialized{false}; -}; - - -/** \} */ -/** \} */ -#endif // GNSS_SDR_Pvt_Kf_H +/*! + * \file pvt_kf.h + * \brief Kalman Filter for Position and Velocity + * \author Javier Arribas, 2023. jarribas(at)cttc.es + * + * + * ----------------------------------------------------------------------------- + * + * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. + * This file is part of GNSS-SDR. + * + * Copyright (C) 2010-2023 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_PVT_KF_H +#define GNSS_SDR_PVT_KF_H + +#include + +/** \addtogroup PVT + * \{ */ +/** \addtogroup PVT_libs + * \{ */ + + +/*! + * \brief Kalman Filter for Position and Velocity + * + */ +class Pvt_Kf +{ +public: + Pvt_Kf() = default; + virtual ~Pvt_Kf() = default; + void init_Kf(const arma::vec& p, + const arma::vec& v, + const arma::vec& res_p, + double solver_interval_s, + bool estatic_measures_sd, + double measures_ecef_pos_sd_m, + double measures_ecef_vel_sd_ms, + double system_ecef_pos_sd_m, + double system_ecef_vel_sd_ms); + bool is_initialized() const; + void run_Kf(const arma::vec& p, const arma::vec& v, const arma::vec& res_p); + void get_pv_Kf(arma::vec& p, arma::vec& v) const; + void reset_Kf(); + +private: + // Kalman Filter class variables + arma::mat d_F; + arma::mat d_H; + arma::mat d_R; + arma::mat d_Q; + arma::mat d_P_old_old; + arma::mat d_P_new_old; + arma::mat d_P_new_new; + arma::vec d_x_old_old; + arma::vec d_x_new_old; + arma::vec d_x_new_new; + bool d_initialized{false}; + bool d_static{false}; +}; + + +/** \} */ +/** \} */ +#endif // GNSS_SDR_Pvt_Kf_H diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc old mode 100644 new mode 100755 index b85d5457c..1667eb632 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -1,1779 +1,1780 @@ -/*! - * \file rtklib_solver.cc - * \brief PVT solver based on rtklib library functions adapted to the GNSS-SDR - * data flow and structures - * \authors
    - *
  • 2017-2019, Javier Arribas - *
  • 2017-2023, Carles Fernandez - *
  • 2007-2013, T. Takasu - *
- * - * This is a derived work from RTKLIB http://www.rtklib.com/ - * The original source code at https://github.com/tomojitakasu/RTKLIB is - * released under the BSD 2-clause license with an additional exclusive clause - * that does not apply here. This additional clause is reproduced below: - * - * " The software package includes some companion executive binaries or shared - * libraries necessary to execute APs on Windows. These licenses succeed to the - * original ones of these software. " - * - * Neither the executive binaries nor the shared libraries are required by, used - * or included in GNSS-SDR. - * - * ----------------------------------------------------------------------------- - * Copyright (C) 2007-2013, T. Takasu - * Copyright (C) 2017-2019, Javier Arribas - * Copyright (C) 2017-2023, Carles Fernandez - * All rights reserved. - * - * SPDX-License-Identifier: BSD-2-Clause - * - * -----------------------------------------------------------------------*/ - -#include "rtklib_solver.h" -#include "Beidou_DNAV.h" -#include "gnss_sdr_filesystem.h" -#include "rtklib_rtkpos.h" -#include "rtklib_solution.h" -#include -#include -#include -#include -#include -#include -#include - - -Rtklib_Solver::Rtklib_Solver(const rtk_t &rtk, - const std::string &dump_filename, - uint32_t type_of_rx, - bool flag_dump_to_file, - bool flag_dump_to_mat, - Pvt_Conf conf) : d_dump_filename(dump_filename), - d_rtk(rtk), - d_conf(std::move(conf)), - d_type_of_rx(type_of_rx), - d_flag_dump_enabled(flag_dump_to_file), - d_flag_dump_mat_enabled(flag_dump_to_mat) -{ - // see freq index at src/algorithms/libs/rtklib/rtklib_rtkcmn.cc - // function: satwavelen - d_rtklib_freq_index[0] = 0; - d_rtklib_freq_index[1] = 1; - d_rtklib_freq_index[2] = 2; - - d_rtklib_band_index["1G"] = 0; - d_rtklib_band_index["1C"] = 0; - d_rtklib_band_index["1B"] = 0; - d_rtklib_band_index["B1"] = 0; - d_rtklib_band_index["B3"] = 2; - d_rtklib_band_index["2G"] = 1; - d_rtklib_band_index["2S"] = 1; - d_rtklib_band_index["7X"] = 2; - d_rtklib_band_index["5X"] = 2; - d_rtklib_band_index["L5"] = 2; - d_rtklib_band_index["E6"] = 0; - - switch (d_type_of_rx) - { - case 6: // E5b only - d_rtklib_freq_index[2] = 4; - break; - case 11: // GPS L1 C/A + Galileo E5b - d_rtklib_freq_index[2] = 4; - break; - case 15: // Galileo E1B + Galileo E5b - d_rtklib_freq_index[2] = 4; - break; - case 18: // GPS L2C + Galileo E5b - d_rtklib_freq_index[2] = 4; - break; - case 19: // Galileo E5a + Galileo E5b - d_rtklib_band_index["5X"] = 0; - d_rtklib_freq_index[0] = 2; - d_rtklib_freq_index[2] = 4; - break; - case 20: // GPS L5 + Galileo E5b - d_rtklib_band_index["L5"] = 0; - d_rtklib_freq_index[0] = 2; - d_rtklib_freq_index[2] = 4; - break; - case 100: // E6B only - d_rtklib_freq_index[0] = 3; - break; - case 101: // E1 + E6B - d_rtklib_band_index["E6"] = 1; - d_rtklib_freq_index[1] = 3; - break; - case 102: // E5a + E6B - d_rtklib_band_index["E6"] = 1; - d_rtklib_freq_index[1] = 3; - break; - case 103: // E5b + E6B - d_rtklib_band_index["E6"] = 1; - d_rtklib_freq_index[1] = 3; - d_rtklib_freq_index[2] = 4; - break; - case 104: // Galileo E1B + Galileo E5a + Galileo E6B - d_rtklib_band_index["E6"] = 1; - d_rtklib_freq_index[1] = 3; - break; - case 105: // Galileo E1B + Galileo E5b + Galileo E6B - d_rtklib_freq_index[2] = 4; - d_rtklib_band_index["E6"] = 1; - d_rtklib_freq_index[1] = 3; - break; - case 106: // GPS L1 C/A + Galileo E1B + Galileo E6B - case 107: // GPS L1 C/A + Galileo E6B - d_rtklib_band_index["E6"] = 1; - d_rtklib_freq_index[1] = 3; - break; - } - // auto empty_map = std::map < int, HAS_obs_corrections >> (); - // d_has_obs_corr_map["L1 C/A"] = empty_map; - - // ############# ENABLE DATA FILE LOG ################# - if (d_flag_dump_enabled == true) - { - if (d_dump_file.is_open() == false) - { - try - { - d_dump_file.exceptions(std::ofstream::failbit | std::ofstream::badbit); - d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); - LOG(INFO) << "PVT lib dump enabled Log file: " << d_dump_filename.c_str(); - } - catch (const std::ofstream::failure &e) - { - LOG(WARNING) << "Exception opening RTKLIB dump file " << e.what(); - } - } - } -} - - -Rtklib_Solver::~Rtklib_Solver() -{ - DLOG(INFO) << "Rtklib_Solver destructor called."; - if (d_dump_file.is_open() == true) - { - const auto pos = d_dump_file.tellp(); - try - { - d_dump_file.close(); - } - catch (const std::exception &ex) - { - LOG(WARNING) << "Exception in destructor closing the RTKLIB dump file " << ex.what(); - } - if (pos == 0) - { - errorlib::error_code ec; - if (!fs::remove(fs::path(d_dump_filename), ec)) - { - std::cerr << "Problem removing temporary file " << d_dump_filename << '\n'; - } - d_flag_dump_mat_enabled = false; - } - } - if (d_flag_dump_mat_enabled) - { - try - { - save_matfile(); - } - catch (const std::exception &ex) - { - LOG(WARNING) << "Exception in destructor saving the PVT .mat dump file " << ex.what(); - } - } -} - - -bool Rtklib_Solver::save_matfile() const -{ - // READ DUMP FILE - const std::string dump_filename = d_dump_filename; - const int32_t number_of_double_vars = 21; - const int32_t number_of_uint32_vars = 2; - const int32_t number_of_uint8_vars = 3; - const int32_t number_of_float_vars = 2; - const int32_t epoch_size_bytes = sizeof(double) * number_of_double_vars + - sizeof(uint32_t) * number_of_uint32_vars + - sizeof(uint8_t) * number_of_uint8_vars + - sizeof(float) * number_of_float_vars; - std::ifstream dump_file; - dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); - try - { - dump_file.open(dump_filename.c_str(), std::ios::binary | std::ios::ate); - } - catch (const std::ifstream::failure &e) - { - std::cerr << "Problem opening dump file:" << e.what() << '\n'; - return false; - } - // count number of epochs and rewind - int64_t num_epoch = 0LL; - if (dump_file.is_open()) - { - std::cout << "Generating .mat file for " << dump_filename << '\n'; - const std::ifstream::pos_type size = dump_file.tellg(); - num_epoch = static_cast(size) / static_cast(epoch_size_bytes); - dump_file.seekg(0, std::ios::beg); - } - else - { - return false; - } - - auto TOW_at_current_symbol_ms = std::vector(num_epoch); - auto week = std::vector(num_epoch); - auto RX_time = std::vector(num_epoch); - auto user_clk_offset = std::vector(num_epoch); - auto pos_x = std::vector(num_epoch); - auto pos_y = std::vector(num_epoch); - auto pos_z = std::vector(num_epoch); - auto vel_x = std::vector(num_epoch); - auto vel_y = std::vector(num_epoch); - auto vel_z = std::vector(num_epoch); - auto cov_xx = std::vector(num_epoch); - auto cov_yy = std::vector(num_epoch); - auto cov_zz = std::vector(num_epoch); - auto cov_xy = std::vector(num_epoch); - auto cov_yz = std::vector(num_epoch); - auto cov_zx = std::vector(num_epoch); - auto latitude = std::vector(num_epoch); - auto longitude = std::vector(num_epoch); - auto height = std::vector(num_epoch); - auto valid_sats = std::vector(num_epoch); - auto solution_status = std::vector(num_epoch); - auto solution_type = std::vector(num_epoch); - auto AR_ratio_factor = std::vector(num_epoch); - auto AR_ratio_threshold = std::vector(num_epoch); - auto gdop = std::vector(num_epoch); - auto pdop = std::vector(num_epoch); - auto hdop = std::vector(num_epoch); - auto vdop = std::vector(num_epoch); - - try - { - if (dump_file.is_open()) - { - for (int64_t i = 0; i < num_epoch; i++) - { - dump_file.read(reinterpret_cast(&TOW_at_current_symbol_ms[i]), sizeof(uint32_t)); - dump_file.read(reinterpret_cast(&week[i]), sizeof(uint32_t)); - dump_file.read(reinterpret_cast(&RX_time[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&user_clk_offset[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&pos_x[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&pos_y[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&pos_z[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&vel_x[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&vel_y[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&vel_z[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&cov_xx[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&cov_yy[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&cov_zz[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&cov_xy[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&cov_yz[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&cov_zx[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&latitude[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&longitude[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&height[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&valid_sats[i]), sizeof(uint8_t)); - dump_file.read(reinterpret_cast(&solution_status[i]), sizeof(uint8_t)); - dump_file.read(reinterpret_cast(&solution_type[i]), sizeof(uint8_t)); - dump_file.read(reinterpret_cast(&AR_ratio_factor[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&AR_ratio_threshold[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&gdop[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&pdop[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&hdop[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&vdop[i]), sizeof(double)); - } - } - dump_file.close(); - } - catch (const std::ifstream::failure &e) - { - std::cerr << "Problem reading dump file:" << e.what() << '\n'; - return false; - } - - // WRITE MAT FILE - mat_t *matfp; - matvar_t *matvar; - std::string filename = dump_filename; - filename.erase(filename.length() - 4, 4); - filename.append(".mat"); - matfp = Mat_CreateVer(filename.c_str(), nullptr, MAT_FT_MAT73); - if (reinterpret_cast(matfp) != nullptr) - { - std::array dims{1, static_cast(num_epoch)}; - matvar = Mat_VarCreate("TOW_at_current_symbol_ms", MAT_C_UINT32, MAT_T_UINT32, 2, dims.data(), TOW_at_current_symbol_ms.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("week", MAT_C_UINT32, MAT_T_UINT32, 2, dims.data(), week.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("RX_time", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), RX_time.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("user_clk_offset", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), user_clk_offset.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("pos_x", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), pos_x.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("pos_y", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), pos_y.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("pos_z", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), pos_z.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("vel_x", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), vel_x.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("vel_y", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), vel_y.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("vel_z", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), vel_z.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("cov_xx", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_xx.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("cov_yy", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_yy.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("cov_zz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_zz.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("cov_xy", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_xy.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("cov_yz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_yz.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("cov_zx", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_zx.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("latitude", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), latitude.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("longitude", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), longitude.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("height", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), height.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("valid_sats", MAT_C_UINT8, MAT_T_UINT8, 2, dims.data(), valid_sats.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("solution_status", MAT_C_UINT8, MAT_T_UINT8, 2, dims.data(), solution_status.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("solution_type", MAT_C_UINT8, MAT_T_UINT8, 2, dims.data(), solution_type.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("AR_ratio_factor", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims.data(), AR_ratio_factor.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("AR_ratio_threshold", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims.data(), AR_ratio_threshold.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("gdop", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), gdop.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("pdop", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), pdop.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("hdop", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), hdop.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("vdop", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), vdop.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - } - - Mat_Close(matfp); - return true; -} - - -double Rtklib_Solver::get_gdop() const -{ - return d_dop[0]; -} - - -double Rtklib_Solver::get_pdop() const -{ - return d_dop[1]; -} - - -double Rtklib_Solver::get_hdop() const -{ - return d_dop[2]; -} - - -double Rtklib_Solver::get_vdop() const -{ - return d_dop[3]; -} - - -Monitor_Pvt Rtklib_Solver::get_monitor_pvt() const -{ - return d_monitor_pvt; -} - - -void Rtklib_Solver::store_has_data(const Galileo_HAS_data &new_has_data) -{ - // Compute time of application HAS SIS ICD, Issue 1.0, Section 7.7 - uint16_t toh = new_has_data.header.toh; - uint32_t hr = std::floor(new_has_data.tow / 3600); - uint32_t tmt = 0; - if ((hr * 3600 + toh) <= new_has_data.tow) - { - tmt = hr * 3600 + toh; - } - else - { - tmt = (hr - 1) * 3600 + toh; - } - - const std::string gps_str("GPS"); - const std::string gal_str("Galileo"); - if (new_has_data.header.orbit_correction_flag) - { - LOG(INFO) << "Received HAS orbit corrections"; - // for each satellite in GPS ephemeris - for (const auto &gpseph : gps_ephemeris_map) - { - int prn = gpseph.second.PRN; - int32_t sis_iod = gpseph.second.IODE_SF3; - uint16_t gnss_iod = new_has_data.get_gnss_iod(gps_str, prn); - if (static_cast(gnss_iod) == sis_iod) - { - float radial_m = new_has_data.get_delta_radial_m(gps_str, prn); - if (std::fabs(radial_m + 10.24) < 0.001) // -10.24 means not available - { - radial_m = 0.0; - } - float in_track_m = new_has_data.get_delta_in_track_m(gps_str, prn); - if (std::fabs(in_track_m + 16.384) < 0.001) // -16.384 means not available - { - in_track_m = 0.0; - } - float cross_track_m = new_has_data.get_delta_in_track_m(gps_str, prn); - if (std::fabs(cross_track_m + 16.384) < 0.001) // -16.384 means not available - { - cross_track_m = 0.0; - } - d_has_orbit_corrections_store_map[gps_str][prn].radial_m = radial_m; - d_has_orbit_corrections_store_map[gps_str][prn].in_track_m = in_track_m; - d_has_orbit_corrections_store_map[gps_str][prn].cross_track_m = cross_track_m; - d_has_orbit_corrections_store_map[gps_str][prn].valid_until = tmt + - new_has_data.get_validity_interval_s(new_has_data.validity_interval_index_orbit_corrections); - d_has_orbit_corrections_store_map[gps_str][prn].iod = gnss_iod; - // TODO: check for end of week - } - } - - // for each satellite in Galileo ephemeris - for (const auto &galeph : galileo_ephemeris_map) - { - int prn = galeph.second.PRN; - int32_t sis_iod = galeph.second.IOD_ephemeris; - uint16_t gnss_iod = new_has_data.get_gnss_iod(gal_str, prn); - if (static_cast(gnss_iod) == sis_iod) - { - float radial_m = new_has_data.get_delta_radial_m(gal_str, prn); - if (std::fabs(radial_m + 10.24) < 0.001) // -10.24 means not available - { - radial_m = 0.0; - } - float in_track_m = new_has_data.get_delta_in_track_m(gal_str, prn); - if (std::fabs(in_track_m + 16.384) < 0.001) // -16.384 means not available - { - in_track_m = 0.0; - } - float cross_track_m = new_has_data.get_delta_in_track_m(gal_str, prn); - if (std::fabs(cross_track_m + 16.384) < 0.001) // -16.384 means not available - { - cross_track_m = 0.0; - } - d_has_orbit_corrections_store_map[gal_str][prn].radial_m = radial_m; - d_has_orbit_corrections_store_map[gal_str][prn].in_track_m = in_track_m; - d_has_orbit_corrections_store_map[gal_str][prn].cross_track_m = cross_track_m; - d_has_orbit_corrections_store_map[gal_str][prn].valid_until = tmt + - new_has_data.get_validity_interval_s(new_has_data.validity_interval_index_orbit_corrections); - d_has_orbit_corrections_store_map[gal_str][prn].iod = gnss_iod; - // TODO: check for end of week - } - } - } - if (new_has_data.header.clock_fullset_flag) - { - LOG(INFO) << "Received HAS clock fullset corrections"; - for (const auto &gpseph : gps_ephemeris_map) - { - int prn = gpseph.second.PRN; - int32_t sis_iod = gpseph.second.IODE_SF3; - auto it = d_has_orbit_corrections_store_map[gps_str].find(prn); - if (it != d_has_orbit_corrections_store_map[gps_str].end()) - { - uint16_t gnss_iod = it->second.iod; - if (static_cast(gnss_iod) == sis_iod) - { - float clock_correction_mult_m = new_has_data.get_clock_correction_mult_m(gps_str, prn); - if ((std::fabs(clock_correction_mult_m + 10.24) < 0.001) || - (std::fabs(clock_correction_mult_m + 20.48) < 0.001) || - (std::fabs(clock_correction_mult_m + 30.72) < 0.001) || - (std::fabs(clock_correction_mult_m + 40.96) < 0.001)) - { - clock_correction_mult_m = 0.0; - } - if ((std::fabs(clock_correction_mult_m - 10.2375) < 0.001) || - (std::fabs(clock_correction_mult_m - 20.475) < 0.001) || - (std::fabs(clock_correction_mult_m - 30.7125) < 0.001) || - (std::fabs(clock_correction_mult_m - 40.95) < 0.001)) - { - // Satellite should not be used! - clock_correction_mult_m = 0.0; - } - d_has_clock_corrections_store_map[gps_str][prn].clock_correction_m = clock_correction_mult_m; - d_has_clock_corrections_store_map[gps_str][prn].valid_until = tmt + - new_has_data.get_validity_interval_s(new_has_data.validity_interval_index_clock_fullset_corrections); - // TODO: check for end of week - } - } - } - - // for each satellite in Galileo ephemeris - for (const auto &galeph : galileo_ephemeris_map) - { - int prn = galeph.second.PRN; - int32_t iod_sis = galeph.second.IOD_ephemeris; - auto it = d_has_orbit_corrections_store_map[gal_str].find(prn); - if (it != d_has_orbit_corrections_store_map[gal_str].end()) - { - uint16_t gnss_iod = it->second.iod; - if (static_cast(gnss_iod) == iod_sis) - { - float clock_correction_mult_m = new_has_data.get_clock_correction_mult_m(gal_str, prn); - // std::cout << "Galileo Satellite " << prn - // << " clock correction=" << new_has_data.get_clock_correction_mult_m(gal_str, prn) - // << std::endl; - if ((std::fabs(clock_correction_mult_m + 10.24) < 0.001) || - (std::fabs(clock_correction_mult_m + 20.48) < 0.001) || - (std::fabs(clock_correction_mult_m + 30.72) < 0.001) || - (std::fabs(clock_correction_mult_m + 40.96) < 0.001)) - { - clock_correction_mult_m = 0.0; - } - d_has_clock_corrections_store_map[gal_str][prn].clock_correction_m = clock_correction_mult_m; - d_has_clock_corrections_store_map[gal_str][prn].valid_until = tmt + - new_has_data.get_validity_interval_s(new_has_data.validity_interval_index_clock_fullset_corrections); - // TODO: check for end of week - } - } - } - } - if (new_has_data.header.clock_subset_flag) - { - LOG(INFO) << "Received HAS clock subset corrections"; - for (const auto &gpseph : gps_ephemeris_map) - { - int prn = gpseph.second.PRN; - int32_t sis_iod = gpseph.second.IODE_SF3; - int32_t gnss_iod = d_has_orbit_corrections_store_map[gps_str][prn].iod; - if (gnss_iod == sis_iod) - { - // d_has_clock_corrections_store_map[gps_str][prn].clock_correction_m = new_has_data.get_clock_subset_correction_mult_m(gps_str, prn); - // d_has_clock_corrections_store_map[gps_str][prn].valid_until = tmt + new_has_data.get_validity_interval_s(new_has_data.validity_interval_index_clock_subset_corrections); - // TODO: check for end of week - } - } - } - if (new_has_data.header.code_bias_flag) - { - LOG(INFO) << "Received HAS code bias corrections"; - uint32_t valid_until = tmt + - new_has_data.get_validity_interval_s(new_has_data.validity_interval_index_code_bias_corrections); - auto signals_gal = new_has_data.get_signals_in_mask(gal_str); - for (const auto &it : signals_gal) - { - auto prns = new_has_data.get_PRNs_in_mask(gal_str); - for (auto prn : prns) - { - float code_bias_m = new_has_data.get_code_bias_m(it, prn); - if ((std::fabs(code_bias_m + 20.48) < 0.01)) // -20.48 means not available - { - code_bias_m = 0.0; - } - d_has_code_bias_store_map[it][prn] = {code_bias_m, valid_until}; - } - } - auto signals_gps = new_has_data.get_signals_in_mask(gps_str); - for (const auto &it : signals_gps) - { - auto prns = new_has_data.get_PRNs_in_mask(gps_str); - for (auto prn : prns) - { - float code_bias_m = new_has_data.get_code_bias_m(it, prn); - if ((std::fabs(code_bias_m + 20.48) < 0.01)) // -20.48 means not available - { - code_bias_m = 0.0; - } - d_has_code_bias_store_map[it][prn] = {code_bias_m, valid_until}; - } - } - } - if (new_has_data.header.phase_bias_flag) - { - LOG(INFO) << "Received HAS phase bias corrections"; - uint32_t valid_until = tmt + - new_has_data.get_validity_interval_s(new_has_data.validity_interval_index_phase_bias_corrections); - - auto signals_gal = new_has_data.get_signals_in_mask(gal_str); - for (const auto &it : signals_gal) - { - auto prns = new_has_data.get_PRNs_in_mask(gal_str); - for (auto prn : prns) - { - float phase_bias_correction_cycles = new_has_data.get_phase_bias_cycle(it, prn); - if (std::fabs(phase_bias_correction_cycles + 10.24) < 0.001) // -10.24 means not available - { - phase_bias_correction_cycles = 0.0; - } - d_has_phase_bias_store_map[it][prn] = {phase_bias_correction_cycles, valid_until}; - // TODO: process Phase Discontinuity Indicator - } - } - auto signals_gps = new_has_data.get_signals_in_mask(gps_str); - for (const auto &it : signals_gps) - { - auto prns = new_has_data.get_PRNs_in_mask(gps_str); - for (auto prn : prns) - { - float phase_bias_correction_cycles = new_has_data.get_phase_bias_cycle(it, prn); - if (std::fabs(phase_bias_correction_cycles + 10.24) < 0.001) // -10.24 means not available - { - phase_bias_correction_cycles = 0.0; - } - d_has_phase_bias_store_map[it][prn] = {phase_bias_correction_cycles, valid_until}; - // TODO: process Phase Discontinuity Indicator - } - } - } -} - - -void Rtklib_Solver::update_has_corrections(const std::map &obs_map) -{ - this->check_has_orbit_clock_validity(obs_map); - this->get_has_biases(obs_map); -} - - -void Rtklib_Solver::check_has_orbit_clock_validity(const std::map &obs_map) -{ - for (const auto &it : obs_map) - { - uint32_t obs_tow = it.second.interp_TOW_ms / 1000.0; - auto prn = static_cast(it.second.PRN); - - if (it.second.System == 'G') - { - auto it_sys = d_has_orbit_corrections_store_map.find("GPS"); - if (it_sys != d_has_orbit_corrections_store_map.end()) - { - auto it_map_corr = it_sys->second.find(prn); - if (it_map_corr != it_sys->second.end()) - { - auto has_data_valid_until = it_map_corr->second.valid_until; - if (has_data_valid_until < obs_tow) - { - // Delete outdated data - it_sys->second.erase(prn); - } - } - } - auto it_sys_clock = d_has_clock_corrections_store_map.find("GPS"); - if (it_sys_clock != d_has_clock_corrections_store_map.end()) - { - auto it_map_corr = it_sys_clock->second.find(prn); - if (it_map_corr != it_sys_clock->second.end()) - { - auto has_data_valid_until = it_map_corr->second.valid_until; - if (has_data_valid_until < obs_tow) - { - // Delete outdated data - it_sys_clock->second.erase(prn); - } - } - } - } - if (it.second.System == 'E') - { - auto it_sys = d_has_orbit_corrections_store_map.find("Galileo"); - if (it_sys != d_has_orbit_corrections_store_map.end()) - { - auto it_map_corr = it_sys->second.find(prn); - if (it_map_corr != it_sys->second.end()) - { - auto has_data_valid_until = it_map_corr->second.valid_until; - if (has_data_valid_until < obs_tow) - { - // Delete outdated data - it_sys->second.erase(prn); - } - } - } - auto it_sys_clock = d_has_clock_corrections_store_map.find("Galileo"); - if (it_sys_clock != d_has_clock_corrections_store_map.end()) - { - auto it_map_corr = it_sys_clock->second.find(prn); - if (it_map_corr != it_sys_clock->second.end()) - { - auto has_data_valid_until = it_map_corr->second.valid_until; - if (has_data_valid_until < obs_tow) - { - // Delete outdated data - it_sys_clock->second.erase(prn); - } - } - } - } - } -} - - -void Rtklib_Solver::get_has_biases(const std::map &obs_map) -{ - d_has_obs_corr_map.clear(); - if (!d_has_clock_corrections_store_map.empty() && !d_has_orbit_corrections_store_map.empty()) - { - const std::vector e1b_signals = {"E1-B I/NAV OS", "E1-C", "E1-B + E1-C"}; - const std::vector e6_signals = {"E6-B C/NAV HAS", "E6-C", "E6-B + E6-C"}; - const std::vector e5_signals = {"E5a-I F/NAV OS", "E5a-Q", "E5a-I+E5a-Q"}; - const std::vector e7_signals = {"E5bI I/NAV OS", "E5b-Q", "E5b-I+E5b-Q"}; - const std::vector g1c_signals = {"L1 C/A"}; - const std::vector g2s_signals = {"L2 CM", "L2 CL", "L2 CM+CL", "L2 P"}; - const std::vector g5_signals = {"L5 I", "L5 Q", "L5 I + L5 Q"}; - - for (const auto &it : obs_map) - { - uint32_t obs_tow = it.second.interp_TOW_ms / 1000.0; - int prn = static_cast(it.second.PRN); - std::string sig(it.second.Signal, 2); - if (it.second.System == 'E') - { - auto it_sys_clock = d_has_clock_corrections_store_map.find("Galileo"); - if (it_sys_clock != d_has_clock_corrections_store_map.end()) - { - auto it_map_corr = it_sys_clock->second.find(prn); - if (it_map_corr != it_sys_clock->second.end()) - { - if (sig == "1B") - { - for (const auto &has_signal : e1b_signals) - { - this->get_current_has_obs_correction(has_signal, obs_tow, prn); - } - } - else if (sig == "E6") - { - for (const auto &has_signal : e6_signals) - { - this->get_current_has_obs_correction(has_signal, obs_tow, prn); - } - } - else if (sig == "5X") - { - for (const auto &has_signal : e5_signals) - { - this->get_current_has_obs_correction(has_signal, obs_tow, prn); - } - } - else if (sig == "7X") - { - for (const auto &has_signal : e7_signals) - { - this->get_current_has_obs_correction(has_signal, obs_tow, prn); - } - } - } - } - } - if (it.second.System == 'G') - { - auto it_sys_clock = d_has_clock_corrections_store_map.find("GPS"); - if (it_sys_clock != d_has_clock_corrections_store_map.end()) - { - auto it_map_corr = it_sys_clock->second.find(prn); - if (it_map_corr != it_sys_clock->second.end()) - { - if (sig == "1C") - { - for (const auto &has_signal : g1c_signals) - { - this->get_current_has_obs_correction(has_signal, obs_tow, prn); - } - } - else if (sig == "2S") - { - for (const auto &has_signal : g2s_signals) - { - this->get_current_has_obs_correction(has_signal, obs_tow, prn); - } - } - else if (sig == "L5") - { - for (const auto &has_signal : g5_signals) - { - this->get_current_has_obs_correction(has_signal, obs_tow, prn); - } - } - } - } - } - } - } -} - - -void Rtklib_Solver::get_current_has_obs_correction(const std::string &signal, uint32_t tow_obs, int prn) -{ - auto code_bias_pair_it = this->d_has_code_bias_store_map[signal].find(prn); - if (code_bias_pair_it != this->d_has_code_bias_store_map[signal].end()) - { - uint32_t valid_until = code_bias_pair_it->second.second; - if (valid_until > tow_obs) - { - this->d_has_obs_corr_map[signal][prn].code_bias_m = code_bias_pair_it->second.first; - } - } - auto phase_bias_pair_it = this->d_has_phase_bias_store_map[signal].find(prn); - if (phase_bias_pair_it != this->d_has_phase_bias_store_map[signal].end()) - { - uint32_t valid_until = phase_bias_pair_it->second.second; - if (valid_until > tow_obs) - { - this->d_has_obs_corr_map[signal][prn].phase_bias_cycle = phase_bias_pair_it->second.first; - } - } -} - - -bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_map, double kf_update_interval_s) -{ - std::map::const_iterator gnss_observables_iter; - std::map::const_iterator galileo_ephemeris_iter; - std::map::const_iterator gps_ephemeris_iter; - std::map::const_iterator gps_cnav_ephemeris_iter; - std::map::const_iterator glonass_gnav_ephemeris_iter; - std::map::const_iterator beidou_ephemeris_iter; - - const Glonass_Gnav_Utc_Model &gnav_utc = this->glonass_gnav_utc_model; - - // ******************************************************************************** - // ****** PREPARE THE DATA (SV EPHEMERIS AND OBSERVATIONS) ************************ - // ******************************************************************************** - int valid_obs = 0; // valid observations counter - int glo_valid_obs = 0; // GLONASS L1/L2 valid observations counter - - d_obs_data.fill({}); - std::vector eph_data(MAXOBS); - std::vector geph_data(MAXOBS); - - // Workaround for NAV/CNAV clash problem - bool gps_dual_band = false; - bool band1 = false; - bool band2 = false; - - for (gnss_observables_iter = gnss_observables_map.cbegin(); - gnss_observables_iter != gnss_observables_map.cend(); - ++gnss_observables_iter) - { - switch (gnss_observables_iter->second.System) - { - case 'G': - { - const std::string sig_(gnss_observables_iter->second.Signal, 2); - if (sig_ == "1C") - { - band1 = true; - } - if (sig_ == "2S") - { - band2 = true; - } - } - break; - default: - { - } - } - } - if (band1 == true and band2 == true) - { - gps_dual_band = true; - } - - for (gnss_observables_iter = gnss_observables_map.cbegin(); - gnss_observables_iter != gnss_observables_map.cend(); - ++gnss_observables_iter) // CHECK INCONSISTENCY when combining GLONASS + other system - { - switch (gnss_observables_iter->second.System) - { - case 'E': - { - const std::string gal_str("Galileo"); - const std::string sig_(gnss_observables_iter->second.Signal, 2); - // Galileo E1 - if (sig_ == "1B") - { - // 1 Gal - find the ephemeris for the current GALILEO SV observation. The SV PRN ID is the map key - galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (galileo_ephemeris_iter != galileo_ephemeris_map.cend()) - { - // convert ephemeris from GNSS-SDR class to RTKLIB structure - eph_data[valid_obs] = eph_to_rtklib(galileo_ephemeris_iter->second, - this->d_has_orbit_corrections_store_map[gal_str], - this->d_has_clock_corrections_store_map[gal_str]); - // convert observation from GNSS-SDR class to RTKLIB structure - obsd_t newobs{}; - d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, - gnss_observables_iter->second, - d_has_obs_corr_map, - galileo_ephemeris_iter->second.WN, - d_rtklib_band_index[sig_]); - valid_obs++; - } - else // the ephemeris are not available for this SV - { - DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; - } - } - - // Galileo E5 - if ((sig_ == "5X") || (sig_ == "7X")) - { - // 1 Gal - find the ephemeris for the current GALILEO SV observation. The SV PRN ID is the map key - galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (galileo_ephemeris_iter != galileo_ephemeris_map.cend()) - { - bool found_E1_obs = false; - for (int i = 0; i < valid_obs; i++) - { - if (eph_data[i].sat == (static_cast(gnss_observables_iter->second.PRN + NSATGPS + NSATGLO))) - { - d_obs_data[i + glo_valid_obs] = insert_obs_to_rtklib(d_obs_data[i + glo_valid_obs], - gnss_observables_iter->second, - d_has_obs_corr_map, - galileo_ephemeris_iter->second.WN, - d_rtklib_band_index[sig_]); - found_E1_obs = true; - break; - } - } - if (!found_E1_obs) - { - // insert Galileo E5 obs as new obs and also insert its ephemeris - // convert ephemeris from GNSS-SDR class to RTKLIB structure - eph_data[valid_obs] = eph_to_rtklib(galileo_ephemeris_iter->second, - this->d_has_orbit_corrections_store_map[gal_str], - this->d_has_clock_corrections_store_map[gal_str]); - // convert observation from GNSS-SDR class to RTKLIB structure - const auto default_code_ = static_cast(CODE_NONE); - obsd_t newobs = {{0, 0}, '0', '0', {}, {}, - {default_code_, default_code_, default_code_}, - {}, {0.0, 0.0, 0.0}, {}}; - d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, - gnss_observables_iter->second, - d_has_obs_corr_map, - galileo_ephemeris_iter->second.WN, - d_rtklib_band_index[sig_]); - valid_obs++; - } - } - else // the ephemeris are not available for this SV - { - DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; - } - } - if (sig_ == "E6" && d_conf.use_e6_for_pvt) - { - galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (galileo_ephemeris_iter != galileo_ephemeris_map.cend()) - { - bool found_E1_obs = false; - for (int i = 0; i < valid_obs; i++) - { - if (eph_data[i].sat == (static_cast(gnss_observables_iter->second.PRN + NSATGPS + NSATGLO))) - { - d_obs_data[i + glo_valid_obs] = insert_obs_to_rtklib(d_obs_data[i + glo_valid_obs], - gnss_observables_iter->second, - d_has_obs_corr_map, - galileo_ephemeris_iter->second.WN, - d_rtklib_band_index[sig_]); - found_E1_obs = true; - break; - } - } - if (!found_E1_obs) - { - // insert Galileo E6 obs as new obs and also insert its ephemeris - // convert ephemeris from GNSS-SDR class to RTKLIB structure - eph_data[valid_obs] = eph_to_rtklib(galileo_ephemeris_iter->second, - this->d_has_orbit_corrections_store_map[gal_str], - this->d_has_clock_corrections_store_map[gal_str]); - // convert observation from GNSS-SDR class to RTKLIB structure - const auto default_code_ = static_cast(CODE_NONE); - obsd_t newobs = {{0, 0}, '0', '0', {}, {}, - {default_code_, default_code_, default_code_}, - {}, {0.0, 0.0, 0.0}, {}}; - d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, - gnss_observables_iter->second, - d_has_obs_corr_map, - galileo_ephemeris_iter->second.WN, - d_rtklib_band_index[sig_]); - valid_obs++; - } - } - else // the ephemeris are not available for this SV - { - DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; - } - } - if (sig_ == "E6") - { - galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (galileo_ephemeris_iter != galileo_ephemeris_map.cend()) - { - bool found_E1_obs = false; - for (int i = 0; i < valid_obs; i++) - { - if (eph_data[i].sat == (static_cast(gnss_observables_iter->second.PRN + NSATGPS + NSATGLO))) - { - d_obs_data[i + glo_valid_obs] = insert_obs_to_rtklib(d_obs_data[i + glo_valid_obs], - gnss_observables_iter->second, - galileo_ephemeris_iter->second.WN, - 2); // Band E6 - found_E1_obs = true; - break; - } - } - if (!found_E1_obs) - { - // insert Galileo E6 obs as new obs and also insert its ephemeris - // convert ephemeris from GNSS-SDR class to RTKLIB structure - eph_data[valid_obs] = eph_to_rtklib(galileo_ephemeris_iter->second); - // convert observation from GNSS-SDR class to RTKLIB structure - const auto default_code_ = static_cast(CODE_NONE); - obsd_t newobs = {{0, 0}, '0', '0', {}, {}, - {default_code_, default_code_, default_code_}, - {}, {0.0, 0.0, 0.0}, {}}; - d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, - gnss_observables_iter->second, - galileo_ephemeris_iter->second.WN, - 2); // Band E6 - // std::cout << "Week " << galileo_ephemeris_iter->second.WN << '\n'; - valid_obs++; - } - } - else // the ephemeris are not available for this SV - { - DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; - } - } - break; - } - case 'G': - { - // GPS L1 - // 1 GPS - find the ephemeris for the current GPS SV observation. The SV PRN ID is the map key - const std::string gps_str("GPS"); - const std::string sig_(gnss_observables_iter->second.Signal, 2); - if (sig_ == "1C") - { - gps_ephemeris_iter = gps_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (gps_ephemeris_iter != gps_ephemeris_map.cend()) - { - // convert ephemeris from GNSS-SDR class to RTKLIB structure - eph_data[valid_obs] = eph_to_rtklib(gps_ephemeris_iter->second, - this->d_has_orbit_corrections_store_map[gps_str], - this->d_has_clock_corrections_store_map[gps_str], - this->is_pre_2009()); - // convert observation from GNSS-SDR class to RTKLIB structure - obsd_t newobs{}; - d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, - gnss_observables_iter->second, - d_has_obs_corr_map, - gps_ephemeris_iter->second.WN, - d_rtklib_band_index[sig_], - this->is_pre_2009()); - valid_obs++; - } - else // the ephemeris are not available for this SV - { - DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->first; - } - } - // GPS L2 (todo: solve NAV/CNAV clash) - if ((sig_ == "2S") and (gps_dual_band == false)) - { - gps_cnav_ephemeris_iter = gps_cnav_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (gps_cnav_ephemeris_iter != gps_cnav_ephemeris_map.cend()) - { - // 1. Find the same satellite in GPS L1 band - gps_ephemeris_iter = gps_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (gps_ephemeris_iter != gps_ephemeris_map.cend()) - { - /* By the moment, GPS L2 observables are not used in pseudorange computations if GPS L1 is available - // 2. If found, replace the existing GPS L1 ephemeris with the GPS L2 ephemeris - // (more precise!), and attach the L2 observation to the L1 observation in RTKLIB structure - for (int i = 0; i < valid_obs; i++) - { - if (eph_data[i].sat == static_cast(gnss_observables_iter->second.PRN)) - { - eph_data[i] = eph_to_rtklib(gps_cnav_ephemeris_iter->second); - d_obs_data[i + glo_valid_obs] = insert_obs_to_rtklib(d_obs_data[i + glo_valid_obs], - gnss_observables_iter->second, - eph_data[i].week, - d_rtklib_band_index[sig_]); - break; - } - } - */ - } - else - { - // 3. If not found, insert the GPS L2 ephemeris and the observation - // convert ephemeris from GNSS-SDR class to RTKLIB structure - eph_data[valid_obs] = eph_to_rtklib(gps_cnav_ephemeris_iter->second); - // convert observation from GNSS-SDR class to RTKLIB structure - const auto default_code_ = static_cast(CODE_NONE); - obsd_t newobs = {{0, 0}, '0', '0', {}, {}, - {default_code_, default_code_, default_code_}, - {}, {0.0, 0.0, 0.0}, {}}; - d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, - gnss_observables_iter->second, - gps_cnav_ephemeris_iter->second.WN, - d_rtklib_band_index[sig_]); - valid_obs++; - } - } - else // the ephemeris are not available for this SV - { - DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; - } - } - // GPS L5 - if (sig_ == "L5") - { - gps_cnav_ephemeris_iter = gps_cnav_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (gps_cnav_ephemeris_iter != gps_cnav_ephemeris_map.cend()) - { - // 1. Find the same satellite in GPS L1 band - gps_ephemeris_iter = gps_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (gps_ephemeris_iter != gps_ephemeris_map.cend()) - { - // 2. If found, replace the existing GPS L1 ephemeris with the GPS L5 ephemeris - // (more precise!), and attach the L5 observation to the L1 observation in RTKLIB structure - for (int i = 0; i < valid_obs; i++) - { - if (eph_data[i].sat == static_cast(gnss_observables_iter->second.PRN)) - { - eph_data[i] = eph_to_rtklib(gps_cnav_ephemeris_iter->second); - d_obs_data[i + glo_valid_obs] = insert_obs_to_rtklib(d_obs_data[i], - gnss_observables_iter->second, - gps_cnav_ephemeris_iter->second.WN, - d_rtklib_band_index[sig_]); - break; - } - } - } - else - { - // 3. If not found, insert the GPS L5 ephemeris and the observation - // convert ephemeris from GNSS-SDR class to RTKLIB structure - eph_data[valid_obs] = eph_to_rtklib(gps_cnav_ephemeris_iter->second); - // convert observation from GNSS-SDR class to RTKLIB structure - const auto default_code_ = static_cast(CODE_NONE); - obsd_t newobs = {{0, 0}, '0', '0', {}, {}, - {default_code_, default_code_, default_code_}, - {}, {0.0, 0.0, 0.0}, {}}; - d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, - gnss_observables_iter->second, - d_has_obs_corr_map, - gps_cnav_ephemeris_iter->second.WN, - d_rtklib_band_index[sig_]); - valid_obs++; - } - } - else // the ephemeris are not available for this SV - { - DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; - } - } - break; - } - case 'R': // TODO This should be using rtk lib nomenclature - { - const std::string sig_(gnss_observables_iter->second.Signal); - // GLONASS GNAV L1 - if (sig_ == "1G") - { - // 1 Glo - find the ephemeris for the current GLONASS SV observation. The SV Slot Number (PRN ID) is the map key - glonass_gnav_ephemeris_iter = glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (glonass_gnav_ephemeris_iter != glonass_gnav_ephemeris_map.cend()) - { - // convert ephemeris from GNSS-SDR class to RTKLIB structure - geph_data[glo_valid_obs] = eph_to_rtklib(glonass_gnav_ephemeris_iter->second, gnav_utc); - // convert observation from GNSS-SDR class to RTKLIB structure - obsd_t newobs{}; - d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, - gnss_observables_iter->second, - glonass_gnav_ephemeris_iter->second.d_WN, - d_rtklib_band_index[sig_]); - glo_valid_obs++; - } - else // the ephemeris are not available for this SV - { - DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; - } - } - // GLONASS GNAV L2 - if (sig_ == "2G") - { - // 1 GLONASS - find the ephemeris for the current GLONASS SV observation. The SV PRN ID is the map key - glonass_gnav_ephemeris_iter = glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (glonass_gnav_ephemeris_iter != glonass_gnav_ephemeris_map.cend()) - { - bool found_L1_obs = false; - for (int i = 0; i < glo_valid_obs; i++) - { - if (geph_data[i].sat == (static_cast(gnss_observables_iter->second.PRN + NSATGPS))) - { - d_obs_data[i + valid_obs] = insert_obs_to_rtklib(d_obs_data[i + valid_obs], - gnss_observables_iter->second, - glonass_gnav_ephemeris_iter->second.d_WN, - d_rtklib_band_index[sig_]); - found_L1_obs = true; - break; - } - } - if (!found_L1_obs) - { - // insert GLONASS GNAV L2 obs as new obs and also insert its ephemeris - // convert ephemeris from GNSS-SDR class to RTKLIB structure - geph_data[glo_valid_obs] = eph_to_rtklib(glonass_gnav_ephemeris_iter->second, gnav_utc); - // convert observation from GNSS-SDR class to RTKLIB structure - obsd_t newobs{}; - d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, - gnss_observables_iter->second, - glonass_gnav_ephemeris_iter->second.d_WN, - d_rtklib_band_index[sig_]); - glo_valid_obs++; - } - } - else // the ephemeris are not available for this SV - { - DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; - } - } - break; - } - case 'C': - { - // BEIDOU B1I - // - find the ephemeris for the current BEIDOU SV observation. The SV PRN ID is the map key - const std::string sig_(gnss_observables_iter->second.Signal); - if (sig_ == "B1") - { - beidou_ephemeris_iter = beidou_dnav_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (beidou_ephemeris_iter != beidou_dnav_ephemeris_map.cend()) - { - // convert ephemeris from GNSS-SDR class to RTKLIB structure - eph_data[valid_obs] = eph_to_rtklib(beidou_ephemeris_iter->second); - // convert observation from GNSS-SDR class to RTKLIB structure - obsd_t newobs{}; - d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, - gnss_observables_iter->second, - beidou_ephemeris_iter->second.WN + BEIDOU_DNAV_BDT2GPST_WEEK_NUM_OFFSET, - d_rtklib_band_index[sig_]); - valid_obs++; - } - else // the ephemeris are not available for this SV - { - DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->first; - } - } - // BeiDou B3 - if (sig_ == "B3") - { - beidou_ephemeris_iter = beidou_dnav_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (beidou_ephemeris_iter != beidou_dnav_ephemeris_map.cend()) - { - bool found_B1I_obs = false; - for (int i = 0; i < valid_obs; i++) - { - if (eph_data[i].sat == (static_cast(gnss_observables_iter->second.PRN + NSATGPS + NSATGLO + NSATGAL + NSATQZS))) - { - d_obs_data[i + glo_valid_obs] = insert_obs_to_rtklib(d_obs_data[i + glo_valid_obs], - gnss_observables_iter->second, - beidou_ephemeris_iter->second.WN + BEIDOU_DNAV_BDT2GPST_WEEK_NUM_OFFSET, - d_rtklib_band_index[sig_]); - found_B1I_obs = true; - break; - } - } - if (!found_B1I_obs) - { - // insert BeiDou B3I obs as new obs and also insert its ephemeris - // convert ephemeris from GNSS-SDR class to RTKLIB structure - eph_data[valid_obs] = eph_to_rtklib(beidou_ephemeris_iter->second); - // convert observation from GNSS-SDR class to RTKLIB structure - const auto default_code_ = static_cast(CODE_NONE); - obsd_t newobs = {{0, 0}, '0', '0', {}, {}, - {default_code_, default_code_, default_code_}, - {}, {0.0, 0.0, 0.0}, {}}; - d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, - gnss_observables_iter->second, - beidou_ephemeris_iter->second.WN + BEIDOU_DNAV_BDT2GPST_WEEK_NUM_OFFSET, - d_rtklib_band_index[sig_]); - valid_obs++; - } - } - else // the ephemeris are not available for this SV - { - DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; - } - } - break; - } - - default: - DLOG(INFO) << "Hybrid observables: Unknown GNSS"; - break; - } - } - - // ********************************************************************** - // ****** SOLVE PVT****************************************************** - // ********************************************************************** - - this->set_valid_position(false); - if ((valid_obs + glo_valid_obs) > 3) - { - int result = 0; - d_nav_data = {}; - d_nav_data.eph = eph_data.data(); - d_nav_data.geph = geph_data.data(); - d_nav_data.n = valid_obs; - d_nav_data.ng = glo_valid_obs; - if (gps_iono.valid) - { - d_nav_data.ion_gps[0] = gps_iono.alpha0; - d_nav_data.ion_gps[1] = gps_iono.alpha1; - d_nav_data.ion_gps[2] = gps_iono.alpha2; - d_nav_data.ion_gps[3] = gps_iono.alpha3; - d_nav_data.ion_gps[4] = gps_iono.beta0; - d_nav_data.ion_gps[5] = gps_iono.beta1; - d_nav_data.ion_gps[6] = gps_iono.beta2; - d_nav_data.ion_gps[7] = gps_iono.beta3; - } - if (!(gps_iono.valid) and gps_cnav_iono.valid) - { - d_nav_data.ion_gps[0] = gps_cnav_iono.alpha0; - d_nav_data.ion_gps[1] = gps_cnav_iono.alpha1; - d_nav_data.ion_gps[2] = gps_cnav_iono.alpha2; - d_nav_data.ion_gps[3] = gps_cnav_iono.alpha3; - d_nav_data.ion_gps[4] = gps_cnav_iono.beta0; - d_nav_data.ion_gps[5] = gps_cnav_iono.beta1; - d_nav_data.ion_gps[6] = gps_cnav_iono.beta2; - d_nav_data.ion_gps[7] = gps_cnav_iono.beta3; - } - if (galileo_iono.ai0 != 0.0) - { - d_nav_data.ion_gal[0] = galileo_iono.ai0; - d_nav_data.ion_gal[1] = galileo_iono.ai1; - d_nav_data.ion_gal[2] = galileo_iono.ai2; - d_nav_data.ion_gal[3] = 0.0; - } - if (beidou_dnav_iono.valid) - { - d_nav_data.ion_cmp[0] = beidou_dnav_iono.alpha0; - d_nav_data.ion_cmp[1] = beidou_dnav_iono.alpha1; - d_nav_data.ion_cmp[2] = beidou_dnav_iono.alpha2; - d_nav_data.ion_cmp[3] = beidou_dnav_iono.alpha3; - d_nav_data.ion_cmp[4] = beidou_dnav_iono.beta0; - d_nav_data.ion_cmp[5] = beidou_dnav_iono.beta0; - d_nav_data.ion_cmp[6] = beidou_dnav_iono.beta0; - d_nav_data.ion_cmp[7] = beidou_dnav_iono.beta3; - } - if (gps_utc_model.valid) - { - d_nav_data.utc_gps[0] = gps_utc_model.A0; - d_nav_data.utc_gps[1] = gps_utc_model.A1; - d_nav_data.utc_gps[2] = gps_utc_model.tot; - d_nav_data.utc_gps[3] = gps_utc_model.WN_T; - d_nav_data.leaps = gps_utc_model.DeltaT_LS; - } - if (!(gps_utc_model.valid) and gps_cnav_utc_model.valid) - { - d_nav_data.utc_gps[0] = gps_cnav_utc_model.A0; - d_nav_data.utc_gps[1] = gps_cnav_utc_model.A1; - d_nav_data.utc_gps[2] = gps_cnav_utc_model.tot; - d_nav_data.utc_gps[3] = gps_cnav_utc_model.WN_T; - d_nav_data.leaps = gps_cnav_utc_model.DeltaT_LS; - } - if (glonass_gnav_utc_model.valid) - { - d_nav_data.utc_glo[0] = glonass_gnav_utc_model.d_tau_c; // ?? - d_nav_data.utc_glo[1] = 0.0; // ?? - d_nav_data.utc_glo[2] = 0.0; // ?? - d_nav_data.utc_glo[3] = 0.0; // ?? - } - if (galileo_utc_model.A0 != 0.0) - { - d_nav_data.utc_gal[0] = galileo_utc_model.A0; - d_nav_data.utc_gal[1] = galileo_utc_model.A1; - d_nav_data.utc_gal[2] = galileo_utc_model.tot; - d_nav_data.utc_gal[3] = galileo_utc_model.WNot; - d_nav_data.leaps = galileo_utc_model.Delta_tLS; - } - if (beidou_dnav_utc_model.valid) - { - d_nav_data.utc_cmp[0] = beidou_dnav_utc_model.A0_UTC; - d_nav_data.utc_cmp[1] = beidou_dnav_utc_model.A1_UTC; - d_nav_data.utc_cmp[2] = 0.0; // ?? - d_nav_data.utc_cmp[3] = 0.0; // ?? - d_nav_data.leaps = beidou_dnav_utc_model.DeltaT_LS; - } - - /* update carrier wave length using native function call in RTKlib */ - for (int i = 0; i < MAXSAT; i++) - { - for (int j = 0; j < NFREQ; j++) - { - d_nav_data.lam[i][j] = satwavelen(i + 1, d_rtklib_freq_index[j], &d_nav_data); - } - } - - result = rtkpos(&d_rtk, d_obs_data.data(), valid_obs + glo_valid_obs, &d_nav_data); - - if (result == 0) - { - LOG(INFO) << "RTKLIB rtkpos error: " << d_rtk.errbuf; - d_rtk.neb = 0; // clear error buffer to avoid repeating the error message - this->set_time_offset_s(0.0); // reset rx time estimation - this->set_num_valid_observations(0); - if (d_conf.enable_pvt_kf == true) - { - d_pvt_kf.reset_Kf(); - } - } - else - { - this->set_num_valid_observations(d_rtk.sol.ns); // record the number of valid satellites used by the PVT solver - pvt_sol = d_rtk.sol; - // DOP computation - unsigned int used_sats = 0; - for (unsigned int i = 0; i < MAXSAT; i++) - { - pvt_ssat[i] = d_rtk.ssat[i]; - if (d_rtk.ssat[i].vs == 1) - { - used_sats++; - } - } - - std::vector azel(used_sats * 2); - int index_aux = 0; - for (auto &i : d_rtk.ssat) - { - if (i.vs == 1) - { - azel[2 * index_aux] = i.azel[0]; - azel[2 * index_aux + 1] = i.azel[1]; - index_aux++; - } - } - - if (index_aux > 0) - { - dops(index_aux, azel.data(), 0.0, d_dop.data()); - } - this->set_valid_position(true); - std::array rx_position_and_time{}; - - if (d_conf.enable_pvt_kf == true) - { - if (d_pvt_kf.is_initialized() == false) - { - arma::vec p = {pvt_sol.rr[0], pvt_sol.rr[1], pvt_sol.rr[2]}; - arma::vec v = {pvt_sol.rr[3], pvt_sol.rr[4], pvt_sol.rr[5]}; - arma::vec res_p = {pvt_sol.qr[0], pvt_sol.qr[1], pvt_sol.qr[2], - pvt_sol.qr[3], pvt_sol.qr[4], pvt_sol.qr[5]}; - - d_pvt_kf.init_Kf(p, - v, - res_p, - kf_update_interval_s, - d_conf.measures_ecef_pos_sd_m, - d_conf.measures_ecef_vel_sd_ms, - d_conf.system_ecef_pos_sd_m, - d_conf.system_ecef_vel_sd_ms); - } - else - { - arma::vec p = {pvt_sol.rr[0], pvt_sol.rr[1], pvt_sol.rr[2]}; - arma::vec v = {pvt_sol.rr[3], pvt_sol.rr[4], pvt_sol.rr[5]}; - arma::vec res_p = {pvt_sol.qr[0], pvt_sol.qr[1], pvt_sol.qr[2], - pvt_sol.qr[3], pvt_sol.qr[4], pvt_sol.qr[5]}; - - d_pvt_kf.run_Kf(p, v, res_p); - d_pvt_kf.get_pv_Kf(p, v); - pvt_sol.rr[0] = p[0]; // [m] - pvt_sol.rr[1] = p[1]; // [m] - pvt_sol.rr[2] = p[2]; // [m] - pvt_sol.rr[3] = v[0]; // [ms] - pvt_sol.rr[4] = v[1]; // [ms] - pvt_sol.rr[5] = v[2]; // [ms] - } - } - - rx_position_and_time[0] = pvt_sol.rr[0]; // [m] - rx_position_and_time[1] = pvt_sol.rr[1]; // [m] - rx_position_and_time[2] = pvt_sol.rr[2]; // [m] - - // todo: fix this ambiguity in the RTKLIB units in receiver clock offset! - if (d_rtk.opt.mode == PMODE_SINGLE) - { - // if the RTKLIB solver is set to SINGLE, the dtr is already expressed in [s] - // add also the clock offset from gps to galileo (pvt_sol.dtr[2]) - rx_position_and_time[3] = pvt_sol.dtr[0] + pvt_sol.dtr[2]; - } - else - { - // the receiver clock offset is expressed in [meters], so we convert it into [s] - // add also the clock offset from gps to galileo (pvt_sol.dtr[2]) - rx_position_and_time[3] = pvt_sol.dtr[2] + pvt_sol.dtr[0] / SPEED_OF_LIGHT_M_S; - } - this->set_rx_pos({rx_position_and_time[0], rx_position_and_time[1], rx_position_and_time[2]}); // save ECEF position for the next iteration - - // compute Ground speed and COG - double ground_speed_ms = 0.0; - std::array pos{}; - std::array enuv{}; - ecef2pos(pvt_sol.rr, pos.data()); - ecef2enu(pos.data(), &pvt_sol.rr[3], enuv.data()); - this->set_speed_over_ground(norm_rtk(enuv.data(), 2)); - double new_cog; - if (ground_speed_ms >= 1.0) - { - new_cog = atan2(enuv[0], enuv[1]) * R2D; - if (new_cog < 0.0) - { - new_cog += 360.0; - } - this->set_course_over_ground(new_cog); - } - - this->set_time_offset_s(rx_position_and_time[3]); - - DLOG(INFO) << "RTKLIB Position at RX TOW = " << gnss_observables_map.cbegin()->second.RX_time - << " in ECEF (X,Y,Z,t[meters]) = " << rx_position_and_time[0] << ", " << rx_position_and_time[1] << ", " << rx_position_and_time[2] << ", " << rx_position_and_time[3]; - - // gtime_t rtklib_utc_time = gpst2utc(pvt_sol.time); // Corrected RX Time (Non integer multiply of 1 ms of granularity) - // Uncorrected RX Time (integer multiply of 1 ms and the same observables time reported in RTCM and RINEX) - const gtime_t rtklib_time = timeadd(pvt_sol.time, rx_position_and_time[3]); // uncorrected rx time - const gtime_t rtklib_utc_time = gpst2utc(rtklib_time); - boost::posix_time::ptime p_time = boost::posix_time::from_time_t(rtklib_utc_time.time); - p_time += boost::posix_time::microseconds(static_cast(round(rtklib_utc_time.sec * 1e6))); // NOLINT(google-runtime-int) - - this->set_position_UTC_time(p_time); - - DLOG(INFO) << "RTKLIB Position at " << boost::posix_time::to_simple_string(p_time) - << " is Lat = " << this->get_latitude() << " [deg], Long = " << this->get_longitude() - << " [deg], Height= " << this->get_height() << " [m]" - << " RX time offset= " << this->get_time_offset_s() << " [s]"; - - // ######## PVT MONITOR ######### - // TOW - d_monitor_pvt.TOW_at_current_symbol_ms = gnss_observables_map.cbegin()->second.TOW_at_current_symbol_ms; - // WEEK - d_monitor_pvt.week = adjgpsweek(d_nav_data.eph[0].week, this->is_pre_2009()); - // PVT GPS time - d_monitor_pvt.RX_time = gnss_observables_map.cbegin()->second.RX_time; - // User clock offset [s] - d_monitor_pvt.user_clk_offset = rx_position_and_time[3]; - - // ECEF POS X,Y,X [m] + ECEF VEL X,Y,X [m/s] (6 x double) - d_monitor_pvt.pos_x = pvt_sol.rr[0]; - d_monitor_pvt.pos_y = pvt_sol.rr[1]; - d_monitor_pvt.pos_z = pvt_sol.rr[2]; - d_monitor_pvt.vel_x = pvt_sol.rr[3]; - d_monitor_pvt.vel_y = pvt_sol.rr[4]; - d_monitor_pvt.vel_z = pvt_sol.rr[5]; - - // position variance/covariance (m^2) {c_xx,c_yy,c_zz,c_xy,c_yz,c_zx} (6 x double) - d_monitor_pvt.cov_xx = pvt_sol.qr[0]; - d_monitor_pvt.cov_yy = pvt_sol.qr[1]; - d_monitor_pvt.cov_zz = pvt_sol.qr[2]; - d_monitor_pvt.cov_xy = pvt_sol.qr[3]; - d_monitor_pvt.cov_yz = pvt_sol.qr[4]; - d_monitor_pvt.cov_zx = pvt_sol.qr[5]; - - // GEO user position Latitude [deg] - d_monitor_pvt.latitude = this->get_latitude(); - // GEO user position Longitude [deg] - d_monitor_pvt.longitude = this->get_longitude(); - // GEO user position Height [m] - d_monitor_pvt.height = this->get_height(); - - // NUMBER OF VALID SATS - d_monitor_pvt.valid_sats = pvt_sol.ns; - // RTKLIB solution status - d_monitor_pvt.solution_status = pvt_sol.stat; - // RTKLIB solution type (0:xyz-ecef,1:enu-baseline) - d_monitor_pvt.solution_type = pvt_sol.type; - // AR ratio factor for validation - d_monitor_pvt.AR_ratio_factor = pvt_sol.ratio; - // AR ratio threshold for validation - d_monitor_pvt.AR_ratio_threshold = pvt_sol.thres; - - // GDOP / PDOP/ HDOP/ VDOP - d_monitor_pvt.gdop = d_dop[0]; - d_monitor_pvt.pdop = d_dop[1]; - d_monitor_pvt.hdop = d_dop[2]; - d_monitor_pvt.vdop = d_dop[3]; - - this->set_rx_vel({enuv[0], enuv[1], enuv[2]}); - - const double clock_drift_ppm = pvt_sol.dtr[5] / SPEED_OF_LIGHT_M_S * 1e6; - - this->set_clock_drift_ppm(clock_drift_ppm); - // User clock drift [ppm] - d_monitor_pvt.user_clk_drift_ppm = clock_drift_ppm; - - // ######## LOG FILE ######### - if (d_flag_dump_enabled == true) - { - // MULTIPLEXED FILE RECORDING - Record results to file - try - { - double tmp_double; - uint32_t tmp_uint32; - // TOW - tmp_uint32 = gnss_observables_map.cbegin()->second.TOW_at_current_symbol_ms; - d_dump_file.write(reinterpret_cast(&tmp_uint32), sizeof(uint32_t)); - // WEEK - tmp_uint32 = adjgpsweek(d_nav_data.eph[0].week, this->is_pre_2009()); - d_dump_file.write(reinterpret_cast(&tmp_uint32), sizeof(uint32_t)); - // PVT GPS time - tmp_double = gnss_observables_map.cbegin()->second.RX_time; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // User clock offset [s] - tmp_double = rx_position_and_time[3]; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - - // ECEF POS X,Y,X [m] + ECEF VEL X,Y,X [m/s] (6 x double) - tmp_double = pvt_sol.rr[0]; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = pvt_sol.rr[1]; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = pvt_sol.rr[2]; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = pvt_sol.rr[3]; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = pvt_sol.rr[4]; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = pvt_sol.rr[5]; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - - // position variance/covariance (m^2) {c_xx,c_yy,c_zz,c_xy,c_yz,c_zx} (6 x double) - tmp_double = pvt_sol.qr[0]; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = pvt_sol.qr[1]; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = pvt_sol.qr[2]; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = pvt_sol.qr[3]; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = pvt_sol.qr[4]; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = pvt_sol.qr[5]; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - - // GEO user position Latitude [deg] - tmp_double = this->get_latitude(); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // GEO user position Longitude [deg] - tmp_double = this->get_longitude(); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // GEO user position Height [m] - tmp_double = this->get_height(); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - - // NUMBER OF VALID SATS - d_dump_file.write(reinterpret_cast(&pvt_sol.ns), sizeof(uint8_t)); - // RTKLIB solution status - d_dump_file.write(reinterpret_cast(&pvt_sol.stat), sizeof(uint8_t)); - // RTKLIB solution type (0:xyz-ecef,1:enu-baseline) - d_dump_file.write(reinterpret_cast(&pvt_sol.type), sizeof(uint8_t)); - // AR ratio factor for validation - d_dump_file.write(reinterpret_cast(&pvt_sol.ratio), sizeof(float)); - // AR ratio threshold for validation - d_dump_file.write(reinterpret_cast(&pvt_sol.thres), sizeof(float)); - - // GDOP / PDOP / HDOP / VDOP - d_dump_file.write(reinterpret_cast(&d_dop[0]), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_dop[1]), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_dop[2]), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_dop[3]), sizeof(double)); - } - catch (const std::ofstream::failure &e) - { - LOG(WARNING) << "Exception writing RTKLIB dump file " << e.what(); - } - } - } - } - return this->is_valid_position(); -} +/*! + * \file rtklib_solver.cc + * \brief PVT solver based on rtklib library functions adapted to the GNSS-SDR + * data flow and structures + * \authors
    + *
  • 2017-2019, Javier Arribas + *
  • 2017-2023, Carles Fernandez + *
  • 2007-2013, T. Takasu + *
+ * + * This is a derived work from RTKLIB http://www.rtklib.com/ + * The original source code at https://github.com/tomojitakasu/RTKLIB is + * released under the BSD 2-clause license with an additional exclusive clause + * that does not apply here. This additional clause is reproduced below: + * + * " The software package includes some companion executive binaries or shared + * libraries necessary to execute APs on Windows. These licenses succeed to the + * original ones of these software. " + * + * Neither the executive binaries nor the shared libraries are required by, used + * or included in GNSS-SDR. + * + * ----------------------------------------------------------------------------- + * Copyright (C) 2007-2013, T. Takasu + * Copyright (C) 2017-2019, Javier Arribas + * Copyright (C) 2017-2023, Carles Fernandez + * All rights reserved. + * + * SPDX-License-Identifier: BSD-2-Clause + * + * -----------------------------------------------------------------------*/ + +#include "rtklib_solver.h" +#include "Beidou_DNAV.h" +#include "gnss_sdr_filesystem.h" +#include "rtklib_rtkpos.h" +#include "rtklib_solution.h" +#include +#include +#include +#include +#include +#include +#include + + +Rtklib_Solver::Rtklib_Solver(const rtk_t &rtk, + const std::string &dump_filename, + uint32_t type_of_rx, + bool flag_dump_to_file, + bool flag_dump_to_mat, + Pvt_Conf conf) : d_dump_filename(dump_filename), + d_rtk(rtk), + d_conf(std::move(conf)), + d_type_of_rx(type_of_rx), + d_flag_dump_enabled(flag_dump_to_file), + d_flag_dump_mat_enabled(flag_dump_to_mat) +{ + // see freq index at src/algorithms/libs/rtklib/rtklib_rtkcmn.cc + // function: satwavelen + d_rtklib_freq_index[0] = 0; + d_rtklib_freq_index[1] = 1; + d_rtklib_freq_index[2] = 2; + + d_rtklib_band_index["1G"] = 0; + d_rtklib_band_index["1C"] = 0; + d_rtklib_band_index["1B"] = 0; + d_rtklib_band_index["B1"] = 0; + d_rtklib_band_index["B3"] = 2; + d_rtklib_band_index["2G"] = 1; + d_rtklib_band_index["2S"] = 1; + d_rtklib_band_index["7X"] = 2; + d_rtklib_band_index["5X"] = 2; + d_rtklib_band_index["L5"] = 2; + d_rtklib_band_index["E6"] = 0; + + switch (d_type_of_rx) + { + case 6: // E5b only + d_rtklib_freq_index[2] = 4; + break; + case 11: // GPS L1 C/A + Galileo E5b + d_rtklib_freq_index[2] = 4; + break; + case 15: // Galileo E1B + Galileo E5b + d_rtklib_freq_index[2] = 4; + break; + case 18: // GPS L2C + Galileo E5b + d_rtklib_freq_index[2] = 4; + break; + case 19: // Galileo E5a + Galileo E5b + d_rtklib_band_index["5X"] = 0; + d_rtklib_freq_index[0] = 2; + d_rtklib_freq_index[2] = 4; + break; + case 20: // GPS L5 + Galileo E5b + d_rtklib_band_index["L5"] = 0; + d_rtklib_freq_index[0] = 2; + d_rtklib_freq_index[2] = 4; + break; + case 100: // E6B only + d_rtklib_freq_index[0] = 3; + break; + case 101: // E1 + E6B + d_rtklib_band_index["E6"] = 1; + d_rtklib_freq_index[1] = 3; + break; + case 102: // E5a + E6B + d_rtklib_band_index["E6"] = 1; + d_rtklib_freq_index[1] = 3; + break; + case 103: // E5b + E6B + d_rtklib_band_index["E6"] = 1; + d_rtklib_freq_index[1] = 3; + d_rtklib_freq_index[2] = 4; + break; + case 104: // Galileo E1B + Galileo E5a + Galileo E6B + d_rtklib_band_index["E6"] = 1; + d_rtklib_freq_index[1] = 3; + break; + case 105: // Galileo E1B + Galileo E5b + Galileo E6B + d_rtklib_freq_index[2] = 4; + d_rtklib_band_index["E6"] = 1; + d_rtklib_freq_index[1] = 3; + break; + case 106: // GPS L1 C/A + Galileo E1B + Galileo E6B + case 107: // GPS L1 C/A + Galileo E6B + d_rtklib_band_index["E6"] = 1; + d_rtklib_freq_index[1] = 3; + break; + } + // auto empty_map = std::map < int, HAS_obs_corrections >> (); + // d_has_obs_corr_map["L1 C/A"] = empty_map; + + // ############# ENABLE DATA FILE LOG ################# + if (d_flag_dump_enabled == true) + { + if (d_dump_file.is_open() == false) + { + try + { + d_dump_file.exceptions(std::ofstream::failbit | std::ofstream::badbit); + d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); + LOG(INFO) << "PVT lib dump enabled Log file: " << d_dump_filename.c_str(); + } + catch (const std::ofstream::failure &e) + { + LOG(WARNING) << "Exception opening RTKLIB dump file " << e.what(); + } + } + } +} + + +Rtklib_Solver::~Rtklib_Solver() +{ + DLOG(INFO) << "Rtklib_Solver destructor called."; + if (d_dump_file.is_open() == true) + { + const auto pos = d_dump_file.tellp(); + try + { + d_dump_file.close(); + } + catch (const std::exception &ex) + { + LOG(WARNING) << "Exception in destructor closing the RTKLIB dump file " << ex.what(); + } + if (pos == 0) + { + errorlib::error_code ec; + if (!fs::remove(fs::path(d_dump_filename), ec)) + { + std::cerr << "Problem removing temporary file " << d_dump_filename << '\n'; + } + d_flag_dump_mat_enabled = false; + } + } + if (d_flag_dump_mat_enabled) + { + try + { + save_matfile(); + } + catch (const std::exception &ex) + { + LOG(WARNING) << "Exception in destructor saving the PVT .mat dump file " << ex.what(); + } + } +} + + +bool Rtklib_Solver::save_matfile() const +{ + // READ DUMP FILE + const std::string dump_filename = d_dump_filename; + const int32_t number_of_double_vars = 21; + const int32_t number_of_uint32_vars = 2; + const int32_t number_of_uint8_vars = 3; + const int32_t number_of_float_vars = 2; + const int32_t epoch_size_bytes = sizeof(double) * number_of_double_vars + + sizeof(uint32_t) * number_of_uint32_vars + + sizeof(uint8_t) * number_of_uint8_vars + + sizeof(float) * number_of_float_vars; + std::ifstream dump_file; + dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); + try + { + dump_file.open(dump_filename.c_str(), std::ios::binary | std::ios::ate); + } + catch (const std::ifstream::failure &e) + { + std::cerr << "Problem opening dump file:" << e.what() << '\n'; + return false; + } + // count number of epochs and rewind + int64_t num_epoch = 0LL; + if (dump_file.is_open()) + { + std::cout << "Generating .mat file for " << dump_filename << '\n'; + const std::ifstream::pos_type size = dump_file.tellg(); + num_epoch = static_cast(size) / static_cast(epoch_size_bytes); + dump_file.seekg(0, std::ios::beg); + } + else + { + return false; + } + + auto TOW_at_current_symbol_ms = std::vector(num_epoch); + auto week = std::vector(num_epoch); + auto RX_time = std::vector(num_epoch); + auto user_clk_offset = std::vector(num_epoch); + auto pos_x = std::vector(num_epoch); + auto pos_y = std::vector(num_epoch); + auto pos_z = std::vector(num_epoch); + auto vel_x = std::vector(num_epoch); + auto vel_y = std::vector(num_epoch); + auto vel_z = std::vector(num_epoch); + auto cov_xx = std::vector(num_epoch); + auto cov_yy = std::vector(num_epoch); + auto cov_zz = std::vector(num_epoch); + auto cov_xy = std::vector(num_epoch); + auto cov_yz = std::vector(num_epoch); + auto cov_zx = std::vector(num_epoch); + auto latitude = std::vector(num_epoch); + auto longitude = std::vector(num_epoch); + auto height = std::vector(num_epoch); + auto valid_sats = std::vector(num_epoch); + auto solution_status = std::vector(num_epoch); + auto solution_type = std::vector(num_epoch); + auto AR_ratio_factor = std::vector(num_epoch); + auto AR_ratio_threshold = std::vector(num_epoch); + auto gdop = std::vector(num_epoch); + auto pdop = std::vector(num_epoch); + auto hdop = std::vector(num_epoch); + auto vdop = std::vector(num_epoch); + + try + { + if (dump_file.is_open()) + { + for (int64_t i = 0; i < num_epoch; i++) + { + dump_file.read(reinterpret_cast(&TOW_at_current_symbol_ms[i]), sizeof(uint32_t)); + dump_file.read(reinterpret_cast(&week[i]), sizeof(uint32_t)); + dump_file.read(reinterpret_cast(&RX_time[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&user_clk_offset[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&pos_x[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&pos_y[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&pos_z[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&vel_x[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&vel_y[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&vel_z[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&cov_xx[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&cov_yy[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&cov_zz[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&cov_xy[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&cov_yz[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&cov_zx[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&latitude[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&longitude[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&height[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&valid_sats[i]), sizeof(uint8_t)); + dump_file.read(reinterpret_cast(&solution_status[i]), sizeof(uint8_t)); + dump_file.read(reinterpret_cast(&solution_type[i]), sizeof(uint8_t)); + dump_file.read(reinterpret_cast(&AR_ratio_factor[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&AR_ratio_threshold[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&gdop[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&pdop[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&hdop[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&vdop[i]), sizeof(double)); + } + } + dump_file.close(); + } + catch (const std::ifstream::failure &e) + { + std::cerr << "Problem reading dump file:" << e.what() << '\n'; + return false; + } + + // WRITE MAT FILE + mat_t *matfp; + matvar_t *matvar; + std::string filename = dump_filename; + filename.erase(filename.length() - 4, 4); + filename.append(".mat"); + matfp = Mat_CreateVer(filename.c_str(), nullptr, MAT_FT_MAT73); + if (reinterpret_cast(matfp) != nullptr) + { + std::array dims{1, static_cast(num_epoch)}; + matvar = Mat_VarCreate("TOW_at_current_symbol_ms", MAT_C_UINT32, MAT_T_UINT32, 2, dims.data(), TOW_at_current_symbol_ms.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("week", MAT_C_UINT32, MAT_T_UINT32, 2, dims.data(), week.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("RX_time", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), RX_time.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("user_clk_offset", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), user_clk_offset.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("pos_x", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), pos_x.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("pos_y", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), pos_y.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("pos_z", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), pos_z.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("vel_x", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), vel_x.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("vel_y", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), vel_y.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("vel_z", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), vel_z.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("cov_xx", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_xx.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("cov_yy", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_yy.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("cov_zz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_zz.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("cov_xy", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_xy.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("cov_yz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_yz.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("cov_zx", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_zx.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("latitude", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), latitude.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("longitude", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), longitude.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("height", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), height.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("valid_sats", MAT_C_UINT8, MAT_T_UINT8, 2, dims.data(), valid_sats.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("solution_status", MAT_C_UINT8, MAT_T_UINT8, 2, dims.data(), solution_status.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("solution_type", MAT_C_UINT8, MAT_T_UINT8, 2, dims.data(), solution_type.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("AR_ratio_factor", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims.data(), AR_ratio_factor.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("AR_ratio_threshold", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims.data(), AR_ratio_threshold.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("gdop", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), gdop.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("pdop", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), pdop.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("hdop", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), hdop.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("vdop", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), vdop.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + } + + Mat_Close(matfp); + return true; +} + + +double Rtklib_Solver::get_gdop() const +{ + return d_dop[0]; +} + + +double Rtklib_Solver::get_pdop() const +{ + return d_dop[1]; +} + + +double Rtklib_Solver::get_hdop() const +{ + return d_dop[2]; +} + + +double Rtklib_Solver::get_vdop() const +{ + return d_dop[3]; +} + + +Monitor_Pvt Rtklib_Solver::get_monitor_pvt() const +{ + return d_monitor_pvt; +} + + +void Rtklib_Solver::store_has_data(const Galileo_HAS_data &new_has_data) +{ + // Compute time of application HAS SIS ICD, Issue 1.0, Section 7.7 + uint16_t toh = new_has_data.header.toh; + uint32_t hr = std::floor(new_has_data.tow / 3600); + uint32_t tmt = 0; + if ((hr * 3600 + toh) <= new_has_data.tow) + { + tmt = hr * 3600 + toh; + } + else + { + tmt = (hr - 1) * 3600 + toh; + } + + const std::string gps_str("GPS"); + const std::string gal_str("Galileo"); + if (new_has_data.header.orbit_correction_flag) + { + LOG(INFO) << "Received HAS orbit corrections"; + // for each satellite in GPS ephemeris + for (const auto &gpseph : gps_ephemeris_map) + { + int prn = gpseph.second.PRN; + int32_t sis_iod = gpseph.second.IODE_SF3; + uint16_t gnss_iod = new_has_data.get_gnss_iod(gps_str, prn); + if (static_cast(gnss_iod) == sis_iod) + { + float radial_m = new_has_data.get_delta_radial_m(gps_str, prn); + if (std::fabs(radial_m + 10.24) < 0.001) // -10.24 means not available + { + radial_m = 0.0; + } + float in_track_m = new_has_data.get_delta_in_track_m(gps_str, prn); + if (std::fabs(in_track_m + 16.384) < 0.001) // -16.384 means not available + { + in_track_m = 0.0; + } + float cross_track_m = new_has_data.get_delta_in_track_m(gps_str, prn); + if (std::fabs(cross_track_m + 16.384) < 0.001) // -16.384 means not available + { + cross_track_m = 0.0; + } + d_has_orbit_corrections_store_map[gps_str][prn].radial_m = radial_m; + d_has_orbit_corrections_store_map[gps_str][prn].in_track_m = in_track_m; + d_has_orbit_corrections_store_map[gps_str][prn].cross_track_m = cross_track_m; + d_has_orbit_corrections_store_map[gps_str][prn].valid_until = tmt + + new_has_data.get_validity_interval_s(new_has_data.validity_interval_index_orbit_corrections); + d_has_orbit_corrections_store_map[gps_str][prn].iod = gnss_iod; + // TODO: check for end of week + } + } + + // for each satellite in Galileo ephemeris + for (const auto &galeph : galileo_ephemeris_map) + { + int prn = galeph.second.PRN; + int32_t sis_iod = galeph.second.IOD_ephemeris; + uint16_t gnss_iod = new_has_data.get_gnss_iod(gal_str, prn); + if (static_cast(gnss_iod) == sis_iod) + { + float radial_m = new_has_data.get_delta_radial_m(gal_str, prn); + if (std::fabs(radial_m + 10.24) < 0.001) // -10.24 means not available + { + radial_m = 0.0; + } + float in_track_m = new_has_data.get_delta_in_track_m(gal_str, prn); + if (std::fabs(in_track_m + 16.384) < 0.001) // -16.384 means not available + { + in_track_m = 0.0; + } + float cross_track_m = new_has_data.get_delta_in_track_m(gal_str, prn); + if (std::fabs(cross_track_m + 16.384) < 0.001) // -16.384 means not available + { + cross_track_m = 0.0; + } + d_has_orbit_corrections_store_map[gal_str][prn].radial_m = radial_m; + d_has_orbit_corrections_store_map[gal_str][prn].in_track_m = in_track_m; + d_has_orbit_corrections_store_map[gal_str][prn].cross_track_m = cross_track_m; + d_has_orbit_corrections_store_map[gal_str][prn].valid_until = tmt + + new_has_data.get_validity_interval_s(new_has_data.validity_interval_index_orbit_corrections); + d_has_orbit_corrections_store_map[gal_str][prn].iod = gnss_iod; + // TODO: check for end of week + } + } + } + if (new_has_data.header.clock_fullset_flag) + { + LOG(INFO) << "Received HAS clock fullset corrections"; + for (const auto &gpseph : gps_ephemeris_map) + { + int prn = gpseph.second.PRN; + int32_t sis_iod = gpseph.second.IODE_SF3; + auto it = d_has_orbit_corrections_store_map[gps_str].find(prn); + if (it != d_has_orbit_corrections_store_map[gps_str].end()) + { + uint16_t gnss_iod = it->second.iod; + if (static_cast(gnss_iod) == sis_iod) + { + float clock_correction_mult_m = new_has_data.get_clock_correction_mult_m(gps_str, prn); + if ((std::fabs(clock_correction_mult_m + 10.24) < 0.001) || + (std::fabs(clock_correction_mult_m + 20.48) < 0.001) || + (std::fabs(clock_correction_mult_m + 30.72) < 0.001) || + (std::fabs(clock_correction_mult_m + 40.96) < 0.001)) + { + clock_correction_mult_m = 0.0; + } + if ((std::fabs(clock_correction_mult_m - 10.2375) < 0.001) || + (std::fabs(clock_correction_mult_m - 20.475) < 0.001) || + (std::fabs(clock_correction_mult_m - 30.7125) < 0.001) || + (std::fabs(clock_correction_mult_m - 40.95) < 0.001)) + { + // Satellite should not be used! + clock_correction_mult_m = 0.0; + } + d_has_clock_corrections_store_map[gps_str][prn].clock_correction_m = clock_correction_mult_m; + d_has_clock_corrections_store_map[gps_str][prn].valid_until = tmt + + new_has_data.get_validity_interval_s(new_has_data.validity_interval_index_clock_fullset_corrections); + // TODO: check for end of week + } + } + } + + // for each satellite in Galileo ephemeris + for (const auto &galeph : galileo_ephemeris_map) + { + int prn = galeph.second.PRN; + int32_t iod_sis = galeph.second.IOD_ephemeris; + auto it = d_has_orbit_corrections_store_map[gal_str].find(prn); + if (it != d_has_orbit_corrections_store_map[gal_str].end()) + { + uint16_t gnss_iod = it->second.iod; + if (static_cast(gnss_iod) == iod_sis) + { + float clock_correction_mult_m = new_has_data.get_clock_correction_mult_m(gal_str, prn); + // std::cout << "Galileo Satellite " << prn + // << " clock correction=" << new_has_data.get_clock_correction_mult_m(gal_str, prn) + // << std::endl; + if ((std::fabs(clock_correction_mult_m + 10.24) < 0.001) || + (std::fabs(clock_correction_mult_m + 20.48) < 0.001) || + (std::fabs(clock_correction_mult_m + 30.72) < 0.001) || + (std::fabs(clock_correction_mult_m + 40.96) < 0.001)) + { + clock_correction_mult_m = 0.0; + } + d_has_clock_corrections_store_map[gal_str][prn].clock_correction_m = clock_correction_mult_m; + d_has_clock_corrections_store_map[gal_str][prn].valid_until = tmt + + new_has_data.get_validity_interval_s(new_has_data.validity_interval_index_clock_fullset_corrections); + // TODO: check for end of week + } + } + } + } + if (new_has_data.header.clock_subset_flag) + { + LOG(INFO) << "Received HAS clock subset corrections"; + for (const auto &gpseph : gps_ephemeris_map) + { + int prn = gpseph.second.PRN; + int32_t sis_iod = gpseph.second.IODE_SF3; + int32_t gnss_iod = d_has_orbit_corrections_store_map[gps_str][prn].iod; + if (gnss_iod == sis_iod) + { + // d_has_clock_corrections_store_map[gps_str][prn].clock_correction_m = new_has_data.get_clock_subset_correction_mult_m(gps_str, prn); + // d_has_clock_corrections_store_map[gps_str][prn].valid_until = tmt + new_has_data.get_validity_interval_s(new_has_data.validity_interval_index_clock_subset_corrections); + // TODO: check for end of week + } + } + } + if (new_has_data.header.code_bias_flag) + { + LOG(INFO) << "Received HAS code bias corrections"; + uint32_t valid_until = tmt + + new_has_data.get_validity_interval_s(new_has_data.validity_interval_index_code_bias_corrections); + auto signals_gal = new_has_data.get_signals_in_mask(gal_str); + for (const auto &it : signals_gal) + { + auto prns = new_has_data.get_PRNs_in_mask(gal_str); + for (auto prn : prns) + { + float code_bias_m = new_has_data.get_code_bias_m(it, prn); + if ((std::fabs(code_bias_m + 20.48) < 0.01)) // -20.48 means not available + { + code_bias_m = 0.0; + } + d_has_code_bias_store_map[it][prn] = {code_bias_m, valid_until}; + } + } + auto signals_gps = new_has_data.get_signals_in_mask(gps_str); + for (const auto &it : signals_gps) + { + auto prns = new_has_data.get_PRNs_in_mask(gps_str); + for (auto prn : prns) + { + float code_bias_m = new_has_data.get_code_bias_m(it, prn); + if ((std::fabs(code_bias_m + 20.48) < 0.01)) // -20.48 means not available + { + code_bias_m = 0.0; + } + d_has_code_bias_store_map[it][prn] = {code_bias_m, valid_until}; + } + } + } + if (new_has_data.header.phase_bias_flag) + { + LOG(INFO) << "Received HAS phase bias corrections"; + uint32_t valid_until = tmt + + new_has_data.get_validity_interval_s(new_has_data.validity_interval_index_phase_bias_corrections); + + auto signals_gal = new_has_data.get_signals_in_mask(gal_str); + for (const auto &it : signals_gal) + { + auto prns = new_has_data.get_PRNs_in_mask(gal_str); + for (auto prn : prns) + { + float phase_bias_correction_cycles = new_has_data.get_phase_bias_cycle(it, prn); + if (std::fabs(phase_bias_correction_cycles + 10.24) < 0.001) // -10.24 means not available + { + phase_bias_correction_cycles = 0.0; + } + d_has_phase_bias_store_map[it][prn] = {phase_bias_correction_cycles, valid_until}; + // TODO: process Phase Discontinuity Indicator + } + } + auto signals_gps = new_has_data.get_signals_in_mask(gps_str); + for (const auto &it : signals_gps) + { + auto prns = new_has_data.get_PRNs_in_mask(gps_str); + for (auto prn : prns) + { + float phase_bias_correction_cycles = new_has_data.get_phase_bias_cycle(it, prn); + if (std::fabs(phase_bias_correction_cycles + 10.24) < 0.001) // -10.24 means not available + { + phase_bias_correction_cycles = 0.0; + } + d_has_phase_bias_store_map[it][prn] = {phase_bias_correction_cycles, valid_until}; + // TODO: process Phase Discontinuity Indicator + } + } + } +} + + +void Rtklib_Solver::update_has_corrections(const std::map &obs_map) +{ + this->check_has_orbit_clock_validity(obs_map); + this->get_has_biases(obs_map); +} + + +void Rtklib_Solver::check_has_orbit_clock_validity(const std::map &obs_map) +{ + for (const auto &it : obs_map) + { + uint32_t obs_tow = it.second.interp_TOW_ms / 1000.0; + auto prn = static_cast(it.second.PRN); + + if (it.second.System == 'G') + { + auto it_sys = d_has_orbit_corrections_store_map.find("GPS"); + if (it_sys != d_has_orbit_corrections_store_map.end()) + { + auto it_map_corr = it_sys->second.find(prn); + if (it_map_corr != it_sys->second.end()) + { + auto has_data_valid_until = it_map_corr->second.valid_until; + if (has_data_valid_until < obs_tow) + { + // Delete outdated data + it_sys->second.erase(prn); + } + } + } + auto it_sys_clock = d_has_clock_corrections_store_map.find("GPS"); + if (it_sys_clock != d_has_clock_corrections_store_map.end()) + { + auto it_map_corr = it_sys_clock->second.find(prn); + if (it_map_corr != it_sys_clock->second.end()) + { + auto has_data_valid_until = it_map_corr->second.valid_until; + if (has_data_valid_until < obs_tow) + { + // Delete outdated data + it_sys_clock->second.erase(prn); + } + } + } + } + if (it.second.System == 'E') + { + auto it_sys = d_has_orbit_corrections_store_map.find("Galileo"); + if (it_sys != d_has_orbit_corrections_store_map.end()) + { + auto it_map_corr = it_sys->second.find(prn); + if (it_map_corr != it_sys->second.end()) + { + auto has_data_valid_until = it_map_corr->second.valid_until; + if (has_data_valid_until < obs_tow) + { + // Delete outdated data + it_sys->second.erase(prn); + } + } + } + auto it_sys_clock = d_has_clock_corrections_store_map.find("Galileo"); + if (it_sys_clock != d_has_clock_corrections_store_map.end()) + { + auto it_map_corr = it_sys_clock->second.find(prn); + if (it_map_corr != it_sys_clock->second.end()) + { + auto has_data_valid_until = it_map_corr->second.valid_until; + if (has_data_valid_until < obs_tow) + { + // Delete outdated data + it_sys_clock->second.erase(prn); + } + } + } + } + } +} + + +void Rtklib_Solver::get_has_biases(const std::map &obs_map) +{ + d_has_obs_corr_map.clear(); + if (!d_has_clock_corrections_store_map.empty() && !d_has_orbit_corrections_store_map.empty()) + { + const std::vector e1b_signals = {"E1-B I/NAV OS", "E1-C", "E1-B + E1-C"}; + const std::vector e6_signals = {"E6-B C/NAV HAS", "E6-C", "E6-B + E6-C"}; + const std::vector e5_signals = {"E5a-I F/NAV OS", "E5a-Q", "E5a-I+E5a-Q"}; + const std::vector e7_signals = {"E5bI I/NAV OS", "E5b-Q", "E5b-I+E5b-Q"}; + const std::vector g1c_signals = {"L1 C/A"}; + const std::vector g2s_signals = {"L2 CM", "L2 CL", "L2 CM+CL", "L2 P"}; + const std::vector g5_signals = {"L5 I", "L5 Q", "L5 I + L5 Q"}; + + for (const auto &it : obs_map) + { + uint32_t obs_tow = it.second.interp_TOW_ms / 1000.0; + int prn = static_cast(it.second.PRN); + std::string sig(it.second.Signal, 2); + if (it.second.System == 'E') + { + auto it_sys_clock = d_has_clock_corrections_store_map.find("Galileo"); + if (it_sys_clock != d_has_clock_corrections_store_map.end()) + { + auto it_map_corr = it_sys_clock->second.find(prn); + if (it_map_corr != it_sys_clock->second.end()) + { + if (sig == "1B") + { + for (const auto &has_signal : e1b_signals) + { + this->get_current_has_obs_correction(has_signal, obs_tow, prn); + } + } + else if (sig == "E6") + { + for (const auto &has_signal : e6_signals) + { + this->get_current_has_obs_correction(has_signal, obs_tow, prn); + } + } + else if (sig == "5X") + { + for (const auto &has_signal : e5_signals) + { + this->get_current_has_obs_correction(has_signal, obs_tow, prn); + } + } + else if (sig == "7X") + { + for (const auto &has_signal : e7_signals) + { + this->get_current_has_obs_correction(has_signal, obs_tow, prn); + } + } + } + } + } + if (it.second.System == 'G') + { + auto it_sys_clock = d_has_clock_corrections_store_map.find("GPS"); + if (it_sys_clock != d_has_clock_corrections_store_map.end()) + { + auto it_map_corr = it_sys_clock->second.find(prn); + if (it_map_corr != it_sys_clock->second.end()) + { + if (sig == "1C") + { + for (const auto &has_signal : g1c_signals) + { + this->get_current_has_obs_correction(has_signal, obs_tow, prn); + } + } + else if (sig == "2S") + { + for (const auto &has_signal : g2s_signals) + { + this->get_current_has_obs_correction(has_signal, obs_tow, prn); + } + } + else if (sig == "L5") + { + for (const auto &has_signal : g5_signals) + { + this->get_current_has_obs_correction(has_signal, obs_tow, prn); + } + } + } + } + } + } + } +} + + +void Rtklib_Solver::get_current_has_obs_correction(const std::string &signal, uint32_t tow_obs, int prn) +{ + auto code_bias_pair_it = this->d_has_code_bias_store_map[signal].find(prn); + if (code_bias_pair_it != this->d_has_code_bias_store_map[signal].end()) + { + uint32_t valid_until = code_bias_pair_it->second.second; + if (valid_until > tow_obs) + { + this->d_has_obs_corr_map[signal][prn].code_bias_m = code_bias_pair_it->second.first; + } + } + auto phase_bias_pair_it = this->d_has_phase_bias_store_map[signal].find(prn); + if (phase_bias_pair_it != this->d_has_phase_bias_store_map[signal].end()) + { + uint32_t valid_until = phase_bias_pair_it->second.second; + if (valid_until > tow_obs) + { + this->d_has_obs_corr_map[signal][prn].phase_bias_cycle = phase_bias_pair_it->second.first; + } + } +} + + +bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_map, double kf_update_interval_s) +{ + std::map::const_iterator gnss_observables_iter; + std::map::const_iterator galileo_ephemeris_iter; + std::map::const_iterator gps_ephemeris_iter; + std::map::const_iterator gps_cnav_ephemeris_iter; + std::map::const_iterator glonass_gnav_ephemeris_iter; + std::map::const_iterator beidou_ephemeris_iter; + + const Glonass_Gnav_Utc_Model &gnav_utc = this->glonass_gnav_utc_model; + + // ******************************************************************************** + // ****** PREPARE THE DATA (SV EPHEMERIS AND OBSERVATIONS) ************************ + // ******************************************************************************** + int valid_obs = 0; // valid observations counter + int glo_valid_obs = 0; // GLONASS L1/L2 valid observations counter + + d_obs_data.fill({}); + std::vector eph_data(MAXOBS); + std::vector geph_data(MAXOBS); + + // Workaround for NAV/CNAV clash problem + bool gps_dual_band = false; + bool band1 = false; + bool band2 = false; + + for (gnss_observables_iter = gnss_observables_map.cbegin(); + gnss_observables_iter != gnss_observables_map.cend(); + ++gnss_observables_iter) + { + switch (gnss_observables_iter->second.System) + { + case 'G': + { + const std::string sig_(gnss_observables_iter->second.Signal, 2); + if (sig_ == "1C") + { + band1 = true; + } + if (sig_ == "2S") + { + band2 = true; + } + } + break; + default: + { + } + } + } + if (band1 == true and band2 == true) + { + gps_dual_band = true; + } + + for (gnss_observables_iter = gnss_observables_map.cbegin(); + gnss_observables_iter != gnss_observables_map.cend(); + ++gnss_observables_iter) // CHECK INCONSISTENCY when combining GLONASS + other system + { + switch (gnss_observables_iter->second.System) + { + case 'E': + { + const std::string gal_str("Galileo"); + const std::string sig_(gnss_observables_iter->second.Signal, 2); + // Galileo E1 + if (sig_ == "1B") + { + // 1 Gal - find the ephemeris for the current GALILEO SV observation. The SV PRN ID is the map key + galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (galileo_ephemeris_iter != galileo_ephemeris_map.cend()) + { + // convert ephemeris from GNSS-SDR class to RTKLIB structure + eph_data[valid_obs] = eph_to_rtklib(galileo_ephemeris_iter->second, + this->d_has_orbit_corrections_store_map[gal_str], + this->d_has_clock_corrections_store_map[gal_str]); + // convert observation from GNSS-SDR class to RTKLIB structure + obsd_t newobs{}; + d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, + gnss_observables_iter->second, + d_has_obs_corr_map, + galileo_ephemeris_iter->second.WN, + d_rtklib_band_index[sig_]); + valid_obs++; + } + else // the ephemeris are not available for this SV + { + DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; + } + } + + // Galileo E5 + if ((sig_ == "5X") || (sig_ == "7X")) + { + // 1 Gal - find the ephemeris for the current GALILEO SV observation. The SV PRN ID is the map key + galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (galileo_ephemeris_iter != galileo_ephemeris_map.cend()) + { + bool found_E1_obs = false; + for (int i = 0; i < valid_obs; i++) + { + if (eph_data[i].sat == (static_cast(gnss_observables_iter->second.PRN + NSATGPS + NSATGLO))) + { + d_obs_data[i + glo_valid_obs] = insert_obs_to_rtklib(d_obs_data[i + glo_valid_obs], + gnss_observables_iter->second, + d_has_obs_corr_map, + galileo_ephemeris_iter->second.WN, + d_rtklib_band_index[sig_]); + found_E1_obs = true; + break; + } + } + if (!found_E1_obs) + { + // insert Galileo E5 obs as new obs and also insert its ephemeris + // convert ephemeris from GNSS-SDR class to RTKLIB structure + eph_data[valid_obs] = eph_to_rtklib(galileo_ephemeris_iter->second, + this->d_has_orbit_corrections_store_map[gal_str], + this->d_has_clock_corrections_store_map[gal_str]); + // convert observation from GNSS-SDR class to RTKLIB structure + const auto default_code_ = static_cast(CODE_NONE); + obsd_t newobs = {{0, 0}, '0', '0', {}, {}, + {default_code_, default_code_, default_code_}, + {}, {0.0, 0.0, 0.0}, {}}; + d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, + gnss_observables_iter->second, + d_has_obs_corr_map, + galileo_ephemeris_iter->second.WN, + d_rtklib_band_index[sig_]); + valid_obs++; + } + } + else // the ephemeris are not available for this SV + { + DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; + } + } + if (sig_ == "E6" && d_conf.use_e6_for_pvt) + { + galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (galileo_ephemeris_iter != galileo_ephemeris_map.cend()) + { + bool found_E1_obs = false; + for (int i = 0; i < valid_obs; i++) + { + if (eph_data[i].sat == (static_cast(gnss_observables_iter->second.PRN + NSATGPS + NSATGLO))) + { + d_obs_data[i + glo_valid_obs] = insert_obs_to_rtklib(d_obs_data[i + glo_valid_obs], + gnss_observables_iter->second, + d_has_obs_corr_map, + galileo_ephemeris_iter->second.WN, + d_rtklib_band_index[sig_]); + found_E1_obs = true; + break; + } + } + if (!found_E1_obs) + { + // insert Galileo E6 obs as new obs and also insert its ephemeris + // convert ephemeris from GNSS-SDR class to RTKLIB structure + eph_data[valid_obs] = eph_to_rtklib(galileo_ephemeris_iter->second, + this->d_has_orbit_corrections_store_map[gal_str], + this->d_has_clock_corrections_store_map[gal_str]); + // convert observation from GNSS-SDR class to RTKLIB structure + const auto default_code_ = static_cast(CODE_NONE); + obsd_t newobs = {{0, 0}, '0', '0', {}, {}, + {default_code_, default_code_, default_code_}, + {}, {0.0, 0.0, 0.0}, {}}; + d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, + gnss_observables_iter->second, + d_has_obs_corr_map, + galileo_ephemeris_iter->second.WN, + d_rtklib_band_index[sig_]); + valid_obs++; + } + } + else // the ephemeris are not available for this SV + { + DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; + } + } + if (sig_ == "E6") + { + galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (galileo_ephemeris_iter != galileo_ephemeris_map.cend()) + { + bool found_E1_obs = false; + for (int i = 0; i < valid_obs; i++) + { + if (eph_data[i].sat == (static_cast(gnss_observables_iter->second.PRN + NSATGPS + NSATGLO))) + { + d_obs_data[i + glo_valid_obs] = insert_obs_to_rtklib(d_obs_data[i + glo_valid_obs], + gnss_observables_iter->second, + galileo_ephemeris_iter->second.WN, + 2); // Band E6 + found_E1_obs = true; + break; + } + } + if (!found_E1_obs) + { + // insert Galileo E6 obs as new obs and also insert its ephemeris + // convert ephemeris from GNSS-SDR class to RTKLIB structure + eph_data[valid_obs] = eph_to_rtklib(galileo_ephemeris_iter->second); + // convert observation from GNSS-SDR class to RTKLIB structure + const auto default_code_ = static_cast(CODE_NONE); + obsd_t newobs = {{0, 0}, '0', '0', {}, {}, + {default_code_, default_code_, default_code_}, + {}, {0.0, 0.0, 0.0}, {}}; + d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, + gnss_observables_iter->second, + galileo_ephemeris_iter->second.WN, + 2); // Band E6 + // std::cout << "Week " << galileo_ephemeris_iter->second.WN << '\n'; + valid_obs++; + } + } + else // the ephemeris are not available for this SV + { + DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; + } + } + break; + } + case 'G': + { + // GPS L1 + // 1 GPS - find the ephemeris for the current GPS SV observation. The SV PRN ID is the map key + const std::string gps_str("GPS"); + const std::string sig_(gnss_observables_iter->second.Signal, 2); + if (sig_ == "1C") + { + gps_ephemeris_iter = gps_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (gps_ephemeris_iter != gps_ephemeris_map.cend()) + { + // convert ephemeris from GNSS-SDR class to RTKLIB structure + eph_data[valid_obs] = eph_to_rtklib(gps_ephemeris_iter->second, + this->d_has_orbit_corrections_store_map[gps_str], + this->d_has_clock_corrections_store_map[gps_str], + this->is_pre_2009()); + // convert observation from GNSS-SDR class to RTKLIB structure + obsd_t newobs{}; + d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, + gnss_observables_iter->second, + d_has_obs_corr_map, + gps_ephemeris_iter->second.WN, + d_rtklib_band_index[sig_], + this->is_pre_2009()); + valid_obs++; + } + else // the ephemeris are not available for this SV + { + DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->first; + } + } + // GPS L2 (todo: solve NAV/CNAV clash) + if ((sig_ == "2S") and (gps_dual_band == false)) + { + gps_cnav_ephemeris_iter = gps_cnav_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (gps_cnav_ephemeris_iter != gps_cnav_ephemeris_map.cend()) + { + // 1. Find the same satellite in GPS L1 band + gps_ephemeris_iter = gps_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (gps_ephemeris_iter != gps_ephemeris_map.cend()) + { + /* By the moment, GPS L2 observables are not used in pseudorange computations if GPS L1 is available + // 2. If found, replace the existing GPS L1 ephemeris with the GPS L2 ephemeris + // (more precise!), and attach the L2 observation to the L1 observation in RTKLIB structure + for (int i = 0; i < valid_obs; i++) + { + if (eph_data[i].sat == static_cast(gnss_observables_iter->second.PRN)) + { + eph_data[i] = eph_to_rtklib(gps_cnav_ephemeris_iter->second); + d_obs_data[i + glo_valid_obs] = insert_obs_to_rtklib(d_obs_data[i + glo_valid_obs], + gnss_observables_iter->second, + eph_data[i].week, + d_rtklib_band_index[sig_]); + break; + } + } + */ + } + else + { + // 3. If not found, insert the GPS L2 ephemeris and the observation + // convert ephemeris from GNSS-SDR class to RTKLIB structure + eph_data[valid_obs] = eph_to_rtklib(gps_cnav_ephemeris_iter->second); + // convert observation from GNSS-SDR class to RTKLIB structure + const auto default_code_ = static_cast(CODE_NONE); + obsd_t newobs = {{0, 0}, '0', '0', {}, {}, + {default_code_, default_code_, default_code_}, + {}, {0.0, 0.0, 0.0}, {}}; + d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, + gnss_observables_iter->second, + gps_cnav_ephemeris_iter->second.WN, + d_rtklib_band_index[sig_]); + valid_obs++; + } + } + else // the ephemeris are not available for this SV + { + DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; + } + } + // GPS L5 + if (sig_ == "L5") + { + gps_cnav_ephemeris_iter = gps_cnav_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (gps_cnav_ephemeris_iter != gps_cnav_ephemeris_map.cend()) + { + // 1. Find the same satellite in GPS L1 band + gps_ephemeris_iter = gps_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (gps_ephemeris_iter != gps_ephemeris_map.cend()) + { + // 2. If found, replace the existing GPS L1 ephemeris with the GPS L5 ephemeris + // (more precise!), and attach the L5 observation to the L1 observation in RTKLIB structure + for (int i = 0; i < valid_obs; i++) + { + if (eph_data[i].sat == static_cast(gnss_observables_iter->second.PRN)) + { + eph_data[i] = eph_to_rtklib(gps_cnav_ephemeris_iter->second); + d_obs_data[i + glo_valid_obs] = insert_obs_to_rtklib(d_obs_data[i], + gnss_observables_iter->second, + gps_cnav_ephemeris_iter->second.WN, + d_rtklib_band_index[sig_]); + break; + } + } + } + else + { + // 3. If not found, insert the GPS L5 ephemeris and the observation + // convert ephemeris from GNSS-SDR class to RTKLIB structure + eph_data[valid_obs] = eph_to_rtklib(gps_cnav_ephemeris_iter->second); + // convert observation from GNSS-SDR class to RTKLIB structure + const auto default_code_ = static_cast(CODE_NONE); + obsd_t newobs = {{0, 0}, '0', '0', {}, {}, + {default_code_, default_code_, default_code_}, + {}, {0.0, 0.0, 0.0}, {}}; + d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, + gnss_observables_iter->second, + d_has_obs_corr_map, + gps_cnav_ephemeris_iter->second.WN, + d_rtklib_band_index[sig_]); + valid_obs++; + } + } + else // the ephemeris are not available for this SV + { + DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; + } + } + break; + } + case 'R': // TODO This should be using rtk lib nomenclature + { + const std::string sig_(gnss_observables_iter->second.Signal); + // GLONASS GNAV L1 + if (sig_ == "1G") + { + // 1 Glo - find the ephemeris for the current GLONASS SV observation. The SV Slot Number (PRN ID) is the map key + glonass_gnav_ephemeris_iter = glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (glonass_gnav_ephemeris_iter != glonass_gnav_ephemeris_map.cend()) + { + // convert ephemeris from GNSS-SDR class to RTKLIB structure + geph_data[glo_valid_obs] = eph_to_rtklib(glonass_gnav_ephemeris_iter->second, gnav_utc); + // convert observation from GNSS-SDR class to RTKLIB structure + obsd_t newobs{}; + d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, + gnss_observables_iter->second, + glonass_gnav_ephemeris_iter->second.d_WN, + d_rtklib_band_index[sig_]); + glo_valid_obs++; + } + else // the ephemeris are not available for this SV + { + DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; + } + } + // GLONASS GNAV L2 + if (sig_ == "2G") + { + // 1 GLONASS - find the ephemeris for the current GLONASS SV observation. The SV PRN ID is the map key + glonass_gnav_ephemeris_iter = glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (glonass_gnav_ephemeris_iter != glonass_gnav_ephemeris_map.cend()) + { + bool found_L1_obs = false; + for (int i = 0; i < glo_valid_obs; i++) + { + if (geph_data[i].sat == (static_cast(gnss_observables_iter->second.PRN + NSATGPS))) + { + d_obs_data[i + valid_obs] = insert_obs_to_rtklib(d_obs_data[i + valid_obs], + gnss_observables_iter->second, + glonass_gnav_ephemeris_iter->second.d_WN, + d_rtklib_band_index[sig_]); + found_L1_obs = true; + break; + } + } + if (!found_L1_obs) + { + // insert GLONASS GNAV L2 obs as new obs and also insert its ephemeris + // convert ephemeris from GNSS-SDR class to RTKLIB structure + geph_data[glo_valid_obs] = eph_to_rtklib(glonass_gnav_ephemeris_iter->second, gnav_utc); + // convert observation from GNSS-SDR class to RTKLIB structure + obsd_t newobs{}; + d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, + gnss_observables_iter->second, + glonass_gnav_ephemeris_iter->second.d_WN, + d_rtklib_band_index[sig_]); + glo_valid_obs++; + } + } + else // the ephemeris are not available for this SV + { + DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; + } + } + break; + } + case 'C': + { + // BEIDOU B1I + // - find the ephemeris for the current BEIDOU SV observation. The SV PRN ID is the map key + const std::string sig_(gnss_observables_iter->second.Signal); + if (sig_ == "B1") + { + beidou_ephemeris_iter = beidou_dnav_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (beidou_ephemeris_iter != beidou_dnav_ephemeris_map.cend()) + { + // convert ephemeris from GNSS-SDR class to RTKLIB structure + eph_data[valid_obs] = eph_to_rtklib(beidou_ephemeris_iter->second); + // convert observation from GNSS-SDR class to RTKLIB structure + obsd_t newobs{}; + d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, + gnss_observables_iter->second, + beidou_ephemeris_iter->second.WN + BEIDOU_DNAV_BDT2GPST_WEEK_NUM_OFFSET, + d_rtklib_band_index[sig_]); + valid_obs++; + } + else // the ephemeris are not available for this SV + { + DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->first; + } + } + // BeiDou B3 + if (sig_ == "B3") + { + beidou_ephemeris_iter = beidou_dnav_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (beidou_ephemeris_iter != beidou_dnav_ephemeris_map.cend()) + { + bool found_B1I_obs = false; + for (int i = 0; i < valid_obs; i++) + { + if (eph_data[i].sat == (static_cast(gnss_observables_iter->second.PRN + NSATGPS + NSATGLO + NSATGAL + NSATQZS))) + { + d_obs_data[i + glo_valid_obs] = insert_obs_to_rtklib(d_obs_data[i + glo_valid_obs], + gnss_observables_iter->second, + beidou_ephemeris_iter->second.WN + BEIDOU_DNAV_BDT2GPST_WEEK_NUM_OFFSET, + d_rtklib_band_index[sig_]); + found_B1I_obs = true; + break; + } + } + if (!found_B1I_obs) + { + // insert BeiDou B3I obs as new obs and also insert its ephemeris + // convert ephemeris from GNSS-SDR class to RTKLIB structure + eph_data[valid_obs] = eph_to_rtklib(beidou_ephemeris_iter->second); + // convert observation from GNSS-SDR class to RTKLIB structure + const auto default_code_ = static_cast(CODE_NONE); + obsd_t newobs = {{0, 0}, '0', '0', {}, {}, + {default_code_, default_code_, default_code_}, + {}, {0.0, 0.0, 0.0}, {}}; + d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, + gnss_observables_iter->second, + beidou_ephemeris_iter->second.WN + BEIDOU_DNAV_BDT2GPST_WEEK_NUM_OFFSET, + d_rtklib_band_index[sig_]); + valid_obs++; + } + } + else // the ephemeris are not available for this SV + { + DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; + } + } + break; + } + + default: + DLOG(INFO) << "Hybrid observables: Unknown GNSS"; + break; + } + } + + // ********************************************************************** + // ****** SOLVE PVT****************************************************** + // ********************************************************************** + + this->set_valid_position(false); + if ((valid_obs + glo_valid_obs) > 3) + { + int result = 0; + d_nav_data = {}; + d_nav_data.eph = eph_data.data(); + d_nav_data.geph = geph_data.data(); + d_nav_data.n = valid_obs; + d_nav_data.ng = glo_valid_obs; + if (gps_iono.valid) + { + d_nav_data.ion_gps[0] = gps_iono.alpha0; + d_nav_data.ion_gps[1] = gps_iono.alpha1; + d_nav_data.ion_gps[2] = gps_iono.alpha2; + d_nav_data.ion_gps[3] = gps_iono.alpha3; + d_nav_data.ion_gps[4] = gps_iono.beta0; + d_nav_data.ion_gps[5] = gps_iono.beta1; + d_nav_data.ion_gps[6] = gps_iono.beta2; + d_nav_data.ion_gps[7] = gps_iono.beta3; + } + if (!(gps_iono.valid) and gps_cnav_iono.valid) + { + d_nav_data.ion_gps[0] = gps_cnav_iono.alpha0; + d_nav_data.ion_gps[1] = gps_cnav_iono.alpha1; + d_nav_data.ion_gps[2] = gps_cnav_iono.alpha2; + d_nav_data.ion_gps[3] = gps_cnav_iono.alpha3; + d_nav_data.ion_gps[4] = gps_cnav_iono.beta0; + d_nav_data.ion_gps[5] = gps_cnav_iono.beta1; + d_nav_data.ion_gps[6] = gps_cnav_iono.beta2; + d_nav_data.ion_gps[7] = gps_cnav_iono.beta3; + } + if (galileo_iono.ai0 != 0.0) + { + d_nav_data.ion_gal[0] = galileo_iono.ai0; + d_nav_data.ion_gal[1] = galileo_iono.ai1; + d_nav_data.ion_gal[2] = galileo_iono.ai2; + d_nav_data.ion_gal[3] = 0.0; + } + if (beidou_dnav_iono.valid) + { + d_nav_data.ion_cmp[0] = beidou_dnav_iono.alpha0; + d_nav_data.ion_cmp[1] = beidou_dnav_iono.alpha1; + d_nav_data.ion_cmp[2] = beidou_dnav_iono.alpha2; + d_nav_data.ion_cmp[3] = beidou_dnav_iono.alpha3; + d_nav_data.ion_cmp[4] = beidou_dnav_iono.beta0; + d_nav_data.ion_cmp[5] = beidou_dnav_iono.beta0; + d_nav_data.ion_cmp[6] = beidou_dnav_iono.beta0; + d_nav_data.ion_cmp[7] = beidou_dnav_iono.beta3; + } + if (gps_utc_model.valid) + { + d_nav_data.utc_gps[0] = gps_utc_model.A0; + d_nav_data.utc_gps[1] = gps_utc_model.A1; + d_nav_data.utc_gps[2] = gps_utc_model.tot; + d_nav_data.utc_gps[3] = gps_utc_model.WN_T; + d_nav_data.leaps = gps_utc_model.DeltaT_LS; + } + if (!(gps_utc_model.valid) and gps_cnav_utc_model.valid) + { + d_nav_data.utc_gps[0] = gps_cnav_utc_model.A0; + d_nav_data.utc_gps[1] = gps_cnav_utc_model.A1; + d_nav_data.utc_gps[2] = gps_cnav_utc_model.tot; + d_nav_data.utc_gps[3] = gps_cnav_utc_model.WN_T; + d_nav_data.leaps = gps_cnav_utc_model.DeltaT_LS; + } + if (glonass_gnav_utc_model.valid) + { + d_nav_data.utc_glo[0] = glonass_gnav_utc_model.d_tau_c; // ?? + d_nav_data.utc_glo[1] = 0.0; // ?? + d_nav_data.utc_glo[2] = 0.0; // ?? + d_nav_data.utc_glo[3] = 0.0; // ?? + } + if (galileo_utc_model.A0 != 0.0) + { + d_nav_data.utc_gal[0] = galileo_utc_model.A0; + d_nav_data.utc_gal[1] = galileo_utc_model.A1; + d_nav_data.utc_gal[2] = galileo_utc_model.tot; + d_nav_data.utc_gal[3] = galileo_utc_model.WNot; + d_nav_data.leaps = galileo_utc_model.Delta_tLS; + } + if (beidou_dnav_utc_model.valid) + { + d_nav_data.utc_cmp[0] = beidou_dnav_utc_model.A0_UTC; + d_nav_data.utc_cmp[1] = beidou_dnav_utc_model.A1_UTC; + d_nav_data.utc_cmp[2] = 0.0; // ?? + d_nav_data.utc_cmp[3] = 0.0; // ?? + d_nav_data.leaps = beidou_dnav_utc_model.DeltaT_LS; + } + + /* update carrier wave length using native function call in RTKlib */ + for (int i = 0; i < MAXSAT; i++) + { + for (int j = 0; j < NFREQ; j++) + { + d_nav_data.lam[i][j] = satwavelen(i + 1, d_rtklib_freq_index[j], &d_nav_data); + } + } + + result = rtkpos(&d_rtk, d_obs_data.data(), valid_obs + glo_valid_obs, &d_nav_data); + + if (result == 0) + { + LOG(INFO) << "RTKLIB rtkpos error: " << d_rtk.errbuf; + d_rtk.neb = 0; // clear error buffer to avoid repeating the error message + this->set_time_offset_s(0.0); // reset rx time estimation + this->set_num_valid_observations(0); + if (d_conf.enable_pvt_kf == true) + { + d_pvt_kf.reset_Kf(); + } + } + else + { + this->set_num_valid_observations(d_rtk.sol.ns); // record the number of valid satellites used by the PVT solver + pvt_sol = d_rtk.sol; + // DOP computation + unsigned int used_sats = 0; + for (unsigned int i = 0; i < MAXSAT; i++) + { + pvt_ssat[i] = d_rtk.ssat[i]; + if (d_rtk.ssat[i].vs == 1) + { + used_sats++; + } + } + + std::vector azel(used_sats * 2); + int index_aux = 0; + for (auto &i : d_rtk.ssat) + { + if (i.vs == 1) + { + azel[2 * index_aux] = i.azel[0]; + azel[2 * index_aux + 1] = i.azel[1]; + index_aux++; + } + } + + if (index_aux > 0) + { + dops(index_aux, azel.data(), 0.0, d_dop.data()); + } + this->set_valid_position(true); + std::array rx_position_and_time{}; + + if (d_conf.enable_pvt_kf == true) + { + if (d_pvt_kf.is_initialized() == false) + { + arma::vec p = {pvt_sol.rr[0], pvt_sol.rr[1], pvt_sol.rr[2]}; + arma::vec v = {pvt_sol.rr[3], pvt_sol.rr[4], pvt_sol.rr[5]}; + arma::vec res_p = {pvt_sol.qr[0], pvt_sol.qr[1], pvt_sol.qr[2], + pvt_sol.qr[3], pvt_sol.qr[4], pvt_sol.qr[5]}; + + d_pvt_kf.init_Kf(p, + v, + res_p, + kf_update_interval_s, + d_conf.estatic_measures_sd, + d_conf.measures_ecef_pos_sd_m, + d_conf.measures_ecef_vel_sd_ms, + d_conf.system_ecef_pos_sd_m, + d_conf.system_ecef_vel_sd_ms); + } + else + { + arma::vec p = {pvt_sol.rr[0], pvt_sol.rr[1], pvt_sol.rr[2]}; + arma::vec v = {pvt_sol.rr[3], pvt_sol.rr[4], pvt_sol.rr[5]}; + arma::vec res_p = {pvt_sol.qr[0], pvt_sol.qr[1], pvt_sol.qr[2], + pvt_sol.qr[3], pvt_sol.qr[4], pvt_sol.qr[5]}; + + d_pvt_kf.run_Kf(p, v, res_p); + d_pvt_kf.get_pv_Kf(p, v); + pvt_sol.rr[0] = p[0]; // [m] + pvt_sol.rr[1] = p[1]; // [m] + pvt_sol.rr[2] = p[2]; // [m] + pvt_sol.rr[3] = v[0]; // [ms] + pvt_sol.rr[4] = v[1]; // [ms] + pvt_sol.rr[5] = v[2]; // [ms] + } + } + + rx_position_and_time[0] = pvt_sol.rr[0]; // [m] + rx_position_and_time[1] = pvt_sol.rr[1]; // [m] + rx_position_and_time[2] = pvt_sol.rr[2]; // [m] + + // todo: fix this ambiguity in the RTKLIB units in receiver clock offset! + if (d_rtk.opt.mode == PMODE_SINGLE) + { + // if the RTKLIB solver is set to SINGLE, the dtr is already expressed in [s] + // add also the clock offset from gps to galileo (pvt_sol.dtr[2]) + rx_position_and_time[3] = pvt_sol.dtr[0] + pvt_sol.dtr[2]; + } + else + { + // the receiver clock offset is expressed in [meters], so we convert it into [s] + // add also the clock offset from gps to galileo (pvt_sol.dtr[2]) + rx_position_and_time[3] = pvt_sol.dtr[2] + pvt_sol.dtr[0] / SPEED_OF_LIGHT_M_S; + } + this->set_rx_pos({rx_position_and_time[0], rx_position_and_time[1], rx_position_and_time[2]}); // save ECEF position for the next iteration + + // compute Ground speed and COG + double ground_speed_ms = 0.0; + std::array pos{}; + std::array enuv{}; + ecef2pos(pvt_sol.rr, pos.data()); + ecef2enu(pos.data(), &pvt_sol.rr[3], enuv.data()); + this->set_speed_over_ground(norm_rtk(enuv.data(), 2)); + double new_cog; + if (ground_speed_ms >= 1.0) + { + new_cog = atan2(enuv[0], enuv[1]) * R2D; + if (new_cog < 0.0) + { + new_cog += 360.0; + } + this->set_course_over_ground(new_cog); + } + + this->set_time_offset_s(rx_position_and_time[3]); + + DLOG(INFO) << "RTKLIB Position at RX TOW = " << gnss_observables_map.cbegin()->second.RX_time + << " in ECEF (X,Y,Z,t[meters]) = " << rx_position_and_time[0] << ", " << rx_position_and_time[1] << ", " << rx_position_and_time[2] << ", " << rx_position_and_time[3]; + + // gtime_t rtklib_utc_time = gpst2utc(pvt_sol.time); // Corrected RX Time (Non integer multiply of 1 ms of granularity) + // Uncorrected RX Time (integer multiply of 1 ms and the same observables time reported in RTCM and RINEX) + const gtime_t rtklib_time = timeadd(pvt_sol.time, rx_position_and_time[3]); // uncorrected rx time + const gtime_t rtklib_utc_time = gpst2utc(rtklib_time); + boost::posix_time::ptime p_time = boost::posix_time::from_time_t(rtklib_utc_time.time); + p_time += boost::posix_time::microseconds(static_cast(round(rtklib_utc_time.sec * 1e6))); // NOLINT(google-runtime-int) + + this->set_position_UTC_time(p_time); + + DLOG(INFO) << "RTKLIB Position at " << boost::posix_time::to_simple_string(p_time) + << " is Lat = " << this->get_latitude() << " [deg], Long = " << this->get_longitude() + << " [deg], Height= " << this->get_height() << " [m]" + << " RX time offset= " << this->get_time_offset_s() << " [s]"; + + // ######## PVT MONITOR ######### + // TOW + d_monitor_pvt.TOW_at_current_symbol_ms = gnss_observables_map.cbegin()->second.TOW_at_current_symbol_ms; + // WEEK + d_monitor_pvt.week = adjgpsweek(d_nav_data.eph[0].week, this->is_pre_2009()); + // PVT GPS time + d_monitor_pvt.RX_time = gnss_observables_map.cbegin()->second.RX_time; + // User clock offset [s] + d_monitor_pvt.user_clk_offset = rx_position_and_time[3]; + + // ECEF POS X,Y,X [m] + ECEF VEL X,Y,X [m/s] (6 x double) + d_monitor_pvt.pos_x = pvt_sol.rr[0]; + d_monitor_pvt.pos_y = pvt_sol.rr[1]; + d_monitor_pvt.pos_z = pvt_sol.rr[2]; + d_monitor_pvt.vel_x = pvt_sol.rr[3]; + d_monitor_pvt.vel_y = pvt_sol.rr[4]; + d_monitor_pvt.vel_z = pvt_sol.rr[5]; + + // position variance/covariance (m^2) {c_xx,c_yy,c_zz,c_xy,c_yz,c_zx} (6 x double) + d_monitor_pvt.cov_xx = pvt_sol.qr[0]; + d_monitor_pvt.cov_yy = pvt_sol.qr[1]; + d_monitor_pvt.cov_zz = pvt_sol.qr[2]; + d_monitor_pvt.cov_xy = pvt_sol.qr[3]; + d_monitor_pvt.cov_yz = pvt_sol.qr[4]; + d_monitor_pvt.cov_zx = pvt_sol.qr[5]; + + // GEO user position Latitude [deg] + d_monitor_pvt.latitude = this->get_latitude(); + // GEO user position Longitude [deg] + d_monitor_pvt.longitude = this->get_longitude(); + // GEO user position Height [m] + d_monitor_pvt.height = this->get_height(); + + // NUMBER OF VALID SATS + d_monitor_pvt.valid_sats = pvt_sol.ns; + // RTKLIB solution status + d_monitor_pvt.solution_status = pvt_sol.stat; + // RTKLIB solution type (0:xyz-ecef,1:enu-baseline) + d_monitor_pvt.solution_type = pvt_sol.type; + // AR ratio factor for validation + d_monitor_pvt.AR_ratio_factor = pvt_sol.ratio; + // AR ratio threshold for validation + d_monitor_pvt.AR_ratio_threshold = pvt_sol.thres; + + // GDOP / PDOP/ HDOP/ VDOP + d_monitor_pvt.gdop = d_dop[0]; + d_monitor_pvt.pdop = d_dop[1]; + d_monitor_pvt.hdop = d_dop[2]; + d_monitor_pvt.vdop = d_dop[3]; + + this->set_rx_vel({enuv[0], enuv[1], enuv[2]}); + + const double clock_drift_ppm = pvt_sol.dtr[5] / SPEED_OF_LIGHT_M_S * 1e6; + + this->set_clock_drift_ppm(clock_drift_ppm); + // User clock drift [ppm] + d_monitor_pvt.user_clk_drift_ppm = clock_drift_ppm; + + // ######## LOG FILE ######### + if (d_flag_dump_enabled == true) + { + // MULTIPLEXED FILE RECORDING - Record results to file + try + { + double tmp_double; + uint32_t tmp_uint32; + // TOW + tmp_uint32 = gnss_observables_map.cbegin()->second.TOW_at_current_symbol_ms; + d_dump_file.write(reinterpret_cast(&tmp_uint32), sizeof(uint32_t)); + // WEEK + tmp_uint32 = adjgpsweek(d_nav_data.eph[0].week, this->is_pre_2009()); + d_dump_file.write(reinterpret_cast(&tmp_uint32), sizeof(uint32_t)); + // PVT GPS time + tmp_double = gnss_observables_map.cbegin()->second.RX_time; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // User clock offset [s] + tmp_double = rx_position_and_time[3]; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + + // ECEF POS X,Y,X [m] + ECEF VEL X,Y,X [m/s] (6 x double) + tmp_double = pvt_sol.rr[0]; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = pvt_sol.rr[1]; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = pvt_sol.rr[2]; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = pvt_sol.rr[3]; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = pvt_sol.rr[4]; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = pvt_sol.rr[5]; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + + // position variance/covariance (m^2) {c_xx,c_yy,c_zz,c_xy,c_yz,c_zx} (6 x double) + tmp_double = pvt_sol.qr[0]; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = pvt_sol.qr[1]; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = pvt_sol.qr[2]; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = pvt_sol.qr[3]; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = pvt_sol.qr[4]; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = pvt_sol.qr[5]; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + + // GEO user position Latitude [deg] + tmp_double = this->get_latitude(); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // GEO user position Longitude [deg] + tmp_double = this->get_longitude(); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // GEO user position Height [m] + tmp_double = this->get_height(); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + + // NUMBER OF VALID SATS + d_dump_file.write(reinterpret_cast(&pvt_sol.ns), sizeof(uint8_t)); + // RTKLIB solution status + d_dump_file.write(reinterpret_cast(&pvt_sol.stat), sizeof(uint8_t)); + // RTKLIB solution type (0:xyz-ecef,1:enu-baseline) + d_dump_file.write(reinterpret_cast(&pvt_sol.type), sizeof(uint8_t)); + // AR ratio factor for validation + d_dump_file.write(reinterpret_cast(&pvt_sol.ratio), sizeof(float)); + // AR ratio threshold for validation + d_dump_file.write(reinterpret_cast(&pvt_sol.thres), sizeof(float)); + + // GDOP / PDOP / HDOP / VDOP + d_dump_file.write(reinterpret_cast(&d_dop[0]), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_dop[1]), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_dop[2]), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_dop[3]), sizeof(double)); + } + catch (const std::ofstream::failure &e) + { + LOG(WARNING) << "Exception writing RTKLIB dump file " << e.what(); + } + } + } + } + return this->is_valid_position(); +}